Merge pull request #1457 from paparazzi/rotorcraft_guided_mode

Rotorcraft guided mode

Set the autopilot mode to GUIDED and then send the GUIDED_SETPOINT_NED message to "goto" a position:
currently you can specify a frame in flags (first 4 bits):
- 0x0: LOCAL_NED, position in local NED frame
- 0x1: LOCAL_OFFSET_NED, position relative to where the vehicle currently is in NED
- 0x2: BODY_NED, same as LOCAL_NED for position
- 0x3: BODY_OFFSET_NED, position relative to vehicle pos AND heading (so X=forward, Y=right, Z=down)

conf/messages.xml

@@ -1995,11 +1995,11 @@
     <field name="rc_status"    type="uint8" values="OK|LOST|REALLY_LOST"/>
     <field name="frame_rate"   type="uint8" unit="Hz"/>
     <field name="gps_status"   type="uint8" values="NO_FIX|NA|2D|3D|DGPS|RTK"/>
-    <field name="ap_mode"      type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE|FLIP"/>
+    <field name="ap_mode"      type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE|FLIP|GUIDED"/>
     <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
     <field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
-    <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>
-    <field name="ap_v_mode"    type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV"/>
+    <field name="ap_h_mode"    type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|RC_DIRECT|CF|FORWARD|MODULE|FLIP|GUIDED"/>
+    <field name="ap_v_mode"    type="uint8" values="KILL|RC_DIRECT|RC_CLIMB|CLIMB|HOVER|NAV|MODULE|FLIP|GUIDED"/>
     <field name="vsupply"      type="uint16" unit="decivolt"/>
     <field name="cpu_time"     type="uint16" unit="s"/>
   </message>
@@ -2437,13 +2437,29 @@
     <field name="ac_id" type="uint8"/>
   </message>
 
+  <message name="GUIDED_SETPOINT_NED" id="40" link="forwarded">
+    <description>
+      Set vehicle position or velocity in NED.
+      Frame can be specified with the bits 0-3 in the flags field:
+      0x0: LOCAL_NED
+      0x1: LOCAL_OFFSET_NED
+      0x2: BODY_NED
+      0x3: BODY_OFFSET_NED
+    </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="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>
+    <field name="yaw" type="float" unit="rad" alt_unit="deg">yaw/rate setpoint</field>
+  </message>
 
- <message name="WINDTURBINE_STATUS" id="50" link="broadcasted">
-   <field name="ac_id" type="uint8"/>
-   <field name="tb_id" type="uint8"/>
-   <field name="sync_itow"  type="uint32" unit="ms"/>
-   <field name="cycle_time" type="uint32" unit="ms"/>
- </message>
+  <message name="WINDTURBINE_STATUS" id="50" link="broadcasted">
+    <field name="ac_id" type="uint8"/>
+    <field name="tb_id" type="uint8"/>
+    <field name="sync_itow"  type="uint32" unit="ms"/>
+    <field name="cycle_time" type="uint32" unit="ms"/>
+  </message>
 
   <message name="RC_3CH" id="51" link="broadcasted">
     <field name="throttle_mode" type="uint8" unit="byte_mask"/>

conf/settings/rotorcraft_basic.xml

@@ -4,7 +4,7 @@
   <dl_settings>
 
     <dl_settings NAME="System">
-      <dl_setting var="autopilot_mode_auto2" min="0" step="1" max="18" module="autopilot" shortname="auto2" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip"/>
+      <dl_setting var="autopilot_mode_auto2" min="0" step="1" max="19" module="autopilot" shortname="auto2" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip|Guided"/>
       <dl_setting var="kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
       <dl_setting var="autopilot_power_switch" min="0" step="1" max="1" module="autopilot" values="OFF|ON" handler="SetPowerSwitch">
         <strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -450,6 +450,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_FLIP:
         guidance_h_mode_changed(GUIDANCE_H_MODE_FLIP);
         break;
+      case AP_MODE_GUIDED:
+        guidance_h_mode_changed(GUIDANCE_H_MODE_GUIDED);
+        break;
       default:
         break;
     }
@@ -499,6 +502,9 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
       case AP_MODE_FLIP:
         guidance_v_mode_changed(GUIDANCE_V_MODE_FLIP);
         break;
+      case AP_MODE_GUIDED:
+        guidance_v_mode_changed(GUIDANCE_V_MODE_GUIDED);
+        break;
       default:
         break;
     }
@@ -507,6 +513,41 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
 
 }
 
