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

[rotorcraft] guidance_v: limit z_ref in update_ref_from_zd_sp

Browse Source 
Limit the gv_z_ref to GUIDANCE_V_REF_MAX_Z_DIFF (default 2m) from current z position.
This should be useful if the aircraft can't follow the alitude in climb mode (e.g. when limited by max throttle).
This commit is contained in:
Felix Ruess
2014-06-18 23:40:19 +02:00
committed by Gautier Hattenberger
parent 32c531f070
commit 2d99588153
3 changed files with 20 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+3 -3
View File
@@ -269,13 +269,13 @@ void guidance_v_run(bool_t in_flight) {
case GUIDANCE_V_MODE_RC_CLIMB:
guidance_v_zd_sp = guidance_v_rc_zd_sp;
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break;
case GUIDANCE_V_MODE_CLIMB:
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
#if NO_RC_THRUST_LIMIT
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
@@ -308,7 +308,7 @@ void guidance_v_run(bool_t in_flight) {
else if (vertical_mode == VERTICAL_MODE_CLIMB) {
guidance_v_z_sp = stateGetPositionNed_i()->z;
guidance_v_zd_sp = -nav_climb;
gv_update_ref_from_zd_sp(guidance_v_zd_sp);
gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
run_hover_loop(in_flight);
}
else if (vertical_mode == VERTICAL_MODE_MANUAL) {
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c
+11 -1
View File
@@ -58,6 +58,12 @@ int64_t gv_z_ref;
#endif
#define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
/** maximum distance altitude setpoint is advanced in climb mode */
#ifndef GUIDANCE_V_REF_MAX_Z_DIFF
#define GUIDANCE_V_REF_MAX_Z_DIFF 2.0
#endif
#define GV_MAX_Z_DIFF BFP_OF_REAL(GUIDANCE_V_REF_MAX_Z_DIFF, GV_Z_REF_FRAC)
#define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
#define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
@@ -117,11 +123,15 @@ void gv_update_ref_from_z_sp(int32_t z_sp) {
}
void gv_update_ref_from_zd_sp(int32_t zd_sp) {
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos) {
gv_z_ref += gv_zd_ref;
gv_zd_ref += gv_zdd_ref;
/* limit z_ref to GUIDANCE_V_REF_MAX_Z_DIFF from current z pos */
int64_t cur_z = ((int64_t)z_pos) << (GV_Z_REF_FRAC - INT32_POS_FRAC);
Bound(gv_z_ref, cur_z - GV_MAX_Z_DIFF, cur_z + GV_MAX_Z_DIFF);
int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC);
gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.h
+6 -1
View File
@@ -76,6 +76,11 @@ extern int64_t gv_z_ref;
extern void gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
extern void gv_update_ref_from_z_sp(int32_t z_sp);
extern void gv_update_ref_from_zd_sp(int32_t zd_sp);
/** update vertical reference from speed setpoint.
* @param zd_sp vertical speed setpoint with INT32_SPEED_FRAC
* @param cur_z current vertical position (z-down) with INT32_POS_FRAC
*/
extern void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos);
#endif /* GUIDANCE_V_REF_H */