dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator, waypoints: save index in mission item and use this in navigator

This commit is contained in:
Julian Oes 2013-11-22 14:03:09 +01:00
parent 42aefe838c
commit 7892a72f90
4 changed files with 100 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
src/modules/mavlink/waypoints.c
View File

@ -60,6 +60,10 @@
bool debug = 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_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->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;
}
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;
@ -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->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
mavlink_mission_item->autocontinue = mission_item->autocontinue;
mavlink_mission_item->seq = seq;
mavlink_mission_item->seq = mission_item->index;
}
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_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)
{
static orb_advert_t mission_pub = -1;
static struct mission_s mission;
uint64_t now = mavlink_missionlib_get_system_timestamp();
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;
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);
} 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_count = wpc.count;
/* prepare mission topic */
mission.count = wpc.count;
mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * mission.count);
if (!mission.items) {
mission.count = 0;
/* XXX reject waypoints if this fails */
warnx("no free RAM to allocate mission, rejecting any waypoints");
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;
}
#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->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) {
// 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;

110
src/modules/navigator/navigator_main.cpp
View File

@ -69,11 +69,6 @@
#include <mathlib/mathlib.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
@ -142,8 +137,6 @@ private:
unsigned _mission_item_count; /** number of mission items copied */
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 */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
@ -213,9 +206,11 @@ private:
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();
@ -260,7 +255,6 @@ Navigator::Navigator() :
_max_mission_item_count(10),
_fence_valid(false),
_inside_fence(true),
_current_mission_item_index(-1),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
@ -626,27 +620,20 @@ Navigator::fence_point(int argc, char *argv[])
errx(1, "can't store fence point");
}
int
Navigator::add_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) {
void
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 */
while (mission_item_index < _mission_item_count) {
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));
return OK;
return;
}
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 (existing_mission_item == nullptr) {
@ -706,54 +693,71 @@ Navigator::add_last_mission_item(const struct mission_item_s *existing_mission_i
}
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.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++;
if (!_mission_item_triplet.current_valid) {
/* maybe there are no more mission item, in this case add a loiter mission item */
if (add_mission_item(_current_mission_item_index + 1, &_mission_item_triplet.next) != OK) {
/* the current mission item is missing, add one */
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;
}
}
}
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
Navigator::restart_mission()
{
reset_mission_item_reached();
_current_mission_item_index = 0;
/* forget about the all mission items */
_mission_item_triplet.previous_valid = false;
_mission_item_triplet.current_valid = false;
_mission_item_triplet.next_valid = false;
/* add a new current mission item */
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;
update_mission_item_triplet();
}

1
src/modules/uORB/topics/mission.h
View File

@ -80,6 +80,7 @@ struct mission_item_s
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 */
bool autocontinue; /**< true if next waypoint should follow after this one */
int index; /**< index matching the mavlink waypoint */
};
struct mission_s

4
src/modules/uORB/topics/mission_item_triplet.h
View File

@ -67,6 +67,10 @@ struct mission_item_triplet_s
struct mission_item_s previous;
struct mission_item_s current;
struct mission_item_s next;
int previous_index;
int current_index;
int next_index;
};
/**