Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
Diff: main.cpp
- Revision:
- 5:b1c4a117eb4b
- Parent:
- 3:82e223a4a4e4
- Child:
- 6:4265663c9a34
diff -r 0d0d62b0a6bd -r b1c4a117eb4b main.cpp --- a/main.cpp Mon Sep 19 13:00:16 2016 +0000 +++ b/main.cpp Fri Sep 08 19:50:34 2017 +0000 @@ -1,17 +1,21 @@ //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed -#define LOG_RATE 50.0 +#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); @@ -25,17 +29,13 @@ Serial xbee(p28, p27); // tx, rx for Xbee - - Timer t; // create timer instance +float t_imu,t_stat; -float t_imu,t_cmd,str_cmd,thr_cmd,str,thr; -float t_hall, dt_hall,t_run,t_stop,t_log; -bool armed, auto_ctrl; -float wheel_spd; -float arm_clock; -bool str_cond,thr_cond,run_ctrl,log_data; -bool log_imu,log_bno,log_odo,log_mag = false; +void setLED(int *colors,float brightness); +float wrapTo2pi(float ang); +int stat_colors[4] = {RED,ORANGE,YELLOW,GREEN}; +int colors[4]; int main() { @@ -43,38 +43,25 @@ pc.baud(115200); xbee.baud(115200); -//imu.init_MPU_i2c(); - left = 0; - str_cmd = 0; - str=0; - thr=0; - thr_cmd = 0; - arm_clock =0; - str_cond = false; - thr_cond = false; - armed = false; - auto_ctrl = false; - run_ctrl = false; - log_data = false; +// Initialize timers for IMU and Status Update t.start(); t_imu = t.read(); - t_cmd = 0; + 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.setmode(OPERATION_MODE_NDOF); //Uses magnetometer - imu.set_angle_units(DEGREES); + imu.SetExternalCrystal(1); + imu.set_angle_units(RADIANS); imu.set_accel_units(MPERSPERS); - imu.set_anglerate_units(DEG_PER_SEC); + imu.set_anglerate_units(RAD_PER_SEC); imu.setoutputformat(WINDOWS); imu.set_mapping(2); - + imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer } else { @@ -95,18 +82,45 @@ pc.printf("ES456 Vehicle Sensor Logger\r\n"); while(1){ - imu.get_angles(); - imu.get_accel(); - imu.get_gyro(); - imu.get_mag(); - if(t.read()-t_imu > (1/LOG_RATE)) { + + 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); + 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); @@ -119,5 +133,37 @@ } // 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(); +} + +