Marcell Rausch / Mbed 2 deprecated F042_BNO055_toNRF24

Send on Sync message

Dependencies:   BNO055_fusion RF24_fork mbed

Fork of F042_BNO055_toNRF24 by BME SmartLab

main.cpp@3:6bba74b99bd3, 2017-02-06 (annotated)

Committer:
mrcrsch
Date:
Mon Feb 06 21:20:55 2017 +0000
Revision:
3:6bba74b99bd3
Parent:
1:03c9c0e3fc70 
working version with sync messages. mbed-dev lib needed without stdio and serial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mrcrsch 0:5283bf22044b 1 #include "mbed.h"
mrcrsch 0:5283bf22044b 2 #include "BNO055.h"
mrcrsch 0:5283bf22044b 3 #include "RF24.h"
mrcrsch 0:5283bf22044b 4 #include "config.h"
mrcrsch 0:5283bf22044b 5
mrcrsch 3:6bba74b99bd3 6 #define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8
mrcrsch 3:6bba74b99bd3 7 #define STM32_UUID ((uint32_t *)0x40048058)
mrcrsch 0:5283bf22044b 8
mrcrsch 0:5283bf22044b 9 DigitalOut led(ledpin);
mrcrsch 1:03c9c0e3fc70 10 I2C i2c(i2c_sda, i2c_scl);
mrcrsch 0:5283bf22044b 11
mrcrsch 0:5283bf22044b 12 RF24 radio(spi_MOSI, spi_MISO, spi_SCK, nrf_CE, nrf_CSN );
mrcrsch 0:5283bf22044b 13
mrcrsch 0:5283bf22044b 14 const uint64_t DataAddress = 0xF0F0F0F0E1LL;
mrcrsch 0:5283bf22044b 15 const uint64_t SyncAddress = 0xF0F0F0F0D2LL;
mrcrsch 0:5283bf22044b 16
mrcrsch 0:5283bf22044b 17
mrcrsch 0:5283bf22044b 18
mrcrsch 0:5283bf22044b 19 struct imuData {
mrcrsch 0:5283bf22044b 20 uint8_t sensorID;
mrcrsch 0:5283bf22044b 21 uint8_t packageCntr;
mrcrsch 0:5283bf22044b 22 int16_t q[4];
mrcrsch 0:5283bf22044b 23 int32_t linAcc[3];
mrcrsch 0:5283bf22044b 24 } imuData;
mrcrsch 0:5283bf22044b 25
mrcrsch 1:03c9c0e3fc70 26 struct SyncMsg {
mrcrsch 1:03c9c0e3fc70 27 bool Sleep; //false if sensor should not sleep
mrcrsch 1:03c9c0e3fc70 28 } SyncMsg;
mrcrsch 0:5283bf22044b 29
mrcrsch 0:5283bf22044b 30 //InterruptIn NRF_irq(PA_0);
mrcrsch 0:5283bf22044b 31
mrcrsch 0:5283bf22044b 32 void sendIRQ();
mrcrsch 0:5283bf22044b 33 void ReadIMU();
mrcrsch 0:5283bf22044b 34 void SendMSG();
mrcrsch 1:03c9c0e3fc70 35 void SetupRadio();
mrcrsch 0:5283bf22044b 36
mrcrsch 0:5283bf22044b 37
mrcrsch 0:5283bf22044b 38
mrcrsch 0:5283bf22044b 39 BNO055 imu(i2c, PA_2); //RESET: not conected, just an unused pin
mrcrsch 0:5283bf22044b 40
mrcrsch 0:5283bf22044b 41 BNO055_ID_INF_TypeDef bno055_id_inf;
mrcrsch 0:5283bf22044b 42 BNO055_QUATERNION_TypeDef BNO055_quaternion;
mrcrsch 0:5283bf22044b 43 BNO055_EULER_TypeDef euler_angles;
mrcrsch 0:5283bf22044b 44 BNO055_LIN_ACC_TypeDef linear_acceleration;
mrcrsch 0:5283bf22044b 45
mrcrsch 0:5283bf22044b 46 uint8_t AcclerometerScale = 3; // 3 = 16G
mrcrsch 0:5283bf22044b 47 bool AGCalFlag = false;
mrcrsch 0:5283bf22044b 48 bool MCalFlag = false;
mrcrsch 0:5283bf22044b 49 bool SendFlag = false;
mrcrsch 0:5283bf22044b 50
mrcrsch 0:5283bf22044b 51
mrcrsch 0:5283bf22044b 52
mrcrsch 0:5283bf22044b 53 int main()
mrcrsch 0:5283bf22044b 54 {
mrcrsch 0:5283bf22044b 55
mrcrsch 0:5283bf22044b 56 if (imu.chip_ready() == 0) {
mrcrsch 0:5283bf22044b 57 do {
mrcrsch 0:5283bf22044b 58 led = !led;
mrcrsch 0:5283bf22044b 59 wait(0.1);
mrcrsch 0:5283bf22044b 60 led = !led;
mrcrsch 0:5283bf22044b 61 wait(0.1);
mrcrsch 0:5283bf22044b 62 } while(imu.reset());
mrcrsch 0:5283bf22044b 63 }
mrcrsch 0:5283bf22044b 64
mrcrsch 0:5283bf22044b 65 imu.set_mounting_position(MT_P0);
mrcrsch 0:5283bf22044b 66 imu.configure_accelerometer_range(AcclerometerScale);
mrcrsch 0:5283bf22044b 67
mrcrsch 0:5283bf22044b 68 imu.read_id_inf(&bno055_id_inf); //read sensor IDs
mrcrsch 0:5283bf22044b 69
mrcrsch 0:5283bf22044b 70
mrcrsch 1:03c9c0e3fc70 71 SetupRadio();
mrcrsch 3:6bba74b99bd3 72
mrcrsch 3:6bba74b99bd3 73 uint32_t idPart1 = STM32_UUID[0];
mrcrsch 3:6bba74b99bd3 74 uint32_t idPart2 = STM32_UUID[1];
mrcrsch 3:6bba74b99bd3 75 uint32_t idPart3 = STM32_UUID[2];
mrcrsch 0:5283bf22044b 76 //radio setup
mrcrsch 1:03c9c0e3fc70 77
mrcrsch 1:03c9c0e3fc70 78
mrcrsch 0:5283bf22044b 79 //Attach Interrupt
mrcrsch 0:5283bf22044b 80 //NRF_irq.fall(&sendIRQ);
mrcrsch 1:03c9c0e3fc70 81
mrcrsch 1:03c9c0e3fc70 82
mrcrsch 0:5283bf22044b 83
mrcrsch 3:6bba74b99bd3 84 imuData.sensorID = idPart2;
mrcrsch 0:5283bf22044b 85 imuData.packageCntr = 0;
mrcrsch 0:5283bf22044b 86
mrcrsch 0:5283bf22044b 87 while(true) {
mrcrsch 1:03c9c0e3fc70 88 //imu.get_Euler_Angles(&euler_angles);
mrcrsch 0:5283bf22044b 89
mrcrsch 0:5283bf22044b 90
mrcrsch 1:03c9c0e3fc70 91 if(radio.available()) {
mrcrsch 1:03c9c0e3fc70 92 if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
mrcrsch 1:03c9c0e3fc70 93 continue;
mrcrsch 1:03c9c0e3fc70 94 radio.read(&SyncMsg,sizeof(SyncMsg));
mrcrsch 1:03c9c0e3fc70 95
mrcrsch 1:03c9c0e3fc70 96 wait_ms( (NodeID-1) * SendDelay); // wait before Send
mrcrsch 1:03c9c0e3fc70 97
mrcrsch 1:03c9c0e3fc70 98 ReadIMU();
mrcrsch 1:03c9c0e3fc70 99 SendMSG();
mrcrsch 1:03c9c0e3fc70 100 }
mrcrsch 1:03c9c0e3fc70 101
mrcrsch 1:03c9c0e3fc70 102
mrcrsch 1:03c9c0e3fc70 103
mrcrsch 0:5283bf22044b 104 }//while
mrcrsch 0:5283bf22044b 105
mrcrsch 0:5283bf22044b 106
mrcrsch 0:5283bf22044b 107 }
mrcrsch 0:5283bf22044b 108
mrcrsch 0:5283bf22044b 109 /*
mrcrsch 0:5283bf22044b 110 void sendIRQ(){
mrcrsch 0:5283bf22044b 111 if(radio.getDynamicPayloadSize() < 1)// Corrupt payload has been flushed
mrcrsch 1:03c9c0e3fc70 112 return;
mrcrsch 1:03c9c0e3fc70 113
mrcrsch 0:5283bf22044b 114 radio.read(&Sync,sizeof(Sync));
mrcrsch 0:5283bf22044b 115 if(Sync.ID == imuData.sensorID){
mrcrsch 0:5283bf22044b 116 if(Sync.ARange > 0)
mrcrsch 0:5283bf22044b 117 AcclerometerScale = Sync.ARange;
mrcrsch 1:03c9c0e3fc70 118
mrcrsch 0:5283bf22044b 119 AGCalFlag = Sync.AGCal;
mrcrsch 0:5283bf22044b 120 MCalFlag = Sync.MCal;
mrcrsch 0:5283bf22044b 121 SendFlag = true;
mrcrsch 0:5283bf22044b 122 }
mrcrsch 1:03c9c0e3fc70 123
mrcrsch 0:5283bf22044b 124 }*/
mrcrsch 0:5283bf22044b 125
mrcrsch 1:03c9c0e3fc70 126 void SetupRadio()
mrcrsch 1:03c9c0e3fc70 127 {
mrcrsch 1:03c9c0e3fc70 128 radio.begin();
mrcrsch 1:03c9c0e3fc70 129 radio.setPALevel(RF24_PA_MAX) ;
mrcrsch 1:03c9c0e3fc70 130 radio.setDataRate(RF24_2MBPS);
mrcrsch 1:03c9c0e3fc70 131 radio.setCRCLength(RF24_CRC_16);
mrcrsch 1:03c9c0e3fc70 132 radio.setChannel(RadioChannel);
mrcrsch 1:03c9c0e3fc70 133
mrcrsch 1:03c9c0e3fc70 134 radio.setRetries(0,2);
mrcrsch 1:03c9c0e3fc70 135
mrcrsch 1:03c9c0e3fc70 136 radio.enableDynamicAck();
mrcrsch 1:03c9c0e3fc70 137 radio.enableDynamicPayloads();
mrcrsch 1:03c9c0e3fc70 138
mrcrsch 1:03c9c0e3fc70 139 radio.openWritingPipe(DataAddress);
mrcrsch 1:03c9c0e3fc70 140 radio.openReadingPipe(1,SyncAddress);
mrcrsch 1:03c9c0e3fc70 141 //radio.stopListening();
mrcrsch 1:03c9c0e3fc70 142 //radio.setAutoAck(1,false);
mrcrsch 1:03c9c0e3fc70 143 radio.startListening();
mrcrsch 1:03c9c0e3fc70 144
mrcrsch 1:03c9c0e3fc70 145 }
mrcrsch 1:03c9c0e3fc70 146
mrcrsch 1:03c9c0e3fc70 147
mrcrsch 1:03c9c0e3fc70 148 void ReadIMU()
mrcrsch 1:03c9c0e3fc70 149 {
mrcrsch 0:5283bf22044b 150 imu.get_quaternion(&BNO055_quaternion);
mrcrsch 0:5283bf22044b 151
mrcrsch 0:5283bf22044b 152 imuData.q[0] = BNO055_quaternion.w;
mrcrsch 0:5283bf22044b 153 imuData.q[1] = BNO055_quaternion.x;
mrcrsch 0:5283bf22044b 154 imuData.q[2] = BNO055_quaternion.y;
mrcrsch 0:5283bf22044b 155 imuData.q[3] = BNO055_quaternion.z;
mrcrsch 0:5283bf22044b 156
mrcrsch 0:5283bf22044b 157
mrcrsch 0:5283bf22044b 158 imu.get_abs_accel(&linear_acceleration);
mrcrsch 0:5283bf22044b 159
mrcrsch 0:5283bf22044b 160 float lax = 1.0;
mrcrsch 0:5283bf22044b 161 float lay = 2.0;
mrcrsch 0:5283bf22044b 162 float laz = 3.0;
mrcrsch 0:5283bf22044b 163
mrcrsch 0:5283bf22044b 164 lax = (float)linear_acceleration.x;
mrcrsch 0:5283bf22044b 165 lay = (float)linear_acceleration.y;
mrcrsch 0:5283bf22044b 166 laz = (float)linear_acceleration.z;
mrcrsch 0:5283bf22044b 167
mrcrsch 0:5283bf22044b 168 memcpy(imuData.linAcc , &lax, 4);
mrcrsch 0:5283bf22044b 169 memcpy(&imuData.linAcc[1] , &lay, 4);
mrcrsch 0:5283bf22044b 170 memcpy(&imuData.linAcc[2] , &laz, 4);
mrcrsch 1:03c9c0e3fc70 171
mrcrsch 0:5283bf22044b 172 }
mrcrsch 0:5283bf22044b 173
mrcrsch 1:03c9c0e3fc70 174 void SendMSG()
mrcrsch 1:03c9c0e3fc70 175 {
mrcrsch 1:03c9c0e3fc70 176 radio.stopListening();
mrcrsch 1:03c9c0e3fc70 177 radio.write(&imuData, sizeof(imuData), true);
mrcrsch 1:03c9c0e3fc70 178 radio.startListening();
mrcrsch 0:5283bf22044b 179
mrcrsch 1:03c9c0e3fc70 180 if(imuData.packageCntr < 255)
mrcrsch 1:03c9c0e3fc70 181 imuData.packageCntr++;
mrcrsch 1:03c9c0e3fc70 182 else
mrcrsch 1:03c9c0e3fc70 183 imuData.packageCntr = 0;
mrcrsch 1:03c9c0e3fc70 184 led = !led;
mrcrsch 0:5283bf22044b 185 }