Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: started adding MAVLink log download

This commit is contained in:
Andrew Tridgell 2013-12-15 18:57:15 +11:00
parent e8f48af1b5
commit d034a4108b
3 changed files with 270 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/GCS_MAVLink/GCS.h
View File

@ -10,8 +10,46 @@
#include <AP_HAL.h>
#include <AP_Common.h>
#include <GPS.h>
#include <GCS_MAVLink.h>
#include <DataFlash.h>
#include <stdint.h>
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
enum ap_message {
MSG_HEARTBEAT,
MSG_ATTITUDE,
MSG_LOCATION,
MSG_EXTENDED_STATUS1,
MSG_EXTENDED_STATUS2,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_CURRENT_WAYPOINT,
MSG_VFR_HUD,
MSG_RADIO_OUT,
MSG_RADIO_IN,
MSG_RAW_IMU1,
MSG_RAW_IMU2,
MSG_RAW_IMU3,
MSG_GPS_RAW,
MSG_SYSTEM_TIME,
MSG_SERVO_OUT,
MSG_NEXT_WAYPOINT,
MSG_NEXT_PARAM,
MSG_STATUSTEXT,
MSG_LIMITS_STATUS,
MSG_FENCE_STATUS,
MSG_AHRS,
MSG_SIMSTATE,
MSG_HWSTATUS,
MSG_WIND,
MSG_RANGEFINDER,
MSG_RETRY_DEFERRED // this must be last
};
///
/// @class GCS
/// @brief Class describing the interface between the APM code
@ -88,6 +126,7 @@ protected:
AP_HAL::UARTDriver * _port;
};
//
// GCS class definitions.
//
@ -188,8 +227,7 @@ private:
uint16_t waypoint_count;
uint32_t waypoint_timelast_receive; // milliseconds
uint32_t waypoint_timelast_request; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
const uint16_t waypoint_receive_timeout; // milliseconds
// saveable rate of each stream
AP_Int16 streamRates[NUM_STREAMS];
@ -203,6 +241,38 @@ private:
// millis value to calculate cli timeout relative to.
// exists so we can separate the cli entry time from the system start time
uint32_t _cli_timeout;
uint8_t _log_listing:1; // sending log list
uint8_t _log_sending:1; // sending log data
// next log list entry to send
uint16_t _log_next_list_entry;
// last log list entry to send
uint16_t _log_last_list_entry;
// number of log files
uint16_t _log_num_logs;
// log number for data send
uint16_t _log_num_data;
// offset in log
uint32_t _log_data_offset;
// number of bytes left to send
uint32_t _log_data_remaining;
// start page of log data
uint16_t _log_data_page;
void handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash);
void handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash);
void handle_log_message(mavlink_message_t *msg, DataFlash_Class &dataflash);
void handle_log_send(DataFlash_Class &dataflash);
void handle_log_send_listing(DataFlash_Class &dataflash);
void handle_log_send_data(DataFlash_Class &dataflash);
};
#endif // __GCS_H

26
libraries/GCS_MAVLink/GCS_Common.cpp Normal file
View File

@ -0,0 +1,26 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
Common GCS MAVLink functions for all vehicle types
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <GCS.h>
GCS_MAVLINK::GCS_MAVLINK() :
waypoint_receive_timeout(1000)
{
AP_Param::setup_object_defaults(this, var_info);
}

172
libraries/GCS_MAVLink/GCS_Logs.cpp Normal file
View File

@ -0,0 +1,172 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
Common GCS MAVLink functions for all vehicle types
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <GCS.h>
#include <DataFlash.h>
#include <stdio.h>
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_message(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
handle_log_request_list(msg, dataflash);
break;
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
handle_log_request_data(msg, dataflash);
break;
}
}
/**
handle all types of log download requests from the GCS
*/
void GCS_MAVLINK::handle_log_request_list(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_list_t packet;
mavlink_msg_log_request_list_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = true;
_log_sending = false;
_log_num_logs = dataflash.get_num_logs();
int16_t last_log_num = dataflash.find_last_log();
_log_next_list_entry = packet.start;
_log_last_list_entry = packet.end;
if (_log_last_list_entry > last_log_num) {
_log_last_list_entry = last_log_num;
}
if (_log_next_list_entry < last_log_num + 1 - _log_num_logs) {
_log_next_list_entry = last_log_num + 1 - _log_num_logs;
}
}
/**
handle request for log data
*/
void GCS_MAVLINK::handle_log_request_data(mavlink_message_t *msg, DataFlash_Class &dataflash)
{
mavlink_log_request_data_t packet;
mavlink_msg_log_request_data_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component))
return;
_log_listing = false;
_log_sending = false;
uint16_t num_logs = dataflash.get_num_logs();
int16_t last_log_num = dataflash.find_last_log();
if (packet.id > last_log_num || packet.id < last_log_num + 1 - num_logs) {
return;
}
uint32_t time_utc, size;
dataflash.get_log_info(packet.id, size, time_utc);
uint16_t end;
dataflash.get_log_boundaries(packet.id, _log_data_page, end);
_log_num_data = packet.id;
_log_data_offset = packet.ofs;
if (_log_data_offset >= size) {
_log_data_remaining = 0;
} else {
_log_data_remaining = size - _log_data_offset;
}
_log_sending = true;
}
/**
trigger sending of log messages if there are some pending
*/
void GCS_MAVLINK::handle_log_send(DataFlash_Class &dataflash)
{
if (_log_listing) {
handle_log_send_listing(dataflash);
}
uint8_t num_sends = 5;
if (stream_slowdown != 0) {
// we're using a radio and starting to clag up, slowdown log send
num_sends = 1;
}
for (uint8_t i=0; i<num_sends; i++) {
if (_log_sending) {
handle_log_send_data(dataflash);
}
}
}
/**
trigger sending of log messages if there are some pending
*/
void GCS_MAVLINK::handle_log_send_listing(DataFlash_Class &dataflash)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space < MAVLINK_MSG_ID_LOG_ENTRY_LEN) {
// no space
return;
}
uint32_t size, time_utc;
dataflash.get_log_info(_log_next_list_entry, size, time_utc);
mavlink_msg_log_entry_send(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;
} else {
_log_next_list_entry++;
}
}
/**
trigger sending of log data if there are some pending
*/
void GCS_MAVLINK::handle_log_send_data(DataFlash_Class &dataflash)
{
int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
if (payload_space < MAVLINK_MSG_ID_LOG_DATA_LEN) {
// no space
return;
}
int16_t ret = 0;
uint32_t len = _log_data_remaining;
uint8_t data[90];
if (len > 90) {
len = 90;
}
ret = dataflash.get_log_data(_log_num_data, _log_data_page, _log_data_offset, len, data);
if (ret < 0) {
// report as EOF on error
ret = 0;
}
mavlink_msg_log_data_send(chan, _log_num_data, _log_data_offset, ret, data);
_log_data_offset += len;
_log_data_remaining -= len;
if (len < 90) {
_log_sending = false;
}
}