From 24487d6b0d5c50eba2b512df7cfcc2914d33d2cc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Apr 2018 09:49:48 +1000 Subject: [PATCH] DataFlash: added logging of safety state in POWR message --- libraries/DataFlash/LogFile.cpp | 8 +++++++- libraries/DataFlash/LogStructure.h | 3 ++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 8bd032d353..83a0ad2899 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1012,12 +1012,18 @@ bool DataFlash_Backend::Log_Write_Message(const char *message) void DataFlash_Class::Log_Write_Power(void) { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + uint8_t safety_and_armed = uint8_t(hal.util->safety_switch_state()); + if (hal.util->get_soft_armed()) { + // encode armed state in bit 3 + safety_and_armed |= 1U<<2; + } struct log_POWR pkt = { LOG_PACKET_HEADER_INIT(LOG_POWR_MSG), time_us : AP_HAL::micros64(), Vcc : hal.analogin->board_voltage(), Vservo : hal.analogin->servorail_voltage(), - flags : hal.analogin->power_status_flags() + flags : hal.analogin->power_status_flags(), + safety_and_arm : safety_and_armed }; WriteBlock(&pkt, sizeof(pkt)); #endif diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index 65edb4ce5e..3017c05566 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -223,6 +223,7 @@ struct PACKED log_POWR { float Vcc; float Vservo; uint16_t flags; + uint8_t safety_and_arm; }; struct PACKED log_EKF1 { @@ -905,7 +906,7 @@ Format characters in the format string for binary log messages { LOG_BARO_MSG, sizeof(log_BARO), \ "BARO", BARO_FMT, BARO_LABELS }, \ { LOG_POWR_MSG, sizeof(log_POWR), \ - "POWR","QffH","TimeUS,Vcc,VServo,Flags" }, \ + "POWR","QffHB","TimeUS,Vcc,VServo,Flags,Safety" }, \ { LOG_CMD_MSG, sizeof(log_Cmd), \ "CMD", "QHHHfffffff","TimeUS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \ { LOG_RADIO_MSG, sizeof(log_Radio), \