Project

General

Profile

Repository access links

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

mikrokopter-genom3 / README.adoc @ 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

    
18
// This file was generated from mikrokopter.gen by the skeleton
19
// template. Manual changes should be preserved, although they should
20
// rather be added to the "doc" attributes of the genom objects defined in
21
// mikrokopter.gen.
22

    
23
= mikrokopter component
24
openrobots@laas.fr
25
2.0.1
26
:toc: left
27

    
28
// fix default asciidoctor stylesheet issue #2407 and add hr clear rule
29
ifdef::backend-html5[]
30
[pass]
31
++++
32
<link rel="stylesheet" href="data:text/css,p{font-size: inherit !important}" >
33
<link rel="stylesheet" href="data:text/css,hr{clear: both}" >
34
++++
35
endif::[]
36

    
37

    
38
Control mikrokopter multi-rotor UAVs.
39

    
40

    
41
== Ports
42

    
43

    
44
[[rotor_input]]
45
=== rotor_input (in)
46

    
47

    
48
[role="small", width="50%", float="right", cols="1"]
49
|===
50
a|.Data structure
51
[disc]
52
 * `struct ::or_rotorcraft::input` `rotor_input`
53
 ** `struct ::or::time::ts` `ts`
54
 *** `long` `sec`
55
 *** `long` `nsec`
56
 ** `enum ::or_rotorcraft::control_type` `control` ∈ { `velocity`, `throttle` }
57
 ** `sequence< double, 8 >` `desired`
58

    
59
|===
60

    
61
'''
62

    
63
[[rotor_measure]]
64
=== rotor_measure (out)
65

    
66

    
67
[role="small", width="50%", float="right", cols="1"]
68
|===
69
a|.Data structure
70
[disc]
71
 * `struct ::or_rotorcraft::output` `rotor_measure`
72
 ** `sequence< struct ::or_rotorcraft::rotor_state, 8 >` `rotor`
73
 *** `struct ::or::time::ts` `ts`
74
 **** `long` `sec`
75
 **** `long` `nsec`
76
 *** `boolean` `emerg`
77
 *** `boolean` `spinning`
78
 *** `boolean` `starting`
79
 *** `boolean` `disabled`
80
 *** `double` `velocity`
81
 *** `double` `throttle`
82
 *** `double` `consumption`
83
 *** `double` `energy_level`
84

    
85
|===
86

    
87
'''
88

    
89
[[imu]]
90
=== imu (out)
91

    
92

    
93
[role="small", width="50%", float="right", cols="1"]
94
|===
95
a|.Data structure
96
[disc]
97
 * `struct ::or_pose_estimator::state` `imu`
98
 ** `struct ::or::time::ts` `ts`
99
 *** `long` `sec`
100
 *** `long` `nsec`
101
 ** `boolean` `intrinsic`
102
 ** `optional< struct ::or::t3d::pos >` `pos`
103
 *** `double` `x`
104
 *** `double` `y`
105
 *** `double` `z`
106
 ** `optional< struct ::or::t3d::att >` `att`
107
 *** `double` `qw`
108
 *** `double` `qx`
109
 *** `double` `qy`
110
 *** `double` `qz`
111
 ** `optional< struct ::or::t3d::vel >` `vel`
112
 *** `double` `vx`
113
 *** `double` `vy`
114
 *** `double` `vz`
115
 ** `optional< struct ::or::t3d::avel >` `avel`
116
 *** `double` `wx`
117
 *** `double` `wy`
118
 *** `double` `wz`
119
 ** `optional< struct ::or::t3d::acc >` `acc`
120
 *** `double` `ax`
121
 *** `double` `ay`
122
 *** `double` `az`
123
 ** `optional< struct ::or::t3d::aacc >` `aacc`
124
 *** `double` `awx`
125
 *** `double` `awy`
126
 *** `double` `awz`
127
 ** `optional< struct ::or::t3d::pos_cov >` `pos_cov`
128
 *** `double` `cov[6]`
