Rev 7 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 7 | Rev 8 | ||
|---|---|---|---|
| Line 28... | Line 28... | ||
| 28 | #include <stdio.h> |
28 | #include <stdio.h> |
| 29 | #include <string.h> |
29 | #include <string.h> |
| 30 | 30 | ||
| 31 | #include "ch.h" |
31 | #include "ch.h" |
| 32 | #include "hal.h" |
32 | #include "hal.h" |
| 33 | //#include "test.h" |
33 | // #include "test.h" |
| 34 | #include "shell.h" |
34 | #include "shell.h" |
| 35 | #include "evtimer.h" |
35 | #include "evtimer.h" |
| 36 | #include "chprintf.h" |
36 | #include "chprintf.h" |
| 37 | 37 | ||
| 38 | #if (HAL_USE_SERIAL_USB == TRUE) |
- | |
| 39 | #include "usbcfg.h" |
- | |
| 40 | #endif |
- | |
| 41 | - | ||
| 42 | #include "useAdc.h" |
- | |
| 43 | - | ||
| 44 | #include "shellCmds.h" |
38 | #include "shellCmds.h" |
| 45 | 39 | ||
| 46 | #include "ap_math.h" |
40 | #include "ap_math.h" |
| 47 | #include "spiInterface.h" |
41 | #include "spiInterface.h" |
| 48 | #include "SSD1306.h" |
42 | #include "SSD1306.h" |
| 49 | #include "font.h" |
43 | #include "font.h" |
| 50 | 44 | ||
| 51 | #include "vl53l0x_api.h" |
- | |
| 52 | #include "vl53l0x_platform.h" |
- | |
| 53 | #include "required_version.h" |
- | |
| 54 | - | ||
| 55 | - | ||
| 56 | static MUTEX_DECL( mutexDisplay); |
45 | static MUTEX_DECL(mutexDisplay); |
| 57 | - | ||
| 58 | unsigned lidar = 0; |
- | |
| 59 | 46 | ||
| 60 | /*===========================================================================*/ |
47 | /*===========================================================================*/ |
| 61 | /* Command line related. */ |
48 | /* Command line related. */ |
| 62 | /*===========================================================================*/ |
49 | /*===========================================================================*/ |
| 63 | 50 | ||
| 64 | #define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(1024) |
51 | #define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(1024) |
| 65 | #define TEST_WA_SIZE THD_WORKING_AREA_SIZE(256) |
52 | #define TEST_WA_SIZE THD_WORKING_AREA_SIZE(256) |
| - | 53 | ||
| - | 54 | #define RPM_ID 0x10d |
|
| - | 55 | ||
| - | 56 | ||
| - | 57 | /* |
|
| - | 58 | * Internal loopback mode, 500KBaud, automatic wakeup, automatic recover |
|
| - | 59 | * from abort mode. |
|
| - | 60 | */ |
|
| - | 61 | ||
| - | 62 | // CAN_BTR_SJW(n), where n = SJW - 1 |
|
| - | 63 | // CAN_BTR_BRP(n), where n = prescaler - 1 |
|
| - | 64 | // CAN_BTR_TS1(n), where n = Seg 1 - 1 |
|
| - | 65 | // CAN_BTR_TS2(n), where n = Seg 2 - 1 |
|
| - | 66 | ||
| - | 67 | static const CANConfig cancfg500 = { |
|
| - | 68 | CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, |
|
| - | 69 | CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(2) | |
|
| - | 70 | CAN_BTR_TS1(3) | CAN_BTR_BRP(8)}; |
|
| - | 71 | ||
| - | 72 | static const CANConfig cancfg1000 = { |
|
| - | 73 | CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP, |
|
| - | 74 | CAN_BTR_LBKM | CAN_BTR_SJW(0) | CAN_BTR_TS2(2) | |
|
| - | 75 | CAN_BTR_TS1(4) | CAN_BTR_BRP(3)}; |
|
| 66 | 76 | ||
| 67 | static const ShellConfig shell_cfg1 = { |
77 | static const ShellConfig shell_cfg1 = { |
| 68 | #if (HAL_USE_SERIAL_USB == TRUE) |
- | |
| 69 | (BaseSequentialStream*) &SDU1, |
- | |
| 70 | #else |
- | |
| 71 | (BaseSequentialStream *)&SD1, |
78 | (BaseSequentialStream *)&SD1, |
| 72 | #endif |
- | |
| 73 | shellCommands }; |
79 | shellCommands}; |
| 74 | //////// |
80 | //////// |
| 75 | // end of shell stuff |
81 | // end of shell stuff |
| 76 | 82 | ||
| 77 | uint16_t sampIndex; |
83 | uint16_t sampIndex; |
| 78 | 84 | ||
| 79 | void setDriveA(uint8_t bit) { |
85 | void setDriveA(uint8_t bit) |
| - | 86 | { |
|
| 80 | if (bit) { |
87 | if (bit) |
| - | 88 | { |
|
| 81 | palSetPad(GPIOA, GPIOA_A1); |
89 | palSetPad(GPIOA, GPIOA_A1); |
| 82 | palClearPad(GPIOA, GPIOA_A2); |
90 | palClearPad(GPIOA, GPIOA_A2); |
| - | 91 | } |
|
| 83 | } else { |
92 | else |
| - | 93 | { |
|
| 84 | palClearPad(GPIOA, GPIOA_A1); |
94 | palClearPad(GPIOA, GPIOA_A1); |
| 85 | palSetPad(GPIOA, GPIOA_A2); |
95 | palSetPad(GPIOA, GPIOA_A2); |
| 86 | } |
96 | } |
| 87 | } |
97 | } |
| 88 | 98 | ||
| 89 | void setDriveB(uint8_t bit) { |
99 | void setDriveB(uint8_t bit) |
| - | 100 | { |
|
| 90 | if (bit) { |
101 | if (bit) |
| - | 102 | { |
|
| 91 | palSetPad(GPIOA, GPIOA_B1); |
103 | palSetPad(GPIOA, GPIOA_B1); |
| 92 | palClearPad(GPIOA, GPIOA_B2); |
104 | palClearPad(GPIOA, GPIOA_B2); |
| - | 105 | } |
|
| 93 | } else { |
106 | else |
| - | 107 | { |
|
| 94 | palClearPad(GPIOA, GPIOA_B1); |
108 | palClearPad(GPIOA, GPIOA_B1); |
| 95 | palSetPad(GPIOA, GPIOA_B2); |
109 | palSetPad(GPIOA, GPIOA_B2); |
| 96 | } |
110 | } |
| 97 | } |
111 | } |
| 98 | 112 | ||
| 99 | - | ||
| 100 | // dial settings |
113 | // dial settings |
| 101 | volatile int origin = 0; |
114 | volatile int origin = 0; |
| 102 | ; |
115 | ; |
| 103 | volatile int target = 0; |
116 | volatile int target = 0; |
| 104 | volatile int count = 0; |
117 | volatile int count = 0; |
| 105 | 118 | ||
| 106 | volatile bool pause; |
119 | volatile bool pause; |
| 107 | // |
120 | // |
| 108 | 121 | ||
| 109 | - | ||
| 110 | void setPause(bool p) { |
122 | void setPause(bool p) |
| - | 123 | { |
|
| 111 | pause = p; |
124 | pause = p; |
| 112 | } |
125 | } |
| 113 | 126 | ||
| 114 | static THD_WORKING_AREA(waThread1, 512); |
127 | static THD_WORKING_AREA(waGaugeThread, 512); |
| 115 | static void Thread1(void *arg) { |
128 | static THD_FUNCTION(gaugeThread, p) { |
| 116 | - | ||
| 117 | (void) arg; |
129 | (void)p; |
| 118 | unsigned const fast = 10; |
130 | unsigned const fast = 10; |
| 119 | unsigned const slow = 30; |
131 | unsigned const slow = 30; |
| 120 | unsigned const range = slow - fast; |
132 | unsigned const range = slow - fast; |
| 121 | unsigned del = fast; |
133 | unsigned del = fast; |
| 122 | int step = 1; |
134 | int step = 1; |
| 123 | chRegSetThreadName("Step"); |
135 | chRegSetThreadName("Step"); |
| 124 | while (TRUE) { |
136 | while (TRUE) |
| - | 137 | { |
|
| 125 | 138 | ||
| 126 | while (pause) |
139 | while (pause) |
| 127 | chThdSleep(1000); |
140 | chThdSleep(1000); |
| 128 | 141 | ||
| 129 | switch (count % 4) { |
142 | switch (count % 4) |
| - | 143 | { |
|
| 130 | case 0: |
144 | case 0: |
| 131 | setDriveA(1); |
145 | setDriveA(1); |
| 132 | setDriveB(0); |
146 | setDriveB(0); |
| 133 | break; |
147 | break; |
| 134 | case 1: |
148 | case 1: |
| Line 145... | Line 159... | ||
| 145 | break; |
159 | break; |
| 146 | } |
160 | } |
| 147 | 161 | ||
| 148 | // all this calculates minimum distance from |
162 | // all this calculates minimum distance from |
| 149 | // target or origin |
163 | // target or origin |
| 150 | int d1 = count - origin; |
164 | int d1 = abs(count - origin); |
| 151 | if (d1 < 0) |
- | |
| 152 | d1 = -d1; |
- | |
| 153 | int d2 = count - target; |
165 | int d2 = abs(count - target); |
| 154 | if (d2 < 0) |
- | |
| 155 | d2 = -d2; |
- | |
| 156 | // finally, minimum distance |
166 | // finally, minimum distance |
| 157 | int dist = d1 < d2 ? d1 : d2; |
167 | int dist = min(d1,d2); |
| 158 | 168 | ||
| 159 | del = fast; |
169 | del = fast; |
| 160 | if (dist < range) // inside lower bound of distance |
170 | if (dist < range) // inside lower bound of distance |
| 161 | { |
171 | { |
| 162 | del = slow - dist; |
172 | del = slow - dist; |
| 163 | } |
173 | } |
| 164 | chThdSleep(del); |
174 | chThdSleep(del); |
| 165 | 175 | ||
| 166 | if (count < target) { |
176 | if (count < target) |
| - | 177 | { |
|
| 167 | step = 1; |
178 | step = 1; |
| 168 | } |
179 | } |
| 169 | if (count > target) { |
180 | if (count > target) |
| - | 181 | { |
|
| 170 | step = -1; |
182 | step = -1; |
| 171 | } |
183 | } |
| 172 | if (count == target) { |
184 | if (count == target) |
| - | 185 | { |
|
| 173 | step = 0; |
186 | step = 0; |
| 174 | } |
187 | } |
| 175 | count = count + step; |
188 | count = count + step; |
| 176 | - | ||
| 177 | } |
189 | } |
| 178 | - | ||
| 179 | } |
190 | } |
| 180 | 191 | ||
| 181 | /* |
192 | /* |
| 182 | * Command Shell Thread |
193 | * Command Shell Thread |
| 183 | */ |
194 | */ |
| 184 | static THD_WORKING_AREA(waThread2, 512); |
195 | static THD_WORKING_AREA(waShell, 512); |
| 185 | static void Thread2(void *arg) { |
196 | static THD_FUNCTION(shell, p) { |
| - | 197 | (void) p; |
|
| 186 | thread_t *shelltp = NULL; |
198 | thread_t *shelltp = NULL; |
| 187 | 199 | ||
| 188 | chRegSetThreadName("Shell "); |
200 | chRegSetThreadName("Shell "); |
| 189 | /* |
201 | /* |
| 190 | * in this demo it just performs |
202 | * in this demo it just performs |
| 191 | * a shell respawn upon its termination. |
203 | * a shell respawn upon its termination. |
| 192 | */ |
204 | */ |
| 193 | while (true) { |
205 | while (true) |
| - | 206 | { |
|
| 194 | if (!shelltp) { |
207 | if (!shelltp) |
| 195 | #if (HAL_USE_SERIAL_USB == TRUE) |
- | |
| 196 | if (SDU1.config->usbp->state == USB_ACTIVE) { |
- | |
| 197 | /* Spawns a new shell.*/ |
- | |
| 198 | shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", |
- | |
| 199 | NORMALPRIO, shellThread, (void*) &shell_cfg1); |
- | |
| 200 | } |
208 | { |
| 201 | #else |
209 | |
| 202 | shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", NORMALPRIO, shellThread, (void *) &shell_cfg1); |
210 | shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE, "shell", NORMALPRIO, shellThread, (void *)&shell_cfg1); |
| 203 | #endif |
211 | } |
| 204 | } else { |
212 | else |
| - | 213 | { |
|
| 205 | /* If the previous shell exited.*/ |
214 | /* If the previous shell exited.*/ |
| 206 | if (chThdTerminatedX(shelltp)) { |
215 | if (chThdTerminatedX(shelltp)) |
| - | 216 | { |
|
| 207 | /* Recovers memory of the previous shell.*/ |
217 | /* Recovers memory of the previous shell.*/ |
| 208 | chThdRelease(shelltp); |
218 | chThdRelease(shelltp); |
| 209 | shelltp = NULL; |
219 | shelltp = NULL; |
| 210 | } |
220 | } |
| 211 | } |
221 | } |
| 212 | chThdSleepMilliseconds(500); |
222 | chThdSleepMilliseconds(500); |
| 213 | } |
223 | } |
| 214 | - | ||
| 215 | } |
224 | } |
| 216 | 225 | ||
| 217 | static THD_WORKING_AREA(waThread3, 1024); |
- | |
| 218 | static void Thread3(void *arg) { |
- | |
| 219 | - | ||
| 220 | VL53L0X_Dev_t MyDevice; |
- | |
| 221 | VL53L0X_Dev_t *pMyDevice = &MyDevice; |
- | |
| 222 | setPause(true); |
- | |
| 223 | int Status = VL53L0XdeviceSetup(pMyDevice); |
- | |
| 224 | setPause(false); |
- | |
| 225 | VL53L0X_RangingMeasurementData_t RangingMeasurementData; |
- | |
| 226 | - | ||
| 227 | while(1){ |
- | |
| 228 | - | ||
| 229 | chThdSleep(chTimeMS2I(100)); |
- | |
| 230 | setPause(true); |
- | |
| 231 | Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice, |
- | |
| 232 | &RangingMeasurementData); |
- | |
| 233 | setPause(false); |
- | |
| 234 | - | ||
| 235 | if (Status != VL53L0X_ERROR_NONE) continue; |
- | |
| 236 | - | ||
| 237 | - | ||
| 238 | lidar = RangingMeasurementData.RangeMilliMeter; |
- | |
| 239 | - | ||
| 240 | - | ||
| 241 | 226 | ||
| - | 227 | static THD_WORKING_AREA(waCanRx, 1024); |
|
| - | 228 | static THD_FUNCTION(canRx, p) { |
|
| - | 229 | (void) p; |
|
| - | 230 | ||
| - | 231 | // initialize can bus hardware |
|
| - | 232 | event_listener_t el; |
|
| - | 233 | CANRxFrame rxmsg; |
|
| - | 234 | ||
| - | 235 | canStart(&CAND1, &cancfg1000); |
|
| - | 236 | ||
| - | 237 | chRegSetThreadName("CAN receiver"); |
|
| - | 238 | chEvtRegister(&CAND1.rxfull_event, &el, 0); |
|
| - | 239 | while (true) |
|
| - | 240 | { |
|
| - | 241 | if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) |
|
| - | 242 | continue; |
|
| - | 243 | while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) |
|
| - | 244 | { |
|
| - | 245 | /* Process message.*/ |
|
| - | 246 | } |
|
| 242 | } |
247 | } |
| - | 248 | chEvtUnregister(&CAND1.rxfull_event, &el); |
|
| 243 | } |
249 | } |
| 244 | /* |
250 | /* |
| 245 | * Application entry point. |
251 | * Application entry point. |
| 246 | */ |
252 | */ |
| 247 | int main(void) { |
253 | int main(void) |
| - | 254 | { |
|
| 248 | // struct EventListener el0, el1; |
255 | // struct EventListener el0, el1; |
| 249 | 256 | ||
| 250 | /* |
257 | /* |
| 251 | * System initializations. |
258 | * System initializations. |
| 252 | * - HAL initialization, this also initializes the configured device drivers |
259 | * - HAL initialization, this also initializes the configured device drivers |
| 253 | * and performs the board-specific initializations. |
260 | * and performs the board-specific initializations. |
| Line 255... | Line 262... | ||
| 255 | * RTOS is active. |
262 | * RTOS is active. |
| 256 | */ |
263 | */ |
| 257 | halInit(); |
264 | halInit(); |
| 258 | chSysInit(); |
265 | chSysInit(); |
| 259 | 266 | ||
| 260 | #if (HAL_USE_SERIAL_USB == TRUE) |
- | |
| 261 | /* |
267 | /* |
| 262 | * Initializes a serial-over-USB CDC driver. |
268 | * Initializes serial port. |
| 263 | */ |
269 | */ |
| 264 | sduObjectInit(&SDU1); |
- | |
| 265 | sduStart(&SDU1, &serusbcfg); |
- | |
| 266 | - | ||
| 267 | #if HAL_USE_USB_DUAL_CDC == TRUE |
- | |
| 268 | sduObjectInit(&SDU2); |
- | |
| 269 | sduStart(&SDU2, &serusbcfg2); |
- | |
| 270 | #endif |
- | |
| 271 | - | ||
| 272 | /* |
- | |
| 273 | * Activates the USB driver and then the USB bus pull-up on D+. |
- | |
| 274 | * Note, a delay is inserted in order to not have to disconnect the cable |
- | |
| 275 | * after a reset. |
- | |
| 276 | */ |
- | |
| 277 | usbDisconnectBus(serusbcfg.usbp); |
- | |
| 278 | chThdSleepMilliseconds(1000); |
- | |
| 279 | usbStart(serusbcfg.usbp, &usbcfg); |
- | |
| 280 | usbConnectBus(serusbcfg.usbp); |
- | |
| 281 | #else |
- | |
| 282 | /* |
- | |
| 283 | * Initializes serial port. |
- | |
| 284 | */ |
- | |
| 285 | sdStart(&SD2, NULL); |
270 | sdStart(&SD1, NULL); |
| 286 | #endif /* HAL_USE_SERIAL_USB */ |
- | |
| 287 | - | ||
| 288 | useAdc(); |
- | |
| 289 | - | ||
| 290 | 271 | ||
| 291 | /* |
272 | /* |
| 292 | * Shell manager initialization. |
273 | * Shell manager initialization. |
| 293 | */ |
274 | */ |
| 294 | // shellInit(); |
275 | // shellInit(); |
| 295 | /* |
276 | /* |
| 296 | * initialise approximate maths |
277 | * initialise approximate maths |
| 297 | */ |
278 | */ |
| 298 | ap_init(); |
279 | ap_init(); |
| 299 | 280 | ||
| 300 | chMtxLock(&mutexDisplay); |
281 | chMtxLock(&mutexDisplay); |
| 301 | 282 | ||
| 302 | /* start the SPI hardware for display */ |
283 | /* start the SPI hardware for display */ |
| 303 | ssd1306spiInit(); |
284 | ssd1306spiInit(); |
| 304 | 285 | ||
| 305 | ssd1306_begin( SSD1306_SWITCHCAPVCC, 0) ; |
286 | ssd1306_begin(SSD1306_SWITCHCAPVCC, 0); |
| 306 | - | ||
| 307 | 287 | ||
| 308 | clearDisplay(); |
288 | clearDisplay(); |
| 309 | 289 | ||
| 310 | display(); |
290 | display(); |
| 311 | 291 | ||
| 312 | chMtxUnlock(&mutexDisplay); |
292 | chMtxUnlock(&mutexDisplay); |
| 313 | - | ||
| 314 | 293 | ||
| 315 | /* |
294 | /* |
| 316 | * Creates the PLL thread |
295 | * Creates the PLL thread |
| 317 | */ |
296 | */ |
| 318 | chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL); |
297 | chThdCreateStatic(waGaugeThread, sizeof(waGaugeThread), NORMALPRIO, gaugeThread, NULL); |
| 319 | 298 | ||
| 320 | chThdCreateStatic(waThread2, sizeof(waThread2), NORMALPRIO, Thread2, NULL); |
299 | chThdCreateStatic(waCanRx, sizeof(waCanRx), NORMALPRIO, canRx, NULL); |
| 321 | 300 | ||
| 322 | // chThdCreateStatic(waThread3, sizeof(waThread3), NORMALPRIO, Thread3, NULL); |
301 | chThdCreateStatic(waShell, sizeof(waShell), NORMALPRIO, shell, NULL); |
| 323 | 302 | ||
| 324 | // reset gauge |
303 | // reset gauge |
| 325 | origin = 540; |
304 | origin = 540; |
| 326 | count = 540; |
305 | count = 540; |
| 327 | target = 0; |
306 | target = 0; |
| 328 | chThdSleepMilliseconds(1000); |
307 | chThdSleepMilliseconds(1000); |
| 329 | target = 0; |
308 | target = 0; |
| 330 | 309 | ||
| 331 | int frac = 0; |
310 | int frac = 0; |
| 332 | /* start the SPI hardware for display */ |
311 | /* start the SPI hardware for display */ |
| 333 | while (1) { |
312 | while (1) |
| 334 | 313 | { |
|
| 335 | chThdSleepMilliseconds(100); |
- | |
| 336 | unsigned const SCALE = frac * 32 + 16; |
- | |
| 337 | frac = ++frac %10; |
- | |
| 338 | 314 | ||
| 339 | // read the dial |
315 | chThdSleepMilliseconds(100); |
| - | 316 | unsigned const SCALE = frac * 32 + 16; |
|
| 340 | adcSample(); |
317 | frac = (++frac) % 10; |
| 341 | 318 | ||
| 342 | origin = count; |
319 | // read the dial |
| 343 | 320 | ||
| 344 | target = target +1; |
321 | origin = count; |
| 345 | 322 | ||
| 346 | // target = lidar * 630L / 2000L; |
323 | target = target + 1; |
| 347 | 324 | ||
| 348 | if (target > 540) target = 0; |
325 | // target = lidar * 630L / 2000L; |
| 349 | 326 | ||
| - | 327 | if (target > 540) |
|
| - | 328 | target = 0; |
|
| 350 | 329 | ||
| 351 | chMtxLock(&mutexDisplay); |
330 | chMtxLock(&mutexDisplay); |
| 352 | 331 | ||
| 353 | font_gotoxy(0,0); |
332 | font_gotoxy(0, 0); |
| 354 | clearDisplay(); |
333 | clearDisplay(); |
| 355 | 334 | ||
| 356 | // print_scaled_string("cm:",0,16,4,SCALE); |
335 | // print_scaled_string("cm:",0,16,4,SCALE); |
| 357 | - | ||
| 358 | - | ||
| 359 | print_digits(0, 0, 4, 1, target,SCALE); |
- | |
| 360 | - | ||
| 361 | chMtxUnlock(&mutexDisplay); |
- | |
| 362 | - | ||
| 363 | display(); |
- | |
| 364 | 336 | ||
| - | 337 | print_digits(0, 0, 4, 1, target, SCALE); |
|
| 365 | 338 | ||
| - | 339 | chMtxUnlock(&mutexDisplay); |
|
| 366 | 340 | ||
| - | 341 | display(); |
|
| 367 | } |
342 | } |
| 368 | - | ||
| 369 | } |
343 | } |