diff --git a/conf/airframes/ardrone2_sdk.xml b/conf/airframes/ardrone2_sdk.xml
index c7bb880258..a6850e6473 100644
--- a/conf/airframes/ardrone2_sdk.xml
+++ b/conf/airframes/ardrone2_sdk.xml
@@ -14,7 +14,7 @@
-
+
diff --git a/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/gps_ardrone2.makefile
new file mode 100644
index 0000000000..fad4a3919f
--- /dev/null
+++ b/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
+
diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h
index 08605e9843..431030eb31 100644
--- a/sw/airborne/boards/ardrone/at_com.h
+++ b/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);
diff --git a/sw/airborne/boards/ardrone2_sdk.h b/sw/airborne/boards/ardrone2_sdk.h
index c325f3ec40..01bb97452d 100644
--- a/sw/airborne/boards/ardrone2_sdk.h
+++ b/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 */
diff --git a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c b/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
index 95c865fb90..afed7a6202 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_ardrone2.c
+++ b/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);
}
}
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c
new file mode 100644
index 0000000000..60a1e5db78
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_ardrone2.c
@@ -0,0 +1,63 @@
+/*
+ *
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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;
+}
diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.h b/sw/airborne/subsystems/gps/gps_ardrone2.h
new file mode 100644
index 0000000000..42e926ff34
--- /dev/null
+++ b/sw/airborne/subsystems/gps/gps_ardrone2.h
@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2013 Freek van Tienen
+ *
+ * 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 */
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c
index 3f42c8085d..a50f0ed908 100644
--- a/sw/airborne/subsystems/ins/ins_ardrone2.c
+++ b/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 */