[mission] add support for custom mission patterns or actions

Specific navigation functions or even payload actions can be registered
in the mission module in order to be called from the mission controller
like the built-in navigation patterns.
Each custom element (callback function) is registered/called with an
unique string identifier of 5 char max.
A maximum of 12 parameters can be passed to the callback function.
An example is provided with the nav_flower navigation function.

conf/Makefile.sim

@@ -57,6 +57,7 @@ CFLAGS += -g
 CFLAGS += -std=gnu99
 CFLAGS += $(shell pkg-config --cflags-only-I ivy-glib)
 CFLAGS += -D_GNU_SOURCE
+CFLAGS += -DPPRZLINK_UNALIGNED_ACCESS=1
 
 LDFLAGS	 =	-lm
 LDFLAGS += $(BOARD_LDFLAGS)

conf/modules/mission_fw.xml

@@ -22,11 +22,13 @@
   <datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA()"/>
   <datalink message="MISSION_PATH" fun="mission_parse_PATH()"/>
   <datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA()"/>
+  <datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM()"/>
   <datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION()"/>
   <datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION()"/>
   <datalink message="END_MISSION" fun="mission_parse_END_MISSION()"/>
 
   <makefile>
+    <define name="USE_MISSION"/>
     <file name="mission_common.c"/>
     <file name="mission_fw_nav.c"/>
   </makefile>

conf/modules/mission_rotorcraft.xml

@@ -22,11 +22,13 @@
   <datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA()"/>
   <datalink message="MISSION_PATH" fun="mission_parse_PATH()"/>
   <datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA()"/>
+  <datalink message="MISSION_CUSTOM" fun="mission_parse_CUSTOM()"/>
   <datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION()"/>
   <datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION()"/>
   <datalink message="END_MISSION" fun="mission_parse_END_MISSION()"/>
 
   <makefile>
+    <define name="USE_MISSION"/>
     <file name="mission_common.c"/>
     <file name="mission_rotorcraft_nav.c"/>
   </makefile>

conf/modules/nav_flower.xml

@@ -9,6 +9,7 @@
   <header>
     <file name="nav_flower.h"/>
   </header>
+  <init fun="nav_flower_init()"/>
   <makefile target="ap|sim|nps|hitl">
     <file name="nav_flower.c"/>
   </makefile>

sw/airborne/modules/mission/mission_common.c

@@ -39,6 +39,14 @@ void mission_init(void)
   mission.insert_idx = 0;
   mission.current_idx = 0;
   mission.element_time = 0.;
+
+  // FIXME
+  // we have no guarantee that nav modules init are called after mission_init
+  // this would erase the already registered elements
+  //for (int i = 0; i < MISSION_REGISTER_NB; i++) {
+  //  mission.registered[i].cb = NULL;
+  //  memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
+  //}
 }
 
 
@@ -77,6 +85,7 @@ bool mission_insert(enum MissionInsertMode insert, struct _mission_element *elem
       tmp = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
       mission.elements[tmp] = *element;
       mission.insert_idx = (mission.current_idx + 2) % MISSION_ELEMENT_NB;
+      break;
     default:
       // unknown insertion mode
       return false;
@@ -85,6 +94,32 @@ bool mission_insert(enum MissionInsertMode insert, struct _mission_element *elem
 
 }
 
+// Register new callback
+bool mission_register(mission_custom_cb cb, char *type)
+{
+  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
+    if (str_equal(mission.registered[i].type, type)) {
+      return false; // identifier already registered
+    }
+    if (mission.registered[i].cb == NULL) {
+      strncpy(mission.registered[i].type, type, MISSION_TYPE_SIZE-1);
+      mission.registered[i].cb = cb;
+      return true;
+    }
+  }
+  return false; // no more room to register callbacks
+}
+
+// Returns a pointer to a register struct with matching types, NULL if not found
+static struct _mission_registered *mission_get_registered(char *type)
+{
+  for (int i = 0; i < MISSION_REGISTER_NB; i++) {
+    if (str_equal(mission.registered[i].type, type)) {
+      return &(mission.registered[i]);
+    }
+  }
+  return NULL; // not found
+}
 
 // Weak implementation of mission_element_convert (leave element unchanged)
 bool __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return true; }
@@ -320,6 +355,28 @@ int mission_parse_PATH_LLA(void)
   return mission_insert(insert, &me);
 }
 
+int mission_parse_CUSTOM(void)
+{
+  if (DL_MISSION_CUSTOM_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft
+
+  struct _mission_element me;
+  me.type = MissionCustom;
+  me.element.mission_custom.reg = mission_get_registered(DL_MISSION_CUSTOM_type(dl_buffer));
+  if (me.element.mission_custom.reg == NULL) {
+    return false; // unknown type
+  }
+  me.element.mission_custom.nb = DL_MISSION_CUSTOM_params_length(dl_buffer);
+  for (int i = 0; i < me.element.mission_custom.nb; i++) {
+    me.element.mission_custom.params[i] = DL_MISSION_CUSTOM_params(dl_buffer)[i];
+  }
+  me.duration = DL_MISSION_CUSTOM_duration(dl_buffer);
+  me.index = DL_MISSION_CUSTOM_index(dl_buffer);
+
+  enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CUSTOM_insert(dl_buffer));
+
+  return mission_insert(insert, &me);
+}
+
 int mission_parse_GOTO_MISSION(void)
 {
   if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) { return false; } // not for this aircraft

sw/airborne/modules/mission/mission_common.h

