/**
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_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++;
}
}
/**
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;
}
}
}
/**
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 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();
}
/**
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_link != nullptr) {
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_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;
}
}
transfer_activity = LISTING;
_log_sending_link = &link;
handle_log_send_listing();
}
/// 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;
}
}
/**
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;
}
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;
}
}
