[guided] support full ref update in guided mode (#3502)

New message can set position, speed, acceleration and heading setpoint
for an accurate trajectory tracking.

Also fix the update of ref when h_ref is disabled.

conf/modules/autopilot_guided.xml

@@ -15,6 +15,7 @@
     <file name="autopilot_guided.h" dir="."/>
   </header>
   <datalink message="GUIDED_SETPOINT_NED" fun="autopilot_guided_parse_GUIDED(buf)" cond="AP_MODE_GUIDED"/>
+  <datalink message="GUIDED_FULL_NED" fun="autopilot_guided_parse_GUIDED_FULL(buf)" cond="AP_MODE_GUIDED"/>
   <makefile target="!fbw" firmware="rotorcraft">
     <file name="autopilot_guided.c" dir="$(SRC_FIRMWARE)"/>
   </makefile>

sw/airborne/firmwares/rotorcraft/autopilot_guided.c

@@ -163,6 +163,29 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) {
       DL_GUIDED_SETPOINT_NED_yaw(buf));
 }
 
+void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf) {
+  if (DL_GUIDED_FULL_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) {
+    return;
+  }
+
+  guidance_h_set_all(
+      DL_GUIDED_FULL_NED_x(buf),
+      DL_GUIDED_FULL_NED_y(buf),
+      DL_GUIDED_FULL_NED_vx(buf),
+      DL_GUIDED_FULL_NED_vy(buf),
+      DL_GUIDED_FULL_NED_ax(buf),
+      DL_GUIDED_FULL_NED_ay(buf)
+      );
+  guidance_h_set_heading(
+      DL_GUIDED_FULL_NED_heading(buf)
+      );
+  guidance_v_set_all(
+      DL_GUIDED_FULL_NED_z(buf),
+      DL_GUIDED_FULL_NED_vz(buf),
+      DL_GUIDED_FULL_NED_az(buf)
+      );
+
+}
 #else
 
 bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
@@ -216,5 +239,8 @@ void autopilot_guided_parse_GUIDED(uint8_t *buf) {
   (void) buf;
 }
 
+void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf) {
+  (void) buf;
+}
 #endif
 

sw/airborne/firmwares/rotorcraft/autopilot_guided.h

@@ -92,6 +92,10 @@ extern void autopilot_guided_update(uint8_t flags, float x, float y, float z, fl
  */
 extern void autopilot_guided_parse_GUIDED(uint8_t *buf);
 
+/** Parse GUIDED_FULL_NED messages from datalink
+ */
+extern void autopilot_guided_parse_GUIDED_FULL(uint8_t *buf);
+
 /** Bitmask for setting the flags attribute in autopilot_guided_update function
  *  See function description for more details
  */

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

@@ -274,7 +274,16 @@ static void guidance_h_update_reference(void)
 {
   /* compute reference even if usage temporarily disabled via guidance_h_use_ref */
 #if GUIDANCE_H_USE_REF
-  if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) {
+  if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ALL) {
+    struct FloatVect2 sp_accel;
+    sp_accel.x = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.x);
+    sp_accel.y = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.y);
+    struct FloatVect2 sp_speed;
+    sp_speed.x = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.x);
+    sp_speed.y = SPEED_FLOAT_OF_BFP(guidance_h.sp.speed.y);
+    gh_set_ref(guidance_h.sp.pos, sp_speed, sp_accel);
+  }
+  else if (guidance_h.sp.h_mask == GUIDANCE_H_SP_ACCEL) {
     struct FloatVect2 sp_accel_local;
     sp_accel_local.x = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.x);
     sp_accel_local.y = ACCEL_FLOAT_OF_BFP(guidance_h.sp.accel.y);
