e00024404dac25929537816a429b744326cbe834
[mardrone] / mardrone / ARDrone_SDK_Version_1_8_20110726 / ARDroneLib / Soft / Common / navdata_common.h
1 /**
2  *  \file     navdata_common.h
3  *  \brief    Common navdata configuration
4  *  \author   Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
5  */
6
7 #ifndef _NAVDATA_COMMON_H_
8 #define _NAVDATA_COMMON_H_
9
10 /*------------------------------------------ NAVDATA STRUCTURES DECLARATIONS ---------------------------------------------------------------*/
11
12 #include <config.h>
13 #include <vision_common.h>
14
15 #include <VP_Os/vp_os_types.h>
16 #include <Maths/maths.h>
17 #include <Maths/matrices.h>
18
19 #if defined(_MSC_VER)
20         #define _ATTRIBUTE_PACKED_
21         /* Asks Visual C++ to pack structures from now on*/
22         #pragma pack(1)
23 #else
24         #define _ATTRIBUTE_PACKED_  __attribute__ ((packed))
25 #endif
26
27 // Define constants for gyrometers handling
28 typedef enum {
29   GYRO_X    = 0,
30   GYRO_Y    = 1,
31   GYRO_Z    = 2,
32   NB_GYROS  = 3
33 } def_gyro_t;
34
35
36 // Define constants for accelerometers handling
37 typedef enum {
38   ACC_X   = 0,
39   ACC_Y   = 1,
40   ACC_Z   = 2,
41   NB_ACCS = 3
42 } def_acc_t;
43
44 /**
45  * \struct _velocities_t
46  * \brief Velocities in float32_t format
47  */
48 typedef struct _velocities_t {
49   float32_t x;
50   float32_t y;
51   float32_t z;
52 } velocities_t;
53
54 // Default control loops gains TODO Put these values in flash memory
55 // To avoid divisions in embedded software, gains are defined as ratios where
56 //  - the numerator is an integer
57 //  - the denominator is an integer
58
59 /**
60  *  \var     CTRL_DEFAULT_NUM_PQ_KP
61  *  \brief   Numerator of default proportionnal gain for pitch (p) and roll (q) angular rate control loops
62  */
63 /**
64  *  \var     CTRL_DEFAULT_NUM_EA_KP
65  *  \brief   Numerator of default proportionnal gain for Euler Angle control loops
66  */
67 /**
68  *  \var     CTRL_DEFAULT_NUM_EA_KI
69  *  \brief   Numerator of default integral gain for Euler Angle control loops
70  */
71
72 #define CTRL_DEFAULT_NUM_PQ_KP_NO_SHELL /*30000  26000 */ 40000
73 #define CTRL_DEFAULT_NUM_EA_KP_NO_SHELL /*7000  9000  7000 */ 8000
74 #define CTRL_DEFAULT_NUM_EA_KI_NO_SHELL /*4000  7000 6000 */ 7000
75
76 #define CTRL_DEFAULT_NUM_PQ_KP_SHELL /*30000 23000*/ 40000
77 #define CTRL_DEFAULT_NUM_EA_KP_SHELL /*9000 10000*/  9000
78 #define CTRL_DEFAULT_NUM_EA_KI_SHELL /*5000 9000*/   8000
79
80 /**
81  *  \var     CTRL_DEFAULT_NUM_H_R
82  *  \brief   Numerator of default proportionnal gain for yaw (r) angular rate control loop
83  */
84 #define CTRL_DEFAULT_NUM_R_KP 200000
85
86 /**
87  *  \var     CTRL_DEFAULT_NUM_R_KI
88  *  \brief   Numerator of default integral gain for yaw control loops
89  */
90 #define CTRL_DEFAULT_NUM_R_KI 10000
91
92 /**
93  *  \var     CTRL_DEFAULT_DEN_W
94  *  \brief   Denominator of default proportionnal gain of pitch (p) roll (q) and yaw (r) angular rate control loops
95  */
96 #define CTRL_DEFAULT_DEN_W 1024.0 //2^10
97
98 /**
99  *  \var     CTRL_DEFAULT_DEN_EA
100  *  \brief   Denominator of default gains for Euler Angle control loops
101  */
102 #define CTRL_DEFAULT_DEN_EA 1024.0 //2^10
103
104 /**
105  *  \var     CTRL_DEFAULT_NUM_ALT_KP
106  *  \brief   Numerator of default proportionnal gain for Altitude control loop
107  */
108 #ifdef NEW_ALT_FUSION
109 #define CTRL_DEFAULT_NUM_ALT_KP 4000
110 #else
111 #define CTRL_DEFAULT_NUM_ALT_KP 2000
112 #endif
113 /**
114  *  \var     CTRL_DEFAULT_NUM_ALT_KI
115  *  \brief   Numerator of default integral gain for Altitude control loop
116  */
117 #ifdef NEW_ALT_FUSION
118         #define CTRL_DEFAULT_NUM_ALT_KI 2000
119 #else
120         #define CTRL_DEFAULT_NUM_ALT_KI 400
121 #endif
122 /**
123  *  \var     CTRL_DEFAULT_NUM_ALT_KD
124  *  \brief   Numerator of default derivative gain for Altitude control loop
125  */
126 #define CTRL_DEFAULT_NUM_VZ_KP 200
127
128 /**
129  *  \var     CTRL_DEFAULT_NUM_ALT_TD
130  *  \brief   Numerator of default derivative time constant gain for Altitude control loop
131  */
132 #define CTRL_DEFAULT_NUM_VZ_KI 100
133
134 /**
135  *  \var     CTRL_DEFAULT_DEN_ALT
136  *  \brief   Denominator of default gains for Altitude control loop
137  */
138 #define CTRL_DEFAULT_DEN_ALT 1024.0
139
140 /**
141  *  \var     CTRL_DEFAULT_NUM_HOVER_KP
142  *  \brief   Numerator of default proportionnal gain for hovering control loop
143  */
144 #define CTRL_DEFAULT_NUM_HOVER_KP_SHELL /*5000* 8000*/ 8000
145 #define CTRL_DEFAULT_NUM_HOVER_KP_NO_SHELL /*6000 12000 5000*/ 7000
146
147 /**
148  *  \var     CTRL_DEFAULT_NUM_HOVER_KP
149  *  \brief   Numerator of default proportionnal gain for hovering beacon control loop
150  */
151 #define CTRL_DEFAULT_NUM_HOVER_B_KP_SHELL 1200
152 #define CTRL_DEFAULT_NUM_HOVER_B_KP_NO_SHELL 1200
153
154 /**
155  *  \var     CTRL_DEFAULT_NUM_HOVER_KI
156  *  \brief   Numerator of default integral gain for hovering control loop
157  */
158 #define CTRL_DEFAULT_NUM_HOVER_KI_SHELL /*3000 10000*/ 8000
159 #define CTRL_DEFAULT_NUM_HOVER_KI_NO_SHELL /*3000 8000 5000*/ 6000
160
161 /**
162  *  \var     CTRL_DEFAULT_NUM_HOVER_KI
163  *  \brief   Numerator of default integral gain for hovering beacon control loop
164  */
165 #define CTRL_DEFAULT_NUM_HOVER_B_KI_SHELL 500
166 #define CTRL_DEFAULT_NUM_HOVER_B_KI_NO_SHELL 500
167
168 /*
169  *  \var     CTRL_DEFAULT_DEN_HOVER
170  *  \brief   Numerator of default proportionnal gain for hovering control loop
171  */
172 #define CTRL_DEFAULT_DEN_HOVER 32768.0 //2^15
173
174
175 /* Timeout for mayday maneuvers*/
176 static const int32_t MAYDAY_TIMEOUT[ARDRONE_NB_ANIM_MAYDAY] = {
177     1000,  // ARDRONE_ANIM_PHI_M30_DEG
178     1000,  // ARDRONE_ANIM_PHI_30_DEG
179     1000,  // ARDRONE_ANIM_THETA_M30_DEG
180     1000,  // ARDRONE_ANIM_THETA_30_DEG
181     1000,  // ARDRONE_ANIM_THETA_20DEG_YAW_200DEG
182     1000,  // ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG
183     5000,  // ARDRONE_ANIM_TURNAROUND
184     5000,  // ARDRONE_ANIM_TURNAROUND_GODOWN
185     2000,  // ARDRONE_ANIM_YAW_SHAKE
186     5000,  // ARDRONE_ANIM_YAW_DANCE
187     5000,  // ARDRONE_ANIM_PHI_DANCE
188     5000,  // ARDRONE_ANIM_THETA_DANCE
189     5000,  // ARDRONE_ANIM_VZ_DANCE
190     5000,  // ARDRONE_ANIM_WAVE
191     5000,  // ARDRONE_ANIM_PHI_THETA_MIXED
192     5000,  // ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED
193 };
194
195 #define NAVDATA_SEQUENCE_DEFAULT  1
196
197 #define NAVDATA_HEADER  0x55667788
198
199 #define NAVDATA_MAX_SIZE 2048
200 #define NAVDATA_MAX_CUSTOM_TIME_SAVE 20
201
202 /* !!! Warning !!! - changing the value below would break compatibility with older applications
203  * DO NOT CHANGE THIS  */
204 #define NB_NAVDATA_DETECTION_RESULTS 4
205
206
207 /**
208  * @brief Tags identifying navdata blocks in a Navdata UDP packet
209  * This tag is stored in the first two bytes of any navdata block (aka 'option').
210  */
211
212 #define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG)  TAG = 0,
213 #define NAVDATA_OPTION(STRUCTURE,NAME,TAG)       TAG ,
214 #define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG)   NAVDATA_NUM_TAGS, TAG = 0xFFFF
215
216 typedef enum _navdata_tag_t {
217         #include <navdata_keys.h>
218 } navdata_tag_t;
219
220 #define NAVDATA_OPTION_MASK(option) ( 1 << (option) )
221 #define NAVDATA_OPTION_FULL_MASK    ((1<<NAVDATA_NUM_TAGS)-1)
222
223
224 typedef struct _navdata_option_t {
225   uint16_t  tag;
226   uint16_t  size;
227 #if defined _MSC_VER
228   uint8_t   *data;
229 #else
230   uint8_t   data[];
231 #endif
232 } navdata_option_t;
233
234
235 /**
236  * @brief Navdata structure sent over the network.
237  */
238 typedef struct _navdata_t {
239   uint32_t    header;                   /*!< Always set to NAVDATA_HEADER */
240   uint32_t    ardrone_state;    /*!< Bit mask built from def_ardrone_state_mask_t */
241   uint32_t    sequence;         /*!< Sequence number, incremented for each sent packet */
242   bool_t      vision_defined;
243
244   navdata_option_t  options[1];
245 }_ATTRIBUTE_PACKED_ navdata_t;
246
247
248 /**
249  * All navdata options can be extended (new values AT THE END) except navdata_demo whose size must be constant across versions
250  * New navdata options may be added, but must not be sent in navdata_demo mode unless requested by navdata_options.
251  */
252
253 /*----------------------------------------------------------------------------*/
254 /**
255  * @brief Minimal navigation data for all flights.
256  */
257 typedef struct _navdata_demo_t {
258   uint16_t    tag;                                        /*!< Navdata block ('option') identifier */
259   uint16_t    size;                                       /*!< set this to the size of this structure */
260
261   uint32_t    ctrl_state;             /*!< Flying state (landed, flying, hovering, etc.) defined in CTRL_STATES enum. */
262   uint32_t    vbat_flying_percentage; /*!< battery voltage filtered (mV) */
263
264   float32_t   theta;                  /*!< UAV's pitch in milli-degrees */
265   float32_t   phi;                    /*!< UAV's roll  in milli-degrees */
266   float32_t   psi;                    /*!< UAV's yaw   in milli-degrees */
267
268   int32_t     altitude;               /*!< UAV's altitude in centimeters */
269
270   float32_t   vx;                     /*!< UAV's estimated linear velocity */
271   float32_t   vy;                     /*!< UAV's estimated linear velocity */
272   float32_t   vz;                     /*!< UAV's estimated linear velocity */
273
274   uint32_t    num_frames;                         /*!< streamed frame index */ // Not used -> To integrate in video stage.
275
276   // Camera parameters compute by detection
277   matrix33_t  detection_camera_rot;   /*!<  Deprecated ! Don't use ! */
278   vector31_t  detection_camera_trans; /*!<  Deprecated ! Don't use ! */
279   uint32_t        detection_tag_index;    /*!<  Deprecated ! Don't use ! */
280
281   uint32_t        detection_camera_type; /*!<  Type of tag searched in detection */
282
283   // Camera parameters compute by drone
284   matrix33_t  drone_camera_rot;         /*!<  Deprecated ! Don't use ! */
285   vector31_t  drone_camera_trans;       /*!<  Deprecated ! Don't use ! */
286 }_ATTRIBUTE_PACKED_ navdata_demo_t;
287
288
289
290 /*----------------------------------------------------------------------------*/
291 /**
292  * @brief Last navdata option that *must* be included at the end of all navdata packets
293  * + 6 bytes
294  */
295 typedef struct _navdata_cks_t {
296   uint16_t  tag;
297   uint16_t  size;
298
299   // Checksum for all navdatas (including options)
300   uint32_t  cks;
301 }_ATTRIBUTE_PACKED_ navdata_cks_t;
302
303
304 /*----------------------------------------------------------------------------*/
305 /**
306  * @brief Timestamp
307  * + 6 bytes
308  */
309 typedef struct _navdata_time_t {
310   uint16_t  tag;
311   uint16_t  size;
312
313   uint32_t  time;  /*!< 32 bit value where the 11 most significant bits represents the seconds, and the 21 least significant bits are the microseconds. */
314 }_ATTRIBUTE_PACKED_ navdata_time_t;
315
316
317
318 /*----------------------------------------------------------------------------*/
319 /**
320  * @brief Raw sensors measurements
321  */
322 typedef struct _navdata_raw_measures_t {
323   uint16_t  tag;
324   uint16_t  size;
325
326   // +12 bytes
327   uint16_t  raw_accs[NB_ACCS];    // filtered accelerometers
328   uint16_t  raw_gyros[NB_GYROS];  // filtered gyrometers
329   uint16_t  raw_gyros_110[2];     // gyrometers  x/y 110 deg/s
330   uint32_t  vbat_raw;             // battery voltage raw (mV)
331   uint16_t  us_debut_echo;
332   uint16_t  us_fin_echo;
333   uint16_t  us_association_echo;
334   uint16_t  us_distance_echo;
335   uint16_t  us_courbe_temps;
336   uint16_t  us_courbe_valeur;
337   uint16_t  us_courbe_ref;
338   uint16_t  flag_echo_ini;
339 }_ATTRIBUTE_PACKED_ navdata_raw_measures_t;
340
341
342 typedef struct _navdata_phys_measures_t {
343   uint16_t   tag;
344   uint16_t   size;
345
346   float32_t   accs_temp;
347   uint16_t    gyro_temp;
348   float32_t   phys_accs[NB_ACCS];
349   float32_t   phys_gyros[NB_GYROS];
350   uint32_t    alim3V3;              // 3.3volt alim [LSB]
351   uint32_t    vrefEpson;            // ref volt Epson gyro [LSB]
352   uint32_t    vrefIDG;              // ref volt IDG gyro [LSB]
353 }_ATTRIBUTE_PACKED_ navdata_phys_measures_t;
354
355
356 typedef struct _navdata_gyros_offsets_t {
357   uint16_t   tag;
358   uint16_t   size;
359
360   float32_t offset_g[NB_GYROS];
361 }_ATTRIBUTE_PACKED_ navdata_gyros_offsets_t;
362
363
364 typedef struct _navdata_euler_angles_t {
365   uint16_t   tag;
366   uint16_t   size;
367
368   float32_t   theta_a;
369   float32_t   phi_a;
370 }_ATTRIBUTE_PACKED_ navdata_euler_angles_t;
371
372
373 typedef struct _navdata_references_t {
374   uint16_t   tag;
375   uint16_t   size;
376
377   int32_t   ref_theta;
378   int32_t   ref_phi;
379   int32_t   ref_theta_I;
380   int32_t   ref_phi_I;
381   int32_t   ref_pitch;
382   int32_t   ref_roll;
383   int32_t   ref_yaw;
384   int32_t   ref_psi;
385 }_ATTRIBUTE_PACKED_ navdata_references_t;
386
387
388 typedef struct _navdata_trims_t {
389   uint16_t   tag;
390   uint16_t   size;
391
392   float32_t angular_rates_trim_r;
393   float32_t euler_angles_trim_theta;
394   float32_t euler_angles_trim_phi;
395 }_ATTRIBUTE_PACKED_ navdata_trims_t;
396
397 typedef struct _navdata_rc_references_t {
398   uint16_t   tag;
399   uint16_t   size;
400
401   int32_t    rc_ref_pitch;
402   int32_t    rc_ref_roll;
403   int32_t    rc_ref_yaw;
404   int32_t    rc_ref_gaz;
405   int32_t    rc_ref_ag;
406 }_ATTRIBUTE_PACKED_ navdata_rc_references_t;
407
408
409 typedef struct _navdata_pwm_t {
410   uint16_t   tag;
411   uint16_t   size;
412
413   uint8_t     motor1;
414   uint8_t     motor2;
415   uint8_t     motor3;
416   uint8_t     motor4;
417   uint8_t         sat_motor1;
418   uint8_t         sat_motor2;
419   uint8_t         sat_motor3;
420   uint8_t         sat_motor4;
421   int32_t     gaz_feed_forward;
422   int32_t     gaz_altitude;
423   float32_t   altitude_integral;
424   float32_t   vz_ref;
425   int32_t     u_pitch;
426   int32_t     u_roll;
427   int32_t     u_yaw;
428   float32_t   yaw_u_I;
429   int32_t     u_pitch_planif;
430   int32_t     u_roll_planif;
431   int32_t     u_yaw_planif;
432   float32_t   u_gaz_planif;
433   uint16_t    current_motor1;
434   uint16_t    current_motor2;
435   uint16_t    current_motor3;
436   uint16_t    current_motor4;
437 }_ATTRIBUTE_PACKED_ navdata_pwm_t;
438
439
440 typedef struct _navdata_altitude_t {
441   uint16_t   tag;
442   uint16_t   size;
443
444   int32_t   altitude_vision;
445   float32_t altitude_vz;
446   int32_t   altitude_ref;
447   int32_t   altitude_raw;
448
449         float32_t       obs_accZ;
450         float32_t obs_alt;
451         vector31_t obs_x;
452         uint32_t        obs_state;
453         vector21_t      est_vb;
454         uint32_t        est_state ;
455
456 }_ATTRIBUTE_PACKED_ navdata_altitude_t;
457
458
459 typedef struct _navdata_vision_raw_t {
460   uint16_t   tag;
461   uint16_t   size;
462
463   float32_t vision_tx_raw;
464   float32_t vision_ty_raw;
465   float32_t vision_tz_raw;
466 }_ATTRIBUTE_PACKED_ navdata_vision_raw_t;
467
468
469 typedef struct _navdata_vision_t {
470   uint16_t   tag;
471   uint16_t   size;
472
473   uint32_t   vision_state;
474   int32_t    vision_misc;
475   float32_t  vision_phi_trim;
476   float32_t  vision_phi_ref_prop;
477   float32_t  vision_theta_trim;
478   float32_t  vision_theta_ref_prop;
479
480   int32_t    new_raw_picture;
481   float32_t  theta_capture;
482   float32_t  phi_capture;
483   float32_t  psi_capture;
484   int32_t    altitude_capture;
485   uint32_t   time_capture;     // time in TSECDEC format (see config.h)
486   velocities_t body_v;
487
488   float32_t  delta_phi;
489   float32_t  delta_theta;
490   float32_t  delta_psi;
491
492         uint32_t  gold_defined;
493         uint32_t  gold_reset;
494         float32_t gold_x;
495         float32_t gold_y;
496 }_ATTRIBUTE_PACKED_ navdata_vision_t;
497
498
499 typedef struct _navdata_vision_perf_t {
500   uint16_t   tag;
501   uint16_t   size;
502
503   // +44 bytes
504   float32_t  time_szo;
505   float32_t  time_corners;
506   float32_t  time_compute;
507   float32_t  time_tracking;
508   float32_t  time_trans;
509   float32_t  time_update;
510         float32_t  time_custom[NAVDATA_MAX_CUSTOM_TIME_SAVE];
511 }_ATTRIBUTE_PACKED_ navdata_vision_perf_t;
512
513
514 typedef struct _navdata_trackers_send_t {
515   uint16_t   tag;
516   uint16_t   size;
517
518   int32_t locked[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT];
519   screen_point_t point[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT];
520 }_ATTRIBUTE_PACKED_ navdata_trackers_send_t;
521
522
523 typedef struct _navdata_vision_detect_t {
524         /* !! Change the function 'navdata_server_reset_vision_detect()' if this structure is modified !! */
525   uint16_t   tag;
526   uint16_t   size;
527
528   uint32_t   nb_detected;
529   uint32_t   type[NB_NAVDATA_DETECTION_RESULTS];
530   uint32_t   xc[NB_NAVDATA_DETECTION_RESULTS];
531   uint32_t   yc[NB_NAVDATA_DETECTION_RESULTS];
532   uint32_t   width[NB_NAVDATA_DETECTION_RESULTS];
533   uint32_t   height[NB_NAVDATA_DETECTION_RESULTS];
534   uint32_t   dist[NB_NAVDATA_DETECTION_RESULTS];
535   float32_t  orientation_angle[NB_NAVDATA_DETECTION_RESULTS];
536 }_ATTRIBUTE_PACKED_ navdata_vision_detect_t;
537
538 typedef struct _navdata_vision_of_t {
539   uint16_t   tag;
540   uint16_t   size;
541
542   float32_t   of_dx[5];
543   float32_t   of_dy[5];
544 }_ATTRIBUTE_PACKED_ navdata_vision_of_t;
545
546
547 typedef struct _navdata_watchdog_t {
548   uint16_t   tag;
549   uint16_t   size;
550
551   // +4 bytes
552   int32_t    watchdog;
553 }_ATTRIBUTE_PACKED_ navdata_watchdog_t;
554
555 typedef struct _navdata_adc_data_frame_t {
556   uint16_t  tag;
557   uint16_t  size;
558
559   uint32_t  version;
560   uint8_t   data_frame[32];
561 }_ATTRIBUTE_PACKED_ navdata_adc_data_frame_t;
562
563 typedef struct _navdata_video_stream_t {
564   uint16_t  tag;
565   uint16_t  size;
566
567   uint8_t       quant;                  // quantizer reference used to encode frame [1:31]
568   uint32_t      frame_size;             // frame size (bytes)
569   uint32_t      frame_number;   // frame index
570   uint32_t      atcmd_ref_seq;  // atmcd ref sequence number
571   uint32_t      atcmd_mean_ref_gap;     // mean time between two consecutive atcmd_ref (ms)
572   float32_t atcmd_var_ref_gap;
573   uint32_t      atcmd_ref_quality; // estimator of atcmd link quality
574 }_ATTRIBUTE_PACKED_ navdata_video_stream_t;
575
576
577 typedef struct _navdata_games_t {
578   uint16_t  tag;
579   uint16_t  size;
580   uint32_t  double_tap_counter;
581   uint32_t  finish_line_counter;
582 }_ATTRIBUTE_PACKED_ navdata_games_t;
583
584 #if defined(_MSC_VER)
585         /* Go back to default packing policy */
586         #pragma pack()
587 #endif
588
589 #endif // _NAVDATA_COMMON_H_
590