amiro-os / devices / DiWheelDrive / main.cpp @ ff7ad65b
History | View | Annotate | Download (33 KB)
1 | 58fe0e0b | Thomas Schöpping | #define BL_CALLBACK_TABLE_ADDR (0x08000000 + 0x01C0) |
---|---|---|---|
2 | #define BL_MAGIC_NUMBER ((uint32_t)0xFF669900u) |
||
3 | |||
4 | #define SHUTDOWN_NONE 0 |
||
5 | #define SHUTDOWN_TRANSPORTATION 1 |
||
6 | #define SHUTDOWN_DEEPSLEEP 2 |
||
7 | #define SHUTDOWN_HIBERNATE 3 |
||
8 | #define SHUTDOWN_RESTART 4 |
||
9 | #define SHUTDOWN_HANDLE_REQUEST 5 |
||
10 | |||
11 | #include <ch.hpp> |
||
12 | |||
13 | #include <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 | 10687985 | Thomas Schöpping | 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 | 58fe0e0b | Thomas Schöpping | 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 | 10687985 | Thomas Schöpping | 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 | 58fe0e0b | Thomas Schöpping | } |
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 | ff7ad65b | Thomas Schöpping | global.motorcontrol.resetGains(); |
674 | chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n");
|
||
675 | BaseThread::sleep(MS2ST(5000));
|
||
676 | 58fe0e0b | Thomas Schöpping | 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 | ff7ad65b | Thomas Schöpping | void shellRequestMotorResetGains(BaseSequentialStream *chp, int argc, char *argv[]) { |
688 | global.motorcontrol.resetGains();; |
||
689 | |||
690 | return;
|
||
691 | } |
||
692 | |||
693 | 58fe0e0b | Thomas Schöpping | static const ShellCommand commands[] = { |
694 | {"shutdown", shellRequestShutdown},
|
||
695 | {"wakeup", shellRequestWakeup},
|
||
696 | {"check", shellRequestCheck},
|
||
697 | {"reset_memory", shellRequestResetMemory},
|
||
698 | {"get_board_id", shellRequestGetBoardId},
|
||
699 | {"set_board_id", shellRequestSetBoardId},
|
||
700 | {"get_memory_data", shellRequestGetMemoryData},
|
||
701 | {"get_vcnl", shellRequestGetVcnl},
|
||
702 | {"calib_vcnl_offset", shellRequestCalib},
|
||
703 | {"set_vcnl_offset", shellRequestSetVcnlOffset},
|
||
704 | {"reset_vcnl_offset", shellRequestResetVcnlOffset},
|
||
705 | {"get_vcnl_offset", shellRequestGetVcnlOffset},
|
||
706 | {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
|
||
707 | {"get_Ed_Eb", shellRequestGetCalibrationConstants},
|
||
708 | {"set_Ed_Eb", shellRequestSetCalibrationConstants},
|
||
709 | {"get_robot_id", shellRequestGetRobotId},
|
||
710 | {"get_system_load", shellRequestGetSystemLoad},
|
||
711 | {"set_lights", shellRequestSetLights},
|
||
712 | {"shell_board", shellSwitchBoardCmd},
|
||
713 | {"get_bootloader_info", shellRequestGetBootloaderInfo},
|
||
714 | {"motor_drive", shellRequestMotorDrive},
|
||
715 | {"motor_stop", shellRequestMotorStop},
|
||
716 | {"motor_calibrate", shellRequestMotorCalibrate},
|
||
717 | {"motor_getGains", shellRequestMotorGetGains},
|
||
718 | ff7ad65b | Thomas Schöpping | {"motor_resetGains", shellRequestMotorResetGains},
|
719 | 58fe0e0b | Thomas Schöpping | {NULL, NULL} |
720 | }; |
||
721 | |||
722 | static const ShellConfig shell_cfg1 = { |
||
723 | (BaseSequentialStream *) &global.sercanmux1, |
||
724 | commands |
||
725 | }; |
||
726 | |||
727 | void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA) |
||
728 | { |
||
729 | INA219::CalibData calibData; |
||
730 | INA219::InitData initData; |
||
731 | |||
732 | calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V; |
||
733 | calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV; |
||
734 | calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us; |
||
735 | calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us; |
||
736 | calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous; |
||
737 | calibData.input.shunt_resistance_O = shuntResistance_O; |
||
738 | calibData.input.max_expected_current_A = maxExpectedCurrent_A; |
||
739 | calibData.input.current_lsb_uA = currentLsb_uA; |
||
740 | if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
|
||
741 | { |
||
742 | chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
|
||
743 | } |
||
744 | |||
745 | initData.configuration.value = calibData.input.configuration.value; |
||
746 | initData.calibration = calibData.output.calibration_value; |
||
747 | initData.current_lsb_uA = calibData.output.current_lsb_uA; |
||
748 | if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
|
||
749 | { |
||
750 | chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
|
||
751 | } |
||
752 | |||
753 | if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
|
||
754 | { |
||
755 | chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
|
||
756 | } |
||
757 | |||
758 | return;
|
||
759 | } |
||
760 | |||
761 | /*
|
||
762 | * Application entry point.
|
||
763 | */
|
||
764 | int main(void) { |
||
765 | |||
766 | // int16_t accel;
|
||
767 | Thread *shelltp = NULL;
|
||
768 | |||
769 | /*
|
||
770 | * System initializations.
|
||
771 | * - HAL initialization, this also initializes the configured device drivers
|
||
772 | * and performs the board-specific initializations.
|
||
773 | * - Kernel initialization, the main() function becomes a thread and the
|
||
774 | * RTOS is active.
|
||
775 | */
|
||
776 | halInit(); |
||
777 | qeiInit(); |
||
778 | System::init(); |
||
779 | |||
780 | // boardWakeup();
|
||
781 | // boardWriteIoPower(1);
|
||
782 | |||
783 | /*
|
||
784 | * Activates the serial driver 2 using the driver default configuration.
|
||
785 | */
|
||
786 | sdStart(&SD1, &global.sd1_config); |
||
787 | |||
788 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
789 | chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n"); |
||
790 | 10687985 | Thomas Schöpping | switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
|
791 | case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)): |
||
792 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
793 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major, |
||
794 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor, |
||
795 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch); |
||
796 | break;
|
||
797 | |||
798 | case BL_MAGIC_NUMBER:
|
||
799 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
800 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
801 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
802 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))); |
||
803 | break;
|
||
804 | |||
805 | default:
|
||
806 | chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
|
||
807 | break;
|
||
808 | 58fe0e0b | Thomas Schöpping | } |
809 | chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n"); |
||
810 | // make sure that the info text is completetly printed
|
||
811 | BaseThread::sleep(10);
|
||
812 | |||
813 | extStart(&EXTD1, &extcfg); |
||
814 | |||
815 | /*
|
||
816 | * Wait for a certain amount of time, so that the PowerBoard can activate
|
||
817 | * the IO voltages for the I2C Bus
|
||
818 | */
|
||
819 | BaseThread::sleep(MS2ST(2000));
|
||
820 | |||
821 | b4885314 | Thomas Schöpping | boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA); |
822 | boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA); |
||
823 | 58fe0e0b | Thomas Schöpping | |
824 | global.HW_I2C1.start(&global.i2c1_config); |
||
825 | global.HW_I2C2.start(&global.i2c2_config); |
||
826 | |||
827 | global.memory.init(); |
||
828 | |||
829 | uint8_t i = 0;
|
||
830 | if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
|
||
831 | chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
|
||
832 | } else {
|
||
833 | chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
|
||
834 | } |
||
835 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
836 | |||
837 | initPowermonitor(global.ina219, 0.1f, 0.075f, 10); |
||
838 | |||
839 | for (i = 0x00u; i < global.vcnl4020.size(); i++) { |
||
840 | uint16_t buffer; |
||
841 | global.memory.getVcnl4020Offset(&buffer,i); |
||
842 | global.vcnl4020[i].setProximityOffset(buffer); |
||
843 | global.vcnl4020[i].start(NORMALPRIO); |
||
844 | } |
||
845 | |||
846 | global.ina219.start(NORMALPRIO); |
||
847 | |||
848 | global.hmc5883l.start(NORMALPRIO + 8);
|
||
849 | |||
850 | global.increments.start(); // Start the qei driver
|
||
851 | |||
852 | global.odometry.start(NORMALPRIO + 20);
|
||
853 | |||
854 | global.robot.start(HIGHPRIO - 1);
|
||
855 | |||
856 | global.motorcontrol.start(NORMALPRIO + 7);
|
||
857 | |||
858 | global.distcontrol.start(NORMALPRIO + 9);
|
||
859 | |||
860 | // Set target velocity
|
||
861 | types::kinematic velocity; |
||
862 | velocity.x = 0; // E.g. "100*1e3" equals "10 cm/s" |
||
863 | velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s" |
||
864 | global.motorcontrol.setTargetSpeed(velocity); |
||
865 | |||
866 | // Start the three axes linear accelerometer
|
||
867 | global.lis331dlh.configure(&global.accel_run_config); |
||
868 | global.lis331dlh.start(NORMALPRIO+4);
|
||
869 | |||
870 | // Start the three axes gyroscope
|
||
871 | global.l3g4200d.configure(&global.gyro_run_config); |
||
872 | global.l3g4200d.start(NORMALPRIO+5);
|
||
873 | |||
874 | // Start the user thread
|
||
875 | global.userThread.start(NORMALPRIO); |
||
876 | |||
877 | /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
|
||
878 | palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH); |
||
879 | |||
880 | /* wait until all modules are done */
|
||
881 | while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
|
||
882 | continue;
|
||
883 | } |
||
884 | |||
885 | while (true) { |
||
886 | |||
887 | if (!shelltp)
|
||
888 | shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
|
||
889 | else if (chThdTerminated(shelltp)) { |
||
890 | chThdRelease(shelltp); /* Recovers memory of the previous shell. */
|
||
891 | shelltp = NULL; /* Triggers spawning of a new shell. */ |
||
892 | } |
||
893 | |||
894 | // Let the LED just blink as an alive signal
|
||
895 | boardWriteLed(1);
|
||
896 | BaseThread::sleep(MS2ST(250));
|
||
897 | boardWriteLed(0);
|
||
898 | BaseThread::sleep(MS2ST(250));
|
||
899 | |||
900 | if (shutdown_now != SHUTDOWN_NONE) {
|
||
901 | 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)) { |
902 | 58fe0e0b | Thomas Schöpping | chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
|
903 | shutdown_now = SHUTDOWN_NONE; |
||
904 | } else {
|
||
905 | uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR; |
||
906 | 10687985 | Thomas Schöpping | // handle bootloader version 0.2.x
|
907 | if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
|
||
908 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) { |
||
909 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
910 | case SHUTDOWN_TRANSPORTATION:
|
||
911 | blCallbackPtrAddr += 6 * 4; |
||
912 | break;
|
||
913 | case SHUTDOWN_DEEPSLEEP:
|
||
914 | blCallbackPtrAddr += 5 * 4; |
||
915 | break;
|
||
916 | case SHUTDOWN_HIBERNATE:
|
||
917 | blCallbackPtrAddr += 4 * 4; |
||
918 | break;
|
||
919 | case SHUTDOWN_HANDLE_REQUEST:
|
||
920 | case SHUTDOWN_RESTART:
|
||
921 | blCallbackPtrAddr += 10 * 4; |
||
922 | break;
|
||
923 | default:
|
||
924 | blCallbackPtrAddr = 0;
|
||
925 | break;
|
||
926 | } |
||
927 | 10687985 | Thomas Schöpping | } |
928 | // handle bootloader version 0.3.x
|
||
929 | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) && |
||
930 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) { |
||
931 | switch (shutdown_now) {
|
||
932 | case SHUTDOWN_TRANSPORTATION:
|
||
933 | blCallbackPtrAddr += 6 * 4; |
||
934 | break;
|
||
935 | case SHUTDOWN_DEEPSLEEP:
|
||
936 | blCallbackPtrAddr += 5 * 4; |
||
937 | break;
|
||
938 | case SHUTDOWN_HIBERNATE:
|
||
939 | blCallbackPtrAddr += 4 * 4; |
||
940 | break;
|
||
941 | case SHUTDOWN_RESTART:
|
||
942 | blCallbackPtrAddr += 7 * 4; |
||
943 | break;
|
||
944 | case SHUTDOWN_HANDLE_REQUEST:
|
||
945 | blCallbackPtrAddr += 8 * 4; |
||
946 | break;
|
||
947 | default:
|
||
948 | blCallbackPtrAddr = 0;
|
||
949 | break;
|
||
950 | } |
||
951 | } |
||
952 | // handle bootloader version 1.0.x
|
||
953 | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && |
||
954 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major == 1 && ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor == 0) { |
||
955 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
956 | case SHUTDOWN_TRANSPORTATION:
|
||
957 | blCallbackPtrAddr += 6 * 4; |
||
958 | break;
|
||
959 | case SHUTDOWN_DEEPSLEEP:
|
||
960 | blCallbackPtrAddr += 5 * 4; |
||
961 | break;
|
||
962 | case SHUTDOWN_HIBERNATE:
|
||
963 | blCallbackPtrAddr += 4 * 4; |
||
964 | break;
|
||
965 | case SHUTDOWN_RESTART:
|
||
966 | blCallbackPtrAddr += 7 * 4; |
||
967 | break;
|
||
968 | case SHUTDOWN_HANDLE_REQUEST:
|
||
969 | blCallbackPtrAddr += 8 * 4; |
||
970 | break;
|
||
971 | default:
|
||
972 | blCallbackPtrAddr = 0;
|
||
973 | break;
|
||
974 | } |
||
975 | } |
||
976 | |||
977 | void (*blCallback)(void) = NULL; |
||
978 | 10687985 | Thomas Schöpping | if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
|
979 | 58fe0e0b | Thomas Schöpping | blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr)); |
980 | |||
981 | if (!blCallback) {
|
||
982 | chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
|
||
983 | shutdown_now = SHUTDOWN_NONE; |
||
984 | } else {
|
||
985 | chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
|
||
986 | palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW); |
||
987 | palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW); |
||
988 | |||
989 | chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
|
||
990 | systemShutdown(); |
||
991 | chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
|
||
992 | BaseThread::sleep(MS2ST(10)); // sleep to print everything |
||
993 | |||
994 | blCallback(); |
||
995 | } |
||
996 | |||
997 | } else {
|
||
998 | chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
|
||
999 | shutdown_now = SHUTDOWN_NONE; |
||
1000 | } |
||
1001 | } |
||
1002 | |||
1003 | // for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
||
1004 | // accel = lis331dlh.getAcceleration(i);
|
||
1005 | // chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
|
||
1006 | // }
|
||
1007 | //
|
||
1008 | // chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
1009 | //
|
||
1010 | // // Print out an alive signal
|
||
1011 | // chprintf((BaseSequentialStream*) &SD1, ".");
|
||
1012 | } |
||
1013 | } |
||
1014 | |||
1015 | return 0; |
||
1016 | } |