129
 ** `optional< struct ::or::t3d::att_cov >` `att_cov`
130
 *** `double` `cov[10]`
131
 ** `optional< struct ::or::t3d::att_pos_cov >` `att_pos_cov`
132
 *** `double` `cov[12]`
133
 ** `optional< struct ::or::t3d::vel_cov >` `vel_cov`
134
 *** `double` `cov[6]`
135
 ** `optional< struct ::or::t3d::avel_cov >` `avel_cov`
136
 *** `double` `cov[6]`
137
 ** `optional< struct ::or::t3d::acc_cov >` `acc_cov`
138
 *** `double` `cov[6]`
139
 ** `optional< struct ::or::t3d::aacc_cov >` `aacc_cov`
140
 *** `double` `cov[6]`
141

    
142
|===
143

    
144
Provides current gyroscopes and accelerometer measurements.
145

    
146
According to the nature of data, the port is filled with the imu
147
data timestamp `ts`, `intrinsic` true, no position (`pos` and
148
`pos_cov` are absent) and linear velocities `vx`, `vy`, `vz` set to
149
`NaN`. All other elements are always present.
150

    
151
'''
152

    
153
== Services
154

    
155
[[get_sensor_rate]]
156
=== get_sensor_rate (attribute)
157

    
158
[role="small", width="50%", float="right", cols="1"]
159
|===
160
a|.Outputs
161
[disc]
162
 * `struct ::mikrokopter::ids::sensor_time_s::rate_s` `rate`
163
 ** `double` `imu` Accelerometer and gyroscopes measurement frequency
164
 ** `double` `motor` Various motor data measurement frequency
165
 ** `double` `battery` Battery level measurement frequency
166

    
167
 * `struct ::mikrokopter::ids::sensor_time_s::rate_s` `measured_rate`
168
 ** `double` `imu` Accelerometer and gyroscopes effective frequency
169
 ** `double` `motor` Various motor data effective frequency
170
 ** `double` `battery` Battery level effective frequency
171

    
172
|===
173

    
174
Get hardware sensor data publishing rate, see <<set_sensor_rate>>.
175

    
176
'''
177

    
178
[[set_sensor_rate]]
179
=== set_sensor_rate (attribute)
180

    
181
[role="small", width="50%", float="right", cols="1"]
182
|===
183
a|.Inputs
184
[disc]
185
 * `struct ::mikrokopter::ids::sensor_time_s::rate_s` `rate`
186
 ** `double` `imu` (default `"1000"`) Accelerometer and gyroscopes measurement frequency
187
 ** `double` `motor` (default `"50"`) Various motor data measurement frequency
188
 ** `double` `battery` (default `"1"`) Battery level measurement frequency
189

    
190
|===
191

    
192
Set hardware sensor data publishing rate, in _Hz_
193

    
194
`imu` controls the update frequency of port <<imu>>, while `motor`
195
and `battery` indirectly control the port <<rotor_measure>>.
196

    
197
CAUTION: The hardware may not be able to achieve the desired
198
frequency, especially for `motor` data when many motors are
199
controlled. In this case, no error will be reported, but the ports
200
update rate may be lower than expected.
201

    
202
'''
203

    
204
[[get_battery]]
205
=== get_battery (attribute)
206

    
207
[role="small", width="50%", float="right", cols="1"]
208
|===
209
a|.Outputs
210
[disc]
211
 * `struct ::mikrokopter::ids::battery_s` `battery`
212
 ** `double` `min` Minimum acceptable battery voltage
213
 ** `double` `max` Full battery voltage
214
 ** `double` `level` Current battery voltage
215

    
216
|===
217

    
218
Get current battery voltage and limits.
219

    
220
'''
221

    
222
[[set_battery_limits]]
223
=== set_battery_limits (attribute)
224

    
225
[role="small", width="50%", float="right", cols="1"]
226
|===
227
a|.Inputs
228
[disc]
229
 * `double` `min` (default `"14"`) Minimum acceptable battery voltage
