diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index d8745f9ce5..8c4c05fba9 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -48,31 +48,24 @@ uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; -/* command output */ int32_t guidance_v_delta_t; -/* direct throttle from radio control */ -/* range 0:200 */ -int32_t guidance_v_rc_delta_t; -/* vertical speed setpoint from radio control */ -/* Q12.19 : accuracy 0.0000019, range +/-4096 */ -int32_t guidance_v_rc_zd_sp; -/* altitude setpoint in meter (input) */ -/* Q23.8 : accuracy 0.0039, range 8388km */ -int32_t guidance_v_z_sp; -/* vertical speed setpoint in meter/s (input) */ -/* Q12.19 : accuracy 0.0000019, range +/-4096 */ -int32_t guidance_v_zd_sp; -#define GUIDANCE_V_ZD_SP_FRAC INT32_SPEED_FRAC -/* altitude reference in meter */ -/* Q23.8 : accuracy 0.0039, range 8388km */ +/** Direct throttle from radio control. + * range 0:200 + */ +int32_t guidance_v_rc_delta_t; + +/** Vertical speed setpoint from radio control. + * fixed point representation: Q12.19 + * accuracy 0.0000019, range +/-4096 + */ +int32_t guidance_v_rc_zd_sp; + +int32_t guidance_v_z_sp; +int32_t guidance_v_zd_sp; int32_t guidance_v_z_ref; -/* vertical speed reference in meter/s */ -/* Q12.19 : accuracy 0.0000038, range 4096 */ int32_t guidance_v_zd_ref; -/* vertical acceleration reference in meter/s^2 */ -/* Q21.10 : accuracy 0.0009766, range 2097152 */ int32_t guidance_v_zdd_ref; int32_t guidance_v_kp; @@ -127,10 +120,6 @@ void guidance_v_mode_changed(uint8_t new_mode) { if (new_mode == guidance_v_mode) return; - // switch ( guidance_v_mode ) { - // - // } - switch (new_mode) { case GUIDANCE_V_MODE_RC_CLIMB: @@ -140,10 +129,11 @@ void guidance_v_mode_changed(uint8_t new_mode) { guidance_v_z_sum_err = 0; GuidanceVSetRef(ins_ltp_pos.z, ins_ltp_speed.z, 0); break; + default: break; - } + } guidance_v_mode = new_mode; @@ -160,10 +150,9 @@ void guidance_v_run(bool_t in_flight) { // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co if (in_flight) { - // we should use something after the supervision!!! fuck!!! + // FIXME: we should use something after the supervision!!! fuck!!! int32_t cmd_hack = Chop(stabilization_cmd[COMMAND_THRUST], SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR); gv_adapt_run(ins_ltp_accel.z, cmd_hack); - //gv_adapt_run(ins_ltp_accel.z, cmd_hack, guidance_v_zd_ref); } else { // reset vertical filter until takeoff @@ -281,28 +270,21 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig #endif const int32_t g_m_zdd = (int32_t)BFP_OF_REAL(9.81, FF_CMD_FRAC) - (guidance_v_zdd_ref<<(FF_CMD_FRAC - INT32_ACCEL_FRAC)); -#if 0 - if (g_m_zdd > 0) - guidance_v_ff_cmd = ( g_m_zdd + (inv_m>>1)) / inv_m; - else - guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m; -#else + guidance_v_ff_cmd = g_m_zdd / inv_m; int32_t cphi,ctheta,cphitheta; PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_body_euler.phi); PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_body_euler.theta); cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC; if (cphitheta < MAX_BANK_COEF) cphitheta = MAX_BANK_COEF; + /* feed forward command */ guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta; -#endif - /* our error command */ + /* our error feed back command */ guidance_v_fb_cmd = ((-guidance_v_kp * err_z) >> 12) + ((-guidance_v_kd * err_zd) >> 21) + ((-guidance_v_ki * guidance_v_z_sum_err) >> 21); guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd; - // guidance_v_delta_t = guidance_v_fb_cmd; - } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index f68367f22b..08e8ae95c5 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -38,19 +36,44 @@ extern uint8_t guidance_v_mode; +/** altitude setpoint in meters (input) + * fixed point representation: Q23.8 + * accuracy 0.0039, range 8388km + */ extern int32_t guidance_v_z_sp; -extern int32_t guidance_v_zd_sp; -extern int32_t guidance_v_z_ref; -extern int32_t guidance_v_zd_ref; -extern int32_t guidance_v_zdd_ref; -extern int32_t guidance_v_z_sum_err; -extern int32_t guidance_v_ff_cmd; -extern int32_t guidance_v_fb_cmd; -extern int32_t guidance_v_delta_t; -extern int32_t guidance_v_kp; -extern int32_t guidance_v_kd; -extern int32_t guidance_v_ki; +/** vertical speed setpoint in meter/s (input) + * fixed point representation: Q12.19 + * accuracy 0.0000019, range +/-4096 + */ +extern int32_t guidance_v_zd_sp; + +/** altitude reference in meters + * fixed point representation: Q23.8 + * accuracy 0.0039, range 8388km + */ +extern int32_t guidance_v_z_ref; + +/** vertical speed reference in meter/s + * fixed point representation: Q12.19 + * accuracy 0.0000038, range 4096 + */ +extern int32_t guidance_v_zd_ref; + +/** vertical acceleration reference in meter/s^2 + * fixed point representation: Q21.10 + * accuracy 0.0009766, range 2097152 + */ +extern int32_t guidance_v_zdd_ref; + +extern int32_t guidance_v_z_sum_err; ///< accumulator for I-gain +extern int32_t guidance_v_ff_cmd; ///< feed-forward command +extern int32_t guidance_v_fb_cmd; ///< feed-back command +extern int32_t guidance_v_delta_t; ///< command output (ff+fb) + +extern int32_t guidance_v_kp; ///< vertical control P-gain +extern int32_t guidance_v_kd; ///< vertical control D-gain +extern int32_t guidance_v_ki; ///< vertical control I-gain extern void guidance_v_init(void); extern void guidance_v_read_rc(void); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h index e57bcbc721..60c3230efb 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2009-2010 The Paparazzi Team * * This file is part of paparazzi. @@ -19,12 +17,14 @@ * 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 guidance_v_adapt.h + * Adaptation bloc of the vertical guidance. * - * - * Adaptation bloc of the vertical guidance - * This is a dimension one kalman filter estimating - * the ratio of vertical acceleration over thrust command ( ~ invert of the mass ) - * needed by the invert dynamic model to produce a nominal command + * This is a dimension one kalman filter estimating + * the ratio of vertical acceleration over thrust command ( ~ invert of the mass ) + * needed by the invert dynamic model to produce a nominal command */ #ifndef GUIDANCE_V_ADPT @@ -37,19 +37,21 @@ extern int32_t gv_adapt_Xmeas; #ifdef GUIDANCE_V_C -/* Our State - Q13.18 -*/ +/** State of the estimator + * fixed point representation with #GV_ADAPT_X_FRAC + * Q13.18 + */ int32_t gv_adapt_X; #define GV_ADAPT_X_FRAC 18 -/* Our covariance - Q13.18 -*/ +/** Covariance + * fixed point representation with #GV_ADAPT_P_FRAC + * Q13.18 + */ int32_t gv_adapt_P; #define GV_ADAPT_P_FRAC 18 -/* Our measurement */ +/** Measurement */ int32_t gv_adapt_Xmeas; @@ -98,23 +100,29 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) { /* Do you really think we want to divide by zero ? */ if (thrust_applied == 0) return; + /* We don't propagate state, it's constant ! */ /* We propagate our covariance */ gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE; - /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */ - const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC); + #if USE_ADAPT_HOVER /* Update only if accel and commands are in a valid range */ if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) return; #endif - if ( g_m_zdd > 0) + + /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */ + const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC); + if ( g_m_zdd > 0) { gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied; - else + } else { gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied; + } + /* Compute a residual */ int32_t residual = gv_adapt_Xmeas - gv_adapt_X; + /* Covariance Error */ int32_t E = 0; #if USE_ADAPT_HOVER @@ -124,12 +132,15 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) { else #endif E = gv_adapt_P + GV_ADAPT_MEAS_NOISE; + /* Kalman gain */ int32_t K = (gv_adapt_P<>K_FRAC); /* Don't let covariance climb over initial value */ if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0; + /* Update State */ gv_adapt_X = gv_adapt_X + ((K * residual)>>K_FRAC); /* Again don't let it climb over a value that would diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index 1b676645c1..171acc679d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -1,6 +1,4 @@ /* - * $Id$ - * * Copyright (C) 2008-2009 Antoine Drouin * * This file is part of paparazzi. @@ -29,24 +27,35 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" -/* update frequency */ +/* update frequency */ #define GV_FREQ_FRAC 9 #define GV_FREQ (1<