@@ -39,9 +39,7 @@ enum MissionType {
   MissionCircle = 2,
   MissionSegment = 3,
   MissionPath = 4,
-  MissionSurvey = 5,
-  MissionEight = 6,
-  MissionOval = 7
+  MissionCustom = 5
 };
 
 enum MissionInsertMode {
@@ -91,6 +89,28 @@ struct _mission_path {
   uint8_t nb;
 };
 
+#define MISSION_CUSTOM_MAX 12 // maximum number of parameters
+#define MISSION_TYPE_SIZE 6
+
+/** custom mission element callback
+ * @param[in] nb number of params
+ * @param[in] params array of params with a maximum of 12
+ * @param[in] init true if the function is called for the first time
+ * @return true until the function ends
+ */
+typedef bool (*mission_custom_cb)(uint8_t nb, float *params, bool init);
+
+struct _mission_registered {
+  mission_custom_cb cb;         ///< navigation/action function callback
+  char type[MISSION_TYPE_SIZE]; ///< mission element identifier (5 char max + 1 \0)
+};
+
+struct _mission_custom {
+  struct _mission_registered *reg;  ///< pointer to a registered custom mission element
+  float params[MISSION_CUSTOM_MAX]; ///< list of parameters
+  uint8_t nb;                       ///< number of parameters
+};
+
 struct _mission_element {
   enum MissionType type;
   union {
@@ -98,6 +118,7 @@ struct _mission_element {
     struct _mission_circle mission_circle;
     struct _mission_segment mission_segment;
     struct _mission_path mission_path;
+    struct _mission_custom mission_custom;
   } element;
 
   float duration; ///< time to spend in the element (<= 0 to disable)
@@ -111,8 +132,16 @@ struct _mission_element {
 #define MISSION_ELEMENT_NB 20
 #endif
 
+/** Max number of registered nav/action callbacks
+ *  can be redefined
+ */
+#ifndef MISSION_REGISTER_NB
+#define MISSION_REGISTER_NB 6
+#endif
+
 struct _mission {
   struct _mission_element elements[MISSION_ELEMENT_NB];
+  struct _mission_registered registered[MISSION_REGISTER_NB];
   float element_time;   ///< time in second spend in the current element
   uint8_t insert_idx;   ///< inserstion index
   uint8_t current_idx;  ///< current mission element index
@@ -131,6 +160,13 @@ extern void mission_init(void);
  */
 extern bool mission_insert(enum MissionInsertMode insert, struct _mission_element *element);
 
+/** Register a new navigation or action callback function
+ * @param cb callback f(nb, param array)
+ * @param type string identifier with 5 characters max (+ 1 '\0' char)
+ * @return return TRUE if register is succesful, FALSE otherwise
+ */
+extern bool mission_register(mission_custom_cb cb, char *type);
+
 /** Convert mission element's points format if needed
  * @param el pointer to the mission element
  * @return return TRUE if conversion is succesful, FALSE otherwise
@@ -177,8 +213,7 @@ extern int mission_parse_SEGMENT(void);
 extern int mission_parse_SEGMENT_LLA(void);
 extern int mission_parse_PATH(void);
 extern int mission_parse_PATH_LLA(void);
-extern int mission_parse_SURVEY(void);
-extern int mission_parse_SURVEY_LLA(void);
+extern int mission_parse_CUSTOM(void);
 extern int mission_parse_GOTO_MISSION(void);
 extern int mission_parse_NEXT_MISSION(void);
 extern int mission_parse_END_MISSION(void);

sw/airborne/modules/mission/mission_fw_nav.c

@@ -138,6 +138,12 @@ static inline bool mission_nav_path(struct _mission_path *path)
   return true;
 }
 
+/** Call custom navigation function
+ */
+static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
+{
+  return custom->reg->cb(custom->nb, custom->params, init);
+}
 
 int mission_run()
 {
@@ -162,6 +168,9 @@ int mission_run()
     case MissionPath:
       el_running = mission_nav_path(&(el->element.mission_path));
       break;
+    case MissionCustom:
+      el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
+      break;
     default:
       // invalid type or pattern not yet handled
       break;

sw/airborne/modules/mission/mission_rotorcraft_nav.c

@@ -220,6 +220,13 @@ static inline bool mission_nav_path(struct _mission_element *el)
   return true;
 }
 
+/** Call custom navigation function
+ */
+static inline bool mission_nav_custom(struct _mission_custom *custom, bool init)
+{
+  return custom->reg->cb(custom->nb, custom->params, init);
+}
+
 int mission_run()
 {
   // current element
@@ -244,6 +251,9 @@ int mission_run()
     case MissionPath:
       el_running = mission_nav_path(el);
       break;
+    case MissionCustom:
+      el_running = mission_nav_custom(&(el->element.mission_custom), mission.element_time < dt_navigation);
+      break;
     default:
       // invalid type or pattern not yet handled
       break;

sw/airborne/modules/nav/nav_flower.c

@@ -32,6 +32,31 @@
 #include "autopilot.h"
 #include "generated/flight_plan.h"
 
+#if USE_MISSION
+#include "modules/mission/mission_common.h"
+
+static bool nav_flower_mission(uint8_t nb, float *params, bool init)
+{
+  if (nb != 2) {
+    return false; // wrong number of parameters
+  }
+  if (init) {
+    uint8_t center = (uint8_t)(params[0]);
+    uint8_t edge = (uint8_t)(params[1]);
+    nav_flower_setup(center, edge);
+  }
+  return nav_flower_run();
+}
+#endif
+
+
+void nav_flower_init(void)
+{
+#if USE_MISSION
+  mission_register(nav_flower_mission, "FLWR");
+#endif
+}
+
 /************** Flower Navigation **********************************************/
 
 /** Makes a flower pattern.

sw/airborne/modules/nav/nav_flower.h

@@ -29,6 +29,7 @@
 
 #include "std.h"
 
+extern void nav_flower_init(void);
 extern bool nav_flower_run(void);
 extern void nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP);