Replies to This Discussion

Permalink Reply by Juan Enrique Martin Boix on January 10, 2011 at 3:02pm

Great!!!

• ▶ Reply

DeveloperPermalink Reply by Doug Weibel on January 10, 2011 at 8:20pm

Your scheme will only work for certain orientations - for any orientation where the z axis is rotated this scheme will compromise the DCM drift correction algorithms.  That is to say if you want to mount the board so that the I/O pins are in the front instead of the back this will be OK, but if you want to mount it so that the components face the side instead of up, the drift correction will not work correctly.

• ▶ Reply

Permalink Reply by Tim Baumgartner on January 11, 2011 at 1:58am

Doug, thanks for the comments, they are much appreciated.

 

I'm not sure I understand how the DCM drift corrections are affected at all by the scheme. The way I look at it, ignoring the patch, if the IMU were placed upside-down in an airframe and turned on (with accelerometer bias calibration removed), the IMU should show itself upside-down. If the IMU were then arbitrarily rotated, the IMU should properly show these rotations and the final orientation (i.e. its normal behavior). So, if I placed the IMU upside-down in an aircraft and I wanted it's orientation to be displayed right-side-up on a flight display, I would e.g. take the orientation reported by the IMU, offset the roll by 180deg, and then display this orientation. The patch basically does this: it adds a static offset to the angles, but only to the reported values (the DCM is untouched). (I don't believe these offsetted angles are fed back into the DCM or IMU anywhere; if so then my patch probably doesn't work as expected.)

 

I apologize if I've just completely missed your point...it's been a long day.

 

Still, would a better approach be to apply the orientation offsets at a lower level (assuming it wouldn't muck up the drift correction)? For example, a secondary, static DCM could be created from the relative angle offsets and this DCM could be applied to the raw accelerometer and gyro vectors. It seems, using this approach, the accelerometer offset calibration could be left in during ground start.

 

Thanks,

Tim

• ▶ Reply

Permalink Reply by Tim Baumgartner on January 16, 2011 at 5:43pm

I've tested the patch a bit more with my current setup (the IMU is pitched down ~35 degrees with ~1 degree roll and 0 yaw) and correction algorithm still seems to function properly, even when twisted and shaken about pretty violently (with gyros saturating). I had disconnected the magnetometer during these informal tests and was able to quite plainly see yaw drift of >20 degrees. Still, this is not proof that the patch works in all orientations (although other setups were initially tested that were different from the setup described above) so I appreciate any specific concerns. (Doug, if this does not alleviate your concerns, can you please give me more specifics?)

Also, I've implemented a proof-of-concept version of the patch using a secondary, static DCM to apply the orientation offsets. This method allows for the use of ground/cold start accelerometer bias calibration. Otherwise, it seems to behave as the original patch. I can clean this code up and integrate it a bit better with the current code base if there is any real potential for getting such a patch accepted at some point in the future.

Please let me know your thoughts.

Thanks,
Tim

• ▶ Reply

DeveloperPermalink Reply by Doug Weibel on January 16, 2011 at 7:17pm

Hi Tim,

 

The approach you are taking is too simplistic.  It would be an acceptable solution if you wanted to mount the IMU say 20 or 35 degees off the normal orientation, but will mess things up for many of the possible 90 degree rotations.  The roll/pitch and yaw drift are corrected independantly using two separate reference vectors.  If you rotate the IMU into a position where one of the reference vectors has little or no orthogonal component with the axis/axes you are trying to correct, then you have effectively suppressed the drift correction for that axis.  It would be OK if you wanted to mount the IMU components up and with the gps connector to the right side for example, but would mess things up if you wanted to mount the IMU on its side.  Similarly, you recognize the euler angle singularity at pitch=90, but handle it just as a "can't do it" case.  Using a rotation matrix there is no singularity and it can be done, but it requires either for the rotation to be implemented within the IMU object or significant rework to the drift correction algorithm in DCM.

 

Orientation changes where you keep the components up but rotate the IMU in yaw are easy - just use an offset approach like you have done.  Orientation changes which only shift the z axis a small amount can also be done with an offset approach, but the further the z axis is moved from vertical the more the roll/pitch drift correction will be compromised.  Orientation changes with significant rotation of the z axis towards the horizontal plane require a better approach.

 

This is a good item for the to do list, but due to the critical nature of the DCM and IMU libraries, not something that will be implemented without thorough testing.  I think the cleanest implementation would be to apply a rotation matrix to rotate the accelerometer and gyro vectors in the IMU object.

• ▶ Reply

Permalink Reply by Tim Baumgartner on January 16, 2011 at 9:08pm

Doug,

Thanks for the detailed response! Your explanation is extremely helpful.

To make sure I completely understand your post and why the DCM method behaves differently, please correct my current understanding where needed:

Ignoring the yaw case completely, in cases where the gravity vector is close to parallel with the roll or pitch axis, the drift cancellation loses its effectiveness. For example, at 90deg pitch, roll cannot be determined from accelerometer readings and cannot therefore be corrected when the IMU is in this position. In the current trunk code, this loss in effective drift cancellation also occurs at certain orientations in flight (e.g. the pitch of the aircraft is 90deg). The static DCM approach (applied to the raw IMU vectors) actually re-orients the axes at the raw IMU level so that drift correction when arbitrary orientation is in use behaves as in the current trunk code (e.g. for the case where the IMU is at 90deg relative to the airframe, drift correction will behave normally when the airframe pitch is at 0deg but will lose effectiveness as the airframe approaches 90deg).

---

I completely agree that code modifying such a critical portion of the autopilot should not just be thrown into a release without thorough testing. My hope is that I am able to submit a patch that addresses the feature (in some form or another) into a publicly available APM branch so others, in addition to me, can test the feature over time until it is deemed safe enough to include in the main APM trunk.

I'll clean up the DCM (applied to raw IMU vectors) version of the code and post it here for further comments.

Thanks again,
Tim

• ▶ Reply

Permalink Reply by Tim Baumgartner on January 23, 2011 at 10:13pm

A patch that applies a DCM at the IMU level is attached. The DCM is applied to the raw accelerometer readings with bias subtracted and to the temperature corrected gyro readings.

During gyro initialization in the base code, the raw gyro values are read 200 times (and seemingly not used) before going through the offset calibration process. At the corresponding position in the accelerometer initialization code, the accel values are stored as the initial offset values. I'm not sure if there is a reason for this gyro code but I attempted to keep this behavior the same in the patch.

The actual function creating the DCM from Euler angles is in the AP_DCM class. I figured this made more sense than having it internal to the IMU class as the DCM applied to the IMU may also have to be applied to other sensors (e.g. a compass that's soldered to the IMU shield).

The test/setup menu usage for initially determining roll/pitch hasn't changed much from the original patch and still seems a bit clumsy.

Any feedback is appreciated.

Thanks,
Tim

Attachments:

• ImuArbitraryOrientation_1_ArduPilotMega.diff, 11 KB
• ImuArbitraryOrientation_1_libraries.diff, 18 KB

• ▶ Reply