amiro-os / devices / DiWheelDrive / main.cpp @ af93a91c
History | View | Annotate | Download (38.7 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 | af93a91c | galberding | #include "linefollow2.hpp" |
21 | |||
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 | void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
705 | int vcnl4020AmbientLight[4]; |
||
706 | int vcnl4020Proximity[4]; |
||
707 | int rounds = 1; |
||
708 | int proxyL = 0; |
||
709 | int proxyR = 0; |
||
710 | int maxDelta = 0; |
||
711 | |||
712 | if (argc == 1){ |
||
713 | chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
||
714 | rounds = atoi(argv[0]);
|
||
715 | |||
716 | }else{
|
||
717 | 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");
|
||
718 | return;
|
||
719 | } |
||
720 | af93a91c | galberding | for (uint8_t led = 0; led < 8; ++led) { |
721 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
722 | } |
||
723 | 12463563 | galberding | |
724 | for (int j = 0; j < rounds; j++) { |
||
725 | for (int i = 0; i < 4; i++) { |
||
726 | vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
||
727 | vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
||
728 | } |
||
729 | af93a91c | galberding | global.robot.setLightColor(j % 8, Color(Color::BLACK));
|
730 | global.robot.setLightColor(j+1 % 8, Color(Color::WHITE)); |
||
731 | 12463563 | galberding | int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
|
732 | - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
||
733 | // Update proximity thresh
|
||
734 | if (delta > maxDelta) {
|
||
735 | af93a91c | galberding | for (uint8_t led = 0; led < 8; ++led) { |
736 | global.robot.setLightColor(led, Color(Color::GREEN)); |
||
737 | } |
||
738 | 12463563 | galberding | maxDelta = delta; |
739 | proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
||
740 | proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
||
741 | } |
||
742 | |||
743 | // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
||
744 | // delta *= -1;
|
||
745 | // }
|
||
746 | |||
747 | chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n",
|
||
748 | vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
||
749 | vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
||
750 | delta, |
||
751 | proxyL, |
||
752 | proxyR, |
||
753 | maxDelta); |
||
754 | // sleep(CAN::UPDATE_PERIOD);
|
||
755 | BaseThread::sleep(CAN::UPDATE_PERIOD); |
||
756 | } |
||
757 | chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR);
|
||
758 | |||
759 | global.threshProxyL = proxyL; |
||
760 | global.threshProxyR = proxyR; |
||
761 | return;
|
||
762 | } |
||
763 | |||
764 | |||
765 | |||
766 | void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) { |
||
767 | af93a91c | galberding | uint16_t vcnl4020AmbientLight[4];
|
768 | uint16_t vcnl4020Proximity[4];
|
||
769 | uint16_t rounds = 1;
|
||
770 | uint16_t proxyL = global.threshProxyL; |
||
771 | uint16_t proxyR = global.threshProxyR; |
||
772 | uint16_t maxDelta = 0;
|
||
773 | 12463563 | galberding | |
774 | if (argc == 1){ |
||
775 | chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
||
776 | rounds = atoi(argv[0]);
|
||
777 | |||
778 | } |
||
779 | af93a91c | galberding | |
780 | 12463563 | galberding | |
781 | for (int j = 0; j < rounds; j++) { |
||
782 | for (int i = 0; i < 4; i++) { |
||
783 | vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
||
784 | vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
||
785 | } |
||
786 | |||
787 | af93a91c | galberding | uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
788 | 12463563 | galberding | - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
789 | // // Update proximity thresh
|
||
790 | // if (delta > maxDelta) {
|
||
791 | // maxDelta = delta;
|
||
792 | // proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
|
||
793 | // proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
|
||
794 | // }
|
||
795 | |||
796 | af93a91c | galberding | // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
797 | // delta *= -1;
|
||
798 | // }
|
||
799 | 12463563 | galberding | |
800 | af93a91c | galberding | chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",
|
801 | 12463563 | galberding | vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
802 | vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
||
803 | vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
||
804 | vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], |
||
805 | delta); |
||
806 | // sleep(CAN::UPDATE_PERIOD);
|
||
807 | BaseThread::sleep(CAN::UPDATE_PERIOD); |
||
808 | } |
||
809 | af93a91c | galberding | // chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
|
810 | 12463563 | galberding | return;
|
811 | } |
||
812 | |||
813 | |||
814 | |||
815 | af93a91c | galberding | void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) { |
816 | int vcnl4020AmbientLight[4]; |
||
817 | int vcnl4020Proximity[4]; |
||
818 | int rpmSpeed[2] = {0}; |
||
819 | int steps = 0; |
||
820 | int proxyL = global.threshProxyL;
|
||
821 | int proxyR = global.threshProxyR;
|
||
822 | int maxDelta = 0; |
||
823 | float KCrit = 0.0f; |
||
824 | LineFollow lf(&global); |
||
825 | |||
826 | if (argc == 2){ |
||
827 | chprintf(chp, "KCrti %f\n", atof(argv[0])); |
||
828 | chprintf(chp, "Steps %i\n", atoi(argv[1])); |
||
829 | KCrit = atof(argv[0]);
|
||
830 | steps = atoi(argv[1]);
|
||
831 | } else{
|
||
832 | chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>");
|
||
833 | return;
|
||
834 | } |
||
835 | // global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
|
||
836 | |||
837 | for(int s=0; s < steps; s++){ |
||
838 | chprintf(chp,"S:%d,",s);
|
||
839 | lf.calibrateZiegler(KCrit, rpmSpeed); |
||
840 | |||
841 | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
||
842 | BaseThread::sleep(CAN::UPDATE_PERIOD); |
||
843 | } |
||
844 | global.motorcontrol.setTargetRPM(0,0); |
||
845 | } |
||
846 | |||
847 | |||
848 | 58fe0e0b | Thomas Schöpping | static const ShellCommand commands[] = { |
849 | {"shutdown", shellRequestShutdown},
|
||
850 | {"wakeup", shellRequestWakeup},
|
||
851 | {"check", shellRequestCheck},
|
||
852 | {"reset_memory", shellRequestResetMemory},
|
||
853 | {"get_board_id", shellRequestGetBoardId},
|
||
854 | {"set_board_id", shellRequestSetBoardId},
|
||
855 | {"get_memory_data", shellRequestGetMemoryData},
|
||
856 | {"get_vcnl", shellRequestGetVcnl},
|
||
857 | {"calib_vcnl_offset", shellRequestCalib},
|
||
858 | {"set_vcnl_offset", shellRequestSetVcnlOffset},
|
||
859 | {"reset_vcnl_offset", shellRequestResetVcnlOffset},
|
||
860 | {"get_vcnl_offset", shellRequestGetVcnlOffset},
|
||
861 | {"reset_Ed_Eb", shellRequestResetCalibrationConstants},
|
||
862 | {"get_Ed_Eb", shellRequestGetCalibrationConstants},
|
||
863 | {"set_Ed_Eb", shellRequestSetCalibrationConstants},
|
||
864 | {"get_robot_id", shellRequestGetRobotId},
|
||
865 | {"get_system_load", shellRequestGetSystemLoad},
|
||
866 | {"set_lights", shellRequestSetLights},
|
||
867 | {"shell_board", shellSwitchBoardCmd},
|
||
868 | {"get_bootloader_info", shellRequestGetBootloaderInfo},
|
||
869 | {"motor_drive", shellRequestMotorDrive},
|
||
870 | {"motor_stop", shellRequestMotorStop},
|
||
871 | {"motor_calibrate", shellRequestMotorCalibrate},
|
||
872 | {"motor_getGains", shellRequestMotorGetGains},
|
||
873 | ff7ad65b | Thomas Schöpping | {"motor_resetGains", shellRequestMotorResetGains},
|
874 | 12463563 | galberding | {"dev_proxi_sensor_data", proxySensorData},
|
875 | af93a91c | galberding | {"dev_ziegler2", zieglerMeth2},
|
876 | // TODO: Stop user process from execution to finish/force calibration before anything starts
|
||
877 | {"calibrate_line", calibrateLineSensores},
|
||
878 | 58fe0e0b | Thomas Schöpping | {NULL, NULL} |
879 | }; |
||
880 | |||
881 | static const ShellConfig shell_cfg1 = { |
||
882 | (BaseSequentialStream *) &global.sercanmux1, |
||
883 | commands |
||
884 | }; |
||
885 | |||
886 | void initPowermonitor(INA219::Driver &ina219, const float shuntResistance_O, const float maxExpectedCurrent_A, const uint16_t currentLsb_uA) |
||
887 | { |
||
888 | INA219::CalibData calibData; |
||
889 | INA219::InitData initData; |
||
890 | |||
891 | calibData.input.configuration.content.brng = INA219::Configuration::BRNG_16V; |
||
892 | calibData.input.configuration.content.pg = INA219::Configuration::PGA_40mV; |
||
893 | calibData.input.configuration.content.badc = INA219::Configuration::ADC_68100us; |
||
894 | calibData.input.configuration.content.sadc = INA219::Configuration::ADC_68100us; |
||
895 | calibData.input.configuration.content.mode = INA219::Configuration::MODE_ShuntBus_Continuous; |
||
896 | calibData.input.shunt_resistance_O = shuntResistance_O; |
||
897 | calibData.input.max_expected_current_A = maxExpectedCurrent_A; |
||
898 | calibData.input.current_lsb_uA = currentLsb_uA; |
||
899 | if (ina219.calibration(&calibData) != BaseSensor<>::SUCCESS)
|
||
900 | { |
||
901 | chprintf((BaseSequentialStream*)&SD1, "WARNING: calibration of INA219 failed.\n");
|
||
902 | } |
||
903 | |||
904 | initData.configuration.value = calibData.input.configuration.value; |
||
905 | initData.calibration = calibData.output.calibration_value; |
||
906 | initData.current_lsb_uA = calibData.output.current_lsb_uA; |
||
907 | if (ina219.init(&initData) != BaseSensor<>::SUCCESS)
|
||
908 | { |
||
909 | chprintf((BaseSequentialStream*)&SD1, "WARNING: initialization of INA219 failed.\n");
|
||
910 | } |
||
911 | |||
912 | if (calibData.input.current_lsb_uA != initData.current_lsb_uA)
|
||
913 | { |
||
914 | chprintf((BaseSequentialStream*)&SD1, "NOTE: LSB for current measurement was limited when initializing INA219 (%u -> %u)", calibData.input.current_lsb_uA, initData.current_lsb_uA);
|
||
915 | } |
||
916 | |||
917 | return;
|
||
918 | } |
||
919 | |||
920 | /*
|
||
921 | * Application entry point.
|
||
922 | */
|
||
923 | int main(void) { |
||
924 | |||
925 | // int16_t accel;
|
||
926 | Thread *shelltp = NULL;
|
||
927 | |||
928 | /*
|
||
929 | * System initializations.
|
||
930 | * - HAL initialization, this also initializes the configured device drivers
|
||
931 | * and performs the board-specific initializations.
|
||
932 | * - Kernel initialization, the main() function becomes a thread and the
|
||
933 | * RTOS is active.
|
||
934 | */
|
||
935 | halInit(); |
||
936 | qeiInit(); |
||
937 | System::init(); |
||
938 | |||
939 | // boardWakeup();
|
||
940 | // boardWriteIoPower(1);
|
||
941 | |||
942 | /*
|
||
943 | * Activates the serial driver 2 using the driver default configuration.
|
||
944 | */
|
||
945 | sdStart(&SD1, &global.sd1_config); |
||
946 | |||
947 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
948 | chprintf((BaseSequentialStream*) &SD1, BOARD_NAME " " BOARD_VERSION "\n"); |
||
949 | 10687985 | Thomas Schöpping | switch (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR))) {
|
950 | case (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0)): |
||
951 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
952 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->major, |
||
953 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->minor, |
||
954 | ((blVersion_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))->patch); |
||
955 | break;
|
||
956 | |||
957 | case BL_MAGIC_NUMBER:
|
||
958 | chprintf((BaseSequentialStream*) &SD1, "Bootloader %u.%u.%u\n",
|
||
959 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
960 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))), |
||
961 | *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4)))); |
||
962 | break;
|
||
963 | |||
964 | default:
|
||
965 | chprintf((BaseSequentialStream*) &SD1, "Bootloader incompatible\n");
|
||
966 | break;
|
||
967 | 58fe0e0b | Thomas Schöpping | } |
968 | chprintf((BaseSequentialStream*) &SD1, "ChibiOS " CH_KERNEL_VERSION "\n"); |
||
969 | // make sure that the info text is completetly printed
|
||
970 | BaseThread::sleep(10);
|
||
971 | |||
972 | extStart(&EXTD1, &extcfg); |
||
973 | |||
974 | /*
|
||
975 | * Wait for a certain amount of time, so that the PowerBoard can activate
|
||
976 | * the IO voltages for the I2C Bus
|
||
977 | */
|
||
978 | BaseThread::sleep(MS2ST(2000));
|
||
979 | |||
980 | b4885314 | Thomas Schöpping | boardClearI2CBus(GPIOB_COMPASS_SCL, GPIOB_COMPASS_SDA); |
981 | boardClearI2CBus(GPIOB_IR_SCL, GPIOB_IR_SDA); |
||
982 | 58fe0e0b | Thomas Schöpping | |
983 | global.HW_I2C1.start(&global.i2c1_config); |
||
984 | global.HW_I2C2.start(&global.i2c2_config); |
||
985 | |||
986 | global.memory.init(); |
||
987 | |||
988 | uint8_t i = 0;
|
||
989 | if (global.memory.getBoardId(&i) == fileSystemIo::FileSystemIoBase::OK) {
|
||
990 | chprintf((BaseSequentialStream*) &SD1, "Board ID: %u\n", i);
|
||
991 | } else {
|
||
992 | chprintf((BaseSequentialStream*) &SD1, "Error reading board ID\n");
|
||
993 | } |
||
994 | chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
995 | |||
996 | initPowermonitor(global.ina219, 0.1f, 0.075f, 10); |
||
997 | |||
998 | for (i = 0x00u; i < global.vcnl4020.size(); i++) { |
||
999 | uint16_t buffer; |
||
1000 | global.memory.getVcnl4020Offset(&buffer,i); |
||
1001 | global.vcnl4020[i].setProximityOffset(buffer); |
||
1002 | global.vcnl4020[i].start(NORMALPRIO); |
||
1003 | } |
||
1004 | |||
1005 | global.ina219.start(NORMALPRIO); |
||
1006 | |||
1007 | global.hmc5883l.start(NORMALPRIO + 8);
|
||
1008 | |||
1009 | global.increments.start(); // Start the qei driver
|
||
1010 | |||
1011 | f336542d | Timo Korthals | // Start the three axes gyroscope
|
1012 | global.l3g4200d.configure(&global.gyro_run_config); |
||
1013 | global.l3g4200d.start(NORMALPRIO+5);
|
||
1014 | |||
1015 | 58fe0e0b | Thomas Schöpping | global.odometry.start(NORMALPRIO + 20);
|
1016 | |||
1017 | global.robot.start(HIGHPRIO - 1);
|
||
1018 | |||
1019 | global.motorcontrol.start(NORMALPRIO + 7);
|
||
1020 | |||
1021 | global.distcontrol.start(NORMALPRIO + 9);
|
||
1022 | |||
1023 | // Set target velocity
|
||
1024 | types::kinematic velocity; |
||
1025 | velocity.x = 0; // E.g. "100*1e3" equals "10 cm/s" |
||
1026 | velocity.w_z = 0; // E.g. "2*1e6" equals "2 rad/s" |
||
1027 | global.motorcontrol.setTargetSpeed(velocity); |
||
1028 | |||
1029 | // Start the three axes linear accelerometer
|
||
1030 | global.lis331dlh.configure(&global.accel_run_config); |
||
1031 | global.lis331dlh.start(NORMALPRIO+4);
|
||
1032 | |||
1033 | // Start the user thread
|
||
1034 | global.userThread.start(NORMALPRIO); |
||
1035 | |||
1036 | /* let the SYS_SYNC_N pin go, to signal that the initialization of the module is done */
|
||
1037 | palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_HIGH); |
||
1038 | |||
1039 | /* wait until all modules are done */
|
||
1040 | while (palReadPad(GPIOC, GPIOC_SYS_INT_N) == PAL_LOW) {
|
||
1041 | continue;
|
||
1042 | } |
||
1043 | |||
1044 | while (true) { |
||
1045 | |||
1046 | if (!shelltp)
|
||
1047 | shelltp = shellCreate(&shell_cfg1, THD_WA_SIZE(1024), NORMALPRIO);
|
||
1048 | else if (chThdTerminated(shelltp)) { |
||
1049 | chThdRelease(shelltp); /* Recovers memory of the previous shell. */
|
||
1050 | shelltp = NULL; /* Triggers spawning of a new shell. */ |
||
1051 | } |
||
1052 | |||
1053 | // Let the LED just blink as an alive signal
|
||
1054 | boardWriteLed(1);
|
||
1055 | BaseThread::sleep(MS2ST(250));
|
||
1056 | boardWriteLed(0);
|
||
1057 | BaseThread::sleep(MS2ST(250));
|
||
1058 | |||
1059 | if (shutdown_now != SHUTDOWN_NONE) {
|
||
1060 | 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)) { |
1061 | 58fe0e0b | Thomas Schöpping | chprintf((BaseSequentialStream*) &SD1, "ERROR: unable to shut down (bootloader deprecated).\n");
|
1062 | shutdown_now = SHUTDOWN_NONE; |
||
1063 | } else {
|
||
1064 | uint32_t blCallbackPtrAddr = BL_CALLBACK_TABLE_ADDR; |
||
1065 | 10687985 | Thomas Schöpping | // handle bootloader version 0.2.x
|
1066 | if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) &&
|
||
1067 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 2)) { |
||
1068 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
1069 | case SHUTDOWN_TRANSPORTATION:
|
||
1070 | blCallbackPtrAddr += 6 * 4; |
||
1071 | break;
|
||
1072 | case SHUTDOWN_DEEPSLEEP:
|
||
1073 | blCallbackPtrAddr += 5 * 4; |
||
1074 | break;
|
||
1075 | case SHUTDOWN_HIBERNATE:
|
||
1076 | blCallbackPtrAddr += 4 * 4; |
||
1077 | break;
|
||
1078 | case SHUTDOWN_HANDLE_REQUEST:
|
||
1079 | case SHUTDOWN_RESTART:
|
||
1080 | blCallbackPtrAddr += 10 * 4; |
||
1081 | break;
|
||
1082 | default:
|
||
1083 | blCallbackPtrAddr = 0;
|
||
1084 | break;
|
||
1085 | } |
||
1086 | 10687985 | Thomas Schöpping | } |
1087 | // handle bootloader version 0.3.x
|
||
1088 | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == BL_MAGIC_NUMBER) && |
||
1089 | (*((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (1*4))) == 0 && *((uint32_t*)(BL_CALLBACK_TABLE_ADDR + (2*4))) == 3)) { |
||
1090 | switch (shutdown_now) {
|
||
1091 | case SHUTDOWN_TRANSPORTATION:
|
||
1092 | blCallbackPtrAddr += 6 * 4; |
||
1093 | break;
|
||
1094 | case SHUTDOWN_DEEPSLEEP:
|
||
1095 | blCallbackPtrAddr += 5 * 4; |
||
1096 | break;
|
||
1097 | case SHUTDOWN_HIBERNATE:
|
||
1098 | blCallbackPtrAddr += 4 * 4; |
||
1099 | break;
|
||
1100 | case SHUTDOWN_RESTART:
|
||
1101 | blCallbackPtrAddr += 7 * 4; |
||
1102 | break;
|
||
1103 | case SHUTDOWN_HANDLE_REQUEST:
|
||
1104 | blCallbackPtrAddr += 8 * 4; |
||
1105 | break;
|
||
1106 | default:
|
||
1107 | blCallbackPtrAddr = 0;
|
||
1108 | break;
|
||
1109 | } |
||
1110 | } |
||
1111 | 5b1b6715 | Thomas Schöpping | // handle bootloader version 1.0.x and 1.1.x
|
1112 | 10687985 | Thomas Schöpping | else if ((*((uint32_t*)(BL_CALLBACK_TABLE_ADDR)) == (('A'<<24) | ('-'<<16) | ('B'<<8) | ('L'<<0))) && |
1113 | 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)) { |
1114 | 58fe0e0b | Thomas Schöpping | switch (shutdown_now) {
|
1115 | case SHUTDOWN_TRANSPORTATION:
|
||
1116 | blCallbackPtrAddr += 6 * 4; |
||
1117 | break;
|
||
1118 | case SHUTDOWN_DEEPSLEEP:
|
||
1119 | blCallbackPtrAddr += 5 * 4; |
||
1120 | break;
|
||
1121 | case SHUTDOWN_HIBERNATE:
|
||
1122 | blCallbackPtrAddr += 4 * 4; |
||
1123 | break;
|
||
1124 | case SHUTDOWN_RESTART:
|
||
1125 | blCallbackPtrAddr += 7 * 4; |
||
1126 | break;
|
||
1127 | case SHUTDOWN_HANDLE_REQUEST:
|
||
1128 | blCallbackPtrAddr += 8 * 4; |
||
1129 | break;
|
||
1130 | default:
|
||
1131 | blCallbackPtrAddr = 0;
|
||
1132 | break;
|
||
1133 | } |
||
1134 | } |
||
1135 | |||
1136 | void (*blCallback)(void) = NULL; |
||
1137 | 10687985 | Thomas Schöpping | if (blCallbackPtrAddr > BL_CALLBACK_TABLE_ADDR) {
|
1138 | 58fe0e0b | Thomas Schöpping | blCallback = (void (*)(void))(*((uint32_t*)blCallbackPtrAddr)); |
1139 | |||
1140 | if (!blCallback) {
|
||
1141 | chprintf((BaseSequentialStream*) &SD1, "ERROR: Requested shutdown not supported.\n");
|
||
1142 | shutdown_now = SHUTDOWN_NONE; |
||
1143 | } else {
|
||
1144 | chprintf((BaseSequentialStream*)&SD1, "initiating shutdown sequence...\n");
|
||
1145 | palWritePad(GPIOC, GPIOC_SYS_INT_N, PAL_LOW); |
||
1146 | palWritePad(GPIOC, GPIOC_SYS_PD_N, PAL_LOW); |
||
1147 | |||
1148 | chprintf((BaseSequentialStream*)&SD1, "stopping all threads and periphery...");
|
||
1149 | systemShutdown(); |
||
1150 | chprintf((BaseSequentialStream*)&SD1, "\tdone\n");
|
||
1151 | BaseThread::sleep(MS2ST(10)); // sleep to print everything |
||
1152 | |||
1153 | blCallback(); |
||
1154 | } |
||
1155 | |||
1156 | } else {
|
||
1157 | chprintf((BaseSequentialStream*) &SD1, "ERROR: invalid shutdown requested (%u).\n", shutdown_now);
|
||
1158 | shutdown_now = SHUTDOWN_NONE; |
||
1159 | } |
||
1160 | } |
||
1161 | |||
1162 | // for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
||
1163 | // accel = lis331dlh.getAcceleration(i);
|
||
1164 | // chprintf((BaseSequentialStream*) &SD1, "%c%04X ", accel < 0 ? '-' : '+', accel < 0 ? -accel : accel);
|
||
1165 | // }
|
||
1166 | //
|
||
1167 | // chprintf((BaseSequentialStream*) &SD1, "\n");
|
||
1168 | //
|
||
1169 | // // Print out an alive signal
|
||
1170 | // chprintf((BaseSequentialStream*) &SD1, ".");
|
||
1171 | } |
||
1172 | } |
||
1173 | |||
1174 | return 0; |
||
1175 | } |