Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Skid landing (#1669)

Browse Source 
Skid landing navigation routine.
Comes with an optional landing control for vertical control system
I did test the build at a localhost.
This commit is contained in:
Michal Podhradsky
2016-05-26 15:37:17 -07:00
parent 3ad41378fe
commit 75f8e34e51
15 changed files with 741 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AGGIEAIR/aggieair_conf.xml
+2 -2
View File
@@ -16,9 +16,9 @@
airframe="airframes/AGGIEAIR/aggieair_rp3_lia.xml"
radio="radios/AGGIEAIR/aggieair_taranis.xml"
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/versatile.xml"
flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
settings_modules=""
settings_modules="modules/nav_survey_poly_osam.xml"
gui_color="#00009e93ffff"
/>
</conf>
conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
+25 -10
View File
@@ -39,7 +39,10 @@ RP3 Lisa MX
<modules>
<module name="nav" type="line"/>
<module name="nav" type="flower"/>
<module name="nav" type="survey_poly_osam"/>
<module name="nav" type="launcher"/>
<module name="nav" type="skid_landing"/>
<module name="sys_mon"/>
</modules>
@@ -163,6 +166,18 @@ RP3 Lisa MX
<define name="AUTO_PITCH_IGAIN" value="0.025"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.31415" unit="rad"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.31415" unit="rad"/>
<!-- landing control loop -->
<define name="LANDING_THROTTLE_PGAIN" value="600.0"/>
<define name="LANDING_THROTTLE_IGAIN" value="10.0"/>
<define name="LANDING_THROTTLE_MAX" value="0.65"/>
<define name="LANDING_DESIRED_SPEED" value="18" unit="m/s"/>
<define name="LANDING_PITCH_PGAIN" value="0.1"/>
<define name="LANDING_PITCH_IGAIN" value="0.1"/>
<define name="LANDING_PITCH_LIMITS" value="0.2" unit="rad"/>
<define name="LANDING_PITCH_FLARE" value="0.06"/>
<define name="LANDING_ALT_THROTTLE_KILL" value="15" unit="m"/>
<define name="LANDING_ALT_FLARE" value="5" unit="m"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
@@ -199,19 +214,19 @@ RP3 Lisa MX
</section>
<!-- Launcher Takeoff Configuration -->
<section name="Takeoff" prefix="Takeoff_">
<define name="Pitch" value="0.23" unit="rad"/>
<define name="Height" value="70" unit="m"/>
<define name="Speed" value="8" unit="m/s"/>
<define name="Distance" value="30" unit="m"/>
<define name="MinSpeed" value="5" unit="m/s"/>
<section name="LAUNCHER" prefix="LAUNCHER_TAKEOFF_">
<define name="PITCH" value="0.23" unit="rad"/>
<define name="HEIGHT" value="70" unit="m"/>
<define name="MIN_SPEED_CIRCLE" value="8" unit="m/s"/>
<define name="DISTANCE" value="30" unit="m"/>
<define name="MIN_SPEED_LINE" value="5" unit="m/s"/>
</section>
<!-- Skid Landing Configuration -->
<section name="Landing" prefix="Landing_">
<define name="AFHeight" value="50" unit="m"/>
<define name="FinalHeight" value="50" unit="m"/>
<define name="FinalStageTime" value="10" unit="s"/>
<section name="LANDING" prefix="SKID_LANDING_">
<define name="AF_HEIGHT" value="50" unit="m"/>
<define name="FINAL_HEIGHT" value="50" unit="m"/>
<define name="FINAL_STAGE_TIME" value="10" unit="s"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
conf/conf_example.xml
+43 -21
View File
@@ -1,7 +1,7 @@
<conf>
<aircraft
name="BOOZ2"
ac_id="150"
ac_id="1"
airframe="airframes/examples/booz2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -12,7 +12,7 @@
/>
<aircraft
name="Bixler"
ac_id="24"
ac_id="2"
airframe="airframes/examples/bixler_lisa_m_2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -34,7 +34,7 @@
/>
<aircraft
name="EasyStar_ETS"
ac_id="8"
ac_id="4"
airframe="airframes/examples/easystar_ets.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -45,7 +45,7 @@
/>
<aircraft
name="Hexa_LisaL"
ac_id="153"
ac_id="5"
airframe="airframes/examples/h_hex.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -56,7 +56,7 @@
/>
<aircraft
name="LadyLisa"
ac_id="164"
ac_id="6"
airframe="airframes/examples/ladybird_lisa_s.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -67,7 +67,7 @@
/>
<aircraft
name="MentorEnergy"
ac_id="77"
ac_id="13"
airframe="airframes/examples/MentorEnergy.xml"
radio="radios/R6107SP_7ch.xml"
telemetry="telemetry/default_fixedwing_imu_9k6.xml"
@@ -78,7 +78,7 @@
/>
<aircraft
name="Microjet"
ac_id="5"
ac_id="14"
airframe="airframes/examples/microjet.xml"
radio="radios/cockpitMM.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -89,7 +89,7 @@
/>
<aircraft
name="Microjet_LisaM"
ac_id="4"
ac_id="15"
airframe="airframes/examples/microjet_lisa_m.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -100,7 +100,7 @@
/>
<aircraft
name="Quad_Elle0"
ac_id="165"
ac_id="19"
airframe="airframes/examples/quadrotor_elle0.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -111,7 +111,7 @@
/>
<aircraft
name="Quad_LisaMX"
ac_id="30"
ac_id="21"
airframe="airframes/examples/quadrotor_lisa_mx.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -144,7 +144,7 @@
/>
<aircraft
name="Quad_NavGo"
ac_id="151"
ac_id="23"
airframe="airframes/examples/quadrotor_navgo.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -155,7 +155,7 @@
/>
<aircraft
name="Quad_Navstik"
ac_id="180"
ac_id="24"
airframe="airframes/examples/quadrotor_navstik.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -166,7 +166,7 @@
/>
<aircraft
name="Twinjet"
ac_id="6"
ac_id="25"
airframe="airframes/examples/twinjet.xml"
radio="radios/cockpitMM.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -177,7 +177,7 @@
/>
<aircraft
name="Twinstar_energyadaptive"
ac_id="13"
ac_id="26"
airframe="airframes/examples/Twinstar_energyadaptive.xml"
radio="radios/Corona_24_DIY.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -188,7 +188,7 @@
/>
<aircraft
name="Twog_IMU"
ac_id="7"
ac_id="27"
airframe="airframes/examples/twog_analogimu.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -199,7 +199,7 @@
/>
<aircraft
name="Umarim_Lite"
ac_id="20"
ac_id="28"
airframe="airframes/examples/umarim_lite_v2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -210,7 +210,7 @@
/>
<aircraft
name="ardrone2"
ac_id="201"
ac_id="31"
airframe="airframes/examples/ardrone2.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -221,7 +221,7 @@
/>
<aircraft
name="bebop"
ac_id="202"
ac_id="33"
airframe="airframes/examples/bebop.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -243,7 +243,7 @@
/>
<aircraft
name="krooz_quad"
ac_id="23"
ac_id="34"
airframe="airframes/examples/krooz_sd_quad_mkk.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -254,7 +254,7 @@
/>
<aircraft
name="quadshot"
ac_id="19"
ac_id="37"
airframe="airframes/examples/quadshot_asp21_spektrum.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -276,7 +276,7 @@
/>
<aircraft
name="setup_lisam2"
ac_id="16"
ac_id="38"
airframe="airframes/examples/setup_lisam2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/dummy.xml"
@@ -285,4 +285,26 @@
settings_modules=""
gui_color="blue"
/>
<aircraft
name="Ark_Quad"
ac_id="41"
airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml"
radio="radios/AGGIEAIR/aggieair_taranis.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml"
settings_modules=""
gui_color="#ffff954c0000"
/>
<aircraft
name="Minion_Lia"
ac_id="42"
airframe="airframes/AGGIEAIR/aggieair_rp3_lia.xml"
radio="radios/AGGIEAIR/aggieair_taranis.xml"
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
settings_modules="modules/nav_survey_poly_osam.xml"
gui_color="#00009e93ffff"
/>
</conf>
conf/conf_tests.xml
+62 -40
View File
@@ -1,7 +1,18 @@
<conf>
<aircraft
name="Ark_Quad"
ac_id="41"
airframe="airframes/AGGIEAIR/aggieair_ark_quad_lisa_mx.xml"
radio="radios/AGGIEAIR/aggieair_taranis.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/nps.xml settings/control/stabilization_att_float_euler.xml"
settings_modules=""
gui_color="#ffff954c0000"
/>
<aircraft
name="BOOZ2"
ac_id="150"
ac_id="1"
airframe="airframes/examples/booz2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -12,7 +23,7 @@
/>
<aircraft
name="Bixler"
ac_id="24"
ac_id="2"
airframe="airframes/examples/bixler_lisa_m_2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -23,7 +34,7 @@
/>
<aircraft
name="DualBoard_AP_FBW"
ac_id="31"
ac_id="3"
airframe="airframes/examples/separate_fbw_ap.xml"
radio="radios/R6107SP_7ch.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -34,7 +45,7 @@
/>
<aircraft
name="EasyStar_ETS"
ac_id="8"
ac_id="4"
airframe="airframes/examples/easystar_ets.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -45,7 +56,7 @@
/>
<aircraft
name="Hexa_LisaL"
ac_id="153"
ac_id="5"
airframe="airframes/examples/h_hex.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -56,7 +67,7 @@
/>
<aircraft
name="JP"
ac_id="40"
ac_id="6"
airframe="airframes/ENAC/fixed-wing/jp.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -67,7 +78,7 @@
/>
<aircraft
name="LadyLisa"
ac_id="164"
ac_id="7"
airframe="airframes/examples/ladybird_lisa_s.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -78,7 +89,7 @@
/>
<aircraft
name="LadyLisaBluetooth"
ac_id="166"
ac_id="8"
airframe="airframes/examples/ladybird_lisa_s_bluegiga.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -89,7 +100,7 @@
/>
<aircraft
name="LisaLv11_Aspirinv15_FW"
ac_id="12"
ac_id="9"
airframe="airframes/testhardware/LisaL_v1.1_aspirin_v1.5_fw.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -100,7 +111,7 @@
/>
<aircraft
name="LisaLv11_Aspirinv15_RC"
ac_id="11"
ac_id="10"
airframe="airframes/testhardware/LisaL_v1.1_aspirin_v1.5_rc.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -111,7 +122,7 @@
/>
<aircraft
name="LisaLv11_Booz2v12_FW"
ac_id="10"
ac_id="11"
airframe="airframes/testhardware/LisaL_v1.1_b2_v1.2_fw.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -122,7 +133,7 @@
/>
<aircraft
name="LisaLv11_Booz2v12_RC"
ac_id="9"
ac_id="12"
airframe="airframes/testhardware/LisaL_v1.1_b2_v1.2_rc.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -133,7 +144,7 @@
/>
<aircraft
name="MentorEnergy"
ac_id="77"
ac_id="13"
airframe="airframes/examples/MentorEnergy.xml"
radio="radios/R6107SP_7ch.xml"
telemetry="telemetry/default_fixedwing_imu_9k6.xml"
@@ -144,7 +155,7 @@
/>
<aircraft
name="Microjet"
ac_id="5"
ac_id="14"
airframe="airframes/examples/microjet.xml"
radio="radios/cockpitMM.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -155,7 +166,7 @@
/>
<aircraft
name="Microjet_LisaM"
ac_id="4"
ac_id="15"
airframe="airframes/examples/microjet_lisa_m.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -166,7 +177,7 @@
/>
<aircraft
name="Microjet_Twog_Aspirin"
ac_id="46"
ac_id="16"
airframe="airframes/examples/microjet_twog_aspirin.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -177,7 +188,7 @@
/>
<aircraft
name="Microjet_xsens_imu"
ac_id="45"
ac_id="17"
airframe="airframes/examples/microjet_imu_xsens.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -188,7 +199,7 @@
/>
<aircraft
name="Microjet_xsens_ins"
ac_id="44"
ac_id="18"
airframe="airframes/examples/microjet_lisa_m_xsens.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -197,9 +208,20 @@
settings_modules=""
gui_color="blue"
/>
<aircraft
name="Minion_Lia"
ac_id="42"
airframe="airframes/AGGIEAIR/aggieair_rp3_lia.xml"
radio="radios/AGGIEAIR/aggieair_taranis.xml"
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
settings_modules="modules/nav_survey_poly_osam.xml"
gui_color="#00009e93ffff"
/>
<aircraft
name="Quad_Elle0"
ac_id="165"
ac_id="19"
airframe="airframes/examples/quadrotor_elle0.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -210,7 +232,7 @@
/>
<aircraft
name="Quad_HBMini"
ac_id="152"
ac_id="20"
airframe="airframes/examples/quadrotor_hbmini.xml"
radio="radios/dx6iCHNI.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -221,7 +243,7 @@
/>
<aircraft
name="Quad_LisaMX"
ac_id="30"
ac_id="21"
airframe="airframes/examples/quadrotor_lisa_mx.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -232,7 +254,7 @@
/>
<aircraft
name="Quad_LisaM_2"
ac_id="162"
ac_id="22"
airframe="airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -243,7 +265,7 @@
/>
<aircraft
name="Quad_NavGo"
ac_id="151"
ac_id="23"
airframe="airframes/examples/quadrotor_navgo.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -254,7 +276,7 @@
/>
<aircraft
name="Quad_Navstik"
ac_id="180"
ac_id="24"
airframe="airframes/examples/quadrotor_navstik.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -276,7 +298,7 @@
/>
<aircraft
name="Twinjet"
ac_id="6"
ac_id="25"
airframe="airframes/examples/twinjet.xml"
radio="radios/cockpitMM.xml"
telemetry="telemetry/default_fixedwing.xml"
@@ -287,7 +309,7 @@
/>
<aircraft
name="Twinstar_energyadaptive"
ac_id="13"
ac_id="26"
airframe="airframes/examples/Twinstar_energyadaptive.xml"
radio="radios/Corona_24_DIY.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -298,7 +320,7 @@
/>
<aircraft
name="Twog_IMU"
ac_id="7"
ac_id="27"
airframe="airframes/examples/twog_analogimu.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -309,7 +331,7 @@
/>
<aircraft
name="Umarim_Lite"
ac_id="20"
ac_id="28"
airframe="airframes/examples/umarim_lite_v2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -320,7 +342,7 @@
/>
<aircraft
name="Yapa2_XSens"
ac_id="32"
ac_id="29"
airframe="airframes/TUDELFT/tudelft_yapa_xsens.xml"
radio="radios/R6107SP_7ch.xml"
telemetry="telemetry/default_fixedwing_imu_9k6.xml"
@@ -331,7 +353,7 @@
/>
<aircraft
name="apogee_chibios"
ac_id="14"
ac_id="30"
airframe="airframes/ENAC/fixed-wing/apogee.xml"
radio="radios/T10CG_SBUS.xml"
telemetry="telemetry/fixedwing_flight_recorder.xml"
@@ -342,7 +364,7 @@
/>
<aircraft
name="ardrone2"
ac_id="201"
ac_id="31"
airframe="airframes/examples/ardrone2.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -353,7 +375,7 @@
/>
<aircraft
name="ardrone2_opticflow"
ac_id="2"
ac_id="32"
airframe="airframes/examples/ardrone2_opticflow_hover.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -364,7 +386,7 @@
/>
<aircraft
name="bebop"
ac_id="202"
ac_id="33"
airframe="airframes/examples/bebop.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -375,7 +397,7 @@
/>
<aircraft
name="krooz_quad"
ac_id="23"
ac_id="34"
airframe="airframes/examples/krooz_sd_quad_mkk.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -386,7 +408,7 @@
/>
<aircraft
name="lisa_l_chimu"
ac_id="28"
ac_id="35"
airframe="airframes/examples/lisa_l_chimu.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
@@ -397,7 +419,7 @@
/>
<aircraft
name="quad_mavlink"
ac_id="1"
ac_id="36"
airframe="airframes/examples/quadrotor_lisa_mx_mavlink.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft_mavlink.xml"
@@ -408,7 +430,7 @@
/>
<aircraft
name="quadshot"
ac_id="19"
ac_id="37"
airframe="airframes/examples/quadshot_asp21_spektrum.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
@@ -419,7 +441,7 @@
/>
<aircraft
name="setup_lisam2"
ac_id="16"
ac_id="38"
airframe="airframes/examples/setup_lisam2.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/dummy.xml"
@@ -430,7 +452,7 @@
/>
<aircraft
name="test_settings"
ac_id="29"
ac_id="39"
airframe="airframes/test_settings.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/dummy.xml"
@@ -441,7 +463,7 @@
/>
<aircraft
name="yapa_chimu_spi"
ac_id="27"
ac_id="40"
airframe="airframes/examples/yapaChimuSpi.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
conf/firmwares/subsystems/fixedwing/control.makefile
+1 -1
View File
@@ -6,4 +6,4 @@
$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/guidance_v.c
$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\"
#$(TARGET).CFLAGS += -DCTRL_VERTICAL_LANDING=1
conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml
+94
View File
@@ -0,0 +1,94 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="1550" ground_alt="1350" lat0="41.815562" lon0="-111.982437" max_dist_from_home="3000" name="BasicTuning" security_height="25" qfu="90">
<header>
#include "subsystems/datalink/datalink.h"
</header>
<waypoints>
<waypoint alt="1545.0" name="HOME" x="1006.3" y="-116.4"/>
<waypoint alt="1346.0" name="Bungee" x="1010.2" y="-266.0"/>
<waypoint alt="1545.0" name="STDBY" x="791.0" y="-215.8"/>
<waypoint alt="1547.0" name="1" x="1000.0" y="308.8"/>
<waypoint alt="1544.0" name="2" x="996.1" y="-541.7"/>
<waypoint alt="1793.0" name="S1" x="877.1" y="1093.2"/>
<waypoint alt="1793.0" name="_S2" x="2124.9" y="1115.0"/>
<waypoint alt="1793.0" name="_S3" x="2124.9" y="-668.0"/>
<waypoint alt="1793.0" name="_S4" x="877.1" y="-660.8"/>
<waypoint name="_P1" x="-1013.7" y="-2188.7"/>
<waypoint name="_P2" x="-999.8" y="1849.7"/>
<waypoint name="_P3" x="3104.1" y="1776.1"/>
<waypoint name="_P4" x="2772.9" y="-2236.1"/>
<waypoint alt="1404.0" name="AF" x="656.6" y="-321.0"/>
<waypoint alt="1347.0" name="TD" x="1155.4" y="-354.1"/>
<waypoint name="CLIMB" x="1142.0" y="-97.9"/>
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
</waypoints>
<sectors>
<sector name="Section1">
<corner name="S1"/>
<corner name="_S2"/>
<corner name="_S3"/>
<corner name="_S4"/>
</sector>
<sector name="FlightArea">
<corner name="_P1"/>
<corner name="_P2"/>
<corner name="_P3"/>
<corner name="_P4"/>
</sector>
</sectors>
<includes>
<include name="L" procedure="AGGIEAIR/aggieair_landing.xml"/>
</includes>
<exceptions>
<!--exception cond="datalink_time > 30" deroute="Standby"/-->
<exception cond="InsideFlightArea(GetPosX(), GetPosY()) == FALSE" deroute="Go Home"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<set value="1" var="kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Setup Wait">
<set value="1" var="kill_throttle"/>
<while cond="TRUE"/>
</block>
<block name="CircleUpTo1000">
<circle alt="2350" radius="500" wp="STDBY"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<circle radius="200" wp="STDBY"/>
</block>
<block name="StandbyLeft">
<circle radius="-200" wp="STDBY"/>
</block>
<block name="Figure 8 around Standby" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png">
<eight center="STDBY" radius="nav_radius" turn_around="1"/>
</block>
<block name="Small Figure 8 around Standby" strip_button="Figure 8 (wp 1-2)" strip_icon="eight.png">
<eight center="STDBY" radius="25" turn_around="1"/>
</block>
<block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png">
<oval p1="1" p2="2" radius="nav_radius"/>
</block>
<block name="Poly Survey">
<call fun="nav_survey_poly_osam_setup(WP_S1, 4, 200, 90)"/>
<exception cond="PolySurveySweepBackNum >=1" deroute="Standby"/>
<call fun="nav_survey_poly_osam_run()"/>
</block>
<block name="Line 1-2" strip_button="Line (wp 1-2)" strip_icon="line.png">
<call fun="nav_line_setup()"/>
<call fun="nav_line_run(WP_1, WP_2, nav_radius)"/>
</block>
<block name="Flower">
<call fun="nav_flower_setup(WP_1,WP_2)"/>
<call fun="nav_flower_run()"/>
</block>
<block name="Go Home">
<circle radius="nav_radius" wp="HOME"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/AGGIEAIR/aggieair_landing.xml
+14
View File
@@ -0,0 +1,14 @@
<!DOCTYPE procedure SYSTEM "../flight_plan.dtd">
<procedure>
<blocks>
<block name="Landing CW">
<call fun="nav_skid_landing_setup(WP_AF, WP_TD, nav_radius)"/>
<call fun="nav_skid_landing_run()"/>
</block>
<block name="Landing CCW">
<call fun="nav_skid_landing_setup(WP_AF, WP_TD, -nav_radius)"/>
<call fun="nav_skid_landing_run()"/>
</block>
</blocks>
</procedure>
conf/modules/nav_skid_landing.xml
+72
View File
@@ -0,0 +1,72 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="nav_skid_landing" dir="nav">
<doc>
<description>
Landing on skidpads
See video of the landing: https://www.youtube.com/watch?v=aYrB7s3oeX4
Standard landing procedure:
1) circle down passing AF waypoint (from left or right)
2) once low enough follow line to TD waypoint
3) once low enough flare
Use this in your airfame config file:
<!--
Block example:
@verbatim
<section name="LANDING" prefix="SKID_LANDING_">
<define name="AF_HEIGHT" value="50" unit="m"/>
<define name="FINAL_HEIGHT" value="50" unit="m"/>
<define name="FINAL_STAGE_TIME" value="10" unit="s"/>
</section>
@endverbatim
-->
Also define
<!--
Block example:
@verbatim:
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="LANDING_THROTTLE_PGAIN" value="600.0"/>
<define name="LANDING_THROTTLE_IGAIN" value="10.0"/>
<define name="LANDING_THROTTLE_MAX" value="0.65"/>
<define name="LANDING_DESIRED_SPEED" value="18" unit="m/s"/>
<define name="LANDING_PITCH_PGAIN" value="0.1"/>
<define name="LANDING_PITCH_IGAIN" value="0.1"/>
<define name="LANDING_PITCH_LIMITS" value="0.2" unit="rad"/>
<define name="LANDING_PITCH_FLARE" value="0.06"/>
<define name="LANDING_ALT_THROTTLE_KILL" value="15" unit="m"/>
<define name="LANDING_ALT_FLARE" value="5" unit="m"/>
</section>
@endverbatim
-->
to properly use landing control loop
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="NavSkidLanding">
<dl_setting var="v_ctl_landing_throttle_pgain" min="0" step="1" max="1000" shortname="throttle_pgain"/>
<dl_setting var="v_ctl_landing_throttle_igain" min="0" step="0.1" max="100" shortname="throttle_igain"/>
<dl_setting var="v_ctl_landing_throttle_max" min="0" step="0.01" max="1" shortname="throttle_max"/>
<dl_setting var="v_ctl_landing_desired_speed" min="1" step="1" max="50" shortname="landing_speed"/>
<dl_setting var="v_ctl_landing_pitch_pgain" min="0" step="0.01" max="10" shortname="pitch_pgain"/>
<dl_setting var="v_ctl_landing_pitch_igain" min="0" step="0.01" max="10" shortname="pitch_igain"/>
<dl_setting var="v_ctl_landing_pitch_limits" min="0" step="0.01" max="1" shortname="pitch_limits"/>
<dl_setting var="v_ctl_landing_pitch_flare" min="0" step="0.01" max="1" shortname="flare_pgain"/>
<dl_setting var="v_ctl_landing_alt_throttle_kill" min="0" step="1" max="30" shortname="motor_kill_alt"/>
<dl_setting var="v_ctl_landing_alt_flare" min="0" step="1" max="30" shortname="flare_alt"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="nav_skid_landing.h"/>
</header>
<makefile target="ap|sim|nps">
<file name="nav_skid_landing.c"/>
<define name="CTRL_VERTICAL_LANDING" value="1"/>
</makefile>
</module>
sw/airborne/firmwares/fixedwing/guidance/guidance_common.h
+3 -1
View File
@@ -37,7 +37,8 @@
#define V_CTL_MODE_AUTO_THROTTLE 1
#define V_CTL_MODE_AUTO_CLIMB 2
#define V_CTL_MODE_AUTO_ALT 3
#define V_CTL_MODE_NB 4
#define V_CTL_MODE_LANDING 4
#define V_CTL_MODE_NB 5
extern uint8_t v_ctl_mode;
/* Inner loop */
@@ -69,6 +70,7 @@ extern float v_ctl_pitch_setpoint;
extern void v_ctl_init(void);
extern void v_ctl_altitude_loop(void);
extern void v_ctl_climb_loop(void);
extern void v_ctl_landing_loop(void);
/** Computes throttle_slewed from throttle_setpoint */
extern void v_ctl_throttle_slew(void);
sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+109
View File
@@ -30,6 +30,7 @@
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "firmwares/fixedwing/autopilot.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" //> allow for roll control during landing final flare
/* mode */
uint8_t v_ctl_mode;
@@ -118,12 +119,75 @@ float v_ctl_auto_groundspeed_sum_err;
#define V_CTL_ALTITUDE_PRE_CLIMB_CORRECTION 1.0f
#endif
#if CTRL_VERTICAL_LANDING
#ifndef V_CTL_LANDING_THROTTLE_PGAIN
#error "V_CTL_LANDING_THROTTLE_PGAIN undefined, please define it in your airfame config file"
#endif
#ifndef V_CTL_LANDING_THROTTLE_IGAIN
#error "V_CTL_LANDING_THROTTLE_IGAIN undefined, please define it in your airfame config file"
#endif
#ifndef V_CTL_LANDING_THROTTLE_MAX
INFO("V_CTL_LANDING_THROTTLE_MAX undefined, using V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE instead")
#define V_CTL_LANDING_THROTTLE_MAX V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE
#endif
#ifndef V_CTL_LANDING_DESIRED_SPEED
#error "V_CTL_LANDING_DESIRED_SPEED undefined, please define it in your airfame config file"
#endif
#ifndef V_CTL_LANDING_PITCH_PGAIN
INFO("V_CTL_LANDING_PITCH_PGAIN undefined, using V_CTL_AUTO_PITCH_PGAIN instead")
#define V_CTL_LANDING_PITCH_PGAIN V_CTL_AUTO_PITCH_PGAIN
#endif
#ifndef V_CTL_LANDING_PITCH_IGAIN
INFO("V_CTL_LANDING_PITCH_IGAIN undefined, using V_CTL_AUTO_PITCH_IGAIN instead")
#define V_CTL_LANDING_PITCH_IGAIN V_CTL_AUTO_PITCH_IGAIN
#endif
#ifndef V_CTL_LANDING_PITCH_LIMITS
INFO("V_CTL_LANDING_PITCH_LIMITS undefined, using V_CTL_AUTO_PITCH_MAX_PITCH instead")
#define V_CTL_LANDING_PITCH_LIMITS V_CTL_AUTO_PITCH_MAX_PITCH
#endif
#ifndef V_CTL_LANDING_PITCH_FLARE
#error "V_CTL_LANDING_PITCH_FLARE undefined, please define it in your airfame config file"
#endif
#ifndef V_CTL_LANDING_ALT_THROTTLE_KILL
#error "V_CTL_LANDING_ALT_THROTTLE_KILL undefined, please define it in your airfame config file"
#endif
#ifndef V_CTL_LANDING_ALT_FLARE
#error "V_CTL_LANDING_ALT_FLARE undefined, please define it in your airfame config file"
#endif
float v_ctl_landing_throttle_pgain;
float v_ctl_landing_throttle_igain;
float v_ctl_landing_throttle_max;
float v_ctl_landing_desired_speed;
float v_ctl_landing_pitch_pgain;
float v_ctl_landing_pitch_igain;
float v_ctl_landing_pitch_limits;
float v_ctl_landing_pitch_flare;
float v_ctl_landing_alt_throttle_kill; //> must be greater than alt_flare
float v_ctl_landing_alt_flare;
#endif /* CTRL_VERTICAL_LANDING */
void v_ctl_init(void)
{
/* mode */
v_ctl_mode = V_CTL_MODE_MANUAL;
#if CTRL_VERTICAL_LANDING
/* improved landing routine */
v_ctl_landing_throttle_pgain = V_CTL_LANDING_THROTTLE_PGAIN;
v_ctl_landing_throttle_igain = V_CTL_LANDING_THROTTLE_IGAIN;
v_ctl_landing_throttle_max = V_CTL_LANDING_THROTTLE_MAX;
v_ctl_landing_desired_speed = V_CTL_LANDING_DESIRED_SPEED;
v_ctl_landing_pitch_pgain = V_CTL_LANDING_PITCH_PGAIN;
v_ctl_landing_pitch_igain = V_CTL_LANDING_PITCH_IGAIN;
v_ctl_landing_pitch_limits = V_CTL_LANDING_PITCH_LIMITS;
v_ctl_landing_pitch_flare = V_CTL_LANDING_PITCH_FLARE;
v_ctl_landing_alt_throttle_kill = V_CTL_LANDING_ALT_THROTTLE_KILL;
v_ctl_landing_alt_flare = V_CTL_LANDING_ALT_FLARE;
#endif /* CTRL_VERTICAL_LANDING */
/* outer loop */
v_ctl_altitude_setpoint = 0.;
v_ctl_altitude_pre_climb = 0.;
@@ -251,6 +315,51 @@ void v_ctl_climb_loop(void)
}
}
void v_ctl_landing_loop(void)
{
#if CTRL_VERTICAL_LANDING
static float land_speed_i_err;
static float land_alt_i_err;
static float kill_alt;
float land_speed_err = v_ctl_landing_desired_speed - stateGetHorizontalSpeedNorm_f();
float land_alt_err = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
if (kill_throttle
&& (kill_alt - v_ctl_altitude_setpoint)
> (v_ctl_landing_alt_throttle_kill - v_ctl_landing_alt_flare)) {
v_ctl_throttle_setpoint = 0.0; // Throttle is already in KILL (command redundancy)
nav_pitch = v_ctl_landing_pitch_flare; // desired final flare pitch
lateral_mode = LATERAL_MODE_ROLL; //override course correction during flare - eliminate possibility of catching wing tip due to course correction
h_ctl_roll_setpoint = 0.0; // command zero roll during flare
} else {
// set throttle based on altitude error
v_ctl_throttle_setpoint = land_alt_err * v_ctl_landing_throttle_pgain
+ land_alt_i_err * v_ctl_landing_throttle_igain;
Bound(v_ctl_throttle_setpoint, 0, v_ctl_landing_throttle_max * MAX_PPRZ); // cut off throttle cmd at specified MAX
land_alt_i_err += land_alt_err / CONTROL_FREQUENCY; // integrator land_alt_err, divide by control frequency
BoundAbs(land_alt_i_err, 50); // integrator sat limits
// set pitch based on ground speed error
nav_pitch -= land_speed_err * v_ctl_landing_pitch_pgain / 1000
+ land_speed_i_err * v_ctl_landing_pitch_igain / 1000; // 1000 is a multiplier to get more reasonable gains for ctl_basic
Bound(nav_pitch, -v_ctl_landing_pitch_limits, v_ctl_landing_pitch_limits); //max pitch authority for landing
land_speed_i_err += land_speed_err / CONTROL_FREQUENCY; // integrator land_speed_err, divide by control frequency
BoundAbs(land_speed_i_err, .2); // integrator sat limits
// update kill_alt until final kill throttle is initiated - allows for mode switch to first part of if statement above
// eliminates the need for knowing the altitude of TD
if (!kill_throttle) {
kill_alt = v_ctl_altitude_setpoint; //
if (land_alt_err > 0.0) {
nav_pitch = 0.01; // if below desired alt close to ground command level pitch
}
}
}
#endif /* CTRL_VERTICAL_LANDING */
}
/**
* auto throttle inner loop
* \brief
sw/airborne/firmwares/fixedwing/guidance/guidance_v.h
+13
View File
@@ -83,4 +83,17 @@ extern float v_ctl_auto_groundspeed_igain;
extern float v_ctl_auto_groundspeed_sum_err;
#endif
#if CTRL_VERTICAL_LANDING
extern float v_ctl_landing_throttle_pgain;
extern float v_ctl_landing_throttle_igain;
extern float v_ctl_landing_throttle_max;
extern float v_ctl_landing_desired_speed;
extern float v_ctl_landing_pitch_pgain;
extern float v_ctl_landing_pitch_igain;
extern float v_ctl_landing_pitch_limits;
extern float v_ctl_landing_pitch_flare;
extern float v_ctl_landing_alt_throttle_kill;
extern float v_ctl_landing_alt_flare;
#endif
#endif /* FW_V_CTL_H */
sw/airborne/firmwares/fixedwing/main_ap.c
+16 -6
View File
@@ -565,12 +565,22 @@ void attitude_loop(void)
{
if (pprz_mode >= PPRZ_MODE_AUTO2) {
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
} else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
v_ctl_climb_loop();
}
#if CTRL_VERTICAL_LANDING
if (v_ctl_mode == V_CTL_MODE_LANDING) {
v_ctl_landing_loop();
} else {
#endif
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
} else {
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) {
v_ctl_climb_loop();
} /* v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB */
} /* v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE */
#if CTRL_VERTICAL_LANDING
} /* v_ctl_mode == V_CTL_MODE_LANDING */
#endif
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ);
sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
+6 -1
View File
@@ -451,7 +451,12 @@ inline static void h_ctl_pitch_loop(void)
h_ctl_elevator_of_roll = 0.;
}
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
if (v_ctl_mode == V_CTL_MODE_LANDING) {
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint;
}
else {
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
}
float err = 0;
sw/airborne/modules/nav/nav_skid_landing.c
+213
View File
@@ -0,0 +1,213 @@
/*
*
* Copyright (C) 2016, Michal Podhradsky, Thomas Fisher
*
* AggieAir, Utah State University
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file modules/nav/nav_skid_landing.c
* @brief Landing on skidpads
* See video of the landing: https://www.youtube.com/watch?v=aYrB7s3oeX4
* Standard landing procedure:
* 1) circle down passing AF waypoint (from left or right)
* 2) once low enough follow line to TD waypoint
* 3) once low enough flare
*
* Use this in your airfame config file:
* <section name="LANDING" prefix="SKID_LANDING_">
* <define name="AF_HEIGHT" value="50" unit="m"/>
* <define name="FINAL_HEIGHT" value="50" unit="m"/>
* <define name="FINAL_STAGE_TIME" value="10" unit="s"/>
* </section>
*
* Also define:
* V_CTL_LANDING_THROTTLE_PGAIN - landing throttle P gain
* V_CTL_LANDING_THROTTLE_IGAIN - landing throttle I gain
* V_CTL_LANDING_THROTTLE_MAX - max landing throttle
* V_CTL_LANDING_DESIRED_SPEED - desired landing speed
* V_CTL_LANDING_PITCH_PGAIN - landing P gain
* V_CTL_LANDING_PITCH_IGAIN - landing I gain
* V_CTL_LANDING_PITCH_LIMITS - pitch limits during landing
* V_CTL_LANDING_PITCH_FLARE - flare P gain
* V_CTL_LANDING_ALT_THROTTLE_KILL - AGL to kill throttle during landing
* V_CTL_LANDING_ALT_FLARE - AGL to initiate final flare
*
* to properly use landing control loop
*/
#include "generated/airframe.h"
#include "state.h"
#include "modules/nav/nav_skid_landing.h"
#include "firmwares/fixedwing/autopilot.h"
#include "firmwares/fixedwing/nav.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
enum LandingStatus
{
CircleDown, LandingWait, Final, Approach
};
static enum LandingStatus skid_landing_status;
static uint8_t aw_waypoint;
static uint8_t td_waypoint;
static float land_radius;
static struct FloatVect2 land_circle;
static float land_app_alt;
static float land_circle_quadrant;
static float approach_quadrant;
static float final_land_altitude;
static uint8_t final_land_count;
#ifndef SKID_LANDING_AF_HEIGHT //> AF height [m]
#define SKID_LANDING_AF_HEIGHT 50
#endif
#ifndef SKID_LANDING_FINAL_HEIGHT //> final height [m]
#define SKID_LANDING_FINAL_HEIGHT 5
#endif
#ifndef SKID_LANDING_FINAL_STAGE_TIME //> final stage time [s]
#define SKID_LANDING_FINAL_STAGE_TIME 5
#endif
static inline float distance_equation(struct FloatVect2 p1,struct FloatVect2 p2)
{
return sqrtf((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
}
bool nav_skid_landing_setup(uint8_t afwp, uint8_t tdwp, float radius)
{
aw_waypoint = afwp;
td_waypoint = tdwp;
skid_landing_status = CircleDown;
land_radius = radius;
land_app_alt = stateGetPositionUtm_f()->alt;
final_land_altitude = SKID_LANDING_FINAL_HEIGHT;
final_land_count = 1;
float x_0 = waypoints[td_waypoint].x - waypoints[aw_waypoint].x;
float y_0 = waypoints[td_waypoint].y - waypoints[aw_waypoint].y;
/* Unit vector from AF to TD */
float d = sqrtf(x_0 * x_0 + y_0 * y_0);
float x_1 = x_0 / d;
float y_1 = y_0 / d;
land_circle.x = waypoints[aw_waypoint].x + y_1 * land_radius;
land_circle.y = waypoints[aw_waypoint].y - x_1 * land_radius;
land_circle_quadrant = atan2(waypoints[aw_waypoint].x - land_circle.x,
waypoints[aw_waypoint].y - land_circle.y);
if (land_radius > 0) {
approach_quadrant = land_circle_quadrant - RadOfDeg(90);
land_circle_quadrant = land_circle_quadrant - RadOfDeg(45);
} else {
approach_quadrant = land_circle_quadrant + RadOfDeg(90);
land_circle_quadrant = land_circle_quadrant + RadOfDeg(45);
}
return FALSE;
}
bool nav_skid_landing_run(void)
{
switch (skid_landing_status) {
case CircleDown:
NavVerticalAutoThrottleMode(0); // No pitch
NavVerticalAltitudeMode(waypoints[aw_waypoint].a, 0);
nav_circle_XY(land_circle.x, land_circle.y, land_radius);
if (stateGetPositionUtm_f()->alt < waypoints[aw_waypoint].a + 5) {
skid_landing_status = LandingWait;
nav_init_stage();
}
break;
case LandingWait:
NavVerticalAutoThrottleMode(0); // No pitch
NavVerticalAltitudeMode(waypoints[aw_waypoint].a, 0);
nav_circle_XY(land_circle.x, land_circle.y, land_radius);
if (NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(approach_quadrant))) {
skid_landing_status = Approach;
nav_init_stage();
}
break;
case Approach:
NavVerticalAutoThrottleMode(0); // No pitch
NavVerticalAltitudeMode(waypoints[aw_waypoint].a, 0);
nav_circle_XY(land_circle.x, land_circle.y, land_radius);
if (NavQdrCloseTo(DegOfRad(land_circle_quadrant))) {
skid_landing_status = Final;
nav_init_stage();
}
break;
case Final:
if ((stateGetPositionUtm_f()->alt - waypoints[td_waypoint].a)
< v_ctl_landing_alt_throttle_kill) {
kill_throttle = 1;
}
nav_skid_landing_glide(aw_waypoint, td_waypoint);
if (!kill_throttle) {
nav_route_xy(waypoints[aw_waypoint].x, waypoints[aw_waypoint].y,
waypoints[td_waypoint].x, waypoints[td_waypoint].y);
}
break;
default:
break;
}
return TRUE;
}
void nav_skid_landing_glide(uint8_t From_WP, uint8_t To_WP)
{
struct FloatVect2 p_from;
struct FloatVect2 p_to;
struct FloatVect2 now;
float start_alt = waypoints[From_WP].a;
float diff_alt = waypoints[To_WP].a - start_alt;
p_from.x = waypoints[From_WP].x;
p_from.y = waypoints[From_WP].y;
p_to.x = waypoints[To_WP].x;
p_to.y = waypoints[To_WP].y;
now.x = stateGetPositionEnu_f()->x;
now.y = stateGetPositionEnu_f()->y;
float end_dist = distance_equation(p_from,p_to);
float here_dist = distance_equation(p_from,now);
float end_here_dist = distance_equation(p_to,now);
float alt = start_alt + (here_dist/end_dist) * diff_alt;
if (end_dist < end_here_dist){
alt = start_alt;
}
v_ctl_mode = V_CTL_MODE_LANDING;
if ((stateGetPositionUtm_f()->alt-waypoints[To_WP].a)<0.0){
alt = waypoints[To_WP].a;
}
nav_altitude = alt;
}
sw/airborne/modules/nav/nav_skid_landing.h
+68
View File
@@ -0,0 +1,68 @@
/*
*
* Copyright (C) 2016, Michal Podhradsky, Thomas Fisher
*
* AggieAir, Utah State University
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/**
* @file modules/nav/nav_skid_landing.h
* @brief Landing on skidpads
* See video of the landing: https://www.youtube.com/watch?v=aYrB7s3oeX4
* Standard landing procedure:
* 1) circle down passing AF waypoint (from left or right)
* 2) once low enough follow line to TD waypoint
* 3) once low enough flare
*
* Use this in your airfame config file:
* <section name="LANDING" prefix="SKID_LANDING_">
* <define name="AF_HEIGHT" value="50" unit="m"/>
* <define name="FINAL_HEIGHT" value="50" unit="m"/>
* <define name="FINAL_STAGE_TIME" value="10" unit="s"/>
* </section>
*
* Also define:
* V_CTL_LANDING_THROTTLE_PGAIN - landing throttle P gain
* V_CTL_LANDING_THROTTLE_IGAIN - landing throttle I gain
* V_CTL_LANDING_THROTTLE_MAX - max landing throttle
* V_CTL_LANDING_DESIRED_SPEED - desired landing speed
* V_CTL_LANDING_PITCH_PGAIN - landing P gain
* V_CTL_LANDING_PITCH_IGAIN - landing I gain
* V_CTL_LANDING_PITCH_LIMITS - pitch limits during landing
* V_CTL_LANDING_PITCH_FLARE - flare P gain
* V_CTL_LANDING_ALT_THROTTLE_KILL - AGL to kill throttle during landing
* V_CTL_LANDING_ALT_FLARE - AGL to initiate final flare
*
* to properly use landing control loop
*/
#ifndef NAV_SKID_LANDING_H
#define NAV_SKID_LANDING_H
#include "std.h"
#include "paparazzi.h"
extern bool nav_skid_landing_setup(uint8_t afwp, uint8_t tdwp, float radius);
extern bool nav_skid_landing_run(void);
void nav_skid_landing_glide(uint8_t from_wp, uint8_t to_wp);
#endif /* NAV_SKID_LANDING_H */