Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / main.cpp @ 12463563

History | View | Annotate | Download (37.2 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 <amiro/util/util.h>
14
#include <global.hpp>
15
#include <exti.hpp>
16

    
17
#include <chprintf.h>
18
#include <shell.h>
19

    
20
using namespace chibios_rt;
21

    
22
Global global;
23

    
24
struct blVersion_t {
25
  const uint8_t identifier;
26
  const uint8_t major;
27
  const uint8_t minor;
28
  const uint8_t patch;
29
} __attribute__((packed));
30

    
31
void systemShutdown() {
32
  types::kinematic k;
33
  uint8_t i;
34

    
35
//  // make sure we assert SYS_PD_N to delay shutdown until we're done.
36
//  boardRequestShutdown();
37

    
38
    // stop the user thread
39
  global.userThread.requestTerminate();
40
  global.userThread.wait();
41

    
42
  k.x = 0x00u;
43
  k.w_z = 0x00u;
44

    
45
  // stop wheels
46
  global.robot.setTargetSpeed(k);
47
  global.robot.terminate();
48

    
49
  for (i = 0x00; i < global.vcnl4020.size(); i++) {
50
    global.vcnl4020[i].requestTerminate();
51
    global.vcnl4020[i].wait();
52
  }
53

    
54
  global.ina219.requestTerminate();
55
  global.ina219.wait();
56
  global.hmc5883l.requestTerminate();
57
  global.hmc5883l.wait();
58
  global.l3g4200d.requestTerminate();
59
  global.l3g4200d.wait();
60

    
61
  global.motorcontrol.requestTerminate();
62
  global.motorcontrol.wait();
63
  global.odometry.requestTerminate();
64
  global.odometry.wait();
65

    
66
  // stop I²C
67
  for (i = 0; i < global.V_I2C2.size(); ++i)
68
    global.V_I2C2[i].stop();
69
  global.HW_I2C2.stop();
70

    
71
  global.lis331dlh.requestTerminate();
72
  global.lis331dlh.wait();
73

    
74
  global.lis331dlh.configure(&global.accel_sleep_config);
75
//  global.lis331dlh.start(NORMALPRIO +4);
76

    
77
//  boardWriteIoPower(0);
78
//  boardStandby();
79

    
80
  return;
81
}
82

    
83

    
84
//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);
85

    
86
void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) {
87

    
88
  chprintf(chp, "shellRequestShutdown\n");
89

    
90
  /* if nor argument was given, print some help text */
91
  if (argc == 0 || strcmp(argv[0], "help") == 0) {
92
    chprintf(chp, "\tUSAGE:\n");
93
    chprintf(chp, "> shutdown <type>\n");
94
    chprintf(chp, "\n");
95
    chprintf(chp, "\ttype\n");
96
    chprintf(chp, "The type of shutdown to perform.\n");
97
    chprintf(chp, "Choose one of the following types:\n");
98
    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
99
    chprintf(chp, "                   The robot can not be charged.\n");
100
    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
101
    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
102
    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
103
    chprintf(chp, "  restart        - Performs a system restart.\n");
104
    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
105
    chprintf(chp, "\n");
106
    return;
107
  }
108

    
109
  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
110
    shutdown_now = SHUTDOWN_TRANSPORTATION;
111
    chprintf(chp, "shutdown to transportation mode initialized\n");
112
  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
113
    shutdown_now = SHUTDOWN_DEEPSLEEP;
114
    chprintf(chp, "shutdown to deepsleep mode initialized\n");
115
  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
116
    shutdown_now = SHUTDOWN_HIBERNATE;
117
    chprintf(chp, "shutdown to hibernate mode initialized\n");
118
  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
119
    chprintf(chp, "restart initialized\n");
120
    shutdown_now = SHUTDOWN_RESTART;
121
  } else {
122
    chprintf(chp, "ERROR: unknown argument!\n");
123
    shutdown_now = SHUTDOWN_NONE;
124
  }
125

    
126
  return;
127
}
128

    
129
void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) {
130
  int i;
131
  chprintf(chp, "shellRequestWakeup\n");
132

    
133
  for (i = 0x00u; i < argc; i++)
134
    chprintf(chp, "%s\n", argv[i]);
135

    
136
  boardWakeup();
137
}
138

    
139
void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
140
  enum Type {HEX, U8, U16, U32, S8, S16, S32};
141

    
142
  chprintf(chp, "shellRequestReadData\n");
143

    
144
  if (argc < 2 || strcmp(argv[0],"help") == 0)