@@ -293,37 +302,44 @@ static void guidance_h_update_reference(void)
   /* either use the reference or simply copy the pos setpoint */
   if (guidance_h.use_ref) {
     /* convert our reference to generic representation */
-    INT32_VECT2_RSHIFT(guidance_h.ref.pos,   gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC));
+    INT32_VECT2_RSHIFT(guidance_h.ref.pos, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC));
     guidance_h.ref.speed.x = SPEED_BFP_OF_REAL(gh_ref.speed.x);
     guidance_h.ref.speed.y = SPEED_BFP_OF_REAL(gh_ref.speed.y);
     guidance_h.ref.accel.x = ACCEL_BFP_OF_REAL(gh_ref.accel.x);
     guidance_h.ref.accel.y = ACCEL_BFP_OF_REAL(gh_ref.accel.y);
   } else {
-    switch (nav.setpoint_mode) {
-      case NAV_SETPOINT_MODE_SPEED:
-        guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
-        guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
-        guidance_h.ref.speed.x = guidance_h.sp.speed.x;
-        guidance_h.ref.speed.y = guidance_h.sp.speed.y;
-        guidance_h.ref.accel.x = 0;
-        guidance_h.ref.accel.y = 0;
-        break;
+    if (guidance_h.mode == GUIDANCE_H_MODE_NAV) {
+      switch (nav.setpoint_mode) {
+        case NAV_SETPOINT_MODE_SPEED:
+          guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
+          guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
+          guidance_h.ref.speed.x = guidance_h.sp.speed.x;
+          guidance_h.ref.speed.y = guidance_h.sp.speed.y;
+          guidance_h.ref.accel.x = 0;
+          guidance_h.ref.accel.y = 0;
+          break;
 
-      case NAV_SETPOINT_MODE_ACCEL:
-        guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
-        guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
-        guidance_h.ref.speed.x = stateGetSpeedNed_i()->x;
-        guidance_h.ref.speed.y = stateGetSpeedNed_i()->y;
-        guidance_h.ref.accel.x = guidance_h.sp.accel.x;
-        guidance_h.ref.accel.y = guidance_h.sp.accel.y;
-        break;
+        case NAV_SETPOINT_MODE_ACCEL:
+          guidance_h.ref.pos.x = stateGetPositionNed_i()->x;
+          guidance_h.ref.pos.y = stateGetPositionNed_i()->y;
+          guidance_h.ref.speed.x = stateGetSpeedNed_i()->x;
+          guidance_h.ref.speed.y = stateGetSpeedNed_i()->y;
+          guidance_h.ref.accel.x = guidance_h.sp.accel.x;
+          guidance_h.ref.accel.y = guidance_h.sp.accel.y;
+          break;
 
-      case NAV_SETPOINT_MODE_POS:
-      default: // Fallback is guidance by pos
-        VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
-        INT_VECT2_ZERO(guidance_h.ref.speed);
-        INT_VECT2_ZERO(guidance_h.ref.accel);
-        break;
+        case NAV_SETPOINT_MODE_POS:
+        default: // Fallback is guidance by pos
+          VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
+          INT_VECT2_ZERO(guidance_h.ref.speed);
+          INT_VECT2_ZERO(guidance_h.ref.accel);
+          break;
+      }
+    } else {
+      // if not in NAV, copy setpoint to ref
+      VECT2_COPY(guidance_h.ref.pos, guidance_h.sp.pos);
+      VECT2_COPY(guidance_h.ref.speed, guidance_h.sp.speed);
+      VECT2_COPY(guidance_h.ref.accel, guidance_h.sp.accel);
     }
   }
 
@@ -447,7 +463,13 @@ struct StabilizationSetpoint guidance_h_guided_run(bool in_flight)
 
   guidance_h_update_reference();
 
-  guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
+  if (guidance_h.sp.h_mask == GUIDANCE_H_SP_POS || guidance_h.sp.h_mask == GUIDANCE_H_SP_ALL) {
+    guidance_h_cmd = guidance_h_run_pos(in_flight, &guidance_h);
+  } else {
+    // if not position control, run speed control (accel not supported in guided mode)
+    guidance_h_cmd = guidance_h_run_speed(in_flight, &guidance_h);
+  }
+
   /* return final attitude setpoint */
   return guidance_h_cmd;
 }
@@ -511,3 +533,14 @@ void guidance_h_set_heading_rate(float rate)
   guidance_h.sp.heading_rate = rate;
 }
 
+void guidance_h_set_all(float x, float y, float vx, float vy, float ax, float ay)
+{
+  guidance_h.sp.h_mask = GUIDANCE_H_SP_ALL;
+  guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
+  guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
+  guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
+  guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
+  guidance_h.sp.accel.x = ACCEL_BFP_OF_REAL(ax);
+  guidance_h.sp.accel.y = ACCEL_BFP_OF_REAL(ay);
+}
+

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h

