Statistics
| Branch: | Tag: | Revision:

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

History | View | Annotate | Download (46.398 KB)

1 b8b3a9c9 galberding
#include <cstdint>
2 58fe0e0b Thomas Schöpping
#define BL_CALLBACK_TABLE_ADDR  (0x08000000 + 0x01C0)
3
#define BL_MAGIC_NUMBER         ((uint32_t)0xFF669900u)
4
5
#define SHUTDOWN_NONE             0
6
#define SHUTDOWN_TRANSPORTATION   1
7
#define SHUTDOWN_DEEPSLEEP        2
8
#define SHUTDOWN_HIBERNATE        3
9
#define SHUTDOWN_RESTART          4
10
#define SHUTDOWN_HANDLE_REQUEST   5
11
12
#include <ch.hpp>
13
14
#include <amiro/util/util.h>
15
#include <global.hpp>
16
#include <exti.hpp>
17
18
#include <chprintf.h>
19
#include <shell.h>
20
21 b8b3a9c9 galberding
#include "linefollow.hpp"
22
#include "amiro_map.hpp"
23 58fe0e0b Thomas Schöpping
using namespace chibios_rt;
24
25
Global global;
26
27 10687985 Thomas Schöpping
struct blVersion_t {
28
  const uint8_t identifier;
29
  const uint8_t major;
30
  const uint8_t minor;
31
  const uint8_t patch;
32
} __attribute__((packed));
33
34 58fe0e0b Thomas Schöpping
void systemShutdown() {
35
  types::kinematic k;
36
  uint8_t i;
37
38
//  // make sure we assert SYS_PD_N to delay shutdown until we're done.
39
//  boardRequestShutdown();
40
41
    // stop the user thread
42
  global.userThread.requestTerminate();
43
  global.userThread.wait();
44
45
  k.x = 0x00u;
46
  k.w_z = 0x00u;
47
48
  // stop wheels
49
  global.robot.setTargetSpeed(k);
50
  global.robot.terminate();
51
52
  for (i = 0x00; i < global.vcnl4020.size(); i++) {
53
    global.vcnl4020[i].requestTerminate();
54
    global.vcnl4020[i].wait();
55
  }
56
57
  global.ina219.requestTerminate();
58
  global.ina219.wait();
59
  global.hmc5883l.requestTerminate();
60
  global.hmc5883l.wait();
61
  global.l3g4200d.requestTerminate();
62
  global.l3g4200d.wait();
63
64
  global.motorcontrol.requestTerminate();
65
  global.motorcontrol.wait();
66
  global.odometry.requestTerminate();
67
  global.odometry.wait();
68
69
  // stop I²C
70
  for (i = 0; i < global.V_I2C2.size(); ++i)
71
    global.V_I2C2[i].stop();
72
  global.HW_I2C2.stop();
73
74
  global.lis331dlh.requestTerminate();
75
  global.lis331dlh.wait();
76
77
  global.lis331dlh.configure(&global.accel_sleep_config);
78
//  global.lis331dlh.start(NORMALPRIO +4);
79
80
//  boardWriteIoPower(0);
81
//  boardStandby();
82
83
  return;
84
}
85
86
87
//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);
88
89
void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) {
90
91
  chprintf(chp, "shellRequestShutdown\n");
92
93
  /* if nor argument was given, print some help text */
94
  if (argc == 0 || strcmp(argv[0], "help") == 0) {
95
    chprintf(chp, "\tUSAGE:\n");
96
    chprintf(chp, "> shutdown <type>\n");
97
    chprintf(chp, "\n");
98
    chprintf(chp, "\ttype\n");
99
    chprintf(chp, "The type of shutdown to perform.\n");
100
    chprintf(chp, "Choose one of the following types:\n");
101
    chprintf(chp, "  transportation - Ultra low-power mode with all wakeups disabled.\n");
102
    chprintf(chp, "                   The robot can not be charged.\n");
103
    chprintf(chp, "  deepsleep      - Ultra low-power mode with several wakeups enabled.\n");
104
    chprintf(chp, "                   The robot can only be charged via the power plug.\n");
105
    chprintf(chp, "  hibernate      - Medium low-power mode, but with full charging capabilities.\n");
106
    chprintf(chp, "  restart        - Performs a system restart.\n");
107
    chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
108
    chprintf(chp, "\n");
109
    return;
110
  }
111
112
  if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) {
113
    shutdown_now = SHUTDOWN_TRANSPORTATION;
114
    chprintf(chp, "shutdown to transportation mode initialized\n");
115
  } else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) {
116
    shutdown_now = SHUTDOWN_DEEPSLEEP;
117
    chprintf(chp, "shutdown to deepsleep mode initialized\n");
118
  } else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) {
119
    shutdown_now = SHUTDOWN_HIBERNATE;
120
    chprintf(chp, "shutdown to hibernate mode initialized\n");
121
  } else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) {
122
    chprintf(chp, "restart initialized\n");
123
    shutdown_now = SHUTDOWN_RESTART;
124
  } else {
125
    chprintf(chp, "ERROR: unknown argument!\n");
126
    shutdown_now = SHUTDOWN_NONE;
127
  }
128
129
  return;
130
}
131
132
void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) {
133
  int i;
134
  chprintf(chp, "shellRequestWakeup\n");
135
136
  for (i = 0x00u; i < argc; i++)
137
    chprintf(chp, "%s\n", argv[i]);
138
139
  boardWakeup();
140
}
141
142
void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) {
143
  enum Type {HEX, U8, U16, U32, S8, S16, S32};
144
145
  chprintf(chp, "shellRequestReadData\n");
146
147
  if (argc < 2 || strcmp(argv[0],"help") == 0)
148
  {
149
    chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]");
150
    chprintf(chp, "\n");
151
    chprintf(chp, "\ttype\n");
152
    chprintf(chp, "The data type as which to interpret the data.\n");
153
    chprintf(chp, "Choose one of the following types:\n");
154
    chprintf(chp, "  hex - one byte as hexadecimal value\n");
155
    chprintf(chp, "  u8  - unsigned integer (8 bit)\n");
156
    chprintf(chp, "  u16 - unsigned integer (16 bit)\n");
157
    chprintf(chp, "  u32 - unsigned integer (32 bit)\n");
158
    chprintf(chp, "  s8  - signed integer (8 bit)\n");
159
    chprintf(chp, "  s16 - signed integer (16 bit)\n");
160
    chprintf(chp, "  s32 - signed integer (32 bit)\n");
161
    chprintf(chp, "\tstart\n");
162
    chprintf(chp, "The first byte to read from the memory.\n");
163
    chprintf(chp, "\tcount [default = 1]\n");
164
    chprintf(chp, "The number of elements to read.\n");
165
    chprintf(chp, "\n");
166
    chprintf(chp, "\tNOTE\n");
167
    chprintf(chp, "Type conversions of this function might fail.\n");
168
    chprintf(chp, "If so, use type=hex and convert by hand.\n");
169
    chprintf(chp, "\n");
170
    return;
171
  }
