[rotorcraft] start adding velocity commands for guided mode

conf/messages.xml

@@ -2445,9 +2445,14 @@
       0x1: LOCAL_OFFSET_NED
       0x2: BODY_NED
       0x3: BODY_OFFSET_NED
+      Bits 4-7 in the flag field can be used to enable velocity control on specifc fields:
+      bit 4: x as velocity
+      bit 5: y as velocity
+      bit 6: z as velocity
+      bit 7: yaw as rate
     </description>
     <field name="ac_id" type="uint8"/>
-    <field name="flags" type="uint8">bits 0-3: frame, bits 4-7: use as velocity?</field>
+    <field name="flags" type="uint8">bits 0-3: frame, bits 4-7: use as velocity</field>
     <field name="x" type="float" unit="m">X position/velocity in NED</field>
     <field name="y" type="float" unit="m">Y position/velocity in NED</field>
     <field name="z" type="float" unit="m">Z position/velocity in NED (negative altitude)</field>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -549,6 +549,17 @@ bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float d
   return FALSE;
 }
 
+bool_t autopilot_guided_move(float vx, float vy, float vz, float heading)
+{
+  if (autopilot_mode == AP_MODE_GUIDED) {
+    guidance_h_set_guided_vel(vx, vy);
+    guidance_h_set_guided_heading(heading);
+    guidance_v_set_guided_z(vz);
+    return TRUE;
+  }
+  return FALSE;
+}
+
 void autopilot_check_in_flight(bool_t motors_on)
 {
   if (autopilot_in_flight) {

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -210,6 +210,13 @@ extern bool_t autopilot_guided_goto_ned_relative(float dx, float dy, float dz, f
  */
 extern bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw);
 
-
+/** Set velocity and heading setpoints in GUIDED mode.
+ * @param vx North velocity (local NED frame) in meters/sec.
+ * @param vy East velocity (local NED frame) in meters/sec.
+ * @param vz Down velocity (local NED frame) in meters/sec.
+ * @param heading Setpoint in radians.
+ * @return TRUE if setpoint was set (currently in AP_MODE_GUIDED)
+ */
+extern bool_t autopilot_guided_move(float vx, float vy, float vz, float heading);
 
 #endif /* AUTOPILOT_H */

sw/airborne/firmwares/rotorcraft/datalink.c

@@ -208,6 +208,10 @@ void dl_parse_msg(void)
           /* body NED offset position setpoints */
           autopilot_guided_goto_body_relative(x, y, z, yaw);
           break;
+        case 0x70:
+          /* local NED with x/y/z as velocity and yaw as absolute angle */
+          autopilot_guided_move(x, y, z, yaw);
+          break;
         default:
           /* others not handled yet */
           break;

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

@@ -442,7 +442,7 @@ 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_USE_SPEED_REF
-  if (guidance_h.mode == GUIDANCE_H_MODE_HOVER) {
+  if (bit_is_set(guidance_h.sp.mask, 4) && bit_is_set(guidance_h.sp.mask, 5)) {
     gh_update_ref_from_speed_sp(guidance_h.sp.speed);
   } else
 #endif
@@ -550,6 +550,9 @@ static void guidance_h_traj_run(bool_t in_flight)
 
 static void guidance_h_hover_enter(void)
 {
+  ClearBit(guidance_h.sp.mask, 4);
+  ClearBit(guidance_h.sp.mask, 5);
+
   /* set horizontal setpoint to current position */
   VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i());
 
@@ -561,6 +564,9 @@ static void guidance_h_hover_enter(void)
 
 static void guidance_h_nav_enter(void)
 {
+  ClearBit(guidance_h.sp.mask, 4);
+  ClearBit(guidance_h.sp.mask, 5);
+
   /* horizontal position setpoint from navigation/flightplan */
   INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
 
@@ -621,6 +627,8 @@ void guidance_h_set_igain(uint32_t igain)
 bool_t guidance_h_set_guided_pos(float x, float y)
 {
   if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
+    ClearBit(guidance_h.sp.mask, 4);
+    ClearBit(guidance_h.sp.mask, 5);
     guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
     guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
     return TRUE;
@@ -631,9 +639,22 @@ bool_t guidance_h_set_guided_pos(float x, float y)
 bool_t guidance_h_set_guided_heading(float heading)
 {
   if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
+    ClearBit(guidance_h.sp.mask, 7);
     guidance_h.sp.heading = ANGLE_BFP_OF_REAL(heading);
     INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
     return TRUE;
   }
   return FALSE;
 }
+
+bool_t guidance_h_set_guided_vel(float vx, float vy)
+{
+  if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
+    SetBit(guidance_h.sp.mask, 4);
+    SetBit(guidance_h.sp.mask, 5);
+    guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx);
+    guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy);
+    return TRUE;
+  }
+  return FALSE;
+}

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

@@ -70,6 +70,7 @@ struct HorizontalGuidanceSetpoint {
   struct Int32Vect2 pos;
   struct Int32Vect2 speed;  ///< only used if GUIDANCE_H_USE_SPEED_REF
   int32_t heading;          ///< with #INT32_ANGLE_FRAC
+  uint8_t mask;             ///< bit 4: vx, bit 5: vy, bit 6: vz, bit 7: vyaw
 };
 
 struct HorizontalGuidanceReference {
@@ -117,13 +118,20 @@ extern void guidance_h_set_igain(uint32_t igain);
  * @param y East position (local NED frame) in meters.
  * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
  */
-bool_t guidance_h_set_guided_pos(float x, float y);
+extern bool_t guidance_h_set_guided_pos(float x, float y);
 
 /** Set heading setpoint in GUIDED mode.
  * @param heading Setpoint in radians.
  * @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED)
  */
-bool_t guidance_h_set_guided_heading(float heading);
+extern bool_t guidance_h_set_guided_heading(float heading);
+
+/** Set horizontal velocity setpoint in GUIDED mode.
+ * @param x North velocity (local NED frame) in meters/sec.
+ * @param y East velocity (local NED frame) in meters/sec.
+ * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED)
+ */
+extern bool_t guidance_h_set_guided_vel(float vx, float vy);
 
 /* 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

@@ -102,7 +102,7 @@ int32_t guidance_v_delta_t;
 
 float guidance_v_nominal_throttle;
 bool_t guidance_v_adapt_throttle_enabled;
-
+bool_t guidance_v_guided_vel_enabled;
 
 /** Direct throttle from radio control.
  *  range 0:#MAX_PPRZ
@@ -184,6 +184,7 @@ void guidance_v_init(void)
 
   guidance_v_nominal_throttle = GUIDANCE_V_NOMINAL_HOVER_THROTTLE;
   guidance_v_adapt_throttle_enabled = GUIDANCE_V_ADAPT_THROTTLE_ENABLED;
+  guidance_v_guided_vel_enabled = FALSE;
 
   gv_adapt_init();
 
@@ -310,10 +311,19 @@ void guidance_v_run(bool_t in_flight)
       break;
 
     case GUIDANCE_V_MODE_HOVER:
+      guidance_v_guided_vel_enabled = FALSE;
     case GUIDANCE_V_MODE_GUIDED:
-      guidance_v_zd_sp = 0;
-      gv_update_ref_from_z_sp(guidance_v_z_sp);
-      run_hover_loop(in_flight);
+      if (guidance_v_guided_vel_enabled) {
+        gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z);
+        run_hover_loop(in_flight);
+        /* update z sp for telemetry/debuging */
+        guidance_v_z_sp = guidance_v_z_ref;
+      }
+      else {
+        guidance_v_zd_sp = 0;
+        gv_update_ref_from_z_sp(guidance_v_z_sp);
+        run_hover_loop(in_flight);
+      }
 #if !NO_RC_THRUST_LIMIT
       /* use rc limitation if available */
       if (radio_control.status == RC_OK) {
@@ -453,8 +463,19 @@ static void run_hover_loop(bool_t in_flight)
 bool_t guidance_v_set_guided_z(float z)
 {
   if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
+    guidance_v_guided_vel_enabled = FALSE;
     guidance_v_z_sp = POS_BFP_OF_REAL(z);
     return TRUE;
   }
   return FALSE;
 }
+
+bool_t guidance_v_set_guided_vz(float vz)
+{
+  if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
+    guidance_v_guided_vel_enabled = TRUE;
+    guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz);
+    return TRUE;
+  }
+  return FALSE;
+}

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

