Get GPS info

CMakeLists.txt

@@ -48,6 +48,10 @@ add_message_files(
  navdata_wifi.msg
  navdata_wind_speed.msg
  navdata_zimmu_3000.msg
+ navdata_gps_channel.msg
+ navdata_gps.msg
+ navdata_camera_settings.msg
+ navdata_host.msg
  vector21.msg
  vector31.msg
 )
@@ -62,7 +66,11 @@ include(ExternalProject)
 ExternalProject_Add(ardronelib
  URL ${PROJECT_SOURCE_DIR}/ARDroneSDK/ardrone-sdk-stripped-2.0.1.tgz
  PREFIX ${CMAKE_BINARY_DIR}/ardrone
- PATCH_COMMAND patch -p1 < ${PROJECT_SOURCE_DIR}/ardrone_lib.patch
+ PATCH_COMMAND
+ patch -p1 < ${PROJECT_SOURCE_DIR}/ardrone_lib.patch &&
+ patch -p0 < ${PROJECT_SOURCE_DIR}/ardrone-sdk-navdata-common-gps.patch &&
+ patch -p0 < ${PROJECT_SOURCE_DIR}/ardrone-sdk-navdata-keys-gps.patch &&
+ patch -p0 < ${PROJECT_SOURCE_DIR}/ardrone-sdk-navdata-file-gps.patch
  CONFIGURE_COMMAND echo "No configure"
  BUILD_COMMAND make
  INSTALL_COMMAND make install INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/lib/ardrone

ardrone-sdk-navdata-common-gps.patch

@@ -0,0 +1,274 @@
+--- ARDroneLib/Soft/Common/navdata_common.h~ 2014-01-10 05:31:23.000000000 +0100
++++ ARDroneLib/Soft/Common/navdata_common.h 2014-02-19 10:13:02.786270040 +0100
+@@ -208,7 +199,10 @@
+
+ #define NAVDATA_SEQUENCE_DEFAULT 1
+
+-#define NAVDATA_HEADER 0x55667788
++#define NAVDATA_HEADER (0x55667788)
++
++/* Quick ACK system */
++#define NAVDATA_DIFFERENTIAL_HEADER (0x55667789)
+
+ #define NAVDATA_MAX_SIZE 4096
+ #define NAVDATA_MAX_CUSTOM_TIME_SAVE 20
+@@ -250,6 +244,9 @@
+ /**
+ * @brief Navdata structure sent over the network.
+ */
++#ifdef _WIN32
++#pragma pack(1)
++#endif
+ typedef struct _navdata_t {
+ uint32_t header; /*!< Always set to NAVDATA_HEADER */
+ uint32_t ardrone_state; /*!< Bit mask built from def_ardrone_state_mask_t */
+@@ -300,13 +297,30 @@
+ 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_host_t_ {
++ uint16_t tag;
++ uint16_t size;
++
++ int32_t host_flag;
++ float32_t host_gaz;
++ float32_t host_yaw;
++ float32_t host_phi;
++ float32_t host_theta;
++ float32_t host_magneto_psi;
++ float32_t host_magneto_psi_accuracy;
++}_ATTRIBUTE_PACKED_ navdata_host_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;
+@@ -343,6 +357,10 @@
+ int16_t raw_gyros[NB_GYROS]; // filtered gyrometers
+ int16_t raw_gyros_110[2]; // gyrometers x/y 110 deg/s
+ uint32_t vbat_raw; // battery voltage raw (mV)
++ /*
++ float32_t vbat_filt_200; // battery voltage filtered @200Hz
++ int16_t emergency_landing_flag; // new emergency flag raised
++ */
+ uint16_t us_debut_echo;
+ uint16_t us_fin_echo;
+ uint16_t us_association_echo;
+@@ -387,6 +405,8 @@
+ float32_t magneto_radius;
+ float32_t error_mean;
+ float32_t error_var;
++ float32_t error_mean_background;
++ float32_t error_var_background;
+
+ }_ATTRIBUTE_PACKED_ navdata_magneto_t;
+
+@@ -407,6 +427,8 @@
+ float32_t magneto_debug1;
+ float32_t magneto_debug2;
+ float32_t magneto_debug3;
++ float32_t state_x7;
++ float32_t state_x8;
+ }_ATTRIBUTE_PACKED_ navdata_wind_speed_t;
+
+ typedef struct _navdata_kalman_pressure_t{
+@@ -433,7 +455,7 @@
+ }_ATTRIBUTE_PACKED_ navdata_kalman_pressure_t;
+
+ // TODO: depreciated struct ? remove it ?
+-typedef struct navdata_zimmu_3000_t {
++/*typedef struct navdata_zimmu_3000_t {
+ uint16_t tag;
+ uint16_t size;
+
+@@ -441,6 +463,7 @@
+ float32_t vzfind;
+
+ }_ATTRIBUTE_PACKED_ navdata_zimmu_3000_t;
++*/
+
+ typedef struct _navdata_phys_measures_t {
+ uint16_t tag;
+@@ -554,12 +577,16 @@
+ uint16_t current_motor2;
+ uint16_t current_motor3;
+ uint16_t current_motor4;
+- //WARNING: new navdata (FC 26/07/2011)
+- float32_t altitude_prop;
+- float32_t altitude_der;
++ //WARNING: new navdata (FC 26/07/2011)
++ float32_t altitude_prop;
++ float32_t altitude_der;
++
++ uint32_t motor1_status;
++ uint32_t motor2_status;
++ uint32_t motor3_status;
++ uint32_t motor4_status;
+ }_ATTRIBUTE_PACKED_ navdata_pwm_t;
+
+-
+ typedef struct _navdata_altitude_t {
+ uint16_t tag;
+ uint16_t size;
+@@ -752,9 +779,148 @@
+ uint32_t link_quality;
+ }_ATTRIBUTE_PACKED_ navdata_wifi_t;
+
+-#if defined(_MSC_VER)
++typedef struct _navdata_gps_t {
++ uint16_t tag;
++ uint16_t size;
++ float64_t latitude;
++ float64_t longitude;
++ float64_t elevation;
++ float64_t hdop;
++ uint32_t data_available;
++ bool_t zero_validated ;
++ bool_t wpt_validated ;
++ float64_t lat0 ;
++ float64_t long0 ;
++ float64_t lat_fused ;
++ float64_t long_fused ;
++
++ uint32_t gps_state ;
++
++ float32_t X_traj ;
++ float32_t X_ref ;
++ float32_t Y_traj ;
++ float32_t Y_ref ;
++
++ float32_t theta_p ;
++ float32_t phi_p ;
++ float32_t theta_i ;
++ float32_t phi_i ;
++ float32_t theta_d ;
++ float32_t phi_d ;
++
++ float64_t vdop;
++ float64_t pdop;
++
++ float32_t speed;
++ uint32_t lastFrameTimestamp;
++ float32_t degree;
++ float32_t degree_magnetic;
++ float32_t ehpe ;
++ float32_t ehve ;
++
++ float32_t c_n0; /* Signal to noise ratio (average of the four best satellites) */
++ uint32_t nbsat; /* Number of acquired satellites */
++ struct{
++ uint8_t sat; /* Satellite ID */
++ uint8_t c_n0; /* Satellite C/N0 */
++ }channels[12];
++ bool_t is_gps_plugged;
++ uint32_t ephemerisStatus;
++
++ float32_t vx_traj ;
++ float32_t vy_traj ;
++
++#if 0
++ /* VectorNav */
++ /* INS */
++ float64_t gpsTime;
++ uint16_t gpsWeek;
++ uint16_t status;
++ float64_t ypr_c0; float64_t ypr_c1; float64_t ypr_c2;
++ float64_t latitudeLongitudeAltitude_c0; float64_t latitudeLongitudeAltitude_c1; float64_t latitudeLongitudeAltitude_c2;
++ float64_t nedVelocity_c0; float64_t nedVelocity_c1; float64_t nedVelocity_c2;
++ float32_t attitudeUncertainty;
++ float32_t positionUncertainty;
++ float32_t velocityUncertainty;
++ /* GPS */
++ float64_t gps_gpsTime;
++ uint16_t gps_gpsWeek;
++ uint8_t gpsFix;
++ uint8_t numberOfSatellites;
++ float64_t gps_latitudeLongitudeAltitude_c0; float64_t gps_latitudeLongitudeAltitude_c1; float64_t gps_latitudeLongitudeAltitude_c2;
++ float64_t gps_nedVelocity_c0; float64_t gps_nedVelocity_c1; float64_t gps_nedVelocity_c2;
++ float64_t positionAccuracy_c0; float64_t positionAccuracy_c1; float64_t positionAccuracy_c2;
++ float32_t speedAccuracy;
++ float32_t timeAccuracy;
++ /* MSR */
++ float64_t magnetic_c0; float64_t magnetic_c1; float64_t magnetic_c2;
++ float64_t acceleration_c0; float64_t acceleration_c1; float64_t acceleration_c2;
++ float64_t angularRate_c0; float64_t angularRate_c1; float64_t angularRate_c2;
++ float32_t temperature;
++ float32_t pressure;
++#endif
++
++ uint32_t firmwareStatus;
++
++}_ATTRIBUTE_PACKED_ navdata_gps_t;
++
++typedef enum {
++ GPS_EPHEMERIS_STATUS__EPHEMERIS_PRESENT = ( 1<<0 ), /* 1= a valid EE file is present on the drone (no matter deprecated or not, used or not) */ 
++ GPS_EPHEMERIS_STATUS__EPHEMERIS_IN_USE = ( 1<<1 ), /* 1= an ephemeris is being used by the GPS (upper 16bits=EE age in days)*/ 
++ GPS_EPHEMERIS_STATUS__EPHEMERIS_UPLOADING = ( 1<<2 ), /* (upper 16bits=upload percentage) */
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_RECEIVED = ( 1<<3 ), /* 1= the 'ephemeris_uploaded' config key was received */
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INVALID_MD5 = ( 1<<4 ),
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INVALID_FORMAT = ( 1<<5 ),
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_CURRENT_FILE_IS_NEWER = ( 1<<6 ),
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INTERNAL_DRONE_ERROR = ( 1<<7 ),
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_NOT_FOUND = ( 1<<8 ), /* This must be =0 before starting an ephemeris upload from the iPhone */
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_SUCCESS = ( 1<<9 ), /* This must be =0 before starting an ephemeris upload from the iPhone */
++
++}_gps_ephemeris_status_t;
++
++#define GPS_EPHEMERIS_STATUS__ERROR_FLAGS ( \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INVALID_MD5 | \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INVALID_FORMAT | \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_INTERNAL_DRONE_ERROR | \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_NOT_FOUND )
++
++#define GPS_EPHEMERIS_STATUS__SUCCESS_FLAGS ( \
++ GPS_EPHEMERIS_STATUS__ERROR_FLAGS | \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_CURRENT_FILE_IS_NEWER ) \
++
++#define GPS_EPHEMERIS_STATUS__END_FLAGS ( \
++ GPS_EPHEMERIS_STATUS__ERROR_FLAGS | \
++ GPS_EPHEMERIS_STATUS__SUCCESS_FLAGS )
++
++
++#define GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_all ( \
++ GPS_EPHEMERIS_STATUS__NEW_EPHEMERIS_RECEIVED | \
++ GPS_EPHEMERIS_STATUS__END_FLAGS )
++
++
++typedef enum {
++ GPS_FIRMWARE_STATUS__FIRMWARE_UP_TO_DATE = ( 1<<0 ), /* FW present but not different */
++ GPS_FIRMWARE_STATUS__FIRMWARE_AVAILABLE_TO_UPLOAD = ( 1<<1 ), /* Other FW present */
++ GPS_FIRMWARE_STATUS__NO_FIRMWARE_PRESENT = ( 1<<2 ), /* FW upload went wrong */
++ GPS_FIRMWARE_STATUS__FIRMWARE_PROCESSING = ( 1<<3 ), /* FW update in progress */
++ GPS_FIRMWARE_STATUS__FIRMWARE_ERASING = ( 1<<4 ), /* FW erase in progress */
++ GPS_FIRMWARE_STATUS__FIRMWARE_UPLOADING = ( 1<<5 ), /* FW upload in progress */
++ GPS_FIRMWARE_STATUS__NEW_FIRMWARE_UPLOAD_ERROR = ( 1<<6 ), /* FW upload went wrong */
++ GPS_FIRMWARE_STATUS__NEW_FIRMWARE_SUCCESS = ( 1<<7 ), /* FW upload went right */
++ /* Let some bytes for upload percentage */
++}_gps_firmware_status_t;
++
++
++typedef struct _navdata_camera_settings_t {
++ uint16_t tag;
++ uint16_t size;
++ int16_t temperature;
++}_ATTRIBUTE_PACKED_ navdata_camera_settings_t;
++
++
++#if defined(_WIN32)
+ /* Go back to default packing policy */
+- #pragma pack()
++ #pragma pack(4)
+ #endif
+
+ #endif // _NAVDATA_COMMON_H_

