[mission] add status report for mission module

paparazzi · Nov 29, 2013 · 898745b · 898745b
1 parent 315225c
commit 898745b
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 3 deletions.
diff --git a/conf/messages.xml b/conf/messages.xml
@@ -654,7 +654,11 @@
   </message>
 
  <!-- 78 is free -->
- <!-- 79 is free -->
+
+  <message name="MISSION_STATUS" id="79">
+    <field name="remaining_time" type="float"/>
+    <field name="task_list" type="uint8[]"/>
+  </message>
 
   <message name="CROSS_TRACK_ERROR" id="80">
     <field name="cross_track_error" type="float" unit="m"/>

diff --git a/conf/modules/mission_fw.xml b/conf/modules/mission_fw.xml
@@ -12,6 +12,7 @@
     <file name="mission.h"/>
   </header>
   <init fun="mission_init()"/>
+  <periodic fun="mission_status_report()" freq="2" autorun="TRUE"/>
 
   <datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP()"/>
   <datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA()"/>

diff --git a/sw/airborne/modules/mission/mission.c b/sw/airborne/modules/mission/mission.c
@@ -31,7 +31,6 @@
 #include "subsystems/datalink/datalink.h"
 #include "subsystems/datalink/downlink.h"
 
-
 struct _mission mission;
 
 
@@ -84,6 +83,26 @@ struct _mission_element * mission_get(void) {
 }
 
 
+// Report function
+void mission_status_report(void) {
+  // build task list
+  uint8_t task_list[MISSION_ELEMENT_NB];
+  uint8_t i = mission.current_idx, j = 0;
+  while (i != mission.insert_idx) {
+    task_list[j++] = (uint8_t)mission.elements[i].type;
+    i = (i+1)%MISSION_ELEMENT_NB;
+  }
+  if (j == 0) { task_list[j++] = 255; } // Dummy value if task list is empty
+  //compute remaining time (or -1. if no time limit)
+  float remaining_time = -1.;
+  if (mission.elements[mission.current_idx].duration > 0.) {
+    remaining_time = mission.elements[mission.current_idx].duration - mission.element_time;
+  }
+
+  // send status
+  DOWNLINK_SEND_MISSION_STATUS(DefaultChannel, DefaultDevice, &remaining_time, j, task_list);
+}
+
 
 ///////////////////////
 // Parsing functions //

diff --git a/sw/airborne/modules/mission/mission.h b/sw/airborne/modules/mission/mission.h
@@ -117,7 +117,13 @@ extern struct _mission_element * mission_get(void);
  *
  * @return return TRUE when the mission is running, FALSE when it is finished
  */
-extern int mission_run();
+extern int mission_run(void);
+
+/** Report mission status
+ *
+ * Send mission status over datalink
+ */
+extern void mission_status_report(void);
 
 /** Parsing functions called when a mission message is received
  */