mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 16:23:56 -04:00
DataFlash: DataFlash-over-MAVLink support
This commit is contained in:
Peter Barker
Andrew Tridgell
parent
31b8432545
commit
0ca07e5245
@ -5,8 +5,8 @@
|
||||
const AP_Param::GroupInfo DataFlash_Class::var_info[] = {
|
||||
// @Param: _BACKEND_TYPES
|
||||
// @DisplayName: DataFlash Backend Storage type
|
||||
// @Description: 0 for None, 1 for File
|
||||
// @Values: 0:None,1:File
|
||||
// @Description: 0 for None, 1 for File, 2 for dataflash mavlink, 3 for both file and dataflash
|
||||
// @Values: 0:None,1:File,2:MAVLink,3:BothFileAndMAVLink
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("_BACKEND_TYPE", 0, DataFlash_Class, _params.backend_types, DATAFLASH_BACKEND_FILE),
|
||||
AP_GROUPEND
|
||||
@ -158,6 +158,13 @@ void DataFlash_Class::EnableWrites(bool enable) {
|
||||
FOR_EACH_BACKEND(EnableWrites(enable));
|
||||
}
|
||||
|
||||
// for DataFlash_MAVLink
|
||||
void DataFlash_Class::remote_log_block_status_msg(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg) {
|
||||
FOR_EACH_BACKEND(remote_log_block_status_msg(chan, msg));
|
||||
}
|
||||
// end for DataFlash_MAVLink
|
||||
|
||||
void DataFlash_Class::periodic_tasks() {
|
||||
FOR_EACH_BACKEND(periodic_tasks());
|
||||
}
|
||||
|
||||
@ -34,6 +34,8 @@ class DataFlash_Backend;
|
||||
enum DataFlash_Backend_Type {
|
||||
DATAFLASH_BACKEND_NONE = 0,
|
||||
DATAFLASH_BACKEND_FILE = 1,
|
||||
DATAFLASH_BACKEND_MAVLINK = 2,
|
||||
DATAFLASH_BACKEND_BOTH = 3,
|
||||
};
|
||||
|
||||
class DataFlash_Class
|
||||
@ -143,6 +145,11 @@ public:
|
||||
void flush(void);
|
||||
#endif
|
||||
|
||||
// for DataFlash_MAVLink:
|
||||
void remote_log_block_status_msg(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg);
|
||||
// end for DataFlash_MAVLink:
|
||||
|
||||
void periodic_tasks(); // may want to split this into GCS/non-GCS duties
|
||||
|
||||
// this is out here for the trickle-startup-messages logging.
|
||||
|
||||
@ -76,6 +76,11 @@ public:
|
||||
virtual void flush(void) { }
|
||||
#endif
|
||||
|
||||
// for Dataflash_MAVlink
|
||||
virtual void remote_log_block_status_msg(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg) { }
|
||||
// end for Dataflash_MAVlink
|
||||
|
||||
virtual void periodic_tasks();
|
||||
|
||||
uint8_t num_types() const;
|
||||
|
||||
574
libraries/DataFlash/DataFlash_MAVLink.cpp
Normal file
574
libraries/DataFlash/DataFlash_MAVLink.cpp
Normal file
@ -0,0 +1,574 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
DataFlash Remote(via MAVLink) logging
|
||||
*/
|
||||
|
||||
#include "DataFlash_MAVLink.h"
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
|
||||
#include "LogStructure.h"
|
||||
|
||||
#define REMOTE_LOG_DEBUGGING 0
|
||||
|
||||
#if REMOTE_LOG_DEBUGGING
|
||||
#include <stdio.h>
|
||||
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#else
|
||||
# define Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
// initialisation
|
||||
void DataFlash_MAVLink::Init()
|
||||
{
|
||||
DataFlash_Backend::Init();
|
||||
|
||||
_blocks = NULL;
|
||||
while (_blockcount >= 8) { // 8 is a *magic* number
|
||||
_blocks = (struct dm_block *) malloc(_blockcount * sizeof(_blocks[0]));
|
||||
if (_blocks != NULL) {
|
||||
break;
|
||||
}
|
||||
_blockcount /= 2;
|
||||
}
|
||||
|
||||
if (_blocks == NULL) {
|
||||
return;
|
||||
}
|
||||
|
||||
free_all_blocks();
|
||||
stats_init();
|
||||
|
||||
_initialised = true;
|
||||
_logging_started = true; // in actual fact, we throw away
|
||||
// everything until a client connects.
|
||||
// This stops calls to start_new_log from
|
||||
// the vehicles
|
||||
}
|
||||
|
||||
uint16_t DataFlash_MAVLink::bufferspace_available() {
|
||||
return (_blockcount_free * 200 + remaining_space_in_current_block());
|
||||
}
|
||||
|
||||
uint8_t DataFlash_MAVLink::remaining_space_in_current_block() {
|
||||
// note that _current_block *could* be NULL ATM.
|
||||
return (MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN - _latest_block_len);
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::enqueue_block(dm_block_queue_t &queue, struct dm_block *block)
|
||||
{
|
||||
if (queue.youngest != NULL) {
|
||||
queue.youngest->next = block;
|
||||
} else {
|
||||
queue.oldest = block;
|
||||
}
|
||||
queue.youngest = block;
|
||||
}
|
||||
|
||||
struct DataFlash_MAVLink::dm_block *DataFlash_MAVLink::dequeue_seqno(DataFlash_MAVLink::dm_block_queue_t &queue, uint32_t seqno)
|
||||
{
|
||||
struct dm_block *prev = NULL;
|
||||
for (struct dm_block *block=queue.oldest; block != NULL; block=block->next) {
|
||||
if (block->seqno == seqno) {
|
||||
if (prev == NULL) {
|
||||
if (queue.youngest == queue.oldest) {
|
||||
queue.oldest = NULL;
|
||||
queue.youngest = NULL;
|
||||
} else {
|
||||
queue.oldest = block->next;
|
||||
}
|
||||
} else {
|
||||
if (queue.youngest == block) {
|
||||
queue.youngest = prev;
|
||||
}
|
||||
prev->next = block->next;
|
||||
}
|
||||
block->next = NULL;
|
||||
return block;
|
||||
}
|
||||
prev = block;
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
bool DataFlash_MAVLink::free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue)
|
||||
{
|
||||
struct dm_block *block = dequeue_seqno(queue, seqno);
|
||||
if (block != NULL) {
|
||||
block->next = _blocks_free;
|
||||
_blocks_free = block;
|
||||
_blockcount_free++; // comment me out to expose a bug!
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Write a block of data at current offset */
|
||||
|
||||
// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
|
||||
bool DataFlash_MAVLink::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
|
||||
{
|
||||
if (!_initialised || !_sending_to_client || !_writes_enabled) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (! WriteBlockCheckStartupMessages()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (bufferspace_available() < size) {
|
||||
if (_startup_messagewriter->finished()) {
|
||||
// do not count the startup packets as being dropped...
|
||||
dropped++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t copied = 0;
|
||||
|
||||
while (copied < size) {
|
||||
if (_current_block == NULL) {
|
||||
_current_block = next_block();
|
||||
if (_current_block == NULL) {
|
||||
// should not happen - there's a sanity check above
|
||||
internal_error();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
uint16_t remaining_to_copy = size - copied;
|
||||
uint16_t _curr_remaining = remaining_space_in_current_block();
|
||||
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
|
||||
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
|
||||
copied += to_copy;
|
||||
_latest_block_len += to_copy;
|
||||
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
|
||||
//block full, mark it to be sent:
|
||||
enqueue_block(_blocks_pending, _current_block);
|
||||
_current_block = next_block();
|
||||
}
|
||||
}
|
||||
|
||||
if (!_writing_startup_messages) {
|
||||
// push_log_blocks();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
//Get a free block
|
||||
struct DataFlash_MAVLink::dm_block *DataFlash_MAVLink::next_block()
|
||||
{
|
||||
DataFlash_MAVLink::dm_block *ret = _blocks_free;
|
||||
if (ret != NULL) {
|
||||
_blocks_free = ret->next;
|
||||
_blockcount_free--;
|
||||
ret->seqno = _next_seq_num++;
|
||||
ret->last_sent = 0;
|
||||
ret->next = NULL;
|
||||
_latest_block_len = 0;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::free_all_blocks()
|
||||
{
|
||||
_blocks_free = NULL;
|
||||
_current_block = NULL;
|
||||
|
||||
_blocks_pending.sent_count = 0;
|
||||
_blocks_pending.oldest = _blocks_pending.youngest = NULL;
|
||||
_blocks_retry.sent_count = 0;
|
||||
_blocks_retry.oldest = _blocks_retry.youngest = NULL;
|
||||
_blocks_sent.sent_count = 0;
|
||||
_blocks_sent.oldest = _blocks_sent.youngest = NULL;
|
||||
|
||||
// add blocks to the free stack:
|
||||
for(uint8_t i=0; i < _blockcount; i++) {
|
||||
_blocks[i].next = _blocks_free;
|
||||
_blocks_free = &_blocks[i];
|
||||
// this value doesn't really matter, but it stops valgrind
|
||||
// complaining when acking blocks (we check seqno before
|
||||
// state). Also, when we receive ACKs we check seqno, and we
|
||||
// want to ack the *real* block zero!
|
||||
_blocks[i].seqno = 9876543;
|
||||
}
|
||||
_blockcount_free = _blockcount;
|
||||
|
||||
_latest_block_len = 0;
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::handle_ack(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg,
|
||||
uint32_t seqno)
|
||||
{
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_STOP) {
|
||||
Debug("Received stop-logging packet");
|
||||
if (_sending_to_client) {
|
||||
_sending_to_client = false;
|
||||
_last_response_time = AP_HAL::millis();
|
||||
}
|
||||
return;
|
||||
}
|
||||
if(seqno == MAV_REMOTE_LOG_DATA_BLOCK_START) {
|
||||
if (!_sending_to_client) {
|
||||
Debug("Starting New Log");
|
||||
free_all_blocks();
|
||||
// _current_block = next_block();
|
||||
// if (_current_block == NULL) {
|
||||
// Debug("No free blocks?!!!\n");
|
||||
// return;
|
||||
// }
|
||||
stats_init();
|
||||
_sending_to_client = true;
|
||||
_target_system_id = msg->sysid;
|
||||
_target_component_id = msg->compid;
|
||||
_chan = chan;
|
||||
_next_seq_num = 0;
|
||||
_startup_messagewriter->reset();
|
||||
_last_response_time = AP_HAL::millis();
|
||||
Debug("Target: (%u/%u)", _target_system_id, _target_component_id);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// check SENT blocks (VERY likely to be first on the list):
|
||||
if (free_seqno_from_queue(seqno, _blocks_sent)) {
|
||||
// celebrate
|
||||
_last_response_time = AP_HAL::millis();
|
||||
} else if(free_seqno_from_queue(seqno, _blocks_retry)) {
|
||||
// party
|
||||
_last_response_time = AP_HAL::millis();
|
||||
} else {
|
||||
// probably acked already and put on the free list.
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg)
|
||||
{
|
||||
mavlink_remote_log_block_status_t packet;
|
||||
mavlink_msg_remote_log_block_status_decode(msg, &packet);
|
||||
if(packet.status == 0){
|
||||
handle_retry(packet.seqno);
|
||||
} else{
|
||||
handle_ack(chan, msg, packet.seqno);
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::handle_retry(uint32_t seqno)
|
||||
{
|
||||
if (!_initialised || !_sending_to_client) {
|
||||
return;
|
||||
}
|
||||
|
||||
struct dm_block *victim = dequeue_seqno(_blocks_sent, seqno);
|
||||
if (victim != NULL) {
|
||||
_last_response_time = AP_HAL::millis();
|
||||
enqueue_block(_blocks_retry, victim);
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::set_channel(mavlink_channel_t chan)
|
||||
{
|
||||
_chan = chan;
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::internal_error() {
|
||||
internal_errors++;
|
||||
DataFlash_Backend::internal_error();
|
||||
}
|
||||
void DataFlash_MAVLink::stats_init() {
|
||||
dropped = 0;
|
||||
internal_errors = 0;
|
||||
stats.resends = 0;
|
||||
stats_reset();
|
||||
}
|
||||
void DataFlash_MAVLink::stats_reset() {
|
||||
stats.state_free = 0;
|
||||
stats.state_free_min = -1; // unsigned wrap
|
||||
stats.state_free_max = 0;
|
||||
stats.state_pending = 0;
|
||||
stats.state_pending_min = -1; // unsigned wrap
|
||||
stats.state_pending_max = 0;
|
||||
stats.state_retry = 0;
|
||||
stats.state_retry_min = -1; // unsigned wrap
|
||||
stats.state_retry_max = 0;
|
||||
stats.state_sent = 0;
|
||||
stats.state_sent_min = -1; // unsigned wrap
|
||||
stats.state_sent_max = 0;
|
||||
stats.collection_count = 0;
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::Log_Write_DF_MAV(DataFlash_MAVLink &df)
|
||||
{
|
||||
if (df.stats.collection_count == 0) {
|
||||
return;
|
||||
}
|
||||
struct log_DF_MAV_Stats pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_DF_MAV_STATS),
|
||||
timestamp : AP_HAL::millis(),
|
||||
seqno : df._next_seq_num-1,
|
||||
dropped : df.dropped,
|
||||
retries : df._blocks_retry.sent_count,
|
||||
resends : df.stats.resends,
|
||||
internal_errors : df.internal_errors,
|
||||
state_free_avg : (uint8_t)(df.stats.state_free/df.stats.collection_count),
|
||||
state_free_min : df.stats.state_free_min,
|
||||
state_free_max : df.stats.state_free_max,
|
||||
state_pending_avg : (uint8_t)(df.stats.state_pending/df.stats.collection_count),
|
||||
state_pending_min : df.stats.state_pending_min,
|
||||
state_pending_max : df.stats.state_pending_max,
|
||||
state_sent_avg : (uint8_t)(df.stats.state_sent/df.stats.collection_count),
|
||||
state_sent_min : df.stats.state_sent_min,
|
||||
state_sent_max : df.stats.state_sent_max,
|
||||
// state_retry_avg : (uint8_t)(df.stats.state_retry/df.stats.collection_count),
|
||||
// state_retry_min : df.stats.state_retry_min,
|
||||
// state_retry_max : df.stats.state_retry_max
|
||||
};
|
||||
WriteBlock(&pkt,sizeof(pkt));
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::stats_log()
|
||||
{
|
||||
if (!_initialised || !_logging_started) {
|
||||
return;
|
||||
}
|
||||
if (stats.collection_count == 0) {
|
||||
return;
|
||||
}
|
||||
Log_Write_DF_MAV(*this);
|
||||
#if REMOTE_LOG_DEBUGGING
|
||||
printf("D:%d Retry:%d Resent:%d E:%d SF:%d/%d/%d SP:%d/%d/%d SS:%d/%d/%d SR:%d/%d/%d\n",
|
||||
dropped,
|
||||
_blocks_retry.sent_count,
|
||||
stats.resends,
|
||||
internal_errors,
|
||||
stats.state_free_min,
|
||||
stats.state_free_max,
|
||||
stats.state_free/stats.collection_count,
|
||||
stats.state_pending_min,
|
||||
stats.state_pending_max,
|
||||
stats.state_pending/stats.collection_count,
|
||||
stats.state_sent_min,
|
||||
stats.state_sent_max,
|
||||
stats.state_sent/stats.collection_count,
|
||||
stats.state_retry_min,
|
||||
stats.state_retry_max,
|
||||
stats.state_retry/stats.collection_count
|
||||
);
|
||||
#endif
|
||||
stats_reset();
|
||||
}
|
||||
|
||||
uint8_t DataFlash_MAVLink::stack_size(struct dm_block *stack)
|
||||
{
|
||||
uint8_t ret = 0;
|
||||
for (struct dm_block *block=stack; block != NULL; block=block->next) {
|
||||
ret++;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
uint8_t DataFlash_MAVLink::queue_size(dm_block_queue_t queue)
|
||||
{
|
||||
return stack_size(queue.oldest);
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::stats_collect()
|
||||
{
|
||||
if (!_initialised || !_logging_started) {
|
||||
return;
|
||||
}
|
||||
uint8_t pending = queue_size(_blocks_pending);
|
||||
uint8_t sent = queue_size(_blocks_sent);
|
||||
uint8_t retry = queue_size(_blocks_retry);
|
||||
uint8_t sfree = stack_size(_blocks_free);
|
||||
|
||||
if (sfree != _blockcount_free) {
|
||||
internal_error();
|
||||
}
|
||||
stats.state_pending += pending;
|
||||
stats.state_sent += sent;
|
||||
stats.state_free += sfree;
|
||||
stats.state_retry += retry;
|
||||
|
||||
if (pending < stats.state_pending_min) {
|
||||
stats.state_pending_min = pending;
|
||||
}
|
||||
if (pending > stats.state_pending_max) {
|
||||
stats.state_pending_max = pending;
|
||||
}
|
||||
if (retry < stats.state_retry_min) {
|
||||
stats.state_retry_min = retry;
|
||||
}
|
||||
if (retry > stats.state_retry_max) {
|
||||
stats.state_retry_max = retry;
|
||||
}
|
||||
if (sent < stats.state_sent_min) {
|
||||
stats.state_sent_min = sent;
|
||||
}
|
||||
if (sent > stats.state_sent_max) {
|
||||
stats.state_sent_max = sent;
|
||||
}
|
||||
if (sfree < stats.state_free_min) {
|
||||
stats.state_free_min = sfree;
|
||||
}
|
||||
if (sfree > stats.state_free_max) {
|
||||
stats.state_free_max = sfree;
|
||||
}
|
||||
|
||||
stats.collection_count++;
|
||||
}
|
||||
|
||||
/* while we "successfully" send log blocks from a queue, move them to
|
||||
* the sent list. DO NOT call this for blocks already sent!
|
||||
*/
|
||||
bool DataFlash_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
|
||||
{
|
||||
uint8_t sent_count = 0;
|
||||
while (queue.oldest != NULL) {
|
||||
if (sent_count++ > _max_blocks_per_send_blocks) {
|
||||
return false;
|
||||
}
|
||||
if (! send_log_block(*queue.oldest)) {
|
||||
return false;
|
||||
}
|
||||
queue.sent_count++;
|
||||
struct DataFlash_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
|
||||
if (tmp != NULL) { // should never be NULL
|
||||
enqueue_block(_blocks_sent, tmp);
|
||||
} else {
|
||||
internal_error();
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::push_log_blocks()
|
||||
{
|
||||
if (!_initialised || !_logging_started ||!_sending_to_client) {
|
||||
return;
|
||||
}
|
||||
|
||||
DataFlash_Backend::WriteMoreStartupMessages();
|
||||
|
||||
if (! send_log_blocks_from_queue(_blocks_retry)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (! send_log_blocks_from_queue(_blocks_pending)) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::do_resends(uint32_t now)
|
||||
{
|
||||
if (!_initialised || !_logging_started ||!_sending_to_client) {
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t count_to_send = 5;
|
||||
if (_blockcount < count_to_send) {
|
||||
count_to_send = _blockcount;
|
||||
}
|
||||
uint32_t oldest = now - 100; // 100 milliseconds before resend. Hmm.
|
||||
while (count_to_send-- > 0) {
|
||||
for (struct dm_block *block=_blocks_sent.oldest; block != NULL; block=block->next) {
|
||||
// only want to send blocks every now-and-then:
|
||||
if (block->last_sent < oldest) {
|
||||
if (! send_log_block(*block)) {
|
||||
// failed to send the block; try again later....
|
||||
return;
|
||||
}
|
||||
stats.resends++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::periodic_10Hz(const uint32_t now)
|
||||
{
|
||||
do_resends(now);
|
||||
stats_collect();
|
||||
}
|
||||
void DataFlash_MAVLink::periodic_1Hz(const uint32_t now)
|
||||
{
|
||||
if (_sending_to_client &&
|
||||
_last_response_time + 10000 < _last_send_time) {
|
||||
// other end appears to have timed out!
|
||||
Debug("Client timed out");
|
||||
_sending_to_client = false;
|
||||
return;
|
||||
}
|
||||
stats_log();
|
||||
}
|
||||
|
||||
void DataFlash_MAVLink::periodic_fullrate(uint32_t now)
|
||||
{
|
||||
push_log_blocks();
|
||||
}
|
||||
|
||||
//TODO: handle full txspace properly
|
||||
bool DataFlash_MAVLink::send_log_block(struct dm_block &block)
|
||||
{
|
||||
mavlink_channel_t chan = mavlink_channel_t(_chan - MAVLINK_COMM_0);
|
||||
if (!_initialised) {
|
||||
return false;
|
||||
}
|
||||
if (comm_get_txspace(chan) < MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK_LEN) {
|
||||
return false;
|
||||
}
|
||||
if (comm_get_txspace(chan) < 500) {
|
||||
return false;
|
||||
}
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
if (rand() < 0.1) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DF_MAVLINK_DISABLE_INTERRUPTS
|
||||
irqstate_t istate = irqsave();
|
||||
#endif
|
||||
|
||||
// DM_packing: 267039 events, 0 overruns, 8440834us elapsed, 31us avg, min 31us max 32us 0.488us rms
|
||||
hal.util->perf_begin(_perf_packing);
|
||||
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t *chan_status = mavlink_get_channel_status(chan);
|
||||
uint8_t saved_seq = chan_status->current_tx_seq;
|
||||
chan_status->current_tx_seq = mavlink_seq++;
|
||||
// Debug("Sending block (%d)", block.seqno);
|
||||
mavlink_msg_remote_log_data_block_pack(mavlink_system.sysid,
|
||||
MAV_COMP_ID_LOG,
|
||||
&msg,
|
||||
_target_system_id,
|
||||
_target_component_id,
|
||||
block.seqno,
|
||||
block.buf);
|
||||
|
||||
hal.util->perf_end(_perf_packing);
|
||||
|
||||
#if DF_MAVLINK_DISABLE_INTERRUPTS
|
||||
irqrestore(istate);
|
||||
#endif
|
||||
|
||||
block.last_sent = AP_HAL::millis();
|
||||
chan_status->current_tx_seq = saved_seq;
|
||||
|
||||
// _last_send_time is set even if we fail to send the packet; if
|
||||
// the txspace is repeatedly chockas we should not add to the
|
||||
// problem and stop attempting to log
|
||||
_last_send_time = AP_HAL::millis();
|
||||
|
||||
_mavlink_resend_uart(chan, &msg);
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
197
libraries/DataFlash/DataFlash_MAVLink.h
Normal file
197
libraries/DataFlash/DataFlash_MAVLink.h
Normal file
@ -0,0 +1,197 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
DataFlash logging - MAVLink variant
|
||||
|
||||
- transfers blocks of the open log file to a client using MAVLink
|
||||
*/
|
||||
|
||||
#ifndef DATAFLASH_MAVLINK_H
|
||||
#define DATAFLASH_MAVLINK_H
|
||||
|
||||
#define DATAFLASH_MAVLINK_SUPPORT 1
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "DataFlash_Backend.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define DF_MAVLINK_DISABLE_INTERRUPTS 0
|
||||
|
||||
class DataFlash_MAVLink : public DataFlash_Backend
|
||||
{
|
||||
friend class DataFlash_Class; // for access to stats on Log_Df_Mav_Stats
|
||||
public:
|
||||
// constructor
|
||||
DataFlash_MAVLink(DataFlash_Class &front, DFMessageWriter_DFLogStart *writer) :
|
||||
DataFlash_Backend(front, writer),
|
||||
_max_blocks_per_send_blocks(8),
|
||||
_blockcount(32) // this may get reduced in Init if allocation fails
|
||||
,_perf_packing(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "DM_packing"))
|
||||
{ }
|
||||
|
||||
// initialisation
|
||||
void Init() override;
|
||||
|
||||
bool logging_started() { return _logging_started; }
|
||||
|
||||
/* Write a block of data at current offset */
|
||||
bool WritePrioritisedBlock(const void *pBuffer, uint16_t size,
|
||||
bool is_critical) override;
|
||||
|
||||
// initialisation
|
||||
bool CardInserted(void) override { return true; }
|
||||
|
||||
// erase handling
|
||||
void EraseAll() override {}
|
||||
|
||||
bool NeedPrep() override { return false; }
|
||||
void Prep() override { }
|
||||
|
||||
// high level interface
|
||||
uint16_t find_last_log(void) override { return 0; }
|
||||
void get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page) override {}
|
||||
void get_log_info(uint16_t log_num, uint32_t &size, uint32_t &time_utc) override {}
|
||||
int16_t get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data) override { return 0; }
|
||||
uint16_t get_num_logs(void) override { return 0; }
|
||||
|
||||
void LogReadProcess(uint16_t log_num,
|
||||
uint16_t start_page, uint16_t end_page,
|
||||
print_mode_fn printMode,
|
||||
AP_HAL::BetterStream *port) override {}
|
||||
void DumpPageInfo(AP_HAL::BetterStream *port) override {}
|
||||
void ShowDeviceInfo(AP_HAL::BetterStream *port) override {}
|
||||
void ListAvailableLogs(AP_HAL::BetterStream *port) override {}
|
||||
|
||||
// enum dm_block_state {
|
||||
// BLOCK_STATE_FREE = 17,
|
||||
// BLOCK_STATE_FILLING,
|
||||
// BLOCK_STATE_SEND_PENDING,
|
||||
// BLOCK_STATE_SEND_RETRY,
|
||||
// BLOCK_STATE_SENT
|
||||
// };
|
||||
struct dm_block {
|
||||
uint32_t seqno;
|
||||
uint8_t buf[MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN];
|
||||
uint32_t last_sent;
|
||||
struct dm_block *next;
|
||||
};
|
||||
void push_log_blocks();
|
||||
virtual bool send_log_block(struct dm_block &block);
|
||||
virtual void handle_ack(mavlink_channel_t chan, mavlink_message_t* msg, uint32_t seqno);
|
||||
virtual void handle_retry(uint32_t block_num);
|
||||
void do_resends(uint32_t now);
|
||||
virtual void set_channel(mavlink_channel_t chan);
|
||||
virtual void remote_log_block_status_msg(mavlink_channel_t chan, mavlink_message_t* msg) override;
|
||||
void free_all_blocks();
|
||||
|
||||
// a stack for free blocks, queues for pending, sent, retries and sent
|
||||
struct dm_block_queue {
|
||||
uint32_t sent_count;
|
||||
struct dm_block *oldest;
|
||||
struct dm_block *youngest;
|
||||
};
|
||||
typedef struct dm_block_queue dm_block_queue_t ;
|
||||
void enqueue_block(dm_block_queue_t &queue, struct dm_block *block);
|
||||
bool queue_has_block(dm_block_queue_t &queue, struct dm_block *block);
|
||||
struct dm_block *dequeue_seqno(dm_block_queue_t &queue, uint32_t seqno);
|
||||
bool free_seqno_from_queue(uint32_t seqno, dm_block_queue_t &queue);
|
||||
bool send_log_blocks_from_queue(dm_block_queue_t &queue);
|
||||
uint8_t stack_size(struct dm_block *stack);
|
||||
uint8_t queue_size(dm_block_queue_t queue);
|
||||
|
||||
struct dm_block *_blocks_free;
|
||||
dm_block_queue_t _blocks_sent;
|
||||
dm_block_queue_t _blocks_pending;
|
||||
dm_block_queue_t _blocks_retry;
|
||||
|
||||
protected:
|
||||
struct _stats {
|
||||
// the following are reset any time we log stats (see "reset_stats")
|
||||
uint32_t resends;
|
||||
uint8_t collection_count;
|
||||
uint16_t state_free; // cumulative across collection period
|
||||
uint8_t state_free_min;
|
||||
uint8_t state_free_max;
|
||||
uint16_t state_pending; // cumulative across collection period
|
||||
uint8_t state_pending_min;
|
||||
uint8_t state_pending_max;
|
||||
uint16_t state_retry; // cumulative across collection period
|
||||
uint8_t state_retry_min;
|
||||
uint8_t state_retry_max;
|
||||
uint16_t state_sent; // cumulative across collection period
|
||||
uint8_t state_sent_min;
|
||||
uint8_t state_sent_max;
|
||||
} stats;
|
||||
|
||||
private:
|
||||
mavlink_channel_t _chan;
|
||||
uint8_t _target_system_id;
|
||||
uint8_t _target_component_id;
|
||||
|
||||
bool _initialised;
|
||||
|
||||
// this controls the maximum number of blocks we will push from
|
||||
// the pending and send queues in any call to push_log_blocks.
|
||||
// push_log_blocks is called by periodic_tasks. Each block is 200
|
||||
// bytes. In Plane, at 50Hz, a _max_blocks_per_send_blocks of 2
|
||||
// means we will push at most 2*50*200 == 20KB of logs per second
|
||||
// _max_blocks_per_send_blocks has to be high enough to push all
|
||||
// of the logs, but low enough that we don't spend way too much
|
||||
// time packing messages in any one loop
|
||||
const uint8_t _max_blocks_per_send_blocks;
|
||||
|
||||
uint32_t _next_seq_num;
|
||||
uint16_t _latest_block_len;
|
||||
bool _logging_started;
|
||||
uint32_t _last_response_time;
|
||||
uint32_t _last_send_time;
|
||||
uint8_t _next_block_number_to_resend;
|
||||
bool _sending_to_client;
|
||||
|
||||
void Log_Write_DF_MAV(DataFlash_MAVLink &df);
|
||||
|
||||
void internal_error();
|
||||
uint16_t bufferspace_available() override; // in bytes
|
||||
uint8_t remaining_space_in_current_block();
|
||||
// write buffer
|
||||
uint8_t _blockcount_free;
|
||||
uint8_t _blockcount;
|
||||
struct dm_block *_blocks;
|
||||
struct dm_block *_current_block;
|
||||
struct dm_block *next_block();
|
||||
|
||||
void periodic_10Hz(uint32_t now);
|
||||
void periodic_1Hz(uint32_t now);
|
||||
void periodic_fullrate(uint32_t now);
|
||||
|
||||
void stats_init();
|
||||
void stats_reset();
|
||||
void stats_collect();
|
||||
void stats_log();
|
||||
uint32_t _stats_last_collected_time;
|
||||
uint32_t _stats_last_logged_time;
|
||||
uint8_t mavlink_seq;
|
||||
|
||||
/* we currently ignore requests to start a new log. Notionally we
|
||||
* could close the currently logging session and hope the client
|
||||
* re-opens one */
|
||||
uint16_t start_new_log(void) override {
|
||||
return 0;
|
||||
}
|
||||
bool ReadBlock(void *pkt, uint16_t size) override {
|
||||
return false;
|
||||
}
|
||||
// performance counters
|
||||
AP_HAL::Util::perf_counter_t _perf_errors;
|
||||
AP_HAL::Util::perf_counter_t _perf_packing;
|
||||
AP_HAL::Util::perf_counter_t _perf_overruns;
|
||||
};
|
||||
|
||||
#endif // DATAFLASH_MAVLINK_SUPPORT
|
||||
|
||||
#endif // DATAFLASH_MAVLINK_H
|
||||
|
||||
@ -15,6 +15,7 @@
|
||||
#include "DataFlash_SITL.h"
|
||||
#include "DataFlash_Block.h"
|
||||
#include "DataFlash_File.h"
|
||||
#include "DataFlash_MAVLink.h"
|
||||
#include "DFMessageWriter.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
@ -22,7 +23,7 @@ extern const AP_HAL::HAL& hal;
|
||||
void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_types)
|
||||
{
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
hal.scheduler->panic("Too many backends");
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
_num_types = num_types;
|
||||
@ -47,6 +48,27 @@ void DataFlash_Class::Init(const struct LogStructure *structures, uint8_t num_ty
|
||||
}
|
||||
#endif
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
if (_params.backend_types == DATAFLASH_BACKEND_MAVLINK ||
|
||||
_params.backend_types == DATAFLASH_BACKEND_BOTH) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
DFMessageWriter_DFLogStart *message_writer =
|
||||
new DFMessageWriter_DFLogStart(_firmware_string);
|
||||
if (message_writer != NULL) {
|
||||
backends[_next_backend] = new DataFlash_MAVLink(*this,
|
||||
message_writer);
|
||||
}
|
||||
if (backends[_next_backend] == NULL) {
|
||||
hal.console->printf("Unable to open DataFlash_MAVLink");
|
||||
} else {
|
||||
_next_backend++;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
for (uint8_t i=0; i<_next_backend; i++) {
|
||||
backends[i]->Init();
|
||||
}
|
||||
|
||||
@ -550,6 +550,28 @@ struct PACKED log_GYRO {
|
||||
float GyrX, GyrY, GyrZ;
|
||||
};
|
||||
|
||||
struct PACKED log_DF_MAV_Stats {
|
||||
LOG_PACKET_HEADER;
|
||||
uint32_t timestamp;
|
||||
uint32_t seqno;
|
||||
uint32_t dropped;
|
||||
uint32_t retries;
|
||||
uint32_t resends;
|
||||
uint8_t internal_errors; // uint8_t - wishful thinking?
|
||||
uint8_t state_free_avg;
|
||||
uint8_t state_free_min;
|
||||
uint8_t state_free_max;
|
||||
uint8_t state_pending_avg;
|
||||
uint8_t state_pending_min;
|
||||
uint8_t state_pending_max;
|
||||
uint8_t state_sent_avg;
|
||||
uint8_t state_sent_min;
|
||||
uint8_t state_sent_max;
|
||||
// uint8_t state_retry_avg;
|
||||
// uint8_t state_retry_min;
|
||||
// uint8_t state_retry_max;
|
||||
};
|
||||
|
||||
struct PACKED log_ORGN {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
@ -634,7 +656,9 @@ Format characters in the format string for binary log messages
|
||||
{ LOG_MODE_MSG, sizeof(log_Mode), \
|
||||
"MODE", "QMB", "TimeUS,Mode,ModeNum" }, \
|
||||
{ LOG_RFND_MSG, sizeof(log_RFND), \
|
||||
"RFND", "QCC", "TimeUS,Dist1,Dist2" }
|
||||
"RFND", "QCC", "TimeUS,Dist1,Dist2" }, \
|
||||
{ LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \
|
||||
"DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }
|
||||
|
||||
// messages for more advanced boards
|
||||
#define LOG_EXTRA_STRUCTURES \
|
||||
@ -839,6 +863,7 @@ enum LogMessages {
|
||||
LOG_NKF7_MSG,
|
||||
LOG_NKF8_MSG,
|
||||
LOG_NKF9_MSG,
|
||||
LOG_DF_MAV_STATS,
|
||||
};
|
||||
|
||||
enum LogOriginType {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user