Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: added Log_Write_POS() call 

write AHRS position to DF log
This commit is contained in:
Andrew Tridgell 2015-05-15 11:59:40 +10:00
parent 474ee2a11e
commit 74485c5754
2 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/DataFlash/DataFlash.h
View File

@ -68,6 +68,7 @@ public:
void Log_Write_Baro(AP_Baro &baro);
void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
void Log_Write_POS(AP_AHRS &ahrs);
#if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
#endif
@ -262,6 +263,15 @@ struct PACKED log_AHRS {
int32_t lng;
};
struct PACKED log_POS {
LOG_PACKET_HEADER;
uint32_t time_ms;
int32_t lat;
int32_t lng;
float alt;
float rel_alt;
};
struct PACKED log_POWR {
LOG_PACKET_HEADER;
uint32_t time_ms;
@ -600,6 +610,8 @@ Format characters in the format string for binary log messages
"IMU3", "IffffffIIf", "TimeMS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,ErrG,ErrA,Temp" }, \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
"AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
{ LOG_POS_MSG, sizeof(log_POS), \
"POS","IIIff","TimeMS,Lat,Lng,Alt,RelAlt" }, \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
{ LOG_EKF1_MSG, sizeof(log_EKF1), \
@ -714,6 +726,7 @@ Format characters in the format string for binary log messages
#define LOG_GYR1_MSG 175
#define LOG_GYR2_MSG 176
#define LOG_GYR3_MSG 177
#define LOG_POS_MSG 178
// message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use

20
libraries/DataFlash/LogFile.cpp
View File

@ -904,6 +904,26 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
WriteBlock(&pkt, sizeof(pkt));
}
// Write a POS packet
void DataFlash_Class::Log_Write_POS(AP_AHRS &ahrs)
{
Location loc;
if (!ahrs.get_position(loc)) {
return;
}
Vector3f pos;
ahrs.get_relative_position_NED(pos);
struct log_POS pkt = {
LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
time_ms : hal.scheduler->millis(),
lat : loc.lat,
lng : loc.lng,
alt : loc.alt*1.0e-2f,
rel_alt : -pos.z
};
WriteBlock(&pkt, sizeof(pkt));
}
#if AP_AHRS_NAVEKF_AVAILABLE
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
{