230

    
231
 * `double` `max` (default `"16.7"`) Full battery voltage
232

    
233
a|.Throws
234
[disc]
235
 * `exception ::mikrokopter::e_range`
236

    
237
|===
238

    
239
Set battery minimum and full voltage
240

    
241
This controls the computed `energy left` percentage in the port <<rotor_measure>>.
242

    
243
'''
244

    
245
[[get_imu_calibration]]
246
=== get_imu_calibration (attribute)
247

    
248
[role="small", width="50%", float="right", cols="1"]
249
|===
250
a|.Outputs
251
[disc]
252
 * `struct ::mikrokopter::ids::imu_calibration_s` `imu_calibration`
253
 ** `double` `gscale[9]` Gyroscopes 3×3 scaling matrix (row major)
254
 ** `double` `gbias[3]` Gyroscopes bias vector
255
 ** `double` `gstddev[3]` Gyroscopes measurement noise
256
 ** `double` `ascale[9]` Accelerometers 3×3 scaling matrix (row major)
257
 ** `double` `abias[3]` Accelerometers bias vector
258
 ** `double` `astddev[3]` Accelerometers measurement noise
259

    
260
|===
261

    
262
Get current gyroscopes and accelerometer calibration data.
263

    
264
'''
265

    
266
[[set_imu_calibration]]
267
=== set_imu_calibration (attribute)
268

    
269
[role="small", width="50%", float="right", cols="1"]
270
|===
271
a|.Inputs
272
[disc]
273
 * `struct ::mikrokopter::ids::imu_calibration_s` `imu_calibration`
274
 ** `double` `gscale[9]` Gyroscopes 3×3 scaling matrix (row major)
275
 ** `double` `gbias[3]` Gyroscopes bias vector
276
 ** `double` `gstddev[3]` Gyroscopes measurement noise
277
 ** `double` `ascale[9]` Accelerometers 3×3 scaling matrix (row major)
278
 ** `double` `abias[3]` Accelerometers bias vector
279
 ** `double` `astddev[3]` Accelerometers measurement noise
280

    
281
|===
282

    
283
Set current gyroscopes and accelerometer calibration data.
284

    
285
Calling this service is mandatory after each component start, in
286
order to obtain precise IMU measurements.
287

    
288
Input parameters are typically those returned by a call to
289
<<get_imu_calibration>> after a successful <<calibrate_imu>>
290
(which see).
291

    
292
'''
293

    
294
[[get_imu_filter]]
295
=== get_imu_filter (attribute)
296

    
297
[role="small", width="50%", float="right", cols="1"]
298
|===
299
a|.Outputs
300
[disc]
301
 * `struct ::mikrokopter::ids::imu_filter_s` `imu_filter`
302
 ** `boolean` `enable`
303
 ** `double` `gain`
304
 ** `double` `Q`
305

    
306
|===
307

    
308
'''
309

    
310
[[set_imu_filter]]
311
=== set_imu_filter (attribute)
312

    
313
[role="small", width="50%", float="right", cols="1"]
314
|===
315
a|.Inputs
316
[disc]
317
 * `struct ::mikrokopter::ids::imu_filter_s` `imu_filter`
318
 ** `boolean` `enable`
319
 ** `double` `gain`
320
 ** `double` `Q`
321

    
322
|===
323

    
324
'''
325

    
326
[[set_ramp]]
327
=== set_ramp (attribute)
328

    
329
[role="small", width="50%", float="right", cols="1"]
330
|===
331
a|.Inputs
332
[disc]
333
 * `double` `ramp`
334

    
335
|===
336

    
337
'''
338

    
339
[[connect]]
340
=== connect (activity)
341

    
342
[role="small", width="50%", float="right", cols="1"]
343
|===
344
a|.Inputs
345
[disc]
346
 * `string<64>` `serial[2]` Serial devices
347
 ** `string<64>` `[0]` (default `"/dev/ttyUSB0"`) Main serial device
