I have recently ported the Sparkfun 9DOF AHRS code by Jordi Munoz and others into WebbotLib.
The main processing is performed by the DCM class added into WebbotLib Version 2.08 for C++. (note that you must tick the C++ box if you generate the code from Project Designer). This class makes it quite simple to port to other boards.
The downloads below contain: the Project Designer project file, the source code, and a compiled .hex file. So if you don't want to make any changes then you can just upload the .hex file to the Razor.
There are 3 versions of the Razor board - so make sure you download the correct one for the board you are using.
|Razor SEN:09623 ||Razor SEN:10125||Razor SEN:10736|
Once programmed: the Razor will output information over the serial port at 115200 baud. The format is as follows:
Where the roll, pitch and yaw values are in degrees.
The reason for outputting the raw magnetometer values is that the Razor design has had a lot of historical issues with the magnetometer. Often it will not start up correctly and just outputs the value 32 or -32 for each axis. The software detects this error and will make the status LED on the Razor flash as well as outputting the message:
!ERR: Magnetometer not working
I recommend that you use a computer running a serial terminal program, such as Hyperterminal, to verify the output. Once satisfied that everything is working then you can use the Python code by Jordi Munoz to visualise the output.
To do this you will need to:
- Install pywin for 2.7 from http://sourceforge.net/projects/pywin32/
- Install pyserial from http://sourceforge.net/projects/pyserial/files/
- Install Vphyton from http://vpython.org/contents/download_windows.html
- Download the Python visualiser code for the Razor from http://sf9domahrs.googlecode.com/files/SF9DOF_AHRS_interface_python.zip
- Unzip the download from step 4 (it only contains one file) and edit it (with any text editor)
- Change the line that starts with "ser = serial.Serial" to use the correct serial port and baud rate.
Run the Python code (double clicking should work) and you will see something like this: