diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index a4ef4590db..d58d4c5fcc 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * \file v_ctl_ctl * \brief Vertical control for fixed wing vehicles. * @@ -123,14 +123,14 @@ void v_ctl_init( void ) { /* "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_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; + 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 */ @@ -161,7 +161,7 @@ void v_ctl_init( void ) { #define AGR_CLIMB_THROTTLE agr_climb_throttle agr_climb_pitch = AGR_CLIMB_PITCH; #undef AGR_CLIMB_PITCH - #define AGR_CLIMB_PITCH 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 @@ -177,9 +177,9 @@ void v_ctl_init( void ) { #endif } -/** +/** * outer loop - * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode + * \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; @@ -228,9 +228,9 @@ void v_ctl_climb_loop ( void ) { } } -/** +/** * auto throttle inner loop - * \brief + * \brief */ #ifndef USE_AIRSPEED @@ -242,12 +242,12 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { 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 * + 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; @@ -257,15 +257,15 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { 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) + 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; @@ -280,7 +280,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { } break; } - + case V_CTL_AUTO_THROTTLE_STANDARD: default: #endif @@ -346,7 +346,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { #endif // USE_AIRSPEED -/** +/** * auto pitch inner loop * \brief computes a nav_pitch from a climb_setpoint given a fixed throttle */ @@ -356,7 +356,7 @@ inline static void v_ctl_climb_auto_pitch_loop(void) { 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 * + 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); } diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h index fb4de97091..b77718c032 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.h @@ -1,6 +1,6 @@ /* * Paparazzi $Id$ - * + * * Copyright (C) 2006 Pascal Brisset, Antoine Drouin * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing vertical control * @@ -51,7 +51,7 @@ extern float v_ctl_altitude_pgain; /* inner loop */ extern float v_ctl_climb_setpoint; extern uint8_t v_ctl_climb_mode; -#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0 +#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0 #define V_CTL_CLIMB_MODE_AUTO_PITCH 1 extern uint8_t v_ctl_auto_throttle_submode; diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 2247721552..3473a4c83d 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -1,6 +1,6 @@ /* * $Id: $ - * + * * Copyright (C) 2010 ENAC * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * \file v_ctl_ctl_n * \brief Vertical control for fixed wing vehicles. * @@ -120,8 +120,8 @@ void v_ctl_init( void ) { 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; + 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; /* "auto pitch" inner loop parameters */ v_ctl_auto_pitch_pgain = V_CTL_AUTO_PITCH_PGAIN; @@ -146,9 +146,9 @@ void v_ctl_init( void ) { v_ctl_throttle_setpoint = 0; } -/** +/** * outer loop - * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode + * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode */ // Don't use lead controller unless you know what you're doing @@ -172,7 +172,7 @@ void v_ctl_altitude_loop( void ) { float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last; BoundAbs(diff_climb, V_CTL_AUTO_CLIMB_LIMIT); v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb; - + // Limit climb setpoint BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB); v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; @@ -289,4 +289,3 @@ void v_ctl_throttle_slew( void ) { BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); v_ctl_throttle_slewed += diff_throttle; } - diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h index ab8ada0e61..4a6261d35b 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.h @@ -1,6 +1,6 @@ /* * $Id: $ - * + * * Copyright (C) 2010 ENAC * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing vertical control * diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index ea6972cf0b..346076ec88 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -1,6 +1,6 @@ /* * Paparazzi $Id: fw_h_ctl.c 3603 2009-07-01 20:06:53Z hecto $ - * + * * Copyright (C) 2009-2010 The Paparazzi Team * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing horizontal adaptive control * @@ -150,8 +150,8 @@ void h_ctl_init( void ) { } -/** - * \brief +/** + * \brief * */ void h_ctl_course_loop ( void ) { @@ -165,7 +165,7 @@ void h_ctl_course_loop ( void ) { last_err = err; NormRadAngle(d_err); - float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank @@ -264,7 +264,7 @@ inline static void h_ctl_roll_loop( void ) { //x cmd /= airspeed_ratio2; // Set aileron commands - h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); + h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); } @@ -342,4 +342,3 @@ inline static void h_ctl_pitch_loop( void ) { h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); } - diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index 74105717df..c4915364cd 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -1,6 +1,6 @@ /* * Paparazzi $Id: fw_h_ctl.h 3784 2009-07-24 14:55:54Z poine $ - * + * * Copyright (C) 2009 ENAC * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing horizontal adaptive control * diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 492fb99314..b525a6dd91 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -1,6 +1,6 @@ /* * Paparazzi $Id$ - * + * * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing horizontal control * @@ -164,8 +164,8 @@ nav_ratio=0; #endif } -/** - * \brief +/** + * \brief * */ void h_ctl_course_loop ( void ) { @@ -180,7 +180,7 @@ void h_ctl_course_loop ( void ) { const float reference_advance = (NOMINAL_AIRSPEED / 2.); float advance = cos(err) * estimator_hspeed_mod / reference_advance; - if ( + if ( (advance < 1.) && // Path speed is small (estimator_hspeed_mod < reference_advance) // Small path speed is due to wind (small groundspeed) ) @@ -203,12 +203,12 @@ void h_ctl_course_loop ( void ) { // Heading error float herr = estimator_psi - h_ctl_course_setpoint; //+crab); NormRadAngle(herr); - + if (advance < -0.5) // 90 degree turn { err = herr; } - else if (advance < 0.) //= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0; - + /* D term calculations */ static float last_err = 0; float d_err = err - last_err; last_err = err; - + float throttle_dep_pgain = Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ)); float cmd = throttle_dep_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err); @@ -414,7 +414,7 @@ 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_pitch_setpoint - h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi); float err = estimator_theta - h_ctl_pitch_loop_setpoint; @@ -426,5 +426,3 @@ inline static void h_ctl_pitch_loop( void ) { #endif h_ctl_elevator_setpoint = TRIM_PPRZ(cmd); } - - diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h index 54fcda9f62..59850f5a06 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h @@ -1,6 +1,6 @@ /* * Paparazzi $Id$ - * + * * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz * * This file is part of paparazzi. @@ -18,11 +18,11 @@ * 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. + * Boston, MA 02111-1307, USA. * */ -/** +/** * * fixed wing horizontal control *