ardrone-sdk-navdata-file-gps.patch

@@ -0,0 +1,39 @@
+--- ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_file.c~ 2014-02-19 11:25:46.459908329 +0100
++++ ARDroneLib/Soft/Lib/ardrone_tool/Navdata/ardrone_navdata_file.c 2014-02-19 11:25:51.507933361 +0100
+@@ -239,14 +239,14 @@
+ fprintf( navdata_file, ";%s", str );
+ fprintf( navdata_file, "; %u", (unsigned int) pnd->nd_seq);
+
+- fprintf( navdata_file, "; %04u; %04u; %04u; %04d; %04d; %04d; %04d; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04d; %04d",
++ fprintf( navdata_file, "; %04u; %04u; %04u; %04d; %04d; %04d; %04d; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04u; %04d; %04d",
+ (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_X],
+ (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Y],
+ (unsigned int) pnd->navdata_raw_measures.raw_accs[ACC_Z],
+ (int) pnd->navdata_raw_measures.raw_gyros[GYRO_X],
+ (int) pnd->navdata_raw_measures.raw_gyros[GYRO_Y],
+ (int) pnd->navdata_raw_measures.raw_gyros[GYRO_Z],
+- (int) pnd->navdata_zimmu_3000.vzimmuLSB,
++ //(int) pnd->navdata_zimmu_3000.vzimmuLSB,
+ (unsigned int) pnd->navdata_raw_measures.raw_gyros_110[0],
+ (unsigned int) pnd->navdata_raw_measures.raw_gyros_110[1],
+ (unsigned int) pnd->navdata_raw_measures.vbat_raw,
+@@ -332,7 +332,7 @@
+ pnd->navdata_wind_speed.magneto_debug2,
+ pnd->navdata_wind_speed.magneto_debug3);
+
+- fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f; %6f",
++ fprintf( navdata_file, "; %f; %04u; % 5f; % 5f; % 5f; % 6f; % 6f; % 6f",//; %6f",
+ pnd->navdata_phys_measures.accs_temp,
+ (unsigned int)pnd->navdata_phys_measures.gyro_temp,
+ pnd->navdata_phys_measures.phys_accs[ACC_X],
+@@ -340,8 +340,8 @@
+ pnd->navdata_phys_measures.phys_accs[ACC_Z],
+ pnd->navdata_phys_measures.phys_gyros[GYRO_X],
+ pnd->navdata_phys_measures.phys_gyros[GYRO_Y],
+- pnd->navdata_phys_measures.phys_gyros[GYRO_Z],
+- pnd->navdata_zimmu_3000.vzfind );
++ pnd->navdata_phys_measures.phys_gyros[GYRO_Z]);
++//, pnd->navdata_zimmu_3000.vzfind );
+
+ fprintf( navdata_file, "; % f; % f; % f",
+ pnd->navdata_gyros_offsets.offset_g[GYRO_X],

ardrone-sdk-navdata-keys-gps.patch

@@ -0,0 +1,17 @@
+--- ARDroneLib/Soft/Common/navdata_keys.h~ 2014-01-10 05:31:23.000000000 +0100
++++ ARDroneLib/Soft/Common/navdata_keys.h 2014-02-19 10:13:07.370292771 +0100
+@@ -38,9 +38,13 @@
+ NAVDATA_OPTION( navdata_hdvideo_stream_t ,navdata_hdvideo_stream , NAVDATA_HDVIDEO_STREAM_TAG )
+ NAVDATA_OPTION( navdata_wifi_t ,navdata_wifi , NAVDATA_WIFI_TAG )
+
++NAVDATA_OPTION( navdata_gps_t ,navdata_gps_info , NAVDATA_GPS_TAG )
++NAVDATA_OPTION( navdata_camera_settings_t,navdata_camera_settings , NAVDATA_CAMERA_SETTINGS_TAG )
+
+ // TODO: maybe navdata_zimmu_3000 coult be integrated into an existing navdata like navdata_adc_data_frame_t or ...
+-NAVDATA_OPTION( navdata_zimmu_3000_t, navdata_zimmu_3000 , NAVDATA_ZIMMU_3000_TAG )
++//NAVDATA_OPTION( navdata_zimmu_3000_t, navdata_zimmu_3000 , NAVDATA_ZIMMU_3000_TAG )
++
++NAVDATA_OPTION( navdata_host_t ,navdata_host , NAVDATA_HOST_TAG )
+
+ NAVDATA_OPTION_CKS( navdata_cks_t, navdata_cks , NAVDATA_CKS_TAG )
+

msg/navdata_camera_settings.msg

@@ -0,0 +1,4 @@
+Header header
+uint16 tag
+uint16 size
+int16 temperature