[mission] add mission module (only parsing messages for now)

2026-06-01 04:46:51 +08:00 · 2013-06-25 13:58:58 +02:00
parent 5a344232d9
commit b361afdeaf
3 changed files with 255 additions and 0 deletions
@@ -0,0 +1,18 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="mission messages" dir="mission">
+  <doc>
+    <description>New messages for Mission Planner</description>
+  </doc>
+  <header>
+    <file name="mission_msg.h"/>
+  </header>
+  <init fun="mission_msg_init()"/>
+  <datalink message="MISSION_GOTO_WP" fun="ParseMissionGotoWp()"/>
+  <datalink message="MISSION_PATH" fun="ParseMissionPath()"/>
+  <datalink message="MISSION_SEGMENT" fun="ParseMissionSegment()"/>
+  
+  <makefile>
+    <file name="mission_msg.c"/>
+  </makefile>
+</module>
@@ -0,0 +1,138 @@
+/** \file mission_msg.c
+ *  \brief the new messages for mission planner library
+ *  \ edited by Anh Truong
+ */
+
+//#define MISSION_C
+
+
+
+#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 "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>
+
+
+
+
+
+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);
+}
+
+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_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));
+    NavVerticalAltitudeMode(5.0, 0.);
+  }
+  return TRUE;
+}
+
+
+
+/** Status along the mission path */
+enum mission_msg_PATH_status { Path12, Path23, Path34, Path45 };
+static enum mission_msg_PATH_status mission_msg_PATH_status;
+
+int mission_msg_PATH_init( void ) {
+  // status of the first segment of the path
+  mission_msg_PATH_status = Path12;
+
+  // path finder here to generate intermediate waypoints
+  // these five points are defined as global variables in mission_info.h
+  //struct tabWaypoint tabW;
+  //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);
+    if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) {
+      mission_msg_PATH_status = Path23;
+      //nav_init_stage();
+    }
+    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;      
+  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;      
+  case Path45: /* path 4-5 */
+    nav_route_xy(x4, y4, x5, y5);
+    if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) {
+      //mission_msg_PATH_status = Path12;
+      //nav_init_stage();
+      return FALSE; /* This path ends when AC arrive to point 5 */
+    }
+    break;           
+  }
+
+
+  return TRUE; /* do the function */
+}
+
+
+
+
@@ -0,0 +1,99 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file mission_msg.h
+ *  \brief the new messages for mission planner library
+ *  \ edited by Anh Truong
+ */
+
+#ifndef MISSION_H
+#define MISSION_H
+
+
+
+#include "std.h"
+#include "subsystems/nav.h"
+#include "subsystems/navigation/traffic_info.h"
+
+
+extern void mission_msg_init(void);
+
+extern int mission_msg_GOTO_WP(float x, float y);
+
+extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2);
+
+extern int mission_msg_PATH_init( void );
+extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5);
+
+
+
+#define ParseMissionGotoWp() { \
+  if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) == AC_ID) { \
+    uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \
+    float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \
+    float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \
+    mission_msg_GOTO_WP(wp_east, wp_north); \
+  } \
+}
+
+#define ParseMissionPath() { \
+  if (DL_MISSION_PATH_ac_id(dl_buffer) == AC_ID) { \
+    uint8_t ac_id = DL_MISSION_PATH_ac_id(dl_buffer); \
+    float point_east_1 = DL_MISSION_PATH_point_east_1(dl_buffer); \
+    float point_north_1 = DL_MISSION_PATH_point_north_1(dl_buffer); \
+    float point_east_2 = DL_MISSION_PATH_point_east_2(dl_buffer); \
+    float point_north_2 = DL_MISSION_PATH_point_north_2(dl_buffer); \
+    float point_east_3 = DL_MISSION_PATH_point_east_3(dl_buffer); \
+    float point_north_3 = DL_MISSION_PATH_point_north_3(dl_buffer); \
+    float point_east_4 = DL_MISSION_PATH_point_east_4(dl_buffer); \
+    float point_north_4 = DL_MISSION_PATH_point_north_4(dl_buffer); \
+    float point_east_5 = DL_MISSION_PATH_point_east_5(dl_buffer); \
+    float point_north_5 = DL_MISSION_PATH_point_north_5(dl_buffer); \
+    mission_msg_PATH(point_east_1, point_north_1, point_east_2, point_north_2, point_east_3, point_north_3, point_east_4, point_north_4, point_east_5, point_north_5); \
+  } \
+}
+
+
+#define ParseMissionSegment() { \
+  if (DL_MISSION_SEGMENT_ac_id(dl_buffer) == AC_ID) { \
+    uint8_t ac_id = DL_MISSION_SEGMENT_ac_id(dl_buffer); \
+    float segment_east_1 = DL_MISSION_SEGMENT_segment_east_1(dl_buffer); \
+    float segment_north_1 = DL_MISSION_SEGMENT_segment_north_1(dl_buffer); \
+    float segment_east_2 = DL_MISSION_SEGMENT_segment_east_2(dl_buffer); \
+    float segment_north_2 = DL_MISSION_SEGMENT_segment_north_2(dl_buffer); \
+    mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2); \
+  } \
+}
+
+
+/*
+#define ParseMissionGotoWp() { \
+    uint8_t ac_id = DL_MISSION_GOTO_WP_ac_id(dl_buffer); \
+    float wp_east = DL_MISSION_GOTO_WP_wp_east(dl_buffer); \
+    float wp_north = DL_MISSION_GOTO_WP_wp_north(dl_buffer); \
+    fly_to_xy(wp_east, wp_north); \
+}
+*/
+
+#endif // MISSION