2019-01-18 00:23:42 -04:00
|
|
|
#include "AP_Logger_Backend.h"
|
2015-08-06 09:18:28 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
#include "LoggerMessageWriter.h"
|
2015-08-06 09:18:28 -03:00
|
|
|
|
2018-08-07 07:22:21 -03:00
|
|
|
#include <AP_InternalError/AP_InternalError.h>
|
|
|
|
|
2015-08-06 09:18:28 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
AP_Logger_Backend::AP_Logger_Backend(AP_Logger &front,
|
|
|
|
class LoggerMessageWriter_DFLogStart *writer) :
|
2015-11-09 18:14:22 -04:00
|
|
|
_front(front),
|
|
|
|
_startup_messagewriter(writer)
|
|
|
|
{
|
2019-02-11 04:38:01 -04:00
|
|
|
writer->set_logger_backend(this);
|
2015-11-09 18:14:22 -04:00
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
uint8_t AP_Logger_Backend::num_types() const
|
2015-11-09 18:14:22 -04:00
|
|
|
{
|
|
|
|
return _front._num_types;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
const struct LogStructure *AP_Logger_Backend::structure(uint8_t num) const
|
2015-11-09 18:14:22 -04:00
|
|
|
{
|
|
|
|
return _front.structure(num);
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
uint8_t AP_Logger_Backend::num_units() const
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
return _front._num_units;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
const struct UnitStructure *AP_Logger_Backend::unit(uint8_t num) const
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
return _front.unit(num);
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
uint8_t AP_Logger_Backend::num_multipliers() const
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
return _front._num_multipliers;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
const struct MultiplierStructure *AP_Logger_Backend::multiplier(uint8_t num) const
|
2015-12-07 20:51:46 -04:00
|
|
|
{
|
|
|
|
return _front.multiplier(num);
|
|
|
|
}
|
|
|
|
|
2021-02-01 12:26:29 -04:00
|
|
|
AP_Logger_Backend::vehicle_startup_message_Writer AP_Logger_Backend::vehicle_message_writer() const {
|
2015-11-09 18:14:22 -04:00
|
|
|
return _front._vehicle_messages;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::periodic_10Hz(const uint32_t now)
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
|
|
|
}
|
2020-05-05 14:00:54 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::periodic_1Hz()
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
2020-05-05 14:00:54 -03:00
|
|
|
if (_rotate_pending && !logging_enabled()) {
|
|
|
|
_rotate_pending = false;
|
|
|
|
// handle log rotation once we stop logging
|
2020-09-10 16:57:40 -03:00
|
|
|
stop_logging_async();
|
2020-05-05 14:00:54 -03:00
|
|
|
}
|
|
|
|
df_stats_log();
|
2015-08-06 09:18:28 -03:00
|
|
|
}
|
2020-05-05 14:00:54 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::periodic_fullrate()
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
2020-05-05 14:00:54 -03:00
|
|
|
push_log_blocks();
|
2015-08-06 09:18:28 -03:00
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::periodic_tasks()
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
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) {
|
2018-07-28 13:28:54 -03:00
|
|
|
periodic_1Hz();
|
2015-08-06 09:18:28 -03:00
|
|
|
_last_periodic_1Hz = now;
|
|
|
|
}
|
|
|
|
if (now - _last_periodic_10Hz > 100) {
|
|
|
|
periodic_10Hz(now);
|
|
|
|
_last_periodic_10Hz = now;
|
|
|
|
}
|
2018-07-28 13:28:54 -03:00
|
|
|
periodic_fullrate();
|
2015-08-06 09:18:28 -03:00
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::start_new_log_reset_variables()
|
2016-04-15 06:53:54 -03:00
|
|
|
{
|
2020-05-05 14:00:54 -03:00
|
|
|
_dropped = 0;
|
2016-04-15 06:53:54 -03:00
|
|
|
_startup_messagewriter->reset();
|
2017-04-28 04:37:13 -03:00
|
|
|
_front.backend_starting_new_log(this);
|
2020-05-05 14:00:54 -03:00
|
|
|
_log_file_size_bytes = 0;
|
2016-04-15 06:53:54 -03:00
|
|
|
}
|
2015-08-06 09:18:28 -03:00
|
|
|
|
2020-11-05 19:28:26 -04:00
|
|
|
// We may need to make sure data is loggable before starting the
|
|
|
|
// EKF; when allow_start_ekf we should be able to log that data
|
|
|
|
bool AP_Logger_Backend::allow_start_ekf() const
|
|
|
|
{
|
|
|
|
if (!_startup_messagewriter->fmt_done()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
// we need to push all startup messages out, or the code in
|
|
|
|
// WriteBlockCheckStartupMessages bites us.
|
|
|
|
if (!_startup_messagewriter->finished()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2015-08-06 09:18:28 -03:00
|
|
|
// this method can be overridden to do extra things with your buffer.
|
2019-01-18 00:23:42 -04:00
|
|
|
// for example, in AP_Logger_MAVLink we may push messages into the UART.
|
|
|
|
void AP_Logger_Backend::push_log_blocks() {
|
2015-08-06 09:18:28 -03:00
|
|
|
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
|
2019-01-18 00:23:42 -04:00
|
|
|
bool AP_Logger_Backend::WriteBlockCheckStartupMessages()
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
2018-04-22 07:19:30 -03:00
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
|
|
return true;
|
|
|
|
#endif
|
2020-11-05 19:28:26 -04:00
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2017-09-17 22:03:48 -03:00
|
|
|
if (!_startup_messagewriter->finished() &&
|
|
|
|
!hal.scheduler->in_main_thread()) {
|
|
|
|
// only the main thread may write startup messages out
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-08-06 09:18:28 -03:00
|
|
|
// 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:
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::WriteMoreStartupMessages()
|
2015-08-06 09:18:28 -03:00
|
|
|
{
|
2020-11-05 19:28:26 -04:00
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
|
|
return;
|
|
|
|
#endif
|
2015-08-06 09:18:28 -03:00
|
|
|
|
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
|
|
|
|
|
|
|
/*
|
2019-01-18 00:24:08 -04:00
|
|
|
* support for Write():
|
2016-04-20 02:07:48 -03:00
|
|
|
*/
|
|
|
|
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_Emit_FMT(uint8_t msg_type)
|
2016-04-20 02:07:48 -03:00
|
|
|
{
|
2020-11-05 19:28:26 -04:00
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_Replay)
|
|
|
|
// sure, sure we did....
|
|
|
|
return true;
|
|
|
|
#endif
|
|
|
|
|
2016-04-20 02:07:48 -03:00
|
|
|
// get log structure from front end:
|
2018-02-22 20:06:45 -04:00
|
|
|
char ls_name[LS_NAME_SIZE] = {};
|
|
|
|
char ls_format[LS_FORMAT_SIZE] = {};
|
|
|
|
char ls_labels[LS_LABELS_SIZE] = {};
|
|
|
|
char ls_units[LS_UNITS_SIZE] = {};
|
|
|
|
char ls_multipliers[LS_MULTIPLIERS_SIZE] = {};
|
2016-04-20 02:07:48 -03:00
|
|
|
struct LogStructure logstruct = {
|
|
|
|
// these will be overwritten, but need to keep the compiler happy:
|
|
|
|
0,
|
|
|
|
0,
|
2018-02-22 20:06:45 -04:00
|
|
|
ls_name,
|
|
|
|
ls_format,
|
|
|
|
ls_labels,
|
|
|
|
ls_units,
|
|
|
|
ls_multipliers
|
2016-04-20 02:07:48 -03:00
|
|
|
};
|
|
|
|
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
|
2020-04-29 21:40:46 -03:00
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::logger_missing_logstructure);
|
2016-04-20 02:07:48 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
if (!Write_Format(&logstruct)) {
|
2016-04-20 02:07:48 -03:00
|
|
|
return false;
|
2015-12-07 20:51:46 -04:00
|
|
|
}
|
2019-01-18 00:24:08 -04:00
|
|
|
if (!Write_Format_Units(&logstruct)) {
|
2015-12-07 20:51:46 -04:00
|
|
|
return false;
|
2016-04-20 02:07:48 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write(const uint8_t msg_type, va_list arg_list, bool is_critical)
|
2016-04-20 02:07:48 -03:00
|
|
|
{
|
|
|
|
// 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?
|
2016-10-30 02:24:21 -03:00
|
|
|
const char *fmt = nullptr;
|
2016-04-20 02:07:48 -03:00
|
|
|
uint8_t msg_len;
|
2019-01-18 00:23:42 -04:00
|
|
|
AP_Logger::log_write_fmt *f;
|
2016-05-04 06:06:23 -03:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
}
|
2016-10-30 02:24:21 -03:00
|
|
|
if (fmt == nullptr) {
|
2020-04-29 21:40:46 -03:00
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::logger_logwrite_missingfmt);
|
2016-04-20 02:07:48 -03:00
|
|
|
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;
|
|
|
|
}
|
2019-12-10 22:08:08 -04:00
|
|
|
case 'a': {
|
|
|
|
int16_t *tmp = va_arg(arg_list, int16_t*);
|
|
|
|
const uint8_t bytes = 32*2;
|
|
|
|
memcpy(&buffer[offset], tmp, bytes);
|
|
|
|
offset += bytes;
|
|
|
|
break;
|
|
|
|
}
|
2016-04-20 02:07:48 -03:00
|
|
|
}
|
2016-04-26 09:32:48 -03:00
|
|
|
if (charlen != 0) {
|
|
|
|
char *tmp = va_arg(arg_list, char*);
|
2020-12-31 17:01:53 -04:00
|
|
|
uint8_t len = strnlen(tmp, charlen);
|
|
|
|
memcpy(&buffer[offset], tmp, len);
|
|
|
|
memset(&buffer[offset+len], 0, charlen-len);
|
2016-04-26 09:32:48 -03:00
|
|
|
offset += charlen;
|
|
|
|
}
|
2016-04-20 02:07:48 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return WritePrioritisedBlock(buffer, msg_len, is_critical);
|
|
|
|
}
|
2017-06-08 22:36:18 -03:00
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
bool AP_Logger_Backend::StartNewLogOK() const
|
2017-06-30 08:09:20 -03:00
|
|
|
{
|
|
|
|
if (logging_started()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (_front._log_bitmask == 0) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (_front.in_log_download()) {
|
|
|
|
return false;
|
|
|
|
}
|
2017-09-17 22:03:48 -03:00
|
|
|
if (!hal.scheduler->in_main_thread()) {
|
|
|
|
return false;
|
|
|
|
}
|
2017-06-30 08:09:20 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2018-08-08 23:12:18 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2019-01-18 00:23:42 -04:00
|
|
|
void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer,
|
2018-08-08 23:12:18 -03:00
|
|
|
uint16_t size)
|
|
|
|
{
|
|
|
|
// just check the first few packets to avoid too much overhead
|
|
|
|
// (finding the structures is expensive)
|
|
|
|
static uint16_t count = 0;
|
|
|
|
if (count > 65534) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
count++;
|
|
|
|
|
|
|
|
// we assume here that we ever WritePrioritisedBlock for a single
|
|
|
|
// message. If this assumption becomes false we can't do these
|
|
|
|
// checks.
|
|
|
|
if (size < 3) {
|
|
|
|
AP_HAL::panic("Short prioritised block");
|
|
|
|
}
|
|
|
|
if (((uint8_t*)pBuffer)[0] != HEAD_BYTE1 ||
|
|
|
|
((uint8_t*)pBuffer)[1] != HEAD_BYTE2) {
|
|
|
|
AP_HAL::panic("Not passed a message");
|
|
|
|
}
|
|
|
|
const uint8_t type = ((uint8_t*)pBuffer)[2];
|
|
|
|
uint8_t type_len;
|
|
|
|
const struct LogStructure *s = _front.structure_for_msg_type(type);
|
|
|
|
if (s == nullptr) {
|
2019-01-18 00:23:42 -04:00
|
|
|
const struct AP_Logger::log_write_fmt *t = _front.log_write_fmt_for_msg_type(type);
|
2018-08-08 23:12:18 -03:00
|
|
|
if (t == nullptr) {
|
|
|
|
AP_HAL::panic("No structure for msg_type=%u", type);
|
|
|
|
}
|
|
|
|
type_len = t->msg_len;
|
|
|
|
} else {
|
|
|
|
type_len = s->msg_len;
|
|
|
|
}
|
|
|
|
if (type_len != size) {
|
|
|
|
char name[5] = {}; // get a null-terminated string
|
2020-03-27 22:03:13 -03:00
|
|
|
if (s->name != nullptr) {
|
|
|
|
memcpy(name, s->name, 4);
|
|
|
|
} else {
|
|
|
|
strncpy(name, "?NM?", ARRAY_SIZE(name));
|
|
|
|
}
|
2018-08-08 23:12:18 -03:00
|
|
|
AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)\n",
|
|
|
|
type, name, type_len, size);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
bool AP_Logger_Backend::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
|
2017-06-30 08:09:20 -03:00
|
|
|
{
|
2020-11-08 16:46:17 -04:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
2018-08-08 23:12:18 -03:00
|
|
|
validate_WritePrioritisedBlock(pBuffer, size);
|
|
|
|
#endif
|
2017-10-26 02:29:09 -03:00
|
|
|
if (!ShouldLog(is_critical)) {
|
2017-07-04 13:53:20 -03:00
|
|
|
return false;
|
|
|
|
}
|
2017-06-30 08:09:20 -03:00
|
|
|
if (StartNewLogOK()) {
|
|
|
|
start_new_log();
|
|
|
|
}
|
2017-07-04 13:53:20 -03:00
|
|
|
if (!WritesOK()) {
|
|
|
|
return false;
|
|
|
|
}
|
2017-06-30 08:09:20 -03:00
|
|
|
return _WritePrioritisedBlock(pBuffer, size, is_critical);
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
bool AP_Logger_Backend::ShouldLog(bool is_critical)
|
2017-06-08 22:36:18 -03:00
|
|
|
{
|
2017-06-14 22:21:17 -03:00
|
|
|
if (!_front.WritesEnabled()) {
|
2017-06-08 22:36:18 -03:00
|
|
|
return false;
|
|
|
|
}
|
2017-06-30 01:42:31 -03:00
|
|
|
if (!_initialised) {
|
|
|
|
return false;
|
|
|
|
}
|
2017-06-09 01:19:11 -03:00
|
|
|
|
2017-09-17 22:03:48 -03:00
|
|
|
if (!_startup_messagewriter->finished() &&
|
|
|
|
!hal.scheduler->in_main_thread()) {
|
|
|
|
// only the main thread may write startup messages out
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-10-26 02:29:09 -03:00
|
|
|
if (is_critical && have_logged_armed && !_front._params.file_disarm_rot) {
|
|
|
|
// if we have previously logged while armed then we log all
|
|
|
|
// critical messages from then on. That fixes a problem where
|
|
|
|
// logs show the wrong flight mode if you disarm then arm again
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!_front.vehicle_is_armed() && !_front.log_while_disarmed()) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (_front.vehicle_is_armed()) {
|
|
|
|
have_logged_armed = true;
|
|
|
|
}
|
|
|
|
|
2017-06-08 22:36:18 -03:00
|
|
|
return true;
|
|
|
|
}
|
2018-06-12 00:34:00 -03:00
|
|
|
|
2020-05-05 14:00:54 -03:00
|
|
|
void AP_Logger_Backend::PrepForArming()
|
|
|
|
{
|
|
|
|
if (_rotate_pending) {
|
|
|
|
_rotate_pending = false;
|
|
|
|
stop_logging();
|
|
|
|
}
|
|
|
|
if (logging_started()) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
start_new_log();
|
|
|
|
}
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
|
2018-06-12 00:34:00 -03:00
|
|
|
{
|
2018-09-05 23:16:49 -03:00
|
|
|
char msg[65] {}; // sizeof(log_Message.msg) + null-termination
|
2018-06-12 00:34:00 -03:00
|
|
|
|
|
|
|
va_list ap;
|
|
|
|
va_start(ap, fmt);
|
|
|
|
hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
|
|
|
|
va_end(ap);
|
|
|
|
|
2019-01-18 00:24:08 -04:00
|
|
|
return Write_Message(msg);
|
2018-06-12 00:34:00 -03:00
|
|
|
}
|
2018-12-29 00:02:29 -04:00
|
|
|
|
|
|
|
// Write rally points
|
|
|
|
bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
|
|
|
|
uint8_t sequence,
|
|
|
|
const RallyLocation &rally_point)
|
|
|
|
{
|
2019-07-02 06:31:02 -03:00
|
|
|
const struct log_Rally pkt_rally{
|
2018-12-29 00:02:29 -04:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_RALLY_MSG),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
total : total,
|
|
|
|
sequence : sequence,
|
|
|
|
latitude : rally_point.lat,
|
|
|
|
longitude : rally_point.lng,
|
|
|
|
altitude : rally_point.alt
|
|
|
|
};
|
|
|
|
return WriteBlock(&pkt_rally, sizeof(pkt_rally));
|
|
|
|
}
|
|
|
|
|
|
|
|
// Write rally points
|
2020-04-13 01:20:30 -03:00
|
|
|
bool AP_Logger_Backend::Write_Rally()
|
2018-12-29 00:02:29 -04:00
|
|
|
{
|
2020-04-13 01:20:30 -03:00
|
|
|
// kick off asynchronous write:
|
|
|
|
return _startup_messagewriter->writeallrallypoints();
|
2018-12-29 00:02:29 -04:00
|
|
|
}
|
2020-05-05 14:00:54 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
convert a list entry number back into a log number (which can then
|
|
|
|
be converted into a filename). A "list entry number" is a sequence
|
|
|
|
where the oldest log has a number of 1, the second-from-oldest 2,
|
|
|
|
and so on. Thus the highest list entry number is equal to the
|
|
|
|
number of logs.
|
|
|
|
*/
|
|
|
|
uint16_t AP_Logger_Backend::log_num_from_list_entry(const uint16_t list_entry)
|
|
|
|
{
|
|
|
|
uint16_t oldest_log = find_oldest_log();
|
|
|
|
if (oldest_log == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint32_t log_num = oldest_log + list_entry - 1;
|
|
|
|
if (log_num > MAX_LOG_FILES) {
|
|
|
|
log_num -= MAX_LOG_FILES;
|
|
|
|
}
|
|
|
|
return (uint16_t)log_num;
|
|
|
|
}
|
|
|
|
|
|
|
|
// find_oldest_log - find oldest log
|
|
|
|
// returns 0 if no log was found
|
|
|
|
uint16_t AP_Logger_Backend::find_oldest_log()
|
|
|
|
{
|
|
|
|
if (_cached_oldest_log != 0) {
|
|
|
|
return _cached_oldest_log;
|
|
|
|
}
|
|
|
|
|
|
|
|
uint16_t last_log_num = find_last_log();
|
|
|
|
if (last_log_num == 0) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
_cached_oldest_log = last_log_num - get_num_logs() + 1;
|
|
|
|
|
|
|
|
return _cached_oldest_log;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Logger_Backend::vehicle_was_disarmed()
|
|
|
|
{
|
|
|
|
if (_front._params.file_disarm_rot) {
|
|
|
|
// rotate our log. Closing the current one and letting the
|
|
|
|
// logging restart naturally based on log_disarmed should do
|
|
|
|
// the trick:
|
|
|
|
_rotate_pending = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// this sensor is enabled if we should be logging at the moment
|
|
|
|
bool AP_Logger_Backend::logging_enabled() const
|
|
|
|
{
|
|
|
|
if (hal.util->get_soft_armed() ||
|
|
|
|
_front.log_while_disarmed()) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Logger_Backend::Write_AP_Logger_Stats_File(const struct df_stats &_stats)
|
|
|
|
{
|
2020-09-05 04:36:12 -03:00
|
|
|
const struct log_DSF pkt {
|
2020-05-05 14:00:54 -03:00
|
|
|
LOG_PACKET_HEADER_INIT(LOG_DF_FILE_STATS),
|
|
|
|
time_us : AP_HAL::micros64(),
|
|
|
|
dropped : _dropped,
|
|
|
|
blocks : _stats.blocks,
|
|
|
|
bytes : _stats.bytes,
|
|
|
|
buf_space_min : _stats.buf_space_min,
|
|
|
|
buf_space_max : _stats.buf_space_max,
|
|
|
|
buf_space_avg : (_stats.blocks) ? (_stats.buf_space_sigma / _stats.blocks) : 0,
|
|
|
|
};
|
|
|
|
WriteBlock(&pkt, sizeof(pkt));
|
|
|
|
}
|
|
|
|
|
2020-09-05 04:36:12 -03:00
|
|
|
void AP_Logger_Backend::df_stats_gather(const uint16_t bytes_written, uint32_t space_remaining)
|
|
|
|
{
|
2020-05-05 14:00:54 -03:00
|
|
|
if (space_remaining < stats.buf_space_min) {
|
|
|
|
stats.buf_space_min = space_remaining;
|
|
|
|
}
|
|
|
|
if (space_remaining > stats.buf_space_max) {
|
|
|
|
stats.buf_space_max = space_remaining;
|
|
|
|
}
|
|
|
|
stats.buf_space_sigma += space_remaining;
|
|
|
|
stats.bytes += bytes_written;
|
|
|
|
_log_file_size_bytes += bytes_written;
|
|
|
|
stats.blocks++;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Logger_Backend::df_stats_clear() {
|
|
|
|
memset(&stats, '\0', sizeof(stats));
|
|
|
|
stats.buf_space_min = -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
void AP_Logger_Backend::df_stats_log() {
|
|
|
|
Write_AP_Logger_Stats_File(stats);
|
|
|
|
df_stats_clear();
|
|
|
|
}
|