A class to receive data from the SparkFun 9DOF Razor IMU. It can be easily adapted to work with IMUs with different data formats.
Diff: main.cpp
- Revision:
- 0:a260d84e07fc
- Child:
- 1:fdfa313b9cc3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Oct 16 05:37:27 2011 +0000 @@ -0,0 +1,111 @@ +/* + * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current + * Updated to use interrupts - this will help when we intergrate this code into AVNavControl + * 9dof razor from sparkfun + */ + +#define GYRO_SCALE 14.375 // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf + +#include "mbed.h" +#include <string> +#include <sstream> + +Serial IMU(p9, p10); // tx, rx +Serial PC(USBTX, USBRX); + +DigitalOut myled1(LED1); +DigitalOut myled2(LED2); +DigitalOut myled3(LED3); +DigitalOut myled4(LED4); + +bool IMUreadable = false; +bool PCreadable = false; +bool parseNow = false; + +bool debugMode = false; + +string buffer; + +void readIMU(); +void readPC(); + +void parse(string); + +int accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ; + +int main() { + IMU.format(8, Serial::None, 1); + PC.format(8, Serial::None, 1); + IMU.baud(57600); + PC.baud(115200); + + PC.printf("----------------------------\n\r"); + PC.printf("debug: device reset\n\r"); + PC.printf("debug: gotta have my bowl; gotta have serial. serial is up.\n\r"); + + PC.attach(&readPC); + IMU.attach(&readIMU); + + //__disable_irq(); + PC.printf("debug: attached interrupts.\n\r"); + + if (!debugMode) IMU.putc('4'); //tell the imu to start sending data, if it's not sending data already + PC.printf("debug: told the IMU to start sending data\n\r"); + + //The main loop + while (1) { + __enable_irq(); + if (IMUreadable) { + myled2 = 1; + if (debugMode){ + while (IMU.readable()) PC.putc(IMU.getc()); + } + else { + while (IMU.readable()){ + buffer.push_back(IMU.getc()); + parseNow = (buffer.at(buffer.length() - 1) == '#'); + } + } + + IMUreadable = false; + myled2 = 0; + } + if (PCreadable) { + myled1 = 1; + while (PC.readable()) IMU.putc(PC.getc()); + PCreadable = false; + myled1 = 0; + } + if (parseNow) { + parse(buffer); + buffer.clear(); + parseNow = false; + } + } +} + +//Interrupt called when there is a character to be read from the PC +//To avoid a livelock, we disable interrupts at the end of the interrupt. +//Then, in the main loop, we read everything from the buffer +void readPC(){ + PCreadable = true; + __disable_irq(); +} + +//Interrupt called when there is a character to be read from the IMU +void readIMU(){ + IMUreadable = true; + __disable_irq(); +} + +//Checks data integrity, then stores the IMU values into variables +void parse(string buf){ + if (buf.find('$') != buf.npos && buf.find('#') != buf.npos){ + for (int i = 0; i < buf.length(); i++) if (buf[i] == '$' || buf[i] == ',' || buf[i] == '#') buf[i] = ' '; + stringstream ss(buf); + ss >> accX >> accY >> accZ >> gyrX >> gyrY >> gyrZ >> magX >> magY >> magZ; + + PC.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ); + } + else PC.printf("Bad parse!\n\r"); +}