diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 148757e0b49f0a..8d18be83a2b7b6 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -354,6 +354,7 @@ class GCS_MAVLINK void send_rc_channels() const; void send_rc_channels_raw() const; void send_raw_imu(); + void send_highres_imu(); void send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff)); void send_scaled_pressure(); @@ -1400,3 +1401,18 @@ enum MAV_SEVERITY #endif // HAL_GCS_ENABLED +#define HIGHRES_IMU_UPDATED_NONE 0x00 +#define HIGHRES_IMU_UPDATED_XACC 0x01 +#define HIGHRES_IMU_UPDATED_YACC 0x02 +#define HIGHRES_IMU_UPDATED_ZACC 0x04 +#define HIGHRES_IMU_UPDATED_XGYRO 0x08 +#define HIGHRES_IMU_UPDATED_YGYRO 0x10 +#define HIGHRES_IMU_UPDATED_ZGYRO 0x20 +#define HIGHRES_IMU_UPDATED_XMAG 0x40 +#define HIGHRES_IMU_UPDATED_YMAG 0x80 +#define HIGHRES_IMU_UPDATED_ZMAG 0x100 +#define HIGHRES_IMU_UPDATED_ABS_PRESSURE 0x200 +#define HIGHRES_IMU_UPDATED_DIFF_PRESSURE 0x400 +#define HIGHRES_IMU_UPDATED_PRESSURE_ALT 0x800 +#define HIGHRES_IMU_UPDATED_TEMPERATURE 0x1000 +#define HIGHRES_IMU_UPDATED_ALL 0xFFFF diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index fadf622bdc21e3..6995549b0e6ae5 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1005,6 +1005,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_SCALED_IMU, MSG_SCALED_IMU}, { MAVLINK_MSG_ID_SCALED_IMU2, MSG_SCALED_IMU2}, { MAVLINK_MSG_ID_SCALED_IMU3, MSG_SCALED_IMU3}, +#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED + { MAVLINK_MSG_ID_HIGHRES_IMU, MSG_HIGHRES_IMU}, +#endif { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, { MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2}, { MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3}, @@ -2127,6 +2130,69 @@ void GCS_MAVLINK::send_raw_imu() #endif } +#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED +void GCS_MAVLINK::send_highres_imu() +{ + const AP_InertialSensor &ins = AP::ins(); + const Vector3f& accel = ins.get_accel(); + const Vector3f& gyro = ins.get_gyro(); + uint16_t fields_updated = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC | + HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO); + const uint8_t accel_id = ins.get_primary_accel(); + + Vector3f mag = Vector3f(); +#if AP_COMPASS_ENABLED + const Compass &compass = AP::compass(); + if (compass.get_count() >= 1) { + mag = compass.get_field() * 1000.0f; // convert to gauss + fields_updated |= (HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG); + } +#endif + + float press_abs = 0.0f; + float temperature = 0.0f; + float altitude = 0.0f; + float diff_pressure = 0.0f; +#if AP_BARO_ENABLED + const AP_Baro &barometer = AP::baro(); + press_abs = barometer.get_pressure() * 0.01f; + temperature = barometer.get_temperature(); + altitude = barometer.get_altitude_AMSL(); + diff_pressure = press_abs - barometer.get_ground_pressure() * 0.01f; + fields_updated |= (HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE | + HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE); +#endif + const uint16_t all_flags = (HIGHRES_IMU_UPDATED_XACC | HIGHRES_IMU_UPDATED_YACC | HIGHRES_IMU_UPDATED_ZACC | + HIGHRES_IMU_UPDATED_XGYRO | HIGHRES_IMU_UPDATED_YGYRO | HIGHRES_IMU_UPDATED_ZGYRO | + HIGHRES_IMU_UPDATED_XMAG | HIGHRES_IMU_UPDATED_YMAG | HIGHRES_IMU_UPDATED_ZMAG | + HIGHRES_IMU_UPDATED_ABS_PRESSURE | HIGHRES_IMU_UPDATED_DIFF_PRESSURE | + HIGHRES_IMU_UPDATED_PRESSURE_ALT | HIGHRES_IMU_UPDATED_TEMPERATURE); + if (fields_updated == all_flags) { + fields_updated |= HIGHRES_IMU_UPDATED_ALL; + } + + mavlink_msg_highres_imu_send( + chan, + AP_HAL::micros64(), + accel.x, + accel.y, + accel.z, + gyro.x, + gyro.y, + gyro.z, + mag.x, + mag.y, + mag.z, + press_abs, + diff_pressure, + altitude, + temperature, + fields_updated, + accel_id + ); +} +#endif + 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)) { #if AP_INERTIALSENSOR_ENABLED @@ -6180,6 +6246,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(SCALED_IMU3); send_scaled_imu(2, mavlink_msg_scaled_imu3_send); break; +#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED + case MSG_HIGHRES_IMU: + CHECK_PAYLOAD_SIZE(HIGHRES_IMU); + send_highres_imu(); + break; +#endif case MSG_SCALED_PRESSURE: CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 64877c10649dd4..e9dfa3eabf4d3a 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -3,6 +3,7 @@ #include #include #include +#include #ifndef HAL_GCS_ENABLED #define HAL_GCS_ENABLED 1 @@ -102,3 +103,7 @@ #ifndef AP_MAVLINK_COMMAND_LONG_ENABLED #define AP_MAVLINK_COMMAND_LONG_ENABLED 1 #endif + +#ifndef AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED +#define AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED (BOARD_FLASH_SIZE > 1024) && AP_INERTIALSENSOR_ENABLED +#endif diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index 42724b12a2548c..4923317266025c 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -94,5 +94,8 @@ enum ap_message : uint8_t { MSG_HYGROMETER, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_RELAY_STATUS, +#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED + MSG_HIGHRES_IMU, +#endif MSG_LAST // MSG_LAST must be the last entry in this enum };