[rotorcraft ]Added ardrone2 sdk version gps, fixed some bugs.

The GPS receiver from parrot is added for the SDK version of the ardrone2.
This new gps receiver is the flightrecorde from parrot.
Also fixed some bugs in the navdata receive of the SDK version, and added some comments to the INS.

Closes #463

conf/airframes/ardrone2_sdk.xml

@@ -14,7 +14,7 @@
 
 		<subsystem name="radio_control" type="datalink" />
 		<subsystem name="telemetry" type="udp" />
-		<subsystem name="gps" type="sirf" />
+		<subsystem name="gps" type="ardrone2" />
 		<subsystem name="ahrs" type="ardrone2" />
 		<subsystem name="ins" type="ardrone2" />
 		<subsystem name="actuators" type="ardrone2" />

conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile

@@ -0,0 +1,20 @@
+# Hey Emacs, this is a -*- makefile -*-
+
+# ARDrone 2 Flightrecorder GPS unit
+
+
+ap.CFLAGS += -DUSE_GPS -DUSE_GPS_ARDRONE2
+
+ifneq ($(GPS_LED),none)
+  ap.CFLAGS += -DGPS_LED=$(GPS_LED)
+endif
+
+ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ardrone2.h\"
+ap.srcs   += $(SRC_SUBSYSTEMS)/gps/gps_ardrone2.c
+
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
+
+nps.CFLAGS += -DUSE_GPS
+nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
+nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
+

sw/airborne/boards/ardrone/at_com.h

@@ -29,6 +29,8 @@
 #ifndef BOARDS_ARDRONE_AT_COM_H
 #define BOARDS_ARDRONE_AT_COM_H
 