348
 ** `string<64>` `[1]` (default `""`) Optional second serial device
349

    
350
 * `unsigned long` `baud` (default `"115200"`) Baud rate
351

    
352
a|.Throws
353
[disc]
354
 * `exception ::mikrokopter::e_sys`
355
 ** `short` `code`
356
 ** `string<128>` `what`
357

    
358
 * `exception ::mikrokopter::e_baddev`
359
 ** `string<256>` `dev`
360

    
361
a|.Context
362
[disc]
363
  * In task `<<comm>>`
364
  * Interrupts `<<servo>>`
365
|===
366

    
367
Connect to the hardware
368

    
369
'''
370

    
371
[[disconnect]]
372
=== disconnect (activity)
373

    
374
[role="small", width="50%", float="right", cols="1"]
375
|===
376
a|.Throws
377
[disc]
378
 * `exception ::mikrokopter::e_sys`
379
 ** `short` `code`
380
 ** `string<128>` `what`
381

    
382
a|.Context
383
[disc]
384
  * In task `<<comm>>`
385
|===
386

    
387
Disconnect from the hardware
388

    
389
'''
390

    
391
[[monitor]]
392
=== monitor (activity)
393

    
394
[role="small", width="50%", float="right", cols="1"]
395
|===
396
a|.Throws
397
[disc]
398
 * `exception ::mikrokopter::e_sys`
399
 ** `short` `code`
400
 ** `string<128>` `what`
401

    
402
a|.Context
403
[disc]
404
  * In task `<<comm>>`
405
|===
406

    
407
Monitor connection status
408

    
409
'''
410

    
411
[[disable_motor]]
412
=== disable_motor (function)
413

    
414
[role="small", width="50%", float="right", cols="1"]
415
|===
416
a|.Inputs
417
[disc]
418
 * `unsigned short` `motor`
419

    
420
|===
421

    
422
Disable checking a motor status when it is disconnected
423

    
424
'''
425

    
426
[[enable_motor]]
427
=== enable_motor (function)
428

    
429
[role="small", width="50%", float="right", cols="1"]
430
|===
431
a|.Inputs
432
[disc]
433
 * `unsigned short` `motor`
434

    
435
|===
436

    
437
Disable checking a motor status when it is disconnected
438

    
439
'''
440

    
441
[[calibrate_imu]]
442
=== calibrate_imu (activity)
443

    
444
[role="small", width="50%", float="right", cols="1"]
445
|===
446
a|.Inputs
447
[disc]
448
 * `double` `tstill` (default `"2"`) Duration in seconds of standstill positions
449

    
450
 * `unsigned short` `nposes` (default `"10"`) Number of different standstill positions
451

    
452
a|.Throws
453
[disc]
454
 * `exception ::mikrokopter::e_sys`
455
 ** `short` `code`
456
 ** `string<128>` `what`
457

    
458
 * `exception ::mikrokopter::e_connection`
459

    
460
a|.Context
461
[disc]
462
  * In task `<<main>>`
463
  (frequency 1000.0 _Hz_)
464
  * Updates port `<<imu>>`
465
  * Interrupts `<<calibrate_imu>>`
466
  * Interrupts `<<set_zero>>`
467
|===
468

    
469
Calibrate accelerometers and gyroscopes.
470

    
471
This service computes the `3×3` scaling matrices and `3D` bias vector
472
for both gyroscopes and accelerometers so that all data is returned
473
in a consistent, orthogonal frame of reference. This is done by
474
implementing the paper '`A robust and easy to implement method for
475
IMU calibration without external equipments, ICRA 2014`'. It requires
476
no external sensor and a minimum of 10 static poses spanning the
477
whole SO(3) space, with moderate motion in between. The standard
478
deviation of the sensor noise is also estimated.
479

    
480
The `tstill` parameter controls the time after which a standstill
481
position is detected (2 seconds is fine), while `nposes` sets the
482
required number of such standstill positions (minimum 10).
483

    
484
While running the calibration, a progress indication will be reported
485
to the standard output of the component. You should first set the
486
platform in the first standstill orientation, then start the service.
487
The service will report `stay still` until it has acquired the
488
first pose, then report `acquired pose 1`. You can then move to the
489
next standstill orientation, leave it until you read the same
490
messages again, and so on for all the `nposes` orientations.
491

    
492
For the calibration to be precise, all the orientations
493
have to be as different as possible one from each other. Also, when
494
moving from one orientation to another, try to perform a motion such
495
that the angular velocities on all 3 axis are not zero.
496

    
497
If you don't read `stay still` after moving to a new
498
pose, this means that the platform may be vibrating or slightly
499
moving, and the standstill detection cannot work. After some time,
500
the service will eventually abort and also report it on the standard
501
output.
502

    
503
Once all orientations have been acquired, the results are set for the
504
current running instance, and available with <<get_imu_calibration>>.
505
Make sure to save the results somewhere before stopping the
506
component, so that you can load them with
507
<<set_imu_calibration>> when you later restart.
508

    
509
CAUTION: This procedure does not set any particular vertical axis
510
and the IMU will typically end up calibrated but not aligned with the
511
gravity. Use <<set_zero>> (after calibration) to align the IMU.
512

    
513
'''
514

    
515
[[set_zero]]
516
=== set_zero (activity)
517

    
518
[role="small", width="50%", float="right", cols="1"]
519
|===
520
a|.Throws
521
[disc]
522
 * `exception ::mikrokopter::e_sys`