145
  {
146
    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
147
    chprintf(chp, "\n");
148
    chprintf(chp, "\ttype\n");
149
    chprintf(chp, "The data type as which to interpret the data.\n");
150
    chprintf(chp, "Choose one of the following types:\n");
151
    chprintf(chp, "  hex - one byte as hexadecimal value\n");
152
    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
153
    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
154
    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
155
    chprintf(chp, "  s8  - signed integer (8 bit)\n");
156
    chprintf(chp, "  s16 - signed integer (16 bit)\n");
157
    chprintf(chp, "  s32 - signed integer (32 bit)\n");
158
    chprintf(chp, "\tstart\n");
159
    chprintf(chp, "The first byte to read from the memory.\n");
160
    chprintf(chp, "\tcount [default = 1]\n");
161
    chprintf(chp, "The number of elements to read.\n");
162
    chprintf(chp, "\n");
163
    chprintf(chp, "\tNOTE\n");
164
    chprintf(chp, "Type conversions of this function might fail.\n");
165
    chprintf(chp, "If so, use type=hex and convert by hand.\n");
166
    chprintf(chp, "\n");
167
    return;
168
  }
169

    
170
  uint8_t type_size = 0;
171
  Type type = HEX;
172
  if (strcmp(argv[0],"hex") == 0) {
173
    type_size = sizeof(unsigned char);
174
    type = HEX;
175
  } else if(strcmp(argv[0],"u8") == 0) {
176
    type_size = sizeof(uint8_t);
177
    type = U8;
178
  } else if(strcmp(argv[0],"u16") == 0) {
179
    type_size = sizeof(uint16_t);
180
    type = U16;
181
  } else if(strcmp(argv[0],"u32") == 0) {
182
    type_size = sizeof(uint32_t);
183
    type = U32;
184
  } else if(strcmp(argv[0],"s8") == 0) {
185
    type_size = sizeof(int8_t);
186
    type = S8;
187
  } else if(strcmp(argv[0],"s16") == 0) {
188
    type_size = sizeof(int16_t);
189
    type = S16;
190
  } else if(strcmp(argv[0],"s32") == 0) {
191
    type_size = sizeof(int32_t);
192
    type = S32;
193
  } else {
194
    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
195
    return;
196
  }
197

    
198
  unsigned int start_byte = atoi(argv[1]);
199

    
200
  unsigned int num_elements = 1;
201
  if (argc >= 3)
202
    num_elements = atoi(argv[2]);
203

    
204
  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
205
  uint8_t buffer[eeprom_size];
206
  if (start_byte + (type_size * num_elements) > eeprom_size) {
207
    num_elements = (eeprom_size - start_byte) / type_size;
208
    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
209
  }
210

    
211
  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
212

    
213
  // Work around, because stm32f1 cannot read a single byte
214
  if (type_size*num_elements < 2)
215
    type_size = 2;
216

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

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

    
222
  for (unsigned int i = 0; i < num_elements; ++i) {
223
    switch (type) {
224
      case HEX:
225
        chprintf(chp, "%02X ", buffer[i]);
226
        break;
227
      case U8:
228
        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
229
        break;
230
      case U16:
231
        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
232
        break;
233
      case U32:
234
        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
235
        break;
236
      case S8:
237
        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
238
        break;
239
      case S16:
240
        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
241
        break;
242
      case S32:
243
        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
244
        break;
245
      default:
246
        break;
247
    }
248
  }
249
  chprintf(chp, "\n");
250

    
251
  return;
252
}
253

    
254
void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) {
255

    
256
  if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) {
257
    chprintf(chp, "\tUSAGE:\n");
258
    chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");
259
    chprintf(chp, "\n");
260
    chprintf(chp, "\tled mask\n");
261
    chprintf(chp, "The LEDs to be set.\n");
262
    chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");
263
    chprintf(chp, "  0x01 - rear left LED (SSW)\n");
264
    chprintf(chp, "  0x02 - left rear LED (WSW)\n");
265
    chprintf(chp, "  0x04 - left front LED (WNW)\n");
266
    chprintf(chp, "  0x08 - front left LED (NNW)\n");
267
    chprintf(chp, "  0x10 - front right LED (NNE)\n");
268
    chprintf(chp, "  0x20 - right front LED (ENE)\n");
269
    chprintf(chp, "  0x40 - right rear LED (ESE)\n");
270
    chprintf(chp, "  0x80 - rear right LED (SSE)\n");
271
    chprintf(chp, "\twhite/red\n");
272
    chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");
273
    chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");
274
    chprintf(chp, "\tgreen\n");
275
    chprintf(chp, "Sets the green color channel value.\n");
276
    chprintf(chp, "\tblue\n");
277
    chprintf(chp, "Sets the blue color channel value.\n");
278
    chprintf(chp, "\n");
279
    chprintf(chp, "\tExample:\n");
280
    chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");
281
    chprintf(chp, "> set_lights 0x66 0 255 255\n");
282
    chprintf(chp, "\n");
283
    return;
284
  }
285

    
286
  int arg_mask = strtol(argv[0], NULL, 0);
287
  int red = strtol(argv[1], NULL, 0);
288
  int green = red;
289
  int blue = red;
