HuMVe-1 / Mbed OS MioTest_ToF_mems1

Second demo with X-NUCLEO-53L1A1.

Dependencies:   X_NUCLEO_53L1A1 X_NUCLEO_IKS01A3

main.cpp@11:da3c8e9bf1f9, 2022-03-24 (annotated)

Committer:
loarri
Date:
Thu Mar 24 17:14:56 2022 +0000
Revision:
11:da3c8e9bf1f9
Parent:
6:054c18d427bf 
IKS01A3 + 53L1A1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
loarri 5:7f6149b9cbbe 1 /*-----------------------------------------------------------
loarri 5:7f6149b9cbbe 2
loarri 5:7f6149b9cbbe 3
johnAlexander 0:6b7696e7df5e 4 */
johnAlexander 0:6b7696e7df5e 5
johnAlexander 0:6b7696e7df5e 6 #include <stdio.h>
johnAlexander 0:6b7696e7df5e 7
johnAlexander 0:6b7696e7df5e 8 #include "mbed.h"
johnAlexander 0:6b7696e7df5e 9 #include "XNucleo53L1A1.h"
johnAlexander 3:d90f22c17d0c 10 #include "VL53L1X_I2C.h"
loarri 11:da3c8e9bf1f9 11 #include "rtos.h"
loarri 11:da3c8e9bf1f9 12 #include "XNucleoIKS01A3.h"
johnAlexander 0:6b7696e7df5e 13
loarri 11:da3c8e9bf1f9 14 #define VL53L1_I2C_SDA D5 //D14
loarri 11:da3c8e9bf1f9 15 #define VL53L1_I2C_SCL D7 //D15
johnAlexander 0:6b7696e7df5e 16
loarri 6:054c18d427bf 17 #define DISPLAY_CENTRE false
loarri 11:da3c8e9bf1f9 18 #define DISPLAY_LEFT false
loarri 6:054c18d427bf 19 #define DISPLAY_RIGHT false
loarri 6:054c18d427bf 20
loarri 11:da3c8e9bf1f9 21 // LED blink
loarri 11:da3c8e9bf1f9 22 #define LED_FREQUENCY 0.5 //frequenza di blink del led
loarri 11:da3c8e9bf1f9 23 #define LED_OFF led =0
loarri 11:da3c8e9bf1f9 24 #define LED_ON led =1
loarri 11:da3c8e9bf1f9 25 #define LED_DURATA 100 // durata del blink
loarri 11:da3c8e9bf1f9 26
loarri 11:da3c8e9bf1f9 27 // Time Of Flight
loarri 11:da3c8e9bf1f9 28 #define ToF_FREQUENCY 0.2 // frequenza di controllo della distanza
loarri 11:da3c8e9bf1f9 29 #define DISPLAY_LEFT false
loarri 11:da3c8e9bf1f9 30 #define DISPLAY_RIGHT false
loarri 11:da3c8e9bf1f9 31 #define ToF_SOGLIA_CRITICA 20 //mm sotto la quale scatta
loarri 11:da3c8e9bf1f9 32 #define ToF_ITERATIONS 50 //iterazioni per la calibrazione
loarri 11:da3c8e9bf1f9 33
loarri 11:da3c8e9bf1f9 34 // ToF
johnAlexander 0:6b7696e7df5e 35 static XNucleo53L1A1 *board=NULL;
johnAlexander 0:6b7696e7df5e 36
loarri 11:da3c8e9bf1f9 37 // mems
loarri 11:da3c8e9bf1f9 38 /* Instantiate the expansion board */
loarri 11:da3c8e9bf1f9 39 static XNucleoIKS01A3 *mems_expansion_board = XNucleoIKS01A3::instance(D14, D15, D4, D5, A3, D6, A4);
loarri 11:da3c8e9bf1f9 40 /* Retrieve the composing elements of the expansion board */
loarri 11:da3c8e9bf1f9 41 static LSM6DSOSensor *acc_gyro = mems_expansion_board->acc_gyro;
loarri 11:da3c8e9bf1f9 42 static LIS2DW12Sensor *accelerometer = mems_expansion_board->accelerometer;
johnAlexander 0:6b7696e7df5e 43
johnAlexander 0:6b7696e7df5e 44 int status = 0;
johnAlexander 0:6b7696e7df5e 45 uint8_t ready_centre = 0;
johnAlexander 0:6b7696e7df5e 46 uint8_t ready_left = 0;
johnAlexander 0:6b7696e7df5e 47 uint8_t ready_right = 0;
johnAlexander 0:6b7696e7df5e 48 uint16_t distance_centre = 0;
johnAlexander 0:6b7696e7df5e 49 uint16_t distance_left = 0;
johnAlexander 0:6b7696e7df5e 50 uint16_t distance_right = 0;
johnAlexander 0:6b7696e7df5e 51
loarri 11:da3c8e9bf1f9 52 DigitalOut led(LED1); // define the LED pin
loarri 11:da3c8e9bf1f9 53 Ticker myTick; // create a ticker object
loarri 11:da3c8e9bf1f9 54 Ticker Tick_ToF;
loarri 11:da3c8e9bf1f9 55 bool checktof =0;
loarri 11:da3c8e9bf1f9 56 bool stop_misura=0;
loarri 11:da3c8e9bf1f9 57
loarri 11:da3c8e9bf1f9 58 void onTick(void) { // function to call every tick
loarri 11:da3c8e9bf1f9 59 led = !led; // toggle the LED
loarri 11:da3c8e9bf1f9 60 }
loarri 11:da3c8e9bf1f9 61
loarri 11:da3c8e9bf1f9 62 void check_ToF(void)
loarri 11:da3c8e9bf1f9 63 {
loarri 11:da3c8e9bf1f9 64 checktof = 1;
loarri 11:da3c8e9bf1f9 65 }
loarri 11:da3c8e9bf1f9 66
loarri 11:da3c8e9bf1f9 67
loarri 11:da3c8e9bf1f9 68
loarri 11:da3c8e9bf1f9 69 /* Helper function for printing floats & doubles */
loarri 11:da3c8e9bf1f9 70 static char *print_double(char *str, double v, int decimalDigits = 2)
loarri 11:da3c8e9bf1f9 71 {
loarri 11:da3c8e9bf1f9 72 int i = 1;
loarri 11:da3c8e9bf1f9 73 int intPart, fractPart;
loarri 11:da3c8e9bf1f9 74 int len;
loarri 11:da3c8e9bf1f9 75 char *ptr;
loarri 11:da3c8e9bf1f9 76
loarri 11:da3c8e9bf1f9 77 /* prepare decimal digits multiplicator */
loarri 11:da3c8e9bf1f9 78 for (; decimalDigits != 0; i *= 10, decimalDigits--);
loarri 11:da3c8e9bf1f9 79
loarri 11:da3c8e9bf1f9 80 /* calculate integer & fractinal parts */
loarri 11:da3c8e9bf1f9 81 intPart = (int)v;
loarri 11:da3c8e9bf1f9 82 fractPart = (int)((v - (double)(int)v) * i);
loarri 11:da3c8e9bf1f9 83
loarri 11:da3c8e9bf1f9 84 /* fill in integer part */
loarri 11:da3c8e9bf1f9 85 sprintf(str, "%i.", intPart);
loarri 11:da3c8e9bf1f9 86
loarri 11:da3c8e9bf1f9 87 /* prepare fill in of fractional part */
loarri 11:da3c8e9bf1f9 88 len = strlen(str);
loarri 11:da3c8e9bf1f9 89 ptr = &str[len];
loarri 11:da3c8e9bf1f9 90
loarri 11:da3c8e9bf1f9 91 /* fill in leading fractional zeros */
loarri 11:da3c8e9bf1f9 92 for (i /= 10; i > 1; i /= 10, ptr++) {
loarri 11:da3c8e9bf1f9 93 if (fractPart >= i) {
loarri 11:da3c8e9bf1f9 94 break;
loarri 11:da3c8e9bf1f9 95 }
loarri 11:da3c8e9bf1f9 96 *ptr = '0';
loarri 11:da3c8e9bf1f9 97 }
loarri 11:da3c8e9bf1f9 98
loarri 11:da3c8e9bf1f9 99 /* fill in (rest of) fractional part */
loarri 11:da3c8e9bf1f9 100 sprintf(ptr, "%i", fractPart);
loarri 11:da3c8e9bf1f9 101
loarri 11:da3c8e9bf1f9 102 return str;
loarri 11:da3c8e9bf1f9 103 }
loarri 11:da3c8e9bf1f9 104
loarri 11:da3c8e9bf1f9 105
loarri 11:da3c8e9bf1f9 106 /*=================================== Main ==================================
loarri 11:da3c8e9bf1f9 107 =============================================================================*/
loarri 11:da3c8e9bf1f9 108 int main()
loarri 11:da3c8e9bf1f9 109 {
loarri 11:da3c8e9bf1f9 110
loarri 11:da3c8e9bf1f9 111
johnAlexander 0:6b7696e7df5e 112 printf("Hello world!\r\n");
johnAlexander 0:6b7696e7df5e 113
johnAlexander 3:d90f22c17d0c 114 VL53L1X_DevI2C *dev_I2C = new VL53L1X_DevI2C(VL53L1_I2C_SDA, VL53L1_I2C_SCL);
johnAlexander 0:6b7696e7df5e 115
johnAlexander 0:6b7696e7df5e 116 printf("I2C device created!\r\n");
johnAlexander 0:6b7696e7df5e 117
johnAlexander 0:6b7696e7df5e 118 /* creates the 53L1A1 expansion board singleton obj */
loarri 5:7f6149b9cbbe 119 //board = XNucleo53L1A1::instance(dev_I2C, A2, D9, D2); /* original code */
loarri 5:7f6149b9cbbe 120 board = XNucleo53L1A1::instance(dev_I2C, A2, PB_13, PB_14);
johnAlexander 0:6b7696e7df5e 121 printf("board created!\r\n");
johnAlexander 0:6b7696e7df5e 122
johnAlexander 0:6b7696e7df5e 123 /* init the 53L1A1 expansion board with default values */
johnAlexander 0:6b7696e7df5e 124 status = board->init_board();
dmathew 1:b7507ca3370f 125 if (status) {
johnAlexander 0:6b7696e7df5e 126 printf("Failed to init board!\r\n");
johnAlexander 0:6b7696e7df5e 127 return status;
johnAlexander 0:6b7696e7df5e 128 }
johnAlexander 0:6b7696e7df5e 129
johnAlexander 0:6b7696e7df5e 130 printf("board initiated! - %d\r\n", status);
johnAlexander 0:6b7696e7df5e 131
johnAlexander 0:6b7696e7df5e 132 /* Start ranging on the centre sensor */
dmathew 1:b7507ca3370f 133 if (board->sensor_centre != NULL) {
johnAlexander 3:d90f22c17d0c 134 status = board->sensor_centre->vl53l1x_start_ranging();
loarri 11:da3c8e9bf1f9 135 board->sensor_centre->disable_interrupt_measure_detection_irq();
dmathew 1:b7507ca3370f 136 if (status != 0) {
dmathew 1:b7507ca3370f 137 printf("Centre sensor failed to start ranging!\r\n");
dmathew 1:b7507ca3370f 138 return status;
dmathew 1:b7507ca3370f 139 }
johnAlexander 0:6b7696e7df5e 140 }
johnAlexander 0:6b7696e7df5e 141
johnAlexander 0:6b7696e7df5e 142 /* Start ranging on the left sensor */
dmathew 1:b7507ca3370f 143 if (board->sensor_left != NULL) {
johnAlexander 3:d90f22c17d0c 144 status = board->sensor_left->vl53l1x_start_ranging();
loarri 11:da3c8e9bf1f9 145 board->sensor_left->disable_interrupt_measure_detection_irq();
dmathew 1:b7507ca3370f 146 if (status != 0) {
dmathew 1:b7507ca3370f 147 printf("Left sensor failed to start ranging!\r\n");
dmathew 1:b7507ca3370f 148 return status;
dmathew 1:b7507ca3370f 149 }
johnAlexander 0:6b7696e7df5e 150 }
johnAlexander 0:6b7696e7df5e 151
johnAlexander 0:6b7696e7df5e 152 /* Start ranging on the right sensor */
dmathew 1:b7507ca3370f 153 if (board->sensor_right != NULL) {
johnAlexander 3:d90f22c17d0c 154 status = board->sensor_right->vl53l1x_start_ranging();
loarri 11:da3c8e9bf1f9 155 board->sensor_right->disable_interrupt_measure_detection_irq();
dmathew 1:b7507ca3370f 156 if (status != 0) {
dmathew 1:b7507ca3370f 157 printf("Right sensor failed to start ranging!\r\n");
dmathew 1:b7507ca3370f 158 return status;
dmathew 1:b7507ca3370f 159 }
johnAlexander 0:6b7696e7df5e 160 }
johnAlexander 0:6b7696e7df5e 161
loarri 11:da3c8e9bf1f9 162
loarri 11:da3c8e9bf1f9 163 //// mems
loarri 11:da3c8e9bf1f9 164 // initialize mems
loarri 11:da3c8e9bf1f9 165 uint8_t id;
loarri 11:da3c8e9bf1f9 166 float value1, value2;
loarri 11:da3c8e9bf1f9 167 char buffer1[32], buffer2[32];
loarri 11:da3c8e9bf1f9 168 int32_t axes[3];
loarri 11:da3c8e9bf1f9 169
loarri 11:da3c8e9bf1f9 170 /* Enable all sensors */
loarri 11:da3c8e9bf1f9 171 accelerometer->enable_x();
loarri 11:da3c8e9bf1f9 172 acc_gyro->enable_x();
loarri 11:da3c8e9bf1f9 173 acc_gyro->enable_g();
loarri 11:da3c8e9bf1f9 174 accelerometer->read_id(&id);
loarri 11:da3c8e9bf1f9 175 printf("LIS2DW12 accelerometer = 0x%X\r\n", id);
loarri 11:da3c8e9bf1f9 176 acc_gyro->read_id(&id);
loarri 11:da3c8e9bf1f9 177 printf("LSM6DSO accelerometer & gyroscope = 0x%X\r\n", id);
loarri 11:da3c8e9bf1f9 178
loarri 11:da3c8e9bf1f9 179 board = XNucleo53L1A1::instance(dev_I2C);
loarri 11:da3c8e9bf1f9 180 printf("I2C device created!\r\n");
loarri 11:da3c8e9bf1f9 181
loarri 11:da3c8e9bf1f9 182
johnAlexander 0:6b7696e7df5e 183 /* Ranging loop
johnAlexander 0:6b7696e7df5e 184 * Checks each sensor for data ready
johnAlexander 0:6b7696e7df5e 185 */
loarri 11:da3c8e9bf1f9 186
loarri 11:da3c8e9bf1f9 187 // calibration for ToF
loarri 11:da3c8e9bf1f9 188 for (int j=0; j<=ToF_ITERATIONS; j++) {
dmathew 1:b7507ca3370f 189 if (board->sensor_left != NULL) {
johnAlexander 3:d90f22c17d0c 190 board->sensor_left->vl53l1x_check_for_data_ready(&ready_left);
dmathew 1:b7507ca3370f 191 }
dmathew 1:b7507ca3370f 192 if (board->sensor_right != NULL) {
johnAlexander 3:d90f22c17d0c 193 board->sensor_right->vl53l1x_check_for_data_ready(&ready_right);
dmathew 1:b7507ca3370f 194 }
johnAlexander 0:6b7696e7df5e 195 if (ready_left) {
johnAlexander 3:d90f22c17d0c 196 board->sensor_left->vl53l1x_get_range_status(&ready_left);
johnAlexander 3:d90f22c17d0c 197 board->sensor_left->vl53l1x_get_distance(&distance_left);
johnAlexander 0:6b7696e7df5e 198 }
johnAlexander 0:6b7696e7df5e 199 if (ready_right) {
johnAlexander 3:d90f22c17d0c 200 board->sensor_right->vl53l1x_get_range_status(&ready_right);
johnAlexander 3:d90f22c17d0c 201 board->sensor_right->vl53l1x_get_distance(&distance_right);
loarri 11:da3c8e9bf1f9 202 }
loarri 11:da3c8e9bf1f9 203 }
loarri 11:da3c8e9bf1f9 204
loarri 11:da3c8e9bf1f9 205 printf("calibration done\r\n");
loarri 11:da3c8e9bf1f9 206
loarri 11:da3c8e9bf1f9 207 // start check ToF
loarri 11:da3c8e9bf1f9 208 Tick_ToF.attach(&check_ToF,ToF_FREQUENCY);
loarri 11:da3c8e9bf1f9 209
loarri 11:da3c8e9bf1f9 210 while (1)
loarri 11:da3c8e9bf1f9 211 {
loarri 11:da3c8e9bf1f9 212 led = 0;
loarri 11:da3c8e9bf1f9 213 if (checktof ==1) {
loarri 11:da3c8e9bf1f9 214 if (board->sensor_left != NULL) {
loarri 11:da3c8e9bf1f9 215 board->sensor_left->vl53l1x_check_for_data_ready(&ready_left);
loarri 11:da3c8e9bf1f9 216 }
loarri 11:da3c8e9bf1f9 217 if (board->sensor_right != NULL) {
loarri 11:da3c8e9bf1f9 218 board->sensor_right->vl53l1x_check_for_data_ready(&ready_right);
loarri 11:da3c8e9bf1f9 219 }
loarri 11:da3c8e9bf1f9 220 if (ready_left) {
loarri 11:da3c8e9bf1f9 221 board->sensor_left->vl53l1x_get_range_status(&ready_left);
loarri 11:da3c8e9bf1f9 222 board->sensor_left->vl53l1x_get_distance(&distance_left);
loarri 11:da3c8e9bf1f9 223 }
loarri 11:da3c8e9bf1f9 224 if (ready_right) {
loarri 11:da3c8e9bf1f9 225 board->sensor_right->vl53l1x_get_range_status(&ready_right);
loarri 11:da3c8e9bf1f9 226 board->sensor_right->vl53l1x_get_distance(&distance_right);
loarri 11:da3c8e9bf1f9 227 }
loarri 11:da3c8e9bf1f9 228 checktof =0;
loarri 11:da3c8e9bf1f9 229 if (distance_left <= ToF_SOGLIA_CRITICA || distance_right <= ToF_SOGLIA_CRITICA) {
loarri 11:da3c8e9bf1f9 230 led = 1;
loarri 11:da3c8e9bf1f9 231 //printf("\r\n DISTANZA PIU' PICCOLA DELLA SOGLIA !!!");
loarri 11:da3c8e9bf1f9 232 //ThisThread::sleep_for(1000);
loarri 11:da3c8e9bf1f9 233 stop_misura =1;
loarri 11:da3c8e9bf1f9 234 } else {
loarri 11:da3c8e9bf1f9 235 stop_misura =0;
loarri 11:da3c8e9bf1f9 236 }
loarri 11:da3c8e9bf1f9 237 }
loarri 11:da3c8e9bf1f9 238 if (stop_misura) {
loarri 11:da3c8e9bf1f9 239 printf("\r\n DISTANZA PIU' PICCOLA DELLA SOGLIA !!!");
loarri 11:da3c8e9bf1f9 240 } else {
loarri 11:da3c8e9bf1f9 241
loarri 11:da3c8e9bf1f9 242 accelerometer->get_x_axes(axes);
loarri 11:da3c8e9bf1f9 243 printf("LIS2DW12 [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
loarri 11:da3c8e9bf1f9 244
loarri 11:da3c8e9bf1f9 245 acc_gyro->get_x_axes(axes);
loarri 11:da3c8e9bf1f9 246 printf("LSM6DSO [acc/mg]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
loarri 11:da3c8e9bf1f9 247
loarri 11:da3c8e9bf1f9 248 acc_gyro->get_g_axes(axes);
loarri 11:da3c8e9bf1f9 249 printf("LSM6DSO [gyro/mdps]: %6d, %6d, %6d\r\n", axes[0], axes[1], axes[2]);
loarri 11:da3c8e9bf1f9 250 //ThisThread::sleep_for(1000);
loarri 11:da3c8e9bf1f9 251
loarri 11:da3c8e9bf1f9 252
loarri 11:da3c8e9bf1f9 253
johnAlexander 0:6b7696e7df5e 254 }
loarri 6:054c18d427bf 255 #if DISPLAY_CENTRE
dmathew 1:b7507ca3370f 256 if (board->sensor_centre != NULL) {
loarri 6:054c18d427bf 257 printf("Distance read by centre sensor is : %d mm\r\n", distance_centre);
dmathew 1:b7507ca3370f 258 }
loarri 6:054c18d427bf 259 #endif
loarri 6:054c18d427bf 260 #if DISPLAY_LEFT
dmathew 1:b7507ca3370f 261 if (board->sensor_left != NULL) {
loarri 6:054c18d427bf 262 printf("Distance read by left satellite sensor is : %d mm\r\n", distance_left);
dmathew 1:b7507ca3370f 263 }
loarri 6:054c18d427bf 264 #endif
loarri 6:054c18d427bf 265 #if DISPLAY_RIGHT
dmathew 1:b7507ca3370f 266 if (board->sensor_right != NULL) {
loarri 6:054c18d427bf 267 printf("Distance read by right satellite sensor is : %d mm\r\n", distance_right);
dmathew 1:b7507ca3370f 268 }
loarri 6:054c18d427bf 269 #endif
loarri 11:da3c8e9bf1f9 270
johnAlexander 0:6b7696e7df5e 271 }
johnAlexander 0:6b7696e7df5e 272
johnAlexander 0:6b7696e7df5e 273 return status;
johnAlexander 0:6b7696e7df5e 274 }