172
173
  uint8_t type_size = 0;
174
  Type type = HEX;
175
  if (strcmp(argv[0],"hex") == 0) {
176
    type_size = sizeof(unsigned char);
177
    type = HEX;
178
  } else if(strcmp(argv[0],"u8") == 0) {
179
    type_size = sizeof(uint8_t);
180
    type = U8;
181
  } else if(strcmp(argv[0],"u16") == 0) {
182
    type_size = sizeof(uint16_t);
183
    type = U16;
184
  } else if(strcmp(argv[0],"u32") == 0) {
185
    type_size = sizeof(uint32_t);
186
    type = U32;
187
  } else if(strcmp(argv[0],"s8") == 0) {
188
    type_size = sizeof(int8_t);
189
    type = S8;
190
  } else if(strcmp(argv[0],"s16") == 0) {
191
    type_size = sizeof(int16_t);
192
    type = S16;
193
  } else if(strcmp(argv[0],"s32") == 0) {
194
    type_size = sizeof(int32_t);
195
    type = S32;
196
  } else {
197
    chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
198
    return;
199
  }
200
201
  unsigned int start_byte = atoi(argv[1]);
202
203
  unsigned int num_elements = 1;
204
  if (argc >= 3)
205
    num_elements = atoi(argv[2]);
206
207
  const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
208
  uint8_t buffer[eeprom_size];
209
  if (start_byte + (type_size * num_elements) > eeprom_size) {
210
    num_elements = (eeprom_size - start_byte) / type_size;
211
    chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
212
  }
213
214
  chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte);
215
216
  // Work around, because stm32f1 cannot read a single byte
217
  if (type_size*num_elements < 2)
218
    type_size = 2;
219
220
  uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements);
221
222
  if (bytes_read != type_size*num_elements)
223
    chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
224
225
  for (unsigned int i = 0; i < num_elements; ++i) {
226
    switch (type) {
227
      case HEX:
228
        chprintf(chp, "%02X ", buffer[i]);
229
        break;
230
      case U8:
231
        chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
232
        break;
233
      case U16:
234
        chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
235
        break;
236
      case U32:
237
        chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
238
        break;
239
      case S8:
240
        chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
241
        break;
242
      case S16:
243
        chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
244
        break;
245
      case S32:
246
        chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
247
        break;
248
      default:
249
        break;
250
    }
251
  }
252
  chprintf(chp, "\n");
253
254
  return;
255
}
256
257
void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) {
258
259
  if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) {
260
    chprintf(chp, "\tUSAGE:\n");
261
    chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");
262
    chprintf(chp, "\n");
263
    chprintf(chp, "\tled mask\n");
264
    chprintf(chp, "The LEDs to be set.\n");
265
    chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");
266
    chprintf(chp, "  0x01 - rear left LED (SSW)\n");
267
    chprintf(chp, "  0x02 - left rear LED (WSW)\n");
268
    chprintf(chp, "  0x04 - left front LED (WNW)\n");
269
    chprintf(chp, "  0x08 - front left LED (NNW)\n");
270
    chprintf(chp, "  0x10 - front right LED (NNE)\n");
271
    chprintf(chp, "  0x20 - right front LED (ENE)\n");
272
    chprintf(chp, "  0x40 - right rear LED (ESE)\n");
273
    chprintf(chp, "  0x80 - rear right LED (SSE)\n");
274
    chprintf(chp, "\twhite/red\n");
275
    chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");
276
    chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");
277
    chprintf(chp, "\tgreen\n");
278
    chprintf(chp, "Sets the green color channel value.\n");
279
    chprintf(chp, "\tblue\n");
280
    chprintf(chp, "Sets the blue color channel value.\n");
281
    chprintf(chp, "\n");
282
    chprintf(chp, "\tExample:\n");
283
    chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");
284
    chprintf(chp, "> set_lights 0x66 0 255 255\n");
285
    chprintf(chp, "\n");
286
    return;
287
  }
288
289
  int arg_mask = strtol(argv[0], NULL, 0);
290
  int red = strtol(argv[1], NULL, 0);
291
  int green = red;
292
  int blue = red;
293
  if (argc >= 4) {
294
    green = strtol(argv[2], NULL, 0);
295
    blue = strtol(argv[3], NULL, 0);
296
  }
297
  Color color(red, green, blue);
298
299
  if (arg_mask & 0x01) {
300
    global.robot.setLightColor(constants::LightRing::LED_SSW, color);
301
  }
302
  if (arg_mask & 0x02) {
303
    global.robot.setLightColor(constants::LightRing::LED_WSW, color);
304
  }
305
  if (arg_mask & 0x04) {
306
    global.robot.setLightColor(constants::LightRing::LED_WNW, color);
307
  }
308
  if (arg_mask & 0x08) {
309
    global.robot.setLightColor(constants::LightRing::LED_NNW, color);
310
  }
311
  if (arg_mask & 0x10) {
312
    global.robot.setLightColor(constants::LightRing::LED_NNE, color);
313
  }
314
  if (arg_mask & 0x20) {
315
    global.robot.setLightColor(constants::LightRing::LED_ENE, color);
316
  }
317
  if (arg_mask & 0x40) {
318
    global.robot.setLightColor(constants::LightRing::LED_ESE, color);
319
  }
320
  if (arg_mask & 0x80) {
321
    global.robot.setLightColor(constants::LightRing::LED_SSE, color);
322
  }
323
324
  return;
