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