File content as of revision 11:d68282e48fd4:

/*
 * mbed Application program for the mbed Nucleo F401
 *  BNO055 Intelligent 9-axis absolute orientation sensor
 *  by Bosch Sensortec
 *
 * Copyright (c) 2015 Kenji Arai / JH1PJL
 *  http://www.page.sannet.ne.jp/kenjia/index.html
 *  http://mbed.org/users/kenjiArai/
 *      Created: March     30th, 2015
 *      Revised: April     16th, 2015
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
 * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

//  Include ---------------------------------------------------------------------------------------
#include    "mbed.h"
#include    "BNO055.h"
#include    "MODSERIAL.h"
//#include    "arm_math.h"

//  Definition ------------------------------------------------------------------------------------
#define NUM_LOOP    100

//  Object ----------------------------------------------------------------------------------------
MODSERIAL  pc(USBTX,USBRX);

volatile bool RAW = true;
volatile bool FUS = false;
bool printed = false;
int Time;
int timeStamp = 0;
int timeOfStart;
int timeNow;

//string sdata = "";

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut pwr_onoff(p30);

I2C    i2c(p28, p27); // SDA, SCL
BNO055 imu(i2c, p29);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
LocalFileSystem local("local");                 // Create the local filesystem under the name "local"

Timer t;

//  RAM -------------------------------------------------------------------------------------------
BNO055_ID_INF_TypeDef       bno055_id_inf;
BNO055_EULER_TypeDef        euler_angles;
BNO055_QUATERNION_TypeDef   quaternion;
BNO055_LIN_ACC_TypeDef      linear_acc;
BNO055_GRAVITY_TypeDef      gravity;
BNO055_TEMPERATURE_TypeDef  chip_temp;

//  ROM / Constant data ---------------------------------------------------------------------------

//  Function prototypes ---------------------------------------------------------------------------


//-------------------------------------------------------------------------------------------------
//  Serial Interrupt
//-------------------------------------------------------------------------------------------------
void rxCallback(MODSERIAL_IRQ_INFO *q)
{
    MODSERIAL *serial = q->serial;
    if ( serial->rxGetLastChar() == 'f') {
        imu.write_reg0(0x3d, 0x0c);
        RAW = false;
        FUS = true;
        pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n");
    }
    if ( serial->rxGetLastChar() == 'r') {
        imu.write_reg0(0x3d, 0x07);
        RAW = true;
        FUS = false;
        pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
    }
    if ( serial->rxGetLastChar() == '*') {
        timeOfStart = t.read_ms();        
        timeStamp = 0;
    }
}

//-------------------------------------------------------------------------------------------------
//  Control Program
//-------------------------------------------------------------------------------------------------
// Calibration
//  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
void bno055_calbration(void)
{
    uint8_t d;
    pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
    //---------- Gyroscope Caliblation ------------------------------------------------------------
    // (a) Place the device in a single stable position for a period of few seconds to allow the
    //     gyroscope to calibrate
    pc.printf("Step1) Please wait few seconds\r\n");
    t.start();
    while (t.read() < 10) {
        d = imu.read_calib_status();
        pc.printf("calibrating = 0x%x target  = 0x30(at least)\r\n", d);
        if ((d & 0x30) == 0x30) {
            break;
        }
        wait(1.0);
    }
    pc.printf("-> Step1) is done\r\n\r\n");
    //---------- Magnetometer Caliblation ---------------------------------------------------------
    // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
    //     CALIB_STAT register indicates fully calibrated.
    // (b) It takes more calibration movements to get the magnetometer calibrated than in the
    //     NDOF mode.
    pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
    t.start();
    while (t.read() < 30) {
        d = imu.read_calib_status();
        pc.printf("calibrating, = 0x%x target  = 0x33(at least)\r\n", d);
        if ((d & 0x03) == 0x03) {
            break;
        }
        wait(1.0);
    }
    pc.printf("-> Step2) is done\r\n\r\n");
    //---------- Magnetometer Caliblation ---------------------------------------------------------
    // a) Place the device in 6 different stable positions for a period of few seconds
    //    to allow the accelerometer to calibrate.
    // b) Make sure that there is slow movement between 2 stable positions
    //    The 6 stable positions could be in any direction, but make sure that the device is
    //    lying at least once perpendicular to the x, y and z axis.
    pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
    pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
    pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
    pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
    pc.printf(" If you will give up, hit any key.\r\n", d);
    t.stop();
    while (true) {
        d = imu.read_calib_status();
        imu.get_gravity(&gravity);
        pc.printf("calibrating = 0x%x target  = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
                  d, gravity.x, gravity.y, gravity.z);
        if (d == 0xff) {
            break;
        }
        if (pc.readable()) {
            break;
        }
        wait(1.0);
    }
    if (imu.read_calib_status() == 0xff) {
        pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
    } else {
        pc.printf("-> Calibration steps are suspended!\r\n\r\n");
    }
    t.stop();
}

//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
//  Main Program
//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------
int main()
{
    pc.baud(115200);
    pc.attach(&rxCallback, MODSERIAL::RxIrq);
    pc.printf("BNO055 Hello World\r\n\r\n");
    imu.set_mounting_position(MT_P6);
    pwr_onoff = 0;
    pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
    char c = pc.getc();
    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
    // Is BNO055 avairable?
    if (imu.chip_ready() == 0) {
        do {
            pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
            pwr_onoff = 1;  // Power off
            wait(0.1);
            pwr_onoff = 0;  // Power on
            wait(0.02);
        } while(imu.reset());
    }
    pc.printf("Bosch BNO055 is available now!!\r\n");
    pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
              imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
    imu.read_id_inf(&bno055_id_inf);
    pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
              bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
    pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
              bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
    pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
    c = pc.getc();
    if (c == 'y') {
        bno055_calbration();
    }
    t.start();
    imu.write_reg0(0x3d, 0x07); // Default FUS

    while (1) {
        while (RAW == true) {            
            Time = t.read_ms();
            timeNow = Time + timeOfStart;
            if (timeNow > timeStamp + 9) {
                pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d,"
                          ,imu.read_reg0(0x08),imu.read_reg0(0x09)
                          ,imu.read_reg0(0x0A),imu.read_reg0(0x0B)
                          ,imu.read_reg0(0x0C),imu.read_reg0(0x0D));
                pc.printf("Time:,%d,/",timeNow);
                timeStamp = timeNow;
            }
        }
        while (RAW == false) {
            Time = t.read_ms();
            timeNow = + Time - timeOfStart;
            if (timeNow > timeStamp + 9) {
                imu.get_linear_accel(&linear_acc);
                pc.printf("FUS,Time:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , , , , , , /"
                          ,timeNow,linear_acc.x ,linear_acc.y ,linear_acc.z);
                timeStamp = timeNow;
            }
        }
    }
}

//         imu.get_Euler_Angles(&euler_angles);
//            pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
//                      euler_angles.h, euler_angles.r, euler_angles.p);
//            imu.get_linear_accel(&linear_acc);
//            pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
//                      linear_acc.x, linear_acc.y, linear_acc.z);
//
//            imu.get_gravity(&gravity);
//            pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
//                      gravity.x, gravity.y, gravity.z);
//
//            pc.printf("Time:,%d,\r\n",t.read_ms());
//        imu.get_Euler_Angles(&euler_angles);
//        pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
//                   euler_angles.h, euler_angles.r, euler_angles.p);
//        imu.get_quaternion(&quaternion);
//        pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
//                   quaternion.w, quaternion.x, quaternion.y, quaternion.z);
//        imu.get_linear_accel(&linear_acc);
//        pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
//                   linear_acc.x, linear_acc.y, linear_acc.z);
//        imu.get_gravity(&gravity);
//        pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
//                   gravity.x, gravity.y, gravity.z);
//        imu.get_chip_temperature(&chip_temp);
//        pc.printf("[T],%+d,%+d,",
//                   chip_temp.acc_chip, chip_temp.gyr_chip);
//        pc.printf("[S],0x%x,[M],%d\r\n",
//                   imu.read_calib_status(), t.read_ms());
//        pc.printf("Words\r\n");