File content as of revision 6:4265663c9a34:

//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed

#define STAT_RATE 1.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"
#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

int log_it;

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();
    log_it = 0;
    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)) {


            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);

 
    
             
           //  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();
}