Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / LightRing / main.cpp @ 58fe0e0b

History | View | Annotate | Download (20.8 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
void shellRequestShutdown(BaseSequentialStream* chp, int argc, char *argv[]) {
27

    
28
  chprintf(chp, "shellRequestShutdown\n");
29

    
30
  /* if nor argument was given, print some help text */
31
  if (argc == 0 || strcmp(argv[0], "help") == 0) {
32
    chprintf(chp, "\tUSAGE:\n");
33
    chprintf(chp, "> shutdown <type>\n");
34
    chprintf(chp, "\n");
35
    chprintf(chp, "\ttype\n");
36
    chprintf(chp, "The type of shutdown to perform.\n");
37
    chprintf(chp, "Choose one of the following types:\n");
38
    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
39
    chprintf(chp, "                   The robot can not be charged.\n");
40
    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
41
    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
42
    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
43
    chprintf(chp, "  restart        - Performs a system restart.\n");
44
    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
45
    chprintf(chp, "\n");
46
    return;
47
  }
48

    
49
  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
50
    shutdown_now = SHUTDOWN_TRANSPORTATION;
51
    chprintf(chp, "shutdown to transportation mode initialized\n");
52
  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
53
    shutdown_now = SHUTDOWN_DEEPSLEEP;
54
    chprintf(chp, "shutdown to deepsleep mode initialized\n");
55
  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
56
    shutdown_now = SHUTDOWN_HIBERNATE;
57
    chprintf(chp, "shutdown to hibernate mode initialized\n");
58
  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
59
    chprintf(chp, "restart initialized\n");
60
    shutdown_now = SHUTDOWN_RESTART;
61
  } else {
62
    chprintf(chp, "ERROR: unknown argument!\n");
63
    shutdown_now = SHUTDOWN_NONE;
64
  }
65

    
66
  return;
67
}
68

    
69
void testLights() {
70
  for (int i = 0; i < (8 * 16 * 2); i++) {
71
    Color color;
72
    int brightness;
73

    
74
    switch (((i / 16) / 2) % 8) {
75
      case 0:
76
        color = Color::WHITE;
77
        break;
78
      case 1:
79
        color = Color::RED;
80
        break;
81
      case 2:
82
        color = Color::FUCHSIA;
83
        break;
84
      case 3:
85
        color = Color::BLUE;
86
        break;
87
      case 4:
88
        color = Color::AQUA;
89
        break;
90
      case 5:
91
        color = Color::LIME;
92
        break;
93
      case 6:
94
        color = Color::YELLOW;
95
        break;
96
      case 7:
97
      default:
98
        color = Color::BLACK;
99
        break;
100
    }
101

    
102
    if (i & (1 << 4))
103
      brightness = 10 + (i % 16) * 6;
104
    else
105
      brightness = 10;
106

    
107
    global.robot.setLightBrightness(brightness);
108
    global.robot.setLightColor((i / 2) % 8, color);
109

    
110
    BaseThread::sleep(MS2ST(250));
111
  }
112
}
113

    
114
void testColors() {
115
  Color color;
116

    
117
  global.robot.setLightBrightness(50);
118

    
119
  for (int i = 0; i < Color::YELLOWGREEN; i++) {
120
    color = Color(static_cast<Color::GlobalColor>(i));
121
    for (int j = 0; j < 8; j++)
122
      global.robot.setLightColor(j, color);
123

    
124
    BaseThread::sleep(MS2ST(250));
125
  }
126

    
127
}
128

    
129
void testFade() {
130
  for (int i = 0; i < 255; i += 16) {
131
    Color color(255 - i, i, 0);
132
    for (int j = 0; j < 8; j++) {
133
      global.robot.setLightColor(j, color);
134
      BaseThread::sleep(MS2ST(250));
135
    }
136
  }
137
}
138

    
139
void testBrightness() {
140
  for (int j = 0; j < 8; j++)
141
    global.robot.setLightColor(j, Color::WHITE);
142

    
143
  for (int i = 0; i < 200; i += 5) {
144
    int brightness = (i > 100) ? 200 - i : i;
145
    global.robot.setLightBrightness(brightness);
146
    BaseThread::sleep(MS2ST(100));
147
  }
148
}
149

    
150
static msg_t shutdownRequest(void *arg) {
151

    
152
  (void) arg;
153
  const size_t max_str_len = strlen("shutdown");
154
  uint8_t buffer[20];
155
  uint8_t num_bytes = 0x00u;
156
  uint8_t consume;
157

    
158
  while (true) {
159

    
160
    num_bytes += sdAsynchronousRead(&SD1, &buffer[num_bytes], sizeof(buffer) - num_bytes);
161

    
162
    if (num_bytes) {
163

    
164
      consume = 0x01u;
165

    
166
      if (num_bytes >= strlen("shutdown") && !strncmp((char*) buffer, "shutdown", strlen("shutdown"))) {
167

    
168
        boardRequestShutdown();
169
        consume = strlen("shutdown");
170

    
171
      }
172

    
173
      if (consume != 0x01 || num_bytes >= max_str_len) {
174

    
175
        num_bytes -= consume;
176
        memcpy(&buffer[0], &buffer[consume], num_bytes);
177

    
178
      }
179

    
180
    }
181

    
182
    BaseThread::sleep(MS2ST(1000));
183

    
184
  }
185

    
186
  return RDY_OK;
187

    
188
}
189

    
190
void systemShutdown() {
191

    
192
  global.userThread.requestTerminate();
193
  global.userThread.wait();
194

    
195
  global.robot.setLightBrightness(0);
196
  global.robot.terminate();
197

    
198
  global.tlc5947.requestTerminate();
199
  global.tlc5947.update();
200
  global.tlc5947.wait();
201

    
202
  global.lidar.requestTerminate();
203
  global.lidar.wait();
204

    
205
//  boardStandby();
206

    
207
  return;
208
}
209

    
210
void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
211
  chprintf(chp, "shellRequestInitMemory\n");
