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

Rover: added compass logging

This commit is contained in:
Andrew Tridgell 2013-04-19 23:54:26 +10:00
parent c56f338fe0
commit 7717cc1450
4 changed files with 50 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
APMrover2/APMrover2.pde
View File

@ -679,6 +679,9 @@ static void medium_loop()
ahrs.set_compass(&compass);
// Calculate heading
compass.null_offsets();
if (g.log_bitmask & MASK_LOG_COMPASS) {
Log_Write_Compass();
}
} else {
ahrs.set_compass(NULL);
}

36
APMrover2/Log.pde
View File

@ -49,6 +49,7 @@ print_log_menu(void)
PLOG(CMD);
PLOG(CURRENT);
PLOG(SONAR);
PLOG(COMPASS);
#undef PLOG
}
@ -137,6 +138,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(CMD);
TARG(CURRENT);
TARG(SONAR);
TARG(COMPASS);
#undef TARG
}
@ -404,6 +406,40 @@ static void Log_Write_Current()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
struct PACKED log_Compass {
LOG_PACKET_HEADER;
int16_t mag_x;
int16_t mag_y;
int16_t mag_z;
int16_t offset_x;
int16_t offset_y;
int16_t offset_z;
int16_t motor_offset_x;
int16_t motor_offset_y;
int16_t motor_offset_z;
};
// Write a Compass packet. Total length : 15 bytes
static void Log_Write_Compass()
{
Vector3f mag_offsets = compass.get_offsets();
Vector3f mag_motor_offsets = compass.get_motor_offsets();
struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
mag_x : compass.mag_x,
mag_y : compass.mag_y,
mag_z : compass.mag_z,
offset_x : (int16_t)mag_offsets.x,
offset_y : (int16_t)mag_offsets.y,
offset_z : (int16_t)mag_offsets.z,
motor_offset_x : (int16_t)mag_motor_offsets.x,
motor_offset_y : (int16_t)mag_motor_offsets.y,
motor_offset_z : (int16_t)mag_motor_offsets.z
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
static const struct LogStructure log_structure[] PROGMEM = {
LOG_COMMON_STRUCTURES,
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),

57
APMrover2/config.h
View File

@ -427,55 +427,16 @@
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
#ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED ENABLED
#endif
#ifndef LOG_GPS
# define LOG_GPS ENABLED
#endif
#ifndef LOG_PM
# define LOG_PM DISABLED
#endif
#ifndef LOG_CTUN
# define LOG_CTUN ENABLED
#endif
#ifndef LOG_NTUN
# define LOG_NTUN ENABLED
#endif
#ifndef LOG_MODE
# define LOG_MODE ENABLED
#endif
#ifndef LOG_IMU
# define LOG_IMU DISABLED
#endif
#ifndef LOG_CMD
# define LOG_CMD ENABLED
#endif
#ifndef LOG_CURRENT
# define LOG_CURRENT DISABLED
#endif
#ifndef LOG_SONAR
# define LOG_SONAR ENABLED
#endif
#define DEFAULT_LOG_BITMASK \
MASK_LOG_ATTITUDE_MED | \
MASK_LOG_GPS | \
MASK_LOG_PM | \
MASK_LOG_NTUN | \
MASK_LOG_MODE | \
MASK_LOG_CMD | \
MASK_LOG_SONAR | \
MASK_LOG_COMPASS
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(IMU) | \
LOGBIT(CMD) | \
LOGBIT(SONAR) | \
LOGBIT(CURRENT)
//////////////////////////////////////////////////////////////////////////////

2
APMrover2/defines.h
View File

@ -135,6 +135,7 @@ enum gcs_severity {
#define LOG_SONAR_MSG 0x07
#define LOG_ATTITUDE_MSG 0x08
#define LOG_MODE_MSG 0x09
#define LOG_COMPASS_MSG 0x0A
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
@ -151,6 +152,7 @@ enum gcs_severity {
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9)
#define MASK_LOG_SONAR (1<<10)
#define MASK_LOG_COMPASS (1<<11)
// Waypoint Modes
// ----------------