Merge first part of Pixhawk board support

First PR for getting Pixhawk to play nice with PPRZ.

The Pixhawk can be flashed without external tools (i.e. no JTAG interface is needed). Instead, the PX4 bootloader is kept in place and used by pprz to flash both the AP (stm32f4) and FBW (stm32f1) devices. One important remark is that for the FBW device, currently the computer needs to be connected to the Telem2 port using a FTDI cable. This is because pprz does not support serial communication over USB, or so I've heard.

In any case, the Iris+ drone factory equipped with a Pixhawk flies both on INDI and PID in ATT mode. However, PPRZ currently uses the MPU6000 IMU, which was broken in my Iris+ (and there are indications that maybe in others Iris' and X8's as well). I had to replace it with a fresh Pixhawk.

Also tested with a Pixhawk clone: seems to work just as well.

closes #1551
This commit is contained in:
Felix Ruess
2016-03-11 18:04:16 +01:00
53 changed files with 3120 additions and 208 deletions
+7
View File
@@ -154,6 +154,13 @@ upload: $(OBJDIR)/$(TARGET).bin
@echo "Using ST-LINK with SWD at $(STLINK_ADDR)"
$(Q)st-flash write $^ $(STLINK_ADDR)
else ifeq ($(FLASH_MODE),PX4_BOOTLOADER)
# Program the device and start it.
upload: $(OBJDIR)/$(TARGET).bin
$(PAPARAZZI_SRC)/sw/tools/px4/px_mkfw.py --prototype $(PX4_PROTOTYPE) --image $(OBJDIR)/$(TARGET).bin > $(OBJDIR)/$(TARGET).px4
$(PAPARAZZI_SRC)/sw/tools/px4/print_message.py
$(PAPARAZZI_SRC)/sw/tools/px4/px_uploader.py --port $(PX4_BL_PORT) $(OBJDIR)/$(TARGET).px4
#
# no known flash mode
else
+11
View File
@@ -109,6 +109,17 @@
settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml"
gui_color="#9b69e5ce9e7a"
/>
<aircraft
name="Iris"
ac_id="66"
airframe="airframes/TUDELFT/tudelft_iris_indi.xml"
radio="radios/FrSky3dr.xml"
telemetry="telemetry/default_rotorcraft_slow.xml"
flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
name="Logo600"
ac_id="111"
@@ -89,6 +89,16 @@
<arg flag="-speech" />
</program>
</session>
<session name="Flight Paparazzi @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/paparazzi/link"/>
<arg flag="-s" constant="57600"/>
</program>
<program name="Server"/>
<program name="GCS">
<arg flag="-speech" />
</program>
</session>
<session name="Flight ACM1 @57600">
<program name="Data Link">
<arg flag="-d" constant="/dev/ttyACM1"/>
@@ -0,0 +1,250 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<?xml version="1.0" encoding="UTF-8"?>
<!-- this is a quadrotor frame equiped with
* Autopilot: 3dr Pixhawk 2.4
* IMU: MPU6000 + L3GD20 + LSM303D
* Actuators: PWM motor controllers?
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC: PPM
-->
<airframe name="iris_indi">
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_2.4" />
<define name="BAT_CHECKER_DELAY" value="80" />
<!-- amount of time it take for the bat to check -->
<!-- to avoid bat low spike detection when strong pullup withch draws short sudden power-->
<define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
<!-- in seconds-->
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim" />
<subsystem name="udp" />
</target>
<subsystem name="telemetry" type="transparent" />
<!-- <subsystem name="imu" type="px4fmu_v2.4">-->
<subsystem name="imu" type="mpu6000">
<configure name="IMU_MPU_SPI_DEV" value="spi1" />
<configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE2" />
</subsystem>
<subsystem name="gps" type="ublox" />
<subsystem name="stabilization" type="indi" />
<subsystem name="ahrs" type="int_cmpl_quat" />
<subsystem name="ins" type="extended" />
<subsystem name="current_sensor">
<configure name="ADC_CURRENT_SENSOR" value="ADC_3" />
</subsystem>
<subsystem name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART6" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</subsystem>
</firmware>
<firmware name="rotorcraft">
<target name="fbw" board="px4io_2.4" />
<subsystem name="motor_mixing" />
<subsystem name="radio_control" type="ppm">
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1" />
<define name="RADIO_KILL_SWITCH" value="RADIO_KILL" />
</subsystem>
<!-- <subsystem name="radio_control" type="spektrum">-->
<!-- <define name="RADIO_CONTROL_SPEKTRUM_NO_SIGN" value="1"/>-->
<!-- <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1"/>-->
<!-- <define name="RADIO_FBW_MODE" value="RADIO_AUX2"/>-->
<!-- <define name="RADIO_MODE" value="RADIO_GEAR"/>-->
<!-- <define name="SPEKTRUM_HAS_SOFT_BIND_PIN" value="1"/>-->
<!-- </subsystem>-->
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400" />
</subsystem>
<define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
<!-- Switch to Failsafe or to Autopilot on RC loss? -->
<define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO" />
<define name="AP_LOST_FBW_MODE" value="FBW_MODE_MANUAL" />
<!-- Switch to Failsafe or to Manual on AP loss? -->
<define name="INTERMCU_LOST_CNT" value="100" />
<subsystem name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART2" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</subsystem>
</firmware>
<firmware name="test_progs">
<target name="test_telemetry" board="px4fmu_2.4" />
</firmware>
<firmware name="test_progs">
<target name="test_baro_board" board="px4fmu_2.4" />
</firmware>
<firmware name="test_progs">
<target name="test_datalink" board="px4fmu_2.4" />
</firmware>
<modules main_freq="512">
<load name="px4io_flash.xml" />
<load name="geo_mag.xml" />
<load name="air_data.xml" />
<load name="send_imu_mag_current.xml" />
<load name="gps_ubx_ucenter.xml" />
<!-- <load name="spektrum_soft_bind.xml"/>-->
</modules>
<section name="MISC">
<define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
</section>
<section name="IMU" prefix="IMU_">
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="14" />
<define name="MAG_Y_NEUTRAL" value="116" />
<define name="MAG_Z_NEUTRAL" value="119" />
<define name="MAG_X_SENS" value="5.09245681612" integer="16" />
<define name="MAG_Y_SENS" value="5.29702744632" integer="16" />
<define name="MAG_Z_SENS" value="5.65287938992" integer="16" />
<define name="BODY_TO_IMU_PHI" value="180." unit="deg" />
<define name="BODY_TO_IMU_THETA" value="180." unit="deg" />
<define name="BODY_TO_IMU_PSI" value="90." unit="deg" />
</section>
<commands>
<axis name="PITCH" failsafe_value="0" />
<axis name="ROLL" failsafe_value="0" />
<axis name="YAW" failsafe_value="0" />
<axis name="THRUST" failsafe_value="0" />
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@ROLL" />
<set command="PITCH" value="@PITCH" />
<set command="YAW" value="@YAW" />
</rc_commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="2" min="1140" neutral="1280" max="1850" />
<servo name="TOP_RIGHT" no="0" min="1140" neutral="1280" max="1850" />
<servo name="BOTTOM_RIGHT" no="3" min="1140" neutral="1280" max="1850" />
<servo name="BOTTOM_LEFT" no="1" min="1140" neutral="1280" max="1850" />
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0" />
<define name="TRIM_PITCH" value="0" />
<define name="TRIM_YAW" value="0" />
<define name="REVERSE" value="FALSE" />
<define name="TYPE" value="QUAD_X" />
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)" />
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]" />
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]" />
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]" />
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]" />
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE" />
<define name="CALC_TAS_FACTOR" value="FALSE" />
<define name="CALC_AMSL_BARO" value="TRUE" />
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503" />
<define name="H_Y" value="0.0017972" />
<define name="H_Z" value="0.9211303" />
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2" />
<define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
</section>
<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg" />
<define name="SP_MAX_THETA" value="45" unit="deg" />
<define name="SP_MAX_R" value="300" unit="deg/s" />
<define name="DEADBAND_A" value="0" />
<define name="DEADBAND_E" value="0" />
<define name="DEADBAND_R" value="50" />
</section>
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s" />
<define name="REF_ZETA_P" value="0.9" />
<define name="REF_MAX_P" value="600." unit="deg/s" />
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)" />
<define name="REF_OMEGA_Q" value="450" unit="deg/s" />
<define name="REF_ZETA_Q" value="0.9" />
<define name="REF_MAX_Q" value="600." unit="deg/s" />
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)" />
<define name="REF_OMEGA_R" value="450" unit="deg/s" />
<define name="REF_ZETA_R" value="0.9" />
<define name="REF_MAX_R" value="600." unit="deg/s" />
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)" />
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.017" />
<define name="G1_Q" value="0.019" />
<define name="G1_R" value="0.0011" />
<define name="G2_R" value="0.089" />
<!-- For the bebop2 we need to filter the roll rate due to the dampers -->
<define name="FILTER_ROLL_RATE" value="FALSE" />
<define name="FILTER_PITCH_RATE" value="FALSE" />
<define name="FILTER_YAW_RATE" value="FALSE" />
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="100.0" />
<define name="REF_ERR_Q" value="100.0" />
<define name="REF_ERR_R" value="100.0" />
<define name="REF_RATE_P" value="14.0" />
<define name="REF_RATE_Q" value="14.0" />
<define name="REF_RATE_R" value="14.0" />
<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0" />
<define name="FILT_ZETA" value="0.55" />
<define name="FILT_OMEGA_R" value="20.0" />
<define name="FILT_ZETA_R" value="0.55" />
<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.04" />
<define name="ACT_DYN_Q" value="0.04" />
<define name="ACT_DYN_R" value="0.04" />
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE" />
<define name="ADAPTIVE_MU" value="0.0001" />
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="350" />
<define name="HOVER_KD" value="85" />
<define name="HOVER_KI" value="20" />
<define name="NOMINAL_HOVER_THROTTLE" value="0.4" />
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE" />
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg" />
<define name="REF_MAX_SPEED" value="2" unit="m/s" />
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="120" />
<define name="DGAIN" value="100" />
<define name="IGAIN" value="30" />
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="4.5" />
<define name="DESCEND_VSPEED" value="-1.0" />
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]" />
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string" />
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string" />
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_RC_DIRECT" />
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT" /> <!-- changing this mode may not work, since fbw will be in mode manual directly from rc!-->
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT" />
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT" />
<define name="NO_RC_THRUST_LIMIT" value="TRUE" />
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700" />
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V" />
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V" />
<define name="LOW_BAT_LEVEL" value="11.1" unit="V" />
<define name="MAX_BAT_LEVEL" value="12.4" unit="V" />
</section>
</airframe>
+230
View File
@@ -0,0 +1,230 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<?xml version="1.0" encoding="UTF-8"?>
<!-- this is a quadrotor frame equiped with
* Autopilot: 3dr Pixhawk 2.4
* IMU: MPU6000
* Actuators: PWM motor controllers
* GPS: Ublox http://wiki.paparazziuav.org/wiki/Subsystem/gps
* RC: PPM
-->
<airframe name="iris_pid">
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_2.4" />
<define name="BAT_CHECKER_DELAY" value="80" />
<!-- amount of time it take for the bat to check -->
<!-- to avoid bat low spike detection when strong pullup withch draws short sudden power-->
<define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
<!-- in seconds-->
<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim" />
<subsystem name="udp" />
</target>
<subsystem name="telemetry" type="transparent" />
<subsystem name="imu" type="mpu6000">
<!-- <subsystem name="imu" type="L3GD20">-->
<configure name="IMU_MPU_SPI_DEV" value="spi1" />
<configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE2" />
</subsystem>
<subsystem name="gps" type="ublox" />
<subsystem name="stabilization" type="int_quat" />
<subsystem name="ahrs" type="int_cmpl_quat" />
<subsystem name="ins" type="extended" />
<subsystem name="current_sensor">
<configure name="ADC_CURRENT_SENSOR" value="ADC_3" />
</subsystem>
<subsystem name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART6" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</subsystem>
</firmware>
<firmware name="rotorcraft">
<target name="fbw" board="px4io_2.4" />
<subsystem name="motor_mixing" />
<subsystem name="radio_control" type="ppm">
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1" />
<define name="RADIO_KILL_SWITCH" value="RADIO_KILL" />
</subsystem>
<!-- <subsystem name="radio_control" type="spektrum">-->
<!-- <define name="RADIO_CONTROL_SPEKTRUM_NO_SIGN" value="1"/>-->
<!-- <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="1"/>-->
<!-- <define name="RADIO_FBW_MODE" value="RADIO_AUX2"/>-->
<!-- <define name="RADIO_MODE" value="RADIO_GEAR"/>-->
<!-- <define name="SPEKTRUM_HAS_SOFT_BIND_PIN" value="1"/>-->
<!-- </subsystem>-->
<subsystem name="actuators" type="pwm">
<define name="SERVO_HZ" value="400" />
</subsystem>
<define name="RC_LOST_FBW_MODE" value="FBW_MODE_FAILSAFE" />
<!-- Switch to Failsafe or to Autopilot on RC loss? -->
<define name="RC_LOST_IN_AUTO_FBW_MODE" value="FBW_MODE_AUTO" />
<define name="AP_LOST_FBW_MODE" value="FBW_MODE_MANUAL" />
<!-- Switch to Failsafe or to Manual on AP loss? -->
<define name="INTERMCU_LOST_CNT" value="100" />
<subsystem name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART2" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</subsystem>
</firmware>
<firmware name="test_progs">
<target name="test_telemetry" board="px4fmu_2.4" />
</firmware>
<firmware name="test_progs">
<target name="test_baro_board" board="px4fmu_2.4" />
</firmware>
<firmware name="test_progs">
<target name="test_datalink" board="px4fmu_2.4" />
</firmware>
<modules main_freq="512">
<load name="px4io_flash.xml" />
<load name="geo_mag.xml" />
<load name="air_data.xml" />
<load name="send_imu_mag_current.xml" />
<load name="gps_ubx_ucenter.xml" />
<!-- <load name="spektrum_soft_bind.xml"/>-->
</modules>
<section name="MISC">
<define name="MilliAmpereOfAdc(adc)" value="((float)adc) * (3.3f / 4096.0f) * (90.0f / 5.0f)" />
<!-- 100Amp = 2Volt -> 2482,42 tick/100Amp"(0.0402832*adc)" -->
</section>
<section name="IMU" prefix="IMU_">
<!-- replace this with your own calibration -->
<define name="MAG_X_NEUTRAL" value="14" />
<define name="MAG_Y_NEUTRAL" value="116" />
<define name="MAG_Z_NEUTRAL" value="119" />
<define name="MAG_X_SENS" value="5.09245681612" integer="16" />
<define name="MAG_Y_SENS" value="5.29702744632" integer="16" />
<define name="MAG_Z_SENS" value="5.65287938992" integer="16" />
<define name="BODY_TO_IMU_PHI" value="180." unit="deg" />
<define name="BODY_TO_IMU_THETA" value="180." unit="deg" />
<define name="BODY_TO_IMU_PSI" value="90." unit="deg" />
</section>
<commands>
<axis name="PITCH" failsafe_value="0" />
<axis name="ROLL" failsafe_value="0" />
<axis name="YAW" failsafe_value="0" />
<axis name="THRUST" failsafe_value="0" />
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@ROLL" />
<set command="PITCH" value="@PITCH" />
<set command="YAW" value="@YAW" />
</rc_commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="2" min="1140" neutral="1280" max="1850" />
<servo name="TOP_RIGHT" no="0" min="1140" neutral="1280" max="1850" />
<servo name="BOTTOM_RIGHT" no="3" min="1140" neutral="1280" max="1850" />
<servo name="BOTTOM_LEFT" no="1" min="1140" neutral="1280" max="1850" />
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0" />
<define name="TRIM_PITCH" value="0" />
<define name="TRIM_YAW" value="0" />
<define name="REVERSE" value="FALSE" />
<define name="TYPE" value="QUAD_X" />
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)" />
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]" />
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]" />
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]" />
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]" />
</command_laws>
<section name="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE" />
<define name="CALC_TAS_FACTOR" value="FALSE" />
<define name="CALC_AMSL_BARO" value="TRUE" />
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
<!-- Toulouse -->
<!--define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/-->
<!-- Delft -->
<define name="H_X" value="0.3892503" />
<define name="H_Y" value="0.0017972" />
<define name="H_Z" value="0.9211303" />
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2" />
<define name="SONAR_UPDATE_ON_AGL" value="TRUE" />
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45." unit="deg" />
<define name="SP_MAX_THETA" value="45." unit="deg" />
<define name="SP_MAX_R" value="90." unit="deg/s" />
<define name="DEADBAND_R" value="250" />
<!-- reference -->
<define name="REF_OMEGA_P" value="800" unit="deg/s" />
<define name="REF_ZETA_P" value="0.85" />
<define name="REF_MAX_P" value="300." unit="deg/s" />
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)" />
<define name="REF_OMEGA_Q" value="800" unit="deg/s" />
<define name="REF_ZETA_Q" value="0.85" />
<define name="REF_MAX_Q" value="300." unit="deg/s" />
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)" />
<define name="REF_OMEGA_R" value="500" unit="deg/s" />
<define name="REF_ZETA_R" value="0.85" />
<define name="REF_MAX_R" value="180." unit="deg/s" />
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" />
<!-- feedback -->
<define name="PHI_PGAIN" value="900" />
<define name="PHI_DGAIN" value="200" />
<define name="PHI_IGAIN" value="200" />
<define name="THETA_PGAIN" value="900" />
<define name="THETA_DGAIN" value="200" />
<define name="THETA_IGAIN" value="200" />
<define name="PSI_PGAIN" value="900" />
<define name="PSI_DGAIN" value="200" />
<define name="PSI_IGAIN" value="10" />
<!-- feedforward -->
<define name="PHI_DDGAIN" value="75" />
<define name="THETA_DDGAIN" value="75" />
<define name="PSI_DDGAIN" value="75" />
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="350" />
<define name="HOVER_KD" value="85" />
<define name="HOVER_KI" value="20" />
<define name="NOMINAL_HOVER_THROTTLE" value="0.4" />
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE" />
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg" />
<define name="REF_MAX_SPEED" value="2" unit="m/s" />
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="120" />
<define name="DGAIN" value="100" />
<define name="IGAIN" value="30" />
</section>
<section name="NAVIGATION" prefix="NAV_">
<define name="CLIMB_VSPEED" value="4.5" />
<define name="DESCEND_VSPEED" value="-1.0" />
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]" />
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string" />
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string" />
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_RC_DIRECT" />
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT" />
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT" />
<define name="MODE_AUTO2" value="AP_MODE_ATTITUDE_DIRECT" />
<define name="NO_RC_THRUST_LIMIT" value="TRUE" />
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700" />
<define name="CATASTROPHIC_BAT_LEVEL" value="9.9" unit="V" />
<define name="CRITIC_BAT_LEVEL" value="10.8" unit="V" />
<define name="LOW_BAT_LEVEL" value="11.1" unit="V" />
<define name="MAX_BAT_LEVEL" value="12.4" unit="V" />
</section>
</airframe>
+8 -8
View File
@@ -2,7 +2,7 @@
#
# px4fmu_2.4.makefile
#
# This is for the main MCU (STM32F427) on the pixhawk board
# This is for the main MCU (STM32F427) on the PX4 board
# See https://pixhawk.org/modules/pixhawk for details
#
@@ -19,10 +19,12 @@ $(TARGET).LDSCRIPT=$(SRC_ARCH)/px4fmu_2.4.ld
HARD_FLOAT=yes
# default flash mode is via usb dfu bootloader
# possibilities: DFU, SWD
FLASH_MODE ?= SWD
# default flash mode is the PX4 bootloader
# possibilities: DFU, SWD, PX4 bootloader
FLASH_MODE ?= PX4_BOOTLOADER
PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4fmu-v2.prototype"
PX4_BL_PORT ?= "/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/pci-3D_Robotics*"
$(TARGET).MAKEFILE = stm32
#
# default LED configuration
@@ -40,12 +42,10 @@ SYS_TIME_LED ?= 1
MODEM_PORT ?= UART2
MODEM_BAUD ?= B57600
#The GPS serial on px4 is called serial 3, but connected to uart4 on the f4
GPS_PORT ?= UART4
GPS_BAUD ?= B38400
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART2
#
# default actuator configuration
#
+48
View File
@@ -0,0 +1,48 @@
# Hey Emacs, this is a -*- makefile -*-
#
# px4io_2.4.makefile
#
# This is for the main MCU (STM32F427) on the PX4 board
# See https://pixhawk.org/modules/pixhawk for details
#
BOARD=px4io
BOARD_VERSION=2.4
BOARD_CFG=\"boards/$(BOARD)_$(BOARD_VERSION).h\"
ARCH=stm32
$(TARGET).ARCHDIR = $(ARCH)
$(TARGET).LDSCRIPT=$(SRC_ARCH)/px4io_2.4.ld
# default flash mode is via usb dfu bootloader
# possibilities: DFU, SWD, PX4_BOOTLOADER
PX4_BL_PORT ?= "/dev/serial/by-id/usb-FTDI_*"
PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/px4io-v2.prototype"
FLASH_MODE ?= PX4_BOOTLOADER
$(TARGET).MAKEFILE = stm32
#
# default LED configuration
#
RADIO_CONTROL_LED ?= none
BARO_LED ?= none
AHRS_ALIGNER_LED ?= none
GPS_LED ?= none
SYS_TIME_LED ?= 1
FBW_MODE_LED ?= 3
#
# default UART configuration (modem, gps, spektrum)
#
RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART1
#
# default actuator configuration
#
# you can use different actuators by adding a configure option to your firmware section
# e.g. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm
@@ -11,6 +11,7 @@ ifeq ($(TARGET),fbw)
INTERMCU_PORT ?= UART2
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B57600
fbw.CFLAGS += -DFBW_MODE_LED=$(FBW_MODE_LED)
else
INTERMCU_PORT ?= UART5
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
@@ -4,18 +4,24 @@
ifeq ($(TARGET),fbw)
INTERMCU_PORT ?= UART3
INTERMCU_BAUD ?= B230400
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
fbw.CFLAGS += -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER) -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=$(INTERMCU_BAUD)
fbw.CFLAGS += -DINTER_MCU_FBW -DDOWNLINK
fbw.CFLAGS += -DFBW_MODE_LED=$(FBW_MODE_LED)
fbw.srcs += pprzlink/src/pprz_transport.c
fbw.srcs += subsystems/intermcu/intermcu_fbw.c
else
INTERMCU_PORT ?= UART3
INTERMCU_BAUD ?= B230400
INTERMCU_PORT_LOWER = $(shell echo $(INTERMCU_PORT) | tr A-Z a-z)
ap.CFLAGS += -DINTER_MCU_AP -DINTERMCU_LINK=$(INTERMCU_PORT_LOWER)
ap.CFLAGS += -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=B230400
ap.CFLAGS += -DUSE_$(INTERMCU_PORT) -D$(INTERMCU_PORT)_BAUD=$(INTERMCU_BAUD)
$(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/intermcu/intermcu_ap.h\" -DRADIO_CONTROL
$(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
RADIO_CONTROL_LED ?= none
ifneq ($(RADIO_CONTROL_LED),none)
$(TARGET).CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED)
endif
ap.srcs += subsystems/intermcu/intermcu_ap.c
ap.srcs += pprzlink/src/pprz_transport.c
@@ -167,8 +167,8 @@ else ifeq ($(BOARD), krooz)
BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c
# PX4FMU
else ifeq ($(BOARD), px4fmu)
BARO_BOARD_CFLAGS += -DUSE_I2C2
else ifeq ($(BOARD),$(filter $(BOARD),px4fmu))
BARO_BOARD_CFLAGS += -DUSE_I2C2
BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c2
BARO_BOARD_SRCS += peripherals/ms5611.c
BARO_BOARD_SRCS += peripherals/ms5611_i2c.c
+17
View File
@@ -229,6 +229,23 @@ test_telemetry.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_telemetry.srcs += $(COMMON_TELEMETRY_SRCS)
test_telemetry.srcs += test/test_telemetry.c
#
# test_datalink : Sends ALIVE and pong telemetry messages
#
# configuration
# MODEM_PORT :
# MODEM_BAUD :
#
test_datalink.ARCHDIR = $(ARCH)
test_datalink.CFLAGS += $(COMMON_TEST_CFLAGS)
test_datalink.srcs += $(COMMON_TEST_SRCS)
test_datalink.CFLAGS += $(COMMON_DATALINK_CFLAGS)
test_datalink.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
test_datalink.srcs += $(COMMON_DATALINK_SRCS)
test_datalink.srcs += $(COMMON_TELEMETRY_SRCS)
test_datalink.srcs += test/test_datalink.c
#
# test_math_trig_compressed: Test math trigonometric using compressed data
#
+7
View File
@@ -96,6 +96,13 @@
<board name="px4fmu_.*"/>
<board name="elle*"/>
</boards>
</mode>
<mode name="Px4 bootloader">
<variable name="FLASH_MODE" value="PX4_BOOTLOADER"/>
<boards>
<board name="px4fmu_.*"/>
<board name="px4io_.*"/>
</boards>
</mode>
<mode name="JTAG (OpenOCD)">
<variable name="FLASH_MODE" value="JTAG"/>
+31
View File
@@ -0,0 +1,31 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="px4io_flash">
<doc>
<description>Flashes the px4io f1 through the px4 bootloader.</description>
</doc>
<header>
<file name="px4io_flash.h"/>
</header>
<init fun="px4ioflash_init()"/>
<event fun="px4ioflash_event()"/>
<makefile target="ap">
<raw>
</raw>
<file name="px4io_flash.c"/>
<configure name="PX4IO_UART" default="uart6" case="upper|lower"/>
<configure name="PX4IO_BAUD" default="B1500000"/>
<define name="USE_$(PX4IO_UART_UPPER)"/>
<define name="PX4IO_UART" value="$(PX4IO_UART_LOWER)"/>
<define name="$(PX4IO_UART_UPPER)_BAUD" value="$(PX4IO_BAUD)"/>
<configure name="TELEM2_UART" default="uart3" case="upper|lower"/>
<configure name="TELEM2_BAUD" default="B115200"/>
<define name="USE_$(TELEM2_UART_UPPER)"/>
<define name="TELEM2_UART" value="$(TELEM2_UART_LOWER)"/>
<define name="$(TELEM2_UART_UPPER)_BAUD" value="$(TELEM2_BAUD)"/>
</makefile>
</module>
+25
View File
@@ -0,0 +1,25 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="spektrum_soft_bind" dir="spektrum_soft_bind">
<doc>
<description>Puts Spektrum in binding mode through software</description>
</doc>
<settings target="ap">
<dl_settings>
<dl_settings name="Spektrum soft bind">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="bind_soft_value" values="FALSE|TRUE" shortname="Bind Spektrum" module="spektrum_soft_bind/spektrum_soft_bind" handler="click"/>
</dl_settings>
</dl_settings>
</settings>
<header >
<file name="spektrum_soft_bind.h"/>
</header>
<init fun="spektrum_soft_bind_init()"/>
<makefile target="ap">
<file name="spektrum_soft_bind_ap.c"/>
</makefile>
<makefile target="fbw">
<file name="spektrum_soft_bind_fbw.c"/>
</makefile>
</module>
+60
View File
@@ -0,0 +1,60 @@
<?xml version="1.0"?>
<!-- $Id$
--
-- (c) 2006 Pascal Brisset, Antoine Drouin
--
-- This file is part of paparazzi.
--
-- paparazzi is free software; you can redistribute it and/or modify
-- it under the terms of the GNU General Public License as published by
-- the Free Software Foundation; either version 2, or (at your option)
-- any later version.
--
-- paparazzi is distributed in the hope that it will be useful,
-- but WITHOUT ANY WARRANTY; without even the implied warranty of
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-- GNU General Public License for more details.
--
-- You should have received a copy of the GNU General Public License
-- along with paparazzi; see the file COPYING. If not, write to
-- the Free Software Foundation, 59 Temple Place - Suite 330,
-- Boston, MA 02111-1307, USA.
-->
<!--
-- Attributes of root (Radio) tag :
-- name: name of RC
-- data_min: min width of a pulse to be considered as a data pulse
-- data_max: max width of a pulse to be considered as a data pulse
-- sync_min: min width of a pulse to be considered as a synchro pulse
-- sync_max: max width of a pulse to be considered as a synchro pulse
-- pulse_type: POSITIVE ( Futaba and others) | NEGATIVE (JR)
-- min, max and sync are expressed in micro-seconds
-->
<!--
-- Attributes of channel tag :
-- ctl: name of the command on the transmitter - only for displaying
-- function: logical command
-- average: (boolean) channel filtered through several frames (for discrete commands)
-- min: minimum pulse length (micro-seconds)
-- max: maximum pulse length (micro-seconds)
-- neutral: neutral pulse length (micro-seconds)
-- Note: a command may be reversed by exchanging min and max values
-->
<!DOCTYPE radio SYSTEM "radio.dtd">
<!-- Values set by experiment using X8R receiver SBus connection -->
<radio name="FrSky X9D + X8R receiver SBus connection" data_min="350" data_max="2500" sync_min ="8000" sync_max ="19000" pulse_type="POSITIVE">
<channel ctl="D" function="ROLL" min="990" neutral="1490" max="2015" average="0"/>
<channel ctl="C" function="PITCH" min="990" neutral="1490" max="2015" average="0"/>
<channel ctl="A" function="THROTTLE" min="990" neutral="990" max="2015" average="0"/>
<channel ctl="B" function="YAW" min="990" neutral="1490" max="2015" average="0"/>
<channel ctl="RSwtiches_Mix" function="MODE" min="1145" neutral="1500" max="1862" average="1"/>
<channel ctl="TILT" function="AUX2" min="990" neutral="990" max="2015" average="0"/>
<channel ctl="CH7" function="KILL" min="990" neutral="1490" max="2015" average="1"/>
<channel ctl="E" function="UNUSED2" min="990" neutral="1900" max="2015" average="1"/>
</radio>
@@ -20,6 +20,9 @@ SUBSYSTEM=="tty", ATTRS{product}=="FT232R USB UART", SYMLINK+="ftdi-serial", GRO
# Paparazzi STM32 Autopilot board USB CDC serial device
SUBSYSTEM=="tty", ATTRS{manufacturer}=="Paparazzi UAV", ATTRS{product}=="CDC Serial STM32", SYMLINK+="paparazzi/stm32-usb-serial"
# Paparazzi 3dr radio
SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", SYMLINK+="paparazzi/link", GROUP="plugdev"
# MaxStream xbee pro box
SUBSYSTEM=="tty", ATTRS{product}=="MaxStream PKG-U", SYMLINK+="paparazzi/xbee", GROUP="plugdev"
+1 -1
View File
@@ -1,5 +1,5 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Ap">
<mode name="default">
+2 -1
View File
@@ -27,7 +27,8 @@
<message name="AIR_DATA" period="2.3"/>
<message name="SURVEY" period="4.5"/>
<message name="OPTIC_FLOW_EST" period="0.5"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="FBW_STATUS" period="2"/>
</mode>
<mode name="ppm">