C++ documentation C documentation

Implements a Direction Cosine Matrix (DCM) and is only available if you are using C++ with WebbotLib.

This can be used, along with a gyro, accelerometer and either a magnetometer or a compass, as the basis of a control system for airborne vehicles. Many boards (normally referred to as an IMU) already exist which combine these sensors with a microprocessor onto a single board. Examples include the Sparkfun Razor and the Mongoose. The combination of such a board along with this library assists you in off-loading the complex mathematics involved away from your main processor onto the slave board. The slave board can then keep sending out information on roll, pitch and yaw via a UART to the main board.

For the theory on DCM then I suggest reading: http://gentlenav.googlecode.com/files/DCMDraft2.pdf

This code is based on work by Doug Weibel and Jose Julio but completely re-written for WebbotLib.

To make use of this class you must regularly update the DCM with the gyro readings. By 'regularly' I mean about every 20ms (50 times per second).

This is done by calling gyroDegrees or gyroRadians passing in a Vector3D that contains the gyro rotations around the x, y, and z axes in degrees or radians respectively.

You may wonder "why can't I just tell the DCM about the gyro device we are using and let WebbotLib do the reading itself?". Well the answer is: each board can be built differently so the concept of the x, y and z axes can vary. Also: how you mount the board onto your vehicle may also change the orientation of the axes. Consequently it is up to you to read the gyro and then decide which readings represent the x, y and z axes and whether or not to multiply each value by -1 to reverse its direction.

This library expects that the X axis is pointing in the direction of travel, the Y axis is pointing to the right and the Z axis is pointing down. Therefore the returned values represent:

- Positive pitch : nose up

- Positive roll : right wing down

- Positive yaw : clockwise rotation

Assuming that you are only using gyro information then the resultant pitch, roll and yaw returned values will accumulate errors and, in a short time, become useless.

We therefore introduce the 'driftCorrection' function which takes the values from an accelerometer and a compass to error correct the drift. The accelerometer value vector needs to be created by you by reading the accelerometer and aligning the x,y and z axes so that they represent the same directions as the gyro. The compass reading should be a bearing, in radians, from magnetic north.

Since the 'driftCorrection' function is correcting accumulative errors then you don't need to call it every time you update the gyro readings.

The error feedback loop uses a proportional and integral term for both the yaw and the roll/pitch. The floating point scale factors for these terms are set to default values for you but you can modify them yourself if you need to. The roll and pitch values are called 'm_Kp_ROLLPITCH' and 'm_Ki_ROLLPITCH'. The yaw values are called 'm_Kp_YAW' and 'm_Ki_YAW'.

For example: if you wanted to change the yaw constants and your DCM variable is called 'matrix' then your code would look like this:-

// The DCM

DCM matrix;

TICK_COUNT appInitSoftware(TICK_COUNT loopStart){

// Print the default settings to standard out

cout.print("Kp="); cout.print(matrix.m_Kp_YAW); cout.println();

cout.print("Ki="); cout.print(matrix.m_Ki_YAW); cout.println();

// Change the Yaw settings

matrix.m_Kp_YAW = 1.5;

matrix.m_Ki_YAW = 0.00003;

}

If you are using a magnetometer, rather than a compass, then you will be reading x,y and z values. Once again you may need to flip these to use the same axis orientation as the gyros. To calculate the magnetic heading you will need to process the values like this (assuming your DCM is called 'matrix' and you have stored the adjusted magnetometer readings in 'magnetomVector'):-

// Calculate the compass magnetic heading

// taking into account the current pitch and roll

double roll = matrix.getRollRadians(); // Get roll from the DCM

double pitch = matrix.getPitchRadians(); // Get pitch from the DCM

// Calculated cos/sin values once

double cos_roll = cos(roll);

double sin_roll = sin(roll);

double cos_pitch = cos(pitch);

double sin_pitch = sin(pitch);

// Tilt compensated Magnetic field X:

double MAG_X = magnetomVector.x*cos_pitch +

magnetomVector.y*sin_roll*sin_pitch +

magnetomVector.z*cos_roll*sin_pitch;

// Tilt compensated Magnetic field Y:

duble MAG_Y = magnetomVector.y*cos_roll - magnetomVector.z*sin_roll;

// Finally we calculate the Magnetic Heading

double magHeadingRadians = atan2(-MAG_Y,MAG_X);

Alternatively: if you don't have a compass or a magnetometer then you can just use the current yaw value from the DCM but this means that the yaw value will continue to drift:

double magHeadingRadians = matrix.getYawRadians();

If you are using a compass rather than a magnetometer then make sure that it gives the correct value irrespective of orientation - ie with the compass pointing North does it give you the same value if the compass is placed on its side or upside down?

The output of the DCM is the current pitch, roll and yaw which can be returned in either radians or degrees.

What you do with this output is up to you! A slave board will normally send the values over a UART to the main board to help it navigate and steer.

There are various GUIs that can be used on computers to visualise the output by sending the roll, pitch and yaw values over a serial port. The Sparkfun Razor board contains a link to some Python code whereas the Mongoose has its own VB display software. You can use any of these so long as you output the information over the UART in the correct format.

Method

- DCM - Create a new direction cosine matrix variable.
- reset - Reset the DCM to it's initial state.
- gyroDegrees - Update the DCM with the latest gyro readings in degrees per second. This function should be called regularly - around 50 times per second.
- gyroRadians - Update the DCM with the latest gyro readings in radians per second. This function should be called regularly - around 50 times per second.
- driftCorrection - Correct any drift errors.
- getPitchRadians - Returns the current pitch in radians.
- getPitchDegrees - Returns the current pitch in degrees.
- getRollRadians - Returns the current roll in radians.
- getRollDegrees - Returns the current roll in degrees.
- getYawRadians - Returns the current yaw in radians.
- getYawDegrees - Returns the current yaw in degrees.