[fix] collection of fixes after AP/FBW merge

some errors after #2828
- fix all airframes using dual boards
- fix calls to ap_state or old imcu API
- fix some includes
- protect some code to compile test programs (without 'commands' API)

close #2840
This commit is contained in:
Gautier Hattenberger
2022-03-11 15:43:47 +01:00
committed by Freek van Tienen
parent 591d9bc6e4
commit 0b62eb89ef
43 changed files with 135 additions and 167 deletions
@@ -27,6 +27,7 @@
<configure name="MODEM_PORT" value="UART2"/>
<configure name="MODEM_BAUD" value="B57600"/>
</module>
<module name="telemetry" type="intermcu"/>
<module name="imu" type="px4fmu_v2.4"/>
<module name="gps" type="ublox"/>
@@ -159,6 +160,11 @@
<configure name="INTERMCU_PORT" value="UART2" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
</target>
</firmware>
@@ -436,11 +436,11 @@
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -290,11 +290,11 @@
<define name="ROTORSTARTER_OFF" value="MIN_PPRZ" />
<!-- <define name="AGR_CLIMB_PITCH" value="35" unit="deg" />-->
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
</section>
<servos>
@@ -198,11 +198,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -518,11 +518,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -526,11 +526,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -389,11 +389,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ" />
<define name="SERVO_HATCH_OPEN" value="0" />
<define name="SERVO_HATCH_CLOSED" value="-9600" />
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
</section>
<!-- ********************** SERVO MIXER **************************** -->
<!--For mixed controls -->
+5 -5
View File
@@ -483,11 +483,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -485,11 +485,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
@@ -287,11 +287,11 @@
<define name="SERVO_HATCH_CLOSED" value="-9600" />
<define name="AGR_CLIMB_PITCH" value="35" unit="deg" />
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />
<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />
<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
</section>
<commands>
@@ -437,11 +437,11 @@
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ" />
<define name="SERVO_HATCH_OPEN" value="0" />
<define name="SERVO_HATCH_CLOSED" value="-9600" />
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)" />
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)" />
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)" />
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)" />
</section>
<!-- ********************** SERVO MIXER **************************** -->
<section name="MIXER">
@@ -300,11 +300,11 @@ NOTES:
<define name="SERVO_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="SERVO_HATCH_OPEN" value="0"/>
<define name="SERVO_HATCH_CLOSED" value="-9600"/>
<define name="AirbrakesOff()" value="(ap_state->commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(ap_state->commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(ap_state->commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(ap_state->commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(ap_state->commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="AirbrakesOff()" value="(commands[COMMAND_BRAKE]=0)"/>
<define name="AirbrakesOn()" value="(commands[COMMAND_BRAKE]=SERVO_BRAKE_FULL)"/>
<!--<define name="Fly()" value="(commands[COMMAND_FORCECRASH]=0)" />-->
<!--<define name="ForceCrash()" value="(commands[COMMAND_FORCECRASH]=9600)" />-->
<define name="ThrottleHigh()" value="(commands[COMMAND_THROTTLE]>9600/2)"/>
<define name="SPOILERON_BRAKE_FULL" value="-MAX_PPRZ"/>
<define name="FLAPERON_BRAKE_FULL" value="MAX_PPRZ"/>
</section>
+1 -1
View File
@@ -214,7 +214,7 @@
</servos>
<section name="ServoPositions">
<define name="ThrottleHigh()" value="(imcu_get_command(COMMAND_THROTTLE)>9600/2)" />
<define name="ThrottleHigh()" value="(command_get(COMMAND_THROTTLE)>9600/2)" />
</section>
<!-- ********************* SERVO MIXER *************************** -->
+6
View File
@@ -19,6 +19,7 @@
<define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
<!-- in seconds-->
<module name="telemetry" type="transparent" />
<module name="telemetry" type="intermcu"/>
<module name="imu" type="px4fmu_v2.4"/>
<module name="gps" type="ublox" >
<configure name="GPS_BAUD" value="B57600"/>
@@ -91,6 +92,11 @@
<configure name="INTERMCU_PORT" value="UART2" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
</firmware>
<section name="MISC">
+6
View File
@@ -26,6 +26,7 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
<target name="ap" board="lisa_mx_2.1">
<module name="telemetry" type="xbee_api"/>
<module name="telemetry" type="intermcu"/>
<module name="imu" type="lisa_mx_v2.1"/>
<module name="gps" type="ublox">
<configure name="GPS_PORT" value="UART4"/>
@@ -68,6 +69,11 @@ The xml in master currently configures the LOGO600 as a pure model aircraft: mea
<module name="intermcu" type="uart">
<define name="REMAP_UART3" value="TRUE"/>
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
</target>
</firmware>
+6
View File
@@ -19,6 +19,7 @@
</target-->
<module name="telemetry" type="xbee_api"/>
<module name="telemetry" type="intermcu"/>
<module name="imu" type="mpu6000"/>
<module name="gps" type="ublox">
<define name="GPS_TIMEOUT" value="2"/>
@@ -72,6 +73,11 @@
</module>
<module name="intermcu" type="uart"/>
<module name="telemetry" type="intermcu"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
<module name="opa_controller"/>
<module name="heli_swashplate_mixing"/>
<module name="gps" type="ublox"/>
+6
View File
@@ -18,6 +18,7 @@ Flapping wing frame equiped with
<define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" />
<!-- in seconds-->
<module name="telemetry" type="xbee_api"/>
<module name="telemetry" type="intermcu"/>
<module name="imu" type="px4fmu_v2.4"/>
<module name="gps" type="ublox" >
<configure name="GPS_BAUD" value="B57600"/>
@@ -82,6 +83,11 @@ Flapping wing frame equiped with
<configure name="INTERMCU_PORT" value="UART2" />
<configure name="INTERMCU_BAUD" value="B1500000" />
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
</firmware>
<section name="MISC">
+10 -5
View File
@@ -9,7 +9,6 @@
<!-- ************************* FIRMWARE ************************* -->
<firmware name="fixedwing">
<define name="RADIO_CONTROL_NB_CHANNEL" value="8" />
<target name="ap" board="lisa_l_1.0">
<configure name="SEPARATE_FBW" value="1"/>
<configure name="AHRS_ALIGNER_LED" value="1"/>
@@ -20,10 +19,10 @@
<module name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART2"/>
</module>
<module name="radio_control" type="sbus_dual">
<configure name="SBUS1_PORT" value="UART3"/>
<configure name="SBUS2_PORT" value="UART5"/>
</module>
<!-- mapping with intermcu channel -->
<define name="RADIO_FLAPS" value="RADIO_AUX1"/>
<define name="RADIO_GAIN1" value="RADIO_AUX2"/>
<define name="RADIO_GAIN2" value="RADIO_AUX3"/>
<!-- AP -->
<module name="control"/>
<module name="navigation"/>
@@ -32,6 +31,7 @@
<configure name="MODEM_BAUD" value="B9600"/>
<configure name="MODEM_PORT" value="UART3"/>
</module>
<module name="telemetry" type="intermcu"/>
<module name="ins" type="xsens700">
<configure name="XSENS_PORT" value="uart1"/>
<configure name="XSENS_BAUD" value="B230400"/>
@@ -60,6 +60,11 @@
<module name="intermcu" type="uart">
<configure name="INTERMCU_PORT" value="UART1"/>
</module>
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="$(INTERMCU_PORT)"/>
<configure name="MODEM_BAUD" value="$(INTERMCU_BAUD)"/>
<define name="TELEMETRY_DISABLE_RX"/>
</module>
<!-- SERVO'S -->
<module name="actuators" type="pwm"/>
<define name="USE_SERVOS_7AND8"/>
+1 -5
View File
@@ -21,22 +21,18 @@
</includes>
<control_block name="actuators_ap">
<call fun="PPRZ_MUTEX_LOCK(ap_state_mtx)"/>
<call fun="AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed)"/>
<call fun="AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint)"/>
<call fun="AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint)"/>
<call fun="AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint)"/>
<call fun="AP_COMMAND_SET_CL(h_ctl_flaps_setpoint)"/>
<call fun="PPRZ_MUTEX_UNLOCK(ap_state_mtx)"/>
</control_block>
<control_block name="set_attitude_from_rc">
<call fun="PPRZ_MUTEX_LOCK(fbw_state_mtx)"/>
<call fun="AP_SETPOINT_ROLL(h_ctl_roll_setpoint, AUTO1_MAX_ROLL)"/>
<call fun="AP_SETPOINT_PITCH(h_ctl_pitch_setpoint, AUTO1_MAX_PITCH)"/>
<call fun="AP_SETPOINT_YAW_RATE(h_ctl_yaw_rate_setpoint, AUTO1_MAX_YAW_RATE)"/>
<call fun="AP_SETPOINT_THROTTLE(v_ctl_throttle_setpoint)"/>
<call fun="PPRZ_MUTEX_UNLOCK(fbw_state_mtx)"/>
</control_block>
<control_block name="attitude">
@@ -60,7 +56,7 @@
<call fun="nav_periodic_task()"/>
</control>
<control> <!-- only for display -->
<call fun="v_ctl_throttle_slewed = imcu_get_radio(RADIO_THROTTLE)"/>
<call fun="v_ctl_throttle_slewed = radio_control_get(RADIO_THROTTLE)"/>
</control>
<exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
</mode>
+1 -5
View File
@@ -26,22 +26,18 @@
</includes>
<control_block name="actuators_ap">
<call fun="PPRZ_MUTEX_LOCK(ap_state_mtx)"/>
<call fun="AP_COMMAND_SET_THROTTLE(v_ctl_throttle_slewed)"/>
<call fun="AP_COMMAND_SET_ROLL(-h_ctl_aileron_setpoint)"/>
<call fun="AP_COMMAND_SET_PITCH(h_ctl_elevator_setpoint)"/>
<call fun="AP_COMMAND_SET_YAW(h_ctl_rudder_setpoint)"/>
<call fun="AP_COMMAND_SET_CL(h_ctl_flaps_setpoint)"/>
<call fun="PPRZ_MUTEX_UNLOCK(ap_state_mtx)"/>
</control_block>
<control_block name="set_attitude_from_rc">
<call fun="PPRZ_MUTEX_LOCK(fbw_state_mtx)"/>
<call fun="AP_SETPOINT_ROLL(h_ctl_roll_setpoint, AUTO1_MAX_ROLL)"/>
<call fun="AP_SETPOINT_PITCH(h_ctl_pitch_setpoint, AUTO1_MAX_PITCH)"/>
<call fun="AP_SETPOINT_YAW_RATE(h_ctl_yaw_rate_setpoint, AUTO1_MAX_YAW_RATE)"/>
<call fun="AP_SETPOINT_THROTTLE(v_ctl_throttle_setpoint)"/>
<call fun="PPRZ_MUTEX_UNLOCK(fbw_state_mtx)"/>
</control_block>
<control_block name="attitude">
@@ -65,7 +61,7 @@
<call fun="nav_periodic_task()"/>
</control>
<control> <!-- only for display -->
<call fun="v_ctl_throttle_slewed = imcu_get_radio(RADIO_THROTTLE)"/>
<call fun="v_ctl_throttle_slewed = radio_control_get(RADIO_THROTTLE)"/>
</control>
<exception cond="RCLost() && autopilot_in_flight()" deroute="HOME"/>
</mode>
@@ -235,8 +235,8 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
<!-- TODO: make some movements on all defelctros as a sign we are in holding and to see if all works Maybe a separate testblock-->
<!-- <set var="waypoints[WP_START].x" value="WaypointX(WP_EL2) + 50"/> -->
<!-- <set var="waypoints[WP_START].y" value="WaypointY(WP_EL2) + 50"/> -->
<!-- <set value="0" var="ap_state->commands[COMMAND_CAMBER]"/> -->
<!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
<!-- <set value="0" var="commands[COMMAND_CAMBER]"/> -->
<!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
<call_once fun="AirbrakesOff()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/><!-- maybe not moving the plane would still make a test of defelction possible -->
</block>
@@ -265,7 +265,7 @@ TST-3 = -26° 35' 23.2" * 151° 50' 45.9"
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<!-- <set value="3000" var="ap_state->commands[COMMAND_CAMBER]"/> -->
<!-- <set value="3000" var="commands[COMMAND_CAMBER]"/> -->
<set value="0" var="autopilot.kill_throttle"/>
<set value="1" var="autopilot.launch"/>
<set value="0" var="autopilot.flight_time"/>
@@ -317,7 +317,7 @@ to “Airfield Home” and orbit for either a landing or regain of Data Link.
***************************************************** -->
<!-- maybe a HelperWaypoint to gettart n return from a mission Tracking to EL1
<set value="0" var="ap_state->commands[COMMAND_CAMBER]"/>-->
<set value="0" var="commands[COMMAND_CAMBER]"/>-->
<!-- *********** Track to EL-1 *********** -->
<block name="EntryLane1" strip_button="Joe" strip_icon="lookdown.png" group="Mission">
@@ -441,7 +441,7 @@ extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t e
#define NavDropComputeApproach(_target, _start, _radius) nav_drop_compute_approach(_target, _start, _radius)
#define NavDropUpdateRelease(_wp) nav_drop_update_release(_wp)
#define NavDropShoot() nav_drop_shoot()
#define NavDropCloseHatch() ({ ap_state->commands[COMMAND_HATCH] = MIN_PPRZ; })
#define NavDropCloseHatch() ({ commands[COMMAND_HATCH] = MIN_PPRZ; })
#define NavDropAligned() Qdr(DegOfRad(nav_drop_qdr_aligned)) -->
@@ -468,7 +468,7 @@ extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t e
<!-- *********** Release Payload *********** -->
<block name="shoot">
<!-- TODO maybe extend crowbreak 80%? for slowes possible speed? -->
<!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
<!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
<!-- TODO is electrcal plane and almost there maybe stop proppelor from spinning for a few seconds -->
<call_once fun="NavDropShoot()"/>
<!-- TODO add maxspeed again -->
@@ -10,7 +10,7 @@
<blocks>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<call fun="Fly()"/>
<!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
<!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
<circle alt="WaypointAlt(WP_STDBY)" radius="nav_radius" wp="STDBY"/>
</block>
<!--Below routes that are handy for testing only when in the field and performing several tests -->
@@ -131,11 +131,11 @@ Should be unified for Hybrid, FW and rotorcraft
<!--<while cond="!GpsFixValid() || gps.pacc @GT 1500 || stateIsAttitudeValid()"/>-->
<while cond="!GpsFixValid()"/>
<!-- Wiggle Wiggle when we have GPS , time to fly...-->
<!--<set var="ap_state->commands[COMMAND_YAW]" value="-MAX_PPRZ"/>
<!--<set var="commands[COMMAND_YAW]" value="-MAX_PPRZ"/>
<while cond="LessThan(NavBlockTime(), 2)"/>
<set var="ap_state->commands[COMMAND_YAW]" value="MAX_PPRZ"/>
<set var="commands[COMMAND_YAW]" value="MAX_PPRZ"/>
<while cond="LessThan(NavBlockTime(), 3)"/>
<set var="ap_state->commands[COMMAND_YAW]" value="-MAX_PPRZ"/>-->
<set var="commands[COMMAND_YAW]" value="-MAX_PPRZ"/>-->
</block>
<!-- *********** Set the ground reference height and the home position *********** -->
@@ -165,8 +165,8 @@ Should be unified for Hybrid, FW and rotorcraft
-->
<block name="ReadyForDeparture" >
<!-- <set value="0" var="ap_state->commands[COMMAND_CAMBER]"/> -->
<!-- <set value="0" var="ap_state->commands[COMMAND_BRAKE]"/> -->
<!-- <set value="0" var="commands[COMMAND_CAMBER]"/> -->
<!-- <set value="0" var="commands[COMMAND_BRAKE]"/> -->
<set var="v_ctl_speed_mode" value="V_CTL_SPEED_AIRSPEED" /><!-- set the preferred flight mode -->
<!-- Must have an RC for this flightplan and on at least for the start, we wait till the throttle is high -->
<exception cond="ThrottleHigh()" deroute="Takeoff"/>
@@ -174,7 +174,7 @@ Should be unified for Hybrid, FW and rotorcraft
</block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png" group="Ops">
<!-- <set var="ap_state->commands[COMMAND_LIGHT]" value="PPRZ_MAX"/> -->
<!-- <set var="commands[COMMAND_LIGHT]" value="PPRZ_MAX"/> -->
<set var="autopilot.kill_throttle" value="0"/>
<!--<var="autopilot.launch" set value="1" />-->
<while cond="1 @GT autopilot.launch"/> <!-- Setting of launch performed with takeoff_detect module disable -->
+1
View File
@@ -28,6 +28,7 @@
<include name="$(SRC_MODULES)"/>
<file name="sys_time.c" dir="mcu_periph"/>
<file name="commands.c"/>
<define name="USE_COMMANDS"/>
<file_arch name="sys_time_arch.c" dir="mcu_periph"/>
<file_arch name="led_hw.c" dir="." cond="ifeq ($(ARCH), stm32)"/>
<file_arch name="led_hw.c" dir="." cond="ifeq ($(TARGET), sim)"/>
-5
View File
@@ -6,11 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
-6
View File
@@ -6,12 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
<dl_setting MAX="9000" MIN="-9000" STEP="1" VAR="ap_state->command_yaw_trim" shortname="yaw_trim" param="COMMAND_YAW_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="60" MIN="0" STEP="1." VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
-5
View File
@@ -6,11 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="25000" MIN="000" STEP="250" VAR="h_ctl_roll_pgain" shortname="roll_pgain" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="60" MIN="0" STEP="1." VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
@@ -6,13 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
-5
View File
@@ -6,11 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
@@ -6,11 +6,6 @@
<dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="trim">
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="modules/intermcu/inter_mcu" param="COMMAND_ROLL_TRIM"/>
<dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
</dl_settings>
<dl_settings NAME="attitude">
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
<dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
@@ -27,7 +27,6 @@
*/
#include "modules/radio_control/radio_control.h"
#include "modules/radio_control/spektrum_arch.h"
#include "modules/radio_control/spektrum.h"
#include "modules/core/abi.h"
#include "std.h"
@@ -1,32 +0,0 @@
/*
* Copyright (C) 2010 Eric Parsonage <eric@eparsonage.com>
*
* 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.
*
*/
#ifndef RADIO_CONTROL_SPEKTRUM_ARCH_H
#define RADIO_CONTROL_SPEKTRUM_ARCH_H
#include "modules/radio_control/spektrum_radio.h"
#if USE_NPS
extern void radio_control_feed(void);
#endif
#endif /* RADIO_CONTROL_SPEKTRUM_ARCH_H */
+1 -1
View File
@@ -136,7 +136,7 @@ extern float baseleg_out_qdr;
extern void nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius);
extern void nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide);
#define RCLost() bit_is_set(imcu_get_status(), STATUS_RADIO_REALLY_LOST)
#define RCLost() RadioControlIsLost()
extern void nav_follow(uint8_t _ac_id, float _distance, float _height);
#define NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height)
-9
View File
@@ -37,15 +37,6 @@
#include "modules/datalink/datalink.h"
#include "modules/datalink/telemetry.h"
/* So one can use these in command_laws section */
#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define LessThan(_x, _y) ((_x) < (_y))
#define MoreThan(_x, _y) ((_x) > (_y))
/** Fly by wire modes */
uint8_t fbw_mode;
bool fbw_motors_on = false;
@@ -87,6 +87,7 @@ void actuators_init(void)
*/
void actuators_periodic(void)
{
#if USE_COMMANDS
pprz_t trimmed_commands[COMMANDS_NB];
int i;
for (i = 0; i < COMMANDS_NB; i++) {trimmed_commands[i] = commands[i];}
@@ -113,6 +114,7 @@ void actuators_periodic(void)
SetActuatorsFromCommands(trimmed_commands, autopilot_get_mode());
// TODO SetApOnlyActuatorsFromCommands(ap_commands, autopilot_get_mode());
#endif
#endif // USE_COMMANDS
}
#else // No command_laws section or no actuators
+4
View File
@@ -31,6 +31,8 @@
#include "paparazzi.h"
#include "generated/airframe.h"
#if COMMANDS_NB // commands are defined and nb is > 0
/** Storage of intermediate command values.
* These values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
* They are asyncronisly used to set the servos
@@ -74,6 +76,8 @@ static inline pprz_t command_get(uint8_t idx)
return 0; // is it the best value ???
}
#endif
extern void commands_init(void);
#endif /* COMMANDS_H */
+4
View File
@@ -30,7 +30,9 @@
#include "modules/datalink/telemetry_common.h"
#include "modules/datalink/telemetry.h"
#include "generated/periodic_telemetry.h"
#if FIXEDWING_FIRMWARE || ROTORCRAFT_FIRMWARE || ROVER_FIRMWARE
#include "autopilot.h"
#endif
/* Implement global structures from generated header.
@@ -74,6 +76,7 @@ int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id,
*/
void telemetry_reporting_task(void)
{
#if FIXEDWING_FIRMWARE || ROTORCRAFT_FIRMWARE || ROVER_FIRMWARE
static uint8_t boot = true;
/* initialisation phase during boot */
@@ -96,6 +99,7 @@ void telemetry_reporting_task(void)
periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device);
#endif
}
#endif
}
@@ -56,13 +56,6 @@ void nav_goto_block(uint8_t block_id);
#define Goto(x) { goto label_ ## x; }
#define Return(x) { nav_block=last_block; if (x==1) {nav_stage=0;} else {nav_stage=last_stage;} block_time=0;}
#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define LessThan(_x, _y) ((_x) < (_y))
#define MoreThan(_x, _y) ((_x) > (_y))
/** Time in s since the entrance in the current block */
#define NavBlockTime() (block_time)
@@ -12,6 +12,7 @@
#include "modules/px4_gimbal/px4_gimbal.h"
#include "modules/radio_control/radio_control.h"
#include "generated/modules.h" // for RC channels definition
#include "generated/airframe.h" // AC_ID is required
#include "modules/actuators/actuators.h"
@@ -67,4 +67,8 @@ extern void spektrum_init(void);
extern void spektrum_event(void);
extern void spektrum_try_bind(void);
#if USE_NPS
extern void radio_control_feed(void);
#endif
#endif /* RADIO_CONTROL_SPEKTRUM_H */
@@ -24,6 +24,7 @@
#include "generated/airframe.h"
#include "autopilot.h"
#include "modules/core/commands.h"
#include "generated/modules.h" // for radio_control channels
void periodic_auto1_commands(void)
{
+4
View File
@@ -93,8 +93,12 @@ typedef uint8_t unit_t;
#define MOfCm(_x) (((float)(_x))/100.)
#define MOfMm(_x) (((float)(_x))/1000.)
#define And(x, y) ((x) && (y))
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define LessThan(_x, _y) ((_x) < (_y))
#define MoreThan(_x, _y) ((_x) > (_y))
#ifndef ABS
#define ABS(val) ((val) < 0 ? -(val) : (val))
+2
View File
@@ -1089,6 +1089,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
List.map (fun w -> incr i; (name_of w, !i)) waypoints in
(* print sectors *)
lprintf out "\n#ifndef FBW\n\n"; (* workaround to hide sector functions on FBW side *)
let sectors_element = try ExtXml.child xml "sectors" with Not_found -> Xml.Element ("", [], []) in
let sectors = List.filter (fun x -> String.lowercase_ascii (Xml.tag x) = "sector") (Xml.children sectors_element) in
List.iter (fun x -> match ExtXml.attrib_opt x "type" with
@@ -1106,6 +1107,7 @@ let print_flight_plan_h = fun xml ref0 xml_file out_file ->
with
_ -> ()
end;
lprintf out "\n#endif\n"; (* workaround to hide sector functions on FBW side *)
(* start "C" part *)
lprintf out "\n#ifdef NAV_C\n\n";