PCA9548 + BNO055
Dependencies: BNO055_fusion RF24_fork mbed
Revision 0:a573c39fe4e8, committed 2017-02-22
- Comitter:
- Makodan
- Date:
- Wed Feb 22 15:52:57 2017 +0000
- Commit message:
- 1;
Changed in this revision
diff -r 000000000000 -r a573c39fe4e8 BNO055_fusion.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO055_fusion.lib Wed Feb 22 15:52:57 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mrcrsch/code/BNO055_fusion/#6a08a4c5b1e1
diff -r 000000000000 -r a573c39fe4e8 RF24.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RF24.lib Wed Feb 22 15:52:57 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mrcrsch/code/RF24_fork/#0a3f44454ee7
diff -r 000000000000 -r a573c39fe4e8 config.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/config.h Wed Feb 22 15:52:57 2017 +0000 @@ -0,0 +1,33 @@ +#ifndef CONFIG_H +#define CONFIG_H + +#include "BNO055.h" + +//This header contains calibration data + +#define NodeID 6 +#define RadioChannel 90 +#define SendDelay 3 //ms + + + #define ledpin PA_8 + #define unused_pin PA_1 + + #define nrf_CE PB_4 + #define nrf_CSN PB_3 + #define spi_SCK PA_5 + #define spi_MOSI PA_7 + #define spi_MISO PA_6 + #define nrf_irq PA_15 + + #define i2c_sda PB_7 + #define i2c_scl PB_6 + + #define RXD PA_3 + #define TXD PA_2 + + + +#endif + +
diff -r 000000000000 -r a573c39fe4e8 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Feb 22 15:52:57 2017 +0000 @@ -0,0 +1,316 @@ +#include "mbed.h" +#include "BNO055.h" +#include "config.h" +#include "RF24.h" + + + +I2C i2c(i2c_sda, i2c_scl); +Serial pc(TXD, RXD, 9600); +DigitalOut myled(ledpin); + +RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN ); + +const uint64_t DataAddress = 0xF0F0F0F0E1LL; +const uint64_t SyncAddress = 0xF0F0F0F0D2LL; + +#define PACKET_Q 1 +#define PACKET_I 2 + +void ScanMUX(); //Scanning on all channels +void Read1MUX(); //Read sensors on 0x28 address (all channels) +void Read2MUX(); //Read sensors on 0x29 address (all channels) +void ChannelMUX(uint8_t channel); +void ReadIMU1(uint8_t channel); //Single sensor reading on 0x28 address +void ReadIMU2(uint8_t channel); //Single sensor reading on 0x29 address + +uint8_t AcclerometerScale = 3; // 3 = 16G +bool AGCalFlag = false; +bool MCalFlag = false; +bool SendFlag = false; + +// Sensor arrays - contain available sensors +bool sensarray1[] = {false, false, false, false, false, false, false}; // 0x28 array +bool sensarray2[] = {false, false, false, false, false, false, false}; // 0x29 array + + +struct imuData { + uint8_t sensorID; + uint8_t packageCntr; + int16_t q[4]; + int32_t linAcc[3]; + +} imuData; + +BNO055 imu1(i2c_sda, i2c_scl, unused_pin, 0x50, MODE_NDOF); // Address: 0x28 +BNO055 imu2(i2c_sda, i2c_scl, unused_pin, 0x52, MODE_NDOF); // Address: 0x29 + +BNO055_ID_INF_TypeDef bno055_id_inf; +BNO055_QUATERNION_TypeDef BNO055_quaternion; +BNO055_EULER_TypeDef euler_angles; +BNO055_LIN_ACC_TypeDef linear_acceleration; + +struct SyncMsg { + bool Sleep; //false if sensor should not sleep +} SyncMsg; + +//InterruptIn NRF_irq(PA_0); + +void sendIRQ(); +void SendMSG(); +void SetupRadio(); + + +int main() +{ + + myled=1; + int i; + + i2c.frequency(400000); + + //radio setup + SetupRadio(); + + + imuData.packageCntr = 0; + + // Sensor scanning + ScanMUX(); + + // Print out sensor arrays + pc.printf("\r\n\t\tCH0\tCH1\tCH2\tCH3\tCH4\tCH5\tCH6"); + pc.printf("\r\n0x28 sensors:"); + for(i=0; i<7; i++){ + pc.printf("\t%d", sensarray1[i]);} + + pc.printf("\r\n0x29 sensors:"); + for(i=0; i<7; i++){ + pc.printf("\t%d", sensarray2[i]);} + pc.printf("\r\n"); + + + while (1) { + + Read1MUX(); + Read2MUX(); + + + myled = !myled; + + } + +} + + +void ScanMUX(){ + + for(uint8_t x=0; x<7; x++) + { + + ChannelMUX(x); + + pc.printf("Scanning CH: %d\r\n", x); + + wait(0.001); + imu1.reset(); + imu2.reset(); + wait(0.01); + + if(imu1.chip_ready() == 0){ + pc.printf("Bosch BNO055 is NOT available at address 0x28!!\r\n"); + + imu1.reset(); + + } + + if(imu2.chip_ready() == 0){ + pc.printf("Bosch BNO055 is NOT available at address 0x29!!\r\n"); + + imu2.reset(); + + } + + + if(imu1.chip_ready() == 1){ + pc.printf("Bosch BNO055 is AVAILABLE at address 0x28!!\r\n"); + imu1.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); + + imu1.set_mounting_position(MT_P0); + imu1.configure_accelerometer_range(AcclerometerScale); + + sensarray1[x] = true; + + } + + + + if(imu2.chip_ready() == 1){ + pc.printf("Bosch BNO055 is AVAILABLE at address 0x29!!\r\n"); + imu2.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); + + imu2.set_mounting_position(MT_P0); + imu2.configure_accelerometer_range(AcclerometerScale); + + sensarray2[x] = true; + + } + } + + + } + + + void Read1MUX(){ + + uint8_t i=0; + + for( i=0; i<7; i++) + { + + ChannelMUX(i); + + if(sensarray1[i]){ + ReadIMU1(i); + pc.printf("ID:%d\t%d\t%d\t%d\t%d\r\n",imuData.sensorID,imuData.q[0],imuData.q[1],imuData.q[2],imuData.q[3]); + + } + + } + + } + + +void Read2MUX(){ + + uint8_t i=0; + + for( i=0; i<7; i++) + { + + ChannelMUX(i); + + if(sensarray2[i]){ + ReadIMU2(i); + pc.printf("ID:%d\t%d\t%d\t%d\t%d\r\n",imuData.sensorID,imuData.q[0],imuData.q[1],imuData.q[2],imuData.q[3]); + + } + + } + + } + + +void ChannelMUX(uint8_t channel) +{ + char get; + char select; + + select = 1 << channel; + + i2c.write(0xE0, &select ,1); + wait(0.001); + i2c.read(0xE0, &get, 1); + wait(0.001); + i2c.write(0xE0, &select ,1); + wait(0.001); + i2c.read(0xE0, &get, 1); + +} + + + void ReadIMU1(uint8_t channel) +{ + imuData.sensorID = (channel * 10); + imu1.get_quaternion(&BNO055_quaternion); + + imuData.q[0] = BNO055_quaternion.w; + imuData.q[1] = BNO055_quaternion.x; + imuData.q[2] = BNO055_quaternion.y; + imuData.q[3] = BNO055_quaternion.z; + + + imu1.get_abs_accel(&linear_acceleration); + + float lax = 1.0; + float lay = 2.0; + float laz = 3.0; + + lax = (float)linear_acceleration.x; + lay = (float)linear_acceleration.y; + laz = (float)linear_acceleration.z; + + memcpy(imuData.linAcc , &lax, 4); + memcpy(&imuData.linAcc[1] , &lay, 4); + memcpy(&imuData.linAcc[2] , &laz, 4); + +} + + void ReadIMU2(uint8_t channel) +{ + imuData.sensorID = ((channel * 10) + 1); // ID: CH number + 1 + imu2.get_quaternion(&BNO055_quaternion); + + imuData.q[0] = BNO055_quaternion.w; + imuData.q[1] = BNO055_quaternion.x; + imuData.q[2] = BNO055_quaternion.y; + imuData.q[3] = BNO055_quaternion.z; + + + imu2.get_abs_accel(&linear_acceleration); + + float lax = 1.0; + float lay = 2.0; + float laz = 3.0; + + lax = (float)linear_acceleration.x; + lay = (float)linear_acceleration.y; + laz = (float)linear_acceleration.z; + + memcpy(imuData.linAcc , &lax, 4); + memcpy(&imuData.linAcc[1] , &lay, 4); + memcpy(&imuData.linAcc[2] , &laz, 4); + +} + +void SetupRadio() +{ + radio.begin(); + radio.setPALevel(RF24_PA_MAX) ; + radio.setDataRate(RF24_2MBPS); + radio.setCRCLength(RF24_CRC_16); + radio.setChannel(RadioChannel); + + radio.setRetries(0,2); + + radio.enableDynamicAck(); + radio.enableDynamicPayloads(); + + radio.openWritingPipe(DataAddress); + radio.openReadingPipe(1,SyncAddress); + //radio.stopListening(); + //radio.setAutoAck(1,false); + radio.startListening(); + +} + +void SendMSG() +{ + radio.stopListening(); + radio.write(&imuData, sizeof(imuData), true); + radio.startListening(); + + if(imuData.packageCntr < 255) + imuData.packageCntr++; + else + imuData.packageCntr = 0; + myled = !myled; +} \ No newline at end of file
diff -r 000000000000 -r a573c39fe4e8 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Feb 22 15:52:57 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/ef9c61f8c49f \ No newline at end of file