AP_Logger: add support for logging float16

This commit is contained in:
Peter Barker 2024-08-22 13:06:44 +10:00 committed by Andrew Tridgell
parent 4e27c51aa1
commit b4e3b6cdd5
5 changed files with 28 additions and 12 deletions

View File

@ -489,7 +489,7 @@ bool AP_Logger::validate_structure(const struct LogStructure *logstructure, cons
if (false && passed) {
for (uint8_t j=0; j<strlen(logstructure->multipliers); j++) {
const char fmt = logstructure->format[j];
if (fmt != 'f') {
if (fmt != 'f' && fmt != 'd' && fmt != 'g') {
continue;
}
const char logmultiplier = logstructure->multipliers[j];
@ -1354,6 +1354,7 @@ int16_t AP_Logger::Write_calc_msg_len(const char *fmt) const
case 'd' : len += sizeof(double); break;
case 'e' : len += sizeof(int32_t); break;
case 'f' : len += sizeof(float); break;
case 'g' : len += sizeof(float16_s); break;
case 'h' : len += sizeof(int16_t); break;
case 'i' : len += sizeof(int32_t); break;
case 'n' : len += sizeof(char[4]); break;

View File

@ -257,6 +257,13 @@ bool AP_Logger_Backend::Write(const uint8_t msg_type, va_list arg_list, bool is_
offset += sizeof(float);
break;
}
case 'g': {
Float16_t tmp;
tmp.set(va_arg(arg_list, double));;
memcpy(&buffer[offset], &tmp, sizeof(tmp));
offset += sizeof(tmp);
break;
}
case 'n':
charlen = 4;
break;

View File

@ -428,7 +428,7 @@ bool AP_Logger_File::StartNewLogOK() const
if (recent_open_error()) {
return false;
}
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
#if !APM_BUILD_TYPE(APM_BUILD_Replay) && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
if (hal.scheduler->in_main_thread()) {
return false;
}
@ -869,7 +869,7 @@ bool AP_Logger_File::write_lastlog_file(uint16_t log_num)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
void AP_Logger_File::flush(void)
#if APM_BUILD_TYPE(APM_BUILD_Replay)
#if APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
{
uint32_t tnow = AP_HAL::millis();
while (_write_fd != -1 && _initialised && !recent_open_error() && _writebuf.available()) {

View File

@ -23,6 +23,7 @@ and how the content should be interpreted.
|M | uint8_t flight mode|
|q | int64_t|
|Q | uint64_t|
|g | float16_t|
Legacy field types - do not use. These have been replaced by using the base C type and an appropriate multiplier column entry.

View File

@ -40,6 +40,7 @@ struct PACKED log_TYP2 {
uint8_t M; // flight mode
int64_t q;
uint64_t Q;
Float16_t g;
};
@ -74,10 +75,10 @@ static const struct LogStructure log_structure[] = {
{ LOG_TYP2_MSG,
sizeof(log_TYP2),
"TYP2",
"QcCeELMqQ",
"TimeUS,c,C,e,E,L,M,q,Q",
"s--------",
"F--------"
"QcCeELMqQg",
"TimeUS,c,C,e,E,L,M,q,Q,g",
"s---------",
"F---------"
},
{ LOG_MESSAGE_MSG,
sizeof(log_Message),
@ -93,8 +94,8 @@ static const struct LogStructure log_structure[] = {
// structure and that in LogStructure.h
#define TYP1_FMT "QabBhHiIfdnNZ"
#define TYP1_LBL "TimeUS,b,B,h,H,i,I,f,d,n,N,Z"
#define TYP2_FMT "QcCeELMqQ"
#define TYP2_LBL "TimeUS,c,C,e,E,L,M,q,Q"
#define TYP2_FMT "QcCeELMqQg"
#define TYP2_LBL "TimeUS,c,C,e,E,L,M,q,Q,g"
static uint16_t log_num;
@ -158,8 +159,12 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P',
'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P' }
};
logger.WriteBlock(&typ1, sizeof(typ1));
if (!logger.WriteBlock_first_succeed(&typ1, sizeof(typ1))) {
abort();
}
Float16_t f16;
f16.set(13.54f);
struct log_TYP2 typ2 = {
LOG_PACKET_HEADER_INIT(LOG_TYP2_MSG),
time_us : AP_HAL::micros64(),
@ -170,7 +175,8 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
L : -3576543, // uint32_t latitude/longitude;
M : 5, // uint8_t; // flight mode;
q : -98239832498328, // int64_t
Q : 3432345232233432 // uint64_t
Q : 3432345232233432, // uint64_t
g : f16 // float16_t
};
logger.WriteBlock(&typ2, sizeof(typ2));
@ -232,7 +238,8 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages_Log_Write()
-3576543, // uint32_t latitude/longitude;
5, // uint8_t; // flight mode;
-98239832498328, // int64_t
3432345232233432 // uint64_t
3432345232233432, // uint64_t
13.54 // float16_t
);
// emit a message which contains NaNs: