Streams BNO055 IMU data
Dependencies: BNO055_fusion mbed NeoStrip
Dependents: MadPulse_Controller_ros
Fork of ES456_Labs by
Diff: main.cpp
- Revision:
- 6:4265663c9a34
- Parent:
- 5:b1c4a117eb4b
- Child:
- 8:7f35f3cd8235
diff -r b1c4a117eb4b -r 4265663c9a34 main.cpp --- a/main.cpp Fri Sep 08 19:50:34 2017 +0000 +++ b/main.cpp Tue Sep 19 17:27:59 2017 +0000 @@ -1,13 +1,18 @@ //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 IMU_RATE 150.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 +#define LOG_IMU 1 +#define LOG_MAG 2 +#define LOG_EUL 3 +#define LOG_ODO 4 + #include "mbed.h" #include "BNO055.h" @@ -28,6 +33,7 @@ Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc Serial xbee(p28, p27); // tx, rx for Xbee +int log_it; Timer t; // create timer instance float t_imu,t_stat; @@ -48,7 +54,7 @@ t.start(); t_imu = t.read(); t_stat =t.read(); - + log_it = 0; status_LED = 1; if(imu.check()) { @@ -107,20 +113,38 @@ if(t.read()-t_imu > (1/IMU_RATE)) { - imu.get_angles(); - imu.get_accel(); - imu.get_gyro(); - imu.get_mag(); + + switch (log_it){ + case (LOG_IMU): { + imu.get_accel(); + imu.get_gyro(); + 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); + + break; + } + case (LOG_MAG): { + imu.get_mag(); + pc.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); + break; + } + case (LOG_EUL): { + imu.get_angles(); + pc.printf("$RPY,%.3f, %.3f, %.3f\r\n", imu.euler.roll,imu.euler.pitch,wrapTo2pi(imu.euler.yaw)); + break; + } + default :{ + log_it = 0; + } + + } + log_it++; + //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);