Daniel Mako
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
--- /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
--- /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
--- /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 + +
--- /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
--- /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