X-Git-Url: http://git.maemo.org/git/?p=mardrone;a=blobdiff_plain;f=mardrone%2FARDrone_SDK_Version_1_8_20110726%2FARDroneLib%2FSoft%2FCommon%2Fnavdata_common.h;fp=mardrone%2FARDrone_SDK_Version_1_8_20110726%2FARDroneLib%2FSoft%2FCommon%2Fnavdata_common.h;h=e00024404dac25929537816a429b744326cbe834;hp=0000000000000000000000000000000000000000;hb=9ec9bc13b75d30bc45535c54a652934debfcea92;hpb=ae0a3c2dc0898400aca0dd6b439c5db8044db7b2 diff --git a/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Common/navdata_common.h b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Common/navdata_common.h new file mode 100644 index 0000000..e000244 --- /dev/null +++ b/mardrone/ARDrone_SDK_Version_1_8_20110726/ARDroneLib/Soft/Common/navdata_common.h @@ -0,0 +1,590 @@ +/** + * \file navdata_common.h + * \brief Common navdata configuration + * \author Sylvain Gaeremynck + */ + +#ifndef _NAVDATA_COMMON_H_ +#define _NAVDATA_COMMON_H_ + +/*------------------------------------------ NAVDATA STRUCTURES DECLARATIONS ---------------------------------------------------------------*/ + +#include +#include + +#include +#include +#include + +#if defined(_MSC_VER) + #define _ATTRIBUTE_PACKED_ + /* Asks Visual C++ to pack structures from now on*/ + #pragma pack(1) +#else + #define _ATTRIBUTE_PACKED_ __attribute__ ((packed)) +#endif + +// Define constants for gyrometers handling +typedef enum { + GYRO_X = 0, + GYRO_Y = 1, + GYRO_Z = 2, + NB_GYROS = 3 +} def_gyro_t; + + +// Define constants for accelerometers handling +typedef enum { + ACC_X = 0, + ACC_Y = 1, + ACC_Z = 2, + NB_ACCS = 3 +} def_acc_t; + +/** + * \struct _velocities_t + * \brief Velocities in float32_t format + */ +typedef struct _velocities_t { + float32_t x; + float32_t y; + float32_t z; +} velocities_t; + +// Default control loops gains TODO Put these values in flash memory +// To avoid divisions in embedded software, gains are defined as ratios where +// - the numerator is an integer +// - the denominator is an integer + +/** + * \var CTRL_DEFAULT_NUM_PQ_KP + * \brief Numerator of default proportionnal gain for pitch (p) and roll (q) angular rate control loops + */ +/** + * \var CTRL_DEFAULT_NUM_EA_KP + * \brief Numerator of default proportionnal gain for Euler Angle control loops + */ +/** + * \var CTRL_DEFAULT_NUM_EA_KI + * \brief Numerator of default integral gain for Euler Angle control loops + */ + +#define CTRL_DEFAULT_NUM_PQ_KP_NO_SHELL /*30000 26000 */ 40000 +#define CTRL_DEFAULT_NUM_EA_KP_NO_SHELL /*7000 9000 7000 */ 8000 +#define CTRL_DEFAULT_NUM_EA_KI_NO_SHELL /*4000 7000 6000 */ 7000 + +#define CTRL_DEFAULT_NUM_PQ_KP_SHELL /*30000 23000*/ 40000 +#define CTRL_DEFAULT_NUM_EA_KP_SHELL /*9000 10000*/ 9000 +#define CTRL_DEFAULT_NUM_EA_KI_SHELL /*5000 9000*/ 8000 + +/** + * \var CTRL_DEFAULT_NUM_H_R + * \brief Numerator of default proportionnal gain for yaw (r) angular rate control loop + */ +#define CTRL_DEFAULT_NUM_R_KP 200000 + +/** + * \var CTRL_DEFAULT_NUM_R_KI + * \brief Numerator of default integral gain for yaw control loops + */ +#define CTRL_DEFAULT_NUM_R_KI 10000 + +/** + * \var CTRL_DEFAULT_DEN_W + * \brief Denominator of default proportionnal gain of pitch (p) roll (q) and yaw (r) angular rate control loops + */ +#define CTRL_DEFAULT_DEN_W 1024.0 //2^10 + +/** + * \var CTRL_DEFAULT_DEN_EA + * \brief Denominator of default gains for Euler Angle control loops + */ +#define CTRL_DEFAULT_DEN_EA 1024.0 //2^10 + +/** + * \var CTRL_DEFAULT_NUM_ALT_KP + * \brief Numerator of default proportionnal gain for Altitude control loop + */ +#ifdef NEW_ALT_FUSION +#define CTRL_DEFAULT_NUM_ALT_KP 4000 +#else +#define CTRL_DEFAULT_NUM_ALT_KP 2000 +#endif +/** + * \var CTRL_DEFAULT_NUM_ALT_KI + * \brief Numerator of default integral gain for Altitude control loop + */ +#ifdef NEW_ALT_FUSION + #define CTRL_DEFAULT_NUM_ALT_KI 2000 +#else + #define CTRL_DEFAULT_NUM_ALT_KI 400 +#endif +/** + * \var CTRL_DEFAULT_NUM_ALT_KD + * \brief Numerator of default derivative gain for Altitude control loop + */ +#define CTRL_DEFAULT_NUM_VZ_KP 200 + +/** + * \var CTRL_DEFAULT_NUM_ALT_TD + * \brief Numerator of default derivative time constant gain for Altitude control loop + */ +#define CTRL_DEFAULT_NUM_VZ_KI 100 + +/** + * \var CTRL_DEFAULT_DEN_ALT + * \brief Denominator of default gains for Altitude control loop + */ +#define CTRL_DEFAULT_DEN_ALT 1024.0 + +/** + * \var CTRL_DEFAULT_NUM_HOVER_KP + * \brief Numerator of default proportionnal gain for hovering control loop + */ +#define CTRL_DEFAULT_NUM_HOVER_KP_SHELL /*5000* 8000*/ 8000 +#define CTRL_DEFAULT_NUM_HOVER_KP_NO_SHELL /*6000 12000 5000*/ 7000 + +/** + * \var CTRL_DEFAULT_NUM_HOVER_KP + * \brief Numerator of default proportionnal gain for hovering beacon control loop + */ +#define CTRL_DEFAULT_NUM_HOVER_B_KP_SHELL 1200 +#define CTRL_DEFAULT_NUM_HOVER_B_KP_NO_SHELL 1200 + +/** + * \var CTRL_DEFAULT_NUM_HOVER_KI + * \brief Numerator of default integral gain for hovering control loop + */ +#define CTRL_DEFAULT_NUM_HOVER_KI_SHELL /*3000 10000*/ 8000 +#define CTRL_DEFAULT_NUM_HOVER_KI_NO_SHELL /*3000 8000 5000*/ 6000 + +/** + * \var CTRL_DEFAULT_NUM_HOVER_KI + * \brief Numerator of default integral gain for hovering beacon control loop + */ +#define CTRL_DEFAULT_NUM_HOVER_B_KI_SHELL 500 +#define CTRL_DEFAULT_NUM_HOVER_B_KI_NO_SHELL 500 + +/* + * \var CTRL_DEFAULT_DEN_HOVER + * \brief Numerator of default proportionnal gain for hovering control loop + */ +#define CTRL_DEFAULT_DEN_HOVER 32768.0 //2^15 + + +/* Timeout for mayday maneuvers*/ +static const int32_t MAYDAY_TIMEOUT[ARDRONE_NB_ANIM_MAYDAY] = { + 1000, // ARDRONE_ANIM_PHI_M30_DEG + 1000, // ARDRONE_ANIM_PHI_30_DEG + 1000, // ARDRONE_ANIM_THETA_M30_DEG + 1000, // ARDRONE_ANIM_THETA_30_DEG + 1000, // ARDRONE_ANIM_THETA_20DEG_YAW_200DEG + 1000, // ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG + 5000, // ARDRONE_ANIM_TURNAROUND + 5000, // ARDRONE_ANIM_TURNAROUND_GODOWN + 2000, // ARDRONE_ANIM_YAW_SHAKE + 5000, // ARDRONE_ANIM_YAW_DANCE + 5000, // ARDRONE_ANIM_PHI_DANCE + 5000, // ARDRONE_ANIM_THETA_DANCE + 5000, // ARDRONE_ANIM_VZ_DANCE + 5000, // ARDRONE_ANIM_WAVE + 5000, // ARDRONE_ANIM_PHI_THETA_MIXED + 5000, // ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED +}; + +#define NAVDATA_SEQUENCE_DEFAULT 1 + +#define NAVDATA_HEADER 0x55667788 + +#define NAVDATA_MAX_SIZE 2048 +#define NAVDATA_MAX_CUSTOM_TIME_SAVE 20 + +/* !!! Warning !!! - changing the value below would break compatibility with older applications + * DO NOT CHANGE THIS */ +#define NB_NAVDATA_DETECTION_RESULTS 4 + + +/** + * @brief Tags identifying navdata blocks in a Navdata UDP packet + * This tag is stored in the first two bytes of any navdata block (aka 'option'). + */ + +#define NAVDATA_OPTION_DEMO(STRUCTURE,NAME,TAG) TAG = 0, +#define NAVDATA_OPTION(STRUCTURE,NAME,TAG) TAG , +#define NAVDATA_OPTION_CKS(STRUCTURE,NAME,TAG) NAVDATA_NUM_TAGS, TAG = 0xFFFF + +typedef enum _navdata_tag_t { + #include +} navdata_tag_t; + +#define NAVDATA_OPTION_MASK(option) ( 1 << (option) ) +#define NAVDATA_OPTION_FULL_MASK ((1< To integrate in video stage. + + // Camera parameters compute by detection + matrix33_t detection_camera_rot; /*!< Deprecated ! Don't use ! */ + vector31_t detection_camera_trans; /*!< Deprecated ! Don't use ! */ + uint32_t detection_tag_index; /*!< Deprecated ! Don't use ! */ + + uint32_t detection_camera_type; /*!< Type of tag searched in detection */ + + // Camera parameters compute by drone + matrix33_t drone_camera_rot; /*!< Deprecated ! Don't use ! */ + vector31_t drone_camera_trans; /*!< Deprecated ! Don't use ! */ +}_ATTRIBUTE_PACKED_ navdata_demo_t; + + + +/*----------------------------------------------------------------------------*/ +/** + * @brief Last navdata option that *must* be included at the end of all navdata packets + * + 6 bytes + */ +typedef struct _navdata_cks_t { + uint16_t tag; + uint16_t size; + + // Checksum for all navdatas (including options) + uint32_t cks; +}_ATTRIBUTE_PACKED_ navdata_cks_t; + + +/*----------------------------------------------------------------------------*/ +/** + * @brief Timestamp + * + 6 bytes + */ +typedef struct _navdata_time_t { + uint16_t tag; + uint16_t size; + + uint32_t time; /*!< 32 bit value where the 11 most significant bits represents the seconds, and the 21 least significant bits are the microseconds. */ +}_ATTRIBUTE_PACKED_ navdata_time_t; + + + +/*----------------------------------------------------------------------------*/ +/** + * @brief Raw sensors measurements + */ +typedef struct _navdata_raw_measures_t { + uint16_t tag; + uint16_t size; + + // +12 bytes + uint16_t raw_accs[NB_ACCS]; // filtered accelerometers + uint16_t raw_gyros[NB_GYROS]; // filtered gyrometers + uint16_t raw_gyros_110[2]; // gyrometers x/y 110 deg/s + uint32_t vbat_raw; // battery voltage raw (mV) + uint16_t us_debut_echo; + uint16_t us_fin_echo; + uint16_t us_association_echo; + uint16_t us_distance_echo; + uint16_t us_courbe_temps; + uint16_t us_courbe_valeur; + uint16_t us_courbe_ref; + uint16_t flag_echo_ini; +}_ATTRIBUTE_PACKED_ navdata_raw_measures_t; + + +typedef struct _navdata_phys_measures_t { + uint16_t tag; + uint16_t size; + + float32_t accs_temp; + uint16_t gyro_temp; + float32_t phys_accs[NB_ACCS]; + float32_t phys_gyros[NB_GYROS]; + uint32_t alim3V3; // 3.3volt alim [LSB] + uint32_t vrefEpson; // ref volt Epson gyro [LSB] + uint32_t vrefIDG; // ref volt IDG gyro [LSB] +}_ATTRIBUTE_PACKED_ navdata_phys_measures_t; + + +typedef struct _navdata_gyros_offsets_t { + uint16_t tag; + uint16_t size; + + float32_t offset_g[NB_GYROS]; +}_ATTRIBUTE_PACKED_ navdata_gyros_offsets_t; + + +typedef struct _navdata_euler_angles_t { + uint16_t tag; + uint16_t size; + + float32_t theta_a; + float32_t phi_a; +}_ATTRIBUTE_PACKED_ navdata_euler_angles_t; + + +typedef struct _navdata_references_t { + uint16_t tag; + uint16_t size; + + int32_t ref_theta; + int32_t ref_phi; + int32_t ref_theta_I; + int32_t ref_phi_I; + int32_t ref_pitch; + int32_t ref_roll; + int32_t ref_yaw; + int32_t ref_psi; +}_ATTRIBUTE_PACKED_ navdata_references_t; + + +typedef struct _navdata_trims_t { + uint16_t tag; + uint16_t size; + + float32_t angular_rates_trim_r; + float32_t euler_angles_trim_theta; + float32_t euler_angles_trim_phi; +}_ATTRIBUTE_PACKED_ navdata_trims_t; + +typedef struct _navdata_rc_references_t { + uint16_t tag; + uint16_t size; + + int32_t rc_ref_pitch; + int32_t rc_ref_roll; + int32_t rc_ref_yaw; + int32_t rc_ref_gaz; + int32_t rc_ref_ag; +}_ATTRIBUTE_PACKED_ navdata_rc_references_t; + + +typedef struct _navdata_pwm_t { + uint16_t tag; + uint16_t size; + + uint8_t motor1; + uint8_t motor2; + uint8_t motor3; + uint8_t motor4; + uint8_t sat_motor1; + uint8_t sat_motor2; + uint8_t sat_motor3; + uint8_t sat_motor4; + int32_t gaz_feed_forward; + int32_t gaz_altitude; + float32_t altitude_integral; + float32_t vz_ref; + int32_t u_pitch; + int32_t u_roll; + int32_t u_yaw; + float32_t yaw_u_I; + int32_t u_pitch_planif; + int32_t u_roll_planif; + int32_t u_yaw_planif; + float32_t u_gaz_planif; + uint16_t current_motor1; + uint16_t current_motor2; + uint16_t current_motor3; + uint16_t current_motor4; +}_ATTRIBUTE_PACKED_ navdata_pwm_t; + + +typedef struct _navdata_altitude_t { + uint16_t tag; + uint16_t size; + + int32_t altitude_vision; + float32_t altitude_vz; + int32_t altitude_ref; + int32_t altitude_raw; + + float32_t obs_accZ; + float32_t obs_alt; + vector31_t obs_x; + uint32_t obs_state; + vector21_t est_vb; + uint32_t est_state ; + +}_ATTRIBUTE_PACKED_ navdata_altitude_t; + + +typedef struct _navdata_vision_raw_t { + uint16_t tag; + uint16_t size; + + float32_t vision_tx_raw; + float32_t vision_ty_raw; + float32_t vision_tz_raw; +}_ATTRIBUTE_PACKED_ navdata_vision_raw_t; + + +typedef struct _navdata_vision_t { + uint16_t tag; + uint16_t size; + + uint32_t vision_state; + int32_t vision_misc; + float32_t vision_phi_trim; + float32_t vision_phi_ref_prop; + float32_t vision_theta_trim; + float32_t vision_theta_ref_prop; + + int32_t new_raw_picture; + float32_t theta_capture; + float32_t phi_capture; + float32_t psi_capture; + int32_t altitude_capture; + uint32_t time_capture; // time in TSECDEC format (see config.h) + velocities_t body_v; + + float32_t delta_phi; + float32_t delta_theta; + float32_t delta_psi; + + uint32_t gold_defined; + uint32_t gold_reset; + float32_t gold_x; + float32_t gold_y; +}_ATTRIBUTE_PACKED_ navdata_vision_t; + + +typedef struct _navdata_vision_perf_t { + uint16_t tag; + uint16_t size; + + // +44 bytes + float32_t time_szo; + float32_t time_corners; + float32_t time_compute; + float32_t time_tracking; + float32_t time_trans; + float32_t time_update; + float32_t time_custom[NAVDATA_MAX_CUSTOM_TIME_SAVE]; +}_ATTRIBUTE_PACKED_ navdata_vision_perf_t; + + +typedef struct _navdata_trackers_send_t { + uint16_t tag; + uint16_t size; + + int32_t locked[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT]; + screen_point_t point[DEFAULT_NB_TRACKERS_WIDTH * DEFAULT_NB_TRACKERS_HEIGHT]; +}_ATTRIBUTE_PACKED_ navdata_trackers_send_t; + + +typedef struct _navdata_vision_detect_t { + /* !! Change the function 'navdata_server_reset_vision_detect()' if this structure is modified !! */ + uint16_t tag; + uint16_t size; + + uint32_t nb_detected; + uint32_t type[NB_NAVDATA_DETECTION_RESULTS]; + uint32_t xc[NB_NAVDATA_DETECTION_RESULTS]; + uint32_t yc[NB_NAVDATA_DETECTION_RESULTS]; + uint32_t width[NB_NAVDATA_DETECTION_RESULTS]; + uint32_t height[NB_NAVDATA_DETECTION_RESULTS]; + uint32_t dist[NB_NAVDATA_DETECTION_RESULTS]; + float32_t orientation_angle[NB_NAVDATA_DETECTION_RESULTS]; +}_ATTRIBUTE_PACKED_ navdata_vision_detect_t; + +typedef struct _navdata_vision_of_t { + uint16_t tag; + uint16_t size; + + float32_t of_dx[5]; + float32_t of_dy[5]; +}_ATTRIBUTE_PACKED_ navdata_vision_of_t; + + +typedef struct _navdata_watchdog_t { + uint16_t tag; + uint16_t size; + + // +4 bytes + int32_t watchdog; +}_ATTRIBUTE_PACKED_ navdata_watchdog_t; + +typedef struct _navdata_adc_data_frame_t { + uint16_t tag; + uint16_t size; + + uint32_t version; + uint8_t data_frame[32]; +}_ATTRIBUTE_PACKED_ navdata_adc_data_frame_t; + +typedef struct _navdata_video_stream_t { + uint16_t tag; + uint16_t size; + + uint8_t quant; // quantizer reference used to encode frame [1:31] + uint32_t frame_size; // frame size (bytes) + uint32_t frame_number; // frame index + uint32_t atcmd_ref_seq; // atmcd ref sequence number + uint32_t atcmd_mean_ref_gap; // mean time between two consecutive atcmd_ref (ms) + float32_t atcmd_var_ref_gap; + uint32_t atcmd_ref_quality; // estimator of atcmd link quality +}_ATTRIBUTE_PACKED_ navdata_video_stream_t; + + +typedef struct _navdata_games_t { + uint16_t tag; + uint16_t size; + uint32_t double_tap_counter; + uint32_t finish_line_counter; +}_ATTRIBUTE_PACKED_ navdata_games_t; + +#if defined(_MSC_VER) + /* Go back to default packing policy */ + #pragma pack() +#endif + +#endif // _NAVDATA_COMMON_H_ +