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

Waypoints/Navigator: Use two different dataman storage places, keep old waypoints until all new ones are written

This commit is contained in:
Julian Oes 2013-12-25 11:23:58 +01:00
parent 5c33aeeb43
commit b10fa3d047
8 changed files with 64 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/dataman/dataman.c
View File

@ -111,7 +111,8 @@ static unsigned g_func_counts[dm_number_of_funcs];
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_SAFE_POINTS_MAX,
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
DM_KEY_WAYPOINTS_ONBOARD_MAX
};

6
src/modules/dataman/dataman.h
View File

@ -50,7 +50,8 @@ extern "C" {
typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
DM_KEY_WAYPOINTS_OFFBOARD, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
@ -59,7 +60,8 @@ extern "C" {
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
DM_KEY_WAYPOINTS_OFFBOARD_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
};

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

@ -169,6 +169,8 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
state->timestamp_lastaction = 0;
// state->timestamp_last_send_setpoint = 0;
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->current_dataman_id = 0;
// state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
// state->idle = false; ///< indicates if the system is following the waypoints or is waiting
// state->current_active_wp_id = -1; ///< id of current waypoint
@ -612,6 +614,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// wpm->yaw_reached = false;
// wpm->pos_reached = false;
mission.current_index = wpc.seq;
publish_mission();
@ -718,7 +721,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
struct mission_item_s mission_item;
ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, wpr.seq, &mission_item, len) == len) {
dm_item_t dm_current;
if (wpm->current_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
if (dm_read(dm_current, wpr.seq, &mission_item, len) == len) {
if (mission.current_index == wpr.seq) {
wp.current = true;
@ -855,10 +866,6 @@ 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 {
/* set count to 0 while copying */
mission.count = 0;
publish_mission();
}
#ifdef MAVLINK_WPM_NO_PRINTF
@ -992,7 +999,17 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
size_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_OFFBOARD, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
dm_item_t dm_next;
if (wpm->current_dataman_id == 0) {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
mission.dataman_id = 1;
} else {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.dataman_id = 0;
}
if (dm_write(dm_next, 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;
@ -1038,6 +1055,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
publish_mission();
wpm->current_dataman_id = mission.dataman_id;
wpm->size = wpm->current_count;
//get the new current waypoint
@ -1132,9 +1150,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// wpm->pos_reached = false;
/* prepare mission topic */
mission.dataman_id = -1;
mission.count = 0;
mission.current_index = -1;
if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD) == OK) {
if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);

1
src/modules/mavlink/waypoints.h
View File

@ -110,6 +110,7 @@ struct mavlink_wpm_storage {
// uint64_t timestamp_firstinside_orbit;
// uint64_t timestamp_lastoutside_orbit;
uint32_t timeout;
int current_dataman_id;
// uint32_t delay_setpoint;
// float accept_range_yaw;
// float accept_range_distance;

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

@ -440,6 +440,7 @@ Navigator::offboard_mission_update()
struct mission_s offboard_mission;
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
_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);

27
src/modules/navigator/navigator_mission.cpp
View File

@ -54,6 +54,7 @@ static const int ERROR = -1;
Mission::Mission() :
_offboard_dataman_id(-1),
_current_offboard_mission_index(0),
_current_onboard_mission_index(0),
_offboard_mission_item_count(0),
@ -67,6 +68,12 @@ Mission::~Mission()
}
void
Mission::set_offboard_dataman_id(int new_id)
{
_offboard_dataman_id = new_id;
}
void
Mission::set_current_offboard_mission_index(int new_index)
{
@ -132,8 +139,16 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index, new_mission_item, len) != len) {
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
@ -166,8 +181,16 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}

2
src/modules/navigator/navigator_mission.h
View File

@ -55,6 +55,7 @@ public:
*/
~Mission();
void set_offboard_dataman_id(int new_id);
void set_current_offboard_mission_index(int new_index);
void set_current_onboard_mission_index(int new_index);
void set_offboard_mission_count(unsigned new_count);
@ -78,6 +79,7 @@ private:
bool next_onboard_mission_available();
bool next_offboard_mission_available();
int _offboard_dataman_id;
unsigned _current_offboard_mission_index;
unsigned _current_onboard_mission_index;
unsigned _offboard_mission_item_count; /** number of offboard mission items available */

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

@ -97,6 +97,7 @@ struct mission_item_s
struct mission_s
{
int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
unsigned count; /**< count of the missions stored in the datamanager */
int current_index; /**< default -1, start at the one changed latest */
};