+bool_t autopilot_guided_goto_ned(float x, float y, float z, float heading)
+{
+  if (autopilot_mode == AP_MODE_GUIDED) {
+    guidance_h_set_guided_pos(x, y);
+    guidance_h_set_guided_heading(heading);
+    guidance_v_set_guided_z(z);
+    return TRUE;
+  }
+  return FALSE;
+}
+
+bool_t autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
+{
+  if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
+    float x = stateGetPositionNed_f()->x + dx;
+    float y = stateGetPositionNed_f()->y + dy;
+    float z = stateGetPositionNed_f()->z + dz;
+    float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
+    return autopilot_guided_goto_ned(x, y, z, heading);
+  }
+  return FALSE;
+}
+
+bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
+{
+  if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
+    float psi = stateGetNedToBodyEulers_f()->psi;
+    float x = stateGetPositionNed_f()->x + cosf(-psi) * dx + sinf(-psi) * dy;
+    float y = stateGetPositionNed_f()->y - sinf(-psi) * dx + cosf(-psi) * dy;
+    float z = stateGetPositionNed_f()->z + dz;
+    float heading = psi + dyaw;
+    return autopilot_guided_goto_ned(x, y, z, heading);
+  }
+  return FALSE;
+}
 
 void autopilot_check_in_flight(bool_t motors_on)
 {

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -52,6 +52,7 @@
 #define AP_MODE_FORWARD           16
 #define AP_MODE_MODULE            17
 #define AP_MODE_FLIP              18
+#define AP_MODE_GUIDED            19
 
 extern uint8_t autopilot_mode;
 extern uint8_t autopilot_mode_auto2;
@@ -182,4 +183,33 @@ static inline void autopilot_ClearSettings(float clear)
 extern void send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
 #endif
 
+/** Set position and heading setpoints in GUIDED mode.
+ * @param x North position (local NED frame) in meters.
+ * @param y East position (local NED frame) in meters.
+ * @param z Down position (local NED frame) in meters.
+ * @param heading Setpoint in radians.
+ * @return TRUE if setpoint was set (currently in AP_MODE_GUIDED)
+ */
+extern bool_t autopilot_guided_goto_ned(float x, float y, float z, float heading);
+
+/** Set position and heading setpoints wrt. current position in GUIDED mode.
+ * @param dx Offset relative to current north position (local NED frame) in meters.
+ * @param dy Offset relative to current east position (local NED frame) in meters.
+ * @param dz Offset relative to current down position (local NED frame) in meters.
+ * @param dyaw Offset relative to current heading setpoint in radians.
+ * @return TRUE if setpoint was set (currently in AP_MODE_GUIDED)
+ */
+extern bool_t autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw);
+
+/** Set position and heading setpoints wrt. current position AND heading in GUIDED mode.
+ * @param dx relative position (body frame, forward) in meters.
+ * @param dy relative position (body frame, right) in meters.
+ * @param dz relative position (body frame, down) in meters.
+ * @param dyaw Offset relative to current heading setpoint in radians.
+ * @return TRUE if setpoint was set (currently in AP_MODE_GUIDED)
+ */
+extern bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw);
+
+
+
 #endif /* AUTOPILOT_H */

sw/airborne/firmwares/rotorcraft/datalink.c

@@ -50,6 +50,7 @@
 #endif
 
 #include "firmwares/rotorcraft/navigation.h"
+#include "firmwares/rotorcraft/autopilot.h"
 
 #include "math/pprz_geodetic_int.h"
 #include "state.h"
@@ -185,6 +186,33 @@ void dl_parse_msg(void)
         );
       break;
 #endif
+
+    case DL_GUIDED_SETPOINT_NED:
+      if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
+      uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
+      float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
+      float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
+      float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
+      float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
+      switch (flags) {
+        case 0x00:
+        case 0x02:
+          /* local NED position setpoints */
+          autopilot_guided_goto_ned(x, y, z, yaw);
+          break;
+        case 0x01:
+          /* local NED offset position setpoints */
+          autopilot_guided_goto_ned_relative(x, y, z, yaw);
+          break;
+        case 0x03:
+          /* body NED offset position setpoints */
+          autopilot_guided_goto_body_relative(x, y, z, yaw);
+          break;
+        default:
+          /* others not handled yet */
+          break;
+      }
+      break;
     default:
       break;
   }

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

@@ -366,23 +366,25 @@ void guidance_h_run(bool_t  in_flight)
       break;
 
     case GUIDANCE_H_MODE_HOVER:
+      /* set psi command from RC */
+      guidance_h.sp.heading = guidance_h.rc_sp.psi;
+      /* fall trough to GUIDED to update ref, run traj and set final attitude setpoint */
+
+    case GUIDANCE_H_MODE_GUIDED:
+      /* guidance_h.sp.pos and guidance_h.sp.heading need to be set from external source */
       if (!in_flight) {
         guidance_h_hover_enter();
       }
 
       guidance_h_update_reference();
 
