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

[mission] improvements to mission module (#3107)

Browse Source 
- register mission status message for telemetry, so it can be used on
  extra link with companion computer
- add altitude proximity option to check alt on waypoints, not only
  2D position
- add documentation to XML file
This commit is contained in:
Gautier Hattenberger
2023-09-26 17:54:49 +02:00
committed by GitHub
parent acc6833af7
commit dd7b07ab60
6 changed files with 67 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/mission_common.xml
+7
View File
@@ -7,6 +7,13 @@
This module parse datalink commands for basic navigation routines
and store them in a queue.
</description>
<section name="MISSION" prefix="MISSION_">
<define name="ELEMENT_NB" value="20" description="maximum number of elements in the mission queue"/>
<define name="REGISTER_NB" value="6" description="maximum number of custom elemets to register"/>
<define name="CHECK_UNIQUE_ID" value="TRUE|FALSE" description="check before inserting that all elements ID are unique in the queue (default: TRUE)"/>
<define name="ALT_PROXIMITY" value="distance" unit="m" description="if defined, enforce a vertical proximity to validate the target points (default: undefined)"/>
<define name="WAIT_TIMEOUT" value="time" unit="s" description="time in waiting pattern when queue is empty before ending the mission (default: fixedwing=120, rotorcraft=30)"/>
</section>
</doc>
<header>
<file name="mission_common.h"/>
conf/modules/mission_fw.xml
+1
View File
@@ -17,6 +17,7 @@
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
<define name="MISSION_ALT_PROXIMITY" value="5."/>
</test>
</makefile>
</module>
conf/modules/mission_rotorcraft.xml
+1
View File
@@ -16,6 +16,7 @@
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
<define name="MISSION_ALT_PROXIMITY" value="2."/>
</test>
</makefile>
</module>
sw/airborne/modules/mission/mission_common.c
+28 -16
View File
@@ -31,6 +31,9 @@
#include "generated/airframe.h"
#include "modules/datalink/datalink.h"
#include "modules/datalink/downlink.h"
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
#endif
// Check for unique ID by default
#ifndef MISSION_CHECK_UNIQUE_ID
@@ -39,6 +42,26 @@
struct _mission mission = { 0 };
static void send_mission_status(struct transport_tx *trans, struct link_device *device)
{
// build index list
uint8_t index_list[MISSION_ELEMENT_NB];
uint8_t i = mission.current_idx, j = 0;
while (i != mission.insert_idx) {
index_list[j++] = mission.elements[i].index;
i = (i + 1) % MISSION_ELEMENT_NB;
}
if (j == 0) { index_list[j++] = 0; } // Dummy value if index list is empty
//compute remaining time (or -1. if no time limit)
float remaining_time = -1.;
if (mission.elements[mission.current_idx].duration > 0.) {
remaining_time = mission.elements[mission.current_idx].duration - mission.element_time;
}
// send status
pprz_msg_send_MISSION_STATUS(trans, device, AC_ID, &remaining_time, j, index_list);
}
void mission_init(void)
{
mission.insert_idx = 0;
@@ -49,6 +72,10 @@ void mission_init(void)
mission.registered[i].cb = NULL;
memset(mission.registered[i].type, '\0', MISSION_TYPE_SIZE);
}
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_MISSION_STATUS, send_mission_status);
#endif
}
@@ -160,22 +187,7 @@ struct _mission_element *mission_get(void)
// Report function
void mission_status_report(void)
{
// build index list
uint8_t index_list[MISSION_ELEMENT_NB];
uint8_t i = mission.current_idx, j = 0;
while (i != mission.insert_idx) {
index_list[j++] = mission.elements[i].index;
i = (i + 1) % MISSION_ELEMENT_NB;
}
if (j == 0) { index_list[j++] = 0; } // Dummy value if index list is empty
//compute remaining time (or -1. if no time limit)
float remaining_time = -1.;
if (mission.elements[mission.current_idx].duration > 0.) {
remaining_time = mission.elements[mission.current_idx].duration - mission.element_time;
}
// send status
DOWNLINK_SEND_MISSION_STATUS(DefaultChannel, DefaultDevice, &remaining_time, j, index_list);
send_mission_status(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
}
sw/airborne/modules/mission/mission_fw_nav.c
+15 -3
View File
@@ -73,7 +73,11 @@ struct EnuCoor_f last_wp_f = { 0.f, 0.f, 0.f };
*/
static inline bool mission_nav_wp(struct _mission_wp *wp)
{
if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp_f.x, last_wp_f.y, CARROT)) {
if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp_f.x, last_wp_f.y, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - wp->wp.z) <= MISSION_ALT_PROXIMITY
#endif
) {
last_wp_f = wp->wp; // store last wp
return false; // end of mission element
}
@@ -98,7 +102,11 @@ static inline bool mission_nav_circle(struct _mission_circle *circle)
*/
static inline bool mission_nav_segment(struct _mission_segment *segment)
{
if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)) {
if (nav_approaching_xy(segment->to.x, segment->to.y, segment->from.x, segment->from.y, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - segment->to.z) <= MISSION_ALT_PROXIMITY
#endif
) {
last_wp_f = segment->to;
return false; // end of mission element
}
@@ -131,7 +139,11 @@ static inline bool mission_nav_path(struct _mission_path *path)
nav_route_xy(from.x, from.y, to.x, to.y);
NavVerticalAutoThrottleMode(0.f);
NavVerticalAltitudeMode(to.z, 0.f); // both altitude should be the same anyway
if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)) {
if (nav_approaching_xy(to.x, to.y, from.x, from.y, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - to.z) <= MISSION_ALT_PROXIMITY
#endif
) {
path->path_idx++; // go to next segment
}
return true;
sw/airborne/modules/mission/mission_rotorcraft_nav.c
+15 -3
View File
@@ -88,7 +88,11 @@ static inline bool mission_nav_wp(struct _mission_element *el)
struct EnuCoor_f *target_wp = &(el->element.mission_wp.wp);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav.nav_approaching(target_wp, NULL, CARROT)) {
if (nav.nav_approaching(target_wp, NULL, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - target_wp->z) <= MISSION_ALT_PROXIMITY
#endif
) {
last_mission_wp = *target_wp;
if (el->duration > 0.f) {
@@ -131,7 +135,11 @@ static inline bool mission_nav_segment(struct _mission_element *el)
struct EnuCoor_f *to_wp = &(el->element.mission_segment.to);
//Check proximity and wait for 'duration' seconds in proximity circle if desired
if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
if (nav.nav_approaching(to_wp, from_wp, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - to_wp->z) <= MISSION_ALT_PROXIMITY
#endif
) {
last_mission_wp = *to_wp;
if (el->duration > 0.f) {
@@ -167,7 +175,11 @@ static inline bool mission_nav_path(struct _mission_element *el)
struct EnuCoor_f *to_wp = &(el->element.mission_path.path[el->element.mission_path.path_idx]);
//Check proximity and wait for t seconds in proximity circle if desired
if (nav.nav_approaching(to_wp, from_wp, CARROT)) {
if (nav.nav_approaching(to_wp, from_wp, CARROT)
#ifdef MISSION_ALT_PROXIMITY
&& fabsf(stateGetPositionEnu_f()->z - from_wp->z) <= MISSION_ALT_PROXIMITY
#endif
) {
last_mission_wp = *to_wp;
if (el->duration > 0.f) {