@@ -2175,7 +2175,6 @@ void GCS_MAVLINK::send_raw_imu()
2175
2175
#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
2176
2176
void GCS_MAVLINK::send_highres_imu ()
2177
2177
{
2178
- // static const uint16_t HIGHRES_IMU_UPDATED_NONE = 0x00;
2179
2178
static const uint16_t HIGHRES_IMU_UPDATED_XACC = 0x01 ;
2180
2179
static const uint16_t HIGHRES_IMU_UPDATED_YACC = 0x02 ;
2181
2180
static const uint16_t HIGHRES_IMU_UPDATED_ZACC = 0x04 ;
@@ -2189,7 +2188,6 @@ void GCS_MAVLINK::send_highres_imu()
2189
2188
static const uint16_t HIGHRES_IMU_UPDATED_DIFF_PRESSURE = 0x400 ;
2190
2189
static const uint16_t HIGHRES_IMU_UPDATED_PRESSURE_ALT = 0x800 ;
2191
2190
static const uint16_t HIGHRES_IMU_UPDATED_TEMPERATURE = 0x1000 ;
2192
- static const uint16_t HIGHRES_IMU_UPDATED_ALL = 0xFFFF ;
2193
2191
2194
2192
const AP_InertialSensor &ins = AP::ins ();
2195
2193
const Vector3f& accel = ins.get_accel ();
@@ -2214,8 +2212,7 @@ void GCS_MAVLINK::send_highres_imu()
2214
2212
HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO),
2215
2213
.id = ins.get_first_usable_accel (),
2216
2214
};
2217
-
2218
-
2215
+
2219
2216
#if AP_COMPASS_ENABLED
2220
2217
const Compass &compass = AP::compass ();
2221
2218
if (compass.get_count () >= 1 ) {
@@ -2236,14 +2233,6 @@ void GCS_MAVLINK::send_highres_imu()
2236
2233
reply.fields_updated |= (HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE |
2237
2234
HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE);
2238
2235
#endif
2239
- static const uint16_t all_flags = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC |
2240
- HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO |
2241
- HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG |
2242
- HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE |
2243
- HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE);
2244
- if (reply.fields_updated == all_flags) {
2245
- reply.fields_updated |= HIGHRES_IMU_UPDATED_ALL;
2246
- }
2247
2236
2248
2237
mavlink_msg_highres_imu_send_struct (chan, &reply);
2249
2238
}
0 commit comments