[rotorcraft] guidance_v: limit z_ref in update_ref_from_zd_sp

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).

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -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

@@ -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

@@ -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 */