IMU cleanup and secondary IMU filter for tracking true orientation

[surfdado]

This will be needed in order to determine the true orientation of the board while using a high-mahony-kp filter to control the balance behavior.

[surfdado]

@Mitchlol pls review/comment

[Mitchlol]

Not really reviewing, but just pointing out 2 things.

1) I'm assuming you want access to the true values in c code in a vesc_pkg, however the c code there can only interact with the vesc via the vesc_c_if. So if my assumption is correct, this code alone is not enough to achieve your goal.

2) This does seem fairly balance specific. Is it possible to pull the gyro/accel x/y/z in the app and duplicate the ahrs filter there? The mahony filter is a lot of code, so i do admit it's somewhat gross. If you don't need extreme precision or accuracy, something like the complementary filter might even be enough, and it's practically one liner. This is how i imagined implementing it when i was considering the use of the same data.

[surfdado]

To 1) understood, I am working on submitting a separate list of data/functions we need access to from the balance package - but I figured I first needed this to make it into the codebase... To 2) We still need clean, well filtered data for the true angles so I can't imagine that a much simpler filter will do the job, but tbh this is above my level of expertise. Replicating the whole IMU filter in the balance app makes me cringe, then we might as well just put all the IMU code into the package. Is it really a problem to have balance specific code in the IMU codebase? We could make it conditional and add a parameter to enable it, but that seems unnecessary to me.

[vedderb]

If you want to have two sets of parameters I don't like the approach of having them in ahrs.c and pass a flag to tell it which set to use. In that case it is much better to move the parameters into the ATTITUDE_INFO-struct and make ahrs state-less.

It would also be better to make it so that you can register an IMU read callback from packages and expose the attitude-info struct and ahrs-functions to VESC_IF so that you can use them to maintain your own attitude copy. That also gives you much more control over it from the package. I can look into that in the next days.

[vedderb]

Done: https://github.com/vedderb/bldc/commit/b8e8952e32c8ab28359027f39cbe85f4e7956f7f I didn't test the vesc_if-functions, but I'm fairly sure they should work.

[surfdado]

Done: b8e8952 I didn't test the vesc_if-functions, but I'm fairly sure they should work.

Let me make sure I understand. Judging by the new lispif_c interface it looks like whatever is in imu.c today will need to be moved into the package? This would automatically allow the package to change rotation info and re-initialize the IMU? And also change mahony kp while riding?

Now wouldn't we also need to disable the current code in imu.c which I think gets called from main and/or app_init? Or is the idea that the primary filter is still handled by imu.c but the optional secondary one goes into the package?

Either way, I like where this is going, this will be great!

[vedderb]

I have added the callback to the end, so you can just use your instance of ATTITUDE_INFO and use ahrs_update on it. Because the callback is at the end the accel, gyro and mag-values will already be pre-rotated according to the settings and you also get the timestep dt. ATTITUDE_INFO now also has kp, ki, beta and the confidence decay in it, so you can change them to what you need before passing it to ahrs_update.

You can use the regular IMU-code at the same time, it is unaffected by running calculations on a separate instance of ATTITUDE_INFO. Then you can, for example, use the regular code for one orientation and generate your "true" orientation with a different set of parameters at the same time.

[vedderb]

One interesting thing you can do now is to run the balance-app from the read callback. That means you get an update for every imu-update with the lowest possible latency. You can also use dt as the timestep for your control loops. I can imagine this improving the performance slightly.