AP_Logger: add support for logging float16

libraries/AP_Logger/AP_Logger.cpp

@@ -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;

libraries/AP_Logger/AP_Logger_Backend.cpp

@@ -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;

libraries/AP_Logger/AP_Logger_File.cpp

@@ -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()) {

libraries/AP_Logger/README.md

@@ -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.
 

libraries/AP_Logger/examples/AP_Logger_AllTypes/AP_Logger_AllTypes.cpp

@@ -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: