Rover: updates for compass API change

This commit is contained in:
Andrew Tridgell 2013-12-09 14:09:56 +11:00
parent cb16733918
commit 615e718524
3 changed files with 21 additions and 22 deletions

View File

@ -345,8 +345,9 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
static void NOINLINE send_raw_imu1(mavlink_channel_t chan) static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{ {
Vector3f accel = ins.get_accel(); const Vector3f &accel = ins.get_accel();
Vector3f gyro = ins.get_gyro(); const Vector3f &gyro = ins.get_gyro();
const Vector3f &mag = compass.get_field();
mavlink_msg_raw_imu_send( mavlink_msg_raw_imu_send(
chan, chan,
@ -357,16 +358,16 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
gyro.x * 1000.0, gyro.x * 1000.0,
gyro.y * 1000.0, gyro.y * 1000.0,
gyro.z * 1000.0, gyro.z * 1000.0,
compass.mag_x, mag.x,
compass.mag_y, mag.y,
compass.mag_z); mag.z);
} }
static void NOINLINE send_raw_imu3(mavlink_channel_t chan) static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{ {
Vector3f mag_offsets = compass.get_offsets(); const Vector3f &mag_offsets = compass.get_offsets();
Vector3f accel_offsets = ins.get_accel_offsets(); const Vector3f &accel_offsets = ins.get_accel_offsets();
Vector3f gyro_offsets = ins.get_gyro_offsets(); const Vector3f &gyro_offsets = ins.get_gyro_offsets();
mavlink_msg_sensor_offsets_send(chan, mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x, mag_offsets.x,
@ -384,7 +385,7 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
static void NOINLINE send_ahrs(mavlink_channel_t chan) static void NOINLINE send_ahrs(mavlink_channel_t chan)
{ {
Vector3f omega_I = ahrs.get_gyro_drift(); const Vector3f &omega_I = ahrs.get_gyro_drift();
mavlink_msg_ahrs_send( mavlink_msg_ahrs_send(
chan, chan,
omega_I.x, omega_I.x,

View File

@ -431,13 +431,14 @@ struct PACKED log_Compass {
// Write a Compass packet. Total length : 15 bytes // Write a Compass packet. Total length : 15 bytes
static void Log_Write_Compass() static void Log_Write_Compass()
{ {
Vector3f mag_offsets = compass.get_offsets(); const Vector3f &mag_offsets = compass.get_offsets();
Vector3f mag_motor_offsets = compass.get_motor_offsets(); const Vector3f &mag_motor_offsets = compass.get_motor_offsets();
const Vector3f &mag = compass.get_field();
struct log_Compass pkt = { struct log_Compass pkt = {
LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG), LOG_PACKET_HEADER_INIT(LOG_COMPASS_MSG),
mag_x : compass.mag_x, mag_x : (int16_t)mag.x,
mag_y : compass.mag_y, mag_y : (int16_t)mag.y,
mag_z : compass.mag_z, mag_z : (int16_t)mag.z,
offset_x : (int16_t)mag_offsets.x, offset_x : (int16_t)mag_offsets.x,
offset_y : (int16_t)mag_offsets.y, offset_y : (int16_t)mag_offsets.y,
offset_z : (int16_t)mag_offsets.z, offset_z : (int16_t)mag_offsets.z,

View File

@ -418,15 +418,12 @@ test_mag(uint8_t argc, const Menu::arg *argv)
counter++; counter++;
if (counter>20) { if (counter>20) {
if (compass.healthy) { if (compass.healthy) {
Vector3f maggy = compass.get_offsets(); const Vector3f mag_ofs = compass.get_offsets();
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"), const Vector3f mag = compass.get_field();
cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360_cd(ToDeg(heading) * 100)) /100, (wrap_360_cd(ToDeg(heading) * 100)) /100,
(int)compass.mag_x, mag.x, mag.y, mag.z,
(int)compass.mag_y, mag_ofs.x, mag_ofs.y, mag_ofs.z);
(int)compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
} else { } else {
cliSerial->println_P(PSTR("compass not healthy")); cliSerial->println_P(PSTR("compass not healthy"));
} }