diff --git a/conf/messages.xml b/conf/messages.xml index 99700443f9..ce098db290 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2214,6 +2214,7 @@ + @@ -2223,6 +2224,7 @@ + @@ -2233,6 +2235,7 @@ + @@ -2249,6 +2252,7 @@ + @@ -2259,6 +2263,12 @@ + + + + + + diff --git a/conf/modules/mission_msg.xml b/conf/modules/mission_msg.xml index 8812e48f81..1dee0bd4a7 100644 --- a/conf/modules/mission_msg.xml +++ b/conf/modules/mission_msg.xml @@ -6,13 +6,18 @@
+ +
- + + + + diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c index 95c084f56b..c897805794 100644 --- a/sw/airborne/modules/mission/mission_msg.c +++ b/sw/airborne/modules/mission/mission_msg.c @@ -5,18 +5,18 @@ //#define MISSION_C - +#include #include "modules/mission/mission_msg.h" #include #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 */ -} - - - diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h index c35d5f42f6..e6436491be 100644 --- a/sw/airborne/modules/mission/mission_msg.h +++ b/sw/airborne/modules/mission/mission_msg.h @@ -30,6 +30,16 @@ #include "std.h" #include "subsystems/navigation/common_nav.h" +#include + +#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