290
  if (argc >= 4) {
291
    green = strtol(argv[2], NULL, 0);
292
    blue = strtol(argv[3], NULL, 0);
293
  }
294
  Color color(red, green, blue);
295

    
296
  if (arg_mask & 0x01) {
297
    global.robot.setLightColor(constants::LightRing::LED_SSW, color);
298
  }
299
  if (arg_mask & 0x02) {
300
    global.robot.setLightColor(constants::LightRing::LED_WSW, color);
301
  }
302
  if (arg_mask & 0x04) {
303
    global.robot.setLightColor(constants::LightRing::LED_WNW, color);
304
  }
305
  if (arg_mask & 0x08) {
306
    global.robot.setLightColor(constants::LightRing::LED_NNW, color);
307
  }
308
  if (arg_mask & 0x10) {
309
    global.robot.setLightColor(constants::LightRing::LED_NNE, color);
310
  }
311
  if (arg_mask & 0x20) {
312
    global.robot.setLightColor(constants::LightRing::LED_ENE, color);
313
  }
314
  if (arg_mask & 0x40) {
315
    global.robot.setLightColor(constants::LightRing::LED_ESE, color);
316
  }
317
  if (arg_mask & 0x80) {
318
    global.robot.setLightColor(constants::LightRing::LED_SSE, color);
319
  }
320

    
321
  return;
322
}
323

    
324
void boardPeripheryCheck(BaseSequentialStream *chp) {
325
  msg_t result;
326
  chprintf(chp, "\nCHECK: START\n");
327
  // Check the accelerometer
328
  result = global.lis331dlh.getCheck();
329
  if (result == global.lis331dlh.CHECK_OK)
330
    chprintf(chp, "LIS331DLH: OK\n");
331
  else
332
    chprintf(chp, "LIS331DLH: FAIL\n");
333

    
334
  // Self-test accelerometer
335
//  lis331dlh.printSelfTest(NULL);
336

    
337
  // Check the eeprom
338
  result = global.memory.getCheck();
339
  if ( result != global.memory.OK)
340
    chprintf(chp, "Memory Structure: FAIL\n");
341
  else
342
    chprintf(chp, "Memory Structure: OK\n");
343

    
344
  // Check the gyroscope
345
  result = global.l3g4200d.getCheck();
346
  if (result == global.l3g4200d.CHECK_OK)
347
    chprintf(chp, "L3G4200D: OK\n");
348
  else
349
    chprintf(chp, "L3G4200D: FAIL\n");
350

    
351
  // Check the magnetometer
352
  result = global.hmc5883l.getCheck();
353
  if (result == global.hmc5883l.CHECK_OK)
354
    chprintf(chp, "HMC5883L: OK\n");
355
  else
356
    chprintf(chp, "HMC5883L: FAIL\n");
357

    
358
  // Check the MUX
359
  result = global.HW_PCA9544.getCheck();
360
  if (result == global.HW_PCA9544.CHECK_OK)
361
    chprintf(chp, "PCA9544: OK\n");
362
  else
363
    chprintf(chp, "PCA9544: FAIL\n");
364

    
365
  // Check the power monitor
366
  chprintf(chp, "INA219:\tVDD (3.3V):\n");
367
  result = global.ina219.selftest();
368
  if (result == BaseSensor<>::NOT_IMPLEMENTED)
369
    chprintf(chp, "->\tnot implemented\n");
370
  else if (result != INA219::Driver::ST_OK)
371
    chprintf(chp, "->\tFAIL (error code 0x%02X)\n", result);
372
  else
373
    chprintf(chp, "->\tOK\n");
374

    
375
  // Check the proximitysensors
376
  for (uint8_t i = 0x00; i < global.vcnl4020.size(); i++) {
377
    result = global.vcnl4020[i].getCheck();
378
    if (result == global.vcnl4020[i].CHECK_OK)
379
      chprintf(chp, "VCNL4020: %d OK\n", i);
380
    else
381
      chprintf(chp, "VCNL4020: %d FAIL\n", i);
382
  }
383
  chprintf(chp, "CHECK: FINISH\n");
384
}
385

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

    
391
void shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
392
  chprintf(chp, "shellRequestInitMemory\n");
393

    
394
  msg_t res = global.memory.resetMemory();
395
  if ( res != global.memory.OK)
396
    chprintf(chp, "Memory Init: FAIL\n");
397
  else
398
    chprintf(chp, "Memory Init: OK\n");
399
}
400

    
401
void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
402
  chprintf(chp, "shellRequestGetBoardId\n");
403
  uint8_t id = 0xFFu;
404

    
405
  msg_t res = global.memory.getBoardId(&id);
406

    
407
  if (res != global.memory.OK)
408
    chprintf(chp, "Get Board ID: FAIL\n");
409
  else
410
    chprintf(chp, "Get Board ID: %u\n", id);