@@ -94,6 +94,7 @@ extern float guidance_v_nominal_throttle;
  */
 extern bool_t guidance_v_adapt_throttle_enabled;
 
+extern bool_t guidance_v_guided_vel_enabled;
 
 extern int32_t guidance_v_thrust_coeff;
 
@@ -113,6 +114,12 @@ extern void guidance_v_run(bool_t in_flight);
  */
 extern bool_t guidance_v_set_guided_z(float z);
 
+/** Set z velocity setpoint in GUIDED mode.
+ * @param vz Setpoint (down is positive) in meters/second.
+ * @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
+ */
+extern bool_t guidance_v_set_guided_vz(float vz);
+
 #define guidance_v_SetKi(_val) {      \
     guidance_v_ki = _val;       \
     guidance_v_z_sum_err = 0;     \

sw/ground_segment/python/guided_mode_example.py

@@ -115,9 +115,23 @@ def goto_body_relative(self, forward, right, down, yaw=0.0):
         print("goto body relative: %s" % msg)
         self._interface.send_raw_datalink(msg)
 
+    def move_at_vel(self, north=0.0, east=0.0, down=0.0, yaw=0.0):
+        """
+        move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
+        """
+        msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
+        msg['ac_id'] = self.ac_id
+        msg['flags'] = 0x70
+        msg['x'] = north
+        msg['y'] = east
+        msg['z'] = down
+        msg['yaw'] = yaw
+        print("move at vel NED: %s" % msg)
+        self._interface.send_raw_datalink(msg)
+
 
 if __name__ == '__main__':
-    ac_id = 30
+    ac_id = 40
     try:
         g = Guidance(ac_id)
         sleep(0.1)
@@ -129,6 +143,8 @@ def goto_body_relative(self, forward, right, down, yaw=0.0):
         sleep(10)
         g.goto_body_relative(forward=0.0, right=5.0, down=2.0)
         sleep(10)
+        g.move_at_vel(north=3.0)
+        sleep(10)
         g.set_nav_mode()
         sleep(0.2)
     except KeyboardInterrupt: