[messages] add POSITION_TARGET_LOCAL_NED to control guided mode

paparazzi · Dec 12, 2015 · 776a1f1 · 776a1f1
1 parent 85777e8
commit 776a1f1
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 6 deletions.
diff --git a/conf/messages.xml b/conf/messages.xml
@@ -2437,13 +2437,29 @@
     <field name="ac_id" type="uint8"/>
   </message>
 
+  <message name="POSITION_TARGET_LOCAL_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"/>

diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/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_POSITION_TARGET_LOCAL_NED:
+      if (DL_POSITION_TARGET_LOCAL_NED_ac_id(dl_buffer) != AC_ID) { break; }
+      uint8_t flags = DL_POSITION_TARGET_LOCAL_NED_flags(dl_buffer);
+      float x = DL_POSITION_TARGET_LOCAL_NED_x(dl_buffer);
+      float y = DL_POSITION_TARGET_LOCAL_NED_y(dl_buffer);
+      float z = DL_POSITION_TARGET_LOCAL_NED_z(dl_buffer);
+      float yaw = DL_POSITION_TARGET_LOCAL_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;
   }