2 * \file navdata_common.h
3 * \brief Common navdata configuration
4 * \author Sylvain Gaeremynck <sylvain.gaeremynck@parrot.com>
7 #ifndef _NAVDATA_COMMON_H_
8 #define _NAVDATA_COMMON_H_
10 /*------------------------------------------ NAVDATA STRUCTURES DECLARATIONS ---------------------------------------------------------------*/
13 #include <vision_common.h>
15 #include <VP_Os/vp_os_types.h>
16 #include <Maths/maths.h>
17 #include <Maths/matrices.h>
20 #define _ATTRIBUTE_PACKED_
21 /* Asks Visual C++ to pack structures from now on*/
24 #define _ATTRIBUTE_PACKED_ __attribute__ ((packed))
27 // Define constants for gyrometers handling
36 // Define constants for accelerometers handling
45 * \struct _velocities_t
46 * \brief Velocities in float32_t format
48 typedef struct _velocities_t {
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
60 * \var CTRL_DEFAULT_NUM_PQ_KP
61 * \brief Numerator of default proportionnal gain for pitch (p) and roll (q) angular rate control loops
64 * \var CTRL_DEFAULT_NUM_EA_KP
65 * \brief Numerator of default proportionnal gain for Euler Angle control loops
68 * \var CTRL_DEFAULT_NUM_EA_KI
69 * \brief Numerator of default integral gain for Euler Angle control loops
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
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
81 * \var CTRL_DEFAULT_NUM_H_R
82 * \brief Numerator of default proportionnal gain for yaw (r) angular rate control loop
84 #define CTRL_DEFAULT_NUM_R_KP 200000
87 * \var CTRL_DEFAULT_NUM_R_KI
88 * \brief Numerator of default integral gain for yaw control loops
90 #define CTRL_DEFAULT_NUM_R_KI 10000
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
96 #define CTRL_DEFAULT_DEN_W 1024.0 //2^10
99 * \var CTRL_DEFAULT_DEN_EA
100 * \brief Denominator of default gains for Euler Angle control loops
102 #define CTRL_DEFAULT_DEN_EA 1024.0 //2^10
105 * \var CTRL_DEFAULT_NUM_ALT_KP
106 * \brief Numerator of default proportionnal gain for Altitude control loop
108 #ifdef NEW_ALT_FUSION
109 #define CTRL_DEFAULT_NUM_ALT_KP 4000
111 #define CTRL_DEFAULT_NUM_ALT_KP 2000
114 * \var CTRL_DEFAULT_NUM_ALT_KI
115 * \brief Numerator of default integral gain for Altitude control loop
117 #ifdef NEW_ALT_FUSION
118 #define CTRL_DEFAULT_NUM_ALT_KI 2000
120 #define CTRL_DEFAULT_NUM_ALT_KI 400
123 * \var CTRL_DEFAULT_NUM_ALT_KD
124 * \brief Numerator of default derivative gain for Altitude control loop
126 #define CTRL_DEFAULT_NUM_VZ_KP 200
129 * \var CTRL_DEFAULT_NUM_ALT_TD
130 * \brief Numerator of default derivative time constant gain for Altitude control loop
132 #define CTRL_DEFAULT_NUM_VZ_KI 100
135 * \var CTRL_DEFAULT_DEN_ALT
136 * \brief Denominator of default gains for Altitude control loop
138 #define CTRL_DEFAULT_DEN_ALT 1024.0
141 * \var CTRL_DEFAULT_NUM_HOVER_KP
142 * \brief Numerator of default proportionnal gain for hovering control loop
144 #define CTRL_DEFAULT_NUM_HOVER_KP_SHELL /*5000* 8000*/ 8000
145 #define CTRL_DEFAULT_NUM_HOVER_KP_NO_SHELL /*6000 12000 5000*/ 7000
148 * \var CTRL_DEFAULT_NUM_HOVER_KP
149 * \brief Numerator of default proportionnal gain for hovering beacon control loop
151 #define CTRL_DEFAULT_NUM_HOVER_B_KP_SHELL 1200
152 #define CTRL_DEFAULT_NUM_HOVER_B_KP_NO_SHELL 1200
155 * \var CTRL_DEFAULT_NUM_HOVER_KI
156 * \brief Numerator of default integral gain for hovering control loop
158 #define CTRL_DEFAULT_NUM_HOVER_KI_SHELL /*3000 10000*/ 8000
159 #define CTRL_DEFAULT_NUM_HOVER_KI_NO_SHELL /*3000 8000 5000*/ 6000
162 * \var CTRL_DEFAULT_NUM_HOVER_KI
163 * \brief Numerator of default integral gain for hovering beacon control loop
165 #define CTRL_DEFAULT_NUM_HOVER_B_KI_SHELL 500
166 #define CTRL_DEFAULT_NUM_HOVER_B_KI_NO_SHELL 500
169 * \var CTRL_DEFAULT_DEN_HOVER
170 * \brief Numerator of default proportionnal gain for hovering control loop
172 #define CTRL_DEFAULT_DEN_HOVER 32768.0 //2^15
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
195 #define NAVDATA_SEQUENCE_DEFAULT 1
197 #define NAVDATA_HEADER 0x55667788
199 #define NAVDATA_MAX_SIZE 2048
200 #define NAVDATA_MAX_CUSTOM_TIME_SAVE 20
202 /* !!! Warning !!! - changing the value below would break compatibility with older applications
203 * DO NOT CHANGE THIS */
204 #define NB_NAVDATA_DETECTION_RESULTS 4
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').
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
216 typedef enum _navdata_tag_t {
217 #include <navdata_keys.h>
220 #define NAVDATA_OPTION_MASK(option) ( 1 << (option) )
221 #define NAVDATA_OPTION_FULL_MASK ((1<<NAVDATA_NUM_TAGS)-1)
224 typedef struct _navdata_option_t {
236 * @brief Navdata structure sent over the network.
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;
244 navdata_option_t options[1];
245 }_ATTRIBUTE_PACKED_ navdata_t;
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.
253 /*----------------------------------------------------------------------------*/
255 * @brief Minimal navigation data for all flights.
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 */
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) */
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 */
268 int32_t altitude; /*!< UAV's altitude in centimeters */
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 */
274 uint32_t num_frames; /*!< streamed frame index */ // Not used -> To integrate in video stage.
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 ! */
281 uint32_t detection_camera_type; /*!< Type of tag searched in detection */
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;
290 /*----------------------------------------------------------------------------*/
292 * @brief Last navdata option that *must* be included at the end of all navdata packets
295 typedef struct _navdata_cks_t {
299 // Checksum for all navdatas (including options)
301 }_ATTRIBUTE_PACKED_ navdata_cks_t;
304 /*----------------------------------------------------------------------------*/
309 typedef struct _navdata_time_t {
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;
318 /*----------------------------------------------------------------------------*/
320 * @brief Raw sensors measurements
322 typedef struct _navdata_raw_measures_t {
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;
342 typedef struct _navdata_phys_measures_t {
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;
356 typedef struct _navdata_gyros_offsets_t {
360 float32_t offset_g[NB_GYROS];
361 }_ATTRIBUTE_PACKED_ navdata_gyros_offsets_t;
364 typedef struct _navdata_euler_angles_t {
370 }_ATTRIBUTE_PACKED_ navdata_euler_angles_t;
373 typedef struct _navdata_references_t {
385 }_ATTRIBUTE_PACKED_ navdata_references_t;
388 typedef struct _navdata_trims_t {
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;
397 typedef struct _navdata_rc_references_t {
401 int32_t rc_ref_pitch;
406 }_ATTRIBUTE_PACKED_ navdata_rc_references_t;
409 typedef struct _navdata_pwm_t {
421 int32_t gaz_feed_forward;
422 int32_t gaz_altitude;
423 float32_t altitude_integral;
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;
440 typedef struct _navdata_altitude_t {
444 int32_t altitude_vision;
445 float32_t altitude_vz;
446 int32_t altitude_ref;
447 int32_t altitude_raw;
456 }_ATTRIBUTE_PACKED_ navdata_altitude_t;
459 typedef struct _navdata_vision_raw_t {
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;
469 typedef struct _navdata_vision_t {
473 uint32_t vision_state;
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;
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)
489 float32_t delta_theta;
492 uint32_t gold_defined;
496 }_ATTRIBUTE_PACKED_ navdata_vision_t;
499 typedef struct _navdata_vision_perf_t {
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;
514 typedef struct _navdata_trackers_send_t {
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;
523 typedef struct _navdata_vision_detect_t {
524 /* !! Change the function 'navdata_server_reset_vision_detect()' if this structure is modified !! */
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;
538 typedef struct _navdata_vision_of_t {
544 }_ATTRIBUTE_PACKED_ navdata_vision_of_t;
547 typedef struct _navdata_watchdog_t {
553 }_ATTRIBUTE_PACKED_ navdata_watchdog_t;
555 typedef struct _navdata_adc_data_frame_t {
560 uint8_t data_frame[32];
561 }_ATTRIBUTE_PACKED_ navdata_adc_data_frame_t;
563 typedef struct _navdata_video_stream_t {
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;
577 typedef struct _navdata_games_t {
580 uint32_t double_tap_counter;
581 uint32_t finish_line_counter;
582 }_ATTRIBUTE_PACKED_ navdata_games_t;
584 #if defined(_MSC_VER)
585 /* Go back to default packing policy */
589 #endif // _NAVDATA_COMMON_H_