212

    
213
  msg_t res = global.memory.resetMemory();
214
  if ( res != global.memory.OK)
215
    chprintf(chp, "Memory Init: FAIL\n");
216
  else
217
    chprintf(chp, "Memory Init: OK\n");
218
}
219

    
220
void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
221
  chprintf(chp, "shellRequestGetBoardId\n");
222

    
223
  uint8_t id = 0xFFu;
224
  msg_t res = global.memory.getBoardId(&id);
225
  if (res != global.memory.OK)
226
    chprintf(chp, "Get Board ID: FAIL\n");
227
  else
228
    chprintf(chp, "Get Board ID: %u\n", id);
229
}
230

    
231
void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
232
  chprintf(chp, "shellRequestSetBoardId\n");
233

    
234
  if (argc == 0) {
235
    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
236
  } else {
237
    msg_t res = global.memory.setBoardId(atoi(argv[0]));
238
    if (res != global.memory.OK)
239
      chprintf(chp, "Set Board ID: FAIL\n");
240
    else
241
      chprintf(chp, "Set Board ID: OK\n");
242
  }
243
}
244

    
245
void boardPeripheryCheck(BaseSequentialStream *chp) {
246
  msg_t result;
247
  chprintf(chp, "\nCHECK: START\n");
248

    
249
  // Check the radio
250
  result = global.a2500r24a.getCheck();
251
  if (result == global.a2500r24a.CHECK_OK)
252
    chprintf(chp, "A2500R24A: OK\n");
253
  else
254
    chprintf(chp, "A2500R24A: FAIL\n");
255

    
256
  // Check the eeprom
257
  result = global.memory.getCheck();
258
  if ( result != global.memory.OK)
259
    chprintf(chp, "Memory Structure: FAIL\n");
260
  else
261
    chprintf(chp, "Memory Structure: OK\n");
262

    
263
  // Check the lights
264
  chprintf(chp, "LEDs: Test colors\n");
265
  testColors();
266
  chprintf(chp, "LEDs: Off\n");
267
  global.robot.setLightBrightness(0);
268

    
269
  chprintf(chp, "CHECK: FINISH\n");
270
}
271

    
272
void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
273

    
274
  enum Type {HEX, U8, U16, U32, S8, S16, S32};
275

    
276
  chprintf(chp, "shellRequestReadData\n");
277

    
278
  if (argc < 2 || strcmp(argv[0],"help") == 0)