@@ -80,7 +80,8 @@ struct HorizontalGuidanceSetpoint {
   enum {
     GUIDANCE_H_SP_POS   = 0,
     GUIDANCE_H_SP_SPEED = 1,
-    GUIDANCE_H_SP_ACCEL = 2
+    GUIDANCE_H_SP_ACCEL = 2,
+    GUIDANCE_H_SP_ALL   = 3
   } h_mask;
   enum {
     GUIDANCE_H_SP_YAW       = 0,
@@ -152,25 +153,35 @@ extern void guidance_h_set_body_vel(float vx, float vy);
  * @param vx North velocity (local NED frame) in meters/sec.
  * @param vy East velocity (local NED frame) in meters/sec.
  */
-extern void guidance_h_set_acc(float ax, float ay);
+extern void guidance_h_set_vel(float vx, float vy);
 
 /** Set body relative horizontal acceleration setpoint.
- * @param vx forward acceleration (body frame) in meters/sec².
- * @param vy right acceleration (body frame) in meters/sec².
+ * @param ax forward acceleration (body frame) in meters/sec².
+ * @param ay right acceleration (body frame) in meters/sec².
  */
 extern void guidance_h_set_body_acc(float ax, float ay);
 
 /** Set horizontal acceleration setpoint.
- * @param vx North acceleration (local NED frame) in meters/sec².
- * @param vy East acceleration (local NED frame) in meters/sec².
+ * @param ax North acceleration (local NED frame) in meters/sec².
+ * @param ay East acceleration (local NED frame) in meters/sec².
  */
-extern void guidance_h_set_vel(float vx, float vy);
+extern void guidance_h_set_acc(float ax, float ay);
 
 /** Set heading rate setpoint.
  * @param rate Heading rate in radians.
  */
 extern void guidance_h_set_heading_rate(float rate);
 
+/** Set position speed and acceleration (NED frame).
+ * @param x North position (local NED frame) in meters.
+ * @param y East position (local NED frame) in meters.
+ * @param vx North velocity (local NED frame) in meters/sec.
+ * @param vy East velocity (local NED frame) in meters/sec.
+ * @param ax North acceleration (local NED frame) in meters/sec².
+ * @param ay East acceleration (local NED frame) in meters/sec².
+ */
+extern void guidance_h_set_all(float x, float y, float vx, float vy, float ax, float ay);
+
 /* Make sure that ref can only be temporarily disabled for testing,
  * but not enabled if GUIDANCE_H_USE_REF was defined to FALSE.
  */

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

@@ -64,6 +64,7 @@ static bool desired_zd_updated;
 #define GUIDANCE_V_GUIDED_MODE_ZHOLD      0
 #define GUIDANCE_V_GUIDED_MODE_CLIMB      1
 #define GUIDANCE_V_GUIDED_MODE_THROTTLE   2
+#define GUIDANCE_V_GUIDED_MODE_ALL        3
 
 static int guidance_v_guided_mode;
 
@@ -145,6 +146,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
     case GUIDANCE_V_MODE_RC_CLIMB:
     case GUIDANCE_V_MODE_CLIMB:
       guidance_v.zd_sp = 0;
+      guidance_v.zdd_sp = 0;
       /* Falls through. */
     case GUIDANCE_V_MODE_NAV:
       guidance_v_run_enter();
@@ -275,6 +277,7 @@ void guidance_v_z_enter(void)
 
   /* reset speed setting */
   guidance_v.zd_sp = 0;
+  guidance_v.zdd_sp = 0;
 }
 
 void guidance_v_set_ref(int32_t pos, int32_t speed, int32_t accel)
@@ -304,18 +307,21 @@ struct ThrustSetpoint guidance_v_from_nav(bool in_flight)
   if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
     guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
     guidance_v.zd_sp = 0;
+    guidance_v.zdd_sp = 0;
     gv_update_ref_from_z_sp(guidance_v.z_sp);
     guidance_v_update_ref();
     sp = guidance_v_run_pos(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
     guidance_v.z_sp = stateGetPositionNed_i()->z;
     guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
+    guidance_v.zdd_sp = 0;
     gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
     guidance_v_update_ref();
     sp = guidance_v_run_speed(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
     guidance_v.z_sp = stateGetPositionNed_i()->z;
     guidance_v.zd_sp = stateGetSpeedNed_i()->z;
+    guidance_v.zdd_sp = 0;
     GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
     guidance_v_run_enter();
     sp = th_sp_from_thrust_i((int32_t)nav.throttle, THRUST_AXIS_Z);
@@ -359,6 +365,11 @@ struct ThrustSetpoint guidance_v_guided_run(bool in_flight)
       guidance_v.z_sp = stateGetPositionNed_i()->z; // for display only
       sp = th_sp_from_thrust_i(guidance_v.th_sp, THRUST_AXIS_Z);
       break;
+    case GUIDANCE_V_GUIDED_MODE_ALL:
+      // update full reference
+      guidance_v_set_ref(guidance_v.z_sp, guidance_v.zd_sp, guidance_v.zdd_sp);
+      sp = guidance_v_run_pos(in_flight, &guidance_v);
+      break;
     default:
       break;
   }
@@ -371,8 +382,9 @@ void guidance_v_set_z(float z)
   guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
   /* set altitude setpoint */
   guidance_v.z_sp = POS_BFP_OF_REAL(z);
-  /* reset speed setting */
+  /* reset speed and accel setting */
   guidance_v.zd_sp = 0;
+  guidance_v.zdd_sp = 0;
 }
 
 void guidance_v_set_vz(float vz)
@@ -381,6 +393,8 @@ void guidance_v_set_vz(float vz)
   guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB;
   /* set speed setting */
   guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz);
