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

Rover: added logging of 2nd compass

This commit is contained in:
Andrew Tridgell 2013-12-10 13:34:53 +11:00
parent cc3ee2ec0c
commit beb9bffcf2
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
APMrover2/Log.pde
View File

@ -447,6 +447,26 @@ static void Log_Write_Compass()
motor_offset_z : (int16_t)mag_motor_offsets.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
#if COMPASS_MAX_INSTANCES > 1
if (compass.get_count() > 1) {
const Vector3f &mag2_offsets = compass.get_offsets(1);
const Vector3f &mag2_motor_offsets = compass.get_motor_offsets(1);
const Vector3f &mag2 = compass.get_field(1);
struct log_Compass pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS2_MSG),
mag_x : (int16_t)mag2.x,
mag_y : (int16_t)mag2.y,
mag_z : (int16_t)mag2.z,
offset_x : (int16_t)mag2_offsets.x,
offset_y : (int16_t)mag2_offsets.y,
offset_z : (int16_t)mag2_offsets.z,
motor_offset_x : (int16_t)mag2_motor_offsets.x,
motor_offset_y : (int16_t)mag2_motor_offsets.y,
motor_offset_z : (int16_t)mag2_motor_offsets.z
};
DataFlash.WriteBlock(&pkt2, sizeof(pkt2));
}
#endif
}
@ -474,6 +494,8 @@ static const struct LogStructure log_structure[] PROGMEM = {
"MODE", "MB", "Mode,ModeNum" },
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_COMPASS2_MSG, sizeof(log_Compass),
"MAG2", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
};

1
APMrover2/defines.h
View File

@ -125,6 +125,7 @@ enum ap_message {
#define LOG_MODE_MSG 0x09
#define LOG_COMPASS_MSG 0x0A
#define LOG_CAMERA_MSG 0x0B
#define LOG_COMPASS2_MSG 0x0C
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01