GCS_MAVLink: supply IMU temperature in mavlink extensions

This commit is contained in:
Andrew Tridgell 2019-10-08 14:22:12 +11:00
parent dc4ad26e96
commit 29e65a7294
1 changed files with 3 additions and 3 deletions

View File

@ -1602,8 +1602,8 @@ void GCS_MAVLINK::send_raw_imu()
mag.x,
mag.y,
mag.z,
0, // for now, only instance 0 is sent
0); // TODO: get the IMU temperature
0, // we use SCALED_IMU and SCALED_IMU2 for other IMUs
int16_t(ins.get_temperature(0)*100));
}
void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature))
@ -1642,7 +1642,7 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan
mag.x,
mag.y,
mag.z,
0); // TODO: get the IMU temperature
int16_t(ins.get_temperature(instance)*100));
}