411
}
412

    
413
void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
414
  chprintf(chp, "shellRequestSetBoardId\n");
415

    
416
  if (argc == 0) {
417
    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
418
  } else {
419
    msg_t res = global.memory.setBoardId(atoi(argv[0]));
420
    if (res != global.memory.OK)
421
      chprintf(chp, "Set Board ID: FAIL\n");
422
    else
423
      chprintf(chp, "Set Board ID: OK\n");
424
  }
425
}
426

    
427
void shellRequestResetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
428
  chprintf(chp, "shellRequestResetCalibrationConstants\n");
429
  chprintf(chp, "Setting Ed=1.0f, Eb=1.0f\n");
430
  msg_t res;
431

    
432
  res = global.memory.setEd(1.0f);
433
  if (res != global.memory.OK)
434
    chprintf(chp, "Set Ed: FAIL\n");
435
  else
436
    chprintf(chp, "Set Ed: OK\n");
437

    
438
  res = global.memory.setEb(1.0f);
439
  if (res != global.memory.OK)
440
    chprintf(chp, "Set Eb: FAIL\n");
441
  else
442
    chprintf(chp, "Set Eb: OK\n");
443
}
444

    
445
void shellRequestGetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
446
  chprintf(chp, "shellRequestGetCalibrationConstants\n");
447
  msg_t res;
448
  float Ed, Eb;
449

    
450
  res = global.memory.getEd(&Ed);
451
  if (res != global.memory.OK)
452
    chprintf(chp, "Get Ed: FAIL\n");
453
  else
454
    chprintf(chp, "Get Ed: OK \t Ed=%f\n", Ed);
455

    
456
  res = global.memory.getEb(&Eb);
457
  if (res != global.memory.OK)
458
    chprintf(chp, "Get Eb: FAIL\n");
459
  else
460
    chprintf(chp, "Get Eb: OK \t Eb=%f\n", Eb);
461
}
462

    
463
void shellRequestSetCalibrationConstants(BaseSequentialStream *chp, int argc, char *argv[]) {
464
  chprintf(chp, "shellRequestSetCalibrationConstants\n");
465
  msg_t res;
466

    
467
  if (argc != 3) {
468
    chprintf(chp, "Usage: %s\n","set_Ed_Eb <Ed> <Eb> <Write To Eeprom ? 1 : 0>");
469
    chprintf(chp, "(Call with floating point values for Ed and Eb values and write condition):\n");
470
    return;
471
  }
472
  // Get the write condition
473
  const float Ed = atof(argv[0]);
474
  const float Eb = atof(argv[1]);
475
  bool_t writeToMemory = atoi(argv[2]) == 1 ? true : false;
476

    
477
  res = global.motorcontrol.setWheelDiameterCorrectionFactor(Ed, writeToMemory);
478
  if (res != global.memory.OK)
479
    chprintf(chp, "Set Ed: FAIL\n");
480
  else
481
    chprintf(chp, "Set Ed: OK \t Ed=%f\n", Ed);
482

    
483
  res = global.motorcontrol.setActualWheelBaseDistance(Eb, writeToMemory);
484
  if (res != global.memory.OK)
485
    chprintf(chp, "Set Eb: FAIL\n");
486
  else
487
    chprintf(chp, "Set Eb: OK \t Ed=%f\n", Eb);
488
}
489

    
490
void shellRequestGetVcnl(BaseSequentialStream *chp, int argc, char *argv[]) {
491
  chprintf(chp, "shellRequestGetVcnl\n");
492
  // Print the sensor information
493
  if (argc != 1) {
494
    chprintf(chp, "Usage: %s\n","get_vcnl <rep>");
495
    return;
496
  }
497
  for (int32_t rep = 0x00; rep < atoi(argv[0]); ++rep) {
498
    for (uint8_t idx = 0x00; idx < global.vcnl4020.size(); idx++) {
499
     chprintf(chp, "%d: Ambi %d\tProx raw %d\tProx scaled %d\n", idx, global.vcnl4020[idx].getAmbientLight(), global.vcnl4020[idx].getProximity(), global.vcnl4020[idx].getProximityScaledWoOffset());
500
    }
501
    chprintf(chp, "\n\n");
502
    BaseThread::sleep(MS2ST(250));
503
  }
504
}
505

    
506
void shellRequestSetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
507
  chprintf(chp, "shellRequestSetVcnlOffset\n");
508
  if (argc != 2) {
509
    chprintf(chp, "Usage: %s\n","set_vcnl <idx> <offset>");
510
    return;
511
  }
512

    
513
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
514
  uint16_t vcnlOffset = static_cast<uint16_t>(atoi(argv[1]));
515

    
516
  if (vcnlIdx >= global.vcnl4020.size()) {
517
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
518
    return;
519
  }
520

    
521
  msg_t res = global.memory.setVcnl4020Offset(vcnlOffset, vcnlIdx);
