Diff: main.cpp

Revision:

0:36135689a3f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 13 14:26:46 2017 +0000
@@ -0,0 +1,176 @@
+#include "mbed.h"
+#include "BNO055.h"
+#include "RF24.h"
+#include "config.h"
+
+
+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 rcvStruct{
+       uint8_t channel_num;
+}rcvStruct;
+
+InterruptIn NRF_irq(nrf_irq);
+InterruptIn IMU_irq(mpu_irq);
+
+void nrf_irq_handler();
+void imu_irq_handler();
+void ReadIMU();
+void SendMSG();
+void SetupRadio();
+
+
+
+BNO055 imu(i2c, PA_2); //RESET: PA_2
+
+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;
+
+
+
+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
+    //imu.setpowermode(1);
+
+    SetupRadio();
+    //radio setup
+
+
+    //Attach Interrupt
+    NRF_irq.fall(&nrf_irq_handler);
+    /*IMU_irq.mode(PullUp);
+    IMU_irq.rise(&imu_irq_handler);*/
+    
+    //imu.initIntr();
+    
+
+
+    imuData.sensorID = NodeID;
+    imuData.packageCntr = 0;
+
+    while(true) {
+      
+
+        ReadIMU();
+        SendMSG();
+        //led = !led;
+        
+        wait_ms(10);
+    
+
+    }//while
+
+
+}
+void imu_irq_handler(){
+    led = !led;
+    //imu.resetIntr();   
+}
+
+
+void nrf_irq_handler(){
+     if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
+        return;
+
+    radio.read(&rcvStruct,sizeof(rcvStruct));
+    radio.setChannel(rcvStruct.channel_num);
+    
+
+}
+
+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 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.stopListening();
+    radio.write(&imuData, sizeof(imuData), true);
+    radio.startListening();
+
+    if(imuData.packageCntr < 255)
+        imuData.packageCntr++;
+    else
+        imuData.packageCntr = 0;
+    
+}
\ No newline at end of file