Tracker: add Vbar dataflash message

This commit is contained in:
stefanlynka 2016-05-12 14:17:31 +09:00 committed by Randy Mackay
parent 8fb318a6e1
commit e06c900812
4 changed files with 31 additions and 0 deletions

View File

@ -26,8 +26,32 @@ void Tracker::Log_Write_Baro(void)
DataFlash.Log_Write_Baro(barometer); DataFlash.Log_Write_Baro(barometer);
} }
struct PACKED log_Vehicle_Baro {
LOG_PACKET_HEADER;
uint64_t time_us;
float press;
float alt_diff;
};
// Write a vehicle baro packet
void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude)
{
struct log_Vehicle_Baro pkt = {
LOG_PACKET_HEADER_INIT(LOG_V_BAR_MSG),
time_us : AP_HAL::micros64(),
press : pressure,
alt_diff : altitude
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
const struct LogStructure Tracker::log_structure[] = { const struct LogStructure Tracker::log_structure[] = {
LOG_COMMON_STRUCTURES, LOG_COMMON_STRUCTURES,
{LOG_V_BAR_MSG, sizeof(log_Vehicle_Baro),
"VBAR", "Qff", "TimeUS,Press,AltDiff" },
}; };
void Tracker::Log_Write_Vehicle_Startup_Messages() void Tracker::Log_Write_Vehicle_Startup_Messages()

View File

@ -255,6 +255,7 @@ private:
void compass_cal_update(); void compass_cal_update();
void Log_Write_Attitude(); void Log_Write_Attitude();
void Log_Write_Baro(void); void Log_Write_Baro(void);
void Log_Write_Vehicle_Baro(float pressure, float altitude);
void Log_Write_Vehicle_Startup_Messages(); void Log_Write_Vehicle_Startup_Messages();
void start_logging(); void start_logging();
void log_init(void); void log_init(void);

View File

@ -32,3 +32,6 @@ enum ServoType {
#define MASK_LOG_RCOUT (1<<4) #define MASK_LOG_RCOUT (1<<4)
#define MASK_LOG_COMPASS (1<<5) #define MASK_LOG_COMPASS (1<<5)
#define MASK_LOG_ANY 0xFFFF #define MASK_LOG_ANY 0xFFFF
// Logging messages
#define LOG_V_BAR_MSG 0x04

View File

@ -150,6 +150,9 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg)
nav_status.altitude_difference = 0; nav_status.altitude_difference = 0;
nav_status.need_altitude_calibration = false; nav_status.need_altitude_calibration = false;
} }
// log altitude difference
Log_Write_Vehicle_Baro(local_pressure, alt_diff);
} }
/** /**