Add AHRS to Tethys equipped model #158

hidmic · 2022-02-08T17:51:08Z

Combining upstream IMU and magnetometer sensors. This is lacking a test but perhaps this is best tested as part of #151 e.g. make the AUV describe a known trajectory and sample IMU and magnetometer measurements along that trajectory to ensure values make sense.

hidmic · 2022-02-08T17:53:28Z

@braanan do we have a reference AHRS system from which to derive a noise model? Not that we have a lot of options -- we have gaussian or gaussian.

Also, I'd really need access to all of our repositories to complete AHRS integration. Couldn't get things going with the latest MBARI Docker image -- it'd appear lrauv code in it is a bit dated w.r.t. main.

braanan · 2022-02-08T18:13:47Z

I think we'll go with Gaussian then. Here's a link to the Sparton AHRS-M2 we have installed across the fleet: https://www.spartonnavex.com/product/ahrs-m2/ All the info you need should be in the datasheet.

As for access, all checks are cleared — just waiting for final approval from our MT.

hidmic · 2022-02-08T21:34:50Z

Alright, after much digging, I think I extracted all that can be extracted from the datasheet. There's no characterization of the magnetometer whatsoever, and IIUC I'm lacking data to use the bias drift model in Ignition (which is loosely explained in https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model).

hidmic · 2022-02-15T20:05:41Z

lrauv_ignition_plugins/test/test_ahrs.cc

+}
+
+//////////////////////////////////////////////////
+TEST_F(AhrsTest, FrameConventionsAreCorrect)


@braanan @arjo129 at first I tried to break this test into smaller ones, but I'm unable to instantiate ignition::gazebo::TestFixture more than once in the same process. It aborts with CHECK failed: GeneratedDatabase()->Add(...), which appears to be coming from the shared library that bears lrauv
protobuf messages. I suspect the Ignition plugin system is attempting to reload it upon fixture construction (see here).

Oh yeah I forgot to mention I think we keep each test isolated in a separate process as there was a bug in the way we are linking the messages. For whatever reason if we rely on external messages google protobuf will go 💥 if we have two tests in the same process. I have not been able to isolate exactly why this is the case. Tracking this issue in #159 as we had not been tracking this.

hidmic · 2022-02-15T20:09:18Z

lrauv_ignition_plugins/test/test_ahrs.cc

+        ignition::gazebo::Entity tethysEntity =
+            world.ModelByName(_ecm, "tethys");
+        using ignition::gazebo::components::Static;
+        _ecm.CreateComponent(tethysEntity, Static(true));


@braanan @arjo129 I forced the model to be static to avoid motion whenever I put the AUV in unusual orientations. Unfortunately, it also means we are not testing AHRS output when moving. It'd be tricky nonetheless. Hydrodynamics modelling gets in the way and it's hard to tell what the angular velocity and linear acceleration is going to be.

I think we agreed that as long as the AHRS is reporting the correct coordinate frame we should hope for the best with the gyro.

That's right. Thing is, I cannot test for gyro frame conventions if its output is always zero. I guess we are testing that the body frame is correct for the accelerometer so it should be good for the gyro too.

arjo129

Hey @hidmic thanks for the tests and the imu. I think this is good to merge. For integration one possibility is to use the lrauv_state to pass the imu outputs. This structure is populated in TethysCommPlugin.cc. In particular the ratePQR field may be of interest. Then again, I don't know much about the LRAUV source code as I am living in Singapore which means I dont have access to the MBARI code.

arjo129 · 2022-03-02T01:22:05Z

@hidmic Should I merge this?

hidmic · 2022-03-02T13:13:57Z

@hidmic Should I merge this?

@arjo129 need to make sure I can integrate it with the other half of the codebase first. Hopefully this week.

hidmic · 2022-03-03T23:16:37Z

