Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[control] rework the stabilization for rotorcraft #3248

Merged
merged 1 commit into from
May 21, 2024
Merged

[control] rework the stabilization for rotorcraft #3248

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions conf/airframes/ENAC/hybrid/falcon_v1.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_">
Expand Down
33 changes: 7 additions & 26 deletions conf/airframes/ENAC/hybrid/falcon_v2.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>

Expand All @@ -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"/>
Expand All @@ -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"/>
Expand All @@ -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_">
Expand All @@ -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>
Expand Down
1 change: 1 addition & 0 deletions conf/airframes/examples/bebop2_opticflow.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
2 changes: 2 additions & 0 deletions conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down
6 changes: 4 additions & 2 deletions conf/airframes/tudelft/bebop_OF_hover.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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" />

Expand Down Expand Up @@ -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>

Expand Down
2 changes: 2 additions & 0 deletions conf/airframes/tudelft/guido_ardrone2_optitrack.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
3 changes: 2 additions & 1 deletion conf/autopilot/autopilot.dtd
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
93 changes: 39 additions & 54 deletions conf/autopilot/rotorcraft_autopilot.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand All @@ -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>
Expand All @@ -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"/>
Expand All @@ -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"/>
Expand All @@ -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"/>
Expand All @@ -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"/>
Expand All @@ -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"/>
Expand All @@ -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)"/>
Expand Down