amiro-os / devices / LightRing / main.cpp @ beb4f137
History | View | Annotate | Download (21.733 KB)
1 | 58fe0e0b | Thomas Schöpping | #define BL_CALLBACK_TABLE_ADDR (0x08000000 + 0x01C0) |
---|---|---|---|
2 | #define BL_MAGIC_NUMBER ((uint32_t)0xFF669900u) |
||
3 | |||
4 | #define SHUTDOWN_NONE 0 |
||
5 | #define SHUTDOWN_TRANSPORTATION 1 |
||
6 | #define SHUTDOWN_DEEPSLEEP 2 |
||
7 | #define SHUTDOWN_HIBERNATE 3 |
||
8 | #define SHUTDOWN_RESTART 4 |
||
9 | #define SHUTDOWN_HANDLE_REQUEST 5 |
||
10 | |||
11 | #include <ch.hpp> |
||
12 | |||
13 | #include "global.hpp" |
||
14 | #include <amiro/util/util.h> |
||
15 | #include <amiro/Color.h> |
||
16 | #include <exti.hpp> |
||
17 | |||
18 | #include <chprintf.h> |
||
19 | #include <shell.h> |
||
20 | |||
21 | using namespace amiro; |
||
22 | using namespace chibios_rt; |
||
23 | |||
24 | Global global; |
||
25 | |||
26 | 10687985 | Thomas Schöpping | struct blVersion_t {
|
27 | const uint8_t identifier;
|
||
28 | const uint8_t major;
|
||
29 | const uint8_t minor;
|
||
30 | const uint8_t patch;
|
||
31 | } __attribute__((packed)); |
||
32 | |||
33 | 58fe0e0b | Thomas Schöpping | void shellRequestShutdown(BaseSequentialStream* chp, int argc, char *argv[]) { |
34 | |||
35 | chprintf(chp, "shellRequestShutdown\n");
|
||
36 | |||
37 | /* if nor argument was given, print some help text */
|
||
38 | if (argc == 0 || strcmp(argv[0], "help") == 0) { |
||
39 | chprintf(chp, "\tUSAGE:\n");
|
||
40 | chprintf(chp, "> shutdown <type>\n");
|
||
41 | chprintf(chp, "\n");
|
||
42 | chprintf(chp, "\ttype\n");
|
||
43 | chprintf(chp, "The type of shutdown to perform.\n");
|
||
44 | chprintf(chp, "Choose one of the following types:\n");
|
||
45 | chprintf(chp, " transportation - Ultra low-power mode with all wakeups disabled.\n");
|
||
46 | chprintf(chp, " The robot can not be charged.\n");
|
||
47 | chprintf(chp, " deepsleep - Ultra low-power mode with several wakeups enabled.\n");
|
||
48 | chprintf(chp, " The robot can only be charged via the power plug.\n");
|
||
49 | chprintf(chp, " hibernate - Medium low-power mode, but with full charging capabilities.\n");
|
||
50 | chprintf(chp, " restart - Performs a system restart.\n");
|
||
51 | chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
|
||
52 | chprintf(chp, "\n");
|
||
53 | return;
|
||
54 | } |
||
55 | |||
56 | if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) { |
||
57 | shutdown_now = SHUTDOWN_TRANSPORTATION; |
||
58 | chprintf(chp, "shutdown to transportation mode initialized\n");
|
||
59 | } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) { |
||
60 | shutdown_now = SHUTDOWN_DEEPSLEEP; |
||
61 | chprintf(chp, "shutdown to deepsleep mode initialized\n");
|
||
62 | } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) { |
||
63 | shutdown_now = SHUTDOWN_HIBERNATE; |
||
64 | chprintf(chp, "shutdown to hibernate mode initialized\n");
|
||
65 | } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) { |
||
66 | chprintf(chp, "restart initialized\n");
|
||
67 | shutdown_now = SHUTDOWN_RESTART; |
||
68 | } else {
|
||
69 | chprintf(chp, "ERROR: unknown argument!\n");
|
||
70 | shutdown_now = SHUTDOWN_NONE; |
||
71 | } |
||
72 | |||
73 | return;
|
||
74 | } |
||
75 | |||
76 | void testLights() {
|
||
77 | for (int i = 0; i < (8 * 16 * 2); i++) { |
||
78 | Color color; |
||
79 | int brightness;
|
||
80 | |||
81 | switch (((i / 16) / 2) % 8) { |
||
82 | case 0: |
||
83 | color = Color::WHITE; |
||
84 | break;
|
||
85 | case 1: |
||
86 | color = Color::RED; |
||
87 | break;
|
||
88 | case 2: |
||
89 | color = Color::FUCHSIA; |
||
90 | break;
|
||
91 | case 3: |
||
92 | color = Color::BLUE; |
||
93 | break;
|
||
94 | case 4: |
||
95 | color = Color::AQUA; |
||
96 | break;
|
||
97 | case 5: |
||
98 | color = Color::LIME; |
||
99 | break;
|
||
100 | case 6: |
||
101 | color = Color::YELLOW; |
||
102 | break;
|
||
103 | case 7: |
||
104 | default:
|
||
105 | color = Color::BLACK; |
||
106 | break;
|
||
107 | } |
||
108 | |||
109 | if (i & (1 << 4)) |
||
110 | brightness = 10 + (i % 16) * 6; |
||
111 | else
|
||
112 | brightness = 10;
|
||
113 | |||
114 | global.robot.setLightBrightness(brightness); |
||
115 | global.robot.setLightColor((i / 2) % 8, color); |
||
116 | |||
117 | BaseThread::sleep(MS2ST(250));
|
||
118 | } |
||
119 | } |
||
120 | |||
121 | void testColors() {
|
||
122 | Color color; |
||
123 | |||
124 | global.robot.setLightBrightness(50);
|
||
125 | |||
126 | for (int i = 0; i < Color::YELLOWGREEN; i++) { |
||
127 | color = Color(static_cast<Color::GlobalColor>(i));
|
||
128 | for (int j = 0; j < 8; j++) |
||
129 | global.robot.setLightColor(j, color); |
||
130 | |||
131 | BaseThread::sleep(MS2ST(250));
|
||
132 | } |
||
133 | |||
134 | } |
||
135 | |||
136 | void testFade() {
|
||
137 | for (int i = 0; i < 255; i += 16) { |
||
138 | Color color(255 - i, i, 0); |
||
139 | for (int j = 0; j < 8; j++) { |
||
140 | global.robot.setLightColor(j, color); |
||
141 | BaseThread::sleep(MS2ST(250));
|
||
142 | } |
||
143 | } |
||
144 | } |
||
145 | |||
146 | void testBrightness() {
|
||
147 | for (int j = 0; j < 8; j++) |
||
148 | global.robot.setLightColor(j, Color::WHITE); |
||
149 | |||
150 | for (int i = 0; i < 200; i += 5) { |
||
151 | int brightness = (i > 100) ? 200 - i : i; |
||
152 | global.robot.setLightBrightness(brightness); |
||
153 | BaseThread::sleep(MS2ST(100));
|
||
154 | } |
||
155 | } |
||
156 | |||
157 | static msg_t shutdownRequest(void *arg) { |
||
158 | |||
159 | (void) arg;
|
||
160 | const size_t max_str_len = strlen("shutdown"); |
||
161 | uint8_t buffer[20];
|
||
162 | uint8_t num_bytes = 0x00u;
|
||
163 | uint8_t consume; |
||
164 | |||
165 | while (true) { |
||
166 | |||
167 | num_bytes += sdAsynchronousRead(&SD1, &buffer[num_bytes], sizeof(buffer) - num_bytes);
|
||
168 | |||
169 | if (num_bytes) {
|
||
170 | |||
171 | consume = 0x01u;
|
||
172 | |||
173 | if (num_bytes >= strlen("shutdown") && !strncmp((char*) buffer, "shutdown", strlen("shutdown"))) { |
||
174 | |||
175 | boardRequestShutdown(); |
||
176 | consume = strlen("shutdown");
|
||
177 | |||
178 | } |
||
179 | |||
180 | if (consume != 0x01 || num_bytes >= max_str_len) { |
||
181 | |||
182 | num_bytes -= consume; |
||
183 | memcpy(&buffer[0], &buffer[consume], num_bytes);
|
||
184 | |||
185 | } |
||
186 | |||
187 | } |
||
188 | |||
189 | BaseThread::sleep(MS2ST(1000));
|
||
190 | |||
191 | } |
||
192 | |||
193 | return RDY_OK;
|
||
194 | |||
195 | } |
||
196 | |||
197 | void systemShutdown() {
|
||
198 | |||
199 | global.userThread.requestTerminate(); |
||
200 | global.userThread.wait(); |
||
201 | |||
202 | global.robot.setLightBrightness(0);
|
||
203 | global.robot.terminate(); |
||
204 | |||
205 | global.tlc5947.requestTerminate(); |
||
206 | global.tlc5947.update(); |
||
207 | global.tlc5947.wait(); |
||
208 | |||
209 | global.lidar.requestTerminate(); |
||
210 | global.lidar.wait(); |
||
211 | |||
212 | // boardStandby();
|
||
213 | |||
214 | return;
|
||
215 | } |
||
216 | |||
217 | void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) { |
||
218 | chprintf(chp, "shellRequestInitMemory\n");
|
||
219 | |||
220 | msg_t res = global.memory.resetMemory(); |
||
221 | if ( res != global.memory.OK)
|
||
222 | chprintf(chp, "Memory Init: FAIL\n");
|
||
223 | else
|
||
224 | chprintf(chp, "Memory Init: OK\n");
|
||
225 | } |
||
226 | |||
227 | void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) { |
||
228 | chprintf(chp, "shellRequestGetBoardId\n");
|
||
229 | |||
230 | uint8_t id = 0xFFu;
|
||
231 | msg_t res = global.memory.getBoardId(&id); |
||
232 | if (res != global.memory.OK)
|
||
233 | chprintf(chp, "Get Board ID: FAIL\n");
|
||
234 | else
|
||
235 | chprintf(chp, "Get Board ID: %u\n", id);
|
||
236 | } |
||
237 | |||
238 | void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
239 | chprintf(chp, "shellRequestSetBoardId\n");
|
||
240 | |||
241 | if (argc == 0) { |
||
242 | chprintf(chp, "Usage: %s\n","set_board_id <idx>"); |
||
243 | } else {
|
||
244 | msg_t res = global.memory.setBoardId(atoi(argv[0]));
|
||
245 | if (res != global.memory.OK)
|
||
246 | chprintf(chp, "Set Board ID: FAIL\n");
|
||
247 | else
|
||
248 | chprintf(chp, "Set Board ID: OK\n");
|
||
249 | } |
||
250 | } |
||
251 | |||
252 | void boardPeripheryCheck(BaseSequentialStream *chp) {
|
||
253 | msg_t result; |
||
254 | chprintf(chp, "\nCHECK: START\n");
|
||
255 | |||
256 | // Check the radio
|
||
257 | result = global.a2500r24a.getCheck(); |
||
258 | if (result == global.a2500r24a.CHECK_OK)
|
||
259 | chprintf(chp, "A2500R24A: OK\n");
|
||
260 | else
|
||
261 | chprintf(chp, "A2500R24A: FAIL\n");
|
||
262 | |||
263 | // Check the eeprom
|
||
264 | result = global.memory.getCheck(); |
||
265 | if ( result != global.memory.OK)
|
||
266 | chprintf(chp, "Memory Structure: FAIL\n");
|
||
267 | else
|
||
268 | chprintf(chp, "Memory Structure: OK\n");
|
||
269 | |||
270 | // Check the lights
|
||
271 | chprintf(chp, "LEDs: Test colors\n");
|
||
272 | testColors(); |
||
273 | chprintf(chp, "LEDs: Off\n");
|
||
274 | global.robot.setLightBrightness(0);
|
||
275 | |||
276 | chprintf(chp, "CHECK: FINISH\n");
|
||
277 | } |
||
278 | |||
279 | void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
280 | |||
281 | enum Type {HEX, U8, U16, U32, S8, S16, S32};
|
||
282 | |||
283 | chprintf(chp, "shellRequestReadData\n");
|
||
284 | |||
285 | if (argc < 2 || strcmp(argv[0],"help") == 0) |
||
286 | { |
||
287 | chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]"); |
||
288 | chprintf(chp, "\n");
|
||
289 | chprintf(chp, "\ttype\n");
|
||
290 | chprintf(chp, "The data type as which to interpret the data.\n");
|
||
291 | chprintf(chp, "Choose one of the following types:\n");
|
||
292 | chprintf(chp, " hex - one byte as hexadecimal value\n");
|
||
293 | chprintf(chp, " u8 - unsigned integer (8 bit)\n");
|
||
294 | chprintf(chp, " u16 - unsigned integer (16 bit)\n");
|
||
295 | chprintf(chp, " u32 - unsigned integer (32 bit)\n");
|
||
296 | chprintf(chp, " s8 - signed integer (8 bit)\n");
|
||
297 | chprintf(chp, " s16 - signed integer (16 bit)\n");
|
||
298 | chprintf(chp, " s32 - signed integer (32 bit)\n");
|
||
299 | chprintf(chp, "\tstart\n");
|
||
300 | chprintf(chp, "The first byte to read from the memory.\n");
|
||
301 | chprintf(chp, "\tcount [default = 1]\n");
|
||
302 | chprintf(chp, "The number of elements to read.\n");
|
||
303 | chprintf(chp, "\n");
|
||
304 | chprintf(chp, "\tNOTE\n");
|
||
305 | chprintf(chp, "Type conversions of this function might fail.\n");
|
||
306 | chprintf(chp, "If so, use type=hex and convert by hand.\n");
|
||
307 | chprintf(chp, "\n");
|
||
308 | return;
|
||
309 | } |
||
310 | |||
311 | uint8_t type_size = 0;
|
||
312 | Type type = HEX; |
||
313 | if (strcmp(argv[0],"hex") == 0) { |
||
314 | type_size = sizeof(unsigned char); |
||
315 | type = HEX; |
||
316 | } else if(strcmp(argv[0],"u8") == 0) { |
||
317 | type_size = sizeof(uint8_t);
|
||
318 | type = U8; |
||
319 | } else if(strcmp(argv[0],"u16") == 0) { |
||
320 | type_size = sizeof(uint16_t);
|
||
321 | type = U16; |
||
322 | } else if(strcmp(argv[0],"u32") == 0) { |
||
323 | type_size = sizeof(uint32_t);
|
||
324 | type = U32; |
||
325 | } else if(strcmp(argv[0],"s8") == 0) { |
||
326 | type_size = sizeof(int8_t);
|
||
327 | type = S8; |
||
328 | } else if(strcmp(argv[0],"s16") == 0) { |
||
329 | type_size = sizeof(int16_t);
|
||
330 | type = S16; |
||
331 | } else if(strcmp(argv[0],"s32") == 0) { |
||
332 | type_size = sizeof(int32_t);
|
||
333 | type = S32; |
||
334 | } else {
|
||
335 | chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
|
||
336 | return;
|
||
337 | } |
||
338 | |||
339 | unsigned int start_byte = atoi(argv[1]); |
||
340 | |||
341 | unsigned int num_elements = 1; |
||
342 | if (argc >= 3) |
||
343 | num_elements = atoi(argv[2]);
|
||
344 | |||
345 | const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
|
||
346 | uint8_t buffer[eeprom_size]; |
||
347 | if (start_byte + (type_size * num_elements) > eeprom_size) {
|
||
348 | num_elements = (eeprom_size - start_byte) / type_size; |
||
349 | chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
|
||
350 | } |
||
351 | |||
352 | chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte); |
||
353 | |||
354 | // Work around, because stm32f1 cannot read a single byte
|
||
355 | if (type_size*num_elements < 2) |
||
356 | type_size = 2;
|
||
357 | |||
358 | uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements); |
||
359 | |||
360 | if (bytes_read != type_size*num_elements)
|
||
361 | chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
|
||
362 | |||
363 | for (unsigned int i = 0; i < num_elements; ++i) { |
||
364 | switch (type) {
|
||
365 | case HEX:
|
||
366 | chprintf(chp, "%02X ", buffer[i]);
|
||
367 | break;
|
||
368 | case U8:
|
||
369 | chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
|
||
370 | break;
|
||
371 | case U16:
|
||
372 | chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
|
||
373 | break;
|
||
374 | case U32:
|
||
375 | chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
|
||
376 | break;
|
||
377 | case S8:
|
||
378 | chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
|
||
379 | break;
|
||
380 | case S16:
|
||
381 | chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
|
||
382 | break;
|
||
383 | case S32:
|
||
384 | chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
|
||
385 | break;
|
||
386 | default:
|
||
387 | break;
|
||
388 | } |
||
389 | } |
||
390 | chprintf(chp, "\n");
|
||
391 | |||
392 | return;
|
||
393 | |||
394 | } |
||
395 | |||
396 | void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) { |
||
397 | chprintf(chp, "shellRequestCheck\n");
|
||
398 | boardPeripheryCheck(chp); |
||
399 | } |
||
400 | |||
401 | void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) { |
||
402 | chprintf(chp, "shellRequestGetRobotId\n");
|
||
403 | chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
|
||
404 | if (global.robot.getRobotID() == 0) |
||
405 | chprintf(chp, "Warning: The ID seems to be uninitialized. Is CAN communication working correctly?\n");
|
||
406 | } |
||
407 | |||
408 | void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
409 | chprintf(chp, "shellRequestGetSystemLoad\n");
|
||
410 | uint8_t seconds = 1;
|
||
411 | if (argc >= 1) { |
||
412 | seconds = atoi(argv[0]);
|
||
413 | } |
||
414 | chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second"); |
||
415 | |||
416 | const systime_t before = chThdGetTicks(chSysGetIdleThread());
|
||
417 | BaseThread::sleep(S2ST(seconds)); |
||
418 | const systime_t after = chThdGetTicks(chSysGetIdleThread());
|
||
419 | const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY)); |
||
420 | |||
421 | chprintf(chp, "CPU load: %3.2f%%\n", usage * 100); |
||
422 | const uint32_t memory_total = 0x10000; |
||
423 | const uint32_t memory_load = memory_total - chCoreStatus();
|
||
424 | chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total); |
||
425 | } |
||
426 | |||
427 | void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
428 | if (argc != 1) { |
||
429 | chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
|
||
430 | return;
|
||
431 | } |
||
432 | uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0])); |
||
433 | |||
434 | chprintf(chp, "shellSwitchBoardCmd\n");
|
||
435 | global.sercanmux1.sendSwitchCmd(boardIdx); |
||
436 | } |
||
437 | |||
438 | void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) { |
||
439 | // check the magic number
|
||
440 | 10687985 | Thomas Schöpping | switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
|
441 | case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)): |
||
442 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
443 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major, |
||
444 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor, |
||
445 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch); |
||
446 | break;
|
||
447 | |||
448 | case BL_MAGIC_NUMBER:
|
||
449 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
450 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
451 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
452 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))); |
||
453 | break;
|
||
454 | |||
455 | default:
|
||
456 | chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
|
||
457 | break;
|
||
458 | 58fe0e0b | Thomas Schöpping | } |
459 | |||
460 | return;
|
||
461 | } |
||
462 | |||
463 | static const ShellCommand commands[] = { |
||
464 | {"shutdown", shellRequestShutdown},
|
||
465 | {"check", shellRequestCheck},
|
||
466 | {"reset_memory", shellRequestResetMemory},
|
||
467 | {"get_board_id", shellRequestGetBoardId},
|
||
468 | {"set_board_id", shellRequestSetBoardId},
|
||
469 | {"get_memory_data", shellRequestGetMemoryData},
|
||
470 | {"get_robot_id", shellRequestGetRobotId},
|
||
471 | {"get_system_load", shellRequestGetSystemLoad},
|
||
472 | {"shell_board", shellSwitchBoardCmd},
|
||
473 | {"get_bootloader_info", shellRequestGetBootloaderInfo},
|
||
474 | {NULL, NULL} |
||
475 | }; |
||
476 | |||
477 | static const ShellConfig shell_cfg1 = { |
||
478 | (BaseSequentialStream *) &global.sercanmux1, |
||
479 | commands |
||
480 | }; |
||
481 | |||
482 | |||
483 | |||
484 | /*
|
||
485 | * Application entry point.
|
||
486 | */
|
||
487 | int main(void) { |
||
488 | |||
489 | Thread *shelltp = NULL;
|
||
490 | // global.shellTermID = CAN::LIGHT_RING_ID;
|
||
491 | /*
|
||
492 | * System initializations.
|
||
493 | * - HAL initialization, this also initializes the configured device drivers
|
||
494 | * and performs the board-specific initializations.
|
||
495 | * - Kernel initialization, the main() function becomes a thread and the
|
||
496 | * RTOS is active.
|
||
497 | */
|
||
498 | halInit(); |
||
499 | System::init(); |
||
500 | |||
501 | /*
|
||
502 | * Activates the serial driver 2 using the driver default configuration.
|
||
503 | */
|
||
504 | sdStart(&SD1, &global.sd1_config); |
||
505 | sdStart(&SD2, &global.sd2_config); |
||
506 | |||
507 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
508 | chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n"); |
||
509 | 10687985 | Thomas Schöpping | switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
|
510 | case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)): |
||
511 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
512 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major, |
||
513 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor, |
||
514 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch); |
||
515 | break;
|
||
516 | |||
517 | case BL_MAGIC_NUMBER:
|
||
518 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
519 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
520 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
521 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))); |
||
522 | break;
|
||
523 | |||
524 | default:
|
||
525 | chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
|
||
526 | break;
|
||
527 | 58fe0e0b | Thomas Schöpping | } |
528 | chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n"); |
||
529 | // make sure that the info text is completetly printed
|
||
530 | BaseThread::sleep(10);
|
||
531 | |||
532 | extStart(&EXTD1, &extcfg); |
||
533 | |||
534 | b4885314 | Thomas Schöpping | boardClearI2CBus(GPIOB_MEM_SCL, GPIOB_MEM_SDA); |
535 | 58fe0e0b | Thomas Schöpping | |
536 | global.HW_I2C2.start(&global.i2c2_config); |
||
537 | |||
538 | global.memory.init(); |
||
539 | |||
540 | uint8_t i = 0;
|
||
541 | if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
|
||
542 | chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
|
||
543 | } else {
|
||
544 | chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
|
||
545 | } |
||
546 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
547 | |||
548 | global.tlc5947.start(NORMALPRIO + 5);
|
||
549 | global.tlc5947.enable(); |
||
550 | |||
551 | global.robot.start(HIGHPRIO - 1);
|
||
552 | |||
553 | // lidar.start(NORMALPRIO + 15); UNCOMMENT TO START LIDAR
|
||
554 | |||
555 | global.lidar.start(NORMALPRIO); |
||
556 | |||
557 | global.userThread.start(NORMALPRIO); |
||
558 | |||
559 | /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
|
||
560 | palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_HIGH); |
||
561 | |||
562 | /* wait until all modules are done */
|
||
563 | while (palReadPad(GPIOD, GPIOD_SYS_INT_N) == PAL_LOW) {
|
||
564 | continue;
|
||
565 | } |
||
566 | |||
567 | global.robot.setLightBrightness(10);
|
||
568 | global.robot.setLightColor(0, Color::RED);
|
||
569 | global.robot.setLightColor(1, Color::LIME);
|
||
570 | global.robot.setLightColor(2, Color::BLUE);
|
||
571 | global.robot.setLightColor(3, Color::WHITE);
|
||
572 | global.robot.setLightColor(4, Color::RED);
|
||
573 | global.robot.setLightColor(5, Color::LIME);
|
||
574 | global.robot.setLightColor(6, Color::BLUE);
|
||
575 | global.robot.setLightColor(7, Color::WHITE);
|
||
576 | |||
577 | // static uint16_t scannedData[NUMBER_OF_STEPS /*see lidar.h for this variable*/] = {}; UNCOMMENT TO START LIDAR
|
||
578 | while (true) { |
||
579 | |||
580 | if (!shelltp)
|
||
581 | shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
|
||
582 | else if (chThdTerminated(shelltp)) { |
||
583 | chThdRelease(shelltp); /* Recovers memory of the previous shell. */
|
||
584 | shelltp = NULL; /* Triggers spawning of a new shell. */ |
||
585 | } |
||
586 | |||
587 | // testLights();
|
||
588 | // testColors();
|
||
589 | // testFade();
|
||
590 | // testBrightness();
|
||
591 | if (shutdown_now != SHUTDOWN_NONE) {
|
||
592 | 10687985 | Thomas Schöpping | if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER)) { |
593 | 58fe0e0b | Thomas Schöpping | chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
|
594 | shutdown_now = SHUTDOWN_NONE; |
||
595 | } else {
|
||
596 | uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR; |
||
597 | 10687985 | Thomas Schöpping | // handle bootloader version 0.2.x
|
598 | if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
|
||
599 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) { |
||
600 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
601 | case SHUTDOWN_TRANSPORTATION:
|
||
602 | blCallbackPtrAddr += 6 * 4; |
||
603 | break;
|
||
604 | case SHUTDOWN_DEEPSLEEP:
|
||
605 | blCallbackPtrAddr += 5 * 4; |
||
606 | break;
|
||
607 | case SHUTDOWN_HIBERNATE:
|
||
608 | blCallbackPtrAddr += 4 * 4; |
||
609 | break;
|
||
610 | case SHUTDOWN_HANDLE_REQUEST:
|
||
611 | case SHUTDOWN_RESTART:
|
||
612 | blCallbackPtrAddr += 10 * 4; |
||
613 | break;
|
||
614 | default:
|
||
615 | blCallbackPtrAddr = 0;
|
||
616 | break;
|
||
617 | } |
||
618 | 10687985 | Thomas Schöpping | } |
619 | // handle bootloader version 0.3.x
|
||
620 | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) && |
||
621 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) { |
||
622 | switch (shutdown_now) {
|
||
623 | case SHUTDOWN_TRANSPORTATION:
|
||
624 | blCallbackPtrAddr += 6 * 4; |
||
625 | break;
|
||
626 | case SHUTDOWN_DEEPSLEEP:
|
||
627 | blCallbackPtrAddr += 5 * 4; |
||
628 | break;
|
||
629 | case SHUTDOWN_HIBERNATE:
|
||
630 | blCallbackPtrAddr += 4 * 4; |
||
631 | break;
|
||
632 | case SHUTDOWN_RESTART:
|
||
633 | blCallbackPtrAddr += 7 * 4; |
||
634 | break;
|
||
635 | case SHUTDOWN_HANDLE_REQUEST:
|
||
636 | blCallbackPtrAddr += 8 * 4; |
||
637 | break;
|
||
638 | default:
|
||
639 | blCallbackPtrAddr = 0;
|
||
640 | break;
|
||
641 | } |
||
642 | } |
||
643 | 5b1b6715 | Thomas Schöpping | // handle bootloader version 1.0.x and 1.1.x
|
644 | 10687985 | Thomas Schöpping | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && |
645 | 5b1b6715 | Thomas Schöpping | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major == 1 && (((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 0 || ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 1)) { |
646 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
647 | case SHUTDOWN_TRANSPORTATION:
|
||
648 | blCallbackPtrAddr += 6 * 4; |
||
649 | break;
|
||
650 | case SHUTDOWN_DEEPSLEEP:
|
||
651 | blCallbackPtrAddr += 5 * 4; |
||
652 | break;
|
||
653 | case SHUTDOWN_HIBERNATE:
|
||
654 | blCallbackPtrAddr += 4 * 4; |
||
655 | break;
|
||
656 | case SHUTDOWN_RESTART:
|
||
657 | blCallbackPtrAddr += 7 * 4; |
||
658 | break;
|
||
659 | case SHUTDOWN_HANDLE_REQUEST:
|
||
660 | blCallbackPtrAddr += 8 * 4; |
||
661 | break;
|
||
662 | default:
|
||
663 | blCallbackPtrAddr = 0;
|
||
664 | break;
|
||
665 | } |
||
666 | } |
||
667 | |||
668 | void (*blCallback)(void) = NULL; |
||
669 | 10687985 | Thomas Schöpping | if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
|
670 | 58fe0e0b | Thomas Schöpping | blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr)); |
671 | |||
672 | if (!blCallback) {
|
||
673 | chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
|
||
674 | shutdown_now = SHUTDOWN_NONE; |
||
675 | } else {
|
||
676 | chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
|
||
677 | palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_LOW); |
||
678 | palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW); |
||
679 | |||
680 | chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
|
||
681 | systemShutdown(); |
||
682 | chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
|
||
683 | BaseThread::sleep(MS2ST(10)); // sleep to print everything |
||
684 | |||
685 | blCallback(); |
||
686 | } |
||
687 | |||
688 | } else {
|
||
689 | 10687985 | Thomas Schöpping | chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
|
690 | 58fe0e0b | Thomas Schöpping | shutdown_now = SHUTDOWN_NONE; |
691 | } |
||
692 | } |
||
693 | } |
||
694 | |||
695 | // // Print LIDAR scan
|
||
696 | // if (global.lidar.getScan(scannedData)) {
|
||
697 | // for (uint16_t idx = 0; idx < NUMBER_OF_STEPS; ++idx) {
|
||
698 | // chprintf((BaseSequentialStream*) &SD1, "%d ", scannedData[idx]);
|
||
699 | // }
|
||
700 | // chprintf((BaseSequentialStream*) &SD1, "\n\n");
|
||
701 | // }
|
||
702 | BaseThread::sleep(MS2ST(500));
|
||
703 | } |
||
704 | |||
705 | return 0; |
||
706 | } |