forked from Archive/PX4-Autopilot
Dataman: Start dataman and use it in waypoints and navigator instead of mission items in mission topic
This commit is contained in:
Julian Oes
parent
69888d28a5
commit
83b09614e7
|
|
@ -111,6 +111,11 @@ then
|
|||
#
|
||||
commander start
|
||||
|
||||
#
|
||||
# Start the datamanager
|
||||
#
|
||||
dataman start
|
||||
|
||||
#
|
||||
# Start the Navigator
|
||||
#
|
||||
|
|
|
|||
|
|
@ -52,10 +52,8 @@
|
|||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
|
||||
|
||||
#ifndef FM_PI
|
||||
#define FM_PI 3.1415926535897932384626433832795f
|
||||
#endif
|
||||
#include <geo/geo.h>
|
||||
#include <dataman/dataman.h>
|
||||
|
||||
bool debug = false;
|
||||
bool verbose = false;
|
||||
|
|
@ -63,13 +61,22 @@ bool verbose = false;
|
|||
orb_advert_t mission_pub = -1;
|
||||
struct mission_s mission;
|
||||
|
||||
#define NUM_MISSIONS_SUPPORTED 10
|
||||
|
||||
//#define MAVLINK_WPM_NO_PRINTF
|
||||
#define MAVLINK_WPM_VERBOSE 1
|
||||
|
||||
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
|
||||
void publish_mission()
|
||||
{
|
||||
/* Initialize mission publication if necessary */
|
||||
if (mission_pub < 0) {
|
||||
mission_pub = orb_advertise(ORB_ID(mission), &mission);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(mission), mission_pub, &mission);
|
||||
}
|
||||
}
|
||||
|
||||
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
|
||||
{
|
||||
/* only support global waypoints for now */
|
||||
|
|
@ -96,7 +103,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
|
|||
return MAV_MISSION_ERROR;
|
||||
}
|
||||
|
||||
mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F;
|
||||
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
|
||||
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
|
||||
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
|
||||
mission_item->nav_cmd = mavlink_mission_item->command;
|
||||
|
|
@ -104,6 +111,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
|
|||
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
|
||||
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
||||
mission_item->index = mavlink_mission_item->seq;
|
||||
mission_item->origin = ORIGIN_MAVLINK;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
|
|||
// state->pos_reached = false; ///< boolean for position reached
|
||||
// state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
|
||||
// state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
|
||||
|
||||
mission.count = 0;
|
||||
mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED);
|
||||
if (!mission.items) {
|
||||
mission.count = 0;
|
||||
/* XXX reject waypoints if this fails */
|
||||
warnx("no free RAM to allocate mission, rejecting any waypoints");
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -597,13 +597,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
|
||||
mission.current_index = wpc.seq;
|
||||
|
||||
/* Initialize mission publication if necessary */
|
||||
if (mission_pub < 0) {
|
||||
mission_pub = orb_advertise(ORB_ID(mission), &mission);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(mission), mission_pub, &mission);
|
||||
}
|
||||
publish_mission();
|
||||
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
|
||||
|
||||
|
|
@ -703,16 +697,24 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
wpm->current_wp_id = wpr.seq;
|
||||
|
||||
mavlink_mission_item_t wp;
|
||||
map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
|
||||
|
||||
if (mission.current_index == wpr.seq) {
|
||||
wp.current = true;
|
||||
struct mission_item_s mission_item;
|
||||
ssize_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) {
|
||||
|
||||
if (mission.current_index == wpr.seq) {
|
||||
wp.current = true;
|
||||
} else {
|
||||
wp.current = false;
|
||||
}
|
||||
|
||||
map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
|
||||
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
|
||||
} else {
|
||||
wp.current = false;
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
|
||||
}
|
||||
|
||||
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
|
||||
|
||||
} else {
|
||||
// if (verbose)
|
||||
{
|
||||
|
|
@ -815,7 +817,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
|
||||
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
|
||||
|
|
@ -837,10 +839,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
|
||||
warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
|
||||
} else {
|
||||
/* prepare mission topic */
|
||||
mission.count = wpc.count;
|
||||
/* reset current index */
|
||||
mission.current_index = -1;
|
||||
/* set count to 0 while copying */
|
||||
mission.count = 0;
|
||||
publish_mission();
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
|
|
@ -962,20 +963,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
|
||||
// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
|
||||
|
||||
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]);
|
||||
struct mission_item_s mission_item;
|
||||
|
||||
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
|
||||
|
||||
if (ret != OK) {
|
||||
|
||||
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
|
||||
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
size_t len = sizeof(struct mission_item_s);
|
||||
|
||||
if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
|
||||
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
if (wp.current) {
|
||||
mission.current_index = wp.seq;
|
||||
warnx("current is: %d", wp.seq);
|
||||
} else {
|
||||
warnx("not current");
|
||||
}
|
||||
|
||||
wpm->current_wp_id = wp.seq + 1;
|
||||
|
|
@ -1009,14 +1016,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
// }
|
||||
// TODO: update count?
|
||||
|
||||
/* Initialize mission publication if necessary */
|
||||
if (mission_pub < 0) {
|
||||
mission_pub = orb_advertise(ORB_ID(mission), &mission);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(mission), mission_pub, &mission);
|
||||
}
|
||||
|
||||
mission.count = wpm->current_count;
|
||||
|
||||
publish_mission();
|
||||
|
||||
wpm->size = wpm->current_count;
|
||||
|
||||
|
|
@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
|||
|
||||
/* prepare mission topic */
|
||||
mission.count = 0;
|
||||
memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED);
|
||||
|
||||
/* Initialize mission publication if necessary */
|
||||
if (mission_pub < 0) {
|
||||
mission_pub = orb_advertise(ORB_ID(mission), &mission);
|
||||
|
||||
if (dm_clear(DM_KEY_WAYPOINTS) == OK) {
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
|
||||
} else {
|
||||
orb_publish(ORB_ID(mission), mission_pub, &mission);
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
|
||||
}
|
||||
|
||||
publish_mission();
|
||||
|
||||
warnx("Mission cleared");
|
||||
|
||||
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
|
||||
|
||||
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
|
||||
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
|
||||
|
|
|
|||
|
|
@ -63,6 +63,7 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <geo/geo.h>
|
||||
|
|
@ -141,7 +142,6 @@ private:
|
|||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _params_sub; /**< notification of parameter updates */
|
||||
int _mission_sub; /**< notification of mission updates */
|
||||
int _onboard_mission_sub; /**< notification of onboard mission updates */
|
||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||
|
||||
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
|
||||
|
|
@ -157,12 +157,8 @@ private:
|
|||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
unsigned _max_mission_item_count; /**< maximum number of mission items supported */
|
||||
unsigned _max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */
|
||||
|
||||
unsigned _mission_item_count; /** number of mission items copied */
|
||||
unsigned _onboard_mission_item_count; /** number of onboard mission items copied */
|
||||
struct mission_item_s *_mission_item; /**< storage for mission */
|
||||
struct mission_item_s *_onboard_mission_item; /**< storage for onboard mission */
|
||||
|
||||
struct fence_s _fence; /**< storage for fence vertices */
|
||||
bool _fence_valid; /**< flag if fence is valid */
|
||||
|
|
@ -174,11 +170,9 @@ private:
|
|||
bool _waypoint_yaw_reached;
|
||||
uint64_t _time_first_inside_orbit;
|
||||
bool _mission_item_reached;
|
||||
bool _onboard_mission_item_reached;
|
||||
|
||||
navigation_mode_t _mode;
|
||||
unsigned _current_mission_index;
|
||||
unsigned _current_onboard_mission_index;
|
||||
|
||||
struct {
|
||||
float min_altitude;
|
||||
|
|
@ -204,11 +198,6 @@ private:
|
|||
*/
|
||||
void mission_update();
|
||||
|
||||
/**
|
||||
* Retrieve onboard mission.
|
||||
*/
|
||||
void onboard_mission_update();
|
||||
|
||||
/**
|
||||
* Shim for calling task_main from task_create.
|
||||
*/
|
||||
|
|
@ -281,7 +270,6 @@ Navigator::Navigator() :
|
|||
_vstatus_sub(-1),
|
||||
_params_sub(-1),
|
||||
_mission_sub(-1),
|
||||
_onboard_mission_sub(-1),
|
||||
_capabilities_sub(-1),
|
||||
|
||||
/* publications */
|
||||
|
|
@ -293,19 +281,15 @@ Navigator::Navigator() :
|
|||
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
|
||||
/* states */
|
||||
_max_mission_item_count(10),
|
||||
_max_onboard_mission_item_count(10),
|
||||
_mission_item_count(0),
|
||||
_onboard_mission_item_count(0),
|
||||
_fence_valid(false),
|
||||
_inside_fence(true),
|
||||
_waypoint_position_reached(false),
|
||||
_waypoint_yaw_reached(false),
|
||||
_time_first_inside_orbit(0),
|
||||
_mission_item_reached(false),
|
||||
_onboard_mission_item_reached(false),
|
||||
_mode(NAVIGATION_MODE_NONE),
|
||||
_current_mission_index(0),
|
||||
_current_onboard_mission_index(0)
|
||||
_current_mission_index(0)
|
||||
{
|
||||
_global_pos.valid = false;
|
||||
memset(&_fence, 0, sizeof(_fence));
|
||||
|
|
@ -313,9 +297,6 @@ Navigator::Navigator() :
|
|||
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
|
||||
_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
|
||||
|
||||
_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count);
|
||||
_onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count);
|
||||
|
||||
_mission_item_triplet.previous_valid = false;
|
||||
_mission_item_triplet.current_valid = false;
|
||||
_mission_item_triplet.next_valid = false;
|
||||
|
|
@ -370,131 +351,50 @@ Navigator::mission_update()
|
|||
{
|
||||
struct mission_s mission;
|
||||
if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) {
|
||||
// XXX this is not optimal yet, but a first prototype /
|
||||
// test implementation
|
||||
|
||||
if (mission.count <= _max_mission_item_count) {
|
||||
|
||||
/* Check if first part of mission (up to _current_mission_index - 1) changed:
|
||||
* if the first part changed: start again at first waypoint
|
||||
* if the first part remained unchanged: continue with the (possibly changed second part)
|
||||
*/
|
||||
if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission
|
||||
for (unsigned i = 0; i < _current_mission_index; i++) {
|
||||
if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
|
||||
/* set flag to restart mission next we're in auto */
|
||||
_current_mission_index = 0;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
//warnx("First part of mission differs i=%d", i);
|
||||
break;
|
||||
}
|
||||
// else {
|
||||
// warnx("Mission item is equivalent i=%d", i);
|
||||
// }
|
||||
}
|
||||
} else if (mission.current_index >= 0 && mission.current_index < mission.count) {
|
||||
/* set flag to restart mission next we're in auto */
|
||||
_current_mission_index = mission.current_index;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
} else {
|
||||
_current_mission_index = 0;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
}
|
||||
// /* Check if first part of mission (up to _current_mission_index - 1) changed:
|
||||
// * if the first part changed: start again at first waypoint
|
||||
// * if the first part remained unchanged: continue with the (possibly changed second part)
|
||||
// */
|
||||
// if (mission.current_index == -1 && _current_mission_index < _mission_item_count && _current_mission_index < mission.count) { //check if not finished and if the new mission is not a shorter mission
|
||||
// for (unsigned i = 0; i < _current_mission_index; i++) {
|
||||
// if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
|
||||
// /* set flag to restart mission next we're in auto */
|
||||
// _current_mission_index = 0;
|
||||
// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
// //warnx("First part of mission differs i=%d", i);
|
||||
// break;
|
||||
// }
|
||||
// // else {
|
||||
// // warnx("Mission item is equivalent i=%d", i);
|
||||
// // }
|
||||
// }
|
||||
// } else if (mission.current_index >= 0 && mission.current_index < mission.count) {
|
||||
// /* set flag to restart mission next we're in auto */
|
||||
// _current_mission_index = mission.current_index;
|
||||
// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
// } else {
|
||||
// _current_mission_index = 0;
|
||||
// mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
|
||||
// }
|
||||
|
||||
/*
|
||||
* Perform an atomic copy & state update
|
||||
*/
|
||||
irqstate_t flags = irqsave();
|
||||
_mission_item_count = mission.count;
|
||||
_current_mission_index = mission.current_index;
|
||||
|
||||
memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s));
|
||||
_mission_item_count = mission.count;
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
|
||||
|
||||
} else {
|
||||
warnx("ERROR: too many waypoints, not supported");
|
||||
mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
|
||||
_mission_item_count = 0;
|
||||
}
|
||||
|
||||
if (_mode == NAVIGATION_MODE_WAYPOINT) {
|
||||
start_waypoint();
|
||||
}
|
||||
|
||||
/* TODO add checks if and how the mission has changed */
|
||||
} else {
|
||||
_mission_item_count = 0;
|
||||
_current_mission_index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::onboard_mission_update()
|
||||
{
|
||||
struct mission_s onboard_mission;
|
||||
if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
|
||||
// XXX this is not optimal yet, but a first prototype /
|
||||
// test implementation
|
||||
|
||||
if (onboard_mission.count <= _max_onboard_mission_item_count) {
|
||||
|
||||
/* Check if first part of mission (up to _current_onboard_mission_index - 1) changed:
|
||||
* if the first part changed: start again at first waypoint
|
||||
* if the first part remained unchanged: continue with the (possibly changed second part)
|
||||
*/
|
||||
if (onboard_mission.current_index == -1 && _current_onboard_mission_index < _onboard_mission_item_count && _current_onboard_mission_index < onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission
|
||||
for (unsigned i = 0; i < _current_onboard_mission_index; i++) {
|
||||
if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) {
|
||||
/* set flag to restart mission next we're in auto */
|
||||
_current_onboard_mission_index = 0;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
|
||||
//warnx("First part of onboard mission differs i=%d", i);
|
||||
break;
|
||||
}
|
||||
// else {
|
||||
// warnx("Onboard mission item is equivalent i=%d", i);
|
||||
// }
|
||||
}
|
||||
} else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) {
|
||||
/* set flag to restart mission next we're in auto */
|
||||
_current_onboard_mission_index = onboard_mission.current_index;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
|
||||
} else {
|
||||
_current_onboard_mission_index = 0;
|
||||
mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
|
||||
}
|
||||
|
||||
/*
|
||||
* Perform an atomic copy & state update
|
||||
*/
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s));
|
||||
_onboard_mission_item_count = onboard_mission.count;
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
|
||||
|
||||
} else {
|
||||
warnx("ERROR: too many waypoints, not supported");
|
||||
mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
|
||||
_onboard_mission_item_count = 0;
|
||||
}
|
||||
|
||||
if (_mode == NAVIGATION_MODE_WAYPOINT) {
|
||||
start_waypoint();
|
||||
}
|
||||
|
||||
/* TODO add checks if and how the mission has changed */
|
||||
} else {
|
||||
_onboard_mission_item_count = 0;
|
||||
_current_onboard_mission_index = 0;
|
||||
if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) {
|
||||
set_mode(NAVIGATION_MODE_LOITER);
|
||||
}
|
||||
else if (_mode == NAVIGATION_MODE_WAYPOINT) {
|
||||
start_waypoint();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void
|
||||
Navigator::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
|
|
@ -514,7 +414,6 @@ Navigator::task_main()
|
|||
*/
|
||||
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_mission_sub = orb_subscribe(ORB_ID(mission));
|
||||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
|
||||
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
|
@ -526,7 +425,6 @@ Navigator::task_main()
|
|||
}
|
||||
|
||||
mission_update();
|
||||
onboard_mission_update();
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vstatus_sub, 200);
|
||||
|
|
@ -541,7 +439,7 @@ Navigator::task_main()
|
|||
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
|
||||
|
||||
/* wakeup source(s) */
|
||||
struct pollfd fds[7];
|
||||
struct pollfd fds[6];
|
||||
|
||||
/* Setup of loop */
|
||||
fds[0].fd = _params_sub;
|
||||
|
|
@ -554,10 +452,8 @@ Navigator::task_main()
|
|||
fds[3].events = POLLIN;
|
||||
fds[4].fd = _mission_sub;
|
||||
fds[4].events = POLLIN;
|
||||
fds[5].fd = _onboard_mission_sub;
|
||||
fds[5].fd = _vstatus_sub;
|
||||
fds[5].events = POLLIN;
|
||||
fds[6].fd = _vstatus_sub;
|
||||
fds[6].events = POLLIN;
|
||||
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
|
@ -579,7 +475,7 @@ Navigator::task_main()
|
|||
perf_begin(_loop_perf);
|
||||
|
||||
/* only update vehicle status if it changed */
|
||||
if (fds[6].revents & POLLIN) {
|
||||
if (fds[5].revents & POLLIN) {
|
||||
/* read from param to clear updated flag */
|
||||
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
|
||||
|
||||
|
|
@ -660,10 +556,6 @@ Navigator::task_main()
|
|||
mission_update();
|
||||
}
|
||||
|
||||
if (fds[5].revents & POLLIN) {
|
||||
onboard_mission_update();
|
||||
}
|
||||
|
||||
if (fds[2].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
|
||||
}
|
||||
|
|
@ -974,22 +866,28 @@ Navigator::set_mode(navigation_mode_t new_nav_mode)
|
|||
int
|
||||
Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item)
|
||||
{
|
||||
if (mission_item_index < _mission_item_count) {
|
||||
memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s));
|
||||
|
||||
if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||
/* if it is a RTL waypoint, append the home position */
|
||||
new_mission_item->lat = (double)_home_pos.lat / 1e7;
|
||||
new_mission_item->lon = (double)_home_pos.lon / 1e7;
|
||||
new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
|
||||
new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
|
||||
new_mission_item->radius = 50.0f; // TODO: get rid of magic number
|
||||
}
|
||||
// warnx("added mission item: %d", mission_item_index);
|
||||
return OK;
|
||||
if (mission_item_index >= _mission_item_count) {
|
||||
return ERROR;
|
||||
}
|
||||
// warnx("could not add mission item: %d", mission_item_index);
|
||||
return ERROR;
|
||||
|
||||
struct mission_item_s mission_item;
|
||||
|
||||
if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s));
|
||||
|
||||
if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
|
||||
/* if it is a RTL waypoint, append the home position */
|
||||
new_mission_item->lat = (double)_home_pos.lat / 1e7;
|
||||
new_mission_item->lon = (double)_home_pos.lon / 1e7;
|
||||
new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
|
||||
new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
|
||||
new_mission_item->radius = 50.0f; // TODO: get rid of magic number
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1036,8 +934,6 @@ Navigator::advance_current_mission_item()
|
|||
return ERROR;
|
||||
}
|
||||
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* copy current mission to previous item */
|
||||
memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
|
||||
_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
|
||||
|
|
@ -1182,14 +1078,11 @@ Navigator::start_waypoint()
|
|||
{
|
||||
reset_mission_item_reached();
|
||||
|
||||
/* this means we should start fresh */
|
||||
if (_current_mission_index == 0) {
|
||||
|
||||
_mission_item_triplet.previous_valid = false;
|
||||
|
||||
} else {
|
||||
if (_current_mission_index > 0) {
|
||||
set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous);
|
||||
_mission_item_triplet.previous_valid = true;
|
||||
} else {
|
||||
_mission_item_triplet.previous_valid = false;
|
||||
}
|
||||
|
||||
set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current);
|
||||
|
|
|
|||
|
|
@ -46,6 +46,8 @@
|
|||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
#define NUM_MISSIONS_SUPPORTED 256
|
||||
|
||||
/* compatible to mavlink MAV_CMD */
|
||||
enum NAV_CMD {
|
||||
NAV_CMD_WAYPOINT=16,
|
||||
|
|
@ -59,6 +61,11 @@ enum NAV_CMD {
|
|||
NAV_CMD_PATHPLANNING=81
|
||||
};
|
||||
|
||||
enum ORIGIN {
|
||||
ORIGIN_MAVLINK = 0,
|
||||
ORIGIN_ONBOARD
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
|
|
@ -84,11 +91,11 @@ struct mission_item_s
|
|||
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
|
||||
bool autocontinue; /**< true if next waypoint should follow after this one */
|
||||
int index; /**< index matching the mavlink waypoint */
|
||||
enum ORIGIN origin; /**< where the waypoint has been generated */
|
||||
};
|
||||
|
||||
struct mission_s
|
||||
{
|
||||
struct mission_item_s *items;
|
||||
unsigned count;
|
||||
int current_index; /**< default -1, start at the one changed latest */
|
||||
};
|
||||
|
|
@ -99,6 +106,5 @@ struct mission_s
|
|||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(mission);
|
||||
ORB_DECLARE(onboard_mission);
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue