Project

General

Profile

Repository access links

This URL has Read-Only access
Download (16.2 KB) Statistics
| Branch: | Tag: | Revision:

mikrokopter-genom3 / mikrokopter.gen @ master

1
/*/
2
 * Copyright (c) 2015-2019 LAAS/CNRS
3
 * All rights reserved.
4
 *
5
 * Redistribution and use  in source  and binary  forms,  with or without
6
 * modification, are permitted provided that the following conditions are
7
 * met:
8
 *
9
 *   1. Redistributions of  source  code must retain the  above copyright
10
 *      notice and this list of conditions.
11
 *   2. Redistributions in binary form must reproduce the above copyright
12
 *      notice and  this list of  conditions in the  documentation and/or
13
 *      other materials provided with the distribution.
14
 *
15
 *					Anthony Mallet on Fri Feb 13 2015
16
 */
17
#pragma require "openrobots2-idl >= 2.0"
18

    
19
#include "or/pose/pose_estimator.gen"
20
#include "or/robot/rotorcraft.gen"
21

    
22
component mikrokopter {
23
  version		"2.0.1";
24
  email			"openrobots@laas.fr";
25
  lang			"c";
26
  require		"genom3 >= 2.99.30";
27
  codels-require	"eigen3";
28

    
29
  doc "Control mikrokopter multi-rotor UAVs.";
30

    
31
  provides	or_rotorcraft;
32
  uses		or_pose_estimator;
33

    
34
  exception e_sys { short code; string<128> what; };
35
  exception e_baddev { string<256> dev; };
36
  exception e_rotor_failure { unsigned short id; };
37
  exception e_rotor_not_disabled { unsigned short id; };
38
  exception e_started, e_connection, e_rate, e_range, e_input;
39

    
40
  native conn_s;
41
  native log_s;
42

    
43
  port out	or_pose_estimator::state imu {
44
    doc "Provides current gyroscopes and accelerometer measurements.";
45
    doc "";
46
    doc "According to the nature of data, the port is filled with the imu";
47
    doc "data timestamp `ts`, `intrinsic` true, no position (`pos` and";
48
    doc "`pos_cov` are absent) and linear velocities `vx`, `vy`, `vz` set to";
49
    doc "`NaN`. All other elements are always present.";
50
  };
51

    
52

    
53
  /* --- internal state ---------------------------------------------------- */
54

    
55
  ids {
56
    /* serial connection */
57
    conn_s conn;
58

    
59
    /* data timestamps and transmission rate */
60
    struct sensor_time_s {
61
      struct ts_s {
62
        octet seq;
63
        double ts, offset;
64
      } imu, motor[or_rotorcraft::max_rotors], battery;
65

    
66
      struct rate_s {
67
        double imu, motor, battery;
68
      } rate, measured_rate;
69
    } sensor_time;
70

    
71
    struct publish_time_s {
72
      or::time::ts imu, motor[or_rotorcraft::max_rotors];
73
    } publish_time;
74

    
75
    /* battery data */
76
    struct battery_s {
77
      double min, max;
78
      double level;
79
    } battery;
80

    
81
    /* imu calibration & filtering */
82
    struct imu_calibration_s {
83
      double gscale[9], gbias[3], gstddev[3];
84
      double ascale[9], abias[3], astddev[3];
85
    } imu_calibration;
86
    boolean imu_calibration_updated;
87

    
88
    struct imu_filter_s {
89
      boolean enable;
90
      double gain, Q;
91
    } imu_filter;
92

    
93
    /* rotors data */
94
    struct rotor_data_s {
95
      or_rotorcraft::rotor_state state[or_rotorcraft::max_rotors];
96
      double wd[or_rotorcraft::max_rotors];
97
      octet clkrate[or_rotorcraft::max_rotors];
98
    } rotor_data;
99

    
100
    /* servo parameters */
101
    struct servo_s {
102
      double ramp;
103
    } servo;
104

    
105
    /* logging */
106
    log_s log;
107
  };
108

    
109
  attribute get_sensor_rate(out sensor_time.rate = {
110
      .imu =: "Accelerometer and gyroscopes measurement frequency",
111
      .motor =: "Various motor data measurement frequency",
112
      .battery =: "Battery level measurement frequency"
113
    },
114
    out sensor_time.measured_rate = {
115
      .imu =: "Accelerometer and gyroscopes effective frequency",
116
      .motor =: "Various motor data effective frequency",
117
      .battery =: "Battery level effective frequency"
118
    }) {
119
    doc "Get hardware sensor data publishing rate, see <<set_sensor_rate>>.";
120
  };
121
  attribute set_sensor_rate(in sensor_time.rate = {
122
      .imu = 1000.: "Accelerometer and gyroscopes measurement frequency",
123
      .motor = 50: "Various motor data measurement frequency",
124
      .battery = 1: "Battery level measurement frequency"
125
    }) {
126
    doc "Set hardware sensor data publishing rate, in _Hz_";
127
    doc "";
128
    doc "`imu` controls the update frequency of port <<imu>>, while `motor`";
129
    doc "and `battery` indirectly control the port <<rotor_measure>>.";
130
    doc "";
131
    doc "CAUTION: The hardware may not be able to achieve the desired";
132
    doc "frequency, especially for `motor` data when many motors are";
133
    doc "controlled. In this case, no error will be reported, but the ports";
134
    doc "update rate may be lower than expected.";
135

    
136
    validate mk_set_sensor_rate(local in rate, in conn, out sensor_time);
137
  };
138

    
139
  attribute get_battery(out battery = {
140
      .min =: "Minimum acceptable battery voltage",
141
      .max =: "Full battery voltage",
142
      .level =: "Current battery voltage"
143
    }) {
144
    doc "Get current battery voltage and limits.";
145
  };
146
  attribute set_battery_limits(
147
      in battery.min = 14.0 : "Minimum acceptable battery voltage",
148
      in battery.max = 16.7 : "Full battery voltage") {
149
    doc "Set battery minimum and full voltage";
150
    doc "";
151
    doc "This controls the computed `energy left` percentage in the "
152
      "port <<rotor_measure>>.";
153
    validate mk_set_battery_limits(in min, in max);
154

    
155
    throw e_range;
156
  };
157

    
158
  attribute get_imu_calibration(out imu_calibration = {
159
      .gscale =: "Gyroscopes 3×3 scaling matrix (row major)",
160
      .gbias =: "Gyroscopes bias vector",
161
      .gstddev =: "Gyroscopes measurement noise",
162
      .ascale =: "Accelerometers 3×3 scaling matrix (row major)",
163
      .abias =: "Accelerometers bias vector",
164
      .astddev =: "Accelerometers measurement noise"
165
    }) {
166
    doc "Get current gyroscopes and accelerometer calibration data.";
167
  };
168
  attribute set_imu_calibration(in imu_calibration = {
169
      .gscale =: "Gyroscopes 3×3 scaling matrix (row major)",
170
      .gbias =: "Gyroscopes bias vector",
171
      .gstddev =: "Gyroscopes measurement noise",
172
      .ascale =: "Accelerometers 3×3 scaling matrix (row major)",
173
      .abias =: "Accelerometers bias vector",
174
      .astddev =: "Accelerometers measurement noise"
175
    }) {
176
    doc "Set current gyroscopes and accelerometer calibration data.";
177
    doc "";
178
    doc "Calling this service is mandatory after each component start, in";
179
    doc "order to obtain precise IMU measurements.";
180
    doc "";
181
    doc "Input parameters are typically those returned by a call to";
182
    doc "<<get_imu_calibration>> after a successful <<calibrate_imu>>";
183
    doc "(which see).";
184

    
185
    validate mk_set_imu_calibration(out imu_calibration_updated);
186
  };
187

    
188
  attribute get_imu_filter(out imu_filter);
189
  attribute set_imu_filter(in imu_filter) {
190
    validate mk_set_imu_filter(local in imu_filter);
191
  };
192

    
193
  attribute set_ramp(in servo.ramp);
194

    
195

    
196
  /* --- tasks ------------------------------------------------------------- */
197

    
198
  const unsigned short control_period_ms = 1;
199

    
200
  task main {
201
    period control_period_ms ms;
202

    
203
    codel<start> mk_main_init(out ::ids, in imu)
204
      yield main;
205
    codel<main> mk_main_perm(in conn, in battery, in imu_calibration,
206
                             in imu_filter, in rotor_data,
207
                             inout sensor_time, inout publish_time,
208
                             inout imu_calibration_updated,
209
                             inout log, out rotor_measure, out imu)
210
      yield pause::main;
211

    
212
    codel<stop> mk_main_stop(inout log)
213
      yield ether;
214
  };
215

    
216
  task comm {
217
    codel<start> mk_comm_start(out conn)
218
      yield poll;
219

    
220
    async codel<poll> mk_comm_poll(in conn)
221
      yield nodata, recv;
222
    codel<nodata> mk_comm_nodata(inout conn, inout sensor_time,
223
                                 out imu, out battery)
224
      yield poll;
225
    codel<recv> mk_comm_recv(inout conn, in imu_calibration, inout sensor_time,
226
                             out imu, out rotor_data, out battery)
227
      yield poll, recv;
228

    
229
    codel<stop> mk_comm_stop(inout conn)
230
      yield ether;
231

    
232
    throw e_sys;
233
  };
234

    
235

    
236
  /* --- hw connection ----------------------------------------------------- */
237

    
238
  activity connect(
239
    in string<64> serial[2] = {
240
      "/dev/ttyUSB0" :"Main serial device",
241
      "" :"Optional second serial device"
242
    } :"Serial devices",
243
    in unsigned long baud = 115200 :"Baud rate") {
244

    
245
    doc		"Connect to the hardware";
246
    task	comm;
247

    
248
    codel<start> mk_connect_start(in serial, in baud, inout conn,
249
                                  inout sensor_time)
250
      yield ether;
251

    
252
    throw e_sys, e_baddev;
253
    interrupt servo;
254
  };
255

    
256
  activity disconnect() {
257
    doc		"Disconnect from the hardware";
258
    task	comm;
259

    
260
    codel<start> mk_disconnect_start(inout conn)
261
      yield ether;
262
  };
263

    
264
  activity monitor() {
265
    doc		"Monitor connection status";
266
    task	comm;
267

    
268
    codel<start, sleep> mk_monitor_check(in conn) yield pause::sleep, ether;
269
  };
270

    
271
  function disable_motor(in unsigned short motor) {
272
    doc		"Disable checking a motor status when it is disconnected";
273

    
274
    codel mk_disable_motor(in motor, in conn, out rotor_data.state);
275
  };
276

    
277
  function enable_motor(in unsigned short motor) {
278
    doc		"Disable checking a motor status when it is disconnected";
279

    
280
    codel mk_enable_motor(in motor, in conn, out rotor_data.state);
281
  };
282

    
283

    
284
  /* --- calibration ------------------------------------------------------- */
285

    
286
  activity calibrate_imu(
287
    in double tstill = 2 :"Duration in seconds of standstill positions",
288
    in unsigned short nposes = 10 :"Number of different standstill positions") {
289
    doc "Calibrate accelerometers and gyroscopes.";
290
    doc	"";
291
    doc "This service computes the `3×3` scaling matrices and `3D` bias vector";
292
    doc "for both gyroscopes and accelerometers so that all data is returned";
293
    doc "in a consistent, orthogonal frame of reference. This is done by";
294
    doc "implementing the paper '`A robust and easy to implement method for";
295
    doc "IMU calibration without external equipments, ICRA 2014`'. It requires";
296
    doc "no external sensor and a minimum of 10 static poses spanning the";
297
    doc "whole SO(3) space, with moderate motion in between. The standard";
298
    doc "deviation of the sensor noise is also estimated.";
299
    doc "";
300
    doc "The `tstill` parameter controls the time after which a standstill";
301
    doc "position is detected (2 seconds is fine), while `nposes` sets the";
302
    doc "required number of such standstill positions (minimum 10).";
303
    doc "";
304
    doc "While running the calibration, a progress indication will be reported";
305
    doc "to the standard output of the component. You should first set the";
306
    doc "platform in the first standstill orientation, then start the service.";
307
    doc "The service will report `stay still` until it has acquired the";
308
    doc "first pose, then report `acquired pose 1`. You can then move to the";
309
    doc "next standstill orientation, leave it until you read the same";
310
    doc "messages again, and so on for all the `nposes` orientations.";
311
    doc "";
312
    doc "For the calibration to be precise, all the orientations";
313
    doc "have to be as different as possible one from each other. Also, when";
314
    doc "moving from one orientation to another, try to perform a motion such";
315
    doc "that the angular velocities on all 3 axis are not zero.";
316
    doc "";
317
    doc "If you don't read `stay still` after moving to a new";
318
    doc "pose, this means that the platform may be vibrating or slightly";
319
    doc "moving, and the standstill detection cannot work. After some time,";
320
    doc "the service will eventually abort and also report it on the standard";
321
    doc "output.";
322
    doc "";
323
    doc "Once all orientations have been acquired, the results are set for the";
324
    doc "current running instance, and available with <<get_imu_calibration>>.";
325
    doc "Make sure to save the results somewhere before stopping the";
326
    doc "component, so that you can load them with";
327
    doc "<<set_imu_calibration>> when you later restart.";
328
    doc "";
329
    doc "CAUTION: This procedure does not set any particular vertical axis";
330
    doc "and the IMU will typically end up calibrated but not aligned with the";
331
    doc "gravity. Use <<set_zero>> (after calibration) to align the IMU.";
332

    
333
    task	main;
334

    
335
    codel<start> mk_calibrate_imu_start(in tstill, in nposes)
336
      yield collect;
337
    codel<collect> mk_calibrate_imu_collect(in imu)
338
      yield pause::collect, main;
339
    codel<main> mk_calibrate_imu_main(out imu_calibration,
340
                                      out imu_calibration_updated)
341
      yield ether;
342

    
343
    throw e_sys, e_connection;
344

    
345
    interrupt calibrate_imu, set_zero;
346
  };
347

    
348
  activity set_zero() {
349
    doc "Align IMU frame with the gravity vector and reset gyroscopes bias.";
350
    doc "";
351
    doc "This service updates the `3×3` scaling matrices and `3D` bias vector";
352
    doc "for both gyroscopes and accelerometers so that the current";
353
    doc "accelerometer measurements are only on the Z axis and the gyroscopes";
354
    doc "return a 0 angular velocity on each axis.";
355
    doc "";
356
    doc "While running this service, the platform should be perfectly";
357
    doc "standstill and in a horizontal configuration (i.e. it's roll and";
358
    doc "pitch angles are considered zero).";
359
    doc "";
360
    doc "After completion, the current calibration results are updated and";
361
    doc "can be retrieved with <<get_imu_calibration>>.";
362
    doc "";
363
    doc "This service should be called quite often, as the gyroscopes bias";
364
    doc "are much dependent on the temperature, so it is important to";
365
    doc "estimate them well.";
366

    
367
    task	main;
368

    
369
    local double accum[3], gycum[3];
370
    local unsigned long n;
371

    
372
    codel<start> mk_set_zero_start(out accum, out gycum, out n)
373
      yield collect;
374
    codel<collect> mk_set_zero_collect(in imu, inout accum, inout gycum,
375
                                       inout n)
376
      yield pause::collect, main;
377
    codel<main> mk_set_zero(inout accum, inout gycum,
378
                            out imu_calibration, out imu_calibration_updated)
379
      yield ether;
380

    
381
    throw e_sys;
382

    
383
    interrupt calibrate_imu, set_zero;
384
  };
385

    
386

    
387
  /* --- flight ------------------------------------------------------------ */
388

    
389
  activity start() {
390
    doc		"Spin propellers at the lowest velocity";
391
    task	main;
392

    
393
    local unsigned short state;
394

    
395
    codel<start> mk_start_start(in conn, out state,
396
                                in rotor_data.state::rotor_state)
397
      yield pause::start, monitor;
398
    codel<monitor> mk_start_monitor(in conn, inout state,
399
                                    in rotor_data.state::rotor_state)
400
      yield pause::monitor, ether;
401
    codel<stop> mk_start_stop(in conn, in rotor_data.state::rotor_state)
402
      yield pause::stop, ether;
403

    
404
    interrupt start;
405
    throw e_connection, e_started, e_rotor_failure, e_rotor_not_disabled;
406
  };
407

    
408
  activity servo() {
409
    doc		"Control the propellers according to the given velocities";
410
    task	main;
411

    
412
    local double scale;
413

    
414
    codel<start> mk_servo_start(out scale)
415
      yield main;
416
    codel<main> mk_servo_main(in conn, in sensor_time, inout rotor_data,
417
                              in rotor_input, in servo, inout scale)
418
      yield pause::main, stop;
419

    
420
    codel<stop> mk_servo_stop(in conn)
421
      yield ether;
422

    
423
    throw e_connection, e_rotor_failure, e_rate, e_input;
424
  };
425

    
426
  function set_velocity(
427
       in or_rotorcraft::rotor_control desired =: "Propeller velocities") {
428
    doc		"Set the given propeller velocity, once";
429

    
430
    validate mk_validate_input(in rotor_data.state, inout desired);
431
    codel mk_set_velocity(in conn, inout rotor_data, in desired);
432

    
433
    interrupt servo;
434
    throw e_connection, e_rotor_failure;
435
  };
436

    
437
  function set_throttle(
438
       in or_rotorcraft::rotor_control desired =: "Propeller throttles") {
439
    doc		"Set the given propeller voltage";
440

    
441
    validate mk_validate_input(in rotor_data.state, inout desired);
442
    codel mk_set_throttle(in conn, inout rotor_data, in desired);
443

    
444
    interrupt servo;
445
    throw e_connection, e_rotor_failure;
446
  };
447

    
448
  activity stop() {
449
    doc		"Stop all propellers";
450
    task	main;
451

    
452
    codel<start> mk_stop(in conn, in rotor_data.state)
453
      yield pause::start, ether;
454

    
455
    interrupt servo, start;
456
  };
457

    
458

    
459
  /* --- logging ----------------------------------------------------------- */
460

    
461
  function log(in string<64> path = "/tmp/mikrokopter.log": "Log file name",
462
               in unsigned long decimation = 1: "Reduced logging frequency") {
463
    doc		"Log IMU and commanded wrench";
464

    
465
    codel mk_log_start(in path, in decimation, inout log);
466

    
467
    throw e_sys;
468
  };
469

    
470
  function log_stop() {
471
    doc		"Stop logging";
472

    
473
    codel mk_log_stop(out log);
474
  };
475

    
476
  function log_info(out unsigned long miss = :"Missed log entries",
477
                    out unsigned long total = :"Total log entries") {
478
    doc		"Show missed log entries";
479

    
480
    codel mk_log_info(in log, out miss, out total);
481
  };
482
};
(8-8/8)