325
}
326
327
void boardPeripheryCheck(BaseSequentialStream *chp) {
328
  msg_t result;
329
  chprintf(chp, "\nCHECK: START\n");
330
  // Check the accelerometer
331
  result = global.lis331dlh.getCheck();
332
  if (result == global.lis331dlh.CHECK_OK)
333
    chprintf(chp, "LIS331DLH: OK\n");
334
  else
335
    chprintf(chp, "LIS331DLH: FAIL\n");
336
337
  // Self-test accelerometer
338
//  lis331dlh.printSelfTest(NULL);
339
340
  // Check the eeprom
341
  result = global.memory.getCheck();
342
  if ( result != global.memory.OK)
343
    chprintf(chp, "Memory Structure: FAIL\n");
344
  else
345
    chprintf(chp, "Memory Structure: OK\n");
346
347
  // Check the gyroscope
348
  result = global.l3g4200d.getCheck();
349
  if (result == global.l3g4200d.CHECK_OK)
350
    chprintf(chp, "L3G4200D: OK\n");
351
  else
352
    chprintf(chp, "L3G4200D: FAIL\n");
353
354
  // Check the magnetometer
355
  result = global.hmc5883l.getCheck();
356
  if (result == global.hmc5883l.CHECK_OK)
357
    chprintf(chp, "HMC5883L: OK\n");
358
  else
359
    chprintf(chp, "HMC5883L: FAIL\n");
360
361
  // Check the MUX
362
  result = global.HW_PCA9544.getCheck();
363
  if (result == global.HW_PCA9544.CHECK_OK)
364
    chprintf(chp, "PCA9544: OK\n");
365
  else
366
    chprintf(chp, "PCA9544: FAIL\n");
367
368
  // Check the power monitor
369
  chprintf(chp, "INA219:\tVDD (3.3V):\n");
370
  result = global.ina219.selftest();
371
  if (result == BaseSensor<>::NOT_IMPLEMENTED)
372
    chprintf(chp, "->\tnot implemented\n");
373
  else if (result != INA219::Driver::ST_OK)
374
    chprintf(chp, "->\tFAIL (error code 0x%02X)\n", result);
375
  else
376
    chprintf(chp, "->\tOK\n");
377
378
  // Check the proximitysensors
379
  for (uint8_t i = 0x00; i < global.vcnl4020.size(); i++) {
380
    result = global.vcnl4020[i].getCheck();
381
    if (result == global.vcnl4020[i].CHECK_OK)
382
      chprintf(chp, "VCNL4020: %d OK\n", i);
383
    else
384
      chprintf(chp, "VCNL4020: %d FAIL\n", i);
385
  }
386
  chprintf(chp, "CHECK: FINISH\n");
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 shellRequestResetMemory(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
395
  chprintf(chp, "shellRequestInitMemory\n");
396
397
  msg_t res = global.memory.resetMemory();
398
  if ( res != global.memory.OK)
399
    chprintf(chp, "Memory Init: FAIL\n");
400
  else
401
    chprintf(chp, "Memory Init: OK\n");
402
}
403
404
void shellRequestGetBoardId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
405
  chprintf(chp, "shellRequestGetBoardId\n");
406
  uint8_t id = 0xFFu;
407
408
  msg_t res = global.memory.getBoardId(&id);
409
410
  if (res != global.memory.OK)
411
    chprintf(chp, "Get Board ID: FAIL\n");
412
  else
413
    chprintf(chp, "Get Board ID: %u\n", id);
414
}
415
416
void shellRequestSetBoardId(BaseSequentialStream *chp, int argc, char *argv[]) {
417
  chprintf(chp, "shellRequestSetBoardId\n");
418
419
  if (argc == 0) {
420
    chprintf(chp, "Usage: %s\n","set_board_id <idx>");
421
  } else {
422
    msg_t res = global.memory.setBoardId(atoi(argv[0]));
423
    if (res != global.memory.OK)
424
      chprintf(chp, "Set Board ID: FAIL\n");
425
    else
426
      chprintf(chp, "Set Board ID: OK\n");
427
  }
428
}
429
430
void shellRequestResetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
431
  chprintf(chp, "shellRequestResetCalibrationConstants\n");
432
  chprintf(chp, "Setting Ed=1.0f, Eb=1.0f\n");
433
  msg_t res;
434
435
  res = global.memory.setEd(1.0f);
436
  if (res != global.memory.OK)
437
    chprintf(chp, "Set Ed: FAIL\n");
438
  else
439
    chprintf(chp, "Set Ed: OK\n");
440
441
  res = global.memory.setEb(1.0f);
442
  if (res != global.memory.OK)
443
    chprintf(chp, "Set Eb: FAIL\n");
444
  else
445
    chprintf(chp, "Set Eb: OK\n");
446
}
447
448
void shellRequestGetCalibrationConstants(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
449
  chprintf(chp, "shellRequestGetCalibrationConstants\n");
450
  msg_t res;
451
  float Ed, Eb;
452
453
  res = global.memory.getEd(&Ed);
454
  if (res != global.memory.OK)
455
    chprintf(chp, "Get Ed: FAIL\n");
456
  else
457
    chprintf(chp, "Get Ed: OK \t Ed=%f\n", Ed);
458
459
  res = global.memory.getEb(&Eb);
460
  if (res != global.memory.OK)
461
    chprintf(chp, "Get Eb: FAIL\n");
462
  else
463
    chprintf(chp, "Get Eb: OK \t Eb=%f\n", Eb);
464
}
465
466
void shellRequestSetCalibrationConstants(BaseSequentialStream *chp, int argc, char *argv[]) {
467
  chprintf(chp, "shellRequestSetCalibrationConstants\n");
468
  msg_t res;
469
470
  if (argc != 3) {
471
    chprintf(chp, "Usage: %s\n","set_Ed_Eb <Ed> <Eb> <Write To Eeprom ? 1 : 0>");
472
    chprintf(chp, "(Call with floating point values for Ed and Eb values and write condition):\n");
473
    return;
474
  }
475
  // Get the write condition
476
  const float Ed = atof(argv[0]);
477
  const float Eb = atof(argv[1]);
478
  bool_t writeToMemory = atoi(argv[2]) == 1 ? true : false;
479
480
  res = global.motorcontrol.setWheelDiameterCorrectionFactor(Ed, writeToMemory);
481
  if (res != global.memory.OK)
482
    chprintf(chp, "Set Ed: FAIL\n");
483
  else
484
    chprintf(chp, "Set Ed: OK \t Ed=%f\n", Ed);
485
486
  res = global.motorcontrol.setActualWheelBaseDistance(Eb, writeToMemory);
487
  if (res != global.memory.OK)
488
    chprintf(chp, "Set Eb: FAIL\n");
489
  else
490
    chprintf(chp, "Set Eb: OK \t Ed=%f\n", Eb);
491
}
492
493
void shellRequestGetVcnl(BaseSequentialStream *chp, int argc, char *argv[]) {
494
  chprintf(chp, "shellRequestGetVcnl\n");
495
  // Print the sensor information
496
  if (argc != 1) {
497
    chprintf(chp, "Usage: %s\n","get_vcnl <rep>");
498
    return;
499
  }
500
  for (int32_t rep = 0x00; rep < atoi(argv[0]); ++rep) {
501
    for (uint8_t idx = 0x00; idx < global.vcnl4020.size(); idx++) {
502
     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());
503
    }
504
    chprintf(chp, "\n\n");
505
    BaseThread::sleep(MS2ST(250));
506
  }
507
}
508
509
void shellRequestSetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
510
  chprintf(chp, "shellRequestSetVcnlOffset\n");
511
  if (argc != 2) {
512
    chprintf(chp, "Usage: %s\n","set_vcnl <idx> <offset>");
513
    return;
514
  }
515
516
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
517
  uint16_t vcnlOffset = static_cast<uint16_t>(atoi(argv[1]));
518
519
  if (vcnlIdx >= global.vcnl4020.size()) {
520
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
521
    return;
522
  }
523
524
  msg_t res = global.memory.setVcnl4020Offset(vcnlOffset, vcnlIdx);
