/** trigger sending of log messages if there are some pending */ void DataFlash_Class::handle_log_send_listing(GCS_MAVLINK &link) { if (!HAVE_PAYLOAD_SPACE(link.get_chan(), LOG_ENTRY)) { // no space return; } if (AP_HAL::millis() - link.get_last_heartbeat_time() > 3000) { // give a heartbeat a chance return; } uint32_t size, time_utc; if (_log_next_list_entry == 0) { size = 0; time_utc = 0; } else { get_log_info(_log_next_list_entry, size, time_utc); } mavlink_msg_log_entry_send(link.get_chan(), _log_next_list_entry, _log_num_logs, _log_last_list_entry, time_utc, size); if (_log_next_list_entry == _log_last_list_entry) { _log_listing = false; _log_sending_chan = -1; } else { _log_next_list_entry++; } }
/** handle request for log data */ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *msg) { if (_log_sending_chan >= 0) { // some GCS (e.g. MAVProxy) attempt to stream request_data // messages when they're filling gaps in the downloaded logs. // This channel check avoids complaining to them, at the cost // of silently dropping any repeated attempts to start logging if (_log_sending_chan != link.get_chan()) { link.send_text(MAV_SEVERITY_INFO, "Log download in progress"); } return; } mavlink_log_request_data_t packet; mavlink_msg_log_request_data_decode(msg, &packet); _in_log_download = true; _log_listing = false; if (!_log_sending || _log_num_data != packet.id) { _log_sending = false; uint16_t num_logs = get_num_logs(); if (packet.id > num_logs || packet.id < 1) { return; } uint32_t time_utc, size; get_log_info(packet.id, size, time_utc); _log_num_data = packet.id; _log_data_size = size; uint16_t end; get_log_boundaries(packet.id, _log_data_page, end); } _log_data_offset = packet.ofs; if (_log_data_offset >= _log_data_size) { _log_data_remaining = 0; } else { _log_data_remaining = _log_data_size - _log_data_offset; } if (_log_data_remaining > packet.count) { _log_data_remaining = packet.count; } _log_sending = true; _log_sending_chan = link.get_chan(); handle_log_send(link); }
/** handle all types of log download requests from the GCS */ void DataFlash_Class::handle_log_request_list(GCS_MAVLINK &link, mavlink_message_t *msg) { if (_log_sending_chan >= 0) { link.send_text(MAV_SEVERITY_INFO, "Log download in progress"); return; } mavlink_log_request_list_t packet; mavlink_msg_log_request_list_decode(msg, &packet); _log_listing = false; _log_sending = false; _log_num_logs = get_num_logs(); if (_log_num_logs == 0) { _log_next_list_entry = 0; _log_last_list_entry = 0; } else { _log_next_list_entry = packet.start; _log_last_list_entry = packet.end; if (_log_last_list_entry > _log_num_logs) { _log_last_list_entry = _log_num_logs; } if (_log_next_list_entry < 1) { _log_next_list_entry = 1; } } _log_listing = true; _log_sending_chan = link.get_chan(); handle_log_send_listing(link); }
/** trigger sending of log messages if there are some pending */ void DataFlash_Class::handle_log_send(GCS_MAVLINK &link) { if (_log_sending_chan != link.get_chan()) { return; } if (_log_listing) { handle_log_send_listing(link); } if (!_log_sending) { return; } #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // assume USB speeds in SITL for the purposes of log download const uint8_t num_sends = 40; #else uint8_t num_sends = 1; if (link.is_high_bandwidth() && hal.gpio->usb_connected()) { // when on USB we can send a lot more data num_sends = 250; } else if (link.have_flow_control()) { #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX num_sends = 80; #else num_sends = 10; #endif } #endif for (uint8_t i=0; i<num_sends; i++) { if (_log_sending) { if (!handle_log_send_data(link)) break; } } }
/** trigger sending of log data if there are some pending */ bool DataFlash_Class::handle_log_send_data(GCS_MAVLINK &link) { if (!HAVE_PAYLOAD_SPACE(link.get_chan(), LOG_DATA)) { // no space return false; } if (AP_HAL::millis() - link.get_last_heartbeat_time() > 3000) { // give a heartbeat a chance return false; } int16_t ret = 0; uint32_t len = _log_data_remaining; mavlink_log_data_t packet; if (len > 90) { len = 90; } ret = get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, packet.data); if (ret < 0) { // report as EOF on error ret = 0; } if (ret < 90) { memset(&packet.data[ret], 0, 90-ret); } packet.ofs = _log_data_offset; packet.id = _log_num_data; packet.count = ret; _mav_finalize_message_chan_send(link.get_chan(), MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); _log_data_offset += len; _log_data_remaining -= len; if (ret < 90 || _log_data_remaining == 0) { _log_sending = false; _log_sending_chan = -1; } return true; }
/** handle request for log data */ void DataFlash_Class::handle_log_request_data(GCS_MAVLINK &link, mavlink_message_t *msg) { if (_log_sending_link != nullptr) { // some GCS (e.g. MAVProxy) attempt to stream request_data // messages when they're filling gaps in the downloaded logs. // This channel check avoids complaining to them, at the cost // of silently dropping any repeated attempts to start logging if (_log_sending_link->get_chan() != link.get_chan()) { link.send_text(MAV_SEVERITY_INFO, "Log download in progress"); } return; } mavlink_log_request_data_t packet; mavlink_msg_log_request_data_decode(msg, &packet); // consider opening or switching logs: if (transfer_activity != SENDING || _log_num_data != packet.id) { uint16_t num_logs = get_num_logs(); if (packet.id > num_logs || packet.id < 1) { // request for an invalid log; cancel any current download transfer_activity = IDLE; return; } uint32_t time_utc, size; get_log_info(packet.id, size, time_utc); _log_num_data = packet.id; _log_data_size = size; uint16_t end; get_log_boundaries(packet.id, _log_data_page, end); } _log_data_offset = packet.ofs; if (_log_data_offset >= _log_data_size) { _log_data_remaining = 0; } else { _log_data_remaining = _log_data_size - _log_data_offset; } if (_log_data_remaining > packet.count) { _log_data_remaining = packet.count; } transfer_activity = SENDING; _log_sending_link = &link; handle_log_send(); }
/// handler for polygon fence messages with GCS void AC_Fence::handle_msg(GCS_MAVLINK &link, mavlink_message_t* msg) { // exit immediately if null message if (msg == nullptr) { return; } switch (msg->msgid) { // receive a fence point from GCS and store in EEPROM case MAVLINK_MSG_ID_FENCE_POINT: { mavlink_fence_point_t packet; mavlink_msg_fence_point_decode(msg, &packet); if (!check_latlng(packet.lat,packet.lng)) { link.send_text(MAV_SEVERITY_WARNING, "Invalid fence point, lat or lng too large"); } else { Vector2l point; point.x = packet.lat*1.0e7f; point.y = packet.lng*1.0e7f; if (!_poly_loader.save_point_to_eeprom(packet.idx, point)) { link.send_text(MAV_SEVERITY_WARNING, "Failed to save polygon point, too many points?"); } else { // trigger reload of points _boundary_loaded = false; } } break; } // send a fence point to GCS case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { mavlink_fence_fetch_point_t packet; mavlink_msg_fence_fetch_point_decode(msg, &packet); // attempt to retrieve from eeprom Vector2l point; if (_poly_loader.load_point_from_eeprom(packet.idx, point)) { mavlink_msg_fence_point_send_buf(msg, link.get_chan(), msg->sysid, msg->compid, packet.idx, _total, point.x*1.0e-7f, point.y*1.0e-7f); } else { link.send_text(MAV_SEVERITY_WARNING, "Bad fence point"); } break; } default: // do nothing break; } }
void DataFlash_Class::handle_mavlink_msg(GCS_MAVLINK &link, mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: FOR_EACH_BACKEND(remote_log_block_status_msg(link.get_chan(), msg)); break; case MAVLINK_MSG_ID_LOG_REQUEST_LIST: /* fall through */ case MAVLINK_MSG_ID_LOG_REQUEST_DATA: /* fall through */ case MAVLINK_MSG_ID_LOG_ERASE: /* fall through */ case MAVLINK_MSG_ID_LOG_REQUEST_END: handle_log_message(link, msg); break; } }
void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: FOR_EACH_BACKEND(remote_log_block_status_msg(link.get_chan(), msg)); break; case MAVLINK_MSG_ID_LOG_REQUEST_LIST: FALLTHROUGH; case MAVLINK_MSG_ID_LOG_REQUEST_DATA: FALLTHROUGH; case MAVLINK_MSG_ID_LOG_ERASE: FALLTHROUGH; case MAVLINK_MSG_ID_LOG_REQUEST_END: handle_log_message(link, msg); break; } }