yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: mission manager moved to separate class and reworked

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-14 23:57:29 +02:00
parent ffd9ac7e08
commit 5be741607c
14 changed files with 1167 additions and 1016 deletions
Download Patch File Download Diff File Expand all files Collapse all files

766
src/modules/mavlink/mavlink_main.cpp
View File

@@ -102,6 +102,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
extern mavlink_system_t mavlink_system;
static uint64_t last_write_success_times[6] = {0};
static uint64_t last_write_try_times[6] = {0};
@@ -200,9 +202,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
}
static void usage(void);
@@ -221,6 +220,7 @@ Mavlink::Mavlink() :
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_result_sub(-1),
_mission_pub(-1),
_mode(MAVLINK_MODE_NORMAL),
_total_counter(0),
@@ -238,12 +238,11 @@ Mavlink::Mavlink() :
_param_component_id(0),
_param_system_type(0),
_param_use_hil_gps(0),
_mission_manager(nullptr),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
_wpm = &_wpm_s;
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
_instance_id = Mavlink::instance_count();
@@ -421,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
}
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
@@ -497,7 +496,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
void Mavlink::mavlink_update_system(void)
{
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
@@ -701,9 +699,32 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
extern mavlink_system_t mavlink_system;
void
Mavlink::send_message(const mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int Mavlink::mavlink_pm_queued_send()
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
/* handle packet with mission manager */
_mission_manager->handle_message(msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(_channel, msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(msg, this);
}
}
int
Mavlink::mavlink_pm_queued_send()
{
if (_mavlink_param_queue_index < param_count()) {
mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
@@ -780,7 +801,7 @@ int Mavlink::mavlink_pm_send_param(param_t param)
mavlink_type,
param_count(),
param_get_index(param));
mavlink_missionlib_send_message(&tx_msg);
send_message(&tx_msg);
return OK;
}
@@ -794,7 +815,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
send_statustext("[mavlink pm] sending list");
}
} break;
@@ -818,7 +839,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
send_statustext(buf);
} else {
/* set and send parameter */
@@ -854,677 +875,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
}
}
void Mavlink::publish_mission()
int
Mavlink::send_statustext(const char *string)
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission);
} else {
orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission);
}
return send_statustext(MAV_SEVERITY_INFO, string);
}
int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{
/* 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;
}
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
mission_item->yaw = _wrap_pi(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 */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return OK;
}
int Mavlink::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;
}
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
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;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
return OK;
}
void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
{
/* get offboard_mission topic */
int mission_sub = orb_subscribe(ORB_ID(offboard_mission));
if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) {
/* error getting topic, init to safe values */
_mission.dataman_id = 0;
_mission.count = 0;
_mission.current_index = -1;
}
close(mission_sub);
/* init WPM state */
state->size = _mission.count;
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
state->dataman_id = _mission.dataman_id;
state->current_state = MAVLINK_WPM_STATE_IDLE;
state->current_partner_sysid = 0;
state->current_partner_compid = 0;
state->current_dataman_id = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timestamp_last_send_request = 0;
state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT;
warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index);
}
/*
* @brief Sends an waypoint ack message
*/
void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
/*
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
{
if (seq < _wpm->size) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
} else if (seq == 0 && _wpm->size == 0) {
/* don't broadcast if no WPs */
} else {
if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); }
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
}
}
void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
{
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _wpm->size;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
struct mission_item_s mission_item;
if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
/* create mission_item_s from mavlink_mission_item_t */
mavlink_mission_item_t wp;
map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); }
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); }
}
}
void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < _wpm->max_size) {
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
_wpm->timestamp_last_send_request = hrt_absolute_time();
if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); }
}
}
/*
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); }
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
{
/* check for timed-out operations */
if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("Operation timeout");
if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
} else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* try to get WP again after short timeout */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
{
uint64_t now = hrt_absolute_time();
switch (msg->msgid) {
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (_wpm->current_wp_id == _wpm->size - 1) {
if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_wp_id = 0;
}
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.seq < _wpm->size) {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); }
_mission.current_index = wpc.seq;
publish_mission();
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); }
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
}
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (_wpm->size > 0) {
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
_wpm->current_partner_compid = msg->compid;
} else {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); }
}
_wpm->current_count = _wpm->size;
mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count);
} else {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) {
_wpm->timestamp_lastaction = now;
if (wpr.seq >= _wpm->size) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
break;
}
/*
* Ensure that we are in the correct state and that the first request has id 0
* and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
*/
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
break;
}
} else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (wpr.seq == _wpm->current_wp_id) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == _wpm->current_wp_id + 1) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
break;
}
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
break;
}
_wpm->current_wp_id = wpr.seq;
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
if (wpr.seq < _wpm->size) {
mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION");
break;
}
if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (_wpm->current_wp_id == 0) {
/* looks like our WAYPOINT_REQUEST was lost, try again */
if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); }
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
} else {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); }
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
break;
}
} else {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); }
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
break;
}
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
_wpm->current_partner_compid = msg->compid;
_wpm->current_count = wpc.count;
_wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission
_mission.current_index = -1;
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
_wpm->timestamp_lastaction = now;
/* 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) {
if (wp.seq != 0) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); }
break;
}
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (wp.seq >= _wpm->current_count) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); }
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
break;
}
if (wp.seq != _wpm->current_wp_id) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); }
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
}
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
struct mission_item_s mission_item;
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
if (ret != OK) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret);
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
/* waypoint marked as current */
if (wp.current) {
_mission.current_index = wp.seq;
}
if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
_wpm->current_wp_id = wp.seq + 1;
if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* got all new mission items successfully */
if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); }
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
/* update WPM state */
_wpm->dataman_id = _wpm->current_dataman_id;
_wpm->size = _wpm->current_count;
/* update mission topic */
_mission.dataman_id = _wpm->dataman_id;
_mission.count = _wpm->current_count;
publish_mission();
} else {
/* request next item */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
/* clear only active dataman storage, don't change storage id */
dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1;
if (dm_clear(dm_id) == OK) {
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
/* update WPM state */
_wpm->timestamp_lastaction = now;
_wpm->size = 0;
/* update mission topic */
_mission.dataman_id = _wpm->dataman_id;
_mission.count = 0;
_mission.current_index = -1;
publish_mission();
} else {
if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
}
}
break;
}
default: {
/* other messages might should get caught by mavlink and others */
break;
}
}
}
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
int
Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
statustext.severity = MAV_SEVERITY_INFO;
statustext.severity = severity;
int i = 0;
@@ -1546,7 +908,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
return OK;
} else {
return 1;
return ERROR;
}
}
@@ -1684,7 +1046,7 @@ Mavlink::message_buffer_is_empty()
bool
Mavlink::message_buffer_write(void *ptr, int size)
Mavlink::message_buffer_write(const void *ptr, int size)
{
// bytes available to write
int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
@@ -1751,7 +1113,7 @@ Mavlink::message_buffer_mark_read(int n)
}
void
Mavlink::pass_message(mavlink_message_t *msg)
Mavlink::pass_message(const mavlink_message_t *msg)
{
if (_passing_on) {
/* size is 8 bytes plus variable payload */
@@ -1762,6 +1124,11 @@ Mavlink::pass_message(mavlink_message_t *msg)
}
}
float
Mavlink::get_rate_mult()
{
return _datarate / 1000.0f;
}
int
Mavlink::task_main(int argc, char *argv[])
@@ -1879,8 +1246,6 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
@@ -1922,11 +1287,11 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
/* initialize waypoint manager */
mavlink_wpm_init(_wpm);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
memset(&_mission_result, 0, sizeof(_mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
@@ -1941,7 +1306,7 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
float rate_mult = get_rate_mult();
configure_stream("HEARTBEAT", 1.0f);
@@ -1976,7 +1341,6 @@ Mavlink::task_main(int argc, char *argv[])
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
@@ -2030,36 +1394,16 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
bool updated;
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result);
if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); }
if (_mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached);
}
mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission);
} else {
if (slow_rate_limiter.check(t)) {
mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission);
}
}
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(hrt_absolute_time());
_mission_manager->eventloop();
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
send_statustext(msg.text);
}
}
}
@@ -2093,11 +1437,11 @@ Mavlink::task_main(int argc, char *argv[])
}
}
perf_end(_loop_perf);
}
delete _mission_manager;
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;