Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #548 from enacuavlab/mission

Browse Source 
Mission
This commit is contained in:
Gautier Hattenberger
2013-10-04 06:33:48 -07:00
parent 780c03c05a ddab784efb
commit cf2e893204
8 changed files with 785 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/flight_plan.dtd
+4 -2
View File
@@ -159,7 +159,8 @@ var CDATA #REQUIRED
value CDATA #REQUIRED>
<!ATTLIST call
fun CDATA #REQUIRED>
fun CDATA #REQUIRED
until CDATA #IMPLIED>
<!ATTLIST follow
ac_id CDATA #REQUIRED
@@ -211,7 +212,8 @@ until CDATA #IMPLIED>
grid CDATA #REQUIRED
orientation CDATA #IMPLIED
wp1 CDATA #REQUIRED
wp2 CDATA #REQUIRED>
wp2 CDATA #REQUIRED
until CDATA #IMPLIED>
<!ATTLIST stay
wp CDATA #REQUIRED
conf/flight_plans/mission_fw.xml
+63
View File
@@ -0,0 +1,63 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
<header/>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="STDBY" x="49.5" y="100.1"/>
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
<waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
</waypoints>
<exceptions/>
<blocks>
<block name="Wait GPS">
<set value="1" var="kill_throttle"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
</block>
<block name="Holding point">
<!--set var="nav_mode" value="NAV_MODE_ROLL"/-->
<set value="1" var="kill_throttle"/>
<attitude roll="0" throttle="0" vmode="throttle"/>
</block>
<block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
<exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
<set value="0" var="kill_throttle"/>
<set value="0" var="autopilot_flight_time"/>
<go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
</block>
<block key="<Control>a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<circle radius="nav_radius" wp="STDBY"/>
</block>
<block name="Mission" strip_button="Mission">
<call fun="mission_run()"/>
<deroute block="Standby"/>
</block>
<block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<deroute block="land"/>
</block>
<block name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png" group="land">
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<deroute block="land"/>
</block>
<block name="land">
<call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
</block>
<block name="final">
<exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
</block>
<block name="flare">
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/messages.xml
+133
View File
@@ -2251,6 +2251,139 @@
<field name="resolve" type="uint8" values="NONE|LEVEL|CLIMB|DESCEND"/>
</message>
<message name="MISSION_GOTO_WP" id="20" link="forwarded">
<field name="ac_id" 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="s"/>
</message>
<message name="MISSION_GOTO_WP_LLA" id="21" link="forwarded">
<field name="ac_id" 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="s"/>
</message>
<message name="MISSION_CIRCLE_LLA" id="23" link="forwarded">
<field name="ac_id" 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="s"/>
</message>
<message name="MISSION_SEGMENT_LLA" id="25" link="forwarded">
<field name="ac_id" 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"/>
<field name="point_north_2" type="float" unit="m"/>
<field name="point_east_3" type="float" unit="m"/>
<field name="point_north_3" type="float" unit="m"/>
<field name="point_east_4" type="float" unit="m"/>
<field name="point_north_4" type="float" unit="m"/>
<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="s"/>
<field name="nb" type="uint8"/>
</message>
<message name="MISSION_PATH_LLA" id="27" link="forwarded">
<field name="ac_id" 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="s"/>
</message>
<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"/>
<field name="tb_id" type="uint8"/>
conf/modules/mission_fw.xml
+32
View File
@@ -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>
sw/airborne/modules/mission/mission.c
+251
View File
@@ -0,0 +1,251 @@
/*
* 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;
}
struct _mission_element * mission_get(void) {
if (mission.current_idx == mission.insert_idx) {
return NULL;
}
return &(mission.elements[mission.current_idx]);
}
///////////////////////
// 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;
}
sw/airborne/modules/mission/mission.h
+142
View File
@@ -0,0 +1,142 @@
/*
* 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
* @return return a pointer to the next mission element or NULL if no more elements
*/
extern struct _mission_element * mission_get(void);
/** 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
sw/airborne/modules/mission/mission_fw_nav.c
+146
View File
@@ -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 ((el = mission_get()) == NULL) {
// 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;
}
sw/tools/gen_flight_plan.ml
+14
View File
@@ -481,6 +481,13 @@ let rec print_stage = fun index_of_waypoints x ->
let statement = ExtXml.attrib x "fun" in
lprintf "if (! (%s))\n" statement;
lprintf " NextStageAndBreak();\n";
begin
try
let c = parsed_attrib x "until" in
lprintf "if (%s) NextStageAndBreak();\n" c
with
ExtXml.Error _ -> ()
end;
lprintf "break;\n"
| "survey_rectangle" ->
let grid = parsed_attrib x "grid"
@@ -495,6 +502,13 @@ let rec print_stage = fun index_of_waypoints x ->
left ();
stage ();
lprintf "NavSurveyRectangle(%s, %s);\n" wp1 wp2;
begin
try
let c = parsed_attrib x "until" in
lprintf "if (%s) NextStageAndBreak();\n" c
with
ExtXml.Error _ -> ()
end;
lprintf "break;\n"
| _s -> failwith "Unreachable"
end;