fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/waypoints.c
+27 -7
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);
src/modules/mavlink/waypoints.h
+1
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;