Had to rename topics in 11c2ad9. Unlike others, no plugin is relaying them from /model/tethys/* to /tethys/*.

Combining upstream IMU and magnetometer sensors Signed-off-by: Michel Hidalgo <[email protected]>

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic · 2022-03-15T21:43:26Z

@braanan @arjo129 this one's ready for a final review. Tests are passing locally. Integration with the rest of the code seems functional (though I see Tethys going for a run as #167 reports).

hidmic · 2022-03-15T21:57:52Z

Integration tests are failing upon installing sdformat 🤔

braanan · 2022-03-16T19:26:01Z

@hidmic looks like there's a reverse sign in the accelerometer data - FSK should have positive down.

I'm seeing:

2022-03-16T19:25:23.046Z,1647458723.046 [AHRS_M2](INFO): PITCH: -3.659353, ROLL: 0.004998, YAW: 347.323370 (deg)
2022-03-16T19:25:23.046Z,1647458723.046 [AHRS_M2](INFO): Accel:   X: -0.625480, Y: -0.000853, Z: -9.780028 (m/s2)

Updated so the printout above is reported with the vehicle sitting on the surface - I'd expect to see gravity reported on Z with a value of roughly 9.8 m/s2

If this is the std convention for the imu in Ign we can process the data in the lrauv-app end.

Thanks

hidmic · 2022-03-16T20:56:51Z

@braanan my (perhaps vague) understanding of MEMS IMUs is that when standing still under constant gravity, its proof mass is displaced as if it was accelerating in the opposite direction (ie. upwards). This matches Ignition's IMU sensor behavior (see here and gravity defaults here).

Having said that, I have no first-hand experience with Sparton's AHRS-M2 so perhaps it behaves differently.

braanan · 2022-03-16T21:14:21Z

It does. Here's the output from an actual vehicle

2022-03-16T19:08:03.896Z,1647457683.896 [AHRS_M2](INFO): PITCH: -0.384570, ROLL: -3.744142, YAW: 213.573527 (deg)
2022-03-16T19:08:03.897Z,1647457683.897 [AHRS_M2](INFO): Accel:   X: 0.064123, Y: -0.640986, Z: 9.850662 (m/s2)

I'm not sure if this output format is true to all AHRS models or unique to the M2. I'll check, since that will inform where we should implement the sign flip. If it's indeed true for all AHRS models we can see about reversing the sign in the plugin, otherwise, the sign flip should go in the AHRS_M2 device driver in the lrauv-application.

braanan · 2022-03-16T21:35:20Z

OK, I've asked around and it seems like there's not much consistency between vendors, so I'll add the sign flip in the AHRS_M2 driver with a comment.

hidmic · 2022-03-16T22:26:01Z

OK, I've asked around and it seems like there's not much consistency between vendors, so I'll add the sign flip in the AHRS_M2 driver with a comment.

@braanan hmm, will that work? It's not that the linear acceleration Z-axis is flipped, only gravity is. I'll go ahead and guess that the real AHRS is estimating gravity's magnitude and direction and re-applying it to provide these acceleration measurements. Maybe we have to do the same in the plugin :/

braanan · 2022-03-16T23:46:49Z

@hidmic Looks like the imu plugin assumes that the linear accelarations (and the sensor) are alligned with the ENU frame of refrence. As far as I can tell, the rotation to NED is only applied to the oriantation data here, so we'll have to add that rotation to NED (FSK) to the linear accelarations and angular velocities in the plugin or take care of that on the LRAUV side. Maybe I'm missing something.

Since this is an upstream plugin the second option will be simpler I think.

hidmic · 2022-03-17T12:53:01Z

Looks like the imu plugin assumes that the linear accelarations (and the sensor) are alligned with the ENU frame of refrence.

@braanan that has been taken care of here. The issue we have now is the minus sign that gets applied to gravity here. As I mentioned above, AFAIK that's correct for most IMUs, but apparently not for the AHRS-M2.

The only solution I see here is to allow the user to flip that sign through configuration.

hidmic · 2022-03-18T22:03:37Z

@braanan may I get an approval?

hidmic force-pushed the hidmic/ahrs branch from d4fb025 to 644bea1 Compare February 15, 2022 19:56

hidmic marked this pull request as ready for review February 15, 2022 19:57

hidmic requested review from arjo129 and braanan February 15, 2022 19:57

hidmic commented Feb 15, 2022

View reviewed changes

caguero mentioned this pull request Feb 16, 2022

2022 M1 meta-issue #150

Closed

39 tasks

arjo129 approved these changes Feb 18, 2022

View reviewed changes

hidmic requested a review from arjo129 March 3, 2022 23:16

hidmic added 6 commits March 15, 2022 17:55

Add AHRS to Tethys equipped model

67c5585

Combining upstream IMU and magnetometer sensors Signed-off-by: Michel Hidalgo <[email protected]>

Model AHRS after Sparton AHRS-M2

5557ad6

Signed-off-by: Michel Hidalgo <[email protected]>

Add AHRS tests

2f0c064

Signed-off-by: Michel Hidalgo <[email protected]>

Fix frame conventions in AHRS

66a6162

Signed-off-by: Michel Hidalgo <[email protected]>

Rename AHRS (sub)topics

200e5da

Signed-off-by: Michel Hidalgo <[email protected]>

Fix failing AHRS tests

9aa5300

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic force-pushed the hidmic/ahrs branch from 11c2ad9 to 9aa5300 Compare March 15, 2022 21:30

braanan approved these changes Mar 18, 2022

View reviewed changes

hidmic merged commit b2c9936 into main Mar 18, 2022

hidmic deleted the hidmic/ahrs branch March 18, 2022 22:11

hidmic mentioned this pull request Jun 22, 2022

Fix AHRS config for Tethys #218

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add AHRS to Tethys equipped model #158

Add AHRS to Tethys equipped model #158

hidmic commented Feb 8, 2022 •

edited

Loading

hidmic commented Feb 8, 2022 •

edited

Loading

braanan commented Feb 8, 2022

hidmic commented Feb 8, 2022 •

edited

Loading

hidmic Feb 15, 2022

arjo129 Feb 16, 2022 •

edited

Loading

hidmic Feb 15, 2022 •

edited

Loading

arjo129 Feb 16, 2022

hidmic Feb 16, 2022 •

edited

Loading

arjo129 left a comment •

edited

Loading

arjo129 commented Mar 2, 2022

hidmic commented Mar 2, 2022

hidmic commented Mar 3, 2022

hidmic commented Mar 15, 2022

hidmic commented Mar 15, 2022

braanan commented Mar 16, 2022 •

edited

Loading

hidmic commented Mar 16, 2022 •

edited

Loading

braanan commented Mar 16, 2022

braanan commented Mar 16, 2022

hidmic commented Mar 16, 2022

braanan commented Mar 16, 2022

hidmic commented Mar 17, 2022

hidmic commented Mar 18, 2022

Add AHRS to Tethys equipped model #158

Add AHRS to Tethys equipped model #158

Conversation

hidmic commented Feb 8, 2022 • edited Loading

hidmic commented Feb 8, 2022 • edited Loading

braanan commented Feb 8, 2022

hidmic commented Feb 8, 2022 • edited Loading

hidmic Feb 15, 2022

Choose a reason for hiding this comment

arjo129 Feb 16, 2022 • edited Loading

Choose a reason for hiding this comment

hidmic Feb 15, 2022 • edited Loading

Choose a reason for hiding this comment

arjo129 Feb 16, 2022

Choose a reason for hiding this comment

hidmic Feb 16, 2022 • edited Loading

Choose a reason for hiding this comment

arjo129 left a comment • edited Loading

Choose a reason for hiding this comment

arjo129 commented Mar 2, 2022

hidmic commented Mar 2, 2022

hidmic commented Mar 3, 2022

hidmic commented Mar 15, 2022

hidmic commented Mar 15, 2022

braanan commented Mar 16, 2022 • edited Loading

hidmic commented Mar 16, 2022 • edited Loading

braanan commented Mar 16, 2022

braanan commented Mar 16, 2022

hidmic commented Mar 16, 2022

braanan commented Mar 16, 2022

hidmic commented Mar 17, 2022

hidmic commented Mar 18, 2022

hidmic commented Feb 8, 2022 •

edited

Loading

hidmic commented Feb 8, 2022 •

edited

Loading

hidmic commented Feb 8, 2022 •

edited

Loading

arjo129 Feb 16, 2022 •

edited

Loading

hidmic Feb 15, 2022 •

edited

Loading

hidmic Feb 16, 2022 •

edited

Loading

arjo129 left a comment •

edited

Loading

braanan commented Mar 16, 2022 •

edited

Loading

hidmic commented Mar 16, 2022 •

edited

Loading