Thomas Van Aken
/
bartho
test
main.cpp@4:40d23f4295f9, 2021-02-10 (annotated)
- Committer:
- TVA
- Date:
- Wed Feb 10 19:45:43 2021 +0000
- Revision:
- 4:40d23f4295f9
- Parent:
- 3:0e37515b92f1
Working test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TVA | 0:8491d8911089 | 1 | /* mbed Microcontroller Library |
TVA | 0:8491d8911089 | 2 | * Copyright (c) 2019 ARM Limited |
TVA | 0:8491d8911089 | 3 | * SPDX-License-Identifier: Apache-2.0 |
TVA | 0:8491d8911089 | 4 | */ |
TVA | 0:8491d8911089 | 5 | |
TVA | 0:8491d8911089 | 6 | #include "mbed.h" |
TVA | 0:8491d8911089 | 7 | #include "platform/mbed_thread.h" |
TVA | 0:8491d8911089 | 8 | |
TVA | 4:40d23f4295f9 | 9 | // populate from databuffer queue raw samples data before sending over ble |
TVA | 4:40d23f4295f9 | 10 | // struct RawData samples_raw_data; |
TVA | 4:40d23f4295f9 | 11 | // int sampleIndex = 0; |
TVA | 4:40d23f4295f9 | 12 | // int ptr = 0; |
TVA | 4:40d23f4295f9 | 13 | // while(sampleIndex < mumberOffSamples) |
TVA | 4:40d23f4295f9 | 14 | // status byte is index 0 |
TVA | 4:40d23f4295f9 | 15 | // samples_raw_data.green_led_cnt = (databuf[ptr+1] << 16) + (databuf[ptr+2] << 8) + (databuf[ptr+3]); |
TVA | 4:40d23f4295f9 | 16 | // samples_raw_data.ir_led_cnt = (databuf[ptr+4] << 16) + (databuf[ptr+5] << 8) + (databuf[ptr+6]); |
TVA | 4:40d23f4295f9 | 17 | // samples_raw_data.red_led_cnt = (databuf[ptr+7] << 16) + (databuf[ptr+8] << 8) + (databuf[ptr+9]); |
TVA | 4:40d23f4295f9 | 18 | // samples_raw_data.green2_led_cnt = (databuf[ptr+10] << 16) + (databuf[ptr+11] << 8) + (databuf[ptr12]); |
TVA | 4:40d23f4295f9 | 19 | // samples_raw_data.x = (databuf[ptr+19] << 8) + (databuf[ptr+20]); |
TVA | 4:40d23f4295f9 | 20 | // samples_raw_data.y = (databuf[ptr+21] << 8) + (databuf[ptr+22]); |
TVA | 4:40d23f4295f9 | 21 | // samples_raw_data.z = (databuf[ptr+23] << 8) + (databuf[ptr+24]); |
TVA | 4:40d23f4295f9 | 22 | struct RawData |
TVA | 4:40d23f4295f9 | 23 | { |
TVA | 4:40d23f4295f9 | 24 | uint32_t green_led_cnt; |
TVA | 4:40d23f4295f9 | 25 | uint32_t ir_led_cnt; |
TVA | 4:40d23f4295f9 | 26 | uint32_t red_led_cnt; |
TVA | 4:40d23f4295f9 | 27 | uint32_t green2_led_cnt; |
TVA | 4:40d23f4295f9 | 28 | int16_t x; |
TVA | 4:40d23f4295f9 | 29 | int16_t y; |
TVA | 4:40d23f4295f9 | 30 | int16_t z; |
TVA | 4:40d23f4295f9 | 31 | }; |
TVA | 0:8491d8911089 | 32 | |
TVA | 0:8491d8911089 | 33 | // Blinking rate in milliseconds |
TVA | 0:8491d8911089 | 34 | #define BLINKING_RATE_MS 500 |
TVA | 0:8491d8911089 | 35 | |
TVA | 0:8491d8911089 | 36 | #define RST_PIN P5_6 |
TVA | 0:8491d8911089 | 37 | #define MFIO_PIN P5_4 |
TVA | 0:8491d8911089 | 38 | |
TVA | 0:8491d8911089 | 39 | I2C i2c(P3_4, P3_5); |
TVA | 0:8491d8911089 | 40 | |
TVA | 2:8fd249f35ceb | 41 | const int addr = 0xAA;//0x55; |
TVA | 0:8491d8911089 | 42 | |
TVA | 0:8491d8911089 | 43 | int main() |
TVA | 0:8491d8911089 | 44 | { |
TVA | 0:8491d8911089 | 45 | i2c.frequency(400000); |
TVA | 0:8491d8911089 | 46 | char cmd[4]; |
TVA | 1:397813654863 | 47 | |
TVA | 0:8491d8911089 | 48 | // Initialise the digital pin LED1 as an output |
TVA | 0:8491d8911089 | 49 | DigitalOut led(LED1); |
TVA | 0:8491d8911089 | 50 | DigitalOut rst(RST_PIN); |
TVA | 0:8491d8911089 | 51 | DigitalOut mfio(MFIO_PIN); |
TVA | 0:8491d8911089 | 52 | |
TVA | 2:8fd249f35ceb | 53 | mfio = 0; |
TVA | 0:8491d8911089 | 54 | rst = 1; |
TVA | 0:8491d8911089 | 55 | thread_sleep_for(10); |
TVA | 0:8491d8911089 | 56 | rst = 0; |
TVA | 0:8491d8911089 | 57 | thread_sleep_for(10); |
TVA | 0:8491d8911089 | 58 | rst = 1; |
TVA | 2:8fd249f35ceb | 59 | //thread_sleep_for(1500); |
TVA | 2:8fd249f35ceb | 60 | thread_sleep_for(100); |
TVA | 2:8fd249f35ceb | 61 | |
TVA | 2:8fd249f35ceb | 62 | //scanI2C(); |
TVA | 0:8491d8911089 | 63 | |
TVA | 0:8491d8911089 | 64 | while (true) { |
TVA | 4:40d23f4295f9 | 65 | //Read out status (app or boot mode) |
TVA | 0:8491d8911089 | 66 | cmd[0] = 0x02; |
TVA | 0:8491d8911089 | 67 | cmd[1] = 0x00; |
TVA | 0:8491d8911089 | 68 | i2c.write(addr, cmd, 2); |
TVA | 0:8491d8911089 | 69 | thread_sleep_for(2); |
TVA | 0:8491d8911089 | 70 | |
TVA | 0:8491d8911089 | 71 | cmd[0] = 0x00; |
TVA | 0:8491d8911089 | 72 | cmd[1] = 0x00; |
TVA | 0:8491d8911089 | 73 | i2c.read(addr, cmd, 2); |
TVA | 0:8491d8911089 | 74 | printf("1: %x %x\n", cmd[0], cmd[1]); |
TVA | 4:40d23f4295f9 | 75 | |
TVA | 4:40d23f4295f9 | 76 | //Switch to APP mode |
TVA | 2:8fd249f35ceb | 77 | cmd[0] = 0x01; |
TVA | 2:8fd249f35ceb | 78 | cmd[1] = 0x00; |
TVA | 2:8fd249f35ceb | 79 | cmd[2] = 0x00; |
TVA | 3:0e37515b92f1 | 80 | i2c.write(addr, cmd, 3); |
TVA | 2:8fd249f35ceb | 81 | thread_sleep_for(1500); |
TVA | 2:8fd249f35ceb | 82 | |
TVA | 4:40d23f4295f9 | 83 | //Switch off MFIO |
TVA | 2:8fd249f35ceb | 84 | mfio = 0; |
TVA | 4:40d23f4295f9 | 85 | thread_sleep_for(1); //Wait 300us |
TVA | 2:8fd249f35ceb | 86 | |
TVA | 4:40d23f4295f9 | 87 | //Read out status (app or boot mode) |
TVA | 2:8fd249f35ceb | 88 | cmd[0] = 0x02; |
TVA | 2:8fd249f35ceb | 89 | cmd[1] = 0x00; |
TVA | 2:8fd249f35ceb | 90 | i2c.write(addr, cmd, 2); |
TVA | 2:8fd249f35ceb | 91 | thread_sleep_for(2); |
TVA | 2:8fd249f35ceb | 92 | |
TVA | 2:8fd249f35ceb | 93 | cmd[0] = 0x00; |
TVA | 2:8fd249f35ceb | 94 | cmd[1] = 0x00; |
TVA | 2:8fd249f35ceb | 95 | i2c.read(addr, cmd, 2); |
TVA | 2:8fd249f35ceb | 96 | printf("2: %x %x\n", cmd[0], cmd[1]); |
TVA | 2:8fd249f35ceb | 97 | |
TVA | 4:40d23f4295f9 | 98 | //Sensor hub firmware nummer opvragen |
TVA | 0:8491d8911089 | 99 | cmd[0] = 0xFF; |
TVA | 0:8491d8911089 | 100 | cmd[1] = 0x03; |
TVA | 0:8491d8911089 | 101 | i2c.write(addr, cmd, 2); |
TVA | 0:8491d8911089 | 102 | thread_sleep_for(2); |
TVA | 0:8491d8911089 | 103 | |
TVA | 0:8491d8911089 | 104 | cmd[0] = 0x00; |
TVA | 0:8491d8911089 | 105 | cmd[1] = 0x00; |
TVA | 0:8491d8911089 | 106 | cmd[2] = 0x00; |
TVA | 0:8491d8911089 | 107 | cmd[3] = 0x00; |
TVA | 0:8491d8911089 | 108 | i2c.read(addr, cmd, 4); |
TVA | 2:8fd249f35ceb | 109 | printf("3: %d %d %d %d\n", cmd[0], cmd[1], cmd[2], cmd[3]); |
TVA | 4:40d23f4295f9 | 110 | |
TVA | 4:40d23f4295f9 | 111 | //Raw data mode |
TVA | 3:0e37515b92f1 | 112 | cmd[0] = 0x10; |
TVA | 3:0e37515b92f1 | 113 | cmd[1] = 0x00; |
TVA | 3:0e37515b92f1 | 114 | cmd[2] = 0x03; |
TVA | 3:0e37515b92f1 | 115 | |
TVA | 3:0e37515b92f1 | 116 | i2c.write(addr, cmd, 3); |
TVA | 3:0e37515b92f1 | 117 | thread_sleep_for(2); |
TVA | 3:0e37515b92f1 | 118 | |
TVA | 3:0e37515b92f1 | 119 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 120 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 121 | printf("4: %x\n", cmd[0]); |
TVA | 3:0e37515b92f1 | 122 | |
TVA | 4:40d23f4295f9 | 123 | //Interrupt threshold |
TVA | 3:0e37515b92f1 | 124 | cmd[0] = 0x10; |
TVA | 3:0e37515b92f1 | 125 | cmd[1] = 0x01; |
TVA | 3:0e37515b92f1 | 126 | cmd[2] = 0x01; |
TVA | 3:0e37515b92f1 | 127 | |
TVA | 3:0e37515b92f1 | 128 | i2c.write(addr, cmd, 3); |
TVA | 3:0e37515b92f1 | 129 | thread_sleep_for(2); |
TVA | 3:0e37515b92f1 | 130 | |
TVA | 3:0e37515b92f1 | 131 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 132 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 133 | printf("5: %x\n", cmd[0]); |
TVA | 3:0e37515b92f1 | 134 | |
TVA | 4:40d23f4295f9 | 135 | //Set sample report period |
TVA | 3:0e37515b92f1 | 136 | cmd[0] = 0x10; |
TVA | 3:0e37515b92f1 | 137 | cmd[1] = 0x02; |
TVA | 3:0e37515b92f1 | 138 | cmd[2] = 0x01; |
TVA | 3:0e37515b92f1 | 139 | |
TVA | 3:0e37515b92f1 | 140 | i2c.write(addr, cmd, 3); |
TVA | 3:0e37515b92f1 | 141 | thread_sleep_for(2); |
TVA | 3:0e37515b92f1 | 142 | |
TVA | 3:0e37515b92f1 | 143 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 144 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 145 | printf("6: %x\n", cmd[0]); |
TVA | 3:0e37515b92f1 | 146 | |
TVA | 4:40d23f4295f9 | 147 | //Enable accelerometer |
TVA | 3:0e37515b92f1 | 148 | cmd[0] = 0x44; |
TVA | 3:0e37515b92f1 | 149 | cmd[1] = 0x04; |
TVA | 3:0e37515b92f1 | 150 | cmd[2] = 0x01; |
TVA | 4:40d23f4295f9 | 151 | cmd[3] = 0x00; //0x01 |
TVA | 3:0e37515b92f1 | 152 | |
TVA | 3:0e37515b92f1 | 153 | i2c.write(addr, cmd, 4); |
TVA | 3:0e37515b92f1 | 154 | thread_sleep_for(20); |
TVA | 3:0e37515b92f1 | 155 | |
TVA | 3:0e37515b92f1 | 156 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 157 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 158 | printf("7: %x\n", cmd[0]); |
TVA | 3:0e37515b92f1 | 159 | |
TVA | 4:40d23f4295f9 | 160 | //Read accelerometer WHO_AM_I register |
TVA | 3:0e37515b92f1 | 161 | cmd[0] = 0x41; |
TVA | 3:0e37515b92f1 | 162 | cmd[1] = 0x04; |
TVA | 3:0e37515b92f1 | 163 | cmd[2] = 0x0F; |
TVA | 3:0e37515b92f1 | 164 | |
TVA | 3:0e37515b92f1 | 165 | i2c.write(addr, cmd, 3); |
TVA | 3:0e37515b92f1 | 166 | thread_sleep_for(2); |
TVA | 3:0e37515b92f1 | 167 | |
TVA | 3:0e37515b92f1 | 168 | cmd[0] = 0x00; |
TVA | 3:0e37515b92f1 | 169 | cmd[1] = 0x00; |
TVA | 3:0e37515b92f1 | 170 | i2c.read(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 171 | printf("8: %x %x\n", cmd[0], cmd[1]); |
TVA | 4:40d23f4295f9 | 172 | |
TVA | 4:40d23f4295f9 | 173 | |
TVA | 4:40d23f4295f9 | 174 | |
TVA | 4:40d23f4295f9 | 175 | printf("Start\n"); |
TVA | 4:40d23f4295f9 | 176 | |
TVA | 4:40d23f4295f9 | 177 | |
TVA | 4:40d23f4295f9 | 178 | |
TVA | 4:40d23f4295f9 | 179 | //Enable AFE |
TVA | 4:40d23f4295f9 | 180 | cmd[0] = 0x44; |
TVA | 4:40d23f4295f9 | 181 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 182 | cmd[2] = 0x01; |
TVA | 4:40d23f4295f9 | 183 | //MAYBE 4 |
TVA | 4:40d23f4295f9 | 184 | |
TVA | 4:40d23f4295f9 | 185 | i2c.write(addr, cmd, 3); |
TVA | 4:40d23f4295f9 | 186 | thread_sleep_for(250); |
TVA | 4:40d23f4295f9 | 187 | |
TVA | 4:40d23f4295f9 | 188 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 189 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 190 | printf("9: %x\n", cmd[0]); |
TVA | 4:40d23f4295f9 | 191 | |
TVA | 4:40d23f4295f9 | 192 | //Set 100Hz read samples |
TVA | 4:40d23f4295f9 | 193 | cmd[0] = 0x40; |
TVA | 4:40d23f4295f9 | 194 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 195 | cmd[2] = 0x12; |
TVA | 4:40d23f4295f9 | 196 | cmd[3] = 0x18; |
TVA | 4:40d23f4295f9 | 197 | |
TVA | 4:40d23f4295f9 | 198 | i2c.write(addr, cmd, 4); |
TVA | 4:40d23f4295f9 | 199 | thread_sleep_for(2); |
TVA | 4:40d23f4295f9 | 200 | |
TVA | 4:40d23f4295f9 | 201 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 202 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 203 | printf("10: %x\n", cmd[0]); |
TVA | 4:40d23f4295f9 | 204 | |
TVA | 4:40d23f4295f9 | 205 | |
TVA | 4:40d23f4295f9 | 206 | |
TVA | 4:40d23f4295f9 | 207 | |
TVA | 4:40d23f4295f9 | 208 | //get data |
TVA | 4:40d23f4295f9 | 209 | struct RawData samples_raw_data; |
TVA | 4:40d23f4295f9 | 210 | int sampleIndex = 0; |
TVA | 4:40d23f4295f9 | 211 | char rawData[1024]; |
TVA | 4:40d23f4295f9 | 212 | uint8_t amount = 0; |
TVA | 4:40d23f4295f9 | 213 | memset(rawData,0,sizeof(rawData)); |
TVA | 4:40d23f4295f9 | 214 | for(uint8_t i=0; i<250; i++) |
TVA | 4:40d23f4295f9 | 215 | { |
TVA | 4:40d23f4295f9 | 216 | //Get status sensor hub |
TVA | 4:40d23f4295f9 | 217 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 218 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 219 | i2c.write(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 220 | thread_sleep_for(2); |
TVA | 4:40d23f4295f9 | 221 | |
TVA | 4:40d23f4295f9 | 222 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 223 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 224 | i2c.read(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 225 | printf("dat1: %x %x\n", cmd[0], cmd[1]); |
TVA | 4:40d23f4295f9 | 226 | |
TVA | 4:40d23f4295f9 | 227 | //Get number of samples in the fifo |
TVA | 4:40d23f4295f9 | 228 | cmd[0] = 0x12; |
TVA | 4:40d23f4295f9 | 229 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 230 | i2c.write(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 231 | thread_sleep_for(2); |
TVA | 4:40d23f4295f9 | 232 | |
TVA | 4:40d23f4295f9 | 233 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 234 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 235 | i2c.read(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 236 | amount = cmd[1]; |
TVA | 4:40d23f4295f9 | 237 | printf("dat2: %x %x\n", cmd[0], cmd[1]); |
TVA | 4:40d23f4295f9 | 238 | |
TVA | 4:40d23f4295f9 | 239 | //Get data from fifo |
TVA | 4:40d23f4295f9 | 240 | cmd[0] = 0x12; |
TVA | 4:40d23f4295f9 | 241 | cmd[1] = 0x01; |
TVA | 4:40d23f4295f9 | 242 | i2c.write(addr, cmd, 2); |
TVA | 4:40d23f4295f9 | 243 | thread_sleep_for(2); |
TVA | 4:40d23f4295f9 | 244 | |
TVA | 4:40d23f4295f9 | 245 | i2c.read(addr, rawData, 1+((36+6)*amount)); |
TVA | 4:40d23f4295f9 | 246 | |
TVA | 4:40d23f4295f9 | 247 | printf("Satus: %x\n", rawData[0]); |
TVA | 4:40d23f4295f9 | 248 | printf("Data: "); |
TVA | 4:40d23f4295f9 | 249 | for(uint32_t j=1; j<1+((36+6)*amount); j+=42) |
TVA | 4:40d23f4295f9 | 250 | { |
TVA | 4:40d23f4295f9 | 251 | //%zu |
TVA | 4:40d23f4295f9 | 252 | printf("%u, %d, %d, %d\n", (uint32_t)(0x00<<24 | rawData[j+1]<<16 | rawData[j+2]<<8 | rawData[j+3]), (rawData[j+37]<<8 | rawData[j+38]), (rawData[j+39]<<8 | rawData[j+40]), (rawData[j+41]<<8 | rawData[j+42])); |
TVA | 4:40d23f4295f9 | 253 | } |
TVA | 4:40d23f4295f9 | 254 | |
TVA | 4:40d23f4295f9 | 255 | //thread_sleep_for(200); |
TVA | 4:40d23f4295f9 | 256 | thread_sleep_for(1); |
TVA | 4:40d23f4295f9 | 257 | } |
TVA | 3:0e37515b92f1 | 258 | |
TVA | 3:0e37515b92f1 | 259 | |
TVA | 3:0e37515b92f1 | 260 | |
TVA | 3:0e37515b92f1 | 261 | |
TVA | 3:0e37515b92f1 | 262 | |
TVA | 4:40d23f4295f9 | 263 | //Disable AFE |
TVA | 4:40d23f4295f9 | 264 | cmd[0] = 0x44; |
TVA | 4:40d23f4295f9 | 265 | cmd[1] = 0x00; |
TVA | 4:40d23f4295f9 | 266 | cmd[2] = 0x00; |
TVA | 4:40d23f4295f9 | 267 | //MAYBE 4 |
TVA | 4:40d23f4295f9 | 268 | |
TVA | 4:40d23f4295f9 | 269 | i2c.write(addr, cmd, 3); |
TVA | 4:40d23f4295f9 | 270 | thread_sleep_for(250); |
TVA | 4:40d23f4295f9 | 271 | |
TVA | 4:40d23f4295f9 | 272 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 273 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 274 | printf("11: %x\n", cmd[0]); |
TVA | 4:40d23f4295f9 | 275 | |
TVA | 4:40d23f4295f9 | 276 | |
TVA | 4:40d23f4295f9 | 277 | |
TVA | 4:40d23f4295f9 | 278 | printf("Stop\n"); |
TVA | 4:40d23f4295f9 | 279 | |
TVA | 4:40d23f4295f9 | 280 | |
TVA | 4:40d23f4295f9 | 281 | |
TVA | 4:40d23f4295f9 | 282 | //Disable Accelerometer |
TVA | 4:40d23f4295f9 | 283 | cmd[0] = 0x44; |
TVA | 4:40d23f4295f9 | 284 | cmd[1] = 0x04; |
TVA | 4:40d23f4295f9 | 285 | cmd[2] = 0x00; |
TVA | 4:40d23f4295f9 | 286 | //MAYBE 4 |
TVA | 4:40d23f4295f9 | 287 | |
TVA | 4:40d23f4295f9 | 288 | i2c.write(addr, cmd, 3); |
TVA | 4:40d23f4295f9 | 289 | thread_sleep_for(20); |
TVA | 4:40d23f4295f9 | 290 | |
TVA | 4:40d23f4295f9 | 291 | cmd[0] = 0x00; |
TVA | 4:40d23f4295f9 | 292 | i2c.read(addr, cmd, 1); |
TVA | 4:40d23f4295f9 | 293 | printf("12: %x\n", cmd[0]); |
TVA | 0:8491d8911089 | 294 | |
TVA | 0:8491d8911089 | 295 | /*rst =! rst; |
TVA | 0:8491d8911089 | 296 | mfio =! mfio;*/ |
TVA | 0:8491d8911089 | 297 | thread_sleep_for(BLINKING_RATE_MS); |
TVA | 0:8491d8911089 | 298 | led = !led; |
TVA | 0:8491d8911089 | 299 | } |
TVA | 0:8491d8911089 | 300 | } |