523
 ** `short` `code`
524
 ** `string<128>` `what`
525

    
526
a|.Context
527
[disc]
528
  * In task `<<main>>`
529
  (frequency 1000.0 _Hz_)
530
  * Updates port `<<imu>>`
531
  * Interrupts `<<calibrate_imu>>`
532
  * Interrupts `<<set_zero>>`
533
|===
534

    
535
Align IMU frame with the gravity vector and reset gyroscopes bias.
536

    
537
This service updates the `3×3` scaling matrices and `3D` bias vector
538
for both gyroscopes and accelerometers so that the current
539
accelerometer measurements are only on the Z axis and the gyroscopes
540
return a 0 angular velocity on each axis.
541

    
542
While running this service, the platform should be perfectly
543
standstill and in a horizontal configuration (i.e. it's roll and
544
pitch angles are considered zero).
545

    
546
After completion, the current calibration results are updated and
547
can be retrieved with <<get_imu_calibration>>.
548

    
549
This service should be called quite often, as the gyroscopes bias
550
are much dependent on the temperature, so it is important to
551
estimate them well.
552

    
553
'''
554

    
555
[[start]]
556
=== start (activity)
557

    
558
[role="small", width="50%", float="right", cols="1"]
559
|===
560
a|.Throws
561
[disc]
562
 * `exception ::mikrokopter::e_connection`
563

    
564
 * `exception ::mikrokopter::e_started`
565

    
566
 * `exception ::mikrokopter::e_rotor_failure`
567
 ** `unsigned short` `id`
568

    
569
 * `exception ::mikrokopter::e_rotor_not_disabled`
570
 ** `unsigned short` `id`
571

    
572
a|.Context
573
[disc]
574
  * In task `<<main>>`
575
  (frequency 1000.0 _Hz_)
576
  * Interrupts `<<start>>`
577
|===
578

    
579
Spin propellers at the lowest velocity
580

    
581
'''
582

    
583
[[servo]]
584
=== servo (activity)
585

    
586
[role="small", width="50%", float="right", cols="1"]
587
|===
588
a|.Throws
589
[disc]
590
 * `exception ::mikrokopter::e_connection`
591

    
592
 * `exception ::mikrokopter::e_rotor_failure`
593
 ** `unsigned short` `id`
594

    
595
 * `exception ::mikrokopter::e_rate`
596

    
597
 * `exception ::mikrokopter::e_input`