525
  if (res != global.memory.OK) {
526
    chprintf(chp, "Set Offset: FAIL\n");
527
  } else {
528
    chprintf(chp, "Set Offset: OK\n");
529
    global.vcnl4020[vcnlIdx].setProximityOffset(vcnlOffset);
530
  }
531
}
532
533
void shellRequestResetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
534
  msg_t res = global.memory.OK;
535
  for (uint8_t idx = 0; idx < 4; ++idx) {
536
    msg_t r = global.memory.setVcnl4020Offset(0, idx);
537
    if (r == global.memory.OK) {
538
      global.vcnl4020[idx].setProximityOffset(0);
539
    } else {
540
      chprintf(chp, "Reset Offset %u: FAIL\n", idx);
541
      res = r;
542
    }
543
  }
544
545
  if (res == global.memory.OK) {
546
    chprintf(chp, "Reset Offset: DONE\n");
547
  }
548
549
  return;
550
}
551
552
void shellRequestGetVcnlOffset(BaseSequentialStream *chp, int argc, char *argv[]) {
553
  chprintf(chp, "shellRequestGetVcnlOffset\n");
554
  if (argc != 1) {
555
    chprintf(chp, "Call with decimal numbers: get_vcnl <idx>\n");
556
    return;
557
  }
558
559
  uint8_t vcnlIdx = static_cast<uint8_t>(atoi(argv[0]));
560
561
  if (vcnlIdx >= global.vcnl4020.size()) {
562
    chprintf(chp, "Wrong VCNL index: Choose [0 .. %d]\n", global.vcnl4020.size()-1);
563
    return;
564
  }
565
566
  uint16_t vcnlOffset;
567
  msg_t res = global.memory.getVcnl4020Offset(&vcnlOffset, vcnlIdx);
568
  if (res != global.memory.OK) {
569
    chprintf(chp, "Get Offset: FAIL\n");
570
  } else {
571
    chprintf(chp, "Get Offset: OK \t Offset=%d\n", vcnlOffset);
572
  }
573
}
574
575
void shellRequestCalib(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
576
  chprintf(chp, "shellRequestCalib\n");
577
  global.robot.calibrate();
578
}
579
580
void shellRequestGetRobotId(BaseSequentialStream *chp, int __unused argc, char __unused *argv[]) {
581
  chprintf(chp, "shellRequestGetRobotId\n");
582
  chprintf(chp, "Robot ID: %u\n", global.robot.getRobotID());
583
  if (global.robot.getRobotID() == 0)
584
    chprintf(chp, "Warning: The board ID seems to be uninitialized.\n");
585
}
586
587
void shellRequestGetSystemLoad(BaseSequentialStream *chp, int argc, char *argv[]) {
588
  chprintf(chp, "shellRequestGetSystemLoad\n");
589
  uint8_t seconds = 1;
590
  if (argc >= 1) {
591
    seconds = atoi(argv[0]);
592
  }
593
  chprintf(chp, "measuring CPU load for %u %s...\n", seconds, (seconds>1)? "seconds" : "second");
594
595
  const systime_t before = chThdGetTicks(chSysGetIdleThread());
596
  BaseThread::sleep(S2ST(seconds));
597
  const systime_t after = chThdGetTicks(chSysGetIdleThread());
598
  const float usage = 1.0f - (float(after - before) / float(seconds * CH_FREQUENCY));
599
600
  chprintf(chp, "CPU load: %3.2f%%\n", usage * 100);
601
  const uint32_t memory_total = 0x10000;
602
  const uint32_t memory_load = memory_total - chCoreStatus();
603
  chprintf(chp, "RAM load: %3.2f%% (%u / %u Byte)\n", float(memory_load)/float(memory_total) * 100, memory_load, memory_total);
604
}
605
606
void shellSwitchBoardCmd(BaseSequentialStream *chp, int argc, char *argv[]) {
607
  if (argc != 1) {
608
    chprintf(chp, "Call with decimal numbers: shell_board <idx>\n");
609
    return;
610
  }
611
  uint8_t boardIdx = static_cast<uint8_t>(atoi(argv[0]));
612
613
  chprintf(chp, "shellSwitchBoardCmd\n");
614
  global.sercanmux1.sendSwitchCmd(boardIdx);
615
}
616
617
void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *argv[]) {
618
  // check the magic number
619 10687985 Thomas Schöpping
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
620
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
621
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
622
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
623
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
624
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
625
      break;
626
627
    case BL_MAGIC_NUMBER:
628
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
629
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
630
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
631
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
632
      break;
633
634
    default:
635
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
636
      break;
637 58fe0e0b Thomas Schöpping
  }
638
639
  return;
640
}
641
642
void shellRequestMotorDrive(BaseSequentialStream *chp, int argc, char *argv[]) {
643
  types::kinematic tmp;
644
  tmp.w_z = 0;
645
  tmp.x = 0;
646
  if (argc == 1){
647
    chprintf(chp, "Set speed to %i um/s \n", atoi(argv[0]));
648
    tmp.x = atoi(argv[0]);
649
  } else {
650
    if(argc == 2){
651
      chprintf(chp, "Set speed to %i \n um/s", atoi(argv[0]));
652
      chprintf(chp, "Set angular speed to %i \n urad/s", atoi(argv[1]));
653
      tmp.x = atoi(argv[0]);
654
      tmp.w_z= atoi(argv[1]);
655
    } else {
656
      chprintf(chp, "Wrong number of parameters given (%i), stopping robot \n", argc);
657
    }
658
  }
659
660
  global.motorcontrol.setTargetSpeed(tmp);
661
  return;
662
}
663
664
void shellRequestMotorStop(BaseSequentialStream *chp, int argc, char *argv[]) {
665
  types::kinematic tmp;
666
  tmp.x = 0;
667
  tmp.w_z = 0;
668
669
  global.motorcontrol.setTargetSpeed(tmp);
670
671
  chprintf(chp, "stop");
672
return;
673
}
674
675
void shellRequestMotorCalibrate(BaseSequentialStream *chp, int argc, char *argv[]) {
676 ff7ad65b Thomas Schöpping
  global.motorcontrol.resetGains();
677
  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
678
  BaseThread::sleep(MS2ST(5000));
679 58fe0e0b Thomas Schöpping
  global.motorcontrol.isCalibrating = true;
680
681
  return;
682
}
683
684
void shellRequestMotorGetGains(BaseSequentialStream *chp, int argc, char *argv[]){
685
  global.motorcontrol.printGains();
686
687
  return;
688
}
689
690 ff7ad65b Thomas Schöpping
void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv[]) {
691
  global.motorcontrol.resetGains();;
692
693
  return;
694
}
695
696 12463563 galberding
697
/**
698
 * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
699
 * be able to detect the correction direction.
700
 * 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.
701 b8b3a9c9 galberding
 *
702 12463563 galberding
 * Note: invert the threshs to drive on the other edge.
703 b8b3a9c9 galberding
 *
704 12463563 galberding
 * */