279
  {
280
    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
281
    chprintf(chp, "\n");
282
    chprintf(chp, "\ttype\n");
283
    chprintf(chp, "The data type as which to interpret the data.\n");
284
    chprintf(chp, "Choose one of the following types:\n");
285
    chprintf(chp, "  hex - one byte as hexadecimal value\n");
286
    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
287
    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
288
    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
289
    chprintf(chp, "  s8  - signed integer (8 bit)\n");
290
    chprintf(chp, "  s16 - signed integer (16 bit)\n");
291
    chprintf(chp, "  s32 - signed integer (32 bit)\n");
292
    chprintf(chp, "\tstart\n");
293
    chprintf(chp, "The first byte to read from the memory.\n");
294
    chprintf(chp, "\tcount [default = 1]\n");
295
    chprintf(chp, "The number of elements to read.\n");
296
    chprintf(chp, "\n");
297
    chprintf(chp, "\tNOTE\n");
298
    chprintf(chp, "Type conversions of this function might fail.\n");
299
    chprintf(chp, "If so, use type=hex and convert by hand.\n");
300
    chprintf(chp, "\n");
301
    return;
302
  }
303

    
304
  uint8_t type_size = 0;
305
  Type type = HEX;
306
  if (strcmp(argv[0],"hex") == 0) {
307
    type_size = sizeof(unsigned char);
308
    type = HEX;
309
  } else if(strcmp(argv[0],"u8") == 0) {
310
    type_size = sizeof(uint8_t);
311
    type = U8;
312
  } else if(strcmp(argv[0],"u16") == 0) {
313
    type_size = sizeof(uint16_t);
314
    type = U16;
315
  } else if(strcmp(argv[0],"u32") == 0) {
316
    type_size = sizeof(uint32_t);
317
    type = U32;
318
  } else if(strcmp(argv[0],"s8") == 0) {
319
    type_size = sizeof(int8_t);
320
    type = S8;
321
  } else if(strcmp(argv[0],"s16") == 0) {
322
    type_size = sizeof(int16_t);
323
    type = S16;
324
  } else if(strcmp(argv[0],"s32") == 0) {
325
    type_size = sizeof(int32_t);
326
    type = S32;
327
  } else {
328
    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
329
    return;
330
  }
331

    
332
  unsigned int start_byte = atoi(argv[1]);
333

    
334
  unsigned int num_elements = 1;
335
  if (argc >= 3)
336
    num_elements = atoi(argv[2]);
337

    
338
  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
339
  uint8_t buffer[eeprom_size];
340
  if (start_byte + (type_size * num_elements) > eeprom_size) {
341
    num_elements = (eeprom_size - start_byte) / type_size;
342
    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
343
  }
344

    
345
  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
346

    
347
  // Work around, because stm32f1 cannot read a single byte
348
  if (type_size*num_elements < 2)
349
    type_size = 2;
350

    
351
  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
352

    
353
  if (bytes_read != type_size*num_elements)
354
    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
355

    
356
  for (unsigned int i = 0; i < num_elements; ++i) {
357
    switch (type) {
358
      case HEX:
359
        chprintf(chp, "%02X ", buffer[i]);
360
        break;
361
      case U8:
362
        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
363
        break;
364
      case U16:
365
        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
366
        break;
367
      case U32:
368
        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
369
        break;
370
      case S8:
371
        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
372
        break;
373
      case S16:
374
        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
375
        break;
376
      case S32:
377
        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
378
        break;
379
      default:
380
        break;
381
    }
382
  }
383
  chprintf(chp, "\n");
384

    
385
  return;
386

    
387
}
388

    
389
void shellRequestCheck(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
390
  chprintf(chp, "shellRequestCheck\n");
391
  boardPeripheryCheck(chp);
392
}
393

    
394
void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
395
  chprintf(chp, "shellRequestGetRobotId\n");
396
  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
397
  if (global.robot.getRobotID() == 0)
398
    chprintf(chp, "Warning: The ID seems to be uninitialized. Is CAN communication working correctly?\n");
399
}
400

    
401
void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
402
  chprintf(chp, "shellRequestGetSystemLoad\n");
403
  uint8_t seconds = 1;
404
  if (argc >= 1) {
405
    seconds = atoi(argv[0]);
406
  }
