Statistics
| Branch: | Tag: | Revision:

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