705 98e7c69b galberding
void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
706 1b3adcdd galberding
    // int vcnl4020AmbientLight[4];
707 12463563 galberding
    int vcnl4020Proximity[4];
708
    int rounds = 1;
709
    int proxyL = 0;
710
    int proxyR = 0;
711
    int maxDelta = 0;
712 22b85da1 galberding
    int sensorL = 0;
713
    int sensorR = 0;
714 b8b3a9c9 galberding
715 12463563 galberding
  if (argc == 1){
716
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
717
    rounds = atoi(argv[0]);
718 b8b3a9c9 galberding
719 12463563 galberding
  }else{
720
    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");
721
    return;
722
  }
723 af93a91c galberding
  for (uint8_t led = 0; led < 8; ++led) {
724
    global.robot.setLightColor(led, Color(Color::BLACK));
725
  }
726 12463563 galberding
727
  for (int j = 0; j < rounds; j++) {
728
    for (int i = 0; i < 4; i++) {
729 1b3adcdd galberding
        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
730 12463563 galberding
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
731
    }
732 af93a91c galberding
    global.robot.setLightColor(j % 8, Color(Color::BLACK));
733
    global.robot.setLightColor(j+1 % 8, Color(Color::WHITE));
734 12463563 galberding
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
735
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
736
    // Update proximity thresh
737
    if (delta > maxDelta) {
738 af93a91c galberding
      for (uint8_t led = 0; led < 8; ++led) {
739
        global.robot.setLightColor(led, Color(Color::GREEN));
740
      }
741 12463563 galberding
      maxDelta = delta;
742
      proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
743
      proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
744
    }
745 22b85da1 galberding
    sensorL += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
746
    sensorR += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
747 12463563 galberding
748
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
749
    //   delta *= -1;
750
    // }
751
752 b8b3a9c9 galberding
    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n",
753 12463563 galberding
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
754
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
755
                  delta,
756
                  proxyL,
757
                  proxyR,
758
                  maxDelta);
759
    // sleep(CAN::UPDATE_PERIOD);
760
    BaseThread::sleep(CAN::UPDATE_PERIOD);
761
  }
762 b8b3a9c9 galberding
763 12463563 galberding
764 06c41883 galberding
  global.linePID.threshProxyL = sensorL / rounds;
765
  global.linePID.threshProxyR = sensorR / rounds;
766
  chprintf(chp,"Thresh FL: %d, FR: %d\n",  global.linePID.threshProxyL, global.linePID.threshProxyR);
767 12463563 galberding
  return;
768
}
769
770
771
772 98e7c69b galberding
void sellRequestgetBottomSensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
773 1b3adcdd galberding
  // uint16_t vcnl4020AmbientLight[4];
774 af93a91c galberding
  uint16_t vcnl4020Proximity[4];
775
  uint16_t rounds = 1;
776 1b3adcdd galberding
  // int sensorR = 0;
777 12463563 galberding
  if (argc == 1){
778
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
779
    rounds = atoi(argv[0]);
780 b8b3a9c9 galberding
781 7f4e10f7 galberding
  } else {
782
    chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n");
783 12463563 galberding
  }
784 98e7c69b galberding
  global.motorcontrol.setMotorEnable(false);
785 12463563 galberding
786
  for (int j = 0; j < rounds; j++) {
787
    for (int i = 0; i < 4; i++) {
788 1b3adcdd galberding
        // vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
789 12463563 galberding
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
790
    }
791 b8b3a9c9 galberding
792 06c41883 galberding
    int32_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
793 12463563 galberding
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
794
795 b8b3a9c9 galberding
    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",
796 12463563 galberding
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
797
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
798
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
799
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT],
800
                  delta);
801
    // sleep(CAN::UPDATE_PERIOD);
802
    BaseThread::sleep(CAN::UPDATE_PERIOD);
803
  }
804 98e7c69b galberding
  global.motorcontrol.setMotorEnable(true);
805 af93a91c galberding
  // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
806 12463563 galberding
  return;
807
}
808
809 98e7c69b galberding
void shellRequestCheckPower(BaseSequentialStream *chp, int argc, char *argv[]){
810 9c46b728 galberding
  int steps = 2000;
811
  int led = 0;
812 98e7c69b galberding
813 7f4e10f7 galberding
  chprintf(chp, "Power over Pins: %s Pins Enabled: %s, Power: %d\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y" : "n", global.robot.getPowerStatus().state_of_charge);
814
}
815
816 84b4c632 galberding
#define window  5
817
int32_t counter = 0;
818
int32_t proxbuf[window]= { 0 };
819
int32_t meanDeviation(uint16_t a, uint16_t b){
820
  int32_t diff = a-b;
821 b8b3a9c9 galberding
  int32_t res = 0;
822 84b4c632 galberding
  proxbuf[counter] = (diff*100)/((a+b)/2);
823
  for (int i = 0; i< window; i++){
824
    res += proxbuf[i];
825
  }
826
  counter++;
827
  counter = counter % window;
828
  return res / window;
829
}
830
831 7f4e10f7 galberding
832 98e7c69b galberding
void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv[]){
833 06c41883 galberding
   int steps = 100;
834 7f4e10f7 galberding
   int i;
835 84b4c632 galberding
   int16_t old3 = 0;
836
   int16_t old4 = 0;
837 7f4e10f7 galberding
  uint16_t prox[8];
838
  uint32_t prox_sum = 0;
839
840
  if (argc == 1){
841
      chprintf(chp, "%i steps \n", atoi(argv[0]));
842
      steps = atoi(argv[0]);
843 9c46b728 galberding
    }else{
844 7f4e10f7 galberding
      chprintf(chp, "Usage: proxyRing <steps> \n");
845
    }
846
  for (int j=0; j<steps; j++){
847
    prox_sum = 0;
848 84b4c632 galberding
    old3 = prox[3];
849
    old4 = prox[4];
850 7f4e10f7 galberding
    for(i=0; i<8;i++){
851
      prox[i] = global.robot.getProximityRingValue(i);
852
      prox_sum += prox[i];
853 9c46b728 galberding
    }
854 84b4c632 galberding
    int32_t deviation = meanDeviation(prox[3] & 0xFFF0  , prox[4] & 0xFFF0);
855
    // int32_t deviation = meanDeviation((prox[3]+old3) / 2  , (prox[4]+old4) / 2);
856 7f4e10f7 galberding
    // uint16_t notouch = 100;
857
    // uint16_t toucht = 20031;
858 b8b3a9c9 galberding
    // sign =
859 7f4e10f7 galberding
    // i = 0;
860 84b4c632 galberding
    chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i Deviation:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], deviation);
861 9c46b728 galberding
    BaseThread::sleep(CAN::UPDATE_PERIOD);
862
  }
