From 75f8e34e51b41852334733a7f51bbbf8808d95fa Mon Sep 17 00:00:00 2001 From: Michal Podhradsky Date: Thu, 26 May 2016 15:37:17 -0700 Subject: [PATCH] Skid landing (#1669) Skid landing navigation routine. Comes with an optional landing control for vertical control system I did test the build at a localhost. --- conf/airframes/AGGIEAIR/aggieair_conf.xml | 4 +- conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml | 35 ++- conf/conf_example.xml | 64 ++++-- conf/conf_tests.xml | 102 +++++---- .../subsystems/fixedwing/control.makefile | 2 +- .../AGGIEAIR/BasicTuning_Launcher.xml | 94 ++++++++ .../AGGIEAIR/aggieair_landing.xml | 14 ++ conf/modules/nav_skid_landing.xml | 72 ++++++ .../fixedwing/guidance/guidance_common.h | 4 +- .../firmwares/fixedwing/guidance/guidance_v.c | 109 +++++++++ .../firmwares/fixedwing/guidance/guidance_v.h | 13 ++ sw/airborne/firmwares/fixedwing/main_ap.c | 22 +- .../stabilization/stabilization_attitude.c | 7 +- sw/airborne/modules/nav/nav_skid_landing.c | 213 ++++++++++++++++++ sw/airborne/modules/nav/nav_skid_landing.h | 68 ++++++ 15 files changed, 741 insertions(+), 82 deletions(-) create mode 100644 conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml create mode 100644 conf/flight_plans/AGGIEAIR/aggieair_landing.xml create mode 100644 conf/modules/nav_skid_landing.xml create mode 100644 sw/airborne/modules/nav/nav_skid_landing.c create mode 100644 sw/airborne/modules/nav/nav_skid_landing.h diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml index 6d825da994..ccbe00d03c 100644 --- a/conf/airframes/AGGIEAIR/aggieair_conf.xml +++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml @@ -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" /> diff --git a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml index 5fb5a0fc97..c6f617d370 100644 --- a/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml +++ b/conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml @@ -39,7 +39,10 @@ RP3 Lisa MX + + + @@ -163,6 +166,18 @@ RP3 Lisa MX + + + + + + + + + + + +
@@ -199,19 +214,19 @@ RP3 Lisa MX
-
- - - - - +
+ + + + +
-
- - - +
+ + +
diff --git a/conf/conf_example.xml b/conf/conf_example.xml index 1b43c464ce..f0d4fca8e0 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -1,7 +1,7 @@ + + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 19b985f2dc..25099bbc90 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -1,7 +1,18 @@ + + + + +
+#include "subsystems/datalink/datalink.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/AGGIEAIR/aggieair_landing.xml b/conf/flight_plans/AGGIEAIR/aggieair_landing.xml new file mode 100644 index 0000000000..59f242b520 --- /dev/null +++ b/conf/flight_plans/AGGIEAIR/aggieair_landing.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/conf/modules/nav_skid_landing.xml b/conf/modules/nav_skid_landing.xml new file mode 100644 index 0000000000..2c9da3a434 --- /dev/null +++ b/conf/modules/nav_skid_landing.xml @@ -0,0 +1,72 @@ + + + + + + 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: + + +Also define + +to properly use landing control loop + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h index eb2d6c67c1..ea632ed2b9 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h @@ -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); diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index 5aec31f7b0..7fc1776ab5 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -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 diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h index 90c9fa3a34..0ecc7b4035 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h @@ -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 */ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 5bbbb64369..cae179a947 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -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); diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 02994c0e70..ae4cfb9afe 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -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; diff --git a/sw/airborne/modules/nav/nav_skid_landing.c b/sw/airborne/modules/nav/nav_skid_landing.c new file mode 100644 index 0000000000..de604f8e87 --- /dev/null +++ b/sw/airborne/modules/nav/nav_skid_landing.c @@ -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: + *
+ * + * + * + *
+ * + * 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; +} diff --git a/sw/airborne/modules/nav/nav_skid_landing.h b/sw/airborne/modules/nav/nav_skid_landing.h new file mode 100644 index 0000000000..ac7c676a14 --- /dev/null +++ b/sw/airborne/modules/nav/nav_skid_landing.h @@ -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: + *
+ * + * + * + *
+ * + * 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 */