Audren Cloitre
/
UM6LT_v1
Same library as the one in "UM6LT" but this publication has a main file to show how to use the library.
Diff: main.cpp
- Revision:
- 0:c5fbc6927336
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Sep 30 21:00:04 2012 +0000 @@ -0,0 +1,140 @@ +#include "mbed.h" +#include "UM6LT.h" + +Serial pc(USBTX, USBRX); +UM6LT imu(p28, p27); + +int stdDelayMs = 2; + +int main() { + + pc.baud(115200); + imu.baud(115200); + + int broadcastRate = 22; + int baudrate = 115200; + + int wantCov = 0; + int wantEulerAngles = 0; + int wantQuat = 0; + int wantProcMag = 0; + int wantProcAccel = 0; + int wantProcGyro = 0; + int wantRawMag = 0; + int wantRawAccel = 0; + int wantRawGyro = 0; + + int dataToTransmit [9] = {wantCov, wantEulerAngles, wantQuat, wantProcMag, wantProcAccel, wantProcGyro, wantRawMag, wantRawAccel, wantRawGyro}; + int broadcastEnabled = 0; + + imu.setCommParams(broadcastRate, baudrate, dataToTransmit, broadcastEnabled); + + int wantPPS = 0; + int wantQuatUpdate = 1; + int wantCal = 1; + int wantAccelUpdate = 1; + int wantMagUpdate = 1; + + imu.setConfigParams(wantPPS, wantQuatUpdate, wantCal, wantAccelUpdate, wantMagUpdate); + + if(imu.getStatus()){ + + int roll = 0; + int pitch = 0; + int yaw = 0; + int rollRate = 0; + int pitchRate = 0; + int yawRate = 0; + int accelX = 0 ; + int accelY = 0 ; + int accelZ = 0 ; + int magX = 0; + int magY = 0; + int magZ = 0; + int gyroBiasX = 0; + int gyroBiasY = 0; + int gyroBiasZ = 0; + int a = 0; + int b = 0; + int c = 0; + int d = 0; + + int byteBuffer; + + for(int i=0; i<5; i++){ + if(imu.getAngles(roll, pitch, yaw)){ + wait_ms(stdDelayMs); + printf("roll: %d pitch: %d yaw: %d\r\n", roll, pitch, yaw); + } + } + + printf("\r\n\r\nAngles should be random but consistent\r\n\r\n"); + + if(imu.zeroGyros(gyroBiasX, gyroBiasY,gyroBiasZ)){ + printf("Gyro Bias X: %d Gyro Bias Y: %d Gyro Bias Z: %d\r\n", gyroBiasX, gyroBiasY, gyroBiasZ); + } + + if(imu.autoSetAccelRef() && imu.autoSetMagRef() && imu.resetEKF()){ + + printf("Press (1) for Euler Angles.\r\n"); + printf("Press (2) for Accelerations.\r\n"); + printf("Press (3) for Magnetic field.\r\n"); + printf("Press (4) for Angle rates.\r\n"); + printf("Press (5) for Quaternion.\r\n"); + printf("Press (0) to stop.\r\n"); + + while(!pc.readable()){ + wait_ms(stdDelayMs); + } + byteBuffer = pc.getc(); + + while(1){ + + if(pc.readable()){ + byteBuffer = pc.getc(); + } + + switch(byteBuffer){ + case '0': + wait_ms(stdDelayMs); + break; + case '1': + if(imu.getAngles(roll, pitch, yaw)){ + wait_ms(stdDelayMs); + printf("roll: %d pitch: %d yaw: %d\r\n", roll, pitch, yaw); + } + break; + case '2': + if(imu.getAccel(accelX, accelY, accelZ)){ + wait_ms(stdDelayMs); + printf("accelX: %d accelY: %d accelZ: %d\r\n", accelX, accelY, accelZ); + } + break; + case '3': + if(imu.getMag(magX, magY, magZ)){ + wait_ms(stdDelayMs); + printf("magX: %d magY: %d magZ: %d\r\n", magX, magY, magZ); + } + break; + case '4': + if(imu.getAngleRates(rollRate, pitchRate, yawRate)){ + wait_ms(stdDelayMs); + printf("rollRate: %d pitchRate: %d yawRate: %d\r\n", rollRate, pitchRate, yawRate); + } + break; + case '5': + if(imu.getQuaternion(a, b, c, d)){ + wait_ms(stdDelayMs); + printf("a: %d b: %d c: %d d: %d\r\n", a, b, c, d); + } + break; + default: + printf("Wrong command. Enter '1', '2', '3', '4', '5' or '6'.\r\n"); + byteBuffer = '0'; + break; + } + } + + } + } +}