863
}
864 84b4c632 galberding
int buf[10][3] = { 0 };
865
void shellRequestMagnetoMeter(BaseSequentialStream *chp, int argc, char *argv[]){
866
   int steps = 10;
867
  //  int i;
868
869
  // uint16_t prox[8];
870
  // uint32_t prox_sum = 0;
871
872
  if (argc == 1){
873
      chprintf(chp, "%i steps \n", atoi(argv[0]));
874
      steps = atoi(argv[0]);
875
    }else{
876
      chprintf(chp, "Usage: proxyRing <steps> \n");
877
    }
878
  chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
879
  BaseThread::sleep(MS2ST(5000));
880
  for (int j=0; j<steps; j++){
881
    // prox_sum = 0;
882
    // for(i=0; i<8;i++){
883
    //   prox[i] = global.robot.getProximityRingValue(i);
884
    //   prox_sum += prox[i];
885
    // }
886
    // uint16_t notouch = 100;
887
    // uint16_t toucht = 20031;
888 b8b3a9c9 galberding
    // sign =
889 84b4c632 galberding
    // i = 0;
890
891
    if (j < 10){
892
      buf[j][0] = global.hmc5883l.getMagnetizationGauss(0x00u);
893
      buf[j][1] = global.hmc5883l.getMagnetizationGauss(0x02u);
894
      buf[j][2] = global.hmc5883l.getMagnetizationGauss(0x01u);
895
    }
896
    chprintf(chp, "X:%i Y:%i Z:%i\n", global.hmc5883l.getMagnetizationGauss(0x00u), global.hmc5883l.getMagnetizationGauss(0x02u), global.hmc5883l.getMagnetizationGauss(0x01u));
897
    BaseThread::sleep(CAN::UPDATE_PERIOD);
898
  }
899
}
900
void shellRequestMagnetoMeterPrint(BaseSequentialStream *chp, int argc, char *argv[]){
901
  for (int j=0; j<10; j++){
902
    chprintf(chp, "X:%i Y:%i Z:%i\n", buf[j][0], buf[j][1], buf[j][2]);
903
    BaseThread::sleep(CAN::UPDATE_PERIOD);
904
  }
905
}
906
907
void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv[]){
908
  types::position oldPos = global.odometry.getPosition();
909
  chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y);
910
}
911 9c46b728 galberding
912 d4c6efa9 galberding
913
void shellRequestErrorInfo(BaseSequentialStream *chp, int argc, char *argv[]){
914
  // Print out the error info collected. Clear buffer after calling
915
  chprintf(chp, "Error Info\n");
916
  chprintf(chp, "\n");
917
  int IDLE                = 0;
918
  int FOLLOW_LINE         = 1;
919
  int DETECT_STATION      = 2;
920
  int REVERSE             = 3;
921
  int PUSH_BACK           = 4;
922
  int CHECK_POSITIONING   = 5;
923
  int CHECK_VOLTAGE       = 6;
924
  int CHARGING            = 7;
925
  int RELEASE             = 8;
926
  int RELEASE_TO_CORRECT  = 9;
927
  int CORRECT_POSITIONING = 10;
928
  int TURN                = 12;
929
  int INACTIVE            = 13;
930
  int CALIBRATION         = 14;
931
  int CALIBRATION_CHECK   = 15;
932
  int DEVIATION_CORRECTION = 16;
933
  int DOCKING_ERROR       = 16+1;
934
  int REVERSE_TIMEOUT_ERROR   = 16+2;
935
  int CALIBRATION_ERROR   = 16+3;
936
  int WHITE_DETECTION_ERROR   = 16+4;
937
  int PROXY_DETECTION_ERROR   = 16+5;
938
  int NO_CHARGING_POWER_ERROR   = 16+6;
939
  int UNKNOWN_STATE_ERROR   = 16+7;
940
941
  chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]);
942
  chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]);
943
  chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]);
944
  chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]);
945
  chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]);
946
  chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]);
947
  chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]);
948
  chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]);
949
  chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]);
950
  chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]);
951
  chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]);
952
  chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]);
953
  chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]);
954
  chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]);
955
  chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]);
956
  chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]);
957
  chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]);
958
  chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]);
959
  chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]);
960
  chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]);
961
  chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]);
962
  chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]);
963
  chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]);
964
965
  for (int i=0; i<24;i++){
966
    global.stateTracker[i] = 0;
967
  }
968 b8b3a9c9 galberding
}
969 d4c6efa9 galberding
970 b8b3a9c9 galberding
inline void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], uint8_t id,
971
                   uint8_t l, uint8_t r, uint8_t att) {
972
  map[id][0] = l;
973
  map[id][1] = r;
974
  map[id][2] = att;
975
}
976
977
#define TEST_CASES 10
978
void shellRequestTestSuite(BaseSequentialStream *chp, int argc, char *argv[]){
979
  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
980
  bool testres[TEST_CASES];
981
  setAttributes(testmap, 0, 1, 2, 1);
982
  setAttributes(testmap, 1, 2, 2, 0);
983
  setAttributes(testmap, 2, 1, 0, 0);
984
  setAttributes(testmap, 3, 0, 0, 0xff);
985
  AmiroMap map = AmiroMap(&global);
986
987
  // --------------------------------------------------
988
989
  int tcase = 0;
990
  map.initialize(testmap);
991
  testres[tcase] = map.get_valid();
992
993
  tcase++; // 1
994
  setAttributes(testmap, 0, 1, 2, 0xff);
995
  map.initialize(testmap);
996
  testres[tcase] = !map.get_valid();
997
998
  tcase++; // 2
999
  setAttributes(testmap, 0, 1, 2, 0);
1000
  setAttributes(testmap, 2, 1, 0, 1);
1001
  map.initialize(testmap);
1002
  testres[tcase] = map.get_current() == 2;
1003
1004
  // --------------------------------------------------
1005
1006
  int failed = 0;
1007
  int passed = 0;
1008
  for (int i=0; i<=tcase; i++) {
1009
    if (testres[i]){
1010
      passed++;
1011
    }else{
1012
      failed++;
1013
      chprintf(chp, "Test %d Failed\n", i);
1014
    }
1015
  }
1016
  chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed);
