Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: added RNFD logging and 3 baros

Browse Source
This commit is contained in:
Andrew Tridgell 2015-09-10 20:27:47 +10:00
parent 8538aa9840
commit 43ac3f86c5
2 changed files with 48 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/DataFlash/DataFlash.h
View File

@ -87,6 +87,7 @@ public:
bool Log_Write_Format(const struct LogStructure *structure);
bool Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_RFND(const RangeFinder &rangefinder);
void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_IMUDT(const AP_InertialSensor &ins);
void Log_Write_Vibration(const AP_InertialSensor &ins);
@ -515,6 +516,18 @@ struct PACKED log_Mode {
uint8_t mode_num;
};
/*
rangefinder - support for 4 sensors
*/
struct PACKED log_RFND {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t dist1;
uint16_t dist2;
uint16_t dist3;
uint16_t dist4;
};
/*
terrain log structure
*/
@ -735,7 +748,9 @@ Format characters in the format string for binary log messages
{ LOG_COMPASS_MSG, sizeof(log_Compass), \
"MAG", "QhhhhhhhhhB", "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health" }, \
{ LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "QMB", "TimeUS,Mode,ModeNum" }
"MODE", "QMB", "TimeUS,Mode,ModeNum" }, \
{ LOG_RFND_MSG, sizeof(log_RFND), \
"RFND", "QCCCC", "TimeUS,Dist1,Dist2,Dist3,Dist4" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \
@ -819,6 +834,8 @@ Format characters in the format string for binary log messages
"PIDS", "Qffffff", "TimeUS,Des,P,I,D,FF,AFF" }, \
{ LOG_BAR2_MSG, sizeof(log_BARO), \
"BAR2", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
{ LOG_BAR3_MSG, sizeof(log_BARO), \
"BAR3", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
{ LOG_VIBE_MSG, sizeof(log_Vibe), \
"VIBE", "QfffIII", "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }, \
{ LOG_IMUDT_MSG, sizeof(log_IMUDT), \
@ -909,6 +926,8 @@ enum LogMessages {
LOG_RPM_MSG,
LOG_GPA_MSG,
LOG_GPA2_MSG,
LOG_RFND_MSG,
LOG_BAR3_MSG,
};
enum LogOriginType {

28
libraries/DataFlash/LogFile.cpp
View File

@ -747,6 +747,21 @@ void DataFlash_Class::Log_Write_GPS(const AP_GPS &gps, uint8_t i, int32_t relati
WriteBlock(&pkt2, sizeof(pkt2));
}
// Write an RFND (rangefinder) packet
void DataFlash_Class::Log_Write_RFND(const RangeFinder &rangefinder)
{
struct log_RFND pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)),
time_us : hal.scheduler->micros64(),
dist1 : rangefinder.distance_cm(0),
dist2 : rangefinder.distance_cm(1),
dist3 : rangefinder.distance_cm(2),
dist4 : rangefinder.distance_cm(3)
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an RCIN packet
void DataFlash_Class::Log_Write_RCIN(void)
{
@ -820,6 +835,19 @@ void DataFlash_Class::Log_Write_Baro(AP_Baro &baro)
WriteBlock(&pkt2, sizeof(pkt2));
}
#endif
#if BARO_MAX_INSTANCES > 2
if (baro.num_instances() > 2 && baro.healthy(2)) {
struct log_BARO pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_BAR3_MSG),
time_us : time_us,
altitude : baro.get_altitude(2),
pressure : baro.get_pressure(2),
temperature : (int16_t)(baro.get_temperature(2) * 100),
climbrate : baro.get_climb_rate()
};
WriteBlock(&pkt3, sizeof(pkt3));
}
#endif
}
// Write an raw accel/gyro data packet