Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 09:36:19 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[mission] update mission interface

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-09-25 10:26:46 +02:00
parent cc10e482d7
commit a1f48373fa
4 changed files with 158 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+10
View File
@@ -2214,6 +2214,7 @@
<field name="wp_east" type="float" unit="m"/>
<field name="wp_north" type="float" unit="m"/>
<field name="wp_alt" type="float" unit="m"/>
<field name="duration" type="float" unit="m"/>
</message>
<message name="MISSION_CIRCLE" id="21" link="forwarded">
@@ -2223,6 +2224,7 @@
<field name="center_north" type="float" unit="m"/>
<field name="center_alt" type="float" unit="m"/>
<field name="radius" type="float" unit="m"/>
<field name="duration" type="float" unit="m"/>
</message>
<message name="MISSION_SEGMENT" id="22" link="forwarded">
@@ -2233,6 +2235,7 @@
<field name="segment_east_2" type="float" unit="m"/>
<field name="segment_north_2" type="float" unit="m"/>
<field name="segment_alt" type="float" unit="m"/>
<field name="duration" type="float" unit="m"/>
</message>
<message name="MISSION_PATH" id="23" link="forwarded">
@@ -2249,6 +2252,7 @@
<field name="point_east_5" type="float" unit="m"/>
<field name="point_north_5" type="float" unit="m"/>
<field name="path_alt" type="float" unit="m"/>
<field name="duration" type="float" unit="m"/>
</message>
<message name="MISSION_SURVEY" id="24" link="forwarded">
@@ -2259,6 +2263,12 @@
<field name="survey_east_2" type="float" unit="m"/>
<field name="survey_north_2" type="float" unit="m"/>
<field name="survey_alt" type="float" unit="m"/>
<field name="duration" type="float" unit="m"/>
</message>
<message name="GOTO_MISSION" id="25" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="mission_id" type="uint8"/>
</message>
conf/modules/mission_msg.xml
+6 -1
View File
@@ -6,13 +6,18 @@
</doc>
<header>
<file name="mission_msg.h"/>
<!--<file name="mission_nav.h"/>-->
</header>
<init fun="mission_msg_init()"/>
<init fun="mission_init()"/>
<datalink message="MISSION_GOTO_WP" fun="ParseMissionGotoWp()"/>
<datalink message="MISSION_CIRCLE" fun="ParseMissionCircle()"/>
<datalink message="MISSION_PATH" fun="ParseMissionPath()"/>
<datalink message="MISSION_SEGMENT" fun="ParseMissionSegment()"/>
<datalink message="GOTO_MISSION" fun="ParseGotoMission()"/>
<makefile>
<file name="mission_msg.c"/>
<file name="mission_nav.c"/>
</makefile>
</module>
sw/airborne/modules/mission/mission_msg.c
+90 -79
View File
@@ -5,18 +5,18 @@
//#define MISSION_C
#include <stdio.h>
#include "modules/mission/mission_msg.h"
#include <math.h>
#include "subsystems/datalink/downlink.h"
#include "state.h"
/*#include "state.h"
#include "autopilot.h"
#include "subsystems/gps.h"
#include "generated/airframe.h"
#include "dl_protocol.h"
#include "dl_protocol.h"*/
struct _mission mission;
@@ -25,92 +25,103 @@ struct _mission mission;
void mission_init(void) {
mission.mission_insert_idx = 0;
mission.mission_extract_idx = 0;
//mission_nav_finish = 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;
//int insert_idx = mission.mission_insert_idx;
mission.elements[mission.mission_insert_idx].type = MissionWP;
mission.elements[mission.mission_insert_idx].element.mission_wp.wp.x = x;
mission.elements[mission.mission_insert_idx].element.mission_wp.wp.y = y;
mission.elements[mission.mission_insert_idx].element.mission_wp.wp.a = a;
mission.elements[mission.mission_insert_idx].duration = d;
insert_idx++;
if (insert_idx == MISSION_ELEMENT_NB) insert_idx = 0;
//printf("stocking mission GOTO_WP...\n");fflush(stdout);
mission.mission_insert_idx++;
if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0;
return TRUE;
}
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.);
}
int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d) {
//int insert_idx = mission.mission_insert_idx;
mission.elements[mission.mission_insert_idx].type = MissionSegment;
mission.elements[mission.mission_insert_idx].element.mission_segment.from.x = x1;
mission.elements[mission.mission_insert_idx].element.mission_segment.from.y = y1;
mission.elements[mission.mission_insert_idx].element.mission_segment.from.a = a;
mission.elements[mission.mission_insert_idx].element.mission_segment.to.x = x2;
mission.elements[mission.mission_insert_idx].element.mission_segment.to.y = y2;
mission.elements[mission.mission_insert_idx].element.mission_segment.to.a = a;
mission.elements[mission.mission_insert_idx].duration = d;
//printf("stocking mission SEGMENT...\n");fflush(stdout);
//printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout);
mission.mission_insert_idx++;
if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0;
return TRUE;
}
int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d) {
//int insert_idx = mission.mission_insert_idx;
mission.elements[mission.mission_insert_idx].type = MissionCircle;
mission.elements[mission.mission_insert_idx].element.mission_circle.center.x = x;
mission.elements[mission.mission_insert_idx].element.mission_circle.center.y = y;
mission.elements[mission.mission_insert_idx].element.mission_circle.center.a = a;
mission.elements[mission.mission_insert_idx].element.mission_circle.radius = radius;
mission.elements[mission.mission_insert_idx].duration = d;
//printf("stocking mission CIRCLE...\n");fflush(stdout);
//printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout);
mission.mission_insert_idx++;
if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 0;
return TRUE;
}
//int i_path, i_point;
int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4, float x5, float y5, float a, uint16_t d) {
//int insert_idx = mission.mission_insert_idx;
mission.elements[mission.mission_insert_idx].type = MissionPath;
int i_path;
i_path = 0;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x1;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y1;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a;
i_path++;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x2;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y2;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a;
i_path++;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x3;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y3;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a;
i_path++;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x4;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y4;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a;
i_path++;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].x = x5;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].y = y5;
mission.elements[mission.mission_insert_idx].element.mission_path.path[i_path].a = a;
mission.elements[mission.mission_insert_idx].duration = d;
//printf("stocking mission PATH...\n");fflush(stdout);
//printf("mission.mission_insert_idx = %d\n\n", mission.mission_insert_idx);fflush(stdout);
mission.mission_insert_idx++;
if (mission.mission_insert_idx == MISSION_ELEMENT_NB) mission.mission_insert_idx = 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();
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 */
}
sw/airborne/modules/mission/mission_msg.h
+52 -19
View File
@@ -30,6 +30,16 @@
#include "std.h"
#include "subsystems/navigation/common_nav.h"
#include <math.h>
#include "subsystems/datalink/downlink.h"
#include "state.h"
#include "autopilot.h"
#include "subsystems/gps.h"
#include "generated/airframe.h"
#include "dl_protocol.h"
enum MissionType {
MissionWP,
MissionCircle,
@@ -56,10 +66,12 @@ struct _mission_segment {
#define MISSION_PATH_NB 5
struct _mission_path {
struct point[MISSION_PATH_NB] path;
//struct point[MISSION_PATH_NB] path;
struct point path[MISSION_PATH_NB];
};
struct _mission_element {
//char *mission_name;
enum MissionType type;
union {
struct _mission_wp mission_wp;
@@ -68,27 +80,34 @@ struct _mission_element {
struct _mission_path mission_path;
} element;
uint16_t duration;
//bool_t mission_finish;
};
#define MISSION_ELEMENT_NB 20
struct _mission {
struct _mission_element[MISSION_ELEMENT_NB] elements;
//struct _mission_element[MISSION_ELEMENT_NB] elements;
struct _mission_element elements[MISSION_ELEMENT_NB];
uint8_t mission_insert_idx;
uint8_t mission_extract_idx;
//bool_t mission_nav_finish = FALSE;
};
extern struct _mission mission;
//extern bool_t mission_nav_finish;
extern void mission_init(void);
extern int mission_msg_GOTO_WP(float x, float y);
extern int mission_msg_GOTO_WP(float x, float y, float a, uint16_t d);
extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2);
extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d);
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);
extern int mission_msg_CIRCLE(float x, float y, float radius, float a, uint16_t d);
//extern int mission_msg_PATH_init( void );
extern int i_path, i_point;
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, float a, uint16_t d);
extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id);
@@ -97,7 +116,9 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl
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); \
float wp_alt = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); \
float duration = DL_MISSION_GOTO_WP_duration(dl_buffer); \
mission_msg_GOTO_WP(wp_east, wp_north, wp_alt, duration); \
} \
}
@@ -114,7 +135,9 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl
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); \
float path_alt = DL_MISSION_PATH_path_alt(dl_buffer); \
float duration = DL_MISSION_PATH_duration(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, path_alt, duration); \
} \
}
@@ -126,18 +149,28 @@ extern int mission_msg_PATH(float x1, float y1, float x2, float y2, float x3, fl
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); \
float segment_alt = DL_MISSION_SEGMENT_segment_alt(dl_buffer); \
float duration = DL_MISSION_SEGMENT_duration(dl_buffer); \
mission_msg_SEGMENT(segment_east_1, segment_north_1, segment_east_2, segment_north_2, segment_alt, duration); \
} \
}
/*
#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); \
#define ParseMissionCircle() { \
if (DL_MISSION_CIRCLE_ac_id(dl_buffer) == AC_ID) { \
uint8_t ac_id = DL_MISSION_CIRCLE_ac_id(dl_buffer); \
float center_east = DL_MISSION_CIRCLE_center_east(dl_buffer); \
float center_north = DL_MISSION_CIRCLE_center_north(dl_buffer); \
float radius = DL_MISSION_CIRCLE_radius(dl_buffer); \
float center_alt = DL_MISSION_CIRCLE_center_alt(dl_buffer); \
float duration = DL_MISSION_CIRCLE_duration(dl_buffer); \
mission_msg_CIRCLE(center_east, center_north, radius, center_alt, duration); \
} \
}
#define ParseGotoMission() { \
uint8_t ac_id = DL_GOTO_MISSION_ac_id(dl_buffer); \
uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); \
goto_mission_msg(ac_id, mission_id); \
}
*/
#endif // MISSION