File content as of revision 50:453855ed9372:

/*
	Ultrasound and IMU Demo
	Sam Walsh 2018
*/


/* Includes ------------------------------------------------------------------*/

#include "mbed.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include "hcsr04.h"
#include    "BNO055.h"


/* Definitions ---------------------------------------------------------------*/

/* Variables -----------------------------------------------------------------*/

HCSR04 UltrasonicSensor(D7,D8); //Trigger,Echo 
Serial pc(USBTX, USBRX); // tx, rx
I2C    i2c(D4, D5); // SDA, SCL
BNO055 imu(i2c, D6);  // Reset =D6, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default

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;

float distance;

/* Functions -----------------------------------------------------------------*/

/*=================================== Main ==================================
  Prints on the serial over USB the measured distance and lux.
  The measures are run in single shot polling mode.
=============================================================================*/
int main()
{ 
	pc.baud(921600);
	pc.printf ("Hello World \n\r"); 
	imu.set_mounting_position(MT_P6);
    // Is BNO055 avairable?
    if (imu.chip_ready() == 0){
        do {
            pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
        } 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("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],");
    pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n");
    t.start();
    while(1) {
		pc.printf ("Ultrasound: %d \t", (int)distance);  
		distance = UltrasonicSensor.distance(); 
        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());
    }
}


	//while(1){
 
//    	wait(0.5);
//    }
//}