1017
}
1018
1019
void shellRequestMapTest(BaseSequentialStream *chp, int argc, char *argv[]) {
1020
1021
  // TODO:
1022
  // For now see everything fixed. Create array with pointer to all node structs. Determine
1023
1024
  // chprintf(chp, "  +-------------------+\n");
1025
  // chprintf(chp, "  |                   |\n");
1026
  // chprintf(chp, "  |                   |\n");
1027
  // chprintf(chp, "+-v--+                |\n");
1028
  // chprintf(chp, "|  0 |              +-++\n");
1029
  // chprintf(chp, "+-+--+     +--------+2 | <-------+\n");
1030
  // chprintf(chp, "  |        |        +--+         |\n");
1031
  // chprintf(chp, "  |        |        ^            |\n");
1032
  // chprintf(chp, "  |        |        |            |\n");
1033
  // chprintf(chp, "  |        v        |            |\n");
1034
  // chprintf(chp, "  |      +-+-+------+            |\n");
1035
  // chprintf(chp, "  +------> 1 |                   |\n");
1036
  // chprintf(chp, "         +---+-------------------+\n");
1037
1038
  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
1039
  setAttributes(testmap, 0, 1, 2, 1);
1040
  setAttributes(testmap, 1, 2, 2, 0);
1041
  setAttributes(testmap, 2, 1, 0, 0);
1042
  setAttributes(testmap, 3, 0, 0, 0xff);
1043
  AmiroMap map = AmiroMap(&global);
1044
  map.initialize(testmap);
1045 d4c6efa9 galberding
}
1046
1047 58fe0e0b Thomas Schöpping
static const ShellCommand commands[] = {
1048 b8b3a9c9 galberding
    {"shutdown", shellRequestShutdown},
1049
    {"wakeup", shellRequestWakeup},
1050
    {"check", shellRequestCheck},
1051
    {"reset_memory", shellRequestResetMemory},
1052
    {"get_board_id", shellRequestGetBoardId},
1053
    {"set_board_id", shellRequestSetBoardId},
1054
    {"get_memory_data", shellRequestGetMemoryData},
1055
    {"get_vcnl", shellRequestGetVcnl},
1056
    {"calib_vcnl_offset", shellRequestCalib},
1057
    {"set_vcnl_offset", shellRequestSetVcnlOffset},
1058
    {"reset_vcnl_offset", shellRequestResetVcnlOffset},
1059
    {"get_vcnl_offset", shellRequestGetVcnlOffset},
1060
    {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
1061
    {"get_Ed_Eb", shellRequestGetCalibrationConstants},
1062
    {"set_Ed_Eb", shellRequestSetCalibrationConstants},
1063
    {"get_robot_id", shellRequestGetRobotId},
1064
    {"get_system_load", shellRequestGetSystemLoad},
1065
    {"set_lights", shellRequestSetLights},
1066
    {"shell_board", shellSwitchBoardCmd},
1067
    {"get_bootloader_info", shellRequestGetBootloaderInfo},
1068
    {"motor_drive", shellRequestMotorDrive},
1069
    {"motor_stop", shellRequestMotorStop},
1070
    {"motor_calibrate", shellRequestMotorCalibrate},
1071
    {"motor_getGains", shellRequestMotorGetGains},
1072
    {"motor_resetGains", shellRequestMotorResetGains},
1073
    {"calibrate_line_sensors", shellRequestCalibrateLineSensores},
1074
    {"printProxyBottom", sellRequestgetBottomSensorData},
1075
    {"printProxyRing", shellRequestProxyRingValues},
1076
    {"printMagnetometer", shellRequestMagnetoMeter},
1077
    {"printMagnetometerRes", shellRequestMagnetoMeterPrint},
1078
    {"printLocation", shellRequestPrintCoordinate},
1079
    {"checkPowerPins", shellRequestCheckPower},
1080
    {"stateInfos", shellRequestErrorInfo},
1081
    {"testMap", shellRequestMapTest},
1082
    {"test", shellRequestTestSuite },
1083
    {NULL, NULL}};
1084 58fe0e0b Thomas Schöpping
1085
static const ShellConfig shell_cfg1 = {
1086
  (BaseSequentialStream *) &global.sercanmux1,
1087
  commands
1088
};
1089
1090
void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA)
1091
{
1092
  INA219::CalibData calibData;
1093
  INA219::InitData initData;
1094
1095
  calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V;
1096
  calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV;
1097
  calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us;
1098
  calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us;
1099
  calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous;
1100
  calibData.input.shunt_resistance_O = shuntResistance_O;
1101
  calibData.input.max_expected_current_A = maxExpectedCurrent_A;
1102
  calibData.input.current_lsb_uA = currentLsb_uA;
1103
  if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
1104
  {
1105
    chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
1106
  }
1107
1108
  initData.configuration.value = calibData.input.configuration.value;
1109
  initData.calibration = calibData.output.calibration_value;
1110
  initData.current_lsb_uA = calibData.output.current_lsb_uA;
1111
  if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
1112
  {
1113
    chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
1114
  }
1115
1116
  if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
1117
  {
1118
    chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
1119
  }
1120
1121
  return;
1122
}
1123
1124
/*
1125
 * Application entry point.
1126
 */
1127
int main(void) {
1128
1129
//  int16_t accel;
1130
  Thread *shelltp = NULL;
1131
1132
  /*
1133
   * System initializations.
1134
   * - HAL initialization, this also initializes the configured device drivers
1135
   *   and performs the board-specific initializations.
1136
   * - Kernel initialization, the main() function becomes a thread and the
1137
   *   RTOS is active.
1138
   */
1139
  halInit();
1140
  qeiInit();
1141
  System::init();
1142
1143
//  boardWakeup();
1144
//  boardWriteIoPower(1);
1145
1146
  /*
1147
   * Activates the serial driver 2 using the driver default configuration.
1148
   */
1149
  sdStart(&SD1, &global.sd1_config);
1150
1151
  chprintf((BaseSequentialStream*) &SD1, "\n");
1152
  chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n");
1153 10687985 Thomas Schöpping
  switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
1154
    case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)):
1155
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
1156
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major,
1157
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor,
1158
               ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch);
1159
      break;
1160
1161
    case BL_MAGIC_NUMBER:
1162
      chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
1163
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
1164
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))),
1165
               *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))));
1166
      break;
1167
1168
    default:
1169
      chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
1170
      break;
1171 58fe0e0b Thomas Schöpping
  }
1172
  chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n");
1173
  // make sure that the info text is completetly printed
1174
  BaseThread::sleep(10);
1175
1176
  extStart(&EXTD1, &extcfg);
1177
1178
  /*
1179
   * Wait for a certain amount of time, so that the PowerBoard can activate
1180
   * the IO voltages for the I2C Bus
1181
   */
1182
  BaseThread::sleep(MS2ST(2000));
1183
1184 b4885314 Thomas Schöpping
  boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA);
1185
  boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA);
1186 58fe0e0b Thomas Schöpping
1187
  global.HW_I2C1.start(&global.i2c1_config);
1188
  global.HW_I2C2.start(&global.i2c2_config);
1189
1190
  global.memory.init();
1191
1192
  uint8_t i = 0;
1193
  if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
1194
    chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
1195
  } else {
1196
    chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
1197
  }
1198
  chprintf((BaseSequentialStream*) &SD1, "\n");
1199
1200
  initPowermonitor(global.ina219, 0.1f, 0.075f, 10);
