diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 408 |
1 files changed, 226 insertions, 182 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 833de5a3d..8dfa32ce8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -105,7 +109,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -113,6 +118,7 @@ static uint64_t last_write_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -149,10 +155,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length instance = Mavlink::get_instance(6); break; #endif - } - - /* no valid instance, bail */ - if (!instance) { + default: return; } @@ -169,29 +172,41 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + instance->count_txerr(); + return; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + instance->count_txerr(); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } @@ -202,9 +217,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - next(nullptr), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), + next(nullptr), _mavlink_fd(-1), _task_running(false), _hil_enabled(false), @@ -216,18 +231,27 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), _message_buffer({}), + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { _wpm = &_wpm_s; mission.count = 0; @@ -280,6 +304,7 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { perf_free(_loop_perf); + perf_free(_txerr_perf); if (_task_running) { /* task wakes up every 10ms or so at the longest */ @@ -305,6 +330,12 @@ Mavlink::~Mavlink() } void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + +void Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; @@ -410,7 +441,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - + Mavlink *inst; LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { @@ -484,48 +515,53 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - static bool initialized = false; - static param_t param_system_id; - static param_t param_component_id; - static param_t param_system_type; - static param_t param_use_hil_gps; - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - param_use_hil_gps = param_find("MAV_USEHILGPS"); - initialized = true; + if (!_param_initialized) { + _param_system_id = param_find("MAV_SYS_ID"); + _param_component_id = param_find("MAV_COMP_ID"); + _param_system_type = param_find("MAV_TYPE"); + _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_initialized = true; } /* update system and component id */ int32_t system_id; - param_get(param_system_id, &system_id); + param_get(_param_system_id, &system_id); if (system_id > 0 && system_id < 255) { mavlink_system.sysid = system_id; } int32_t component_id; - param_get(param_component_id, &component_id); + param_get(_param_component_id, &component_id); if (component_id > 0 && component_id < 255) { mavlink_system.compid = component_id; } int32_t system_type; - param_get(param_system_type, &system_type); + param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { mavlink_system.type = system_type; } int32_t use_hil_gps; - param_get(param_use_hil_gps, &use_hil_gps); + param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -727,9 +763,9 @@ int Mavlink::mavlink_pm_send_param(param_t param) if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; float val_buf; - static mavlink_message_t tx_msg; + mavlink_message_t tx_msg; /* query parameter type */ param_type_t type = param_type(param); @@ -780,9 +816,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + mavlink_param_request_list_t req; + mavlink_msg_param_request_list_decode(msg, &req); + if (req.target_system == mavlink_system.sysid && + (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"); + } } break; case MAVLINK_MSG_ID_PARAM_SET: { @@ -804,7 +845,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, "[mavlink pm] unknown: %s", name); + sprintf(buf, "[pm] unknown: %s", name); mavlink_missionlib_send_gcs_string(buf); } else { @@ -881,11 +922,16 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; + 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_current_count = 0; + 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; } @@ -894,11 +940,13 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item 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->time_inside = mavlink_mission_item->param1; 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; } @@ -913,11 +961,17 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; + 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; } @@ -928,7 +982,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ 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->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; @@ -944,6 +997,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->current_partner_compid = 0; state->timestamp_lastaction = 0; state->timestamp_last_send_setpoint = 0; + state->timestamp_last_send_request = 0; state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->current_dataman_id = 0; } @@ -992,8 +1046,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - - if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -1044,6 +1096,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t } 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("ERROR: could not read WP%u", seq); } } @@ -1059,13 +1112,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u 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("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -1096,11 +1148,13 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("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 > 500000 && _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); } } @@ -1128,8 +1182,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; @@ -1153,21 +1205,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - - if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - - if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; @@ -1197,14 +1239,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - - if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } - - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - - if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; @@ -1221,8 +1256,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } - break; } @@ -1233,15 +1266,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } - break; } @@ -1249,17 +1280,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - break; } @@ -1267,8 +1296,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } - break; } @@ -1282,7 +1309,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } + mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds"); } @@ -1293,12 +1320,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } @@ -1322,15 +1343,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - - if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + mavlink_missionlib_send_gcs_string("WP COUNT 0"); break; } - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; _wpm->current_wp_id = 0; _wpm->current_partner_sysid = msg->sysid; @@ -1344,25 +1361,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP"); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - - if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; @@ -1384,7 +1389,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); break; } @@ -1392,12 +1396,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.seq >= _wpm->current_count) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } if (wp.seq != _wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch"); mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } @@ -1430,6 +1433,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) 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); + mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1461,11 +1465,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1501,13 +1500,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - - if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1523,9 +1515,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) void Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); } @@ -1599,31 +1592,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* delete stream */ LL_DELETE(_streams, stream); delete stream; + warnx("deleted stream %s", stream->get_name()); } return OK; } } - if (interval > 0) { - /* search for stream with specified name in supported streams list */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create new instance */ - stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); - stream->set_interval(interval); - stream->subscribe(this); - LL_APPEND(_streams, stream); - return OK; - } - } - - } else { - /* stream not found, nothing to disable */ + if (interval == 0) { + /* stream was not active and is requested to be disabled, do nothing */ return OK; } + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(interval); + stream->subscribe(this); + LL_APPEND(_streams, stream); + + return OK; + } + } + + /* if we reach here, the stream list does not contain the stream */ + warnx("stream %s not found", stream_name); + return ERROR; } @@ -1658,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1790,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1846,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1911,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { - /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(500)) { + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1943,9 +1958,12 @@ Mavlink::task_main(int argc, char *argv[]) _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); + uint64_t status_time = 0; - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + struct vehicle_status_s status; + status_sub->update(&status_time, &status); MavlinkCommandsStream commands_stream(this, _channel); @@ -2002,14 +2020,14 @@ Mavlink::task_main(int argc, char *argv[]) hrt_abstime t = hrt_absolute_time(); - if (param_sub->update(t)) { + if (param_sub->update(¶m_time, nullptr)) { /* parameters updated */ mavlink_update_system(); } - if (status_sub->update(t)) { + if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ - set_hil_enabled(status->hil_state == HIL_STATE_ON); + set_hil_enabled(status.hil_state == HIL_STATE_ON); } /* update commands stream */ @@ -2019,14 +2037,14 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); } else { warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, _device_name); + warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name); } delete _subscribe_to_stream; @@ -2073,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); - message_buffer_mark_read(available); + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + size_t read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -2148,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2166,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[]) /* create the instance in task context */ Mavlink *instance = new Mavlink(); - /* this will actually only return once MAVLink exits */ - int res = instance->task_main(argc, argv); + int res; + + if (!instance) { - /* delete instance on main thread end */ - delete instance; + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } return res; } @@ -2193,7 +2238,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); @@ -2221,18 +2266,17 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; const char *stream_name = nullptr; - int ch; argc -= 2; argv += 2; @@ -2269,7 +2313,7 @@ Mavlink::stream(int argc, char *argv[]) i++; } - if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + if (!err_flag && rate >= 0.0f && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { @@ -2291,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) @@ -2316,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); |