diff --git a/sw/airborne/fw_v_ctl.c b/sw/airborne/fw_v_ctl.c deleted file mode 100644 index 851bcd5a6c..0000000000 --- a/sw/airborne/fw_v_ctl.c +++ /dev/null @@ -1,379 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz - * - * 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 v_ctl_ctl - * \brief Vertical control for fixed wing vehicles. - * - */ - -#include "fw_v_ctl.h" -#include "estimator.h" -#include "nav.h" -#include "airframe.h" -#include "firmwares/fixedwing/autopilot.h" - -/* mode */ -uint8_t v_ctl_mode; - -/* outer loop */ -float v_ctl_altitude_setpoint; -float v_ctl_altitude_pre_climb; -float v_ctl_altitude_pgain; -float v_ctl_altitude_error; - -/* inner loop */ -float v_ctl_climb_setpoint; -uint8_t v_ctl_climb_mode; -uint8_t v_ctl_auto_throttle_submode; - -/* "auto throttle" inner loop parameters */ -float v_ctl_auto_throttle_cruise_throttle; -float v_ctl_auto_throttle_nominal_cruise_throttle; -float v_ctl_auto_throttle_climb_throttle_increment; -float v_ctl_auto_throttle_pgain; -float v_ctl_auto_throttle_igain; -float v_ctl_auto_throttle_dgain; -float v_ctl_auto_throttle_sum_err; -#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150 -float v_ctl_auto_throttle_pitch_of_vz_pgain; -float v_ctl_auto_throttle_pitch_of_vz_dgain; - -#ifndef V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN -#define V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN 0. -#endif - -/* agressive tuning */ -#ifdef TUNE_AGRESSIVE_CLIMB -float agr_climb_throttle; -float agr_climb_pitch; -float agr_climb_nav_ratio; -float agr_descent_throttle; -float agr_descent_pitch; -float agr_descent_nav_ratio; -#endif - -/* "auto pitch" inner loop parameters */ -float v_ctl_auto_pitch_pgain; -float v_ctl_auto_pitch_igain; -float v_ctl_auto_pitch_sum_err; -#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100 - -pprz_t v_ctl_throttle_setpoint; -pprz_t v_ctl_throttle_slewed; - -inline static void v_ctl_climb_auto_throttle_loop( void ); -#ifdef V_CTL_AUTO_PITCH_PGAIN -inline static void v_ctl_climb_auto_pitch_loop( void ); -#endif - -#ifdef USE_AIRSPEED -float v_ctl_auto_airspeed_setpoint; -float v_ctl_auto_airspeed_controlled; -float v_ctl_auto_airspeed_pgain; -float v_ctl_auto_airspeed_igain; -float v_ctl_auto_airspeed_sum_err; -float v_ctl_auto_groundspeed_setpoint; -float v_ctl_auto_groundspeed_pgain; -float v_ctl_auto_groundspeed_igain; -float v_ctl_auto_groundspeed_sum_err; -#define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200 -#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100 -#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s -#define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in aggressive climb mode -#endif - - -void v_ctl_init( void ) { - /* mode */ - v_ctl_mode = V_CTL_MODE_MANUAL; - - /* outer loop */ - v_ctl_altitude_setpoint = 0.; - v_ctl_altitude_pre_climb = 0.; - v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN; - v_ctl_altitude_error = 0.; - - /* inner loops */ - v_ctl_climb_setpoint = 0.; - v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; - v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD; - - /* "auto throttle" inner loop parameters */ - v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; - v_ctl_auto_throttle_cruise_throttle = v_ctl_auto_throttle_nominal_cruise_throttle; - v_ctl_auto_throttle_climb_throttle_increment = - V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT; - v_ctl_auto_throttle_pgain = V_CTL_AUTO_THROTTLE_PGAIN; - v_ctl_auto_throttle_igain = V_CTL_AUTO_THROTTLE_IGAIN; - v_ctl_auto_throttle_dgain = 0.; - v_ctl_auto_throttle_sum_err = 0.; - v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN; - v_ctl_auto_throttle_pitch_of_vz_dgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_DGAIN; - -#ifdef V_CTL_AUTO_PITCH_PGAIN - /* "auto pitch" inner loop parameters */ - v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN; - v_ctl_auto_pitch_igain = V_CTL_AUTO_PITCH_IGAIN; - v_ctl_auto_pitch_sum_err = 0.; -#endif - -#ifdef USE_AIRSPEED - v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT; - v_ctl_auto_airspeed_controlled = V_CTL_AUTO_AIRSPEED_SETPOINT; - v_ctl_auto_airspeed_pgain = V_CTL_AUTO_AIRSPEED_PGAIN; - v_ctl_auto_airspeed_igain = V_CTL_AUTO_AIRSPEED_IGAIN; - v_ctl_auto_airspeed_sum_err = 0.; - - v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT; - v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN; - v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN; - v_ctl_auto_groundspeed_sum_err = 0.; -#endif - - v_ctl_throttle_setpoint = 0; - -/*agressive tuning*/ -#ifdef TUNE_AGRESSIVE_CLIMB - agr_climb_throttle = AGR_CLIMB_THROTTLE; - #undef AGR_CLIMB_THROTTLE - #define AGR_CLIMB_THROTTLE agr_climb_throttle - agr_climb_pitch = AGR_CLIMB_PITCH; - #undef AGR_CLIMB_PITCH - #define AGR_CLIMB_PITCH agr_climb_pitch - agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO; - #undef AGR_CLIMB_NAV_RATIO - #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio - agr_descent_throttle = AGR_DESCENT_THROTTLE; - #undef AGR_DESCENT_THROTTLE - #define AGR_DESCENT_THROTTLE agr_descent_throttle - agr_descent_pitch = AGR_DESCENT_PITCH; - #undef AGR_DESCENT_PITCH - #define AGR_DESCENT_PITCH agr_descent_pitch - agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO; - #undef AGR_DESCENT_NAV_RATIO - #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio -#endif -} - -/** - * outer loop - * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode - */ -void v_ctl_altitude_loop( void ) { - float altitude_pgain_boost = 1.0; - -#if defined(USE_AIRSPEED) && defined(AGR_CLIMB) - // Aggressive climb mode (boost gain of altitude loop) - if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { - float dist = fabs(v_ctl_altitude_error); - altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END); - Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN); - } -#endif - - v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint; - v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error - + v_ctl_altitude_pre_climb; - BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB); - -#ifdef AGR_CLIMB - if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { - float dist = fabs(v_ctl_altitude_error); - if (dist < AGR_BLEND_END) { - v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD; - } - else if (dist > AGR_BLEND_START) { - v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_AGRESSIVE; - } - else { - v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_BLENDED; - } - } -#endif -} - -void v_ctl_climb_loop ( void ) { - switch (v_ctl_climb_mode) { - case V_CTL_CLIMB_MODE_AUTO_THROTTLE: - default: - v_ctl_climb_auto_throttle_loop(); - break; -#ifdef V_CTL_AUTO_PITCH_PGAIN - case V_CTL_CLIMB_MODE_AUTO_PITCH: - v_ctl_climb_auto_pitch_loop(); - break; -#endif - } -} - -/** - * auto throttle inner loop - * \brief - */ - -#ifndef USE_AIRSPEED - -inline static void v_ctl_climb_auto_throttle_loop(void) { - static float last_err; - - float f_throttle = 0; - float err = estimator_z_dot - v_ctl_climb_setpoint; - float d_err = err - last_err; - last_err = err; - float controlled_throttle = v_ctl_auto_throttle_cruise_throttle - + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - + v_ctl_auto_throttle_pgain * - (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err - + v_ctl_auto_throttle_dgain * d_err); - - /* pitch pre-command */ - float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain; - -#if defined AGR_CLIMB - switch (v_ctl_auto_throttle_submode) { - case V_CTL_AUTO_THROTTLE_AGRESSIVE: - if (v_ctl_climb_setpoint > 0) { /* Climbing */ - f_throttle = AGR_CLIMB_THROTTLE; - nav_pitch = AGR_CLIMB_PITCH; - } - else { /* Going down */ - f_throttle = AGR_DESCENT_THROTTLE; - nav_pitch = AGR_DESCENT_PITCH; - } - break; - - case V_CTL_AUTO_THROTTLE_BLENDED: { - float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END) - / (AGR_BLEND_START - AGR_BLEND_END); - f_throttle = (1-ratio) * controlled_throttle; - nav_pitch = (1-ratio) * v_ctl_pitch_of_vz; - v_ctl_auto_throttle_sum_err += (1-ratio) * err; - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - if (v_ctl_altitude_error < 0) { - f_throttle += ratio * AGR_CLIMB_THROTTLE; - nav_pitch += ratio * AGR_CLIMB_PITCH; - } else { - f_throttle += ratio * AGR_DESCENT_THROTTLE; - nav_pitch += ratio * AGR_DESCENT_PITCH; - } - break; - } - - case V_CTL_AUTO_THROTTLE_STANDARD: - default: -#endif - f_throttle = controlled_throttle; - v_ctl_auto_throttle_sum_err += err; - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - nav_pitch += v_ctl_pitch_of_vz; -#if defined AGR_CLIMB - break; - } /* switch submode */ -#endif - - v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); -} - -#else // USE_AIRSPEED - -inline static void v_ctl_climb_auto_throttle_loop(void) { - float f_throttle = 0; - float controlled_throttle; - float v_ctl_pitch_of_vz; - - // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up) - static float v_ctl_climb_setpoint_last = 0; - float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last; - Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT); - v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb; - v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; - - // Pitch control (input: rate of climb error, output: pitch setpoint) - float err = estimator_z_dot - v_ctl_climb_setpoint; - v_ctl_auto_pitch_sum_err += err; - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); - v_ctl_pitch_of_vz = v_ctl_auto_pitch_pgain * - (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); - - // Ground speed control loop (input: groundspeed error, output: airspeed controlled) - float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod); - v_ctl_auto_groundspeed_sum_err += err_groundspeed; - BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); - v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; - - // Do not allow controlled airspeed below the setpoint - if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) { - v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint; - v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop - } - - // Airspeed control loop (input: airspeed controlled, output: throttle controlled) - float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed); - v_ctl_auto_airspeed_sum_err += err_airspeed; - BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR); - controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain; - - // Done, set outputs - Bound(controlled_throttle, 0, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); - f_throttle = controlled_throttle; - nav_pitch = v_ctl_pitch_of_vz; - v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); - Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH); -} - -#endif // USE_AIRSPEED - - -/** - * auto pitch inner loop - * \brief computes a nav_pitch from a climb_setpoint given a fixed throttle - */ -#ifdef V_CTL_AUTO_PITCH_PGAIN -inline static void v_ctl_climb_auto_pitch_loop(void) { - float err = estimator_z_dot - v_ctl_climb_setpoint; - v_ctl_throttle_setpoint = nav_throttle_setpoint; - v_ctl_auto_pitch_sum_err += err; - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); - nav_pitch = v_ctl_auto_pitch_pgain * - (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); - Bound(nav_pitch, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH); -} -#endif - -#ifdef V_CTL_THROTTLE_SLEW_LIMITER -#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER)) -#endif - -#ifndef V_CTL_THROTTLE_SLEW -#define V_CTL_THROTTLE_SLEW 1. -#endif -/** \brief Computes slewed throttle from throttle setpoint - called at 20Hz - */ -void v_ctl_throttle_slew( void ) { - pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; - BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); - v_ctl_throttle_slewed += diff_throttle; -}