aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Logger: use instance number for Mag logging

Browse Source
This commit is contained in:
Peter Barker 2020-11-02 12:46:43 +11:00 committed by Peter Barker
parent 12f0e9dd43
commit 48385b82dc
3 changed files with 17 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Logger/AP_Logger.h
View File

@ -460,9 +460,7 @@ private:
void Write_Baro_instance(uint64_t time_us, uint8_t baro_instance);
void Write_IMU_instance(uint64_t time_us, uint8_t imu_instance);
void Write_Compass_instance(uint64_t time_us,
uint8_t mag_instance,
enum LogMessages type);
void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
void Write_Current_instance(uint64_t time_us, uint8_t battery_instance);
void backend_starting_new_log(const AP_Logger_Backend *backend);

19
libraries/AP_Logger/LogFile.cpp
View File

@ -663,16 +663,17 @@ void AP_Logger::Write_Current()
}
}
void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance, const enum LogMessages type)
void AP_Logger::Write_Compass_instance(const uint64_t time_us, const uint8_t mag_instance)
{
const Compass &compass = AP::compass();
const Vector3f &mag_field = compass.get_field(mag_instance);
const Vector3f &mag_offsets = compass.get_offsets(mag_instance);
const Vector3f &mag_motor_offsets = compass.get_motor_offsets(mag_instance);
const struct log_Compass pkt{
LOG_PACKET_HEADER_INIT(type),
const struct log_MAG pkt{
LOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
time_us : time_us,
instance : mag_instance,
mag_x : (int16_t)mag_field.x,
mag_y : (int16_t)mag_field.y,
mag_z : (int16_t)mag_field.z,
@ -695,16 +696,8 @@ void AP_Logger::Write_Compass(uint64_t time_us)
time_us = AP_HAL::micros64();
}
const Compass &compass = AP::compass();
if (compass.get_count() > 0) {
Write_Compass_instance(time_us, 0, LOG_COMPASS_MSG);
}
if (compass.get_count() > 1) {
Write_Compass_instance(time_us, 1, LOG_COMPASS2_MSG);
}
if (compass.get_count() > 2) {
Write_Compass_instance(time_us, 2, LOG_COMPASS3_MSG);
for (uint8_t i=0; i<compass.get_count(); i++) {
Write_Compass_instance(time_us, i);
}
}

29
libraries/AP_Logger/LogStructure.h
View File

@ -838,9 +838,10 @@ struct PACKED log_Current_Cells {
uint16_t cell_voltages[12];
};
struct PACKED log_Compass {
struct PACKED log_MAG {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t instance;
int16_t mag_x;
int16_t mag_y;
int16_t mag_z;
@ -1337,11 +1338,6 @@ struct PACKED log_PSC {
#define ISBD_UNITS "s--ooo"
#define ISBD_MULTS "F--???"
#define MAG_LABELS "TimeUS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ,Health,S"
#define MAG_FMT "QhhhhhhhhhBI"
#define MAG_UNITS "sGGGGGGGGG-s"
#define MAG_MULTS "FCCCCCCCCC-F"
#define PID_LABELS "TimeUS,Tar,Act,Err,P,I,D,FF,Dmod"
#define PID_FMT "Qffffffff"
#define PID_UNITS "s--------"
@ -1759,18 +1755,19 @@ struct PACKED log_PSC {
// @Field: LandingGear: Current landing gear state
// @Field: WeightOnWheels: True if there is weight on wheels
// @LoggerMessage: MAG,MAG2,MAG3
// @LoggerMessage: MAG
// @Description: Information received from compasses
// @Field: TimeUS: Time since system startup
// @Field: I: magnetometer sensor instance number
// @Field: MagX: magnetic field strength in body frame
// @Field: MagY: magnetic field strength in body frame
// @Field: MagZ: magnetic field strength in body frame
// @Field: OfsX: magnetic field offset in body frame
// @Field: OfsY: magnetic field offset in body frame
// @Field: OfsZ: magnetic field offset in body frame
// @Field: MOfsX: motor interference magnetic field offset in body frame
// @Field: MOfsY: motor interference magnetic field offset in body frame
// @Field: MOfsZ: motor interference magnetic field offset in body frame
// @Field: MOX: motor interference magnetic field offset in body frame
// @Field: MOY: motor interference magnetic field offset in body frame
// @Field: MOZ: motor interference magnetic field offset in body frame
// @Field: Health: true if the compass is considered healthy
// @Field: S: time measurement was taken
@ -2553,8 +2550,8 @@ struct PACKED log_PSC {
"BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" }, \
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),\
"ATT", "QccccCCCC", "TimeUS,DesRoll,Roll,DesPitch,Pitch,DesYaw,Yaw,ErrRP,ErrYaw", "sddddhhdh", "FBBBBBBBB" }, \
{ LOG_COMPASS_MSG, sizeof(log_Compass), \
"MAG", MAG_FMT, MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
{ LOG_MAG_MSG, sizeof(log_MAG), \
"MAG", "QBhhhhhhhhhBI", "TimeUS,I,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOX,MOY,MOZ,Health,S", "s#GGGGGGGGG-s", "F-CCCCCCCCC-F" }, \
{ LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
{ LOG_RFND_MSG, sizeof(log_RFND), \
@ -2633,10 +2630,6 @@ struct PACKED log_PSC {
"CSRV","QBfffB","TimeUS,Id,Pos,Force,Speed,Pow", "s#---%", "F-0000" }, \
{ LOG_CESC_MSG, sizeof(log_CESC), \
"CESC","QBIfffiB","TimeUS,Id,ECnt,Voltage,Curr,Temp,RPM,Pow", "s#-vAOq%", "F-000000" }, \
{ LOG_COMPASS2_MSG, sizeof(log_Compass), \
"MAG2",MAG_FMT, MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
{ LOG_COMPASS3_MSG, sizeof(log_Compass), \
"MAG3",MAG_FMT, MAG_LABELS, MAG_UNITS, MAG_MULTS }, \
{ LOG_ACC_MSG, sizeof(log_ACC), \
"ACC", "QBQfff", "TimeUS,I,SampleUS,AccX,AccY,AccZ", "s#sooo", "F-F000" }, \
{ LOG_GYR_MSG, sizeof(log_GYR), \
@ -2784,9 +2777,7 @@ enum LogMessages : uint8_t {
LOG_ATTITUDE_MSG,
LOG_CURRENT_MSG,
LOG_CURRENT_CELLS_MSG,
LOG_COMPASS_MSG,
LOG_COMPASS2_MSG,
LOG_COMPASS3_MSG,
LOG_MAG_MSG,
LOG_MODE_MSG,
LOG_GPS_RAW_MSG,