Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / LightRing / main.cpp @ 10687985

History | View | Annotate | Download (21.7 KB)

1
#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
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
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
  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
  }
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
  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
  }
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
  boardClearI2CBus(GPIOB_MEM_SCL, GPIOB_MEM_SDA);
535

    
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
      if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER)) {
593
        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
        // 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
          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
        }
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
        // handle bootloader version 1.0.x
644
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) &&
645
                 ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major == 1 && ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 0) {
646
          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
        if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
670
          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
          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
690
          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
}