522
  if (res != global.memory.OK) {
523
    chprintf(chp, "Set Offset: FAIL\n");
524
  } else {
525
    chprintf(chp, "Set Offset: OK\n");
526
    global.vcnl4020[vcnlIdx].setProximityOffset(vcnlOffset);
527
  }
528
}
529

    
530
void shellRequestResetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
531
  msg_t res = global.memory.OK;
532
  for (uint8_t idx = 0; idx < 4; ++idx) {
533
    msg_t r = global.memory.setVcnl4020Offset(0, idx);
534
    if (r == global.memory.OK) {
535
      global.vcnl4020[idx].setProximityOffset(0);
536
    } else {
537
      chprintf(chp, "Reset Offset %u: FAIL\n", idx);
538
      res = r;
539
    }
540
  }
541

    
542
  if (res == global.memory.OK) {
543
    chprintf(chp, "Reset Offset: DONE\n");
544
  }
545

    
546
  return;
547
}
548

    
549
void shellRequestGetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
550
  chprintf(chp, "shellRequestGetVcnlOffset\n");
551
  if (argc != 1) {
552
    chprintf(chp, "Call with decimal numbers: get_vcnl <idx>\n");
553
    return;
554
  }
555

    
556
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
557

    
558
  if (vcnlIdx >= global.vcnl4020.size()) {
559
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
560
    return;
561
  }
562

    
563
  uint16_t vcnlOffset;
564
  msg_t res = global.memory.getVcnl4020Offset(&vcnlOffset, vcnlIdx);
565
  if (res != global.memory.OK) {
566
    chprintf(chp, "Get Offset: FAIL\n");
567
  } else {
568
    chprintf(chp, "Get Offset: OK \t Offset=%d\n", vcnlOffset);
569
  }
570
}
571

    
572
void shellRequestCalib(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
573
  chprintf(chp, "shellRequestCalib\n");
574
  global.robot.calibrate();
575
}
576

    
577
void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
578
  chprintf(chp, "shellRequestGetRobotId\n");
579
  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
580
  if (global.robot.getRobotID() == 0)
581
    chprintf(chp, "Warning: The board ID seems to be uninitialized.\n");
582
}
583

    
584
void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
585
  chprintf(chp, "shellRequestGetSystemLoad\n");
586
  uint8_t seconds = 1;
587
  if (argc >= 1) {
588
    seconds = atoi(argv[0]);
589
  }
590
  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
591

    
592
  const systime_t before = chThdGetTicks(chSysGetIdleThread());
593
  BaseThread::sleep(S2ST(seconds));
594
  const systime_t after = chThdGetTicks(chSysGetIdleThread());
595
  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
596

    
597
  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
598
  const uint32_t memory_total = 0x10000;
599
  const uint32_t memory_load = memory_total - chCoreStatus();
600
  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
601
}
602

    
603
void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
604
  if (argc != 1) {
605
    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
606
    return;
607
  }
608
  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
609

    
610
  chprintf(chp, "shellSwitchBoardCmd\n");
611
  global.sercanmux1.sendSwitchCmd(boardIdx);
612
}
613

    
614
void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
615
  // check the magic number
616
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
617
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
618
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
619
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
620
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
621
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
622
      break;
623

    
624
    case BL_MAGIC_NUMBER:
625
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
626
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
627
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
628
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
629
      break;
630

    
631
    default:
632
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
633
      break;
634
  }
635

    
636
  return;
637
}
638

    
639
void shellRequestMotorDrive(BaseSequentialStream *chp, int argc, char *argv[]) {
640
  types::kinematic tmp;
641
  tmp.w_z = 0;
642
  tmp.x = 0;
643
  if (argc == 1){
644
    chprintf(chp, "Set speed to %i um/s \n", atoi(argv[0]));
645
    tmp.x = atoi(argv[0]);
646
  } else {
647
    if(argc == 2){
648
      chprintf(chp, "Set speed to %i \n um/s", atoi(argv[0]));
649
      chprintf(chp, "Set angular speed to %i \n urad/s", atoi(argv[1]));
650
      tmp.x = atoi(argv[0]);
651
      tmp.w_z= atoi(argv[1]);
652
    } else {
653
      chprintf(chp, "Wrong number of parameters given (%i), stopping robot \n", argc);
654
    }
655
  }
656

    
657
  global.motorcontrol.setTargetSpeed(tmp);
658
  return;
659
}
660

    
661
void shellRequestMotorStop(BaseSequentialStream *chp, int argc, char *argv[]) {
662
  types::kinematic tmp;
663
  tmp.x = 0;
664
  tmp.w_z = 0;
665

    
666
  global.motorcontrol.setTargetSpeed(tmp);
667

    
668
  chprintf(chp, "stop");
669
return;
670
}
671

    
672
void shellRequestMotorCalibrate(BaseSequentialStream *chp, int argc, char *argv[]) {
673
  global.motorcontrol.resetGains();
674
  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
675
  BaseThread::sleep(MS2ST(5000));
676
  global.motorcontrol.isCalibrating = true;
677

    
678
  return;
679
}
680

    
681
void shellRequestMotorGetGains(BaseSequentialStream *chp, int argc, char *argv[]){
682
  global.motorcontrol.printGains();
683

    
684
  return;
685
}
686

    
687
void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv[]) {
688
  global.motorcontrol.resetGains();;
689

    
690
  return;
691
}
692

    
693

    
694
/**
695
 * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
696
 * be able to detect the correction direction.
697
 * In this case it is expected that the FL-Sensor sould be in the white part of the edge and the FR-Sensor in the black one.
698
 * 
699
 * Note: invert the threshs to drive on the other edge.
700
 * 
701
 * */
