Skip to content

Commit

Permalink
GCS_MAVLink: support HIGHRES_IMU
Browse files Browse the repository at this point in the history
HIGHRES_IMU MAVLink message. Enable, when BOARD_FLASH_SIZE is > 1024
  • Loading branch information
KoehlerT committed May 8, 2024
1 parent bcf0733 commit 5b841dc
Show file tree
Hide file tree
Showing 4 changed files with 104 additions and 0 deletions.
20 changes: 20 additions & 0 deletions libraries/GCS_MAVLink/GCS.h
Original file line number Diff line number Diff line change
Expand Up @@ -354,6 +354,9 @@ class GCS_MAVLINK
void send_rc_channels() const;
void send_rc_channels_raw() const;
void send_raw_imu();
#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
void send_highres_imu();
#endif

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();
Expand Down Expand Up @@ -1400,3 +1403,20 @@ enum MAV_SEVERITY

#endif // HAL_GCS_ENABLED

#if AP_MAVLINK_MSG_HIGHRES_IMU_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
#endif
77 changes: 77 additions & 0 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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},
Expand Down Expand Up @@ -2127,6 +2130,74 @@ void GCS_MAVLINK::send_raw_imu()
#endif
}

#if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
void GCS_MAVLINK::send_highres_imu()
{
uint16_t fields_updated = HIGHRES_IMU_UPDATED_NONE;
Vector3f accel = Vector3f();
Vector3f gyro = Vector3f();
uint8_t accel_id = 0;
#if AP_INERTIALSENSOR_ENABLED
const AP_InertialSensor &ins = AP::ins();
accel = ins.get_accel();
gyro = ins.get_gyro();
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;
accel_id = ins.get_primary_accel();
#endif

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();
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
// set to HIGHRES_IMU_UPDATED_ALL
fields_updated = fields_updated == 0x0FFFF ? 0xFFFF : fields_updated;

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 == 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) ? HIGHRES_IMU_UPDATED_ALL : 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
Expand Down Expand Up @@ -6180,6 +6251,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);
Expand Down
4 changes: 4 additions & 0 deletions libraries/GCS_MAVLink/GCS_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -102,3 +102,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)
#endif
3 changes: 3 additions & 0 deletions libraries/GCS_MAVLink/ap_message.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
};

0 comments on commit 5b841dc

Please sign in to comment.