AP_DDS: Support IMU+gyro+accel data for rate control or offboard fusion

[Ryanf55]

From AP_AHRS, the data is filtered and good for rate control. This is the data after notch+low pass, phase lag and all. This goes on imu_filtered topic.

For offboard fusion, we can do it "badly". We would need delta angle, delta velocity, all the data products from INS that are not filtered. We have to know the lag to do delayed time fusion with lag on the GPS. This would be on imu_raw.

Current EKF does delayed time fusion (in delayed time) with forward propagation to the current time. Doesn't work as well in dynamic flight. Researchers want to do this.

Adding things like vision odometry back into ArduPilot (visual mavlink odometry) - this is preferred for performance so ArduPilot's EKF can fuse the data.

[Astik-2002]

Hello Ryan. I would like to work on this issue as I have an interest in sensor fusion and control. Can you share in more detail what needs to be implemented?

[Astik-2002]

Here's the proposed workflow for this issue.
I have broken down the problem into these parts
a) receive raw imu readings from AP
b) enable body frame rate setpoints in guided mode for multirotors (later, for other systems)
c) fuse imu data with other sensor readings for attitude estimation and send the attitude estimate back to AP

[Astik-2002]

Solutions:
a) Raw_IMU mavlink msg for imu data transmission. Frequency of msg may need to be changed depending on application
b) populate the rate_controller_run() function in mode_guided.cpp and enable bf rate setpoints in SET_ATTITUDE_TARGET msg
c) use EKF or unscented KF for attitude estimation offboard, then send the values back to AP via mavlink, at the frequency required by angle controller

[Ryanf55]

This approach sounds good! Can you elaborate on which libraries and functions you plan to call in AP to make this work?

Additionally, can you compare it to the corresponding MAVlink ways of doing the same thing today, assuming they exist?

Your proposed workflow step c of fusing the data and sending an estimate back can be later. Let's get the interface designed and prototyped out.

[Astik-2002]

@Ryanf55 For offboard rate control
The approach simply involves calling the rate_controller_run function in mode_guided.cpp when a set_attitude_target msg is received with an appropriate typemask (ignoring attitude commands and keeping rate and thrust).

I don't think any additional libraries need to be added in AP to perform this, as the rate commands will be generated offboard. We however may need to increase the frequency of mavlink msg to allow smooth control.

For attitude estimation, I'm not sure if outside attitude estimates are accepted by AP. I'm also not sure of a mavlink msg that sends attitude estimates back to AP (there are those that receive attitude data from AP though).

[haarshitgarg]

Hello, @Ryanf55

Guessing this issue is still open I would like to work on this.
If I am not wrong what we want is to off board the processing of raw IMU data and get it back, processed and at a certain frequency, for rate control.

I have noticed that ArduPilot right now is publishing a topic /ap/imu/experimental , which I guess is raw IMU data. We can similarly subscribe to a similar topic which would return processed data using the raw IMU data already being published.
These messages Can be used in AP_Inertial I believe as it is handling the IMU data right now.

Am I missing something?