fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 11:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink and navigator: handle current waypoint onboard

Browse Source
This commit is contained in:
Julian Oes
2014-02-15 11:29:36 +01:00
parent d15fc32097
commit 30dacbd154
6 changed files with 142 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+55 -47
View File
@@ -59,11 +59,11 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
@@ -958,7 +958,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
}
/*
@@ -981,11 +981,11 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
if (verbose) warnx("ERROR: index out of bounds");
if (_verbose) warnx("ERROR: index out of bounds");
}
}
@@ -1001,7 +1001,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
@@ -1031,10 +1031,10 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
if (verbose) warnx("ERROR: could not read WP%u", seq);
if (_verbose) warnx("ERROR: could not read WP%u", seq);
}
}
@@ -1049,11 +1049,11 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
if (verbose) warnx("ERROR: Waypoint index exceeds list capacity");
if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity");
}
}
@@ -1074,10 +1074,9 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
{
/* check for timed-out operations */
@@ -1085,7 +1084,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
mavlink_missionlib_send_gcs_string("Operation timeout");
if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_partner_sysid = 0;
@@ -1117,7 +1116,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
if (verbose) warnx("REJ. WP CMD: curr partner id mismatch");
if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch");
}
break;
@@ -1140,18 +1139,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
if (verbose) warnx("IGN WP CURR CMD: Not in list");
if (_verbose) warnx("IGN WP CURR CMD: Not in list");
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
if (verbose) warnx("IGN WP CURR CMD: Busy");
if (_verbose) warnx("IGN WP CURR CMD: Busy");
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (verbose) warnx("REJ. WP CMD: target id mismatch");
if (_verbose) warnx("REJ. WP CMD: target id mismatch");
}
break;
@@ -1173,7 +1172,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
wpm->current_partner_compid = msg->compid;
} else {
if (verbose) warnx("No waypoints send");
if (_verbose) warnx("No waypoints send");
}
wpm->current_count = wpm->size;
@@ -1181,11 +1180,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
if (verbose) warnx("IGN REQUEST LIST: Busy");
if (_verbose) warnx("IGN REQUEST LIST: Busy");
}
} else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
if (verbose) warnx("REJ. REQUEST LIST: target id mismatch");
if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch");
}
break;
@@ -1201,7 +1200,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq >= wpm->size) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
break;
}
@@ -1212,11 +1211,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
if (verbose) warnx("REJ. WP CMD: First id != 0");
if (_verbose) warnx("REJ. WP CMD: First id != 0");
break;
}
@@ -1224,22 +1223,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == wpm->current_wp_id) {
if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
} else if (wpr.seq == wpm->current_wp_id + 1) {
if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
break;
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
break;
}
@@ -1252,7 +1251,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
}
@@ -1261,12 +1260,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
}
}
break;
@@ -1282,18 +1281,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("COUNT 0");
if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
break;
}
if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
wpm->current_wp_id = 0;
@@ -1307,19 +1306,19 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (verbose) warnx("IGN MISSION_COUNT CMD: Busy");
if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy");
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
}
}
break;
@@ -1389,15 +1388,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
break;
}
if (wp.current) {
mission.current_index = wp.seq;
}
// if (wp.current) {
// warnx("current is: %d", wp.seq);
// mission.current_index = wp.seq;
// }
// XXX ignore current set
mission.current_index = -1;
wpm->current_wp_id = wp.seq + 1;
if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
@@ -1416,7 +1418,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
}
break;
@@ -1448,14 +1450,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
if (verbose) warnx("IGN WP CLEAR CMD: Busy");
if (_verbose) warnx("IGN WP CLEAR CMD: Busy");
}
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
}
break;
@@ -1550,6 +1552,9 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MODE_ONBOARD;
break;
case 'v':
_verbose = true;
default:
usage();
break;
@@ -1767,9 +1772,12 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
}
if (_verbose) warnx("Got mission result");
if (mission_result.mission_reached)
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
mavlink_waypoint_eventloop(hrt_absolute_time());
@@ -1842,7 +1850,7 @@ Mavlink::status()
static void usage()
{
errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]");
errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]");
}
int mavlink_main(int argc, char *argv[])
src/modules/mavlink/mavlink_main.h
+1 -1
View File
@@ -293,7 +293,7 @@ private:
mavlink_wpm_storage wpm_s;
mavlink_wpm_storage *wpm;
bool verbose;
bool _verbose;
int _uart;
int _baudrate;
bool gcs_link;
src/modules/navigator/navigator_main.cpp
+16 -9
View File
@@ -65,7 +65,6 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
@@ -288,9 +287,9 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main task.
*/
void task_main() __attribute__((noreturn));
void task_main();
void publish_safepoints(unsigned points);
@@ -384,7 +383,6 @@ Navigator::Navigator() :
/* publications */
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -416,7 +414,6 @@ Navigator::Navigator() :
_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
memset(&_mission_result, 0, sizeof(struct mission_result_s));
memset(&_mission_item, 0, sizeof(struct mission_item_s));
memset(&nav_states_str, 0, sizeof(nav_states_str));
@@ -518,13 +515,16 @@ Navigator::offboard_mission_update(bool isrotaryWing)
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
_mission.set_offboard_mission_count(offboard_mission.count);
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
} else {
_mission.set_current_offboard_mission_index(0);
_mission.set_offboard_mission_count(0);
_mission.set_current_offboard_mission_index(0);
}
_mission.publish_mission_result();
}
void
@@ -534,12 +534,12 @@ Navigator::onboard_mission_update()
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
} else {
_mission.set_current_onboard_mission_index(0);
_mission.set_onboard_mission_count(0);
_mission.set_current_onboard_mission_index(0);
}
}
@@ -1116,6 +1116,9 @@ Navigator::set_mission_item()
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
_mission.report_current_mission_item();
/* reset time counter for new item */
_time_first_inside_orbit = 0;
@@ -1537,6 +1540,9 @@ void
Navigator::on_mission_item_reached()
{
if (myState == NAV_STATE_MISSION) {
_mission.report_mission_item_reached();
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
@@ -1589,6 +1595,7 @@ Navigator::on_mission_item_reached()
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
_mission.publish_mission_result();
}
void
src/modules/navigator/navigator_mission.cpp
+59 -8
View File
@@ -36,13 +36,12 @@
* Helper class to access missions
*/
// #include <stdio.h>
// #include <stdlib.h>
// #include <string.h>
// #include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
@@ -60,8 +59,11 @@ Mission::Mission() :
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE)
{}
_current_mission_type(MISSION_TYPE_NONE),
_mission_result_pub(-1)
{
memset(&_mission_result, 0, sizeof(struct mission_result_s));
}
Mission::~Mission()
{
@@ -78,8 +80,16 @@ void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
warnx("specifically set to %d", new_index);
_current_offboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
_current_offboard_mission_index = 0;
}
}
report_current_mission_item();
}
void
@@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
_current_onboard_mission_index = 0;
}
}
// TODO: implement this for onboard missions as well
// report_current_mission_item();
}
void
@@ -266,4 +284,37 @@ Mission::move_to_next()
default:
break;
}
}
}
void
Mission::report_mission_item_reached()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
}
void
Mission::report_current_mission_item()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.index_current_mission = _current_offboard_mission_index;
}
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
}
src/modules/navigator/navigator_mission.h
+9 -2
View File
@@ -40,6 +40,7 @@
#define NAVIGATOR_MISSION_H
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
class __EXPORT Mission
@@ -71,7 +72,9 @@ public:
void move_to_next();
void add_home_pos(struct mission_item_s *new_mission_item);
void report_mission_item_reached();
void report_current_mission_item();
void publish_mission_result();
private:
bool current_onboard_mission_available();
@@ -92,6 +95,10 @@ private:
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
int _mission_result_pub;
struct mission_result_s _mission_result;
};
#endif
#endif
src/modules/uORB/topics/mission_result.h
+2 -1
View File
@@ -54,7 +54,8 @@
struct mission_result_s
{
bool mission_reached; /**< true if mission has been reached */
unsigned mission_index; /**< index of the mission which has been reached */
unsigned mission_index_reached; /**< index of the mission which has been reached */
unsigned index_current_mission; /**< index of the current mission */
};
/**