702
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
703
    int vcnl4020AmbientLight[4];
704
    int vcnl4020Proximity[4];
705
    int rounds = 1;
706
    int proxyL = 0;
707
    int proxyR = 0;
708
    int maxDelta = 0;
709
 
710
  if (argc == 1){
711
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
712
    rounds = atoi(argv[0]);
713
    
714
  }else{
715
    chprintf(chp, "Usage: calbrate_line_sensors [1,n]\nThis will calibrate the thresholds for the left and right sensor\naccording to the maximum delta value recorded.\n");
716
    return;
717
  }
718

    
719

    
720
  for (int j = 0; j < rounds; j++) {
721
    for (int i = 0; i < 4; i++) {
722
        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
723
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
724
    }
725

    
726
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
727
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
728
    // Update proximity thresh
729
    if (delta > maxDelta) {
730
      maxDelta = delta;
731
      proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
732
      proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
733
    }
734

    
735
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
736
    //   delta *= -1;
737
    // }
738

    
739
    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", 
740
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
741
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
742
                  delta,
743
                  proxyL,
744
                  proxyR,
745
                  maxDelta);
746
    // sleep(CAN::UPDATE_PERIOD);
747
    BaseThread::sleep(CAN::UPDATE_PERIOD);
748
  }
749
  chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR);
750

    
751
  global.threshProxyL = proxyL;
752
  global.threshProxyR = proxyR;
753
  return;
754
}
755

    
756

    
757

    
758
void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
759
  int vcnl4020AmbientLight[4];
760
  int vcnl4020Proximity[4];
761
  int rounds = 1;
762
  int proxyL = global.threshProxyL;
763
  int proxyR = global.threshProxyR;
764
  int maxDelta = 0;
765
 
766
  if (argc == 1){
767
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
768
    rounds = atoi(argv[0]);
769
    
770
  }
771

    
772
  for (int j = 0; j < rounds; j++) {
773
    for (int i = 0; i < 4; i++) {
774
        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
775
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
776
    }
777

    
778
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
779
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
780
    // // Update proximity thresh
781
    // if (delta > maxDelta) {
782
    //   maxDelta = delta;
783
    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
784
    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
785
    // }
786

    
787
    if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
788
      delta *= -1;
789
    }
790

    
791
    chprintf(chp,"WL: %x, FL: %x, FR: %x, WR: %x, ProxyL: %x, ProxyR: %x, Delta: %d\n", 
792
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
793
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
794
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
795
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT],
796
                  proxyL,
797
                  proxyR,
798
                  delta);
799
    // sleep(CAN::UPDATE_PERIOD);
800
    BaseThread::sleep(CAN::UPDATE_PERIOD);
801
  }
802
  chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
803
  return;
