From 91678391d0c52b6210a279d155cfad6d7b2d5934 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 15 Sep 2013 21:39:57 +0200 Subject: [PATCH] [rotorcraft] RC_CLIMB cleanup - add GUIDANCE_V_CLIMB_RC_DEADBAND (in pprz units, default MAX_PPRZ/10) - GUIDANCE_V_RC_CLIMB_DEAD_BAND is obsolete - GUIDANCE_V_MAX_RC_CLIMB_SPEED defaults to GUIDANCE_V_REF_MIN_ZD - GUIDANCE_V_MAX_RC_DESCENT_SPEED defaults to GUIDANCE_V_REF_MAX_ZD --- .../rotorcraft/guidance/guidance_v.c | 37 ++++++++++++++----- .../rotorcraft/guidance/guidance_v_ref.c | 7 ---- .../rotorcraft/guidance/guidance_v_ref.h | 10 +++++ 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 36bcf836c5..7ec75d152d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -62,6 +62,19 @@ PRINT_CONFIG_VAR(GUIDANCE_V_NOMINAL_HOVER_THROTTLE) PRINT_CONFIG_VAR(GUIDANCE_V_ADAPT_THROTTLE_ENABLED) +#ifndef GUIDANCE_V_CLIMB_RC_DEADBAND +#define GUIDANCE_V_CLIMB_RC_DEADBAND MAX_PPRZ/10 +#endif + +#ifndef GUIDANCE_V_MAX_RC_CLIMB_SPEED +#define GUIDANCE_V_MAX_RC_CLIMB_SPEED GUIDANCE_V_REF_MIN_ZD +#endif + +#ifndef GUIDANCE_V_MAX_RC_DESCENT_SPEED +#define GUIDANCE_V_MAX_RC_DESCENT_SPEED GUIDANCE_V_REF_MAX_ZD +#endif + + uint8_t guidance_v_mode; int32_t guidance_v_ff_cmd; int32_t guidance_v_fb_cmd; @@ -98,10 +111,10 @@ int32_t guidance_v_thrust_coeff; #define GuidanceVSetRef(_pos, _speed, _accel) { \ - gv_set_ref(_pos, _speed, _accel); \ - guidance_v_z_ref = _pos; \ - guidance_v_zd_ref = _speed; \ - guidance_v_zdd_ref = _accel; \ + gv_set_ref(_pos, _speed, _accel); \ + guidance_v_z_ref = _pos; \ + guidance_v_zd_ref = _speed; \ + guidance_v_zdd_ref = _accel; \ } static int32_t get_vertical_thrust_coeff(void); @@ -131,12 +144,18 @@ void guidance_v_read_rc(void) { guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]); - DeadBand(guidance_v_rc_zd_sp, MAX_PPRZ/10); + guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; + DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); + + static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / + (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + if(guidance_v_rc_zd_sp > 0) - guidance_v_rc_zd_sp = SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; - else - guidance_v_rc_zd_sp = -SPEED_BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD) / (MAX_PPRZ/2 - MAX_PPRZ/10) * guidance_v_rc_zd_sp; + guidance_v_rc_zd_sp *= descent_scale; + else + guidance_v_rc_zd_sp *= climb_scale; } void guidance_v_mode_changed(uint8_t new_mode) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c index 281827bb0b..31d1e2c0e4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -58,14 +58,7 @@ int64_t gv_z_ref; #endif #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC) -#ifndef GUIDANCE_V_REF_MIN_ZD -#define GUIDANCE_V_REF_MIN_ZD (-3.) -#endif #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC) - -#ifndef GUIDANCE_V_REF_MAX_ZD -#define GUIDANCE_V_REF_MAX_ZD ( 3.) -#endif #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC) /* second order model natural frequency and damping */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h index f462a07a2b..3816a9031b 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h @@ -32,6 +32,16 @@ #include "math/pprz_algebra.h" #include "math/pprz_algebra_int.h" + +#ifndef GUIDANCE_V_REF_MIN_ZD +#define GUIDANCE_V_REF_MIN_ZD (-3.) +#endif + +#ifndef GUIDANCE_V_REF_MAX_ZD +#define GUIDANCE_V_REF_MAX_ZD ( 3.) +#endif + + /** Update frequency */ #define GV_FREQ_FRAC 9