598

    
599
a|.Context
600
[disc]
601
  * In task `<<main>>`
602
  (frequency 1000.0 _Hz_)
603
  * Reads port `<<rotor_input>>`
604
|===
605

    
606
Control the propellers according to the given velocities
607

    
608
'''
609

    
610
[[set_velocity]]
611
=== set_velocity (function)
612

    
613
[role="small", width="50%", float="right", cols="1"]
614
|===
615
a|.Inputs
616
[disc]
617
 * `sequence< double, 8 >` `desired` Propeller velocities
618

    
619
a|.Throws
620
[disc]
621
 * `exception ::mikrokopter::e_connection`
622

    
623
 * `exception ::mikrokopter::e_rotor_failure`
624
 ** `unsigned short` `id`
625

    
626
a|.Context
627
[disc]
628
  * Interrupts `<<servo>>`
629
|===
630

    
631
Set the given propeller velocity, once
632

    
633
'''
634

    
635
[[set_throttle]]
636
=== set_throttle (function)
637

    
638
[role="small", width="50%", float="right", cols="1"]
639
|===
640
a|.Inputs
641
[disc]
642
 * `sequence< double, 8 >` `desired` Propeller throttles
643

    
644
a|.Throws
645
[disc]
646
 * `exception ::mikrokopter::e_connection`
647

    
648
 * `exception ::mikrokopter::e_rotor_failure`
649
 ** `unsigned short` `id`
650

    
651
a|.Context
652
[disc]
653
  * Interrupts `<<servo>>`
654
|===
655

    
656
Set the given propeller voltage
657

    
658
'''
659

    
660
[[stop]]
661
=== stop (activity)
662

    
663
[role="small", width="50%", float="right", cols="1"]
664
|===
665
a|.Context
666
[disc]
667
  * In task `<<main>>`
668
  (frequency 1000.0 _Hz_)
669
  * Interrupts `<<servo>>`
670
  * Interrupts `<<start>>`
671
|===
672

    
673
Stop all propellers
674

    
675
'''
676

    
677
[[log]]
678
=== log (function)
679

    
680
[role="small", width="50%", float="right", cols="1"]
681
|===
682
a|.Inputs
683
[disc]
684
 * `string<64>` `path` (default `"/tmp/mikrokopter.log"`) Log file name
685

    
686
 * `unsigned long` `decimation` (default `"1"`) Reduced logging frequency
687

    
688
a|.Throws
689
[disc]
690
 * `exception ::mikrokopter::e_sys`
691
 ** `short` `code`
692
 ** `string<128>` `what`
693

    
694
|===
695

    
696
Log IMU and commanded wrench
697

    
698
'''
699

    
700
[[log_stop]]
701
=== log_stop (function)
702

    
703

    
704
Stop logging
705

    
706
'''
707

    
708
[[log_info]]
709
=== log_info (function)
710

    
711
[role="small", width="50%", float="right", cols="1"]
712
|===
713
a|.Outputs
714
[disc]
715
 * `unsigned long` `miss` Missed log entries
716

    
717
 * `unsigned long` `total` Total log entries
718

    
719
|===
720

    
721
Show missed log entries
722

    
723
'''
724

    
725
== Tasks
726

    
727
[[main]]
728
=== main
729

    
730
[role="small", width="50%", float="right", cols="1"]
731
|===
732
a|.Context
733
[disc]
734
  * Frequency 1000.0 _Hz_
735
* Updates port `<<rotor_measure>>`
736
* Updates port `<<imu>>`
737
|===
738

    
739
'''
740

    
741
[[comm]]
742
=== comm
743

    
744
[role="small", width="50%", float="right", cols="1"]
745
|===
746
a|.Context
747
[disc]
748
  * Free running
749
* Updates port `<<imu>>`
750
a|.Throws
751
[disc]
752
 * `exception ::mikrokopter::e_sys`
753
 ** `short` `code`
754
 ** `string<128>` `what`
755

    
756
|===
757

    
758
'''
(3-3/8)