diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 36bf112649..4ce0cce904 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -429,6 +429,7 @@ struct PACKED log_Current { int16_t current_amps; uint16_t board_voltage; float current_total; + int16_t battery2_voltage; }; /* @@ -529,7 +530,7 @@ struct PACKED log_AIRSPEED { { LOG_ARSP_MSG, sizeof(log_AIRSPEED), \ "ARSP", "Iffcff", "TimeMS,Airspeed,DiffPress,Temp,RawPress,Offset" } { LOG_CURRENT_MSG, sizeof(log_Current), \ - "CURR", "Ihhhhf","TimeMS,ThrOut,Volt,Curr,Vcc,CurrTot" } + "CURR", "Ihhhhfh","TimeMS,Throttle,Volt,Curr,Vcc,CurrTot,Volt2" } // messages for more advanced boards #define LOG_EXTRA_STRUCTURES \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 97e7c5a5d3..e0d3671861 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1135,6 +1135,7 @@ void DataFlash_Class::Log_Write_Attitude(AP_AHRS &ahrs, Vector3f targets) // Write an Current data packet void DataFlash_Class::Log_Write_Current(AP_BattMonitor battery, int16_t throttle) { + float voltage2 = battery.voltage2(); struct log_Current pkt = { LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), time_ms : hal.scheduler->millis(), @@ -1142,7 +1143,8 @@ void DataFlash_Class::Log_Write_Current(AP_BattMonitor battery, int16_t throttl battery_voltage : (int16_t) (battery.voltage() * 100.0f), current_amps : (int16_t) (battery.current_amps() * 100.0f), board_voltage : (uint16_t)(hal.analogin->board_voltage()*1000), - current_total : battery.current_total_mah() + current_total : battery.current_total_mah(), + battery2_voltage : (int16_t)(voltage2 * 100.0f) }; WriteBlock(&pkt, sizeof(pkt)); }