Diff: main.cpp

Revision:

0:5283bf22044b

Child:

1:03c9c0e3fc70

diff -r 000000000000 -r 5283bf22044b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jan 16 16:51:56 2017 +0000
@@ -0,0 +1,167 @@
+#include "mbed.h"
+#include "BNO055.h"
+#include "RF24.h"
+#include "config.h"
+
+#ifdef DEVICE_SERIAL
+#undef DEVICE_SERIAL
+#endif
+
+DigitalOut led(ledpin);
+I2C i2c(i2c_sda,i2c_scl);
+
+RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );
+
+const uint64_t DataAddress =  0xF0F0F0F0E1LL;
+const uint64_t SyncAddress = 0xF0F0F0F0D2LL;
+
+
+
+struct imuData {
+    uint8_t sensorID;
+    uint8_t packageCntr;
+    int16_t q[4];
+    int32_t linAcc[3];
+
+} imuData;
+
+struct Sync {
+    uint8_t ID;
+    uint8_t ARange;
+    bool AGCal;
+    bool MCal;
+} Sync;
+
+//InterruptIn NRF_irq(PA_0);
+
+void sendIRQ();
+void ReadIMU();
+void SendMSG();
+
+
+
+BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin
+
+BNO055_ID_INF_TypeDef bno055_id_inf;
+BNO055_QUATERNION_TypeDef BNO055_quaternion;
+BNO055_EULER_TypeDef  euler_angles;
+BNO055_LIN_ACC_TypeDef linear_acceleration;
+
+uint8_t AcclerometerScale = 3; // 3 = 16G
+bool AGCalFlag = false;
+bool MCalFlag = false;
+bool SendFlag = false;
+
+
+//Timer SendTimer;
+
+
+int main()
+{
+
+    if (imu.chip_ready() == 0) {
+        do {
+            led = !led;
+            wait(0.1);
+            led = !led;
+            wait(0.1);
+        } while(imu.reset());
+    }
+
+    imu.set_mounting_position(MT_P0);
+    imu.configure_accelerometer_range(AcclerometerScale);
+
+    imu.read_id_inf(&bno055_id_inf);  //read sensor IDs
+
+
+
+    //radio setup
+    radio.begin();
+    radio.setPALevel(RF24_PA_MAX) ;
+    radio.setDataRate(RF24_2MBPS);
+    radio.setCRCLength(RF24_CRC_16);
+    radio.setChannel(120);
+    /*
+    radio.enableDynamicAck();
+    radio.enableDynamicPayloads();
+    */
+    radio.openWritingPipe(DataAddress);    
+    radio.openReadingPipe(1,SyncAddress);
+    radio.stopListening();
+   /* radio.setAutoAck(1,false);
+    radio.startListening();*/
+    
+    //Attach Interrupt
+    //NRF_irq.fall(&sendIRQ);
+    
+    
+
+    imuData.sensorID = NodeID;
+    imuData.packageCntr = 0;
+
+    while(true) {
+       //imu.get_Euler_Angles(&euler_angles);
+    
+        wait_ms(33);
+        ReadIMU();
+        SendMSG();
+        
+
+
+        
+    }//while
+
+
+}
+
+/*
+void sendIRQ(){
+     if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
+        return; 
+        
+    radio.read(&Sync,sizeof(Sync));
+    if(Sync.ID == imuData.sensorID){
+        if(Sync.ARange > 0)
+            AcclerometerScale = Sync.ARange;
+            
+        AGCalFlag = Sync.AGCal;
+        MCalFlag = Sync.MCal;
+        SendFlag = true;
+    }
+    
+}*/
+
+void ReadIMU(){
+    imu.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;
+
+
+    imu.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 SendMSG(){
+    radio.write(&imuData, sizeof(imuData));
+
+        if(imuData.packageCntr < 255)
+            imuData.packageCntr++;
+        else
+            imuData.packageCntr = 0;
+        led = !led;
+}
\ No newline at end of file