407
  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
408

    
409
  const systime_t before = chThdGetTicks(chSysGetIdleThread());
410
  BaseThread::sleep(S2ST(seconds));
411
  const systime_t after = chThdGetTicks(chSysGetIdleThread());
412
  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
413

    
414
  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
415
  const uint32_t memory_total = 0x10000;
416
  const uint32_t memory_load = memory_total - chCoreStatus();
417
  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
418
}
419

    
420
void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
421
  if (argc != 1) {
422
    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
423
    return;
424
  }
425
  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
426

    
427
  chprintf(chp, "shellSwitchBoardCmd\n");
428
  global.sercanmux1.sendSwitchCmd(boardIdx);
429
}
430

    
431
void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
432
  // check the magic number
433
  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
434
    chprintf(chp, "Bootloader version:  %u.%u.%u\n",
435
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
436
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
437
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
438
    chprintf(chp, "Callback functions:\n");if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
439
      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
440
      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
441
      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
442
      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (10*4))) ? "available" : "unsupported");
443
    } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
444
      chprintf(chp, "\thibernate:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (4*4))) ? "available" : "unsupported");
445
      chprintf(chp, "\tdeepsleep:      %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (5*4))) ? "available" : "unsupported");
446
      chprintf(chp, "\ttransportation: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (6*4))) ? "available" : "unsupported");
447
      chprintf(chp, "\trestart:        %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (7*4))) ? "available" : "unsupported");
448
      chprintf(chp, "\thandle request: %s\n", *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (8*4))) ? "available" : "unsupported");
449
    }
450
  } else {
451
    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
452
  }
453

    
454
  return;
455
}
456

    
457
static const ShellCommand commands[] = {
458
  {"shutdown", shellRequestShutdown},
459
  {"check", shellRequestCheck},
460
  {"reset_memory", shellRequestResetMemory},
461
  {"get_board_id", shellRequestGetBoardId},
462
  {"set_board_id", shellRequestSetBoardId},
463
  {"get_memory_data", shellRequestGetMemoryData},
464
  {"get_robot_id", shellRequestGetRobotId},
465
  {"get_system_load", shellRequestGetSystemLoad},
466
  {"shell_board", shellSwitchBoardCmd},
467
  {"get_bootloader_info", shellRequestGetBootloaderInfo},
468
  {NULL, NULL}
469
};
470

    
471
static const ShellConfig shell_cfg1 = {
472
  (BaseSequentialStream *) &global.sercanmux1,
473
  commands
474
};
475

    
476

    
477

    
478
/*
479
 * Application entry point.
480
 */
481
int main(void) {
482

    
483
  Thread *shelltp = NULL;
484
//  global.shellTermID = CAN::LIGHT_RING_ID;
485
  /*
486
   * System initializations.
487
   * - HAL initialization, this also initializes the configured device drivers
488
   *   and performs the board-specific initializations.
489
   * - Kernel initialization, the main() function becomes a thread and the
490
   *   RTOS is active.
491
   */
492
  halInit();
493
  System::init();
494

    
495
  /*
496
   * Activates the serial driver 2 using the driver default configuration.
497
   */
498
  sdStart(&SD1, &global.sd1_config);
499
  sdStart(&SD2, &global.sd2_config);
500

    
501
  chprintf((BaseSequentialStream*) &SD1, "\n");
502
  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
503
  if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) {
504
    chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
505
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
506
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))),
507
             *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (3*4))));
508
  } else {
509
    chprintf((BaseSequentialStream*) &SD1, "Bootloader deprecated\n");
510
  }
511
  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
512
  // make sure that the info text is completetly printed
513
  BaseThread::sleep(10);
514

    
515
  extStart(&EXTD1, &extcfg);
516

    
517
  boardClearI2CBus(GPIOB_MEM_SCL);
518

    
519
  global.HW_I2C2.start(&global.i2c2_config);
520

    
521
  global.memory.init();
522

    
523
  uint8_t i = 0;
524
  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
525
    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
526
  } else {
527
    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
528
  }
529
  chprintf((BaseSequentialStream*) &SD1, "\n");
530

    
531
  global.tlc5947.start(NORMALPRIO + 5);
