Merge 1c41caf into 757c637

paparazzi · Mar 21, 2024 · ad30eea · ad30eea
2 parents 757c637 + 1c41caf
commit ad30eea
Show file tree

Hide file tree

Showing 116 changed files with 2,128 additions and 2,388 deletions.
diff --git a/conf/airframes/ENAC/hybrid/falcon_v1.xml b/conf/airframes/ENAC/hybrid/falcon_v1.xml
@@ -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_">

diff --git a/conf/airframes/ENAC/hybrid/falcon_v2.xml b/conf/airframes/ENAC/hybrid/falcon_v2.xml
@@ -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,22 +273,14 @@
     <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"/>
     <define name="FE_LIFT_A_AS" value="0.0008"/>
     <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,30 +299,17 @@
 
   <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"/>
 
     <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>

diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
@@ -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"/>

diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -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"/>

diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -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>
 

diff --git a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
@@ -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>

diff --git a/conf/autopilot/autopilot.dtd b/conf/autopilot/autopilot.dtd
@@ -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>

diff --git a/conf/autopilot/rotorcraft_autopilot.xml b/conf/autopilot/rotorcraft_autopilot.xml
@@ -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,38 +102,31 @@
     <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 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)"/>