aconno acnsensa project for iOS devices with iBeacon packets support.
Dependencies: LSM9DS1 Si7006A20 aconno_SEGGER_RTT aconno_bsp adc52832_common
main.cpp@0:12899fa39f88, 2017-11-27 (annotated)
- Committer:
- jurica238814
- Date:
- Mon Nov 27 12:12:59 2017 +0000
- Revision:
- 0:12899fa39f88
- Child:
- 1:326ce5e200fb
Sensor board FW. Works with aconno v2 sensor board.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jurica238814 | 0:12899fa39f88 | 1 | /* |
jurica238814 | 0:12899fa39f88 | 2 | * aconno.de |
jurica238814 | 0:12899fa39f88 | 3 | * Made by Jurica Resetar |
jurica238814 | 0:12899fa39f88 | 4 | * All right reserved |
jurica238814 | 0:12899fa39f88 | 5 | * |
jurica238814 | 0:12899fa39f88 | 6 | */ |
jurica238814 | 0:12899fa39f88 | 7 | |
jurica238814 | 0:12899fa39f88 | 8 | #include "mbed.h" |
jurica238814 | 0:12899fa39f88 | 9 | #include "ble/BLE.h" |
jurica238814 | 0:12899fa39f88 | 10 | #include "acd52832_bsp.h" |
jurica238814 | 0:12899fa39f88 | 11 | #include "GapAdvertisingData.h" |
jurica238814 | 0:12899fa39f88 | 12 | #include "Si7006A20.h" |
jurica238814 | 0:12899fa39f88 | 13 | #include "LSM9DS1.h" |
jurica238814 | 0:12899fa39f88 | 14 | #include "math.h" |
jurica238814 | 0:12899fa39f88 | 15 | #include "nrf52_digital.h" |
jurica238814 | 0:12899fa39f88 | 16 | |
jurica238814 | 0:12899fa39f88 | 17 | #define V0 0.47 /* In volts */ |
jurica238814 | 0:12899fa39f88 | 18 | #define TC 0.01 /* In volts */ |
jurica238814 | 0:12899fa39f88 | 19 | #define VCC (3.6) |
jurica238814 | 0:12899fa39f88 | 20 | |
jurica238814 | 0:12899fa39f88 | 21 | #define DEBUG_PRINT (0) |
jurica238814 | 0:12899fa39f88 | 22 | #define SLEEP_TIME (0.150) /* Sleep time in seconds */ |
jurica238814 | 0:12899fa39f88 | 23 | #define WAKE_UP_TIME (0.150) /* Awake time in ms */ |
jurica238814 | 0:12899fa39f88 | 24 | #define MSD_SIZE (25) /* Manufacturer Specific Data lenght (in B) */ |
jurica238814 | 0:12899fa39f88 | 25 | #define ADV_INTERVAL (100) /* Advertising interval in ms */ |
jurica238814 | 0:12899fa39f88 | 26 | #define TX_POWER (4) /* TX power (in dB) */ |
jurica238814 | 0:12899fa39f88 | 27 | #define GO_TO_SLEEP (0) /* Sleep flag: 0 -> Device will not go to sleep, 1 -> Will go to sleep mode */ |
jurica238814 | 0:12899fa39f88 | 28 | |
jurica238814 | 0:12899fa39f88 | 29 | // Definitions of the location within MSD for the data |
jurica238814 | 0:12899fa39f88 | 30 | #define ADV_TYPE (4) |
jurica238814 | 0:12899fa39f88 | 31 | #define GYRO (5) |
jurica238814 | 0:12899fa39f88 | 32 | #define ACC (11) |
jurica238814 | 0:12899fa39f88 | 33 | #define MAGNETO (17) |
jurica238814 | 0:12899fa39f88 | 34 | #define TEMPERATURE (5) |
jurica238814 | 0:12899fa39f88 | 35 | #define HUMIDITY (9) |
jurica238814 | 0:12899fa39f88 | 36 | #define PRESSURE (13) |
jurica238814 | 0:12899fa39f88 | 37 | #define LIGHT (17) |
jurica238814 | 0:12899fa39f88 | 38 | |
jurica238814 | 0:12899fa39f88 | 39 | |
jurica238814 | 0:12899fa39f88 | 40 | AnalogIn temperature(ADC_TEMP); |
jurica238814 | 0:12899fa39f88 | 41 | AnalogIn light(ADC_LIGHT); |
jurica238814 | 0:12899fa39f88 | 42 | NRF52_DigitalOut lightPower(p28); |
jurica238814 | 0:12899fa39f88 | 43 | NRF52_DigitalOut temperaturePower(p31); |
jurica238814 | 0:12899fa39f88 | 44 | SPI spi(p3, p5, p4); // mosi, miso, sclk |
jurica238814 | 0:12899fa39f88 | 45 | NRF52_DigitalOut cs(p7); |
jurica238814 | 0:12899fa39f88 | 46 | NRF52_DigitalOut shdn(p6); |
jurica238814 | 0:12899fa39f88 | 47 | NRF52_DigitalOut led(p23); |
jurica238814 | 0:12899fa39f88 | 48 | NRF52_DigitalOut power(p2); |
jurica238814 | 0:12899fa39f88 | 49 | Si7006 *si; |
jurica238814 | 0:12899fa39f88 | 50 | LSM9DS1 *mems; |
jurica238814 | 0:12899fa39f88 | 51 | |
jurica238814 | 0:12899fa39f88 | 52 | #if DEBUG_PRINT |
jurica238814 | 0:12899fa39f88 | 53 | #include "nrf52_uart.h" |
jurica238814 | 0:12899fa39f88 | 54 | NRF52_UART serial = NRF52_UART(p25,p26,Baud9600); //Tx, RX BaudRate |
jurica238814 | 0:12899fa39f88 | 55 | uint8_t charCounter; |
jurica238814 | 0:12899fa39f88 | 56 | char buffer[256] = {0}; |
jurica238814 | 0:12899fa39f88 | 57 | #define SEND(...) {uint8_t len = sprintf(buffer, __VA_ARGS__); serial.send(buffer, len);} |
jurica238814 | 0:12899fa39f88 | 58 | #else |
jurica238814 | 0:12899fa39f88 | 59 | #define SEND(...); |
jurica238814 | 0:12899fa39f88 | 60 | #endif |
jurica238814 | 0:12899fa39f88 | 61 | |
jurica238814 | 0:12899fa39f88 | 62 | bool SLEEP = true; |
jurica238814 | 0:12899fa39f88 | 63 | int8_t txPower = 4; |
jurica238814 | 0:12899fa39f88 | 64 | |
jurica238814 | 0:12899fa39f88 | 65 | union float2bytes{ |
jurica238814 | 0:12899fa39f88 | 66 | float f; |
jurica238814 | 0:12899fa39f88 | 67 | char b[sizeof(float)]; |
jurica238814 | 0:12899fa39f88 | 68 | }; |
jurica238814 | 0:12899fa39f88 | 69 | |
jurica238814 | 0:12899fa39f88 | 70 | float2bytes temp2byte; |
jurica238814 | 0:12899fa39f88 | 71 | float2bytes hum2byte; |
jurica238814 | 0:12899fa39f88 | 72 | float2bytes light2byte; |
jurica238814 | 0:12899fa39f88 | 73 | float2bytes pressure2byte; |
jurica238814 | 0:12899fa39f88 | 74 | |
jurica238814 | 0:12899fa39f88 | 75 | uint16_t a0_frac_mask = 0x0007; |
jurica238814 | 0:12899fa39f88 | 76 | uint8_t a0_frac_bits = 3; |
jurica238814 | 0:12899fa39f88 | 77 | uint16_t b1_frac_mask = 0x1FFF; |
jurica238814 | 0:12899fa39f88 | 78 | uint8_t b1_frac_bits = 13; |
jurica238814 | 0:12899fa39f88 | 79 | uint16_t b2_frac_mask = 0x3FFF; |
jurica238814 | 0:12899fa39f88 | 80 | uint8_t b2_frac_bits = 14; |
jurica238814 | 0:12899fa39f88 | 81 | uint16_t c12_frac_mask = 0x1FFF; |
jurica238814 | 0:12899fa39f88 | 82 | uint8_t c12_frac_bits = 22; |
jurica238814 | 0:12899fa39f88 | 83 | |
jurica238814 | 0:12899fa39f88 | 84 | int16_t memsBuffer[3]; |
jurica238814 | 0:12899fa39f88 | 85 | int16_t memsAccInit[3]={0,0,0}; |
jurica238814 | 0:12899fa39f88 | 86 | int16_t memsGyroInit[3]={0,0,0}; |
jurica238814 | 0:12899fa39f88 | 87 | int16_t memsMagInit[3]={0,0,0}; |
jurica238814 | 0:12899fa39f88 | 88 | |
jurica238814 | 0:12899fa39f88 | 89 | BLE &ble = BLE::Instance(); |
jurica238814 | 0:12899fa39f88 | 90 | GapAdvertisingData adv_data = GapAdvertisingData(); |
jurica238814 | 0:12899fa39f88 | 91 | uint8_t MSD[MSD_SIZE] = {0x59, 0x00, 0x17, 0xCF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; |
jurica238814 | 0:12899fa39f88 | 92 | |
jurica238814 | 0:12899fa39f88 | 93 | |
jurica238814 | 0:12899fa39f88 | 94 | /** |
jurica238814 | 0:12899fa39f88 | 95 | * Restart Advertising on disconnection |
jurica238814 | 0:12899fa39f88 | 96 | */ |
jurica238814 | 0:12899fa39f88 | 97 | void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){ |
jurica238814 | 0:12899fa39f88 | 98 | BLE::Instance().gap().startAdvertising(); |
jurica238814 | 0:12899fa39f88 | 99 | } |
jurica238814 | 0:12899fa39f88 | 100 | |
jurica238814 | 0:12899fa39f88 | 101 | /** |
jurica238814 | 0:12899fa39f88 | 102 | * Function for waking the core up |
jurica238814 | 0:12899fa39f88 | 103 | */ |
jurica238814 | 0:12899fa39f88 | 104 | void wakeMeUp(void){ |
jurica238814 | 0:12899fa39f88 | 105 | SLEEP = false; |
jurica238814 | 0:12899fa39f88 | 106 | } |
jurica238814 | 0:12899fa39f88 | 107 | |
jurica238814 | 0:12899fa39f88 | 108 | /** |
jurica238814 | 0:12899fa39f88 | 109 | * This function is called when the ble initialization process has failed |
jurica238814 | 0:12899fa39f88 | 110 | */ |
jurica238814 | 0:12899fa39f88 | 111 | void onBleInitError(BLE &ble, ble_error_t error){ |
jurica238814 | 0:12899fa39f88 | 112 | /* Avoid compiler warnings */ |
jurica238814 | 0:12899fa39f88 | 113 | (void) ble; |
jurica238814 | 0:12899fa39f88 | 114 | (void) error; |
jurica238814 | 0:12899fa39f88 | 115 | /* Initialization error handling should go here */ |
jurica238814 | 0:12899fa39f88 | 116 | } |
jurica238814 | 0:12899fa39f88 | 117 | |
jurica238814 | 0:12899fa39f88 | 118 | /** |
jurica238814 | 0:12899fa39f88 | 119 | * Callback triggered when the ble initialization process has finished |
jurica238814 | 0:12899fa39f88 | 120 | */ |
jurica238814 | 0:12899fa39f88 | 121 | void bleInitComplete(BLE::InitializationCompleteCallbackContext *params){ |
jurica238814 | 0:12899fa39f88 | 122 | BLE& ble = params->ble; |
jurica238814 | 0:12899fa39f88 | 123 | ble_error_t error = params->error; |
jurica238814 | 0:12899fa39f88 | 124 | |
jurica238814 | 0:12899fa39f88 | 125 | if (error != BLE_ERROR_NONE) { |
jurica238814 | 0:12899fa39f88 | 126 | /* In case of error, forward the error handling to onBleInitError */ |
jurica238814 | 0:12899fa39f88 | 127 | onBleInitError(ble, error); |
jurica238814 | 0:12899fa39f88 | 128 | return; |
jurica238814 | 0:12899fa39f88 | 129 | } |
jurica238814 | 0:12899fa39f88 | 130 | |
jurica238814 | 0:12899fa39f88 | 131 | /* Ensure that it is the default instance of BLE */ |
jurica238814 | 0:12899fa39f88 | 132 | if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) { |
jurica238814 | 0:12899fa39f88 | 133 | return; |
jurica238814 | 0:12899fa39f88 | 134 | } |
jurica238814 | 0:12899fa39f88 | 135 | |
jurica238814 | 0:12899fa39f88 | 136 | ble.gap().onDisconnection(disconnectionCallback); |
jurica238814 | 0:12899fa39f88 | 137 | |
jurica238814 | 0:12899fa39f88 | 138 | /* setup advertising */ |
jurica238814 | 0:12899fa39f88 | 139 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
jurica238814 | 0:12899fa39f88 | 140 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::MANUFACTURER_SPECIFIC_DATA, (uint8_t *)MSD, MSD_SIZE); |
jurica238814 | 0:12899fa39f88 | 141 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED); |
jurica238814 | 0:12899fa39f88 | 142 | ble.gap().setAdvertisingInterval(ADV_INTERVAL); |
jurica238814 | 0:12899fa39f88 | 143 | ble.gap().startAdvertising(); |
jurica238814 | 0:12899fa39f88 | 144 | } |
jurica238814 | 0:12899fa39f88 | 145 | |
jurica238814 | 0:12899fa39f88 | 146 | |
jurica238814 | 0:12899fa39f88 | 147 | |
jurica238814 | 0:12899fa39f88 | 148 | void getLight(uint8_t *light_data){ |
jurica238814 | 0:12899fa39f88 | 149 | light2byte.f = light.read() * 100; |
jurica238814 | 0:12899fa39f88 | 150 | *(light_data+0) = light2byte.b[0]; |
jurica238814 | 0:12899fa39f88 | 151 | *(light_data+1) = light2byte.b[1]; |
jurica238814 | 0:12899fa39f88 | 152 | *(light_data+2) = light2byte.b[2]; |
jurica238814 | 0:12899fa39f88 | 153 | *(light_data+3) = light2byte.b[3]; |
jurica238814 | 0:12899fa39f88 | 154 | } |
jurica238814 | 0:12899fa39f88 | 155 | |
jurica238814 | 0:12899fa39f88 | 156 | float voltage2temp(float vOut){ |
jurica238814 | 0:12899fa39f88 | 157 | return ((float)vOut - (float)V0)/((float)TC); |
jurica238814 | 0:12899fa39f88 | 158 | } |
jurica238814 | 0:12899fa39f88 | 159 | |
jurica238814 | 0:12899fa39f88 | 160 | void getTemperature(uint8_t *temperature_data){ |
jurica238814 | 0:12899fa39f88 | 161 | temp2byte.f = voltage2temp(temperature.read()*(float)VCC); |
jurica238814 | 0:12899fa39f88 | 162 | *(temperature_data+0) = temp2byte.b[0]; |
jurica238814 | 0:12899fa39f88 | 163 | *(temperature_data+1) = temp2byte.b[1]; |
jurica238814 | 0:12899fa39f88 | 164 | *(temperature_data+2) = temp2byte.b[2]; |
jurica238814 | 0:12899fa39f88 | 165 | *(temperature_data+3) = temp2byte.b[3]; |
jurica238814 | 0:12899fa39f88 | 166 | } |
jurica238814 | 0:12899fa39f88 | 167 | |
jurica238814 | 0:12899fa39f88 | 168 | void getHumidity(uint8_t *hum_data){ |
jurica238814 | 0:12899fa39f88 | 169 | //float temp; |
jurica238814 | 0:12899fa39f88 | 170 | float humid; |
jurica238814 | 0:12899fa39f88 | 171 | si->getHumidity(&humid); |
jurica238814 | 0:12899fa39f88 | 172 | //si.getTemperature(&temp); |
jurica238814 | 0:12899fa39f88 | 173 | hum2byte.f = humid; |
jurica238814 | 0:12899fa39f88 | 174 | *(hum_data+0) = hum2byte.b[0]; |
jurica238814 | 0:12899fa39f88 | 175 | *(hum_data+1) = hum2byte.b[1]; |
jurica238814 | 0:12899fa39f88 | 176 | *(hum_data+2) = hum2byte.b[2]; |
jurica238814 | 0:12899fa39f88 | 177 | *(hum_data+3) = hum2byte.b[3]; |
jurica238814 | 0:12899fa39f88 | 178 | } |
jurica238814 | 0:12899fa39f88 | 179 | |
jurica238814 | 0:12899fa39f88 | 180 | void readGyro(int8_t *gyro_data){ |
jurica238814 | 0:12899fa39f88 | 181 | mems->readGyro(memsBuffer); |
jurica238814 | 0:12899fa39f88 | 182 | // Use init data values |
jurica238814 | 0:12899fa39f88 | 183 | memsBuffer[0] -= memsGyroInit[0]; |
jurica238814 | 0:12899fa39f88 | 184 | memsBuffer[1] -= memsGyroInit[1]; |
jurica238814 | 0:12899fa39f88 | 185 | memsBuffer[2] -= memsGyroInit[2]; |
jurica238814 | 0:12899fa39f88 | 186 | gyro_data[0] = memsBuffer[0] & 0xFF; // X axis |
jurica238814 | 0:12899fa39f88 | 187 | gyro_data[1] = (memsBuffer[0] >> 8); // X axis |
jurica238814 | 0:12899fa39f88 | 188 | gyro_data[2] = memsBuffer[1] & 0xFF; // Y axis |
jurica238814 | 0:12899fa39f88 | 189 | gyro_data[3] = (memsBuffer[1] >> 8); // Y axis |
jurica238814 | 0:12899fa39f88 | 190 | gyro_data[4] = memsBuffer[2] & 0xFF; // Z axis |
jurica238814 | 0:12899fa39f88 | 191 | gyro_data[5] = (memsBuffer[2] >> 8); // Z axis |
jurica238814 | 0:12899fa39f88 | 192 | } |
jurica238814 | 0:12899fa39f88 | 193 | |
jurica238814 | 0:12899fa39f88 | 194 | void readMyAcc(int8_t *acc_data){ |
jurica238814 | 0:12899fa39f88 | 195 | mems->readAcc(memsBuffer); |
jurica238814 | 0:12899fa39f88 | 196 | // Use init data values |
jurica238814 | 0:12899fa39f88 | 197 | memsBuffer[0] -= memsAccInit[0]; |
jurica238814 | 0:12899fa39f88 | 198 | memsBuffer[1] -= memsAccInit[1]; |
jurica238814 | 0:12899fa39f88 | 199 | memsBuffer[2] -= memsAccInit[2]; |
jurica238814 | 0:12899fa39f88 | 200 | acc_data[0] = memsBuffer[0] & 0xFF; // X axis |
jurica238814 | 0:12899fa39f88 | 201 | acc_data[1] = (memsBuffer[0] >> 8); // X axis |
jurica238814 | 0:12899fa39f88 | 202 | acc_data[2] = memsBuffer[1] & 0xFF; // Y axis |
jurica238814 | 0:12899fa39f88 | 203 | acc_data[3] = (memsBuffer[1] >> 8); // Y axis |
jurica238814 | 0:12899fa39f88 | 204 | acc_data[4] = memsBuffer[2] & 0xFF; // Z axis |
jurica238814 | 0:12899fa39f88 | 205 | acc_data[5] = (memsBuffer[2] >> 8); // Z axis |
jurica238814 | 0:12899fa39f88 | 206 | } |
jurica238814 | 0:12899fa39f88 | 207 | |
jurica238814 | 0:12899fa39f88 | 208 | void readMagneto(int8_t *magneto_data){ |
jurica238814 | 0:12899fa39f88 | 209 | mems->readMag(memsBuffer); |
jurica238814 | 0:12899fa39f88 | 210 | // Use init data values |
jurica238814 | 0:12899fa39f88 | 211 | memsBuffer[0] -= memsMagInit[0]; |
jurica238814 | 0:12899fa39f88 | 212 | memsBuffer[1] -= memsMagInit[1]; |
jurica238814 | 0:12899fa39f88 | 213 | memsBuffer[2] -= memsMagInit[2]; |
jurica238814 | 0:12899fa39f88 | 214 | magneto_data[0] = memsBuffer[0] * 0xFF; // X axis |
jurica238814 | 0:12899fa39f88 | 215 | magneto_data[1] = (memsBuffer[0] >> 8); // X axis |
jurica238814 | 0:12899fa39f88 | 216 | magneto_data[2] = memsBuffer[1] * 0xFF; // X axis |
jurica238814 | 0:12899fa39f88 | 217 | magneto_data[3] = (memsBuffer[1] >> 8); // X axis |
jurica238814 | 0:12899fa39f88 | 218 | magneto_data[4] = memsBuffer[2] * 0xFF; // X axis |
jurica238814 | 0:12899fa39f88 | 219 | magneto_data[5] = (memsBuffer[2] >> 8); // X axis |
jurica238814 | 0:12899fa39f88 | 220 | } |
jurica238814 | 0:12899fa39f88 | 221 | |
jurica238814 | 0:12899fa39f88 | 222 | void calibrateAcc(){ |
jurica238814 | 0:12899fa39f88 | 223 | uint8_t counter=0; |
jurica238814 | 0:12899fa39f88 | 224 | int8_t acc_data[6]; |
jurica238814 | 0:12899fa39f88 | 225 | for(counter=0; counter<20; counter++){ |
jurica238814 | 0:12899fa39f88 | 226 | readMyAcc(acc_data); |
jurica238814 | 0:12899fa39f88 | 227 | memsAccInit[0] += acc_data[0]; |
jurica238814 | 0:12899fa39f88 | 228 | memsAccInit[1] += acc_data[1]; |
jurica238814 | 0:12899fa39f88 | 229 | memsAccInit[2] += acc_data[2]; |
jurica238814 | 0:12899fa39f88 | 230 | } |
jurica238814 | 0:12899fa39f88 | 231 | memsAccInit[0] /= counter; |
jurica238814 | 0:12899fa39f88 | 232 | memsAccInit[1] /= counter; |
jurica238814 | 0:12899fa39f88 | 233 | memsAccInit[2] /= counter; |
jurica238814 | 0:12899fa39f88 | 234 | } |
jurica238814 | 0:12899fa39f88 | 235 | |
jurica238814 | 0:12899fa39f88 | 236 | void calibrateGyro(){ |
jurica238814 | 0:12899fa39f88 | 237 | uint8_t counter; |
jurica238814 | 0:12899fa39f88 | 238 | int8_t gyro_data[6]; |
jurica238814 | 0:12899fa39f88 | 239 | for(counter=0; counter<20; counter++){ |
jurica238814 | 0:12899fa39f88 | 240 | readMyAcc(gyro_data); |
jurica238814 | 0:12899fa39f88 | 241 | memsGyroInit[0] += gyro_data[0]; |
jurica238814 | 0:12899fa39f88 | 242 | memsGyroInit[1] += gyro_data[1]; |
jurica238814 | 0:12899fa39f88 | 243 | memsGyroInit[2] += gyro_data[2]; |
jurica238814 | 0:12899fa39f88 | 244 | } |
jurica238814 | 0:12899fa39f88 | 245 | memsGyroInit[0] /= counter; |
jurica238814 | 0:12899fa39f88 | 246 | memsGyroInit[1] /= counter; |
jurica238814 | 0:12899fa39f88 | 247 | memsGyroInit[2] /= counter; |
jurica238814 | 0:12899fa39f88 | 248 | } |
jurica238814 | 0:12899fa39f88 | 249 | |
jurica238814 | 0:12899fa39f88 | 250 | void calibrateMag(){ |
jurica238814 | 0:12899fa39f88 | 251 | uint8_t counter=0; |
jurica238814 | 0:12899fa39f88 | 252 | int8_t mag_data[6]; |
jurica238814 | 0:12899fa39f88 | 253 | for(counter=0; counter<20; counter++){ |
jurica238814 | 0:12899fa39f88 | 254 | readMyAcc(mag_data); |
jurica238814 | 0:12899fa39f88 | 255 | memsMagInit[0] += mag_data[0]; |
jurica238814 | 0:12899fa39f88 | 256 | memsMagInit[1] += mag_data[1]; |
jurica238814 | 0:12899fa39f88 | 257 | memsMagInit[2] += mag_data[2]; |
jurica238814 | 0:12899fa39f88 | 258 | } |
jurica238814 | 0:12899fa39f88 | 259 | memsMagInit[0] /= counter; |
jurica238814 | 0:12899fa39f88 | 260 | memsMagInit[1] /= counter; |
jurica238814 | 0:12899fa39f88 | 261 | memsMagInit[2] /= counter; |
jurica238814 | 0:12899fa39f88 | 262 | } |
jurica238814 | 0:12899fa39f88 | 263 | |
jurica238814 | 0:12899fa39f88 | 264 | |
jurica238814 | 0:12899fa39f88 | 265 | float get_a0(uint16_t a0){ |
jurica238814 | 0:12899fa39f88 | 266 | float temp_a0; |
jurica238814 | 0:12899fa39f88 | 267 | float temp_frac; |
jurica238814 | 0:12899fa39f88 | 268 | int temp_a0_int; |
jurica238814 | 0:12899fa39f88 | 269 | uint8_t negative_flag = 0; |
jurica238814 | 0:12899fa39f88 | 270 | |
jurica238814 | 0:12899fa39f88 | 271 | if(a0& 0x8000){ |
jurica238814 | 0:12899fa39f88 | 272 | a0 ^= 0xFFFF; // Transform from 2's complement |
jurica238814 | 0:12899fa39f88 | 273 | a0 -= 1; |
jurica238814 | 0:12899fa39f88 | 274 | negative_flag = 1; |
jurica238814 | 0:12899fa39f88 | 275 | } |
jurica238814 | 0:12899fa39f88 | 276 | |
jurica238814 | 0:12899fa39f88 | 277 | temp_a0_int = a0 & 0x7FF8; |
jurica238814 | 0:12899fa39f88 | 278 | temp_a0_int = temp_a0_int >> a0_frac_bits; |
jurica238814 | 0:12899fa39f88 | 279 | |
jurica238814 | 0:12899fa39f88 | 280 | temp_a0 = temp_a0_int * 1.0; // Int part |
jurica238814 | 0:12899fa39f88 | 281 | temp_frac = (1.0/(pow(2.0,3))); |
jurica238814 | 0:12899fa39f88 | 282 | temp_frac *= (a0 & a0_frac_mask); |
jurica238814 | 0:12899fa39f88 | 283 | temp_a0 = temp_a0 + temp_frac; |
jurica238814 | 0:12899fa39f88 | 284 | |
jurica238814 | 0:12899fa39f88 | 285 | if(negative_flag) temp_a0 = -temp_a0; |
jurica238814 | 0:12899fa39f88 | 286 | |
jurica238814 | 0:12899fa39f88 | 287 | return temp_a0; |
jurica238814 | 0:12899fa39f88 | 288 | } |
jurica238814 | 0:12899fa39f88 | 289 | |
jurica238814 | 0:12899fa39f88 | 290 | float get_b1(uint16_t b1){ |
jurica238814 | 0:12899fa39f88 | 291 | float temp_b1; |
jurica238814 | 0:12899fa39f88 | 292 | float temp_frac; |
jurica238814 | 0:12899fa39f88 | 293 | int temp_b1_int; |
jurica238814 | 0:12899fa39f88 | 294 | uint8_t negative_flag = 0; |
jurica238814 | 0:12899fa39f88 | 295 | |
jurica238814 | 0:12899fa39f88 | 296 | if (b1 & 0x8000){ |
jurica238814 | 0:12899fa39f88 | 297 | b1 ^= 0xFFFF; |
jurica238814 | 0:12899fa39f88 | 298 | b1 -= 1; |
jurica238814 | 0:12899fa39f88 | 299 | negative_flag = 1; |
jurica238814 | 0:12899fa39f88 | 300 | } |
jurica238814 | 0:12899fa39f88 | 301 | |
jurica238814 | 0:12899fa39f88 | 302 | temp_b1_int = b1 & 0x6000; |
jurica238814 | 0:12899fa39f88 | 303 | temp_b1_int = temp_b1_int >> b1_frac_bits; |
jurica238814 | 0:12899fa39f88 | 304 | |
jurica238814 | 0:12899fa39f88 | 305 | temp_b1 = temp_b1_int * 1.0; |
jurica238814 | 0:12899fa39f88 | 306 | temp_frac = (b1 & b1_frac_mask) * (1.0/(pow(2.0,b1_frac_bits))); |
jurica238814 | 0:12899fa39f88 | 307 | temp_b1 = temp_b1 + temp_frac; |
jurica238814 | 0:12899fa39f88 | 308 | |
jurica238814 | 0:12899fa39f88 | 309 | if (negative_flag) temp_b1 = -temp_b1; |
jurica238814 | 0:12899fa39f88 | 310 | |
jurica238814 | 0:12899fa39f88 | 311 | return temp_b1; |
jurica238814 | 0:12899fa39f88 | 312 | } |
jurica238814 | 0:12899fa39f88 | 313 | |
jurica238814 | 0:12899fa39f88 | 314 | float get_b2(uint16_t b2){ |
jurica238814 | 0:12899fa39f88 | 315 | float temp_b2; |
jurica238814 | 0:12899fa39f88 | 316 | float temp_frac; |
jurica238814 | 0:12899fa39f88 | 317 | int temp_b2_int; |
jurica238814 | 0:12899fa39f88 | 318 | uint8_t negative_flag = 0; |
jurica238814 | 0:12899fa39f88 | 319 | |
jurica238814 | 0:12899fa39f88 | 320 | if (b2 & 0x8000){ |
jurica238814 | 0:12899fa39f88 | 321 | b2 ^= 0xFFFF; |
jurica238814 | 0:12899fa39f88 | 322 | b2 -= 1; |
jurica238814 | 0:12899fa39f88 | 323 | negative_flag = 1; |
jurica238814 | 0:12899fa39f88 | 324 | } |
jurica238814 | 0:12899fa39f88 | 325 | |
jurica238814 | 0:12899fa39f88 | 326 | temp_b2_int = b2 & 0x4000; |
jurica238814 | 0:12899fa39f88 | 327 | temp_b2_int = temp_b2_int >> b2_frac_bits; |
jurica238814 | 0:12899fa39f88 | 328 | |
jurica238814 | 0:12899fa39f88 | 329 | temp_b2 = temp_b2_int * 1.0; |
jurica238814 | 0:12899fa39f88 | 330 | temp_frac = (b2 & b2_frac_mask) * (1.0/(pow(2.0,b2_frac_bits))); |
jurica238814 | 0:12899fa39f88 | 331 | temp_b2 = temp_b2 + temp_frac; |
jurica238814 | 0:12899fa39f88 | 332 | |
jurica238814 | 0:12899fa39f88 | 333 | if (negative_flag) temp_b2 = -temp_b2; |
jurica238814 | 0:12899fa39f88 | 334 | |
jurica238814 | 0:12899fa39f88 | 335 | return temp_b2; |
jurica238814 | 0:12899fa39f88 | 336 | } |
jurica238814 | 0:12899fa39f88 | 337 | |
jurica238814 | 0:12899fa39f88 | 338 | |
jurica238814 | 0:12899fa39f88 | 339 | float get_c12(uint16_t c12){ |
jurica238814 | 0:12899fa39f88 | 340 | float temp_c12; |
jurica238814 | 0:12899fa39f88 | 341 | float temp_frac; |
jurica238814 | 0:12899fa39f88 | 342 | uint8_t negative_flag = 0; |
jurica238814 | 0:12899fa39f88 | 343 | |
jurica238814 | 0:12899fa39f88 | 344 | c12 = c12 >> 2; |
jurica238814 | 0:12899fa39f88 | 345 | |
jurica238814 | 0:12899fa39f88 | 346 | if (c12 & 0x2000){ |
jurica238814 | 0:12899fa39f88 | 347 | c12 ^= 0xFFFF; |
jurica238814 | 0:12899fa39f88 | 348 | c12 -= 1; |
jurica238814 | 0:12899fa39f88 | 349 | negative_flag = 1; |
jurica238814 | 0:12899fa39f88 | 350 | } |
jurica238814 | 0:12899fa39f88 | 351 | |
jurica238814 | 0:12899fa39f88 | 352 | temp_c12 = 0.000000000; |
jurica238814 | 0:12899fa39f88 | 353 | temp_frac = (c12 & c12_frac_mask) * (1.0/(pow(2.0,c12_frac_bits))); |
jurica238814 | 0:12899fa39f88 | 354 | temp_c12 = temp_c12 + temp_frac; |
jurica238814 | 0:12899fa39f88 | 355 | |
jurica238814 | 0:12899fa39f88 | 356 | if (negative_flag) temp_c12 = -temp_c12; |
jurica238814 | 0:12899fa39f88 | 357 | |
jurica238814 | 0:12899fa39f88 | 358 | return temp_c12; |
jurica238814 | 0:12899fa39f88 | 359 | } |
jurica238814 | 0:12899fa39f88 | 360 | |
jurica238814 | 0:12899fa39f88 | 361 | void get_pressure(uint8_t *pressure_ret){ |
jurica238814 | 0:12899fa39f88 | 362 | float pcomp = 0x00; |
jurica238814 | 0:12899fa39f88 | 363 | volatile float pressure; |
jurica238814 | 0:12899fa39f88 | 364 | |
jurica238814 | 0:12899fa39f88 | 365 | uint16_t a0 = 0; |
jurica238814 | 0:12899fa39f88 | 366 | uint16_t b1 = 0; |
jurica238814 | 0:12899fa39f88 | 367 | uint16_t b2 = 0; |
jurica238814 | 0:12899fa39f88 | 368 | uint16_t c12 = 0; |
jurica238814 | 0:12899fa39f88 | 369 | uint16_t padc = 0; |
jurica238814 | 0:12899fa39f88 | 370 | uint16_t tadc = 0; |
jurica238814 | 0:12899fa39f88 | 371 | |
jurica238814 | 0:12899fa39f88 | 372 | float a0_f, b1_f, b2_f, c12_f; |
jurica238814 | 0:12899fa39f88 | 373 | cs = 0; |
jurica238814 | 0:12899fa39f88 | 374 | spi.write(0x88); // MSB a0 |
jurica238814 | 0:12899fa39f88 | 375 | a0 = spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 376 | a0 = a0 << 8; |
jurica238814 | 0:12899fa39f88 | 377 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 378 | spi.write(0x8A); // LSB a0 |
jurica238814 | 0:12899fa39f88 | 379 | a0 |= spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 380 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 381 | |
jurica238814 | 0:12899fa39f88 | 382 | spi.write(0x8C); // MSB b1 |
jurica238814 | 0:12899fa39f88 | 383 | b1 = spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 384 | b1 = b1 << 8; |
jurica238814 | 0:12899fa39f88 | 385 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 386 | spi.write(0x8E); // LSB b1 |
jurica238814 | 0:12899fa39f88 | 387 | b1 |= spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 388 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 389 | |
jurica238814 | 0:12899fa39f88 | 390 | spi.write(0x90); // MSB b2 |
jurica238814 | 0:12899fa39f88 | 391 | b2 = spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 392 | b2 = b2 << 8; |
jurica238814 | 0:12899fa39f88 | 393 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 394 | spi.write(0x92); // LSB b2 |
jurica238814 | 0:12899fa39f88 | 395 | b2 |= spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 396 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 397 | |
jurica238814 | 0:12899fa39f88 | 398 | spi.write(0x94); // MSB c12 |
jurica238814 | 0:12899fa39f88 | 399 | c12 = spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 400 | c12 = c12 << 8; |
jurica238814 | 0:12899fa39f88 | 401 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 402 | spi.write(0x96); // LSB c12 |
jurica238814 | 0:12899fa39f88 | 403 | c12 |= spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 404 | wait_ms(1); |
jurica238814 | 0:12899fa39f88 | 405 | spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 406 | cs = 1; |
jurica238814 | 0:12899fa39f88 | 407 | |
jurica238814 | 0:12899fa39f88 | 408 | cs = 0; |
jurica238814 | 0:12899fa39f88 | 409 | spi.write(0x24); // Start conversion |
jurica238814 | 0:12899fa39f88 | 410 | spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 411 | cs = 1; |
jurica238814 | 0:12899fa39f88 | 412 | wait_ms(3); |
jurica238814 | 0:12899fa39f88 | 413 | |
jurica238814 | 0:12899fa39f88 | 414 | cs = 0; |
jurica238814 | 0:12899fa39f88 | 415 | spi.write(0x80); |
jurica238814 | 0:12899fa39f88 | 416 | padc = spi.write(0x00); // MSB Padc |
jurica238814 | 0:12899fa39f88 | 417 | padc = padc << 8; |
jurica238814 | 0:12899fa39f88 | 418 | spi.write(0x82); |
jurica238814 | 0:12899fa39f88 | 419 | padc |= spi.write(0x00); // LSB Padc |
jurica238814 | 0:12899fa39f88 | 420 | padc = padc >> 6; |
jurica238814 | 0:12899fa39f88 | 421 | |
jurica238814 | 0:12899fa39f88 | 422 | spi.write(0x84); |
jurica238814 | 0:12899fa39f88 | 423 | tadc = spi.write(0x00); // MSB Padc |
jurica238814 | 0:12899fa39f88 | 424 | tadc = tadc << 8; |
jurica238814 | 0:12899fa39f88 | 425 | spi.write(0x86); |
jurica238814 | 0:12899fa39f88 | 426 | tadc |= spi.write(0x00); // LSB Padc |
jurica238814 | 0:12899fa39f88 | 427 | tadc = tadc >> 6; |
jurica238814 | 0:12899fa39f88 | 428 | |
jurica238814 | 0:12899fa39f88 | 429 | spi.write(0x00); |
jurica238814 | 0:12899fa39f88 | 430 | cs = 1; |
jurica238814 | 0:12899fa39f88 | 431 | |
jurica238814 | 0:12899fa39f88 | 432 | a0_f = get_a0(a0); |
jurica238814 | 0:12899fa39f88 | 433 | b1_f = get_b1(b1); |
jurica238814 | 0:12899fa39f88 | 434 | b2_f = get_b2(b2); |
jurica238814 | 0:12899fa39f88 | 435 | c12_f = get_c12(c12); |
jurica238814 | 0:12899fa39f88 | 436 | /* |
jurica238814 | 0:12899fa39f88 | 437 | float c12x2 = c12_f * tadc; |
jurica238814 | 0:12899fa39f88 | 438 | float a1_f = b1_f + c12x2; |
jurica238814 | 0:12899fa39f88 | 439 | float a1x1 = a1_f * padc; |
jurica238814 | 0:12899fa39f88 | 440 | float y1 = a0_f + a1x1; |
jurica238814 | 0:12899fa39f88 | 441 | float a2x2 = b2_f * tadc; |
jurica238814 | 0:12899fa39f88 | 442 | pcomp = y1 + a2x2; |
jurica238814 | 0:12899fa39f88 | 443 | */ |
jurica238814 | 0:12899fa39f88 | 444 | |
jurica238814 | 0:12899fa39f88 | 445 | pcomp = a0_f + (b1_f + c12_f * tadc) * padc + b2_f * tadc; |
jurica238814 | 0:12899fa39f88 | 446 | pressure = pcomp * ((115-50)/(1023.0)) + 52; // Was + 50 |
jurica238814 | 0:12899fa39f88 | 447 | pressure *= 10; // Calculate in hPa |
jurica238814 | 0:12899fa39f88 | 448 | pressure2byte.f = pressure; |
jurica238814 | 0:12899fa39f88 | 449 | *(pressure_ret+0) = pressure2byte.b[0]; |
jurica238814 | 0:12899fa39f88 | 450 | *(pressure_ret+1) = pressure2byte.b[1]; |
jurica238814 | 0:12899fa39f88 | 451 | *(pressure_ret+2) = pressure2byte.b[2]; |
jurica238814 | 0:12899fa39f88 | 452 | *(pressure_ret+3) = pressure2byte.b[3]; |
jurica238814 | 0:12899fa39f88 | 453 | } |
jurica238814 | 0:12899fa39f88 | 454 | |
jurica238814 | 0:12899fa39f88 | 455 | void updateData(){ |
jurica238814 | 0:12899fa39f88 | 456 | uint8_t temperature_data[4]; |
jurica238814 | 0:12899fa39f88 | 457 | uint8_t light_data[4]; |
jurica238814 | 0:12899fa39f88 | 458 | int8_t gyro_data[6]; |
jurica238814 | 0:12899fa39f88 | 459 | int8_t acc_data[6]; |
jurica238814 | 0:12899fa39f88 | 460 | int8_t magneto_data[6]; |
jurica238814 | 0:12899fa39f88 | 461 | uint8_t hum_data[4]; |
jurica238814 | 0:12899fa39f88 | 462 | uint8_t pressure_data[4]; |
jurica238814 | 0:12899fa39f88 | 463 | static uint8_t adv_type = 0; |
jurica238814 | 0:12899fa39f88 | 464 | |
jurica238814 | 0:12899fa39f88 | 465 | if(adv_type < 1){ |
jurica238814 | 0:12899fa39f88 | 466 | |
jurica238814 | 0:12899fa39f88 | 467 | // Set adv type |
jurica238814 | 0:12899fa39f88 | 468 | MSD[ADV_TYPE] = 0x00; |
jurica238814 | 0:12899fa39f88 | 469 | // Read gyro |
jurica238814 | 0:12899fa39f88 | 470 | readGyro(gyro_data); |
jurica238814 | 0:12899fa39f88 | 471 | MSD[GYRO+0] = *(gyro_data+0); // X axis |
jurica238814 | 0:12899fa39f88 | 472 | MSD[GYRO+1] = *(gyro_data+1); // X axis |
jurica238814 | 0:12899fa39f88 | 473 | MSD[GYRO+2] = *(gyro_data+2); // Y axis |
jurica238814 | 0:12899fa39f88 | 474 | MSD[GYRO+3] = *(gyro_data+3); // Y axis |
jurica238814 | 0:12899fa39f88 | 475 | MSD[GYRO+4] = *(gyro_data+4); // Z axis |
jurica238814 | 0:12899fa39f88 | 476 | MSD[GYRO+5] = *(gyro_data+5); // Z axis |
jurica238814 | 0:12899fa39f88 | 477 | |
jurica238814 | 0:12899fa39f88 | 478 | |
jurica238814 | 0:12899fa39f88 | 479 | // Read acc |
jurica238814 | 0:12899fa39f88 | 480 | readMyAcc(acc_data); |
jurica238814 | 0:12899fa39f88 | 481 | MSD[ACC+0] = *(acc_data+0); // X axis |
jurica238814 | 0:12899fa39f88 | 482 | MSD[ACC+1] = *(acc_data+1); // X axis |
jurica238814 | 0:12899fa39f88 | 483 | MSD[ACC+2] = *(acc_data+2); // Y axis |
jurica238814 | 0:12899fa39f88 | 484 | MSD[ACC+3] = *(acc_data+3); // Y axis |
jurica238814 | 0:12899fa39f88 | 485 | MSD[ACC+4] = *(acc_data+4); // Z axis |
jurica238814 | 0:12899fa39f88 | 486 | MSD[ACC+5] = *(acc_data+5); // Z axis |
jurica238814 | 0:12899fa39f88 | 487 | |
jurica238814 | 0:12899fa39f88 | 488 | // Read magneto |
jurica238814 | 0:12899fa39f88 | 489 | readMagneto(magneto_data); |
jurica238814 | 0:12899fa39f88 | 490 | //magneto_data[0] = 0x11; |
jurica238814 | 0:12899fa39f88 | 491 | //magneto_data[1] = 0x23; |
jurica238814 | 0:12899fa39f88 | 492 | MSD[MAGNETO+0] = *(magneto_data+0); // X axis |
jurica238814 | 0:12899fa39f88 | 493 | MSD[MAGNETO+1] = *(magneto_data+1); // X axis |
jurica238814 | 0:12899fa39f88 | 494 | MSD[MAGNETO+2] = *(magneto_data+2); // Y axis |
jurica238814 | 0:12899fa39f88 | 495 | MSD[MAGNETO+3] = *(magneto_data+3); // Y axis |
jurica238814 | 0:12899fa39f88 | 496 | MSD[MAGNETO+4] = *(magneto_data+4); // Z axis |
jurica238814 | 0:12899fa39f88 | 497 | MSD[MAGNETO+5] = *(magneto_data+5); // Z axis |
jurica238814 | 0:12899fa39f88 | 498 | |
jurica238814 | 0:12899fa39f88 | 499 | } |
jurica238814 | 0:12899fa39f88 | 500 | else{ |
jurica238814 | 0:12899fa39f88 | 501 | // Set adv type |
jurica238814 | 0:12899fa39f88 | 502 | MSD[ADV_TYPE] = 0x01; |
jurica238814 | 0:12899fa39f88 | 503 | // Read temperature |
jurica238814 | 0:12899fa39f88 | 504 | getTemperature(temperature_data); |
jurica238814 | 0:12899fa39f88 | 505 | MSD[TEMPERATURE+0] = *(temperature_data+0); |
jurica238814 | 0:12899fa39f88 | 506 | MSD[TEMPERATURE+1] = *(temperature_data+1); |
jurica238814 | 0:12899fa39f88 | 507 | MSD[TEMPERATURE+2] = *(temperature_data+2); |
jurica238814 | 0:12899fa39f88 | 508 | MSD[TEMPERATURE+3] = *(temperature_data+3); |
jurica238814 | 0:12899fa39f88 | 509 | |
jurica238814 | 0:12899fa39f88 | 510 | // Read light |
jurica238814 | 0:12899fa39f88 | 511 | getLight(light_data); |
jurica238814 | 0:12899fa39f88 | 512 | MSD[LIGHT+0] = *(light_data+0); |
jurica238814 | 0:12899fa39f88 | 513 | MSD[LIGHT+1] = *(light_data+1); |
jurica238814 | 0:12899fa39f88 | 514 | MSD[LIGHT+2] = *(light_data+2); |
jurica238814 | 0:12899fa39f88 | 515 | MSD[LIGHT+3] = *(light_data+3); |
jurica238814 | 0:12899fa39f88 | 516 | |
jurica238814 | 0:12899fa39f88 | 517 | // Read Pressure |
jurica238814 | 0:12899fa39f88 | 518 | get_pressure(pressure_data); |
jurica238814 | 0:12899fa39f88 | 519 | MSD[PRESSURE+0] = *(pressure_data+0); |
jurica238814 | 0:12899fa39f88 | 520 | MSD[PRESSURE+1] = *(pressure_data+1); |
jurica238814 | 0:12899fa39f88 | 521 | MSD[PRESSURE+2] = *(pressure_data+2); |
jurica238814 | 0:12899fa39f88 | 522 | MSD[PRESSURE+3] = *(pressure_data+3); |
jurica238814 | 0:12899fa39f88 | 523 | |
jurica238814 | 0:12899fa39f88 | 524 | // Read Humidity |
jurica238814 | 0:12899fa39f88 | 525 | getHumidity(hum_data); |
jurica238814 | 0:12899fa39f88 | 526 | MSD[HUMIDITY+0] = *(hum_data+0); |
jurica238814 | 0:12899fa39f88 | 527 | MSD[HUMIDITY+1] = *(hum_data+1); |
jurica238814 | 0:12899fa39f88 | 528 | MSD[HUMIDITY+2] = *(hum_data+2); |
jurica238814 | 0:12899fa39f88 | 529 | MSD[HUMIDITY+3] = *(hum_data+3); |
jurica238814 | 0:12899fa39f88 | 530 | } |
jurica238814 | 0:12899fa39f88 | 531 | adv_type++; |
jurica238814 | 0:12899fa39f88 | 532 | if(adv_type>2) adv_type = 0; |
jurica238814 | 0:12899fa39f88 | 533 | |
jurica238814 | 0:12899fa39f88 | 534 | adv_data = ble.getAdvertisingData(); |
jurica238814 | 0:12899fa39f88 | 535 | adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, MSD, 25); |
jurica238814 | 0:12899fa39f88 | 536 | ble.setAdvertisingData(adv_data); |
jurica238814 | 0:12899fa39f88 | 537 | } |
jurica238814 | 0:12899fa39f88 | 538 | |
jurica238814 | 0:12899fa39f88 | 539 | |
jurica238814 | 0:12899fa39f88 | 540 | int main(void){ |
jurica238814 | 0:12899fa39f88 | 541 | |
jurica238814 | 0:12899fa39f88 | 542 | SEND("Main started.\r\n"); |
jurica238814 | 0:12899fa39f88 | 543 | power = 1; |
jurica238814 | 0:12899fa39f88 | 544 | SEND("Main power regulator turned ON.\r\n"); |
jurica238814 | 0:12899fa39f88 | 545 | wait_ms(150); |
jurica238814 | 0:12899fa39f88 | 546 | temperaturePower = 1; |
jurica238814 | 0:12899fa39f88 | 547 | SEND("Temperature power turned ON.\r\n"); |
jurica238814 | 0:12899fa39f88 | 548 | lightPower = 1; |
jurica238814 | 0:12899fa39f88 | 549 | SEND("Light power turned ON.\r\n"); |
jurica238814 | 0:12899fa39f88 | 550 | shdn = 1; // Wake up the pressure sensor |
jurica238814 | 0:12899fa39f88 | 551 | SEND("Pressure sensor woken up.\r\n"); |
jurica238814 | 0:12899fa39f88 | 552 | |
jurica238814 | 0:12899fa39f88 | 553 | /* |
jurica238814 | 0:12899fa39f88 | 554 | while(1){ |
jurica238814 | 0:12899fa39f88 | 555 | led = !led; |
jurica238814 | 0:12899fa39f88 | 556 | wait_ms(500); |
jurica238814 | 0:12899fa39f88 | 557 | } |
jurica238814 | 0:12899fa39f88 | 558 | */ |
jurica238814 | 0:12899fa39f88 | 559 | |
jurica238814 | 0:12899fa39f88 | 560 | I2C i2c(p19, p20); |
jurica238814 | 0:12899fa39f88 | 561 | si = new Si7006(&i2c); |
jurica238814 | 0:12899fa39f88 | 562 | mems = new LSM9DS1(i2c); |
jurica238814 | 0:12899fa39f88 | 563 | |
jurica238814 | 0:12899fa39f88 | 564 | mems->startAcc(); |
jurica238814 | 0:12899fa39f88 | 565 | mems->startGyro(); |
jurica238814 | 0:12899fa39f88 | 566 | mems->startMag(); |
jurica238814 | 0:12899fa39f88 | 567 | |
jurica238814 | 0:12899fa39f88 | 568 | led = 1; |
jurica238814 | 0:12899fa39f88 | 569 | |
jurica238814 | 0:12899fa39f88 | 570 | Ticker ticker; |
jurica238814 | 0:12899fa39f88 | 571 | ticker.attach(wakeMeUp, SLEEP_TIME); // Wake the device up |
jurica238814 | 0:12899fa39f88 | 572 | |
jurica238814 | 0:12899fa39f88 | 573 | ble.init(bleInitComplete); |
jurica238814 | 0:12899fa39f88 | 574 | ble.gap().setTxPower(TX_POWER); // Set TX power to TX_POWER |
jurica238814 | 0:12899fa39f88 | 575 | |
jurica238814 | 0:12899fa39f88 | 576 | /* SpinWait for initialization to complete. This is necessary because the |
jurica238814 | 0:12899fa39f88 | 577 | * BLE object is used in the main loop below. */ |
jurica238814 | 0:12899fa39f88 | 578 | while (ble.hasInitialized() == false) { /* spin loop */ } |
jurica238814 | 0:12899fa39f88 | 579 | while (true){ |
jurica238814 | 0:12899fa39f88 | 580 | |
jurica238814 | 0:12899fa39f88 | 581 | if (SLEEP && GO_TO_SLEEP){ |
jurica238814 | 0:12899fa39f88 | 582 | ble.gap().stopAdvertising(); |
jurica238814 | 0:12899fa39f88 | 583 | sleep(); |
jurica238814 | 0:12899fa39f88 | 584 | ble.waitForEvent(); |
jurica238814 | 0:12899fa39f88 | 585 | } |
jurica238814 | 0:12899fa39f88 | 586 | else{ |
jurica238814 | 0:12899fa39f88 | 587 | // I'm awake |
jurica238814 | 0:12899fa39f88 | 588 | updateData(); |
jurica238814 | 0:12899fa39f88 | 589 | ble.gap().startAdvertising(); |
jurica238814 | 0:12899fa39f88 | 590 | wait_ms(WAKE_UP_TIME); |
jurica238814 | 0:12899fa39f88 | 591 | SLEEP = true; |
jurica238814 | 0:12899fa39f88 | 592 | } |
jurica238814 | 0:12899fa39f88 | 593 | } |
jurica238814 | 0:12899fa39f88 | 594 | |
jurica238814 | 0:12899fa39f88 | 595 | |
jurica238814 | 0:12899fa39f88 | 596 | |
jurica238814 | 0:12899fa39f88 | 597 | } |