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

Copter: log up to 3 mags

Browse Source
This commit is contained in:
Andrew Tridgell 2014-07-04 12:07:59 +10:00
parent c138244155
commit 21b0b3bc15
2 changed files with 28 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopter/Log.pde
View File

@ -393,6 +393,27 @@ static void Log_Write_Compass()
DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
}
#endif
#if COMPASS_MAX_INSTANCES > 2
if (compass.get_count() > 2) {
const Vector3f &mag3_offsets = compass.get_offsets(2);
const Vector3f &mag3_motor_offsets = compass.get_motor_offsets(2);
const Vector3f &mag3 = compass.get_field(2);
struct log_Compass pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS3_MSG),
time_ms : hal.scheduler->millis(),
mag_x : (int16_t)mag3.x,
mag_y : (int16_t)mag3.y,
mag_z : (int16_t)mag3.z,
offset_x : (int16_t)mag3_offsets.x,
offset_y : (int16_t)mag3_offsets.y,
offset_z : (int16_t)mag3_offsets.z,
motor_offset_x : (int16_t)mag3_motor_offsets.x,
motor_offset_y : (int16_t)mag3_motor_offsets.y,
motor_offset_z : (int16_t)mag3_motor_offsets.z
};
DataFlash.WriteBlock(&pkt3, sizeof(pkt3));
}
#endif
}
struct PACKED log_Performance {
@ -649,8 +670,14 @@ static const struct LogStructure log_structure[] PROGMEM = {
"CTUN", "Ihhhffecchh", "TimeMS,ThrIn,AngBst,ThrOut,DAlt,Alt,BarAlt,DSAlt,SAlt,DCRt,CRt" },
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
#if COMPASS_MAX_INSTANCES > 1
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
"MAG2","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
#endif
#if COMPASS_MAX_INSTANCES > 2
{ LOG_COMPASS3_MSG, sizeof(log_Compass),
"MAG3","Ihhhhhhhhh", "TimeMS,MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
#endif
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "HHIhBHB", "NLon,NLoop,MaxT,PMT,I2CErr,INSErr,INAVErr" },
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),

1
ArduCopter/defines.h
View File

@ -230,6 +230,7 @@ enum FlipState {
#define LOG_AUTOTUNE_MSG 0x19
#define LOG_AUTOTUNEDETAILS_MSG 0x1A
#define LOG_COMPASS2_MSG 0x1B
#define LOG_COMPASS3_MSG 0x1C
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)