2015-08-06 09:18:28 -03:00
|
|
|
#include "DataFlash_Backend.h"
|
|
|
|
|
|
|
|
#include "DFMessageWriter.h"
|
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2015-11-09 18:14:22 -04:00
|
|
|
DataFlash_Backend::DataFlash_Backend(DataFlash_Class &front,
|
|
|
|
class DFMessageWriter_DFLogStart *writer) :
|
|
|
|
_front(front),
|
|
|
|
_startup_messagewriter(writer)
|
|
|
|
{
|
|
|
|
writer->set_dataflash_backend(this);
|
|
|
|
}
|
|
|
|
|
|
|
|
uint8_t DataFlash_Backend::num_types() const
|
|
|
|
{
|
|
|
|
return _front._num_types;
|
|
|
|
}
|
|
|
|
|
|
|
|
const struct LogStructure *DataFlash_Backend::structure(uint8_t num) const
|
|
|
|
{
|
|
|
|
return _front.structure(num);
|
|
|
|
}
|
|
|
|
|
|
|
|
DataFlash_Backend::vehicle_startup_message_Log_Writer DataFlash_Backend::vehicle_message_writer() {
|
|
|
|
return _front._vehicle_messages;
|
|
|
|
}
|
|
|
|
|
2015-08-06 09:18:28 -03:00
|
|
|
void DataFlash_Backend::periodic_10Hz(const uint32_t now)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
void DataFlash_Backend::periodic_1Hz(const uint32_t now)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
void DataFlash_Backend::periodic_fullrate(const uint32_t now)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
void DataFlash_Backend::periodic_tasks()
|
|
|
|
{
|
2015-11-19 23:15:08 -04:00
|
|
|
uint32_t now = AP_HAL::millis();
|
2015-08-06 09:18:28 -03:00
|
|
|
if (now - _last_periodic_1Hz > 1000) {
|
|
|
|
periodic_1Hz(now);
|
|
|
|
_last_periodic_1Hz = now;
|
|
|
|
}
|
|
|
|
if (now - _last_periodic_10Hz > 100) {
|
|
|
|
periodic_10Hz(now);
|
|
|
|
_last_periodic_10Hz = now;
|
|
|
|
}
|
|
|
|
periodic_fullrate(now);
|
|
|
|
}
|
|
|
|
|
2016-04-15 06:53:54 -03:00
|
|
|
void DataFlash_Backend::start_new_log_reset_variables()
|
|
|
|
{
|
|
|
|
_startup_messagewriter->reset();
|
|
|
|
}
|
2015-08-06 09:18:28 -03:00
|
|
|
|
|
|
|
void DataFlash_Backend::internal_error() {
|
|
|
|
_internal_errors++;
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
|
|
abort();
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2015-11-09 18:14:22 -04:00
|
|
|
void DataFlash_Backend::set_mission(const AP_Mission *mission) {
|
|
|
|
_startup_messagewriter->set_mission(mission);
|
|
|
|
}
|
|
|
|
|
2015-08-06 09:18:28 -03:00
|
|
|
// this method can be overridden to do extra things with your buffer.
|
|
|
|
// for example, in DataFlash_MAVLink we may push messages into the UART.
|
|
|
|
void DataFlash_Backend::push_log_blocks() {
|
|
|
|
WriteMoreStartupMessages();
|
|
|
|
}
|
|
|
|
|
2015-09-17 07:28:50 -03:00
|
|
|
// returns true if all format messages have been written, and thus it is OK
|
|
|
|
// for other messages to go out to the log
|
2015-08-06 09:18:28 -03:00
|
|
|
bool DataFlash_Backend::WriteBlockCheckStartupMessages()
|
|
|
|
{
|
2015-11-09 18:14:22 -04:00
|
|
|
if (_startup_messagewriter->fmt_done()) {
|
2015-08-06 09:18:28 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (_writing_startup_messages) {
|
|
|
|
// we have been called by a messagewriter, so writing is OK
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// we're not writing startup messages, so this must be some random
|
|
|
|
// caller hoping to write blocks out. Push out log blocks - we
|
|
|
|
// might end up clearing the buffer.....
|
|
|
|
push_log_blocks();
|
2016-09-12 23:26:42 -03:00
|
|
|
|
|
|
|
// even if we did finish writing startup messages, we can't
|
|
|
|
// permit any message to go in as its timestamp will be before
|
|
|
|
// any we wrote in. Time going backwards annoys log readers.
|
2015-08-06 09:18:28 -03:00
|
|
|
|
|
|
|
// sorry! currently busy writing out startup messages...
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// source more messages from the startup message writer:
|
|
|
|
void DataFlash_Backend::WriteMoreStartupMessages()
|
|
|
|
{
|
|
|
|
|
2015-11-09 18:14:22 -04:00
|
|
|
if (_startup_messagewriter->finished()) {
|
2015-08-06 09:18:28 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
_writing_startup_messages = true;
|
2015-11-09 18:14:22 -04:00
|
|
|
_startup_messagewriter->process();
|
2015-08-06 09:18:28 -03:00
|
|
|
_writing_startup_messages = false;
|
|
|
|
}
|
2016-04-20 02:07:48 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
* support for Log_Write():
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
bool DataFlash_Backend::Log_Write_Emit_FMT(uint8_t msg_type)
|
|
|
|
{
|
|
|
|
// get log structure from front end:
|
|
|
|
struct LogStructure logstruct = {
|
|
|
|
// these will be overwritten, but need to keep the compiler happy:
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
"IGNO",
|
|
|
|
"",
|
|
|
|
""
|
|
|
|
};
|
|
|
|
if (!_front.fill_log_write_logstructure(logstruct, msg_type)) {
|
|
|
|
// this is a bug; we've been asked to write out the FMT
|
|
|
|
// message for a msg_type, but the frontend can't supply the
|
|
|
|
// required information
|
|
|
|
internal_error();
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!Log_Write_Format(&logstruct)) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool DataFlash_Backend::Log_Write(const uint8_t msg_type, va_list arg_list, bool is_critical)
|
|
|
|
{
|
|
|
|
// stack-allocate a buffer so we can WriteBlock(); this could be
|
|
|
|
// 255 bytes! If we were willing to lose the WriteBlock
|
|
|
|
// abstraction we could do WriteBytes() here instead?
|
|
|
|
const char *fmt = NULL;
|
|
|
|
uint8_t msg_len;
|
2016-05-04 06:06:23 -03:00
|
|
|
DataFlash_Class::log_write_fmt *f;
|
|
|
|
for (f = _front.log_write_fmts; f; f=f->next) {
|
|
|
|
if (f->msg_type == msg_type) {
|
|
|
|
fmt = f->fmt;
|
|
|
|
msg_len = f->msg_len;
|
2016-04-20 02:07:48 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (fmt == NULL) {
|
|
|
|
// this is a bug.
|
|
|
|
internal_error();
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (bufferspace_available() < msg_len) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
uint8_t buffer[msg_len];
|
|
|
|
uint8_t offset = 0;
|
|
|
|
buffer[offset++] = HEAD_BYTE1;
|
|
|
|
buffer[offset++] = HEAD_BYTE2;
|
|
|
|
buffer[offset++] = msg_type;
|
|
|
|
for (uint8_t i=0; i<strlen(fmt); i++) {
|
2016-04-26 09:32:48 -03:00
|
|
|
uint8_t charlen = 0;
|
2016-04-20 02:07:48 -03:00
|
|
|
switch(fmt[i]) {
|
|
|
|
case 'b': {
|
|
|
|
int8_t tmp = va_arg(arg_list, int);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(int8_t));
|
|
|
|
offset += sizeof(int8_t);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'h':
|
2016-04-20 02:07:48 -03:00
|
|
|
case 'c': {
|
|
|
|
int16_t tmp = va_arg(arg_list, int);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(int16_t));
|
|
|
|
offset += sizeof(int16_t);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case 'd': {
|
|
|
|
double tmp = va_arg(arg_list, double);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(double));
|
|
|
|
offset += sizeof(double);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'i':
|
|
|
|
case 'L':
|
2016-04-20 02:07:48 -03:00
|
|
|
case 'e': {
|
|
|
|
int32_t tmp = va_arg(arg_list, int);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
|
|
|
|
offset += sizeof(int32_t);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case 'f': {
|
|
|
|
float tmp = va_arg(arg_list, double);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(float));
|
|
|
|
offset += sizeof(float);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'n':
|
|
|
|
charlen = 4;
|
2016-04-20 02:07:48 -03:00
|
|
|
break;
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'M':
|
2016-04-20 02:07:48 -03:00
|
|
|
case 'B': {
|
|
|
|
uint8_t tmp = va_arg(arg_list, int);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
|
|
|
|
offset += sizeof(uint8_t);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'H':
|
2016-04-20 02:07:48 -03:00
|
|
|
case 'C': {
|
|
|
|
uint16_t tmp = va_arg(arg_list, int);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
|
|
|
|
offset += sizeof(uint16_t);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'I':
|
2016-04-20 02:07:48 -03:00
|
|
|
case 'E': {
|
|
|
|
uint32_t tmp = va_arg(arg_list, uint32_t);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
|
|
|
|
offset += sizeof(uint32_t);
|
|
|
|
break;
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'N':
|
|
|
|
charlen = 16;
|
2016-04-20 02:07:48 -03:00
|
|
|
break;
|
2016-04-26 09:32:48 -03:00
|
|
|
case 'Z':
|
|
|
|
charlen = 64;
|
2016-04-20 02:07:48 -03:00
|
|
|
break;
|
|
|
|
case 'q': {
|
|
|
|
int64_t tmp = va_arg(arg_list, int64_t);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(int64_t));
|
|
|
|
offset += sizeof(int64_t);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case 'Q': {
|
|
|
|
uint64_t tmp = va_arg(arg_list, uint64_t);
|
|
|
|
memcpy(&buffer[offset], &tmp, sizeof(uint64_t));
|
|
|
|
offset += sizeof(uint64_t);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
if (charlen != 0) {
|
|
|
|
char *tmp = va_arg(arg_list, char*);
|
|
|
|
memcpy(&buffer[offset], tmp, charlen);
|
|
|
|
offset += charlen;
|
|
|
|
}
|
2016-04-20 02:07:48 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return WritePrioritisedBlock(buffer, msg_len, is_critical);
|
|
|
|
}
|