forked from Archive/PX4-Autopilot
Navigator, waypoints: save index in mission item and use this in navigator
This commit is contained in:
parent
42aefe838c
commit
7892a72f90
|
@ -60,6 +60,10 @@
|
||||||
bool debug = false;
|
bool debug = false;
|
||||||
bool verbose = false;
|
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_NO_PRINTF
|
||||||
#define MAVLINK_WPM_VERBOSE 1
|
#define MAVLINK_WPM_VERBOSE 1
|
||||||
|
@ -78,9 +82,10 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl
|
||||||
mission_item->radius = mavlink_mission_item->param1;
|
mission_item->radius = mavlink_mission_item->param1;
|
||||||
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
|
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
|
||||||
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
||||||
|
mission_item->index = mavlink_mission_item->seq;
|
||||||
}
|
}
|
||||||
|
|
||||||
void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, const uint16_t seq, mavlink_mission_item_t *mavlink_mission_item)
|
void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
|
||||||
{
|
{
|
||||||
|
|
||||||
mavlink_mission_item->x = (float)mission_item->lat;
|
mavlink_mission_item->x = (float)mission_item->lat;
|
||||||
|
@ -92,8 +97,7 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi
|
||||||
mavlink_mission_item->param1 = mission_item->radius;
|
mavlink_mission_item->param1 = mission_item->radius;
|
||||||
mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
|
mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
|
||||||
mavlink_mission_item->autocontinue = mission_item->autocontinue;
|
mavlink_mission_item->autocontinue = mission_item->autocontinue;
|
||||||
|
mavlink_mission_item->seq = mission_item->index;
|
||||||
mavlink_mission_item->seq = seq;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void mavlink_wpm_init(mavlink_wpm_storage *state)
|
void mavlink_wpm_init(mavlink_wpm_storage *state)
|
||||||
|
@ -117,6 +121,13 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
|
||||||
state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
|
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
|
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");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
@ -558,9 +569,6 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
|
||||||
|
|
||||||
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
|
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos)
|
||||||
{
|
{
|
||||||
static orb_advert_t mission_pub = -1;
|
|
||||||
static struct mission_s mission;
|
|
||||||
|
|
||||||
uint64_t now = mavlink_missionlib_get_system_timestamp();
|
uint64_t now = mavlink_missionlib_get_system_timestamp();
|
||||||
|
|
||||||
switch (msg->msgid) {
|
switch (msg->msgid) {
|
||||||
|
@ -711,7 +719,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
||||||
wpm->current_wp_id = wpr.seq;
|
wpm->current_wp_id = wpr.seq;
|
||||||
|
|
||||||
mavlink_mission_item_t wp;
|
mavlink_mission_item_t wp;
|
||||||
map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], wpr.seq, &wp);
|
map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
|
||||||
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
|
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -835,14 +843,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
||||||
wpm->current_partner_compid = msg->compid;
|
wpm->current_partner_compid = msg->compid;
|
||||||
wpm->current_count = wpc.count;
|
wpm->current_count = wpc.count;
|
||||||
|
|
||||||
/* prepare mission topic */
|
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
|
||||||
mission.count = wpc.count;
|
warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
|
||||||
mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count);
|
} else {
|
||||||
|
/* prepare mission topic */
|
||||||
if (!mission.items) {
|
mission.count = wpc.count;
|
||||||
mission.count = 0;
|
|
||||||
/* XXX reject waypoints if this fails */
|
|
||||||
warnx("no free RAM to allocate mission, rejecting any waypoints");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||||
|
@ -1087,8 +1092,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
|
||||||
wpm->yaw_reached = false;
|
wpm->yaw_reached = false;
|
||||||
wpm->pos_reached = false;
|
wpm->pos_reached = false;
|
||||||
|
|
||||||
|
/* 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);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
orb_publish(ORB_ID(mission), mission_pub, &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) {
|
} 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);
|
// 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);
|
||||||
|
warnx("not cleared");
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -69,11 +69,6 @@
|
||||||
#include <mathlib/mathlib.h>
|
#include <mathlib/mathlib.h>
|
||||||
#include <dataman/dataman.h>
|
#include <dataman/dataman.h>
|
||||||
|
|
||||||
/* oddly, ERROR is not defined for c++ */
|
|
||||||
#ifdef ERROR
|
|
||||||
# undef ERROR
|
|
||||||
#endif
|
|
||||||
static const int ERROR = -1;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* navigator app start / stop handling function
|
* navigator app start / stop handling function
|
||||||
|
@ -142,8 +137,6 @@ private:
|
||||||
unsigned _mission_item_count; /** number of mission items copied */
|
unsigned _mission_item_count; /** number of mission items copied */
|
||||||
struct mission_item_s *_mission_item; /**< storage for mission */
|
struct mission_item_s *_mission_item; /**< storage for mission */
|
||||||
|
|
||||||
int _current_mission_item_index; /** current active mission item , -1 for none */
|
|
||||||
|
|
||||||
struct fence_s _fence; /**< storage for fence vertices */
|
struct fence_s _fence; /**< storage for fence vertices */
|
||||||
bool _fence_valid; /**< flag if fence is valid */
|
bool _fence_valid; /**< flag if fence is valid */
|
||||||
bool _inside_fence; /**< vehicle is inside fence */
|
bool _inside_fence; /**< vehicle is inside fence */
|
||||||
|
@ -213,9 +206,11 @@ private:
|
||||||
|
|
||||||
bool fence_valid(const struct fence_s &fence);
|
bool fence_valid(const struct fence_s &fence);
|
||||||
|
|
||||||
int add_mission_item(unsigned mission_item, struct mission_item_s *new_mission_item);
|
void add_mission_item(unsigned mission_item_index,
|
||||||
|
const struct mission_item_s *existing_mission_item,
|
||||||
|
struct mission_item_s *new_mission_item);
|
||||||
|
|
||||||
void add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item);
|
void update_mission_item_triplet();
|
||||||
|
|
||||||
void advance_current_mission_item();
|
void advance_current_mission_item();
|
||||||
|
|
||||||
|
@ -260,7 +255,6 @@ Navigator::Navigator() :
|
||||||
_max_mission_item_count(10),
|
_max_mission_item_count(10),
|
||||||
_fence_valid(false),
|
_fence_valid(false),
|
||||||
_inside_fence(true),
|
_inside_fence(true),
|
||||||
_current_mission_item_index(-1),
|
|
||||||
_waypoint_position_reached(false),
|
_waypoint_position_reached(false),
|
||||||
_waypoint_yaw_reached(false),
|
_waypoint_yaw_reached(false),
|
||||||
_time_first_inside_orbit(0),
|
_time_first_inside_orbit(0),
|
||||||
|
@ -626,27 +620,20 @@ Navigator::fence_point(int argc, char *argv[])
|
||||||
errx(1, "can't store fence point");
|
errx(1, "can't store fence point");
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
void
|
||||||
Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) {
|
Navigator::add_mission_item(unsigned mission_item_index, const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item) {
|
||||||
|
|
||||||
/* Check if there is a further mission as the new next item */
|
/* Check if there is a further mission as the new next item */
|
||||||
while (mission_item_index < _mission_item_count) {
|
while (mission_item_index < _mission_item_count) {
|
||||||
|
|
||||||
if (1 /* TODO: check for correct frame */) {
|
if (1 /* TODO: check for correct frame */) {
|
||||||
|
|
||||||
warnx("copying item number %d", mission_item_index);
|
|
||||||
memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s));
|
memcpy(new_mission_item, &_mission_item[mission_item_index], sizeof(mission_item_s));
|
||||||
return OK;
|
return;
|
||||||
}
|
}
|
||||||
mission_item_index++;
|
mission_item_index++;
|
||||||
}
|
}
|
||||||
|
|
||||||
return ERROR;
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_item, struct mission_item_s *new_mission_item)
|
|
||||||
{
|
|
||||||
/* if no existing mission item exists, take curent location */
|
/* if no existing mission item exists, take curent location */
|
||||||
if (existing_mission_item == nullptr) {
|
if (existing_mission_item == nullptr) {
|
||||||
|
|
||||||
|
@ -706,54 +693,71 @@ Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_i
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Navigator::advance_current_mission_item()
|
Navigator::update_mission_item_triplet()
|
||||||
{
|
{
|
||||||
/* if there is one more mission available we can just advance by one, otherwise return */
|
if (!_mission_item_triplet.current_valid) {
|
||||||
if (_mission_item_triplet.next_valid) {
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
/* copy the next to current */
|
|
||||||
memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s));
|
|
||||||
_mission_item_triplet.current_valid = _mission_item_triplet.next_valid;
|
|
||||||
|
|
||||||
_current_mission_item_index++;
|
|
||||||
|
|
||||||
|
|
||||||
/* maybe there are no more mission item, in this case add a loiter mission item */
|
/* the current mission item is missing, add one */
|
||||||
if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
|
if (_mission_item_triplet.previous_valid) {
|
||||||
|
/* if we know the last one, proceed to succeeding one */
|
||||||
|
add_mission_item(_mission_item_triplet.previous.index + 1, &_mission_item_triplet.previous, &_mission_item_triplet.current);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
/* if we don't remember the last one, start new */
|
||||||
|
add_mission_item(0, nullptr, &_mission_item_triplet.current);
|
||||||
|
}
|
||||||
|
_mission_item_triplet.current_valid = true;
|
||||||
|
}
|
||||||
|
|
||||||
add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
|
if (_mission_item_triplet.current_valid && !_mission_item_triplet.next_valid) {
|
||||||
|
|
||||||
|
if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
|
||||||
|
/* if we are already loitering, don't bother about a next mission item */
|
||||||
|
|
||||||
|
_mission_item_triplet.next_valid = false;
|
||||||
|
} else {
|
||||||
|
|
||||||
|
add_mission_item(_mission_item_triplet.current.index + 1, &_mission_item_triplet.current, &_mission_item_triplet.next);
|
||||||
_mission_item_triplet.next_valid = true;
|
_mission_item_triplet.next_valid = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
Navigator::advance_current_mission_item()
|
||||||
|
{
|
||||||
|
/* if there is no more mission available, don't advance and return */
|
||||||
|
if (!_mission_item_triplet.next_valid) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
/* copy the next to current */
|
||||||
|
memcpy(&_mission_item_triplet.current, &_mission_item_triplet.next, sizeof(mission_item_s));
|
||||||
|
_mission_item_triplet.current_valid = _mission_item_triplet.next_valid;
|
||||||
|
|
||||||
|
/* flag the next mission as invalid */
|
||||||
|
_mission_item_triplet.next_valid = false;
|
||||||
|
|
||||||
|
update_mission_item_triplet();
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Navigator::restart_mission()
|
Navigator::restart_mission()
|
||||||
{
|
{
|
||||||
reset_mission_item_reached();
|
reset_mission_item_reached();
|
||||||
|
|
||||||
_current_mission_item_index = 0;
|
/* forget about the all mission items */
|
||||||
|
|
||||||
_mission_item_triplet.previous_valid = false;
|
_mission_item_triplet.previous_valid = false;
|
||||||
|
_mission_item_triplet.current_valid = false;
|
||||||
|
_mission_item_triplet.next_valid = false;
|
||||||
|
|
||||||
/* add a new current mission item */
|
update_mission_item_triplet();
|
||||||
if (add_mission_item(_current_mission_item_index, &_mission_item_triplet.current) != OK) {
|
|
||||||
|
|
||||||
add_last_mission_item(nullptr, &_mission_item_triplet.current);
|
|
||||||
} else {
|
|
||||||
/* if current succeeds, we can even add a next item */
|
|
||||||
if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
|
|
||||||
add_last_mission_item(&_mission_item_triplet.current, &_mission_item_triplet.next);
|
|
||||||
}
|
|
||||||
_mission_item_triplet.next_valid = true;
|
|
||||||
}
|
|
||||||
_mission_item_triplet.current_valid = true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -80,6 +80,7 @@ struct mission_item_s
|
||||||
float radius; /**< radius in which the mission is accepted as reached in meters */
|
float radius; /**< radius in which the mission is accepted as reached in meters */
|
||||||
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
|
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 */
|
bool autocontinue; /**< true if next waypoint should follow after this one */
|
||||||
|
int index; /**< index matching the mavlink waypoint */
|
||||||
};
|
};
|
||||||
|
|
||||||
struct mission_s
|
struct mission_s
|
||||||
|
|
|
@ -67,6 +67,10 @@ struct mission_item_triplet_s
|
||||||
struct mission_item_s previous;
|
struct mission_item_s previous;
|
||||||
struct mission_item_s current;
|
struct mission_item_s current;
|
||||||
struct mission_item_s next;
|
struct mission_item_s next;
|
||||||
|
|
||||||
|
int previous_index;
|
||||||
|
int current_index;
|
||||||
|
int next_index;
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
Loading…
Reference in New Issue