Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: fixed accel and gyro sensor offsets in MAVLink

Browse Source
This commit is contained in:
Andrew Tridgell 2012-11-20 18:23:31 +11:00
parent f80783665f
commit 69fbcc8683
3 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
APMrover2/GCS_Mavlink.pde
View File

@ -378,16 +378,21 @@ static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
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(); Vector3f mag_offsets = compass.get_offsets();
Vector3f accel_offsets = ins.get_accel_offsets();
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,
mag_offsets.y, mag_offsets.y,
mag_offsets.z, mag_offsets.z,
compass.get_declination(), compass.get_declination(),
0, 0, 0,
0, gyro_offsets.x,
ins.gx(), ins.gy(), ins.gz(), gyro_offsets.y,
ins.ax(), ins.ay(), ins.az()); gyro_offsets.z,
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
} }
static void NOINLINE send_ahrs(mavlink_channel_t chan) static void NOINLINE send_ahrs(mavlink_channel_t chan)

10
ArduCopter/GCS_Mavlink.pde
View File

@ -442,6 +442,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
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(); Vector3f mag_offsets = compass.get_offsets();
Vector3f accel_offsets = ins.get_accel_offsets();
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,
@ -450,8 +452,12 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
compass.get_declination(), compass.get_declination(),
barometer.get_raw_pressure(), barometer.get_raw_pressure(),
barometer.get_raw_temp(), barometer.get_raw_temp(),
ins.gx(), ins.gy(), ins.gz(), gyro_offsets.x,
ins.ax(), ins.ay(), ins.az()); gyro_offsets.y,
gyro_offsets.z,
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
} }
static void NOINLINE send_gps_status(mavlink_channel_t chan) static void NOINLINE send_gps_status(mavlink_channel_t chan)

10
ArduPlane/GCS_Mavlink.pde
View File

@ -405,6 +405,8 @@ static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
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(); Vector3f mag_offsets = compass.get_offsets();
Vector3f accel_offsets = ins.get_accel_offsets();
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,
@ -413,8 +415,12 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
compass.get_declination(), compass.get_declination(),
barometer.get_raw_pressure(), barometer.get_raw_pressure(),
barometer.get_raw_temp(), barometer.get_raw_temp(),
ins.gx(), ins.gy(), ins.gz(), gyro_offsets.x,
ins.ax(), ins.ay(), ins.az()); gyro_offsets.y,
gyro_offsets.z,
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
} }
static void NOINLINE send_ahrs(mavlink_channel_t chan) static void NOINLINE send_ahrs(mavlink_channel_t chan)