Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[rotorcraft] vertical guidance: bound nominal command to 90% of max to keep some authority

Browse Source
This commit is contained in:
Felix Ruess
2012-07-07 23:00:46 +02:00
parent 6b875f40df
commit a5a4ac672f
4 changed files with 15 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml
+2
View File
@@ -182,6 +182,8 @@
<define name="RC_CLIMB_COEF" value ="163"/> <define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% --> <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation -->
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+2
View File
@@ -166,6 +166,8 @@
<define name="RC_CLIMB_COEF" value ="163"/> <define name="RC_CLIMB_COEF" value ="163"/>
<!-- BOOZ_SPEED_I_OF_F(1.5) * 20% --> <!-- BOOZ_SPEED_I_OF_F(1.5) * 20% -->
<define name="RC_CLIMB_DEAD_BAND" value ="160000"/> <define name="RC_CLIMB_DEAD_BAND" value ="160000"/>
<!-- NOMINAL_HOVER_THROTTLE sets a fixed value instead of the adaptive estimation -->
<!--define name="NOMINAL_HOVER_THROTTLE" value="0.5"/-->
</section> </section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_"> <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+3
View File
@@ -291,6 +291,9 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig
/* feed forward command */ /* feed forward command */
guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta; guidance_v_ff_cmd = (guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
/* bound the nominal command to 0.9*MAX_PPRZ */
Bound(guidance_v_ff_cmd, 0, 8640);
/* our error feed back command */ /* our error feed back command */
/* z-axis pointing down -> positive error means we need less thrust */ /* z-axis pointing down -> positive error means we need less thrust */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
+8 -12
View File
@@ -143,21 +143,17 @@ static inline void gv_adapt_init(void) {
*/ */
static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
/* Do you really think we want to divide by zero ?
* Negative commands are also prohibited here
*/
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;
/* Update only if accel and commands are in a valid range */ /* Update only if accel and commands are in a valid range */
/* This also ensures we don't divide by zero */
if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD 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) { || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
return; 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 30 bits */ /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 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); 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) { if ( g_m_zdd > 0) {
@@ -169,15 +165,15 @@ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_
/* Compute a residual */ /* Compute a residual */
int32_t residual = gv_adapt_Xmeas - gv_adapt_X; int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
/* Covariance Error */ /* Covariance Error E = P + R */
int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC); int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC);
if (zd_ref < 0) ref = -ref; if (zd_ref < 0) ref = -ref;
int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD; int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD;
/* Kalman gain */ /* Kalman gain K = P / (P + R) = P / E */
int32_t K = (gv_adapt_P<<K_FRAC) / E; int32_t K = (gv_adapt_P<<K_FRAC) / E;
/* Update Covariance */ /* Update Covariance Pnew = P - K * P */
gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC); gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
/* Don't let covariance climb over initial value */ /* Don't let covariance climb over initial value */
if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0; if (gv_adapt_P > GV_ADAPT_P0) gv_adapt_P = GV_ADAPT_P0;