Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

rotorcraft guidance_v: some doxygen comments and readability improvements

Browse Source
This commit is contained in:
Felix Ruess
2012-03-13 14:37:12 +01:00
parent cd845ec022
commit 6c020702fe
4 changed files with 103 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+19 -37
View File
@@ -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;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+36 -13
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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);
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+29 -18
View File
@@ -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) / E;
/* Update Covariance */
gv_adapt_P = gv_adapt_P - ((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
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
+19 -10
View File
@@ -1,6 +1,4 @@
/*
* $Id$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* 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<<GV_FREQ_FRAC)
/* reference model vaccel in meters/sec2 (output) */
/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
/* int32_4_8_t */
/** reference model vertical accel in meters/s^2 (output)
* fixed point representation with #GV_ZDD_REF_FRAC
* Q23.8 : accuracy 0.0039 , range 8388km/s^2
*/
extern int32_t gv_zdd_ref;
/** number of bits for the fractional part of #gv_zdd_ref */
#define GV_ZDD_REF_FRAC 8
/* reference model vspeed in meters/sec (output) */
/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
/** reference model vertical speed in meters/sec (output)
* fixed point representation with #GV_ZD_REF_FRAC
* Q14.17 : accuracy 0.0000076 , range 16384m/s2
*/
extern int32_t gv_zd_ref;
/** number of bits for the fractional part of #gv_zd_ref */
#define GV_ZD_REF_FRAC (GV_ZDD_REF_FRAC + GV_FREQ_FRAC)
/* reference model altitude in meters (output) */
/* Q37.26 : */
/** reference model altitude in meters (output)
* fixed point representation with #GV_Z_REF_FRAC
* Q37.26 :
*/
extern int64_t gv_z_ref;
/** number of bits for the fractional part of #gv_z_ref */
#define GV_Z_REF_FRAC (GV_ZD_REF_FRAC + GV_FREQ_FRAC)
/* Saturations definition */