+#define NAVDATA_HEADER  (0x55667788)
+
 //Define the AT_REF bits
 typedef enum {
   REF_TAKEOFF    = 1U << 9,
@@ -149,6 +151,55 @@ typedef struct _navdata_phys_measures_t {
   uint32_t        vrefIDG;              // ref volt IDG gyro [LSB]
 } __attribute__ ((packed)) navdata_phys_measures_t;
 
+//Navdata gps packet
+typedef double float64_t;               //TODO: Fix this nicely, but this is only used here
+typedef float float32_t;               //TODO: Fix this nicely, but this is only used here
+typedef struct _navdata_gps_t {
+  uint16_t      tag;                    /*!< Navdata block ('option') identifier */
+  uint16_t      size;                   /*!< set this to the size of this structure */
+  float64_t     lat;                    /*!< Latitude */
+  float64_t     lon;                    /*!< Longitude */
+  float64_t     elevation;              /*!< Elevation */
+  float64_t     hdop;                   /*!< hdop */
+  int32_t       data_available;         /*!< When there is data available */
+  uint8_t       unk_0[8];
+  float64_t     lat0;                   /*!< Latitude ??? */
+  float64_t     lon0;                   /*!< Longitude ??? */
+  float64_t     lat_fuse;               /*!< Latitude fused */
+  float64_t     lon_fuse;               /*!< Longitude fused */
+  uint32_t      gps_state;              /*!< State of the GPS, still need to figure out */
+  uint8_t       unk_1[40];
+  float64_t     vdop;                   /*!< vdop */
+  float64_t     pdop;                   /*!< pdop */
+  float32_t     speed;                  /*!< speed */
+  uint32_t      last_frame_timestamp;   /*!< Timestamp from the last frame */
+  float32_t     degree;                 /*!< Degree */
+  float32_t     degree_mag;             /*!< Degree of the magnetic */
+  uint8_t       unk_2[16];
+  struct{
+    uint8_t     sat;
+    uint8_t     unk;
+  }channels[12];
+  int32_t       gps_plugged;            /*!< When the gps is plugged */
+  uint8_t       unk_3[108];
+  float64_t     gps_time;               /*!< The gps time of week */
+  uint16_t      week;                   /*!< The gps week */
+  uint8_t       gps_fix;                /*!< The gps fix */
+  uint8_t       num_sattelites;         /*!< Number of sattelites */
+  uint8_t       unk_4[24];
+  float64_t     ned_vel_c0;             /*!< NED velocity */
+  float64_t     ned_vel_c1;             /*!< NED velocity */
+  float64_t     ned_vel_c2;             /*!< NED velocity */
+  float64_t     pos_accur_c0;           /*!< Position accuracy */
+  float64_t     pos_accur_c1;           /*!< Position accuracy */
+  float64_t     pos_accur_c2;           /*!< Position accuracy */
+  float32_t     speed_acur;             /*!< Speed accuracy */
+  float32_t     time_acur;              /*!< Time accuracy */
+  uint8_t       unk_5[72];
+  float32_t     temprature;
+  float32_t     pressure;
+} __attribute__ ((packed)) navdata_gps_t;
+
 //External functions
 extern void init_at_com(void);
 extern void at_com_recieve_navdata(unsigned char* buffer);

sw/airborne/boards/ardrone2_sdk.h

@@ -6,7 +6,7 @@
 /* Internal communication */
 #define ARDRONE_NAVDATA_PORT 5554
 #define ARDRONE_AT_PORT 5556
-#define ARDRONE_NAVDATA_BUFFER_SIZE 2048
+#define ARDRONE_NAVDATA_BUFFER_SIZE 4096
 #define ARDRONE_IP "192.168.1.1"
 
 /* Default actuators driver */

sw/airborne/subsystems/ahrs/ahrs_ardrone2.c

@@ -33,9 +33,13 @@
 #include "boards/ardrone/at_com.h"
 #include "subsystems/electrical.h"
 
+#ifdef USE_GPS_ARDRONE2
+#include "subsystems/gps/gps_ardrone2.h"
+#endif
+
 struct AhrsARDrone ahrs_impl;
 struct AhrsAligner ahrs_aligner;
-unsigned char buffer[2048]; //Packet buffer
+unsigned char buffer[4096]; //Packet buffer
 
 void ahrs_init(void) {
   init_at_com();
@@ -55,6 +59,12 @@ void ahrs_propagate(void) {
   //Recieve the main packet
   at_com_recieve_navdata(buffer);
   navdata_t* main_packet = (navdata_t*) &buffer;
+
+  //When this isn't a valid packet return
+  if(main_packet->header != NAVDATA_HEADER)
+    return;
+
+  //Set the state
   ahrs_impl.state = main_packet->ardrone_state;
 
   //Init the option
@@ -63,10 +73,11 @@ void ahrs_propagate(void) {
 
   //The possible packets
   navdata_demo_t* navdata_demo;
+  navdata_gps_t* navdata_gps;
   navdata_phys_measures_t* navdata_phys_measures;
 
   //Read the navdata until packet is fully readed
-  while(!full_read) {
+  while(!full_read && navdata_option->size > 0) {
     //Check the tag for the right option
     switch(navdata_option->tag) {
     case 0: //NAVDATA_DEMO
@@ -99,6 +110,14 @@ void ahrs_propagate(void) {
       //Set the AHRS accel state
       INT32_VECT3_SCALE_2(ahrs_impl.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
       break;
+#ifdef USE_GPS_ARDRONE2
+    case 27: //NAVDATA_GPS
+      navdata_gps = (navdata_gps_t*) navdata_option;
+
+      // Send the data to the gps parser
+      gps_ardrone2_parse(navdata_gps);
+      break;
+#endif
     case 0xFFFF: //CHECKSUM
       //TODO: Check the checksum
       full_read = TRUE;
@@ -107,7 +126,7 @@ void ahrs_propagate(void) {
       //printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
       break;
     }
-    navdata_option = (navdata_option_t*) ((int)navdata_option + navdata_option->size);
+    navdata_option = (navdata_option_t*) ((uint32_t)navdata_option + navdata_option->size);
   }
 }
 

sw/airborne/subsystems/gps/gps_ardrone2.c

@@ -0,0 +1,63 @@
+/*
+ *
+ * Copyright (C) 2013 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/**
+ * @file subsystems/gps/gps_ardrone2.c
+ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
+ */
+
+#include "subsystems/gps.h"
+#include "math/pprz_geodetic_double.h"
+
+bool_t gps_ardrone2_available;
+
+void gps_impl_init( void ) {
+  gps_ardrone2_available = FALSE;
+}
+
+void gps_ardrone2_parse(navdata_gps_t *navdata_gps) {
+  // Set the lla double struct from the navdata
+  struct LlaCoor_d gps_lla_d;
+  gps_lla_d.lat = RadOfDeg(navdata_gps->lat);
+  gps_lla_d.lon = RadOfDeg(navdata_gps->lon);
+  gps_lla_d.alt = navdata_gps->elevation;
+
+  // Convert it to ecef
+  struct EcefCoor_d gps_ecef_d;
+  ecef_of_lla_d(&gps_ecef_d, &gps_lla_d);
+
+  // Convert the lla and ecef to int and set them in gps
+  ECEF_BFP_OF_REAL(gps.ecef_pos, gps_ecef_d);
+  LLA_BFP_OF_REAL(gps.lla_pos, gps_lla_d);
+
+  // TODO: parse other stuff
+
+  // Check if we have a fix TODO: check if 2D or 3D fix?
+  if (navdata_gps->gps_state == 1)
+    gps.fix = GPS_FIX_3D;
+  else
+    gps.fix = GPS_FIX_NONE;
+
+  // Set that there is a packet
+  gps_ardrone2_available = TRUE;
+}

sw/airborne/subsystems/gps/gps_ardrone2.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** @file subsystems/gps/gps_ardrone2.h
+ * ARdrone 2 gps trough navdata for the SDK version and only works in combination with the ahrs ardrone2.
+ *
+*/
+
+#ifndef GPS_ARDRONE_H
+#define GPS_ARDRONE_H
+
+#include "boards/ardrone/at_com.h"
+
+//#define GPS_NB_CHANNELS 12 // TODO: Get channels out of packet
+extern bool_t gps_ardrone2_available;
+
+/*
+ * The GPS event
+ */
+#define GpsEvent(_sol_available_callback) {        \
+    if (gps_ardrone2_available) {                   \
+    	if (gps.fix == GPS_FIX_3D) {               \
+        gps.last_fix_ticks = sys_time.nb_sec_rem;  \
+        gps.last_fix_time = sys_time.nb_sec;       \
+      }                                            \
+      _sol_available_callback();                   \
+      gps_ardrone2_available = FALSE;               \
+    }                                              \
+  }
+
+void gps_ardrone2_parse(navdata_gps_t *navdata_gps);
+
+/* Maybe needed?
+#define gps_nmea_Reset(_val) { }
+*/
+
+#endif /* GPS_ARDRONE_H */

sw/airborne/subsystems/ins/ins_ardrone2.c

@@ -110,6 +110,7 @@ void ins_propagate() {
   stateSetAccelNed_f(&ins_ltp_accel);
   stateSetSpeedNed_f(&ins_ltp_speed);
 
+  //Don't set the height if we use the one from the gps
 #if !USE_GPS_HEIGHT
   //Set the height and save the position
   ins_ltp_pos.z = -(ahrs_impl.altitude * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
@@ -138,11 +139,15 @@ void ins_update_gps(void) {
     //Set the x and y and maybe z position in ltp and save
     struct NedCoor_i ins_gps_pos_cm_ned;
     ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
+
+    //When we don't want to use the height of the navdata we can use the gps height
 #if USE_GPS_HEIGHT
     INT32_VECT3_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
 #else
     INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
 #endif
+
+    //Set the local origin
     stateSetPositionNed_i(&ins_ltp_pos);
   }
 #endif /* USE_GPS */