mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
[mission] better mission interface
- cleaning interface - better parsing - better handling of navigation routine - implement several insertion modes - implement duration - conversions from LLA to local are missing in parsing function - some patterns are missing - only fixedwing so far
This commit is contained in:
Gautier Hattenberger
+83
-15
@@ -2210,37 +2210,67 @@
|
||||
|
||||
<message name="MISSION_GOTO_WP" id="20" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<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"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_CIRCLE" id="21" link="forwarded">
|
||||
<message name="MISSION_GOTO_WP_LLA" id="21" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="wp_lat" type="float" unit="rad"/>
|
||||
<field name="wp_lon" type="float" unit="rad"/>
|
||||
<field name="wp_alt" type="float" unit="m"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_CIRCLE" id="22" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="center_east" type="float" unit="m"/>
|
||||
<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"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_SEGMENT" id="22" link="forwarded">
|
||||
<message name="MISSION_CIRCLE_LLA" id="23" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="center_lat" type="float" unit="rad"/>
|
||||
<field name="center_lon" type="float" unit="rad"/>
|
||||
<field name="center_alt" type="float" unit="m"/>
|
||||
<field name="radius" type="float" unit="m"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_SEGMENT" id="24" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="segment_east_1" type="float" unit="m"/>
|
||||
<field name="segment_north_1" type="float" unit="m"/>
|
||||
<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"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_PATH" id="23" link="forwarded">
|
||||
<message name="MISSION_SEGMENT_LLA" id="25" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="segment_lat_1" type="float" unit="rad"/>
|
||||
<field name="segment_lon_1" type="float" unit="rad"/>
|
||||
<field name="segment_lat_2" type="float" unit="rad"/>
|
||||
<field name="segment_lon_2" type="float" unit="rad"/>
|
||||
<field name="segment_alt" type="float" unit="m"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_PATH" id="26" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="point_east_1" type="float" unit="m"/>
|
||||
<field name="point_north_1" type="float" unit="m"/>
|
||||
<field name="point_east_2" type="float" unit="m"/>
|
||||
@@ -2252,25 +2282,63 @@
|
||||
<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"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
<field name="nb" type="uint8"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_SURVEY" id="24" link="forwarded">
|
||||
<message name="MISSION_PATH_LLA" id="27" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="point_lat_1" type="float" unit="rad"/>
|
||||
<field name="point_lon_1" type="float" unit="rad"/>
|
||||
<field name="point_lat_2" type="float" unit="rad"/>
|
||||
<field name="point_lon_2" type="float" unit="rad"/>
|
||||
<field name="point_lat_3" type="float" unit="rad"/>
|
||||
<field name="point_lon_3" type="float" unit="rad"/>
|
||||
<field name="point_lat_4" type="float" unit="rad"/>
|
||||
<field name="point_lon_4" type="float" unit="rad"/>
|
||||
<field name="point_lat_5" type="float" unit="rad"/>
|
||||
<field name="point_lon_5" type="float" unit="rad"/>
|
||||
<field name="path_alt" type="float" unit="m"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
<field name="nb" type="uint8"/>
|
||||
</message>
|
||||
|
||||
<message name="MISSION_SURVEY" id="28" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="survey_east_1" type="float" unit="m"/>
|
||||
<field name="survey_north_1" type="float" unit="m"/>
|
||||
<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"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="GOTO_MISSION" id="25" link="forwarded">
|
||||
<message name="MISSION_SURVEY_LLA" id="29" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
|
||||
<field name="survey_lat_1" type="float" unit="rad"/>
|
||||
<field name="survey_lon_1" type="float" unit="rad"/>
|
||||
<field name="survey_lat_2" type="float" unit="rad"/>
|
||||
<field name="survey_lon_2" type="float" unit="rad"/>
|
||||
<field name="survey_alt" type="float" unit="m"/>
|
||||
<field name="duration" type="float" unit="s"/>
|
||||
</message>
|
||||
|
||||
<message name="GOTO_MISSION" id="30" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
<field name="mission_id" type="uint8"/>
|
||||
</message>
|
||||
|
||||
<message name="NEXT_MISSION" id="31" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
</message>
|
||||
|
||||
<message name="END_MISSION" id="32" link="forwarded">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
</message>
|
||||
|
||||
|
||||
<message name="WINDTURBINE_STATUS" id="50" link="broadcasted">
|
||||
<field name="ac_id" type="uint8"/>
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="mission_fw" dir="mission">
|
||||
<doc>
|
||||
<description>
|
||||
Interface for mission control of fixed wing aircraft.
|
||||
This module parse datalink commands for basic navigation routines
|
||||
and store them in a queue.
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="mission.h"/>
|
||||
</header>
|
||||
<init fun="mission_init()"/>
|
||||
|
||||
<datalink message="MISSION_GOTO_WP" fun="mission_parse_GOTO_WP()"/>
|
||||
<datalink message="MISSION_GOTO_WP_LLA" fun="mission_parse_GOTO_WP_LLA()"/>
|
||||
<datalink message="MISSION_CIRCLE" fun="mission_parse_CIRCLE()"/>
|
||||
<datalink message="MISSION_CIRCLE_LLA" fun="mission_parse_CIRCLE_LLA()"/>
|
||||
<datalink message="MISSION_SEGMENT" fun="mission_parse_SEGMENT()"/>
|
||||
<datalink message="MISSION_SEGMENT_LLA" fun="mission_parse_SEGMENT_LLA()"/>
|
||||
<datalink message="MISSION_PATH" fun="mission_parse_PATH()"/>
|
||||
<datalink message="MISSION_PATH_LLA" fun="mission_parse_PATH_LLA()"/>
|
||||
<datalink message="GOTO_MISSION" fun="mission_parse_GOTO_MISSION()"/>
|
||||
<datalink message="NEXT_MISSION" fun="mission_parse_NEXT_MISSION()"/>
|
||||
<datalink message="END_MISSION" fun="mission_parse_END_MISSION()"/>
|
||||
|
||||
<makefile>
|
||||
<file name="mission.c"/>
|
||||
<file name="mission_fw_nav.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -1,23 +0,0 @@
|
||||
<!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"/>
|
||||
|
||||
<!--<file name="mission_nav.h"/>-->
|
||||
</header>
|
||||
<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>
|
||||
@@ -0,0 +1,253 @@
|
||||
/*
|
||||
* Copyright (C) 2013 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 modules/mission/mission.c
|
||||
* @brief messages parser for mission interface
|
||||
*/
|
||||
|
||||
#include "modules/mission/mission.h"
|
||||
|
||||
#include <string.h>
|
||||
#include "generated/airframe.h"
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
|
||||
|
||||
struct _mission mission;
|
||||
|
||||
|
||||
void mission_init(void) {
|
||||
mission.insert_idx = 0;
|
||||
mission.current_idx = 0;
|
||||
mission.element_time = 0.;
|
||||
}
|
||||
|
||||
bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element) {
|
||||
uint8_t tmp;
|
||||
|
||||
switch (insert) {
|
||||
case Append:
|
||||
tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB;
|
||||
if (tmp == mission.current_idx) return FALSE; // no room to insert element
|
||||
memcpy(&mission.elements[mission.insert_idx], element, sizeof(struct _mission_element)); // add element
|
||||
mission.insert_idx = tmp; // move insert index
|
||||
break;
|
||||
case Prepend:
|
||||
if (mission.current_idx == 0) tmp = MISSION_ELEMENT_NB-1;
|
||||
else tmp = mission.current_idx - 1;
|
||||
if (tmp == mission.insert_idx) return FALSE; // no room to inser element
|
||||
memcpy(&mission.elements[tmp], element, sizeof(struct _mission_element)); // add element
|
||||
mission.current_idx = tmp; // move current index
|
||||
break;
|
||||
case ReplaceCurrent:
|
||||
// current element can always be modified, index are not changed
|
||||
memcpy(&mission.elements[mission.current_idx], element, sizeof(struct _mission_element));
|
||||
break;
|
||||
case ReplaceAll:
|
||||
// reset queue and index
|
||||
memcpy(&mission.elements[0], element, sizeof(struct _mission_element));
|
||||
mission.insert_idx = 0;
|
||||
mission.current_idx = 0;
|
||||
break;
|
||||
default:
|
||||
// unknown insertion mode
|
||||
return FALSE;
|
||||
}
|
||||
return TRUE;
|
||||
|
||||
}
|
||||
|
||||
bool_t mission_get(struct _mission_element * element __attribute__((unused))) {
|
||||
if (mission.current_idx == mission.insert_idx) {
|
||||
element = NULL;
|
||||
return FALSE;
|
||||
}
|
||||
element = &(mission.elements[mission.current_idx]);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
///////////////////////
|
||||
// Parsing functions //
|
||||
///////////////////////
|
||||
|
||||
int mission_parse_GOTO_WP(void) {
|
||||
if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionWP;
|
||||
me.element.mission_wp.wp.x = DL_MISSION_GOTO_WP_wp_east(dl_buffer);
|
||||
me.element.mission_wp.wp.y = DL_MISSION_GOTO_WP_wp_north(dl_buffer);
|
||||
me.element.mission_wp.wp.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer);
|
||||
me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_GOTO_WP_LLA(void) {
|
||||
if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionWP;
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_LLA_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_CIRCLE(void) {
|
||||
if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionCircle;
|
||||
me.element.mission_circle.center.x = DL_MISSION_CIRCLE_center_east(dl_buffer);
|
||||
me.element.mission_circle.center.y = DL_MISSION_CIRCLE_center_north(dl_buffer);
|
||||
me.element.mission_circle.center.z = DL_MISSION_CIRCLE_center_alt(dl_buffer);
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer);
|
||||
me.duration = DL_MISSION_CIRCLE_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_CIRCLE_LLA(void) {
|
||||
if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionCircle;
|
||||
me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer);
|
||||
me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_LLA_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_SEGMENT(void) {
|
||||
if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionSegment;
|
||||
me.element.mission_segment.from.x = DL_MISSION_SEGMENT_segment_east_1(dl_buffer);
|
||||
me.element.mission_segment.from.y = DL_MISSION_SEGMENT_segment_north_1(dl_buffer);
|
||||
me.element.mission_segment.from.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
|
||||
me.element.mission_segment.to.x = DL_MISSION_SEGMENT_segment_east_2(dl_buffer);
|
||||
me.element.mission_segment.to.y = DL_MISSION_SEGMENT_segment_north_2(dl_buffer);
|
||||
me.element.mission_segment.to.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer);
|
||||
me.duration = DL_MISSION_SEGMENT_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_SEGMENT_LLA(void) {
|
||||
if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionSegment;
|
||||
me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_LLA_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_PATH(void) {
|
||||
if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionPath;
|
||||
me.element.mission_path.path[0].x = DL_MISSION_PATH_point_east_1(dl_buffer);
|
||||
me.element.mission_path.path[0].y = DL_MISSION_PATH_point_north_1(dl_buffer);
|
||||
me.element.mission_path.path[0].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path[1].x = DL_MISSION_PATH_point_east_2(dl_buffer);
|
||||
me.element.mission_path.path[1].y = DL_MISSION_PATH_point_north_2(dl_buffer);
|
||||
me.element.mission_path.path[1].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path[2].x = DL_MISSION_PATH_point_east_3(dl_buffer);
|
||||
me.element.mission_path.path[2].y = DL_MISSION_PATH_point_north_3(dl_buffer);
|
||||
me.element.mission_path.path[2].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path[3].x = DL_MISSION_PATH_point_east_4(dl_buffer);
|
||||
me.element.mission_path.path[3].y = DL_MISSION_PATH_point_north_4(dl_buffer);
|
||||
me.element.mission_path.path[3].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.path[4].x = DL_MISSION_PATH_point_east_5(dl_buffer);
|
||||
me.element.mission_path.path[4].y = DL_MISSION_PATH_point_north_5(dl_buffer);
|
||||
me.element.mission_path.path[4].z = DL_MISSION_PATH_path_alt(dl_buffer);
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer);
|
||||
if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB;
|
||||
me.element.mission_path.path_idx = 0;
|
||||
me.duration = DL_MISSION_PATH_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_PATH_LLA(void) {
|
||||
if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
struct _mission_element me;
|
||||
me.type = MissionPath;
|
||||
me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer);
|
||||
if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB;
|
||||
me.element.mission_path.path_idx = 0;
|
||||
me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer);
|
||||
|
||||
enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_LLA_insert(dl_buffer));
|
||||
|
||||
return mission_insert(insert, &me);
|
||||
}
|
||||
|
||||
int mission_parse_GOTO_MISSION(void) {
|
||||
if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer);
|
||||
if (mission_id < MISSION_ELEMENT_NB) {
|
||||
mission.current_idx = mission_id;
|
||||
}
|
||||
else return FALSE;
|
||||
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
int mission_parse_NEXT_MISSION(void) {
|
||||
if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
if (mission.current_idx == mission.insert_idx) return FALSE; // already at the last position
|
||||
|
||||
// increment current index
|
||||
mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
int mission_parse_END_MISSION(void) {
|
||||
if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
|
||||
|
||||
// set current index to insert index (last position)
|
||||
mission.current_idx = mission.insert_idx;
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,143 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 modules/mission/mission.h
|
||||
* @brief mission planner library
|
||||
*
|
||||
* Provide the generic interface for the mission control
|
||||
* Handle the parsing of datalink messages
|
||||
*/
|
||||
|
||||
#ifndef MISSION_H
|
||||
#define MISSION_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
enum MissionType {
|
||||
MissionWP,
|
||||
MissionCircle,
|
||||
MissionSegment,
|
||||
MissionPath,
|
||||
MissionSurvey,
|
||||
MissionEight,
|
||||
MissionOval
|
||||
};
|
||||
|
||||
enum MissionInsertMode {
|
||||
Append, ///< add at the last position
|
||||
Prepend, ///< add before the current element
|
||||
ReplaceCurrent, ///< replace current element
|
||||
ReplaceAll ///< remove all elements and add the new one
|
||||
};
|
||||
|
||||
struct _mission_wp {
|
||||
struct EnuCoor_f wp;
|
||||
};
|
||||
|
||||
struct _mission_circle {
|
||||
struct EnuCoor_f center;
|
||||
float radius;
|
||||
};
|
||||
|
||||
struct _mission_segment {
|
||||
struct EnuCoor_f from;
|
||||
struct EnuCoor_f to;
|
||||
};
|
||||
|
||||
#define MISSION_PATH_NB 5
|
||||
struct _mission_path {
|
||||
struct EnuCoor_f path[MISSION_PATH_NB];
|
||||
uint8_t path_idx;
|
||||
uint8_t nb;
|
||||
};
|
||||
|
||||
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;
|
||||
float duration; ///< time to spend in the element (<= 0 to disable)
|
||||
};
|
||||
|
||||
#define MISSION_ELEMENT_NB 20
|
||||
struct _mission {
|
||||
struct _mission_element elements[MISSION_ELEMENT_NB];
|
||||
float element_time; ///< time in second spend in the current element
|
||||
uint8_t insert_idx; ///< inserstion index
|
||||
uint8_t current_idx; ///< current mission element index
|
||||
};
|
||||
|
||||
extern struct _mission mission;
|
||||
|
||||
/** Init mission structure
|
||||
*/
|
||||
extern void mission_init(void);
|
||||
|
||||
/** Insert a mission element according to the insertion mode
|
||||
* @param insert insertion mode
|
||||
* @param element mission element structure
|
||||
* @return return TRUE if insertion is succesful, FALSE otherwise
|
||||
*/
|
||||
extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element);
|
||||
|
||||
/** Get current mission element
|
||||
* @param element pointer to the current mission element
|
||||
* @return TRUE if valid element, FALSE if mission queue is empty (add element is set to NULL)
|
||||
*/
|
||||
extern bool_t mission_get(struct _mission_element * element);
|
||||
|
||||
/** Run mission
|
||||
*
|
||||
* This function should be implemented into a dedicated file since
|
||||
* navigation functions are different for different firmwares
|
||||
*
|
||||
* Currently, this function should be called from the flight plan
|
||||
*
|
||||
* @return return TRUE when the mission is running, FALSE when it is finished
|
||||
*/
|
||||
extern int mission_run();
|
||||
|
||||
/** Parsing functions called when a mission message is received
|
||||
*/
|
||||
extern int mission_parse_GOTO_WP(void);
|
||||
extern int mission_parse_GOTO_WP_LLA(void);
|
||||
extern int mission_parse_CIRCLE(void);
|
||||
extern int mission_parse_CIRCLE_LLA(void);
|
||||
extern int mission_parse_SEGMENT(void);
|
||||
extern int mission_parse_SEGMENT_LLA(void);
|
||||
extern int mission_parse_PATH(void);
|
||||
extern int mission_parse_PATH_LLA(void);
|
||||
extern int mission_parse_SURVEY(void);
|
||||
extern int mission_parse_SURVEY_LLA(void);
|
||||
extern int mission_parse_GOTO_MISSION(void);
|
||||
extern int mission_parse_NEXT_MISSION(void);
|
||||
extern int mission_parse_END_MISSION(void);
|
||||
|
||||
/** Status report messages
|
||||
* @todo
|
||||
*/
|
||||
|
||||
#endif // MISSION
|
||||
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Gautier Hattenberger
|
||||
*
|
||||
* 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 modules/mission/mission_fw_nav.c
|
||||
* @brief mission navigation for fixedwing aircraft
|
||||
*
|
||||
* Implement specific navigation routines for the mission control
|
||||
* of a fixedwing aircraft
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "modules/mission/mission.h"
|
||||
#include "firmwares/fixedwing/autopilot.h"
|
||||
#include "subsystems/nav.h"
|
||||
|
||||
// navigation time step
|
||||
const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
|
||||
|
||||
// dirty hack to comply with nav_approaching_xy function
|
||||
struct EnuCoor_f last_wp = { 0., 0., 0. };
|
||||
|
||||
/** Navigation function to a single waypoint
|
||||
*/
|
||||
static inline bool_t mission_nav_wp(struct _mission_wp * wp) {
|
||||
if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) {
|
||||
last_wp = wp->wp; // store last wp
|
||||
return FALSE; // end of mission element
|
||||
}
|
||||
// set navigation command
|
||||
fly_to_xy(wp->wp.x, wp->wp.y);
|
||||
NavVerticalAutoThrottleMode(0.);
|
||||
NavVerticalAltitudeMode(wp->wp.z, 0.);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/** Navigation function on a circle
|
||||
*/
|
||||
static inline bool_t mission_nav_circle(struct _mission_circle * circle) {
|
||||
nav_circle_XY(circle->center.x, circle->center.y, circle->radius);
|
||||
NavVerticalAutoThrottleMode(0.);
|
||||
NavVerticalAltitudeMode(circle->center.z, 0.);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/** Navigation function along a segment
|
||||
*/
|
||||
static inline bool_t mission_nav_segment(struct _mission_segment * segment) {
|
||||
if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) {
|
||||
last_wp = segment->to;
|
||||
return FALSE; // end of mission element
|
||||
}
|
||||
nav_route_xy(segment->from.x, segment->from.y, segment->to.x, segment->to.y);
|
||||
NavVerticalAutoThrottleMode(0.);
|
||||
NavVerticalAltitudeMode(segment->to.z, 0.); // both altitude should be the same anyway
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/** Navigation function along a path
|
||||
*/
|
||||
static inline bool_t mission_nav_path(struct _mission_path * path) {
|
||||
if (path->nb == 0) {
|
||||
return FALSE; // nothing to do
|
||||
}
|
||||
if (path->nb == 1) {
|
||||
// handle as a single waypoint
|
||||
struct _mission_wp wp = { path->path[0] };
|
||||
return mission_nav_wp(&wp);
|
||||
}
|
||||
if (path->path_idx == path->nb-1) {
|
||||
last_wp = path->path[path->path_idx]; // store last wp
|
||||
return FALSE; // end of path
|
||||
}
|
||||
// normal case
|
||||
struct EnuCoor_f from = path->path[path->path_idx];
|
||||
struct EnuCoor_f to = path->path[path->path_idx+1];
|
||||
nav_route_xy(from.x, from.y, to.x, to.y);
|
||||
NavVerticalAutoThrottleMode(0.);
|
||||
NavVerticalAltitudeMode(to.z, 0.); // both altitude should be the same anyway
|
||||
if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) {
|
||||
path->path_idx++; // go to next segment
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
int mission_run() {
|
||||
// current element
|
||||
struct _mission_element * el = NULL;
|
||||
if (!mission_get(el)) {
|
||||
// TODO do something special like a waiting circle before ending the mission ?
|
||||
return FALSE; // end of mission
|
||||
}
|
||||
|
||||
bool_t el_running = FALSE;
|
||||
switch (el->type){
|
||||
case MissionWP:
|
||||
el_running = mission_nav_wp(&(el->element.mission_wp));
|
||||
break;
|
||||
case MissionCircle:
|
||||
el_running = mission_nav_circle(&(el->element.mission_circle));
|
||||
break;
|
||||
case MissionSegment:
|
||||
el_running = mission_nav_segment(&(el->element.mission_segment));
|
||||
break;
|
||||
case MissionPath:
|
||||
el_running = mission_nav_path(&(el->element.mission_path));
|
||||
break;
|
||||
default:
|
||||
// invalid type or pattern not yet handled
|
||||
break;
|
||||
}
|
||||
|
||||
// increment element_time
|
||||
mission.element_time += dt_navigation;
|
||||
|
||||
// test if element is finished or element time is elapsed
|
||||
if (!el_running || (el->duration > 0. && mission.element_time >= el->duration)) {
|
||||
// reset timer
|
||||
mission.element_time = 0.;
|
||||
// go to next element
|
||||
mission.current_idx++;
|
||||
}
|
||||
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,127 +0,0 @@
|
||||
/** \file mission_msg.c
|
||||
* \brief the new messages for mission planner library
|
||||
* \ edited by Anh Truong
|
||||
*/
|
||||
|
||||
//#define MISSION_C
|
||||
|
||||
#include <stdio.h>
|
||||
#include "modules/mission/mission_msg.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"*/
|
||||
|
||||
|
||||
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) {
|
||||
//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;
|
||||
|
||||
//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, 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,176 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2013 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
|
||||
*/
|
||||
|
||||
#ifndef MISSION_H
|
||||
#define MISSION_H
|
||||
|
||||
#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,
|
||||
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 point path[MISSION_PATH_NB];
|
||||
};
|
||||
|
||||
struct _mission_element {
|
||||
//char *mission_name;
|
||||
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;
|
||||
//bool_t mission_finish;
|
||||
};
|
||||
|
||||
#define MISSION_ELEMENT_NB 20
|
||||
struct _mission {
|
||||
//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, float a, uint16_t d);
|
||||
|
||||
extern int mission_msg_SEGMENT(float x1, float y1, float x2, float y2, float a, uint16_t d);
|
||||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
#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); \
|
||||
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); \
|
||||
} \
|
||||
}
|
||||
|
||||
#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); \
|
||||
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); \
|
||||
} \
|
||||
}
|
||||
|
||||
|
||||
#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); \
|
||||
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 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
|
||||
@@ -1,180 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2013 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_nav.h
|
||||
* @brief mission navigation
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "modules/mission/mission_nav.h"
|
||||
//#include "std.h"
|
||||
//#include "subsystems/nav.h"
|
||||
|
||||
|
||||
int mission_nav_GOTO_WP(float x, float y) {
|
||||
fly_to_xy(x, y);
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
//int mission_nav_SEGMENT(float x1, float y1, float x2, float y2) {
|
||||
bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2) {
|
||||
|
||||
if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) {
|
||||
//mission_nav_finish = 1;
|
||||
return FALSE; // end of block and go to the next block
|
||||
}
|
||||
else {
|
||||
nav_route_xy(x1, y1, x2, y2);
|
||||
printf("doing mission SEGMENT ...\n\n");fflush(stdout);
|
||||
}
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
//int mission_nav_CIRCLE(float x, float y, float radius){
|
||||
bool_t mission_nav_CIRCLE(float x, float y, float radius){
|
||||
nav_circle_XY(x, y, radius);
|
||||
printf("doing mission CIRCLE ...\n\n");fflush(stdout);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int mission_path_seg_idx;
|
||||
mission_path_seg_idx = 1;
|
||||
//int mission_nav_PATH(struct _mission_path mission_path_nav) {
|
||||
bool_t mission_nav_PATH(struct _mission_path mission_path_nav) {
|
||||
float x1, y1, x2, y2, x3, y3, x4, y4, x5, y5;
|
||||
switch (mission_path_seg_idx) {
|
||||
case 1: // path 1-2
|
||||
x1 = mission_path_nav.path[0].x;
|
||||
y1 = mission_path_nav.path[0].y;
|
||||
x2 = mission_path_nav.path[1].x;
|
||||
y2 = mission_path_nav.path[1].y;
|
||||
nav_route_xy(x1, y1, x2, y2);
|
||||
printf("doing mission PATH: path 1-2...\n\n");fflush(stdout);
|
||||
if (nav_approaching_xy(x2, y2, x1, y1, CARROT)) {
|
||||
mission_path_seg_idx = 2;
|
||||
}
|
||||
break;
|
||||
case 2: // path 2-3
|
||||
x2 = mission_path_nav.path[1].x;
|
||||
y2 = mission_path_nav.path[1].y;
|
||||
x3 = mission_path_nav.path[2].x;
|
||||
y3 = mission_path_nav.path[2].y;
|
||||
nav_route_xy(x2, y2, x3, y3);
|
||||
printf("doing mission PATH: path 2-3...\n\n");fflush(stdout);
|
||||
if (nav_approaching_xy(x3, y3, x2, y2, CARROT)) {
|
||||
mission_path_seg_idx = 3;
|
||||
}
|
||||
break;
|
||||
case 3: // path 3-4
|
||||
x3 = mission_path_nav.path[2].x;
|
||||
y3 = mission_path_nav.path[2].y;
|
||||
x4 = mission_path_nav.path[3].x;
|
||||
y4 = mission_path_nav.path[3].y;
|
||||
nav_route_xy(x3, y3, x4, y4);
|
||||
printf("doing mission PATH: path 3-4...\n\n");fflush(stdout);
|
||||
if (nav_approaching_xy(x4, y4, x3, y3, CARROT)) {
|
||||
mission_path_seg_idx = 4;
|
||||
}
|
||||
break;
|
||||
case 4: // path 4-5
|
||||
x4 = mission_path_nav.path[3].x;
|
||||
y4 = mission_path_nav.path[3].y;
|
||||
x5 = mission_path_nav.path[4].x;
|
||||
y5 = mission_path_nav.path[4].y;
|
||||
nav_route_xy(x4, y4, x5, y5);
|
||||
printf("doing mission PATH: path 4-5...\n\n");fflush(stdout);
|
||||
if (nav_approaching_xy(x5, y5, x4, y4, CARROT)) {
|
||||
//mission_nav_finish = 1;
|
||||
return FALSE; // This path ends when AC arrive to point 5
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return TRUE; // do the function
|
||||
}
|
||||
|
||||
|
||||
bool_t mission_nav_return;
|
||||
bool_t mission_nav_finish;
|
||||
int i_mission;
|
||||
int mission_call(struct _mission mission) {
|
||||
//void mission_nav() {
|
||||
//int i_mission;
|
||||
// detect where the mission list is (by detecting duration non NULL)??????????????????????????????????????????????????????????????????????
|
||||
|
||||
float wp_mission_x, wp_mission_y;
|
||||
float wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y;
|
||||
float wp_center_x, wp_center_y, radius;
|
||||
struct _mission_path mission_path_nav;
|
||||
|
||||
switch (mission.elements[i_mission].type){
|
||||
case MissionWP:
|
||||
wp_mission_x = mission.elements[i_mission].element.mission_wp.wp.x;
|
||||
wp_mission_y = mission.elements[i_mission].element.mission_wp.wp.y;
|
||||
//mission_nav_GOTO_WP(wp_mission_x, wp_mission_y);
|
||||
mission_nav_GOTO_WP(wp_mission_x, wp_mission_y);
|
||||
break;
|
||||
|
||||
case MissionSegment:
|
||||
wp_mission_from_x = mission.elements[i_mission].element.mission_segment.from.x;
|
||||
wp_mission_from_y = mission.elements[i_mission].element.mission_segment.from.y;
|
||||
wp_mission_to_x = mission.elements[i_mission].element.mission_segment.to.x;
|
||||
wp_mission_to_y = mission.elements[i_mission].element.mission_segment.to.y;
|
||||
//mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y);
|
||||
mission_nav_return = mission_nav_SEGMENT(wp_mission_from_x, wp_mission_from_y, wp_mission_to_x, wp_mission_to_y);
|
||||
break;
|
||||
|
||||
case MissionCircle:
|
||||
wp_center_x = mission.elements[i_mission].element.mission_circle.center.x;
|
||||
wp_center_y = mission.elements[i_mission].element.mission_circle.center.y;
|
||||
radius = mission.elements[i_mission].element.mission_circle.radius;
|
||||
//mission_nav_CIRCLE(wp_center_x, wp_center_y, radius);
|
||||
mission_nav_return = mission_nav_CIRCLE(wp_center_x, wp_center_y, radius);
|
||||
break;
|
||||
|
||||
case MissionPath:
|
||||
mission_path_nav = mission.elements[i_mission].element.mission_path;
|
||||
//mission_nav_PATH(mission_path_nav);
|
||||
mission_nav_return = mission_nav_PATH(mission_path_nav);
|
||||
break;
|
||||
}
|
||||
|
||||
if (!(mission_nav_return)){ //when mission.elements is finished
|
||||
printf("finish mission.elements %d\n\n\n", i_mission);fflush(stdout);
|
||||
i_mission++;
|
||||
printf("goto mission.elements %d\n\n\n", i_mission);fflush(stdout);
|
||||
}
|
||||
|
||||
|
||||
if (i_mission == MISSION_ELEMENT_NB) i_mission = 0;
|
||||
return TRUE; /* do the function */
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
int goto_mission_msg(uint8_t ac_id, uint8_t mission_id){
|
||||
i_mission = mission_id;
|
||||
mission_call(mission);
|
||||
}
|
||||
@@ -1,49 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2013 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_nav.h
|
||||
* @brief mission navigation
|
||||
*/
|
||||
|
||||
|
||||
#include "modules/mission/mission_msg.h"
|
||||
#include "std.h"
|
||||
#include "subsystems/nav.h"
|
||||
|
||||
|
||||
extern int mission_nav_GOTO_WP(float x, float y);
|
||||
//extern int mission_nav_SEGMENT(float x1, float y1, float x2, float y2);
|
||||
extern bool_t mission_nav_SEGMENT(float x1, float y1, float x2, float y2);
|
||||
|
||||
//extern int mission_path_seg_idx;
|
||||
//extern int i_seg;
|
||||
|
||||
//extern int mission_nav_PATH_init( void );
|
||||
extern bool_t mission_nav_PATH(struct _mission_path mission_path_nav);
|
||||
|
||||
extern bool_t mission_nav_return, mission_nav_finish;
|
||||
//extern int mission_nav_return;
|
||||
//extern int mission_nav_finish;
|
||||
extern int i_mission;
|
||||
extern int mission_call(struct _mission mission);
|
||||
|
||||
//extern int goto_mission_msg(uint8_t ac_id, uint8_t mission_id);
|
||||
Reference in New Issue
Block a user