From 898745b7bf5b07ab40a27a5aefff5cf342666d71 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Fri, 29 Nov 2013 23:43:18 +0100
Subject: [PATCH] [mission] add status report for mission module

---
 conf/messages.xml                     |  6 +++++-
 conf/modules/mission_fw.xml           |  1 +
 sw/airborne/modules/mission/mission.c | 21 ++++++++++++++++++++-
 sw/airborne/modules/mission/mission.h |  8 +++++++-
 4 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/conf/messages.xml b/conf/messages.xml
index d1b9267047d..5fc0c2dfa8f 100644
--- 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
index e908686cac2..a0ceaae7b91 100644
--- 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
index d3794bbca9a..b8335f81032 100644
--- 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
index d423c48c12d..582ca3754f7 100644
--- 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
  */