diff --git a/conf/Makefile.sim b/conf/Makefile.sim
index a799f05c01..d6f47a5477 100644
--- a/conf/Makefile.sim
+++ b/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)
diff --git a/conf/modules/mission_fw.xml b/conf/modules/mission_fw.xml
index 9b6558ca32..592261bd71 100644
--- a/conf/modules/mission_fw.xml
+++ b/conf/modules/mission_fw.xml
@@ -22,11 +22,13 @@
+
+
diff --git a/conf/modules/mission_rotorcraft.xml b/conf/modules/mission_rotorcraft.xml
index fa5171d6c4..4b32ef0b10 100644
--- a/conf/modules/mission_rotorcraft.xml
+++ b/conf/modules/mission_rotorcraft.xml
@@ -22,11 +22,13 @@
+
+
diff --git a/conf/modules/nav_flower.xml b/conf/modules/nav_flower.xml
index 710f2fef8a..e8eb6990c9 100644
--- a/conf/modules/nav_flower.xml
+++ b/conf/modules/nav_flower.xml
@@ -9,6 +9,7 @@
+
diff --git a/sw/airborne/modules/mission/mission_common.c b/sw/airborne/modules/mission/mission_common.c
index 71499b4a2c..9355009e46 100644
--- a/sw/airborne/modules/mission/mission_common.c
+++ b/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
diff --git a/sw/airborne/modules/mission/mission_common.h b/sw/airborne/modules/mission/mission_common.h
index 8481cb94a1..e0c6673172 100644
--- a/sw/airborne/modules/mission/mission_common.h
+++ b/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);
diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c
index 80c82c5068..44b2fba526 100644
--- a/sw/airborne/modules/mission/mission_fw_nav.c
+++ b/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;
diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
index 485804f5d7..144c6f377d 100644
--- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c
+++ b/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;
diff --git a/sw/airborne/modules/nav/nav_flower.c b/sw/airborne/modules/nav/nav_flower.c
index 570e989356..5538132b97 100644
--- a/sw/airborne/modules/nav/nav_flower.c
+++ b/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.
diff --git a/sw/airborne/modules/nav/nav_flower.h b/sw/airborne/modules/nav/nav_flower.h
index b99b31a766..57eb3f0c68 100644
--- a/sw/airborne/modules/nav/nav_flower.h
+++ b/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);
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 277c146fd1..94eed2346d 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 277c146fd15f26d53f1b6810a392e10de8b9c700
+Subproject commit 94eed2346ddaecb94611ee2461874c7b2b863196