Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
main.cpp
- Committer:
- jdawkins
- Date:
- 2017-09-08
- Revision:
- 5:b1c4a117eb4b
- Parent:
- 3:82e223a4a4e4
- Child:
- 6:4265663c9a34
File content as of revision 5:b1c4a117eb4b:
//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed #define STAT_RATE 1.0 #define IMU_RATE 50.0 #define LOOP_RATE 200.0 #define CMD_TIMEOUT 1.0 #define GEAR_RATIO (1/2.75) #define PI 3.14159 #define LED_CLUSTERS 3 #define LED_PER_CLUSTER 1 #include "mbed.h" #include "BNO055.h" #include "NeoStrip.h" BNO055 imu(p9, p10); NeoStrip leds(p30,LED_CLUSTERS*LED_PER_CLUSTER); int left; float saturateCmd(float cmd); void menuFunction(Serial *port); DigitalOut status_LED(LED1); DigitalOut armed_LED(LED2); DigitalOut auto_LED(LED3); DigitalOut imu_LED(LED4); Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc Serial xbee(p28, p27); // tx, rx for Xbee Timer t; // create timer instance float t_imu,t_stat; void setLED(int *colors,float brightness); float wrapTo2pi(float ang); int stat_colors[4] = {RED,ORANGE,YELLOW,GREEN}; int colors[4]; int main() { pc.baud(115200); xbee.baud(115200); // Initialize timers for IMU and Status Update t.start(); t_imu = t.read(); t_stat =t.read(); status_LED = 1; if(imu.check()) { pc.printf("BNO055 connected\r\n"); imu.setmode(OPERATION_MODE_CONFIG); imu.SetExternalCrystal(1); imu.set_angle_units(RADIANS); imu.set_accel_units(MPERSPERS); imu.set_anglerate_units(RAD_PER_SEC); imu.setoutputformat(WINDOWS); imu.set_mapping(2); imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer } else { pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); status_LED = 1; armed_LED = 1; imu_LED = 1; auto_LED = 1; while(1) { status_LED = !status_LED; armed_LED = !armed_LED; imu_LED = !imu_LED; auto_LED = !auto_LED; wait(0.5); } } pc.printf("ES456 Vehicle Sensor Logger\r\n"); while(1){ if(t.read()-t_stat > (1/STAT_RATE)){ uint8_t acc_stat; uint8_t gyro_stat; uint8_t mag_stat; uint8_t imu_stat; imu.get_calib(); // int x = (number >> (8*n)) & 0xff mag_stat = (imu.calib & 0x03); acc_stat = (imu.calib & 0x0C)>> 2; gyro_stat = (imu.calib & 0x30) >> 4; imu_stat = (imu.calib & 0xC0) >> 6; // pc.printf("%mag %d, acc %d, gyro %d imu %d\r\n",mag_stat, acc_stat, gyro_stat, imu_stat); colors[0] = stat_colors[mag_stat]; colors[1] = stat_colors[acc_stat]; colors[2] = stat_colors[gyro_stat]; setLED(colors,0.05); t_stat = t.read(); } if(t.read()-t_imu > (1/IMU_RATE)) { imu.get_angles(); imu.get_accel(); imu.get_gyro(); imu.get_mag(); //pc.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z); //xbee.printf("$MAD,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.1f,%.1f,%.1f\r\n", imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.accel.x,imu.accel.y,imu.accel.z,imu.mag.x,imu.mag.y,imu.mag.z); pc.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); wait(0.001); pc.printf("$RPY,%.3f, %.3f, %.3f\r\n", imu.euler.roll,imu.euler.pitch,wrapTo2pi(imu.euler.yaw)); wait(0.001); pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); wait(0.001); // xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", imu.accel.x,imu.accel.y,imu.accel.z,imu.gyro.x,imu.gyro.y,imu.gyro.z); // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); t_imu = t.read(); } // if t.read wait(1/LOOP_RATE); status_LED=!status_LED; } // while (1) } // main float wrapTo2pi(float ang){ if(ang > 2*PI){ ang = ang - 2*PI; } if(ang < 0){ ang = ang + 2*PI; } return ang; } void setLED(int *colors,float brightness) { leds.setBrightness(brightness); int cidx = 0; int ctr = 0; for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) { if(ctr >= LED_PER_CLUSTER) { ctr = 0; cidx++; } leds.setPixel(i,colors[cidx]); ctr++; } leds.write(); }