From 7b8f824fbdba670e496237d3b805bf228bd60540 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 2 Oct 2013 15:14:21 +0200 Subject: [PATCH] [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 --- conf/messages.xml | 98 +++++-- conf/modules/mission_fw.xml | 32 +++ conf/modules/mission_msg.xml | 23 -- sw/airborne/modules/mission/mission.c | 253 +++++++++++++++++++ sw/airborne/modules/mission/mission.h | 143 +++++++++++ sw/airborne/modules/mission/mission_fw_nav.c | 146 +++++++++++ sw/airborne/modules/mission/mission_msg.c | 127 ---------- sw/airborne/modules/mission/mission_msg.h | 176 ------------- sw/airborne/modules/mission/mission_nav.c | 180 ------------- sw/airborne/modules/mission/mission_nav.h | 49 ---- 10 files changed, 657 insertions(+), 570 deletions(-) create mode 100644 conf/modules/mission_fw.xml delete mode 100644 conf/modules/mission_msg.xml create mode 100644 sw/airborne/modules/mission/mission.c create mode 100644 sw/airborne/modules/mission/mission.h create mode 100644 sw/airborne/modules/mission/mission_fw_nav.c delete mode 100644 sw/airborne/modules/mission/mission_msg.c delete mode 100644 sw/airborne/modules/mission/mission_msg.h delete mode 100644 sw/airborne/modules/mission/mission_nav.c delete mode 100644 sw/airborne/modules/mission/mission_nav.h diff --git a/conf/messages.xml b/conf/messages.xml index ce098db290..919b48adec 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2210,37 +2210,67 @@ - + - + - + - + + + + + + + + + + - + - + - + + + + + + + + + + + - + - + - + + + + + + + + + + + + @@ -2252,25 +2282,63 @@ - + + - + - + + + + + + + + + + + + + + + + + + + - + - + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/mission_fw.xml b/conf/modules/mission_fw.xml new file mode 100644 index 0000000000..096e896394 --- /dev/null +++ b/conf/modules/mission_fw.xml @@ -0,0 +1,32 @@ + + + + + + Interface for mission control of fixed wing aircraft. + This module parse datalink commands for basic navigation routines + and store them in a queue. + + +
+ +
+ + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/mission_msg.xml b/conf/modules/mission_msg.xml deleted file mode 100644 index 1dee0bd4a7..0000000000 --- a/conf/modules/mission_msg.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - New messages for Mission Planner - -
- - - -
- - - - - - - - - - - - diff --git a/sw/airborne/modules/mission/mission.c b/sw/airborne/modules/mission/mission.c new file mode 100644 index 0000000000..dc20909f2f --- /dev/null +++ b/sw/airborne/modules/mission/mission.c @@ -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 +#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; +} + diff --git a/sw/airborne/modules/mission/mission.h b/sw/airborne/modules/mission/mission.h new file mode 100644 index 0000000000..14cd363ea4 --- /dev/null +++ b/sw/airborne/modules/mission/mission.h @@ -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 diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c new file mode 100644 index 0000000000..7bc3494a0d --- /dev/null +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -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 +#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; +} + + diff --git a/sw/airborne/modules/mission/mission_msg.c b/sw/airborne/modules/mission/mission_msg.c deleted file mode 100644 index c897805794..0000000000 --- a/sw/airborne/modules/mission/mission_msg.c +++ /dev/null @@ -1,127 +0,0 @@ -/** \file mission_msg.c - * \brief the new messages for mission planner library - * \ edited by Anh Truong - */ - -//#define MISSION_C - -#include -#include "modules/mission/mission_msg.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"*/ - - -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; -} - - - - diff --git a/sw/airborne/modules/mission/mission_msg.h b/sw/airborne/modules/mission/mission_msg.h deleted file mode 100644 index e6436491be..0000000000 --- a/sw/airborne/modules/mission/mission_msg.h +++ /dev/null @@ -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 - -#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 diff --git a/sw/airborne/modules/mission/mission_nav.c b/sw/airborne/modules/mission/mission_nav.c deleted file mode 100644 index 419a8fe1b5..0000000000 --- a/sw/airborne/modules/mission/mission_nav.c +++ /dev/null @@ -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 -#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); -} diff --git a/sw/airborne/modules/mission/mission_nav.h b/sw/airborne/modules/mission/mission_nav.h deleted file mode 100644 index 63509f8bc8..0000000000 --- a/sw/airborne/modules/mission/mission_nav.h +++ /dev/null @@ -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);