GCS_MAVLink: support HIGHRES_IMU

[KoehlerT]

trafficstars

Duplicates #24258 It allows a companion computer to query the flightcontrollers IMU for navigation and control. This PR implements all fields for the HIGHRES_IMU Mavlink message.

[KoehlerT]

Thanks for all the (really quick!) suggestions and improvements :-) I now refrained from implementing sending every gyros/magnetometers high-res data because it would be quite a large addition into the code base, because it is (as far as I can see) not a usual control flow to send a response over many iterations and - more importantly - it would create a lot of load on the communication link. Usually, applications which want the high resolution data also want a high time resolution (at least in my application). Getting the current data is more important than waiting for the values for all accelerometers to arrive.

I am open for discussing this more :)

[tridge]

do you want the mag, baro airspeed data etc? should we do a true IMU_DATA msg, instanced, mavlink2, just IMU?

[KoehlerT]

do you want the mag, baro airspeed data etc? should we do a true IMU_DATA msg, instanced, mavlink2, just IMU?

Yes, I want it. It is used in outdoor applications for SLAM etc. I would also prefer to keep it implemented as good as we can according to the protocol spec to not cause confusion when others use this message. PR #24258 was rejected, because it didn't implement all the fields.

[KoehlerT]

@tridge @peterbarker @magicrub I have implemented your improvements (by amending the commit) and think this PR is mergeable. Could you please merge it? Or tell me the proecess on how it can be merged?

[shubham-shahh]

What's the difference between HIGHRES_IMU SCALED_IMU RAW_IMU

[KoehlerT]

@shubham-shahh there are some subtle differences: SCALED IMU the values are converted to other units and rounded to uint16_t, so they are not as accurate as HIGHRES_IMU, but use less bandwidth and are human readable.

RAW IMU these values are ment for debugging and do not contain any additional transformations (apart from calibration) to get SI units. Therefore it is much harder for other systems to work with those values.

HIGHRES IMU provides all values of gyro, accelerometer, compass and barometer at once, in units which can be easiliy understood by downstream programs (e.g. a companion computer), but without rounding and all values at once, making additional position estimation by those programs easier.

[shubham-shahh]

@shubham-shahh there are some subtle differences: SCALED IMU the values are converted to other units and rounded to uint16_t, so they are not as accurate as HIGHRES_IMU, but use less bandwidth and are human readable.

RAW IMU these values are ment for debugging and do not contain any additional transformations (apart from calibration) to get SI units. Therefore it is much harder for other systems to work with those values.

HIGHRES IMU provides all values of gyro, accelerometer, compass and barometer at once, in units which can be easiliy understood by downstream programs (e.g. a companion computer), but without rounding and all values at once, making additional position estimation by those programs easier.

Thanks for the detailed response. Are the HIGHRES IMU values offset to a particular frame before they are sent? and for a particular message pack, the accels, gyros, and compass values are time synced?

[KoehlerT]

I don't know if I understand your comment correctly. So there is no additional special care taken to transmit the messages within one mavlink frame. So the functionality I am looking for is to get all IMU values, in full resolution, without rounding at one point in time with timestamp for processing on a companion computer.

