Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[mission] add mission module (only parsing messages for now)
- Loading branch information
1 parent
5a34423
commit b361afd
Showing
3 changed files
with
255 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 */ | ||
} | ||
|
||
|
||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 |