From b85001bf4a67d56fec033c9d5c9fd76cc497831d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Jan 2015 08:37:23 +1100
Subject: [PATCH] DataFlash: added logging of 2nd baro
---
libraries/DataFlash/DataFlash.h | 3 +++
libraries/DataFlash/LogFile.cpp | 22 ++++++++++++++++++----
2 files changed, 21 insertions(+), 4 deletions(-)
diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h
index 535e759c3..b5110cecd 100644
--- a/libraries/DataFlash/DataFlash.h
+++ b/libraries/DataFlash/DataFlash.h
@@ -478,6 +478,8 @@ struct PACKED log_Esc {
"RCOU", "Ihhhhhhhhhhhh", "TimeMS,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Ch8,Ch9,Ch10,Ch11,Ch12" }, \
{ LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \
+ { LOG_BAR2_MSG, sizeof(log_BARO), \
+ "BAR2", "Iffcf", "TimeMS,Alt,Press,Temp,CRt" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","ICCH","TimeMS,Vcc,VServo,Flags" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \
@@ -578,6 +580,7 @@ struct PACKED log_Esc {
#define LOG_ESC7_MSG 160
#define LOG_ESC8_MSG 161
#define LOG_EKF5_MSG 162
+#define LOG_BAR2_MSG 163
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use
diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp
index 874fe3abc..bf408cfe0 100644
--- a/libraries/DataFlash/LogFile.cpp
+++ b/libraries/DataFlash/LogFile.cpp
@@ -765,15 +765,29 @@ void DataFlash_Class::Log_Write_RCOUT(void)
// Write a BARO packet
void DataFlash_Class::Log_Write_Baro(AP_Baro &baro)
{
+ uint32_t now = hal.scheduler->millis();
struct log_BARO pkt = {
LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
- timestamp : hal.scheduler->millis(),
- altitude : baro.get_altitude(),
- pressure : baro.get_pressure(),
- temperature : (int16_t)(baro.get_temperature() * 100),
+ timestamp : now,
+ altitude : baro.get_altitude(0),
+ pressure : baro.get_pressure(0),
+ temperature : (int16_t)(baro.get_temperature(0) * 100),
climbrate : baro.get_climb_rate()
};
WriteBlock(&pkt, sizeof(pkt));
+#if BARO_MAX_INSTANCES > 1
+ if (baro.num_instances() > 1 && baro.healthy(1)) {
+ struct log_BARO pkt2 = {
+ LOG_PACKET_HEADER_INIT(LOG_BAR2_MSG),
+ timestamp : now,
+ altitude : baro.get_altitude(1),
+ pressure : baro.get_pressure(1),
+ temperature : (int16_t)(baro.get_temperature(1) * 100),
+ climbrate : baro.get_climb_rate()
+ };
+ WriteBlock(&pkt2, sizeof(pkt2));
+ }
+#endif
}
// Write an raw accel/gyro data packet
--
GitLab