[mission] define mission structures

sw/airborne/modules/mission/mission_msg.c

@@ -6,51 +6,46 @@
 //#define MISSION_C
 
 
+#include "modules/mission/mission_msg.h"
 
 #include <math.h>
 
-
 #include "subsystems/datalink/downlink.h"
 
-#include "mission/mission_msg.h"
-//#include "estimator.h"
-//#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-//#include "firmwares/fixedwing/guidance/guidance_v.h"
+#include "state.h"
 #include "autopilot.h"
-//#include "subsystems/ins.h"
-//#include "subsystems/nav.h"
 #include "subsystems/gps.h"
-#include "generated/flight_plan.h"
 #include "generated/airframe.h"
 #include "dl_protocol.h"
 
-#include <stdio.h>
+
+struct _mission mission;
 
 
-
-
-
-void mission_msg_init(void) {
-  //float x_to_go = 85.1; // I5 in FP file
-  //float y_to_go = 173.6;
-  //fly_to_xy(x, y);
+void mission_init(void) {
+  mission.mission_insert_idx = 0;
+  mission.mission_extract_idx = 0;
 }
 
-int mission_msg_GOTO_WP(float x, float y) {
-  //float x_to_go = 85.1; // I5 in FP file
-  //float y_to_go = 173.6;
-  fly_to_xy(x, y);
-  return FALSE;
+int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d) {
+  mission.elements[insert_idx].type = MissionWP;
+  mission.elements[insert_idx].element.mission_wp.wp.x = x;
+  mission.elements[insert_idx].element.mission_wp.wp.y = y;
+  mission.elements[insert_idx].element.mission_wp.wp.a = a;
+  mission.elements[insert_idx].element.duration = d;
+
+  insert_idx++;
+  if (insert_idx == MISSION_ELEMENT_NB) insert_idx = 0;
+  return TRUE;
 }
 
-int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) { 
+int mission_msg_SEGMENT(float x1, float y1, float x2, float y2) {
   if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) {
     return FALSE; // end of block and go to the next block
 //    NextBlock();
-  } 
+  }
   else {
     //NextBlock()
-    
     //nav_init_stage();
     nav_route_xy(x1, y1, x2, y2);
     NavVerticalAutoThrottleMode(RadOfDeg(0.000000));
@@ -75,28 +70,11 @@ int mission_msg_PATH_init( void ) {
   //tabW = pathFinder();
   //pathFinder();
 
-
-  /*AstarStartPoint_east = estimator_x;
-  AstarStartPoint_north = estimator_y;
-
-  InterPoint_east_1 =
-  InterPoint_north_1 =
-  InterPoint_east_2 =
-  InterPoint_north_2 =
-  InterPoint_east_3 =
-  InterPoint_north_3 =
-  
-
-  AstarEndPoint_east = estimator_x;
-  AstarEndPoint_north = estimator_y;*/
-  
-  //PathFinder(int argc, const char * argv[]); -->> AstarPath_x1, AstarPath_y1...
-
   return FALSE;
-}  
+}
 
 int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5) {
-  
+
   switch(mission_msg_PATH_status) {
   case Path12: /* path 1-2 */
     nav_route_xy(x1, y1, x2, y2);
@@ -104,21 +82,21 @@ int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3,
       mission_msg_PATH_status = Path23;
       //nav_init_stage();
     }
-    break; 
+    break;
   case Path23: /* path 2-3 */
     nav_route_xy(x2, y2, x3, y3);
     if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) {
       mission_msg_PATH_status = Path34;
       //nav_init_stage();
     }
-    break;      
+    break;
   case Path34: /* path 3-4 */
     nav_route_xy(x3, y3, x4, y4);
     if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) {
       mission_msg_PATH_status = Path45;
       //nav_init_stage();
     }
-    break;      
+    break;
   case Path45: /* path 4-5 */
     nav_route_xy(x4, y4, x5, y5);
     if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) {
@@ -126,7 +104,7 @@ int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3,
       //nav_init_stage();
       return FALSE; /* This path ends when AC arrive to point 5 */
     }
-    break;           
+    break;
   }
 
 

sw/airborne/modules/mission/mission_msg.h

@@ -1,7 +1,5 @@
 /*
- * $Id$
- *
- * Copyright (C) 2010 ENAC
+ * Copyright (C) 2013 ENAC
  *
  * This file is part of paparazzi.
  *
@@ -22,19 +20,65 @@
  *
  */
 
-/** \file mission_msg.h
- *  \brief the new messages for mission planner library
- *  \ edited by Anh Truong
+/** @file mission_msg.h
+ *  @brief the new messages for mission planner library
  */
 
 #ifndef MISSION_H
 #define MISSION_H
 
-
-
 #include "std.h"
-#include "subsystems/nav.h"
-#include "subsystems/navigation/traffic_info.h"
+#include "subsystems/navigation/common_nav.h"
+
+enum MissionType {
+  MissionWP,
+  MissionCircle,
+  MissionSegment,
+  MissionPath,
+  MissionSurvey,
+  MissionEight,
+  MissionOval
+};
+
+struct _mission_wp {
+  struct point wp;
+};
+
+struct _mission_circle {
+  struct point center;
+  float radius;
+};
+
+struct _mission_segment {
+  struct point from;
+  struct point to;
+};
+
+#define MISSION_PATH_NB 5
+struct _mission_path {
+  struct point[MISSION_PATH_NB] path;
+};
+
+struct _mission_element {
+  enum MissionType type;
+  union {
+    struct _mission_wp mission_wp;
+    struct _mission_circle mission_circle;
+    struct _mission_segment mission_segment;
+    struct _mission_path mission_path;
+  } element;
+  uint16_t duration;
+};
+
+#define MISSION_ELEMENT_NB 20
+struct _mission {
+  struct _mission_element[MISSION_ELEMENT_NB] mission_tasks;
+  uint8_t mission_insert_idx;
+  uint8_t mission_extract_idx;
+
+};
+
+extern struct _mission mission;
 
 
 extern void mission_msg_init(void);