804
}
805

    
806

    
807

    
808
static const ShellCommand commands[] = {
809
  {"shutdown", shellRequestShutdown},
810
  {"wakeup", shellRequestWakeup},
811
  {"check", shellRequestCheck},
812
  {"reset_memory", shellRequestResetMemory},
813
  {"get_board_id", shellRequestGetBoardId},
814
  {"set_board_id", shellRequestSetBoardId},
815
  {"get_memory_data", shellRequestGetMemoryData},
816
  {"get_vcnl", shellRequestGetVcnl},
817
  {"calib_vcnl_offset", shellRequestCalib},
818
  {"set_vcnl_offset", shellRequestSetVcnlOffset},
819
  {"reset_vcnl_offset", shellRequestResetVcnlOffset},
820
  {"get_vcnl_offset", shellRequestGetVcnlOffset},
821
  {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
822
  {"get_Ed_Eb", shellRequestGetCalibrationConstants},
823
  {"set_Ed_Eb", shellRequestSetCalibrationConstants},
824
  {"get_robot_id", shellRequestGetRobotId},
825
  {"get_system_load", shellRequestGetSystemLoad},
826
  {"set_lights", shellRequestSetLights},
827
  {"shell_board", shellSwitchBoardCmd},
828
  {"get_bootloader_info", shellRequestGetBootloaderInfo},
829
  {"motor_drive", shellRequestMotorDrive},
830
  {"motor_stop", shellRequestMotorStop},
831
  {"motor_calibrate", shellRequestMotorCalibrate},
832
  {"motor_getGains", shellRequestMotorGetGains},
833
  {"motor_resetGains", shellRequestMotorResetGains},
834
  {"dev_proxi_sensor_data", proxySensorData},
835
  {"calibrate_line", calibrateLineSensores},
836
  {NULL, NULL}
837
};
838

    
839
static const ShellConfig shell_cfg1 = {
840
  (BaseSequentialStream *) &global.sercanmux1,
841
  commands
842
};
843

    
844
void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA)
845
{
846
  INA219::CalibData calibData;
847
  INA219::InitData initData;
848

    
849
  calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V;
850
  calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV;
851
  calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us;
852
  calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us;
853
  calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous;
854
  calibData.input.shunt_resistance_O = shuntResistance_O;
855
  calibData.input.max_expected_current_A = maxExpectedCurrent_A;
856
  calibData.input.current_lsb_uA = currentLsb_uA;
857
  if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
858
  {
859
    chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
860
  }
861

    
862
  initData.configuration.value = calibData.input.configuration.value;
863
  initData.calibration = calibData.output.calibration_value;
864
  initData.current_lsb_uA = calibData.output.current_lsb_uA;
865
  if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
866
  {
867
    chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
868
  }
869

    
870
  if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
871
  {
872
    chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
873
  }
874

    
875
  return;
876
}
877

    
878
/*
879
 * Application entry point.
880
 */
881
int main(void) {
882

    
883
//  int16_t accel;
884
  Thread *shelltp = NULL;
885

    
886
  /*
887
   * System initializations.
888
   * - HAL initialization, this also initializes the configured device drivers
889
   *   and performs the board-specific initializations.
890
   * - Kernel initialization, the main() function becomes a thread and the
891
   *   RTOS is active.
892
   */
893
  halInit();
894
  qeiInit();
895
  System::init();
896

    
897
//  boardWakeup();
898
//  boardWriteIoPower(1);
899

    
900
  /*
901
   * Activates the serial driver 2 using the driver default configuration.
902
   */
903
  sdStart(&SD1, &global.sd1_config);
904

    
905
  chprintf((BaseSequentialStream*) &SD1, "\n");
906
  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
907
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
908
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
909
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
910
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
911
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
912
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
913
      break;
914

    
915
    case BL_MAGIC_NUMBER:
916
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
917
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
918
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
919
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
920
      break;
921

    
922
    default:
923
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
924
      break;
925
  }
926
  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
927
  // make sure that the info text is completetly printed
928
  BaseThread::sleep(10);
929

    
930
  extStart(&EXTD1, &extcfg);
931

    
932
  /*
933
   * Wait for a certain amount of time, so that the PowerBoard can activate
934
   * the IO voltages for the I2C Bus
935
   */
936
  BaseThread::sleep(MS2ST(2000));
937

    
938
  boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA);
939
  boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA);
940

    
941
  global.HW_I2C1.start(&global.i2c1_config);
942
  global.HW_I2C2.start(&global.i2c2_config);
943

    
944
  global.memory.init();
945

    
946
  uint8_t i = 0;
947
  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
948
    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
949
  } else {
950
    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
951
  }
952
  chprintf((BaseSequentialStream*) &SD1, "\n");
953

    
954
  initPowermonitor(global.ina219, 0.1f, 0.075f, 10);
955

    
956
  for (i = 0x00u; i < global.vcnl4020.size(); i++) {
957
    uint16_t buffer;
958
    global.memory.getVcnl4020Offset(&buffer,i);
959
    global.vcnl4020[i].setProximityOffset(buffer);
960
    global.vcnl4020[i].start(NORMALPRIO);
961
  }
962

    
963
  global.ina219.start(NORMALPRIO);
964

    
965
  global.hmc5883l.start(NORMALPRIO + 8);
966

    
967
  global.increments.start();  // Start the qei driver
968

    
969
  // Start the three axes gyroscope
970
  global.l3g4200d.configure(&global.gyro_run_config);
971
  global.l3g4200d.start(NORMALPRIO+5);
972

    
973
  global.odometry.start(NORMALPRIO + 20);
974

    
975
  global.robot.start(HIGHPRIO - 1);
976

    
977
  global.motorcontrol.start(NORMALPRIO + 7);
978

    
979
  global.distcontrol.start(NORMALPRIO + 9);
980

    
981
  // Set target velocity
982
  types::kinematic velocity;
983
  velocity.x = 0; // E.g.  "100*1e3" equals "10 cm/s"
984
  velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s"
985
  global.motorcontrol.setTargetSpeed(velocity);
986

    
987
  // Start the three axes linear accelerometer
988
  global.lis331dlh.configure(&global.accel_run_config);