1201
1202
  for (i = 0x00u; i < global.vcnl4020.size(); i++) {
1203
    uint16_t buffer;
1204
    global.memory.getVcnl4020Offset(&buffer,i);
1205
    global.vcnl4020[i].setProximityOffset(buffer);
1206
    global.vcnl4020[i].start(NORMALPRIO);
1207
  }
1208
1209
  global.ina219.start(NORMALPRIO);
1210
1211
  global.hmc5883l.start(NORMALPRIO + 8);
1212
1213
  global.increments.start();  // Start the qei driver
1214
1215 f336542d Timo Korthals
  // Start the three axes gyroscope
1216
  global.l3g4200d.configure(&global.gyro_run_config);
1217
  global.l3g4200d.start(NORMALPRIO+5);
1218
1219 58fe0e0b Thomas Schöpping
  global.odometry.start(NORMALPRIO + 20);
1220
1221
  global.robot.start(HIGHPRIO - 1);
1222
1223
  global.motorcontrol.start(NORMALPRIO + 7);
1224
1225
  global.distcontrol.start(NORMALPRIO + 9);
1226
1227
  // Set target velocity
1228
  types::kinematic velocity;
1229
  velocity.x = 0; // E.g.  "100*1e3" equals "10 cm/s"
1230
  velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s"
1231
  global.motorcontrol.setTargetSpeed(velocity);
1232
1233
  // Start the three axes linear accelerometer
1234
  global.lis331dlh.configure(&global.accel_run_config);
1235
  global.lis331dlh.start(NORMALPRIO+4);
1236
1237
  // Start the user thread
1238
  global.userThread.start(NORMALPRIO);
1239
1240
  /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
1241
  palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH);
1242
1243
  /* wait until all modules are done */
1244
  while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
1245
    continue;
1246
  }
1247
1248
  while (true) {
1249
1250
    if (!shelltp)
1251
      shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
1252
    else if (chThdTerminated(shelltp)) {
1253
      chThdRelease(shelltp);    /* Recovers memory of the previous shell. */
1254
      shelltp = NULL;           /* Triggers spawning of a new shell.      */
1255
    }
1256
1257
    // Let the LED just blink as an alive signal
1258
    boardWriteLed(1);
1259
    BaseThread::sleep(MS2ST(250));
1260
    boardWriteLed(0);
1261
    BaseThread::sleep(MS2ST(250));
1262
1263
    if (shutdown_now != SHUTDOWN_NONE) {
1264 10687985 Thomas Schöpping
      if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) != BL_MAGIC_NUMBER)) {
1265 58fe0e0b Thomas Schöpping
        chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
1266
        shutdown_now = SHUTDOWN_NONE;
1267
      } else {
1268
        uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR;
1269 10687985 Thomas Schöpping
        // handle bootloader version 0.2.x
1270
        if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1271
            (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) {
1272 58fe0e0b Thomas Schöpping
          switch (shutdown_now) {
1273
            case SHUTDOWN_TRANSPORTATION:
1274
              blCallbackPtrAddr += 6 * 4;
1275
              break;
1276
            case SHUTDOWN_DEEPSLEEP:
1277
              blCallbackPtrAddr += 5 * 4;
1278
              break;
1279
            case SHUTDOWN_HIBERNATE:
1280
              blCallbackPtrAddr += 4 * 4;
1281
              break;
1282
            case SHUTDOWN_HANDLE_REQUEST:
1283
            case SHUTDOWN_RESTART:
1284
              blCallbackPtrAddr += 10 * 4;
1285
              break;
1286
            default:
1287
              blCallbackPtrAddr = 0;
1288
              break;
1289
          }
1290 10687985 Thomas Schöpping
        }
1291
        // handle bootloader version 0.3.x
1292
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
1293
                 (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) {
1294
          switch (shutdown_now) {
1295
            case SHUTDOWN_TRANSPORTATION:
1296
              blCallbackPtrAddr += 6 * 4;
1297
              break;
1298
            case SHUTDOWN_DEEPSLEEP:
1299
              blCallbackPtrAddr += 5 * 4;
1300
              break;
1301
            case SHUTDOWN_HIBERNATE:
1302
              blCallbackPtrAddr += 4 * 4;
1303
              break;
1304
            case SHUTDOWN_RESTART:
1305
              blCallbackPtrAddr += 7 * 4;
1306
              break;
1307
            case SHUTDOWN_HANDLE_REQUEST:
1308
              blCallbackPtrAddr += 8 * 4;
1309
              break;
1310
            default:
1311
              blCallbackPtrAddr = 0;
1312
              break;
1313
          }
1314
        }
1315 5b1b6715 Thomas Schöpping
        // handle bootloader version 1.0.x and 1.1.x
1316 10687985 Thomas Schöpping
        else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) &&
1317 5b1b6715 Thomas Schöpping
                 ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major == 1 && (((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 0 || ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 1)) {
1318 58fe0e0b Thomas Schöpping
          switch (shutdown_now) {
1319
            case SHUTDOWN_TRANSPORTATION:
1320
              blCallbackPtrAddr += 6 * 4;
1321
              break;
1322
            case SHUTDOWN_DEEPSLEEP:
1323
              blCallbackPtrAddr += 5 * 4;
1324
              break;
1325
            case SHUTDOWN_HIBERNATE:
1326
              blCallbackPtrAddr += 4 * 4;
1327
              break;
1328
            case SHUTDOWN_RESTART:
1329
              blCallbackPtrAddr += 7 * 4;
1330
              break;
1331
            case SHUTDOWN_HANDLE_REQUEST:
1332
              blCallbackPtrAddr += 8 * 4;
1333
              break;
1334
            default:
1335
              blCallbackPtrAddr = 0;
1336
              break;
1337
          }
1338
        }
1339
1340
        void (*blCallback)(void) = NULL;
1341 10687985 Thomas Schöpping
        if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
1342 58fe0e0b Thomas Schöpping
          blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr));
1343
1344
          if (!blCallback) {
1345
            chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
1346
            shutdown_now = SHUTDOWN_NONE;
1347
          } else {
1348
            chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
1349
            palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW);
1350
            palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW);
1351
1352
            chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
1353
            systemShutdown();
1354
            chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
1355
            BaseThread::sleep(MS2ST(10)); // sleep to print everything
1356
1357
            blCallback();
1358
          }
1359
1360
        } else {
1361
          chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
1362
          shutdown_now = SHUTDOWN_NONE;
1363
        }
1364
      }
1365
1366
//    for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
1367
//        accel = lis331dlh.getAcceleration(i);
1368
//        chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
1369
//    }
1370
//
1371
//    chprintf((BaseSequentialStream*) &SD1, "\n");
1372
//
1373
//    // Print out an alive signal
1374
//    chprintf((BaseSequentialStream*) &SD1, ".");
1375
    }
1376
  }
1377
1378
  return 0;
1379
}