I am not the only one profiting from this inclusion (#24258), I think it is good if ardupilot tries to support the MAVLink protocol as good as possible. If you'd like we can talk about the changes if you are unsure they are necessary/wrongly implemented.

[shubham-shahh]

I don't know if I understand your comment correctly. So there is no additional special care taken to transmit the messages within one mavlink frame. So the functionality I am looking for is to get all IMU values, in full resolution, without rounding at one point in time with timestamp for processing on a companion computer.

I am not the only one profiting from this inclusion (#24258), I think it is good if ardupilot tries to support the MAVLink protocol as good as possible. If you'd like we can talk about the changes if you are unsure they are necessary/wrongly implemented.

No, What I meant is, since these values are meant to be uitlised by Intertial SLAM algorithms, I was trying to understand in terms of the transformations that are already applied to these values to be considered while calibrating the Extrensics and also understanding when are these messages stamped since time sync is also a critical aspect when fusing these values for localisation.

[KoehlerT]

I am not a pro regarding SLAM or MAVLink, but as far as I understand there are several systems in mavlink which read the sensors and update the values with no way of knowing how 'old'/'new' respective values are.

When the answer to the HIGHRES_IMU frame is constructed, all current values are read at once and copied into the answer struct, alongside the current time. Then it is transmitted and the downstream computer knows, where Ardupilot thought to be at this specific point in time. So there is a common time reference for every message/value, which just simplifies signal processing.

Doing SLAM integrating accelerometer values etc. is problematic in itself because the poll rate going through OS -> MavLink -> Uart -> Mavlink -> Ardupilot is pretty limiting.

Regarding transformations of values, they are transformed using the calibration and conversion formulars of the sensors. This is important to abstract the sensor specific code and calibrations away.

The values are not only/really used for SLAM. They are important for aligning data from the flight (altitude, acceleration, heading, ...) with data from other measurement systems controlled by the companion computer. I cannot go into the details on how the signal processing is going to work and it is also not important for this particular PR.

Imagine a big measurement system on a UAV, with ardupilot just acting as a sensor, providing a set of values. Those values need to be as accurate as possible and without sensor specific offsets/scalings.

[peterbarker]

Thanks for the detailed response. Are the HIGHRES IMU values offset to a particular frame before they are sent? and for a particular message pack, the accels, gyros, and compass values are time synced?

I think you're asking whether the IMU data is translated to avoid any lever-arm effect. It is.

[peterbarker]

We really do need to think about sending data in this message like we do for SCALED_IMU.

That is, we should be sending data from the second compass and baro, rather than just sending from the first healthy instance of each.

[peterbarker]

@KoehlerT we've merged this so people have something to iterate on, but if you could consider PR'ing something which gets us all of the sensor data that would be good. It would be nice to be able to sunset RAW_IMU and SCALED_IMU messages.

[lida2003]

AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED is defined in "GCS_config.h", but not included in "ap_message.h".

I think it should be removed from "ap_message.h".

PS: 4.5.6 stable + cherry-pick version

In file included from ../../libraries/GCS_MAVLink/MissionItemProtocol.h:5,
                 from ../../libraries/GCS_MAVLink/MissionItemProtocol_Rally.h:7,
                 from ../../libraries/AP_Filesystem/AP_Filesystem_Mission.cpp:28:
../../libraries/GCS_MAVLink/ap_message.h:93:5: error: "AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED" is not defined, evaluates to 0 [-Werror=undef]
   93 | #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
      |     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated due to -Wfatal-errors.
cc1plus: all warnings being treated as errors

[tejalbarnwal]

I tested the master branch for the HighRes IMU feature in SITL. While the MAVLink message structure is set up, ArduPilot is not currently publishing the message, as it's not visible in the MAVLink Inspector on QGC.

I have a couple of suggestions in mind to address this and more other to discuss:

• Setting stream rate for HighRes IMU msg:
  • We could include MSG_HIGHRES_IMU as part of one of the existing stream messages, like raw sensors, extended status, position, extra1, extra2, or extra3. This would allow us to set the stream rate for this message.
  • Another option might be to provide a dedicated stream rate parameter for MSG_HIGHRES_IMU, separate from the existing streams. This could give us finer control over the data frequency without affecting other sensor data streams.
• Timestamping HighRes IMU msg frame:
  • When constructing the HIGHRES_IMU message frame, all current sensor values are read at once and copied into the response structure along with the current time.
  • Instead, how about we timestamp the HighRes IMU message with the inertial sensor’s last update timestamp using AP::ins().get_last_update_usec()? This would ensure that the timestamp reflects the most recent IMU data.
  • However, since the HIGHRES_IMU message also includes data from the compass and barometer, which are updated at different frequencies, I am concerned about potential desynchronization. Specifically for systems with 6-DOF IMU and different compass, ensuring synchronized timestamps across all sensors is crucial to maintaining consistency in the data

Additional comment: If I specifically talk about my use case of using HighRes IMU msg for Visual/ Lidar Inertial odometry/ SLAM algorithms, accurate timestamping of the IMU data is critical for proper sensor fusion with visual data. The IMU’s timestamp should reflect the most recent measurement to ensure alignment with the visual/ lidar data. The slower sensors (compass and barometer) might not need to be as tightly synchronized, but the primary concern is the alignment of the IMU data.

Please let me know what you guys think in regard to the above concerns. I am also willing to contribute to the code to address both of these concerns.

[lida2003]

I tested the master branch for the HighRes IMU feature in SITL.

Did you try 100Hz or 200Hz setup in SITL? As I have found that it can't be set 100Hz in Rover.

[KoehlerT]

Is it possible to request the HIGHRES_IMU data? With code like e.g. this?

import time

# Import mavutil - Install the dependency if not installed
from pymavlink import mavutil


# Create the connection
master = mavutil.mavlink_connection('COM15',baud=57600)
# Wait a heartbeat before sending commands
master.wait_heartbeat()

# Request all parameters
def request_message_interval(message_id: int, frequency_hz: float):
    master.mav.command_long_send(
        master.target_system, master.target_component,
        mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
        message_id,  # The MAVLink message ID
        1e6 / frequency_hz,  # The interval between two messages in microseconds.
        0, 0, 0, 0,  # Unused parameters
        0,  # Target address of message stream 0: Flight-stack default (recommended)
    )

request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_HIGHRES_IMU, 100)

while True:
    try:
        imu = master.recv_match(type='HIGHRES_IMU', blocking=True)
        print("imu: " +str(imu))
    except Exception as e:
        print(repr(e))
    time.sleep(1)

I am currently working on getting SITL working for myself

[lida2003]

Hi,

I want to ask what's the difference between barometer.get_altitude() in 4.5.6 and barometer.get_altitude_AMSL() in 4.6-dev?

As I have ported this PR back to 4.5.6, I'm NOT sure if it will make a mess.

[tejalbarnwal]

Is it possible to request the HIGHRES_IMU data? With code like e.g. this?

import time

# Import mavutil - Install the dependency if not installed
from pymavlink import mavutil


# Create the connection
master = mavutil.mavlink_connection('COM15',baud=57600)
# Wait a heartbeat before sending commands
master.wait_heartbeat()

# Request all parameters
def request_message_interval(message_id: int, frequency_hz: float):
    master.mav.command_long_send(
        master.target_system, master.target_component,
        mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, 0,
        message_id,  # The MAVLink message ID
        1e6 / frequency_hz,  # The interval between two messages in microseconds.
        0, 0, 0, 0,  # Unused parameters
        0,  # Target address of message stream 0: Flight-stack default (recommended)
    )

request_message_interval(mavutil.mavlink.MAVLINK_MSG_ID_HIGHRES_IMU, 100)

while True:
    try:
        imu = master.recv_match(type='HIGHRES_IMU', blocking=True)
        print("imu: " +str(imu))
    except Exception as e:
        print(repr(e))
    time.sleep(1)

I am currently working on getting SITL working for myself

My main aim to do this from OBC, which already uses MAVROS to communicate to FCU. I figured that out that one can use /mavros/set_message_interval to set the data rate for HighRes IMU message.

@KoehlerT , let me know your thoughts on Timestamping HighRes IMU msg frame with the inertial sensor’s last update timestamp.