532
  global.tlc5947.enable();
533

    
534
  global.robot.start(HIGHPRIO - 1);
535

    
536
//   lidar.start(NORMALPRIO + 15); UNCOMMENT TO START LIDAR
537

    
538
  global.lidar.start(NORMALPRIO);
539

    
540
  global.userThread.start(NORMALPRIO);
541

    
542
  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
543
  palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_HIGH);
544

    
545
  /* wait until all modules are done */
546
  while (palReadPad(GPIOD, GPIOD_SYS_INT_N) == PAL_LOW) {
547
    continue;
548
  }
549

    
550
  global.robot.setLightBrightness(10);
551
  global.robot.setLightColor(0, Color::RED);
552
  global.robot.setLightColor(1, Color::LIME);
553
  global.robot.setLightColor(2, Color::BLUE);
554
  global.robot.setLightColor(3, Color::WHITE);
555
  global.robot.setLightColor(4, Color::RED);
556
  global.robot.setLightColor(5, Color::LIME);
557
  global.robot.setLightColor(6, Color::BLUE);
558
  global.robot.setLightColor(7, Color::WHITE);
559

    
560
//   static uint16_t scannedData[NUMBER_OF_STEPS /*see lidar.h for this variable*/] = {}; UNCOMMENT TO START LIDAR
561
  while (true) {
562

    
563
    if (!shelltp)
564
      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
565
    else if (chThdTerminated(shelltp)) {
566
      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
567
      shelltp = NULL;           /* Triggers spawning of a new shell.      */
568
    }
569

    
570
//    testLights();
571
//    testColors();
572
//    testFade();
573
//    testBrightness();
574
    if (shutdown_now != SHUTDOWN_NONE) {
575
      if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER) {
576
        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
577
        shutdown_now = SHUTDOWN_NONE;
578
      } else {
579
        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
580
        if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2) {
581
          switch (shutdown_now) {
582
            case SHUTDOWN_TRANSPORTATION:
583
              blCallbackPtrAddr += 6 * 4;
584
              break;
585
            case SHUTDOWN_DEEPSLEEP:
586
              blCallbackPtrAddr += 5 * 4;
587
              break;
588
            case SHUTDOWN_HIBERNATE:
589
              blCallbackPtrAddr += 4 * 4;
590
              break;
591
            case SHUTDOWN_HANDLE_REQUEST:
592
            case SHUTDOWN_RESTART:
593
              blCallbackPtrAddr += 10 * 4;
594
              break;
595
            default:
596
              blCallbackPtrAddr = 0;
597
              break;
598
          }
599
        } else if (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3) {
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_RESTART:
611
              blCallbackPtrAddr += 7 * 4;
612
              break;
613
            case SHUTDOWN_HANDLE_REQUEST:
614
              blCallbackPtrAddr += 8 * 4;
615
              break;
616
            default:
617
              blCallbackPtrAddr = 0;
618
              break;
619
          }
620
        }
621

    
622
        void (*blCallback)(void) = NULL;
623
        if (blCallbackPtrAddr) {
624
          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
625

    
626
          if (!blCallback) {
627
            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
628
            shutdown_now = SHUTDOWN_NONE;
629
          } else {
630
            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
631
            palWritePad(GPIOD, GPIOD_SYS_INT_N, PAL_LOW);
632
            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
633

    
634
            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
635
            systemShutdown();
636
            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
637
            BaseThread::sleep(MS2ST(10)); // sleep to print everything
638

    
639
            blCallback();
640
          }
641

    
642
        } else {
643
          chprintf((BaseSequentialStream*)&SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
644
          shutdown_now = SHUTDOWN_NONE;
645
        }
646
      }
647
    }
648

    
649
//     // Print LIDAR scan
650
//     if (global.lidar.getScan(scannedData)) {
651
//       for (uint16_t idx = 0; idx < NUMBER_OF_STEPS; ++idx) {
652
//         chprintf((BaseSequentialStream*) &SD1, "%d ", scannedData[idx]);
653
//       }
654
//       chprintf((BaseSequentialStream*) &SD1, "\n\n");
655
//     }
656
    BaseThread::sleep(MS2ST(500));
657
  }
658

    
659
  return 0;
660
}