diff --git a/sw/airborne/fw_h_ctl_a.c b/sw/airborne/fw_h_ctl_a.c new file mode 100644 index 0000000000..8be8a053ed --- /dev/null +++ b/sw/airborne/fw_h_ctl_a.c @@ -0,0 +1,265 @@ +/* + * Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $ + * + * 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. + * + */ + +/** + * + * fixed wing horizontal adaptive control + * + */ + +#include "std.h" +#include "led.h" +#include "fw_h_ctl.h" +#include "fw_h_ctl_a.h" +#include "estimator.h" +#include "nav.h" +#include "airframe.h" +#include "fw_v_ctl.h" +#include "autopilot.h" + + +/* outer loop parameters */ +float h_ctl_course_setpoint; /* rad, CW/north */ +float h_ctl_course_pre_bank; +float h_ctl_course_pre_bank_correction; +float h_ctl_course_pgain; +float h_ctl_course_dgain; +float h_ctl_roll_max_setpoint; + +/* roll and pitch disabling */ +bool_t h_ctl_disabled; + +/* AUTO1 rate mode */ +bool_t h_ctl_auto1_rate; + + +float h_ctl_ref_roll_angle; +float h_ctl_ref_roll_rate; +float h_ctl_ref_roll_accel; + +/* inner roll loop parameters */ +float h_ctl_roll_setpoint; +float h_ctl_roll_pgain; +float h_ctl_roll_igain; +float h_ctl_roll_sum_err; +pprz_t h_ctl_aileron_setpoint; +float h_ctl_roll_slew; + +/* inner pitch loop parameters */ +float h_ctl_pitch_setpoint; +float h_ctl_pitch_pgain; +float h_ctl_pitch_dgain; +pprz_t h_ctl_elevator_setpoint; + +/* inner loop pre-command */ +float h_ctl_aileron_of_throttle; +float h_ctl_elevator_of_roll; + +#ifdef H_CTL_COURSE_SLEW_INCREMENT +float h_ctl_course_slew_increment; +#endif + + +inline static void h_ctl_roll_loop( void ); +inline static void h_ctl_pitch_loop( void ); + +#ifndef H_CTL_COURSE_PRE_BANK_CORRECTION +#define H_CTL_COURSE_PRE_BANK_CORRECTION 1. +#endif + +#ifndef H_CTL_COURSE_DGAIN +#define H_CTL_COURSE_DGAIN 0. +#endif + +#ifndef H_CTL_ROLL_RATE_GAIN +#define H_CTL_ROLL_RATE_GAIN 0. +#endif + +float h_ctl_roll_attitude_gain; +float h_ctl_roll_rate_gain; + +#ifdef AGR_CLIMB +static float nav_ratio; +#endif + +void h_ctl_init( void ) { + h_ctl_course_setpoint = 0.; + h_ctl_course_pre_bank = 0.; + h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION; + h_ctl_course_pgain = H_CTL_COURSE_PGAIN; + h_ctl_course_dgain = H_CTL_COURSE_DGAIN; + h_ctl_roll_max_setpoint = H_CTL_ROLL_MAX_SETPOINT; + + h_ctl_disabled = FALSE; + + h_ctl_roll_setpoint = 0.; +#ifdef H_CTL_ROLL_PGAIN + h_ctl_roll_pgain = H_CTL_ROLL_PGAIN; +#endif + h_ctl_aileron_setpoint = 0; +#ifdef H_CTL_AILERON_OF_THROTTLE + h_ctl_aileron_of_throttle = H_CTL_AILERON_OF_THROTTLE; +#endif + + h_ctl_pitch_setpoint = 0.; + h_ctl_pitch_pgain = H_CTL_PITCH_PGAIN; + h_ctl_pitch_dgain = H_CTL_PITCH_DGAIN; + h_ctl_elevator_setpoint = 0; + h_ctl_elevator_of_roll = H_CTL_ELEVATOR_OF_ROLL; + +#ifdef H_CTL_ROLL_SLEW + h_ctl_roll_slew = H_CTL_ROLL_SLEW; +#endif + +#ifdef H_CTL_COURSE_SLEW_INCREMENT + h_ctl_course_slew_increment = H_CTL_COURSE_SLEW_INCREMENT; +#endif + +#ifdef H_CTL_ROLL_ATTITUDE_GAIN + h_ctl_roll_attitude_gain = H_CTL_ROLL_ATTITUDE_GAIN; + h_ctl_roll_rate_gain = H_CTL_ROLL_RATE_GAIN; +#endif + +#ifdef AGR_CLIMB +nav_ratio=0; +#endif +} + +/** + * \brief + * + */ +void h_ctl_course_loop ( void ) { + static float last_err; + + // Ground path error + float err = estimator_hspeed_dir - h_ctl_course_setpoint; + NormRadAngle(err); + + float d_err = err - last_err; + last_err = err; + + NormRadAngle(d_err); + + float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + Bound(speed_depend_nav, 0.66, 1.5); + + float cmd = h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain); + + + +#if defined AGR_CLIMB + /** limit navigation during extreme altitude changes */ + if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */ + if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || V_CTL_AUTO_THROTTLE_BLENDED) { + BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */ + if (v_ctl_altitude_error < 0) { + nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END)); + Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1); + } else { + nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END)); + Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1); + } + cmd *= nav_ratio; + } + } +#endif + + float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank; + + h_ctl_roll_setpoint = roll_setpoint; + + BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); +} + +void h_ctl_attitude_loop ( void ) { + if (!h_ctl_disabled) { + h_ctl_roll_loop(); + h_ctl_pitch_loop(); + } +} + +inline static void h_ctl_roll_filter_setpoint( void ) { +} + +inline static void h_ctl_roll_loop( void ) { + float err = estimator_phi - h_ctl_roll_setpoint; +#ifdef SITL + static float last_err = 0; + estimator_p = (err - last_err)/(1/60.); + last_err = err; +#endif + float cmd = - h_ctl_roll_attitude_gain * err + - h_ctl_roll_rate_gain * estimator_p + + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; + + h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); +} + + + +#ifdef LOITER_TRIM + +float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM; +float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; + +inline static float loiter(void) { + static float last_elevator_trim; + float elevator_trim; + + float throttle_dif = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle; + if (throttle_dif > 0) { + float max_dif = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1); + elevator_trim = throttle_dif / max_dif * v_ctl_auto_throttle_dash_trim; + } else { + float max_dif = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1); + elevator_trim = - throttle_dif / max_dif * v_ctl_auto_throttle_loiter_trim; + } + + float max_change = (v_ctl_auto_throttle_loiter_trim - v_ctl_auto_throttle_dash_trim) / 80.; + Bound(elevator_trim, last_elevator_trim - max_change, last_elevator_trim + max_change); + + last_elevator_trim = elevator_trim; + return elevator_trim; +} +#endif + + +inline static void h_ctl_pitch_loop( void ) { + static float last_err; + /* sanity check */ + if (h_ctl_elevator_of_roll <0.) + h_ctl_elevator_of_roll = 0.; + float err = estimator_theta - h_ctl_pitch_setpoint; + float d_err = err - last_err; + last_err = err; + float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err) + + h_ctl_elevator_of_roll * fabs(estimator_phi); +#ifdef LOITER_TRIM + cmd += loiter(); +#endif + h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); +} + + diff --git a/sw/airborne/fw_h_ctl_a.h b/sw/airborne/fw_h_ctl_a.h new file mode 100644 index 0000000000..f942b0d800 --- /dev/null +++ b/sw/airborne/fw_h_ctl_a.h @@ -0,0 +1,50 @@ +/* + * Paparazzi $Id: fw_h_ctl.h 3784 2009-07-24 14:55:54Z poine $ + * + * 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. + * + */ + +/** + * + * fixed wing horizontal adaptive control + * + */ + +#ifndef FW_H_CTL_A_H +#define FW_H_CTL_A_H + +#include +#include "std.h" +#include "paparazzi.h" +#include "airframe.h" + +extern float h_ctl_roll_sum_err; +extern float h_ctl_roll_igain; +#define H_CTL_ROLL_SUM_ERR_MAX 100. + +/* inner roll loop parameters */ +extern float h_ctl_ref_roll_angle; +extern float h_ctl_ref_roll_rate; +extern float h_ctl_ref_roll_accel; + +/* inner pitch loop parameters */ + +#endif /* FW_H_CTL_A_H */