Diff: main.cpp

Revision:

0:a573c39fe4e8

Child:

1:01b7a386487e

--- /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