fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator and mavlink: more mavlink cleanup and set current waypoint option support added

Browse Source
This commit is contained in:
Julian Oes 2013-11-27 16:17:44 +01:00
parent 3f25298798
commit 81023fe5aa
4 changed files with 138 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,11 +70,32 @@ struct mission_s mission;
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
return MAV_MISSION_ERROR;
}
mission_item->yaw = 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 */
@ -83,14 +104,22 @@ void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavl
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;
return OK;
}
void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
@ -98,6 +127,8 @@ void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missi
mavlink_mission_item->param2 = mission_item->time_inside * 1e3f; /* from seconds to milliseconds */
mavlink_mission_item->autocontinue = mission_item->autocontinue;
mavlink_mission_item->seq = mission_item->index;
return OK;
}
void mavlink_wpm_init(mavlink_wpm_storage *state)
@ -147,15 +178,15 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
if (MAVLINK_WPM_TEXT_FEEDBACK) {
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
#else
// if (MAVLINK_WPM_TEXT_FEEDBACK) {
// #ifdef MAVLINK_WPM_NO_PRINTF
// mavlink_missionlib_send_gcs_string("Sent waypoint ACK");
// #else
if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
// if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
#endif
}
// #endif
// }
}
/*
@ -563,7 +594,20 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// wpm->yaw_reached = false;
// wpm->pos_reached = false;
mavlink_wpm_send_waypoint_current(wpc.seq);
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);
}
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
//mavlink_wpm_send_waypoint_current(wpc.seq);
// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
// wpm->timestamp_firstinside_orbit = 0;
@ -788,6 +832,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
} else {
/* prepare mission topic */
mission.count = wpc.count;
/* reset current index */
mission.current_index = -1;
}
#ifdef MAVLINK_WPM_NO_PRINTF
@ -896,7 +942,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) {
if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) ||
(wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id &&
wp.seq < wpm->current_count)) {
//mavlink_missionlib_send_gcs_string("DEBUG 2");
// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
@ -904,9 +952,25 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
//
wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
// printf("WP seq: %d\n",wp.seq);
// 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]);
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;
}
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;
// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
@ -923,14 +987,19 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
// wpm->current_active_wp_id = wpm->rcv_size - 1;
// }
// switch the waypoints list
// FIXME CHECK!!!
uint32_t i;
// bool copy_error = false;
for (i = 0; i < wpm->current_count; ++i) {
wpm->waypoints[i] = wpm->rcv_waypoints[i];
map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]);
}
// // switch the waypoints list
// // FIXME CHECK!!!
// uint32_t i;
// for (i = 0; i < wpm->current_count; ++i) {
// wpm->waypoints[i] = wpm->rcv_waypoints[i];
// if (map_mavlink_mission_item_to_mission_item(&wpm->rcv_waypoints[i], &mission.items[i]) != OK) {
// copy_error = true;
// }
// }
// TODO: update count?
/* Initialize mission publication if necessary */
@ -973,38 +1042,41 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
}
} else {
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
//we're done receiving waypoints, answer with ack.
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
}
// if (verbose)
{
if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
break;
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_INVALID_SEQUENCE);
} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (!(wp.seq == 0)) {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
} else {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (!(wp.seq == wpm->current_wp_id)) {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
// if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
// //we're done receiving waypoints, answer with ack.
// mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0);
// printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
// }
} else if (!(wp.seq < wpm->current_count)) {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
} else {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
} else {
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
// // if (verbose)
// {
// if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state);
// break;
// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
// if (!(wp.seq == 0)) {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
// } else {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
// }
// } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
// if (!(wp.seq == wpm->current_wp_id)) {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id);
// mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
// } else if (!(wp.seq < wpm->current_count)) {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
// } else {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
// }
// } else {
// // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
// }
// }
}
} else {
//we we're target but already communicating with someone else

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

@ -82,7 +82,7 @@ enum MAVLINK_WPM_CODES {
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 15
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
// #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#ifndef MAVLINK_WPM_TEXT_FEEDBACK
#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text
#endif
@ -93,9 +93,9 @@ enum MAVLINK_WPM_CODES {
struct mavlink_wpm_storage {
mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
#endif
// #ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
// mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
// #endif
uint16_t size;
uint16_t max_size;
uint16_t rcv_size;
@ -120,8 +120,8 @@ struct mavlink_wpm_storage {
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
void map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
void map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void mavlink_wpm_init(mavlink_wpm_storage *state);

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

@ -358,7 +358,7 @@ Navigator::mission_update()
* if the first part changed: start again at first waypoint
* if the first part remained unchanged: continue with the (possibly changed second part)
*/
if (_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
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 */
@ -371,8 +371,11 @@ Navigator::mission_update()
// warnx("Mission item is equivalent i=%d", i);
// }
}
} else {
} 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);
}

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

@ -90,6 +90,7 @@ struct mission_s
{
struct mission_item_s *items;
unsigned count;
int current_index; /**< default -1, start at the one changed latest */
};
/**