Browse Source

DataFlash: log baro drift offset

mission-4.1.18
Tom Pittenger 9 years ago
parent
commit
38b7d7e1c6
  1. 10
      libraries/DataFlash/LogFile.cpp
  2. 3
      libraries/DataFlash/LogStructure.h

10
libraries/DataFlash/LogFile.cpp

@ -826,6 +826,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) @@ -826,6 +826,7 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
if (time_us == 0) {
time_us = AP_HAL::micros64();
}
float drift_offset = baro.get_baro_drift_offset();
struct log_BARO pkt = {
LOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
time_us : time_us,
@ -833,7 +834,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) @@ -833,7 +834,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
pressure : baro.get_pressure(0),
temperature : (int16_t)(baro.get_temperature(0) * 100 + 0.5f),
climbrate : baro.get_climb_rate(),
sample_time_ms: baro.get_last_update(0)
sample_time_ms: baro.get_last_update(0),
drift_offset : drift_offset,
};
WriteBlock(&pkt, sizeof(pkt));
@ -845,7 +847,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) @@ -845,7 +847,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
pressure : baro.get_pressure(1),
temperature : (int16_t)(baro.get_temperature(1) * 100 + 0.5f),
climbrate : baro.get_climb_rate(),
sample_time_ms: baro.get_last_update(1)
sample_time_ms: baro.get_last_update(1),
drift_offset : drift_offset,
};
WriteBlock(&pkt2, sizeof(pkt2));
}
@ -858,7 +861,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us) @@ -858,7 +861,8 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro, uint64_t time_us)
pressure : baro.get_pressure(2),
temperature : (int16_t)(baro.get_temperature(2) * 100 + 0.5f),
climbrate : baro.get_climb_rate(),
sample_time_ms: baro.get_last_update(2)
sample_time_ms: baro.get_last_update(2),
drift_offset : drift_offset,
};
WriteBlock(&pkt3, sizeof(pkt3));
}

3
libraries/DataFlash/LogStructure.h

@ -188,6 +188,7 @@ struct PACKED log_BARO { @@ -188,6 +188,7 @@ struct PACKED log_BARO {
int16_t temperature;
float climbrate;
uint32_t sample_time_ms;
float drift_offset;
};
struct PACKED log_AHRS {
@ -736,7 +737,7 @@ Format characters in the format string for binary log messages @@ -736,7 +737,7 @@ Format characters in the format string for binary log messages
{ LOG_RSSI_MSG, sizeof(log_RSSI), \
"RSSI", "Qf", "TimeUS,RXRSSI" }, \
{ LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "QffcfI", "TimeUS,Alt,Press,Temp,CRt,SMS" }, \
"BARO", "QffcfIf", "TimeUS,Alt,Press,Temp,CRt,SMS,Offset" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","QCCH","TimeUS,Vcc,VServo,Flags" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \

Loading…
Cancel
Save