amiro-os / devices / DiWheelDrive / main.cpp @ 58fe0e0b
History | View | Annotate | Download (31.7 KB)
1 |
#define BL_CALLBACK_TABLE_ADDR (0x08000000 + 0x01C0) |
---|---|
2 |
#define BL_MAGIC_NUMBER ((uint32_t)0xFF669900u) |
3 |
|
4 |
#define SHUTDOWN_NONE 0 |
5 |
#define SHUTDOWN_TRANSPORTATION 1 |
6 |
#define SHUTDOWN_DEEPSLEEP 2 |
7 |
#define SHUTDOWN_HIBERNATE 3 |
8 |
#define SHUTDOWN_RESTART 4 |
9 |
#define SHUTDOWN_HANDLE_REQUEST 5 |
10 |
|
11 |
#include <ch.hpp> |
12 |
|
13 |
#include <amiro/util/util.h> |
14 |
#include <global.hpp> |
15 |
#include <exti.hpp> |
16 |
|
17 |
#include <chprintf.h> |
18 |
#include <shell.h> |
19 |
|
20 |
using namespace chibios_rt; |
21 |
|
22 |
Global global; |
23 |
|
24 |
void systemShutdown() {
|
25 |
types::kinematic k; |
26 |
uint8_t i; |
27 |
|
28 |
// // make sure we assert SYS_PD_N to delay shutdown until we're done.
|
29 |
// boardRequestShutdown();
|
30 |
|
31 |
// stop the user thread
|
32 |
global.userThread.requestTerminate(); |
33 |
global.userThread.wait(); |
34 |
|
35 |
k.x = 0x00u;
|
36 |
k.w_z = 0x00u;
|
37 |
|
38 |
// stop wheels
|
39 |
global.robot.setTargetSpeed(k); |
40 |
global.robot.terminate(); |
41 |
|
42 |
for (i = 0x00; i < global.vcnl4020.size(); i++) { |
43 |
global.vcnl4020[i].requestTerminate(); |
44 |
global.vcnl4020[i].wait(); |
45 |
} |
46 |
|
47 |
global.ina219.requestTerminate(); |
48 |
global.ina219.wait(); |
49 |
global.hmc5883l.requestTerminate(); |
50 |
global.hmc5883l.wait(); |
51 |
global.l3g4200d.requestTerminate(); |
52 |
global.l3g4200d.wait(); |
53 |
|
54 |
global.motorcontrol.requestTerminate(); |
55 |
global.motorcontrol.wait(); |
56 |
global.odometry.requestTerminate(); |
57 |
global.odometry.wait(); |
58 |
|
59 |
// stop I²C
|
60 |
for (i = 0; i < global.V_I2C2.size(); ++i) |
61 |
global.V_I2C2[i].stop(); |
62 |
global.HW_I2C2.stop(); |
63 |
|
64 |
global.lis331dlh.requestTerminate(); |
65 |
global.lis331dlh.wait(); |
66 |
|
67 |
global.lis331dlh.configure(&global.accel_sleep_config); |
68 |
// global.lis331dlh.start(NORMALPRIO +4);
|
69 |
|
70 |
// boardWriteIoPower(0);
|
71 |
// boardStandby();
|
72 |
|
73 |
return;
|
74 |
} |
75 |
|
76 |
|
77 |
//void (*shellcmd_t)(BaseSequentialStream *chp, int argc, char *argv[]);
|
78 |
|
79 |
void shellRequestShutdown(BaseSequentialStream *chp, int argc, char *argv[]) { |
80 |
|
81 |
chprintf(chp, "shellRequestShutdown\n");
|
82 |
|
83 |
/* if nor argument was given, print some help text */
|
84 |
if (argc == 0 || strcmp(argv[0], "help") == 0) { |
85 |
chprintf(chp, "\tUSAGE:\n");
|
86 |
chprintf(chp, "> shutdown <type>\n");
|
87 |
chprintf(chp, "\n");
|
88 |
chprintf(chp, "\ttype\n");
|
89 |
chprintf(chp, "The type of shutdown to perform.\n");
|
90 |
chprintf(chp, "Choose one of the following types:\n");
|
91 |
chprintf(chp, " transportation - Ultra low-power mode with all wakeups disabled.\n");
|
92 |
chprintf(chp, " The robot can not be charged.\n");
|
93 |
chprintf(chp, " deepsleep - Ultra low-power mode with several wakeups enabled.\n");
|
94 |
chprintf(chp, " The robot can only be charged via the power plug.\n");
|
95 |
chprintf(chp, " hibernate - Medium low-power mode, but with full charging capabilities.\n");
|
96 |
chprintf(chp, " restart - Performs a system restart.\n");
|
97 |
chprintf(chp, "Alternatively, you can use the shortcuts 't', 'd', 'h', and 'r' respectively.");
|
98 |
chprintf(chp, "\n");
|
99 |
return;
|
100 |
} |
101 |
|
102 |
if (strcmp(argv[0],"transportation") == 0 || strcmp(argv[0],"t") == 0) { |
103 |
shutdown_now = SHUTDOWN_TRANSPORTATION; |
104 |
chprintf(chp, "shutdown to transportation mode initialized\n");
|
105 |
} else if (strcmp(argv[0],"deepsleep") == 0 || strcmp(argv[0],"d") == 0) { |
106 |
shutdown_now = SHUTDOWN_DEEPSLEEP; |
107 |
chprintf(chp, "shutdown to deepsleep mode initialized\n");
|
108 |
} else if (strcmp(argv[0],"hibernate") == 0 || strcmp(argv[0],"h") == 0) { |
109 |
shutdown_now = SHUTDOWN_HIBERNATE; |
110 |
chprintf(chp, "shutdown to hibernate mode initialized\n");
|
111 |
} else if (strcmp(argv[0],"restart") == 0 || strcmp(argv[0],"r") == 0) { |
112 |
chprintf(chp, "restart initialized\n");
|
113 |
shutdown_now = SHUTDOWN_RESTART; |
114 |
} else {
|
115 |
chprintf(chp, "ERROR: unknown argument!\n");
|
116 |
shutdown_now = SHUTDOWN_NONE; |
117 |
} |
118 |
|
119 |
return;
|
120 |
} |
121 |
|
122 |
void shellRequestWakeup(BaseSequentialStream *chp, int argc, char *argv[]) { |
123 |
int i;
|
124 |
chprintf(chp, "shellRequestWakeup\n");
|
125 |
|
126 |
for (i = 0x00u; i < argc; i++) |
127 |
chprintf(chp, "%s\n", argv[i]);
|
128 |
|
129 |
boardWakeup(); |
130 |
} |
131 |
|
132 |
void shellRequestGetMemoryData(BaseSequentialStream *chp, int argc, char *argv[]) { |
133 |
enum Type {HEX, U8, U16, U32, S8, S16, S32};
|
134 |
|
135 |
chprintf(chp, "shellRequestReadData\n");
|
136 |
|
137 |
if (argc < 2 || strcmp(argv[0],"help") == 0) |
138 |
{ |
139 |
chprintf(chp, "Usage: %s\n","get_memory_data <type> <start> [<count>]"); |
140 |
chprintf(chp, "\n");
|
141 |
chprintf(chp, "\ttype\n");
|
142 |
chprintf(chp, "The data type as which to interpret the data.\n");
|
143 |
chprintf(chp, "Choose one of the following types:\n");
|
144 |
chprintf(chp, " hex - one byte as hexadecimal value\n");
|
145 |
chprintf(chp, " u8 - unsigned integer (8 bit)\n");
|
146 |
chprintf(chp, " u16 - unsigned integer (16 bit)\n");
|
147 |
chprintf(chp, " u32 - unsigned integer (32 bit)\n");
|
148 |
chprintf(chp, " s8 - signed integer (8 bit)\n");
|
149 |
chprintf(chp, " s16 - signed integer (16 bit)\n");
|
150 |
chprintf(chp, " s32 - signed integer (32 bit)\n");
|
151 |
chprintf(chp, "\tstart\n");
|
152 |
chprintf(chp, "The first byte to read from the memory.\n");
|
153 |
chprintf(chp, "\tcount [default = 1]\n");
|
154 |
chprintf(chp, "The number of elements to read.\n");
|
155 |
chprintf(chp, "\n");
|
156 |
chprintf(chp, "\tNOTE\n");
|
157 |
chprintf(chp, "Type conversions of this function might fail.\n");
|
158 |
chprintf(chp, "If so, use type=hex and convert by hand.\n");
|
159 |
chprintf(chp, "\n");
|
160 |
return;
|
161 |
} |
162 |
|
163 |
uint8_t type_size = 0;
|
164 |
Type type = HEX; |
165 |
if (strcmp(argv[0],"hex") == 0) { |
166 |
type_size = sizeof(unsigned char); |
167 |
type = HEX; |
168 |
} else if(strcmp(argv[0],"u8") == 0) { |
169 |
type_size = sizeof(uint8_t);
|
170 |
type = U8; |
171 |
} else if(strcmp(argv[0],"u16") == 0) { |
172 |
type_size = sizeof(uint16_t);
|
173 |
type = U16; |
174 |
} else if(strcmp(argv[0],"u32") == 0) { |
175 |
type_size = sizeof(uint32_t);
|
176 |
type = U32; |
177 |
} else if(strcmp(argv[0],"s8") == 0) { |
178 |
type_size = sizeof(int8_t);
|
179 |
type = S8; |
180 |
} else if(strcmp(argv[0],"s16") == 0) { |
181 |
type_size = sizeof(int16_t);
|
182 |
type = S16; |
183 |
} else if(strcmp(argv[0],"s32") == 0) { |
184 |
type_size = sizeof(int32_t);
|
185 |
type = S32; |
186 |
} else {
|
187 |
chprintf(chp, "First argument invalid. Use 'get_memory_data help' for help.\n");
|
188 |
return;
|
189 |
} |
190 |
|
191 |
unsigned int start_byte = atoi(argv[1]); |
192 |
|
193 |
unsigned int num_elements = 1; |
194 |
if (argc >= 3) |
195 |
num_elements = atoi(argv[2]);
|
196 |
|
197 |
const size_t eeprom_size = EEPROM::getsize(&global.at24c01);
|
198 |
uint8_t buffer[eeprom_size]; |
199 |
if (start_byte + (type_size * num_elements) > eeprom_size) {
|
200 |
num_elements = (eeprom_size - start_byte) / type_size; |
201 |
chprintf(chp, "Warning: request exceeds eeprom size -> limiting to %u values.\n", num_elements);
|
202 |
} |
203 |
|
204 |
chFileStreamSeek((BaseFileStream*)&global.at24c01, start_byte); |
205 |
|
206 |
// Work around, because stm32f1 cannot read a single byte
|
207 |
if (type_size*num_elements < 2) |
208 |
type_size = 2;
|
209 |
|
210 |
uint32_t bytes_read = chSequentialStreamRead((BaseFileStream*)&global.at24c01, buffer, type_size*num_elements); |
211 |
|
212 |
if (bytes_read != type_size*num_elements)
|
213 |
chprintf(chp, "Warning: %u of %u requested bytes were read.\n", bytes_read, type_size*num_elements);
|
214 |
|
215 |
for (unsigned int i = 0; i < num_elements; ++i) { |
216 |
switch (type) {
|
217 |
case HEX:
|
218 |
chprintf(chp, "%02X ", buffer[i]);
|
219 |
break;
|
220 |
case U8:
|
221 |
chprintf(chp, "%03u ", ((uint8_t*)buffer)[i]);
|
222 |
break;
|
223 |
case U16:
|
224 |
chprintf(chp, "%05u ", ((uint16_t*)buffer)[i]);
|
225 |
break;
|
226 |
case U32:
|
227 |
chprintf(chp, "%010u ", ((uint32_t*)buffer)[i]);
|
228 |
break;
|
229 |
case S8:
|
230 |
chprintf(chp, "%+03d ", ((int8_t*)buffer)[i]);
|
231 |
break;
|
232 |
case S16:
|
233 |
chprintf(chp, "%+05d ", ((int16_t*)buffer)[i]);
|
234 |
break;
|
235 |
case S32:
|
236 |
chprintf(chp, "%+010d ", ((int32_t*)buffer)[i]);
|
237 |
break;
|
238 |
default:
|
239 |
break;
|
240 |
} |
241 |
} |
242 |
chprintf(chp, "\n");
|
243 |
|
244 |
return;
|
245 |
} |
246 |
|
247 |
void shellRequestSetLights(BaseSequentialStream *chp, int argc, char *argv[]) { |
248 |
|
249 |
if (argc < 2 || argc == 3 ||strcmp(argv[0],"help") == 0) { |
250 |
chprintf(chp, "\tUSAGE:\n");
|
251 |
chprintf(chp, "> set_lights <led mask> <white/red> [<green> <blue>]\n");
|
252 |
chprintf(chp, "\n");
|
253 |
chprintf(chp, "\tled mask\n");
|
254 |
chprintf(chp, "The LEDs to be set.\n");
|
255 |
chprintf(chp, "You can set multiple LEDs at once by adding the following values:\n");
|
256 |
chprintf(chp, " 0x01 - rear left LED (SSW)\n");
|
257 |
chprintf(chp, " 0x02 - left rear LED (WSW)\n");
|
258 |
chprintf(chp, " 0x04 - left front LED (WNW)\n");
|
259 |
chprintf(chp, " 0x08 - front left LED (NNW)\n");
|
260 |
chprintf(chp, " 0x10 - front right LED (NNE)\n");
|
261 |
chprintf(chp, " 0x20 - right front LED (ENE)\n");
|
262 |
chprintf(chp, " 0x40 - right rear LED (ESE)\n");
|
263 |
chprintf(chp, " 0x80 - rear right LED (SSE)\n");
|
264 |
chprintf(chp, "\twhite/red\n");
|
265 |
chprintf(chp, "If no optional argument is given, this arguments sets the white value of the selected LEDs.\n");
|
266 |
chprintf(chp, "Otherwise this arguments sets the red color channel value.\n");
|
267 |
chprintf(chp, "\tgreen\n");
|
268 |
chprintf(chp, "Sets the green color channel value.\n");
|
269 |
chprintf(chp, "\tblue\n");
|
270 |
chprintf(chp, "Sets the blue color channel value.\n");
|
271 |
chprintf(chp, "\n");
|
272 |
chprintf(chp, "\tExample:\n");
|
273 |
chprintf(chp, "This line will set the two most left and two most right LEDs to bright cyan.\n");
|
274 |
chprintf(chp, "> set_lights 0x66 0 255 255\n");
|
275 |
chprintf(chp, "\n");
|
276 |
return;
|
277 |
} |
278 |
|
279 |
int arg_mask = strtol(argv[0], NULL, 0); |
280 |
int red = strtol(argv[1], NULL, 0); |
281 |
int green = red;
|
282 |
int blue = red;
|
283 |
if (argc >= 4) { |
284 |
green = strtol(argv[2], NULL, 0); |
285 |
blue = strtol(argv[3], NULL, 0); |
286 |
} |
287 |
Color color(red, green, blue); |
288 |
|
289 |
if (arg_mask & 0x01) { |
290 |
global.robot.setLightColor(constants::LightRing::LED_SSW, color); |
291 |
} |
292 |
if (arg_mask & 0x02) { |
293 |
global.robot.setLightColor(constants::LightRing::LED_WSW, color); |
294 |
} |
295 |
if (arg_mask & 0x04) { |
296 |
global.robot.setLightColor(constants::LightRing::LED_WNW, color); |
297 |
} |
298 |
if (arg_mask & 0x08) { |
299 |
global.robot.setLightColor(constants::LightRing::LED_NNW, color); |
300 |
} |
301 |
|