-      /* set psi command */
-      guidance_h.sp.heading = guidance_h.rc_sp.psi;
-
 #if GUIDANCE_INDI
       guidance_indi_run(in_flight, guidance_h.sp.heading);
 #else
       /* compute x,y earth commands */
       guidance_h_traj_run(in_flight);
       /* set final attitude setpoint */
-      stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth,
-                                             guidance_h.sp.heading);
+      stabilization_attitude_set_earth_cmd_i(&guidance_h_cmd_earth, guidance_h.sp.heading);
 #endif
       stabilization_attitude_run(in_flight);
       break;
@@ -548,18 +550,17 @@ static void guidance_h_traj_run(bool_t in_flight)
 
 static void guidance_h_hover_enter(void)
 {
-
   /* set horizontal setpoint to current position */
   VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i());
 
   reset_guidance_reference_from_current_position();
 
   guidance_h.rc_sp.psi = stateGetNedToBodyEulers_i()->psi;
+  guidance_h.sp.heading = guidance_h.rc_sp.psi;
 }
 
 static void guidance_h_nav_enter(void)
 {
-
   /* horizontal position setpoint from navigation/flightplan */
   INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot);
 
@@ -616,3 +617,23 @@ void guidance_h_set_igain(uint32_t igain)
   guidance_h.gains.i = igain;
   INT_VECT2_ZERO(guidance_h_trim_att_integrator);
 }
+
+bool_t guidance_h_set_guided_pos(float x, float y)
+{
+  if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
+    guidance_h.sp.pos.x = POS_BFP_OF_REAL(x);
+    guidance_h.sp.pos.y = POS_BFP_OF_REAL(y);
+    return TRUE;
+  }
+  return FALSE;
+}
+
+bool_t guidance_h_set_guided_heading(float heading)
+{
+  if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) {
+    guidance_h.sp.heading = ANGLE_BFP_OF_REAL(heading);
+    INT32_ANGLE_NORMALIZE(guidance_h.sp.heading);
+    return TRUE;
+  }
+  return FALSE;
+}

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

@@ -59,6 +59,7 @@
 #define GUIDANCE_H_MODE_FORWARD     7
 #define GUIDANCE_H_MODE_MODULE      8
 #define GUIDANCE_H_MODE_FLIP        9
+#define GUIDANCE_H_MODE_GUIDED      10
 
 
 struct HorizontalGuidanceSetpoint {
@@ -111,6 +112,18 @@ extern void guidance_h_run(bool_t in_flight);
 
 extern void guidance_h_set_igain(uint32_t igain);
 
+/** Set horizontal position setpoint in GUIDED mode.
+ * @param x North position (local NED frame) in meters.
+ * @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);
+
+/** 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);
 
 /* 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

@@ -229,6 +229,7 @@ void guidance_v_mode_changed(uint8_t new_mode)
 
   switch (new_mode) {
     case GUIDANCE_V_MODE_HOVER:
+    case GUIDANCE_V_MODE_GUIDED:
       guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint
       guidance_v_z_sum_err = 0;
       GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0);
@@ -309,6 +310,7 @@ void guidance_v_run(bool_t in_flight)
       break;
 
     case GUIDANCE_V_MODE_HOVER:
+    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);
@@ -447,3 +449,12 @@ static void run_hover_loop(bool_t in_flight)
   Bound(guidance_v_delta_t, 0, MAX_PPRZ);
 
 }
+
+bool_t guidance_v_set_guided_z(float z)
+{
+  if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) {
+    guidance_v_z_sp = POS_BFP_OF_REAL(z);
+    return TRUE;
+  }
+  return FALSE;
+}

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

@@ -40,6 +40,7 @@
 #define GUIDANCE_V_MODE_NAV       5
 #define GUIDANCE_V_MODE_MODULE    6
 #define GUIDANCE_V_MODE_FLIP      7
+#define GUIDANCE_V_MODE_GUIDED    8
 
 extern uint8_t guidance_v_mode;
 
@@ -106,6 +107,12 @@ extern void guidance_v_mode_changed(uint8_t new_mode);
 extern void guidance_v_notify_in_flight(bool_t in_flight);
 extern void guidance_v_run(bool_t in_flight);
 
+/** Set z setpoint in GUIDED mode.
+ * @param z Setpoint (down is positive) in meters.
+ * @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED)
+ */
+extern bool_t guidance_v_set_guided_z(float z);
+
 #define guidance_v_SetKi(_val) {      \
     guidance_v_ki = _val;       \
     guidance_v_z_sum_err = 0;     \

sw/ground_segment/cockpit/live.ml

@@ -1294,7 +1294,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
         match ap_mode with
             "AUTO2" | "NAV" -> ok_color
           | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0"
-          | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" | "FLIP" -> warning_color
+          | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" | "FLIP" | "GUIDED" -> warning_color
           | _ -> alert_color in
       ac.strip#set_color "AP" color;
     end;