+  /* reset accel setpoint */
+  guidance_v.zdd_sp = 0;
 }
 
 void guidance_v_set_th(float th)
@@ -394,4 +408,11 @@ void guidance_v_set_th(float th)
   Bound(guidance_v.th_sp, 0, MAX_PPRZ);
 }
 
+void guidance_v_set_all(float z, float vz, float az)
+{
+  guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD;
+  guidance_v.z_sp = POS_BFP_OF_REAL(z);
+  guidance_v.zd_sp = SPEED_BFP_OF_REAL(vz);
+  guidance_v.zdd_sp = ACCEL_BFP_OF_REAL(az);
+}
 

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

@@ -55,6 +55,12 @@ struct VerticalGuidance {
    */
   int32_t zd_sp;
 
+  /** vertical acceleration setpoint in meter/s^2.
+   *  fixed point representation: Q21.10
+   *  accuracy 0.0009766, range 2097152
+   */
+  int32_t zdd_sp;
+
   /** altitude reference in meters.
    *  fixed point representation: Q23.8
    *  accuracy 0.0039, range 8388km
@@ -155,4 +161,11 @@ extern void guidance_v_set_vz(float vz);
  */
 extern void guidance_v_set_th(float th);
 
+/** Set z position, velocity and acceleration setpoint.
+ * @param z Setpoint (down is positive) in meters.
+ * @param vz Setpoint (down is positive) in meters/second.
+ * @param az Setpoint (down is positive) in meters/second².
+ */
+extern void guidance_v_set_all(float z, float vz, float az);
+
 #endif /* GUIDANCE_V_H */

sw/lib/python/guided_mode.py

@@ -125,6 +125,24 @@ class GuidedMode(object):
         """
         self.guided_cmd(ac_id, self.FLAG_XY_BODY | self.FLAG_XY_VEL | self.FLAG_Z_VEL | self.FLAG_YAW_VEL, forward, right, down, yaw_rate)
 
+    def set_full_ned(self, ac_id, x, y, z, vx=0.0, vy=0.0, vz=0.0, ax=0.0, ay=0.0, az=0.0, heading=0.0):
+        """
+        set full reference for trajectory following in NED frame
+        """
+        msg = PprzMessage("datalink", "GUIDED_FULL_NED")
+        msg['ac_id'] = ac_id
+        msg['x'] = x
+        msg['y'] = y
+        msg['z'] = z
+        msg['vx'] = vx
+        msg['vy'] = vy
+        msg['vz'] = vz
+        msg['ax'] = ax
+        msg['ay'] = ay
+        msg['az'] = az
+        msg['heading'] = heading
+        self._interface.send_raw_datalink(msg)
+
 
 
 def main():