989
  global.lis331dlh.start(NORMALPRIO+4);
990

    
991
  // Start the user thread
992
  global.userThread.start(NORMALPRIO);
993

    
994
  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
995
  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
996

    
997
  /* wait until all modules are done */
998
  while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
999
    continue;
1000
  }
1001

    
1002
  while (true) {
1003

    
1004
    if (!shelltp)
1005
      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
1006
    else if (chThdTerminated(shelltp)) {
1007
      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
1008
      shelltp = NULL;           /* Triggers spawning of a new shell.      */
1009
    }
1010

    
1011
    // Let the LED just blink as an alive signal
1012
    boardWriteLed(1);
1013
    BaseThread::sleep(MS2ST(250));
1014
    boardWriteLed(0);
1015
    BaseThread::sleep(MS2ST(250));
1016

    
1017
    if (shutdown_now != SHUTDOWN_NONE) {
1018
      if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER)) {
1019
        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
1020
        shutdown_now = SHUTDOWN_NONE;
1021
      } else {
1022
        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
1023
        // handle bootloader version 0.2.x
1024
        if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1025
            (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) {
1026
          switch (shutdown_now) {
1027
            case SHUTDOWN_TRANSPORTATION:
1028
              blCallbackPtrAddr += 6 * 4;
1029
              break;
1030
            case SHUTDOWN_DEEPSLEEP:
1031
              blCallbackPtrAddr += 5 * 4;
1032
              break;
1033
            case SHUTDOWN_HIBERNATE:
1034
              blCallbackPtrAddr += 4 * 4;
1035
              break;
1036
            case SHUTDOWN_HANDLE_REQUEST:
1037
            case SHUTDOWN_RESTART:
1038
              blCallbackPtrAddr += 10 * 4;
1039
              break;
1040
            default:
1041
              blCallbackPtrAddr = 0;
1042
              break;
1043
          }
1044
        }
1045
        // handle bootloader version 0.3.x
1046
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1047
                 (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) {
1048
          switch (shutdown_now) {
1049
            case SHUTDOWN_TRANSPORTATION:
1050
              blCallbackPtrAddr += 6 * 4;
1051
              break;
1052
            case SHUTDOWN_DEEPSLEEP:
1053
              blCallbackPtrAddr += 5 * 4;
1054
              break;
1055
            case SHUTDOWN_HIBERNATE:
1056
              blCallbackPtrAddr += 4 * 4;
1057
              break;
1058
            case SHUTDOWN_RESTART:
1059
              blCallbackPtrAddr += 7 * 4;
1060
              break;
1061
            case SHUTDOWN_HANDLE_REQUEST:
1062
              blCallbackPtrAddr += 8 * 4;
1063
              break;
1064
            default:
1065
              blCallbackPtrAddr = 0;
1066
              break;
1067
          }
1068
        }
1069
        // handle bootloader version 1.0.x and 1.1.x
1070
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) &&
1071
                 ((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)) {
1072
          switch (shutdown_now) {
1073
            case SHUTDOWN_TRANSPORTATION:
1074
              blCallbackPtrAddr += 6 * 4;
1075
              break;
1076
            case SHUTDOWN_DEEPSLEEP:
1077
              blCallbackPtrAddr += 5 * 4;
1078
              break;
1079
            case SHUTDOWN_HIBERNATE:
1080
              blCallbackPtrAddr += 4 * 4;
1081
              break;
1082
            case SHUTDOWN_RESTART:
1083
              blCallbackPtrAddr += 7 * 4;
1084
              break;
1085
            case SHUTDOWN_HANDLE_REQUEST:
1086
              blCallbackPtrAddr += 8 * 4;
1087
              break;
1088
            default:
1089
              blCallbackPtrAddr = 0;
1090
              break;
1091
          }
1092
        }
1093

    
1094
        void (*blCallback)(void) = NULL;
1095
        if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
1096
          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
1097

    
1098
          if (!blCallback) {
1099
            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
1100
            shutdown_now = SHUTDOWN_NONE;
1101
          } else {
1102
            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
1103
            palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW);
1104
            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
1105

    
1106
            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
1107
            systemShutdown();
1108
            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
1109
            BaseThread::sleep(MS2ST(10)); // sleep to print everything
1110

    
1111
            blCallback();
1112
          }
1113

    
1114
        } else {
1115
          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
1116
          shutdown_now = SHUTDOWN_NONE;
1117
        }
1118
      }
1119

    
1120
//    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
1121
//        accel = lis331dlh.getAcceleration(i);
1122
//        chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
1123
//    }
1124
//
1125
//    chprintf((BaseSequentialStream*) &SD1, "\n");
1126
//
1127
//    // Print out an alive signal
1128
//    chprintf((BaseSequentialStream*) &SD1, ".");
1129
    }
1130
  }
1131

    
1132
  return 0;
1133
}