Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 19:36:19 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[control] rework the stabilization for rotorcraft (#3248)

Browse Source 
Introducing several changes to the control stask for the stabilization:
- new structures: stabilization, stab (att and/or rates) and thrust (vector)
- more functional approach: stabilization receives the stab and thrust
  setpoints from guidance and fill command vector at the end
- modes and sub-modes are redefined (split stab and guidance modes),
  some AP modes are removed (FLIP and MODULE)
- RC read functions are removed and replaced by ABI bindings, one for
  each part (AP, stab, guidance_h, guidance_v) with dedicated functions
- transition ratio (float in [0-1]) replace transition_percentage (int)
- the AP_MODULE mode is replaced by generated autopilot
- the ABI hack for guidance indi is not needed anymore
- AP generator supports a new 'store' attribute to use return values of
  functions
- cleaning of the stabilization API (for setter functions)
This commit is contained in:
Gautier Hattenberger
2024-05-21 17:01:38 +02:00
committed by GitHub
parent f41c32a4bf
commit ce7fe4c23f
116 changed files with 2280 additions and 2533 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/hybrid/falcon_v1.xml
+2
View File
@@ -297,6 +297,8 @@
<section name="NAV">
<define name="NAV_CLIMB_VSPEED" value="1.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.8"/>
<define name="NAV_HYBRID_MAX_AIRSPEED" value="GUIDANCE_INDI_MAX_AIRSPEED"/>
<define name="NAV_HYBRID_POS_GAIN" value="GUIDANCE_INDI_POS_GAIN"/>
</section>
<section name="AHRS" prefix="AHRS_">
conf/airframes/ENAC/hybrid/falcon_v2.xml
+7 -26
View File
@@ -237,9 +237,6 @@
<define name="REF_RATE_Q" value="18.0"/>
<define name="REF_RATE_R" value="11.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="60.0" unit="deg/s"/>
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
<define name="FILT_CUTOFF" value="5.0"/>
@@ -253,6 +250,10 @@
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="60" unit="deg"/>
</section>
<section name="GUIDANCE_INDI">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
@@ -272,6 +273,7 @@
<define name="GUIDANCE_INDI_PITCH_OFFSET_GAIN" value="0.06"/>
<define name="GUIDANCE_INDI_FILTER_CUTOFF" value="1.5"/>
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.7"/>
<define name="FWD_SIDESLIP_GAIN" value="0.20"/>
<!--Flap effectiveness on lift-->
<define name="FE_LIFT_A_PITCH" value="0.00018"/>
<define name="FE_LIFT_B_PITCH" value="0.00072"/>
@@ -279,15 +281,6 @@
<define name="FE_LIFT_B_AS" value="0.00009"/>
</section>
<!-- Gains for vertical navigation -->
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="174"/>
<define name="HOVER_KD" value="92"/>
<define name="HOVER_KI" value="72"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.4"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="NAV">
<define name="NAV_CLIMB_VSPEED" value="1.5"/>
<define name="NAV_DESCEND_VSPEED" value="-1.5"/>
@@ -297,6 +290,7 @@
<define name="NAV_LANDING_DESCEND_SPEED" value="-1.0"/>
<define name="SURVEY_HYBRID_MAX_SWEEP_BACK" value="1"/>
<define name="MISSION_ALT_PROXIMITY" value="4."/>
<define name="NAV_HYBRID_LINE_GAIN" value="0.8"/>
</section>
<section name="AHRS" prefix="AHRS_">
@@ -305,18 +299,9 @@
<include href="conf/mag/toulouse_muret.xml"/>
<!-- Gains for horizontal navigation-->
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="50" unit="deg"/>
<define name="PGAIN" value="100"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="10"/>
</section>
<section name="MISC">
<!--The Quadshot uses (when TRUE) a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="COORDINATED_TURN_AIRSPEED" value="18.0"/>
@@ -324,11 +309,7 @@
<define name="BARO_PERIODIC_FREQUENCY" value="50"/>
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="FWD_SIDESLIP_GAIN" value="0.20"/> <!-- most flight done with 0.3 -->
<define name="EFF_SCHED_USE_FUNCTION" value="TRUE"/>
<define name="ARRIVED_AT_WAYPOINT" value="4.0"/> <!-- For outdoor -->
<define name="ARRIVED_AT_WAYPOINT" value="5.0"/> <!-- For outdoor -->
<define name="DEFAULT_CIRCLE_RADIUS" value="60"/> <!-- For outdoor -->
<define name="CARROT" value="3.0"/>
</section>
conf/airframes/examples/bebop2_opticflow.xml
+1
View File
@@ -9,6 +9,7 @@
</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_control_loop"/>
<target name="ap" board="bebop">
<define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+2
View File
@@ -3,6 +3,8 @@
<airframe name="bebop2_optitrack_visionfront">
<firmware name="rotorcraft">
<autopilot name="rotorcraft_control_loop"/>
<target name="ap" board="bebop2"/>
<module name="telemetry" type="transparent_udp"/>
conf/airframes/tudelft/bebop_OF_hover.xml
+4 -2
View File
@@ -2,6 +2,8 @@
<airframe name="tudelft_tb_bebop">
<firmware name="rotorcraft">
<autopilot name="rotorcraft_control_loop"/>
<target name="ap" board="bebop"/>
<define name="USE_SONAR" value="FALSE" />
@@ -210,10 +212,10 @@
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<!--define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/-->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
conf/airframes/tudelft/guido_ardrone2_optitrack.xml
+2
View File
@@ -6,6 +6,8 @@ ARDrone2 with optical_flow landing.
</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_control_loop"/>
<target name="ap" board="ardrone2">
<configure name="USE_BARO_BOARD" value="FALSE"/>
</target>
conf/autopilot/autopilot.dtd
+2 -1
View File
@@ -65,7 +65,8 @@ name CDATA #REQUIRED>
<!ATTLIST call
fun CDATA #REQUIRED
cond CDATA #IMPLIED>
cond CDATA #IMPLIED
store CDATA #IMPLIED>
<!ATTLIST call_block
name CDATA #REQUIRED>
conf/autopilot/rotorcraft_autopilot.xml
+39 -54
View File
@@ -10,7 +10,7 @@
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="stabilization.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
@@ -29,25 +29,18 @@
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="attitude_loop">
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="throttle_direct">
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block>
<control_block name="altitude_loop">
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
<!--call fun="SaturateThrottle(rc_values)"/-->
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<exceptions>
@@ -58,14 +51,15 @@
<mode name="ATTITUDE_DIRECT" shortname="ATT">
<select cond="RCMode0()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="attitude_loop"/>
<call_block name="throttle_direct"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -74,15 +68,15 @@
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<select cond="RCMode1()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="attitude_loop"/>
<call_block name="altitude_loop"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -91,18 +85,15 @@
<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -111,19 +102,15 @@
<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -131,18 +118,15 @@
<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
@@ -151,17 +135,15 @@
<!-- Safe landing -->
<mode name="FAILSAFE" shortname="FAIL">
<on_enter>
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
</on_enter>
<control>
<call fun="stabilization_attitude_set_failsafe_setpoint()"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -172,9 +154,12 @@
<select cond="$DEFAULT_MODE"/>
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
<!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
conf/autopilot/rotorcraft_control_loop.xml
+147
View File
@@ -0,0 +1,147 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="Quadrotor Autopilot (Basic version)">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="stabilization.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
<mode name="ATTITUDE_DIRECT" shortname="ATT">
<select cond="RCMode0()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="MODULE">
<select cond="RCMode1()"/>
<on_enter>
<call fun="guidance_module_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_module_run(autopilot_in_flight())"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<mode name="NAV">
<select cond="RCMode2()" exception="HOME"/>
<select cond="$DEFAULT_MODE"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
<mode name="HOME">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- Safe landing -->
<mode name="FAILSAFE" shortname="FAIL">
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
</on_enter>
<control>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>
<!-- Kill throttle mode -->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
</state_machine>
</autopilot>
conf/autopilot/rotorcraft_oneloop.xml
+59 -48
View File
@@ -11,7 +11,7 @@
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="oneloop/oneloop_andi.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="stabilization.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
@@ -31,7 +31,18 @@
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<exceptions>
@@ -43,13 +54,16 @@
<select cond="RCMode0()"/>
<select cond="$DEFAULT_MODE"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
@@ -57,57 +71,49 @@
<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
</mode>
<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<select cond="RCMode1() && DLModeAZH()"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -129,16 +135,16 @@
<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
@@ -147,13 +153,15 @@
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -163,9 +171,12 @@
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
<!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
conf/autopilot/rotorcraft_oneloop_with_backup.xml
+42 -53
View File
@@ -11,7 +11,7 @@
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="oneloop/oneloop_andi.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="stabilization.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
@@ -31,24 +31,18 @@
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="attitude_loop">
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="throttle_direct">
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block>
<control_block name="altitude_loop">
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/>
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<exceptions>
@@ -59,14 +53,15 @@
<mode name="ATTITUDE_DIRECT" shortname="ATT">
<select cond="RCMode0()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call_block name="attitude_loop"/>
<call_block name="throttle_direct"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
@@ -75,60 +70,49 @@
<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v_z_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
</mode>
<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode>
</mode>
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<select cond="RCMode1() && DLModeAZH()"/>
<on_enter>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_HOVER)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
@@ -150,16 +134,16 @@
<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/>
<call_block name="run_guidance_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
@@ -168,13 +152,15 @@
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
@@ -184,9 +170,12 @@
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
<!--call fun="stabilization.cmd[COMMAND_THRUST] = 0"/-->
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
conf/modules/ctrl_module_innerloop_demo.xml
+2
View File
@@ -22,9 +22,11 @@
<header>
<file name="ctrl_module_innerloop_demo.h"/>
</header>
<init fun="ctrl_module_init()"/>
<makefile>
<file name="ctrl_module_innerloop_demo.c"/>
<test/>
</makefile>
</module>
conf/modules/ctrl_module_outerloop_demo.xml
+2
View File
@@ -19,9 +19,11 @@
<header>
<file name="ctrl_module_outerloop_demo.h"/>
</header>
<init fun="ctrl_module_init()"/>
<makefile>
<file name="ctrl_module_outerloop_demo.c"/>
<test firmware="rotorcraft"/>
</makefile>
</module>
conf/modules/ctrl_windtunnel.xml
+10 -4
View File
@@ -37,13 +37,19 @@
<header>
<file name="ctrl_windtunnel.h"/>
</header>
<init fun="ctrl_windtunnel_init()"/>
<makefile>
<file name="ctrl_windtunnel.c"/>
</makefile>
<makefile target="ap">
<define name="GUIDANCE_V_MODE_MODULE_SETTING" value="GUIDANCE_V_MODE_HOVER"/>
<define name="DGUIDANCE_H_MODE_MODULE_SETTING" value="GUIDANCE_H_MODE_MODULE"/>
<test>
<define name="CTRL_WINDTUNNEL_STEPTIME" value="8.0"/>
<define name="CTRL_WINDTUNNEL_THR_MIN" value="0"/>
<define name="CTRL_WINDTUNNEL_THR_MAX" value="9600"/>
<define name="CTRL_WINDTUNNEL_THR_STEP" value="1200"/>
<define name="CTRL_WINDTUNNEL_FLAP_MIN" value="0"/>
<define name="CTRL_WINDTUNNEL_FLAP_MAX" value="9600"/>
<define name="CTRL_WINDTUNNEL_FLAP_STEP" value="2400"/>
</test>
</makefile>
</module>
conf/modules/guidance_rotorcraft.xml
+3 -2
View File
@@ -7,7 +7,6 @@
It provides:
- horizontal guidance with reference
- vertical guidance with reference and adaptive control
- flip mode
</description>
</doc>
<settings target="ap|nps">
@@ -43,6 +42,8 @@
<file name="guidance_v.c" dir="$(SRC_FIRMWARE)/guidance"/>
<file name="guidance_v_ref.c" dir="$(SRC_FIRMWARE)/guidance"/>
<file name="guidance_v_adapt.c" dir="$(SRC_FIRMWARE)/guidance"/>
<file name="guidance_flip.c" dir="$(SRC_FIRMWARE)/guidance"/>
<test firmware="rotorcraft">
<define name="PERIODIC_FREQUENCY" value="500"/>
</test>
</makefile>
</module>
conf/modules/hard_fault_recovery.xml
-1
View File
@@ -22,7 +22,6 @@
<file name="main_recovery.c" dir="firmwares/fixedwing"/>
<test firmware="fixedwing">
<define name="RADIO_CONTROL"/>
<define name="RADIO_MODE" value="0"/>
<define name="DOWNLINK_DEVICE" value="uart1"/>
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="USE_UART1"/>
conf/modules/obstacle_avoidance.xml
-6
View File
@@ -63,12 +63,6 @@
<makefile>
<file name="obstacle_avoidance.c"/>
<file name="guidance_OA.c"/>
<raw>
ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
</raw>
</makefile>
</module>
conf/modules/optical_flow_hover.xml
+7
View File
@@ -90,6 +90,13 @@
<makefile target="ap">
<file name="optical_flow_hover.c" />
<file name="optical_flow_functions.c" />
<test firmware="rotorcraft">
<define name="COV_WINDOW_SIZE" value="300"/>
<define name="GUIDANCE_H_MAX_BANK" value="0.5"/>
<define name="GUIDANCE_H_PGAIN" value="1"/>
<define name="GUIDANCE_H_IGAIN" value="1"/>
<define name="GUIDANCE_H_DGAIN" value="1"/>
</test>
</makefile>
</module>
conf/modules/optical_flow_landing.xml
+2
View File
@@ -85,11 +85,13 @@
<header>
<file name="optical_flow_landing.h"/>
</header>
<init fun="optical_flow_landing_init()"/>
<makefile target="ap|nps">
<file name="optical_flow_landing.c"/>
<file name="pprz_algebra_float.c" dir="math"/>
<file name="pprz_matrix_decomp_float.c" dir="math"/>
<test firmware="rotorcraft"/>
</makefile>
</module>
conf/modules/opticflow_hover.xml
+3 -6
View File
@@ -23,7 +23,7 @@
</doc>
<settings>
<dl_settings NAME="Vision stabilization">
<dl_settings NAME="Vision stabilization">
<!-- Satabilization loop parameters and gains -->
<dl_settings name="vision_stab">
<dl_setting var="opticflow_stab.phi_pgain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
@@ -43,13 +43,10 @@
<header>
<file name="guidance_opticflow_hover.h"/>
</header>
<init fun="guidance_opticflow_hover_init()"/>
<makefile>
<file name="guidance_opticflow_hover.c"/>
<raw>
ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
</raw>
<test firmware="rotorcraft"/>
</makefile>
</module>
conf/modules/stabilization_float_euler.xml
+1 -4
View File
@@ -62,14 +62,11 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude.h"/>
<file name="stabilization_attitude_euler_float.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<init fun="stabilization_attitude_euler_float_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_euler_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_ref_euler_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_TYPE_FLOAT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_euler_float.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_float_quat.xml
+1 -4
View File
@@ -68,14 +68,11 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude.h"/>
<file name="stabilization_attitude_quat_float.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<init fun="stabilization_attitude_quat_float_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_quat_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_ref_quat_float.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_TYPE_FLOAT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_quat_float.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_heli_indi.xml
+2 -4
View File
@@ -51,13 +51,11 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude.h"/>
<file name="stabilization_attitude_heli_indi.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<init fun="stabilization_attitude_heli_indi_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_heli_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_heli_indi.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_indi.xml
+1 -3
View File
@@ -70,7 +70,7 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_indi.h"/>
<file name="stabilization_attitude_quat_indi.h"/>
</header>
<init fun="stabilization_indi_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
@@ -80,8 +80,6 @@
<configure name="INDI_NUM_ACT" default="4"/>
<define name="INDI_OUTPUTS" value="$(INDI_OUTPUTS)"/>
<define name="INDI_NUM_ACT" value="$(INDI_NUM_ACT)"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
<define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
</makefile>
</module>
conf/modules/stabilization_int_euler.xml
+1 -4
View File
@@ -59,14 +59,11 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude.h"/>
<file name="stabilization_attitude_euler_int.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<init fun="stabilization_attitude_euler_int_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_euler_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_ref_euler_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_euler_int.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_int_quat.xml
+1 -4
View File
@@ -65,14 +65,11 @@
<provides>commands</provides>
</dep>
<header>
<file name="stabilization_attitude.h"/>
<file name="stabilization_attitude_quat_int.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<init fun="stabilization_attitude_quat_int_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_quat_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_ref_quat_int.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization_attitude_quat_int.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_oneloop.xml
-2
View File
@@ -15,7 +15,5 @@
</header>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_oneloop.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<!-- <define name="STABILIZATION_ATTITUDE_TYPE_INT"/> -->
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_oneloop.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_passthrough.xml
-3
View File
@@ -13,12 +13,9 @@
<header>
<file name="stabilization_attitude_passthrough.h"/>
</header>
<init fun="stabilization_attitude_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization_attitude_passthrough.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<define name="STABILIZATION_ATTITUDE_NO_REF"/>
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_passthrough.h" type="string"/>
</makefile>
</module>
conf/modules/stabilization_rotorcraft.xml
+6 -3
View File
@@ -20,14 +20,17 @@
</dep>
<header>
<file name="stabilization.h" dir="firmwares/rotorcraft"/>
<file name="stabilization_none.h"/>
<file name="stabilization_direct.h"/>
</header>
<init fun="stabilization_init()"/>
<init fun="stabilization_none_init()"/>
<init fun="stabilization_direct_init()"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="stabilization.c" dir="$(SRC_FIRMWARE)"/>
<file name="stabilization_none.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_direct.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_rc_setpoint.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<file name="stabilization_attitude_quat_transformations.c" dir="$(SRC_FIRMWARE)/stabilization"/>
<test firmware="rotorcraft">
<define name="PERIODIC_FREQUENCY" value="500"/>
</test>
</makefile>
</module>
conf/modules/vertical_ctrl_module_demo.xml
+2
View File
@@ -26,9 +26,11 @@
<header>
<file name="vertical_ctrl_module_demo.h"/>
</header>
<init fun="vertical_ctrl_module_init()"/>
<makefile>
<file name="vertical_ctrl_module_demo.c"/>
<test firmware="rotorcraft"/>
</makefile>
</module>
conf/userconf/tudelft/conf.xml
+1 -1
View File
@@ -469,7 +469,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft
sw/airborne/boards/bebop/actuators.c
+4 -4
View File
@@ -39,10 +39,10 @@
static void send_bebop_actuators(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_BEBOP_ACTUATORS(trans, dev, AC_ID,
&stabilization_cmd[COMMAND_THRUST],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&stabilization.cmd[COMMAND_THRUST],
&stabilization.cmd[COMMAND_ROLL],
&stabilization.cmd[COMMAND_PITCH],
&stabilization.cmd[COMMAND_YAW],
&actuators_bebop.rpm_ref[0],
&actuators_bebop.rpm_ref[1],
&actuators_bebop.rpm_ref[2],
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+8 -6
View File
@@ -92,7 +92,7 @@ bool WEAK autopilot_ground_detection(void) {
bool WEAK autopilot_in_flight_end_detection(bool motors_on UNUSED) {
if (autopilot_in_flight_counter > 0) {
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
if ((stabilization.cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
autopilot_in_flight_counter--;
@@ -229,10 +229,10 @@ static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *d
static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID,
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_THRUST]);
&stabilization.cmd[COMMAND_ROLL],
&stabilization.cmd[COMMAND_PITCH],
&stabilization.cmd[COMMAND_YAW],
&stabilization.cmd[COMMAND_THRUST]);
}
#else
static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_device *dev UNUSED) {}
@@ -242,7 +242,9 @@ static void send_rotorcraft_cmd(struct transport_tx *trans UNUSED, struct link_d
void autopilot_firmware_init(void)
{
autopilot_in_flight_counter = 0;
#ifdef MODE_AUTO2
autopilot_mode_auto2 = MODE_AUTO2;
#endif
// register messages
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
@@ -296,7 +298,7 @@ void autopilot_check_in_flight(bool motors_on)
/* if thrust above min threshold, assume in_flight.
* Don't check for velocity and acceleration above threshold here...
*/
if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
if (stabilization.cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
autopilot_in_flight_counter++;
if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) {
autopilot.in_flight = true;
sw/airborne/firmwares/rotorcraft/autopilot_generated.c
-3
View File
@@ -125,9 +125,6 @@ void autopilot_generated_on_rc_frame(void)
// SetCommandsFromRC(commands, radio_control.values);
// }
//#endif
//
// guidance_v_read_rc();
// guidance_h_read_rc(autopilot.in_flight);
// }
}
sw/airborne/firmwares/rotorcraft/autopilot_guided.c
+56
View File
@@ -32,6 +32,7 @@
#include "state.h"
#include "pprzlink/dl_protocol.h"
#ifdef AP_MODE_GUIDED
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
{
@@ -162,3 +163,58 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) {
DL_GUIDED_SETPOINT_NED_yaw(buf));
}
#else
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
{
(void) x;
(void) y;
(void) z;
(void) heading;
return false;
}
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
{
(void) dx;
(void) dy;
(void) dz;
(void) dyaw;
return false;
}
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
{
(void) dx;
(void) dy;
(void) dz;
(void) dyaw;
return false;
}
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
{
(void) vx;
(void) vy;
(void) vz;
(void) heading;
return false;
}
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
{
(void) flags;
(void) x;
(void) y;
(void) z;
(void) yaw;
}
/** Parse GUIDED_SETPOINT_NED messages from datalink
*/
void autopilot_guided_parse_GUIDED(uint8_t *buf) {
(void) buf;
}
#endif
sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h
+4
View File
@@ -53,6 +53,10 @@
(radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \
radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD)
// macros with pointer to radio control struct
#define THROTTLE_STICK_DOWN_FROM_RC(_rc) \
(_rc->values[RADIO_THROTTLE] < AUTOPILOT_THROTTLE_THRESHOLD)
/** RC mode switch position helper
* switch positions threshold are evenly spaced
*
sw/airborne/firmwares/rotorcraft/autopilot_static.c
+56 -39
View File
@@ -39,7 +39,6 @@
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_none.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#if USE_STABILIZATION_RATE
@@ -158,14 +157,35 @@ void autopilot_static_periodic(void)
* If in FAILSAFE mode, run normal loops with failsafe attitude and
* downwards velocity setpoints.
*/
if (autopilot.mode == AP_MODE_KILL) {
SetCommands(commands_failsafe);
} else {
guidance_v_run(autopilot_in_flight());
guidance_h_run(autopilot_in_flight());
SetRotorcraftCommands(stabilization_cmd, autopilot.in_flight, autopilot.motors_on);
struct StabilizationSetpoint stab_sp;
struct ThrustSetpoint thrust_sp;
switch (autopilot.mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
thrust_sp = guidance_v_run(autopilot_in_flight());
stab_sp = stabilization_get_failsafe_sp();
stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd);
SetRotorcraftCommands(stabilization.cmd, autopilot.in_flight, autopilot.motors_on);
break;
#endif
case AP_MODE_KILL:
SetCommands(commands_failsafe);
break;
default:
thrust_sp = guidance_v_run(autopilot_in_flight());
if (guidance_h.mode != GUIDANCE_H_MODE_NONE) {
stab_sp = guidance_h_run(autopilot_in_flight());
} else {
stab_sp = stabilization.rc_sp;
}
stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd);
// TODO maybe add RC limiter here as an option ?
SetRotorcraftCommands(stabilization.cmd, autopilot.in_flight, autopilot.motors_on);
break;
}
#ifdef COMMAND_THRUST
autopilot.throttle = commands[COMMAND_THRUST];
#endif
}
@@ -180,7 +200,7 @@ void autopilot_static_SetModeHandler(float mode)
autopilot_static_set_mode(mode);
} else {
if (radio_control.status != RC_OK &&
(mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
(mode == AP_MODE_NAV || mode == AP_MODE_GUIDED)) {
// without RC, only nav-like modes are accessible
autopilot_static_set_mode(mode);
}
@@ -196,22 +216,26 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
switch (new_autopilot_mode) {
case AP_MODE_FAILSAFE:
#ifndef KILL_AS_FAILSAFE
stabilization_attitude_set_failsafe_setpoint();
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_HEADING);
break;
#endif
case AP_MODE_KILL:
autopilot_set_in_flight(false);
guidance_h_mode_changed(GUIDANCE_H_MODE_KILL);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_NONE, 0);
break;
case AP_MODE_RC_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_RC_DIRECT);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_DIRECT, 0);
break;
case AP_MODE_RATE_RC_CLIMB:
case AP_MODE_RATE_DIRECT:
case AP_MODE_RATE_Z_HOLD:
#if USE_STABILIZATION_RATE
guidance_h_mode_changed(GUIDANCE_H_MODE_RATE);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_RATE, 0);
#else
return;
#endif
@@ -220,33 +244,37 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_ATTITUDE_DIRECT:
case AP_MODE_ATTITUDE_CLIMB:
case AP_MODE_ATTITUDE_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_HEADING);
break;
case AP_MODE_FORWARD:
guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_FORWARD);
break;
case AP_MODE_CARE_FREE_DIRECT:
guidance_h_mode_changed(GUIDANCE_H_MODE_CARE_FREE);
guidance_h_mode_changed(GUIDANCE_H_MODE_NONE);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_CARE_FREE);
break;
case AP_MODE_HOVER_DIRECT:
case AP_MODE_HOVER_CLIMB:
case AP_MODE_HOVER_Z_HOLD:
guidance_h_mode_changed(GUIDANCE_H_MODE_HOVER);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_HEADING);
break;
case AP_MODE_HOME:
case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_H_MODE_MODULE_SETTING
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
#endif
break;
case AP_MODE_FLIP:
guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_HEADING); // TODO check
break;
case AP_MODE_GUIDED:
guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,
STABILIZATION_ATT_SUBMODE_HEADING);
break;
default:
break;
@@ -260,8 +288,10 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
break;
#endif
case AP_MODE_KILL:
autopilot_set_motors_on(FALSE);
stabilization_cmd[COMMAND_THRUST] = 0;
autopilot_set_motors_on(false);
#ifdef COMMAND_THRUST
stabilization.cmd[COMMAND_THRUST] = 0; // FIXME maybe not needed ?
#endif
guidance_v_mode_changed(GUIDANCE_V_MODE_KILL);
break;
case AP_MODE_RC_DIRECT:
@@ -289,14 +319,6 @@ void autopilot_static_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_V_MODE_MODULE_SETTING
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
#endif
break;
case AP_MODE_FLIP:
guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
break;
case AP_MODE_GUIDED:
guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
break;
@@ -374,9 +396,6 @@ void autopilot_static_on_rc_frame(void)
SetCommandsFromRC(commands, radio_control.values);
}
#endif
guidance_v_read_rc();
guidance_h_read_rc(autopilot_in_flight());
}
}
@@ -393,8 +412,6 @@ void autopilot_failsafe_checks(void)
autopilot_get_mode() != AP_MODE_HOME &&
autopilot_get_mode() != AP_MODE_FAILSAFE &&
autopilot_get_mode() != AP_MODE_NAV &&
autopilot_get_mode() != AP_MODE_MODULE &&
autopilot_get_mode() != AP_MODE_FLIP &&
autopilot_get_mode() != AP_MODE_GUIDED) {
autopilot_set_mode(RC_LOST_MODE);
}
sw/airborne/firmwares/rotorcraft/autopilot_static.h
-2
View File
@@ -49,8 +49,6 @@
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
#define AP_MODE_CARE_FREE_DIRECT 15
#define AP_MODE_FORWARD 16
#define AP_MODE_MODULE 17
#define AP_MODE_FLIP 18
#define AP_MODE_GUIDED 19
sw/airborne/firmwares/rotorcraft/autopilot_utils.c
+2
View File
@@ -35,6 +35,7 @@
/** Display descent speed in failsafe mode if needed */
PRINT_CONFIG_VAR(FAILSAFE_DESCENT_SPEED)
#if (defined MODE_MANUAL) && (defined MODE_AUTO1)
#if defined RADIO_MODE_2x3
@@ -113,6 +114,7 @@ uint8_t ap_mode_of_two_switches(void)
}
#endif
#endif // MODE_MANUAL && MODE_AUTO1
/** Set Rotorcraft commands.
* Limit thrust and/or yaw depending of the in_flight
sw/airborne/firmwares/rotorcraft/guidance/guidance_flip.c
+18 -17
View File
@@ -77,10 +77,11 @@ void guidance_flip_run(void)
case 0:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight());
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
// FIXME maybe better remove the flip guidance
//stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
// heading_save);
//stabilization_attitude_run(autopilot_in_flight());
stabilization.cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
timer_save = 0;
if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
@@ -89,10 +90,10 @@ void guidance_flip_run(void)
break;
case 1:
stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
stabilization.cmd[COMMAND_ROLL] = 9000; // Rolling command
stabilization.cmd[COMMAND_PITCH] = 0;
stabilization.cmd[COMMAND_YAW] = 0;
stabilization.cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
flip_state++;
@@ -100,10 +101,10 @@ void guidance_flip_run(void)
break;
case 2:
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
stabilization.cmd[COMMAND_ROLL] = 0;
stabilization.cmd[COMMAND_PITCH] = 0;
stabilization.cmd[COMMAND_YAW] = 0;
stabilization.cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
timer_save = timer;
@@ -118,7 +119,7 @@ void guidance_flip_run(void)
heading_save);
stabilization_attitude_run(autopilot_in_flight());
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
stabilization.cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
flip_state++;
@@ -134,10 +135,10 @@ void guidance_flip_run(void)
timer_save = 0;
flip_state = 0;
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
stabilization.cmd[COMMAND_ROLL] = 0;
stabilization.cmd[COMMAND_PITCH] = 0;
stabilization.cmd[COMMAND_YAW] = 0;
stabilization.cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
break;
}
#else
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+118 -244
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+14 -19
View File
@@ -53,17 +53,10 @@ extern "C" {
#define GUIDANCE_H_USE_SPEED_REF TRUE
#endif
#define GUIDANCE_H_MODE_KILL 0
#define GUIDANCE_H_MODE_RATE 1
#define GUIDANCE_H_MODE_ATTITUDE 2
#define GUIDANCE_H_MODE_HOVER 3
#define GUIDANCE_H_MODE_NAV 4
#define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE 8
#define GUIDANCE_H_MODE_FLIP 9
#define GUIDANCE_H_MODE_GUIDED 10
#define GUIDANCE_H_MODE_NONE 0
#define GUIDANCE_H_MODE_HOVER 1
#define GUIDANCE_H_MODE_NAV 2
#define GUIDANCE_H_MODE_GUIDED 3
/** Max bank controlled by guidance
*/
@@ -101,6 +94,12 @@ struct HorizontalGuidanceReference {
struct Int32Vect2 accel; ///< with #INT32_ACCEL_FRAC
};
struct HorizontalGuidanceRCInput {
struct Int32Vect2 vect;
float heading;
float last_ts;
};
struct HorizontalGuidance {
uint8_t mode;
/* configuration options */
@@ -108,19 +107,15 @@ struct HorizontalGuidance {
struct HorizontalGuidanceSetpoint sp; ///< setpoints
struct HorizontalGuidanceReference ref; ///< reference calculated from setpoints
struct FloatEulers rc_sp; ///< remote control setpoint
struct HorizontalGuidanceRCInput rc_sp; ///< remote control setpoint
};
extern struct HorizontalGuidance guidance_h;
extern int32_t transition_percentage;
extern void guidance_h_init(void);
extern void guidance_h_mode_changed(uint8_t new_mode);
extern void guidance_h_read_rc(bool in_flight);
extern void guidance_h_run(bool in_flight);
extern void guidance_h_run_enter(void);
extern struct StabilizationSetpoint guidance_h_run(bool in_flight);
extern struct StabilizationSetpoint guidance_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
extern struct StabilizationSetpoint guidance_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
extern struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
@@ -130,11 +125,11 @@ extern void guidance_h_nav_enter(void);
/** Set horizontal guidance from NAV and run control loop
*/
extern void guidance_h_from_nav(bool in_flight);
extern struct StabilizationSetpoint guidance_h_from_nav(bool in_flight);
/** Run GUIDED mode control
*/
extern void guidance_h_guided_run(bool in_flight);
extern struct StabilizationSetpoint guidance_h_guided_run(bool in_flight);
/** Set horizontal position setpoint.
* @param x North position (local NED frame) in meters.
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+24 -25
View File
@@ -33,18 +33,14 @@
*/
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "modules/ins/ins_int.h"
#include "modules/radio_control/radio_control.h"
#include "state.h"
#include "modules/imu/imu.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
@@ -124,6 +120,7 @@ float time_of_accel_sp_2d = 0.0;
float time_of_accel_sp_3d = 0.0;
struct FloatEulers guidance_euler_cmd;
struct ThrustSetpoint thrust_sp;
float thrust_in;
static void guidance_indi_propagate_filters(struct FloatEulers *eulers);
@@ -155,6 +152,8 @@ static void send_indi_guidance(struct transport_tx *trans, struct link_device *d
*/
void guidance_indi_init(void)
{
FLOAT_EULERS_ZERO(guidance_euler_cmd);
THRUST_SP_SET_ZERO(thrust_sp);
AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);
#if PERIODIC_TELEMETRY
@@ -171,7 +170,7 @@ void guidance_indi_enter(void)
/* set nav_heading to current heading */
nav.heading = stateGetNedToBodyEulers_f()->psi;
thrust_in = stabilization_cmd[COMMAND_THRUST];
thrust_in = stabilization.cmd[COMMAND_THRUST];
thrust_act = thrust_in;
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
@@ -273,31 +272,32 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(control_increment, Ga_inv, a_diff);
struct FloatVect3 thrust_vect;
thrust_vect.x = 0.0; // Fill for quadplanes
thrust_vect.y = 0.0;
thrust_vect.z = control_increment.z;
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
guidance_euler_cmd.psi = heading_sp;
// Compute and store thust setpoint
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
guidance_indi_filter_thrust();
//Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
thrust_in = thrust_filt.o[0] + control_increment.z * guidance_indi_specific_force_gain;
Bound(thrust_in, 0, 9600);
#if GUIDANCE_INDI_RC_DEBUG
if (radio_control.values[RADIO_THROTTLE] < 300) {
thrust_in = 0;
}
#endif
// return required thrust
thrust_sp = th_sp_from_thrust_i(thrust_in, THRUST_AXIS_Z);
//Overwrite the thrust command from guidance_v
stabilization_cmd[COMMAND_THRUST] = thrust_in;
#else
float thrust_vect[3];
thrust_vect[0] = 0.0f; // Fill for quadplanes
thrust_vect[1] = 0.0f;
thrust_vect[2] = control_increment.z;
// specific force not defined, return required increment
thrust_sp = th_sp_from_incr_vect_f(thrust_vect);
#endif
//Bound euler angles to prevent flipping
@@ -372,8 +372,7 @@ void guidance_indi_filter_thrust(void)
// Actuator dynamics
thrust_act = thrust_act + thrust_dyn * (thrust_in - thrust_act);
// same filter as for the acceleration
update_butterworth_2_low_pass(&thrust_filt, thrust_act);
// same filter as for the acceleorth_2_low_pass(&thrust_filt, thrust_act);
}
#endif
@@ -504,25 +503,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
return guidance_indi_run_mode(in_flight, gh, _gv, GUIDANCE_INDI_H_ACCEL, _v_mode);
}
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_V_POS;
return 0; // nothing to do
return thrust_sp;
}
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_V_SPEED;
return 0; // nothing to do
return thrust_sp;
}
int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_V_ACCEL;
return 0; // nothing to do
return thrust_sp;
}
#endif
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+33 -38
View File
@@ -30,15 +30,14 @@
#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "modules/radio_control/radio_control.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "state.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "stdio.h"
#include "filters/low_pass_filter.h"
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h"
// The acceleration reference is calculated with these gains. If you use GPS,
@@ -176,7 +175,7 @@ float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
struct FloatEulers eulers_zxy;
float thrust_dyn = 0.f;
float thrust_act = 0;
float thrust_act = 0.f;
Butterworth2LowPass filt_accel_ned[3];
Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
@@ -216,6 +215,7 @@ float guidance_indi_airspeed_filt_cutoff = GUIDANCE_INDI_AIRSPEED_FILT_CUTOFF;
float guidance_indi_hybrid_heading_sp = 0.f;
struct FloatEulers guidance_euler_cmd;
struct ThrustSetpoint thrust_sp;
float thrust_in;
struct FloatVect3 gi_speed_sp = {0.0, 0.0, 0.0};
@@ -339,12 +339,13 @@ void guidance_indi_init(void)
*
* Call upon entering indi guidance
*/
void guidance_indi_enter(void) {
void guidance_indi_enter(void)
{
/*Obtain eulers with zxy rotation order*/
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
nav.heading = eulers_zxy.psi;
thrust_in = stabilization_cmd[COMMAND_THRUST];
thrust_in = stabilization.cmd[COMMAND_THRUST];
thrust_act = thrust_in;
guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;
@@ -366,7 +367,6 @@ void guidance_indi_enter(void) {
init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
}
#include "firmwares/rotorcraft/navigation.h"
/**
* @param accel_sp accel setpoint in NED frame [m/s^2]
* @param heading_sp the desired heading [rad]
@@ -382,12 +382,9 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
/* Obtain eulers with zxy rotation order */
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
/* Calculate the transition percentage so that the ctrl_effecitveness scheduling works */
transition_percentage = BFP_OF_REAL((eulers_zxy.theta/RadOfDeg(-75.0f))*100,INT32_PERCENTAGE_FRAC);
Bound(transition_percentage,0,BFP_OF_REAL(100.0f,INT32_PERCENTAGE_FRAC));
const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET);
transition_theta_offset = INT_MULT_RSHIFT((transition_percentage <<
(INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100, max_offset, INT32_ANGLE_FRAC);
/* Calculate the transition ratio so that the ctrl_effecitveness scheduling works */
stabilization.transition_ratio = eulers_zxy.theta / RadOfDeg(-75.0f);
Bound(stabilization.transition_ratio, 0.f, 1.f);
// filter accel to get rid of noise and filter attitude to synchronize with accel
guidance_indi_propagate_filters();
@@ -411,9 +408,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
accel_filt.z = filt_accel_ned[2].o[0];
struct FloatVect3 a_diff;
a_diff.x = sp_accel.x - accel_filt.x;
a_diff.y = sp_accel.y - accel_filt.y;
a_diff.z = sp_accel.z - accel_filt.z;
VECT3_DIFF(a_diff, sp_accel, accel_filt);
// Bound the acceleration error so that the linearization still holds
Bound(a_diff.x, -6.0, 6.0);
@@ -428,7 +423,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif
#endif
// Calculate matrix of partial derivatives and control objective
guidance_indi_calcg_wing(Ga, a_diff, v_gih);
@@ -456,16 +450,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
float_mat3_mult(&euler_cmd, Ga_inv, a_diff);
#endif
struct FloatVect3 thrust_vect;
#if GUIDANCE_INDI_HYBRID_U > 3
thrust_vect.x = du_gih[3];
#else
thrust_vect.x = 0;
#endif
thrust_vect.y = 0;
thrust_vect.z = euler_cmd.z;
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);
// Coordinated turn
// feedforward estimate angular rotation omega = g*tan(phi)/v
float omega;
@@ -546,21 +530,31 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
guidance_euler_cmd.psi = guidance_indi_hybrid_heading_sp;
}
// compute required thrust setpoint
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
guidance_indi_filter_thrust();
// Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
thrust_in = thrust_filt.o[0] + euler_cmd.z * guidance_indi_specific_force_gain;
Bound(thrust_in, GUIDANCE_INDI_MIN_THROTTLE, 9600);
#if GUIDANCE_INDI_RC_DEBUG
if (radio_control.values[RADIO_THROTTLE] < 300) {
thrust_in = 0;
}
#endif
// return required thrust
thrust_sp = th_sp_from_thrust_i(thrust_in, THRUST_AXIS_Z);
// Overwrite the thrust command from guidance_v
stabilization_cmd[COMMAND_THRUST] = thrust_in;
#else
float thrust_vect[3];
#if GUIDANCE_INDI_HYBRID_U > 3
thrust_vect[0] = du_gih[3];
#else
thrust_vect[0] = 0;
#endif
thrust_vect[1] = 0;
thrust_vect[2] = euler_cmd.z;
// specific force not defined, return required increment
thrust_sp = th_sp_from_incr_vect_f(thrust_vect);
#endif
// Set the quaternion setpoint from eulers_zxy
@@ -791,7 +785,8 @@ void guidance_indi_filter_thrust(void)
* acceleration
* Called as a periodic function with PERIODIC_FREQ
*/
void guidance_indi_propagate_filters(void) {
void guidance_indi_propagate_filters(void)
{
struct NedCoor_f *accel = stateGetAccelNed_f();
update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
@@ -888,25 +883,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
return guidance_indi_run_mode(in_flight, gh, _gv, GUIDANCE_INDI_HYBRID_H_ACCEL, _v_mode);
}
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_POS;
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
return thrust_sp;
}
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_SPEED;
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
return thrust_sp;
}
int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_ACCEL;
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
return thrust_sp;
}
#endif
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+6 -14
View File
@@ -25,13 +25,11 @@
*/
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "filters/low_pass_filter.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#ifndef COMMAND_THRUST_X
#error "Quadplanes require a forward thrust actuator"
#endif
@@ -48,13 +46,10 @@ float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
#endif
float bodyz_filter_cutoff = 0.2;
Butterworth2LowPass accel_bodyz_filt;
/**
*
* Call upon entering indi guidance
@@ -65,7 +60,6 @@ void guidance_indi_quadplane_init(void) {
init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
}
/**
* Low pass the accelerometer measurements to remove noise from vibrations.
* The roll and pitch also need to be filtered to synchronize them with the
@@ -78,9 +72,6 @@ void guidance_indi_quadplane_propagate_filters(void) {
update_butterworth_2_low_pass(&accel_bodyz_filt, accelz);
}
/**
* Perform WLS
*
@@ -145,14 +136,14 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl
// Set lower limits
du_min_gih[0] = -roll_limit_rad - roll_angle; //roll
du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch
du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;
du_min_gih[2] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
du_min_gih[3] = -stabilization.cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;
// Set upper limits limits
du_max_gih[0] = roll_limit_rad - roll_angle; //roll
du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch
du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;
du_max_gih[2] = -stabilization.cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
du_max_gih[3] = (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;
// Set prefered states
du_pref_gih[0] = -roll_angle; // prefered delta roll angle
@@ -165,4 +156,5 @@ void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angl
/** Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts with all thrust off */
bool autopilot_in_flight_end_detection(bool motors_on UNUSED) {
return ! motors_on;
}
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
+7 -9
View File
@@ -31,17 +31,15 @@
*
* The guidance that the module implement must be activated with following defines:
*
* a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
* One must then implement:
* - void guidance_h_module_init(void);
* - void guidance_h_module_enter(void);
* - void guidance_h_module_read_rc(void);
* - void guidance_h_module_run(bool in_flight);
* a) Implement own loops
* - void guidance_module_enter(void);
* - void guidance_module_run(bool in_flight);
*
*
* b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE
* - void guidance_v_module_enter(void);
* - void guidance_v_module_run(bool in_flight);
* b) Use or copy the generated autopilot file 'conf/autopilot/rotorcraft_control_loop.xml'
* - load custom autopilot from your airframe filewith by adding
* <autopilot name="rotorcraft_control_loop"/>
* to the firmware section
*
* If the module implements both V and H mode, take into account that the V is called first and later H
*
sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.c
+12 -10
View File
@@ -261,7 +261,7 @@ static struct StabilizationSetpoint guidance_pid_h_run(bool in_flight, struct Ho
if (guidance_pid.approx_force_by_thrust && in_flight) {
static int32_t thrust_cmd_filt;
// FIXME strong coupling with guidance_v here !!!
int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
int32_t vertical_thrust = (stabilization.cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) /
(GUIDANCE_H_THRUST_CMD_FILTER + 1);
guidance_pid.cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_pid.cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2),
@@ -318,7 +318,7 @@ struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight UNUSED, str
#define FF_CMD_FRAC 18
static int32_t guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
static struct ThrustSetpoint guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
{
/* compute the error to our reference */
int32_t err_z = gv->z_ref - stateGetPositionNed_i()->z;
@@ -365,23 +365,25 @@ static int32_t guidance_pid_v_run(bool in_flight, struct VerticalGuidance *gv)
/* bound the result */
Bound(guidance_pid.cmd_thrust, 0, MAX_PPRZ);
return guidance_pid.cmd_thrust;
return th_sp_from_thrust_i(guidance_pid.cmd_thrust, THRUST_AXIS_Z);
}
int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
{
return guidance_pid_v_run(in_flight, gv);
}
int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
{
return guidance_pid_v_run(in_flight, gv);
}
int32_t guidance_pid_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
struct ThrustSetpoint guidance_pid_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)
{
// TODO
return 0;
struct ThrustSetpoint sp;
THRUST_SP_SET_ZERO(sp);
return sp;
}
void guidance_pid_h_enter(void)
@@ -445,17 +447,17 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
return guidance_pid_h_run_accel(in_flight, gh);
}
int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv)
{
return guidance_pid_v_run_pos(in_flight, gv);
}
int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv)
{
return guidance_pid_v_run_speed(in_flight, gv);
}
int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv)
{
return guidance_pid_v_run_accel(in_flight, gv);
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_pid.h
+3 -3
View File
@@ -64,9 +64,9 @@ extern void guidance_pid_v_enter(void);
extern struct StabilizationSetpoint guidance_pid_h_run_pos(bool in_flight, struct HorizontalGuidance *gh);
extern struct StabilizationSetpoint guidance_pid_h_run_speed(bool in_flight, struct HorizontalGuidance *gh);
extern struct StabilizationSetpoint guidance_pid_h_run_accel(bool in_flight, struct HorizontalGuidance *gh);
extern int32_t guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
extern int32_t guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
extern int32_t guidance_pid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
extern struct ThrustSetpoint guidance_pid_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
extern struct ThrustSetpoint guidance_pid_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
extern struct ThrustSetpoint guidance_pid_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
extern void guidance_pid_set_h_igain(uint32_t igain);
extern void guidance_pid_set_v_igain(uint32_t igain);
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+39 -64
View File
@@ -32,6 +32,7 @@
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/navigation.h"
#include "modules/core/abi.h"
#include "state.h"
@@ -66,6 +67,13 @@ static bool desired_zd_updated;
static int guidance_v_guided_mode;
#ifndef GUIDANCE_V_RC_ID
#define GUIDANCE_V_RC_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(GUIDANCE_V_RC_ID)
static abi_event rc_ev;
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -92,24 +100,21 @@ void guidance_v_init(void)
gv_adapt_init();
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
guidance_v_module_init();
#endif
// bind ABI messages
AbiBindMsgRADIO_CONTROL(GUIDANCE_V_RC_ID, &rc_ev, rc_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TUNE_VERT, send_tune_vert);
#endif
}
void guidance_v_read_rc(void)
static void rc_cb(uint8_t sender_id UNUSED, struct RadioControl *rc)
{
/* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */
guidance_v.rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE];
guidance_v.rc_delta_t = (int32_t)rc->values[RADIO_THROTTLE];
/* used in RC_CLIMB */
guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE];
guidance_v.rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)rc->values[RADIO_THROTTLE];
DeadBand(guidance_v.rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND);
static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) /
@@ -146,15 +151,6 @@ void guidance_v_mode_changed(uint8_t new_mode)
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_enter();
break;
#endif
case GUIDANCE_V_MODE_FLIP:
break;
default:
break;
@@ -210,9 +206,10 @@ void guidance_v_thrust_adapt(bool in_flight)
* This means that the estimation is not updated when using direct throttle commands.
*
* FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT, AKA SUPERVISION and co
* FIXME get this out of here !!!! and don't get stab cmd directly
*/
if (desired_zd_updated) {
int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
int32_t vertical_thrust = (stabilization.cmd[COMMAND_THRUST] * guidance_v.thrust_coeff) >> INT32_TRIG_FRAC;
gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v.zd_ref);
}
} else {
@@ -221,63 +218,49 @@ void guidance_v_thrust_adapt(bool in_flight)
}
}
void guidance_v_run(bool in_flight)
struct ThrustSetpoint guidance_v_run(bool in_flight)
{
guidance_v_thrust_adapt(in_flight);
/* reset flag indicating if desired zd was updated */
desired_zd_updated = false;
/* reset setpoint */
THRUST_SP_SET_ZERO(guidance_v.thrust);
switch (guidance_v.mode) {
case GUIDANCE_V_MODE_RC_DIRECT:
guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t;
guidance_v.thrust = th_sp_from_thrust_i(guidance_v.rc_delta_t, THRUST_AXIS_Z);
break;
case GUIDANCE_V_MODE_RC_CLIMB:
guidance_v.zd_sp = guidance_v.rc_zd_sp;
gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
break;
case GUIDANCE_V_MODE_CLIMB:
gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
guidance_v.thrust = guidance_v_run_speed(in_flight, &guidance_v);
break;
case GUIDANCE_V_MODE_HOVER:
guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
/* Falls through. */
case GUIDANCE_V_MODE_GUIDED:
guidance_v_guided_run(in_flight);
guidance_v.thrust = guidance_v_guided_run(in_flight);
break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_run(in_flight);
break;
#endif
case GUIDANCE_V_MODE_NAV: {
guidance_v_from_nav(in_flight);
guidance_v.thrust = guidance_v_from_nav(in_flight);
break;
}
case GUIDANCE_V_MODE_FLIP:
break;
default:
break;
}
return guidance_v.thrust;
}
@@ -314,36 +297,32 @@ void guidance_v_update_ref(void)
desired_zd_updated = true;
}
void guidance_v_from_nav(bool in_flight)
struct ThrustSetpoint guidance_v_from_nav(bool in_flight)
{
struct ThrustSetpoint sp;
THRUST_SP_SET_ZERO(sp);
if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
guidance_v.zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v.z_sp);
guidance_v_update_ref();
guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
sp = guidance_v_run_pos(in_flight, &guidance_v);
} else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
guidance_v.z_sp = stateGetPositionNed_i()->z;
guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
guidance_v_update_ref();
guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
sp = guidance_v_run_speed(in_flight, &guidance_v);
} else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
guidance_v.z_sp = stateGetPositionNed_i()->z;
guidance_v.zd_sp = stateGetSpeedNed_i()->z;
GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
guidance_v_run_enter();
guidance_v.delta_t = nav.throttle;
sp = th_sp_from_thrust_i((int32_t)nav.throttle, THRUST_AXIS_Z);
} else if (nav.vertical_mode == NAV_VERTICAL_MODE_GUIDED) {
guidance_v_guided_run(in_flight);
sp = guidance_v_guided_run(in_flight);
}
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
return sp;
}
void guidance_v_guided_enter(void)
@@ -356,8 +335,10 @@ void guidance_v_guided_enter(void)
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
}
void guidance_v_guided_run(bool in_flight)
struct ThrustSetpoint guidance_v_guided_run(bool in_flight)
{
struct ThrustSetpoint sp;
THRUST_SP_SET_ZERO(sp);
switch(guidance_v_guided_mode)
{
case GUIDANCE_V_GUIDED_MODE_ZHOLD:
@@ -365,29 +346,23 @@ void guidance_v_guided_run(bool in_flight)
guidance_v.zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v.z_sp);
guidance_v_update_ref();
guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
sp = guidance_v_run_pos(in_flight, &guidance_v);
break;
case GUIDANCE_V_GUIDED_MODE_CLIMB:
// Climb
gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
guidance_v_update_ref();
guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
sp = guidance_v_run_speed(in_flight, &guidance_v);
break;
case GUIDANCE_V_GUIDED_MODE_THROTTLE:
// Throttle
guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
guidance_v.delta_t = guidance_v.th_sp;
sp = th_sp_from_thrust_i(guidance_v.th_sp, THRUST_AXIS_Z);
break;
default:
break;
}
#if !NO_RC_THRUST_LIMIT
/* use rc limitation if available */
if (radio_control.status == RC_OK) {
stabilization_cmd[COMMAND_THRUST] = Min(guidance_v.rc_delta_t, guidance_v.delta_t);
} else
#endif
stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
return sp;
}
void guidance_v_set_z(float z)
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+16 -15
View File
@@ -30,6 +30,7 @@
#include "firmwares/rotorcraft/guidance/guidance_v_ref.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "firmwares/rotorcraft/stabilization.h"
#define GUIDANCE_V_MODE_KILL 0
#define GUIDANCE_V_MODE_RC_DIRECT 1
@@ -37,9 +38,7 @@
#define GUIDANCE_V_MODE_CLIMB 3
#define GUIDANCE_V_MODE_HOVER 4
#define GUIDANCE_V_MODE_NAV 5
#define GUIDANCE_V_MODE_MODULE 6
#define GUIDANCE_V_MODE_FLIP 7
#define GUIDANCE_V_MODE_GUIDED 8
#define GUIDANCE_V_MODE_GUIDED 6
struct VerticalGuidance {
uint8_t mode;
@@ -85,16 +84,16 @@ struct VerticalGuidance {
*/
int32_t rc_zd_sp;
/** thrust setpoint.
/** input thrust setpoint.
* valid range 0 : #MAX_PPRZ
*/
int32_t th_sp;
/** thrust command.
/** Final thrust setpoint
* summation of feed-forward and feed-back commands,
* valid range 0 : #MAX_PPRZ
* can be a total thrust or increment, float or int
*/
int32_t delta_t;
struct ThrustSetpoint thrust; // FIXME maybe not needed to store the value ?
/** nominal throttle for hover.
* This is only used if #GUIDANCE_V_NOMINAL_HOVER_THROTTLE is defined!
@@ -108,18 +107,20 @@ struct VerticalGuidance {
extern struct VerticalGuidance guidance_v;
extern void guidance_v_init(void);
extern void guidance_v_read_rc(void);
extern void guidance_v_mode_changed(uint8_t new_mode);
extern void guidance_v_notify_in_flight(bool in_flight);
extern void guidance_v_thrust_adapt(bool in_flight);
extern void guidance_v_update_ref(void);
extern void guidance_v_run(bool in_flight);
extern void guidance_v_z_enter(void);
extern void guidance_v_run_enter(void);
extern int32_t guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
extern int32_t guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
extern int32_t guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
/** Guidance vertical run functions
* @return a thrust setpoint structure
*/
extern struct ThrustSetpoint guidance_v_run(bool in_flight);
extern struct ThrustSetpoint guidance_v_run_pos(bool in_flight, struct VerticalGuidance *gv);
extern struct ThrustSetpoint guidance_v_run_speed(bool in_flight, struct VerticalGuidance *gv);
extern struct ThrustSetpoint guidance_v_run_accel(bool in_flight, struct VerticalGuidance *gv);
/** Set guidance ref parameters
*/
@@ -129,7 +130,7 @@ extern void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel);
/** Set guidance setpoint from NAV and run hover loop
*/
extern void guidance_v_from_nav(bool in_flight);
extern struct ThrustSetpoint guidance_v_from_nav(bool in_flight);
/** Enter GUIDED mode control
*/
@@ -137,7 +138,7 @@ extern void guidance_v_guided_enter(void);
/** Run GUIDED mode control
*/
extern void guidance_v_guided_run(bool in_flight);
extern struct ThrustSetpoint guidance_v_guided_run(bool in_flight);
/** Set z position setpoint.
* @param z Setpoint (down is positive) in meters.

Some files were not shown because too many files have changed in this diff Show More