diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-1.xml b/conf/airframes/ENAC/fixed-wing/spocIII-1.xml
new file mode 100644
index 00000000000..2d16f9489be
--- /dev/null
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-1.xml
@@ -0,0 +1,212 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
new file mode 100644
index 00000000000..ebe2cde45e3
--- /dev/null
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
@@ -0,0 +1,213 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/corsica.xml b/conf/flight_plans/corsica.xml
new file mode 100644
index 00000000000..943ffbf2597
--- /dev/null
+++ b/conf/flight_plans/corsica.xml
@@ -0,0 +1,95 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/flight_plans/corsica_sectors.xml b/conf/flight_plans/corsica_sectors.xml
new file mode 100644
index 00000000000..6deabe5bcd5
--- /dev/null
+++ b/conf/flight_plans/corsica_sectors.xml
@@ -0,0 +1,37 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/alt_filter.xml b/conf/modules/alt_filter.xml
new file mode 100644
index 00000000000..40eba53bd78
--- /dev/null
+++ b/conf/modules/alt_filter.xml
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/generic_com.xml b/conf/modules/generic_com.xml
new file mode 100644
index 00000000000..6a141a38f08
--- /dev/null
+++ b/conf/modules/generic_com.xml
@@ -0,0 +1,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c
new file mode 100644
index 00000000000..52ce9af13f4
--- /dev/null
+++ b/sw/airborne/modules/com/generic_com.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (C) 2011 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+/* Generic module to send data to a gsm/satcom module controlled by arduino over i2c */
+
+#include "std.h"
+
+#include "modules/com/generic_com.h"
+#include "mcu_periph/i2c.h"
+
+#include "estimator.h"
+#include "gps.h"
+#include "subsystems/electrical.h"
+#include "generated/airframe.h"
+#include "inter_mcu.h"
+#include "firmwares/fixedwing/autopilot.h"
+#include "subsystems/navigation/common_nav.h"
+
+#define NB_DATA 21
+
+#ifndef GENERIC_COM_I2C_DEV
+#define GENERIC_COM_I2C_DEV i2c0
+#endif
+
+#ifndef GENERIC_COM_SLAVE_ADDR
+#define GENERIC_COM_SLAVE_ADDR 0x26
+#endif
+
+struct i2c_transaction com_trans;
+
+bool_t active_com;
+
+void generic_com_init( void ) {
+ active_com = FALSE;
+}
+
+#define FillBufWith32bit(_buf, _index, _value) { \
+ _buf[_index] = (uint8_t) (_value); \
+ _buf[_index+1] = (uint8_t) ((_value) >> 8); \
+ _buf[_index+2] = (uint8_t) ((_value) >> 16); \
+ _buf[_index+3] = (uint8_t) ((_value) >> 24); \
+}
+
+#define FillBufWith16bit(_buf, _index, _value) { \
+ _buf[_index] = (uint8_t) (_value); \
+ _buf[_index+1] = (uint8_t) ((_value) >> 8); \
+}
+
+void generic_com_periodic( void ) {
+
+ if (com_trans.status != I2CTransDone) { return; }
+
+ com_trans.buf[0] = active_com;
+ FillBufWith32bit(com_trans.buf, 1, gps_lat);
+ FillBufWith32bit(com_trans.buf, 5, gps_lon);
+ FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps_alt/100)); // meters
+ FillBufWith16bit(com_trans.buf, 11, gps_gspeed); // ground speed
+ FillBufWith16bit(com_trans.buf, 13, gps_course); // course
+ FillBufWith16bit(com_trans.buf, 15, (uint16_t)(estimator_airspeed*100)); // TAS (cm/s)
+ com_trans.buf[16] = electrical.vsupply;
+ com_trans.buf[17] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ);
+ com_trans.buf[18] = pprz_mode;
+ com_trans.buf[19] = nav_block;
+ FillBufWith16bit(com_trans.buf, 20, estimator_flight_time);
+ I2CTransmit(GENERIC_COM_I2C_DEV, com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);
+
+}
+
+void generic_com_event( void ) {
+ // Handle I2C event
+ if (com_trans.status == I2CTransSuccess || com_trans.status == I2CTransFailed) {
+ com_trans.status = I2CTransDone;
+ }
+}
+
+void start_com( void ) {
+ active_com = TRUE;
+}
+
+void stop_com( void ) {
+ active_com = FALSE;
+ com_trans.buf[0] = active_com;
+ I2CTransmit(GENERIC_COM_I2C_DEV, com_trans, GENERIC_COM_SLAVE_ADDR, 1);
+}
+
diff --git a/sw/airborne/modules/com/generic_com.h b/sw/airborne/modules/com/generic_com.h
new file mode 100644
index 00000000000..2221a7c8f55
--- /dev/null
+++ b/sw/airborne/modules/com/generic_com.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (C) 2011 Gautier Hattenberger
+ *
+ * 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.
+ *
+ */
+
+/* Generic module to send data to a gsm/satcom module controlled by arduino over i2c */
+
+#ifndef GENERIC_COM_H
+#define GENERIC_COM_H
+
+void generic_com_init( void );
+void generic_com_periodic( void );
+void generic_com_event( void );
+void start_com( void );
+void stop_com( void );
+
+#endif // GENERIC_COM_H
diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c
new file mode 100644
index 00000000000..cf461919614
--- /dev/null
+++ b/sw/airborne/modules/ins/alt_filter.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright (C) 2010 ENAC
+ *
+ * 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.
+ *
+ */
+
+#include "modules/ins/alt_filter.h"
+#include "gps.h"
+#include "modules/sensors/baro_ets.h"
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+TypeKalman alt_filter;
+
+/* Kalman parameters */
+float SigAltiGPS;
+float SigAltiAltimetre;
+
+/* Function declaration */
+void kalmanInit(TypeKalman *k);
+void kalmanEstimation(TypeKalman *k, float accVert);
+void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps);
+void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre);
+
+/* last measured values */
+float last_gps_alt;
+float last_baro_alt;
+
+void alt_filter_init(void) {
+ SigAltiGPS = 20.;
+ SigAltiAltimetre = 0.5;
+ last_gps_alt = 0.;
+ last_baro_alt = 0.;
+
+ kalmanInit(&alt_filter);
+}
+
+void alt_filter_periodic(void) {
+ // estimation at each step
+ kalmanEstimation(&alt_filter,0.);
+
+ // update on new data
+ float ga = (float)gps_alt / 100.;
+ if (ga != last_gps_alt && GpsFixValid()) {
+ kalmanCorrectionGPS(&alt_filter, ga);
+ last_gps_alt = ga;
+ }
+ if (baro_ets_altitude != last_baro_alt) {
+ kalmanCorrectionAltimetre(&alt_filter, baro_ets_altitude);
+ last_baro_alt = baro_ets_altitude;
+ }
+
+ RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, &baro_ets_altitude,
+ &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]),
+ &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2])));
+
+}
+
+
+/*************************************************************************
+ *
+ * Filter Initialisation
+ *
+ *************************************************************************/
+
+
+void kalmanInit(TypeKalman *k){
+
+ k->W[0][0] = 0.001; k->W[0][1] = 0;
+ k->W[1][0] = 0; k->W[1][1] = 0.001;
+
+ k->X[0] = 0;
+ k->X[1] = 0;
+ k->X[2] = 0;
+
+ k->P[0][0] = 1; k->P[0][1] = 0; k->P[0][2] = 0;
+ k->P[1][0] = 0; k->P[1][1] = 1; k->P[1][2] = 0;
+ k->P[2][0] = 0; k->P[2][1] = 0; k->P[2][2] = 0;
+
+ k->Te = (1./60.);
+
+ // System dynamic
+ k->Ad[0][0] = 1; k->Ad[0][1] = k->Te; k->Ad[0][2] = 0;
+ k->Ad[1][0] = 0; k->Ad[1][1] = 1; k->Ad[1][2] = 0;
+ k->Ad[2][0] = 0; k->Ad[2][1] = 0; k->Ad[2][2] = 1;
+
+ // System command
+ k->Bd[0] = pow(k->Te,2)/2;
+ k->Bd[1] = k->Te;
+ k->Bd[2] = 0;
+
+ k->Md[0][0] = pow(k->Te, 1.5)/2; k->Md[0][1] = k->Te;
+ k->Md[1][0] = pow(k->Te, 0.5); k->Md[1][1] = 1;
+ k->Md[2][0] = 0; k->Md[2][1] = pow(k->Te, 0.5);
+}
+
+
+/*************************************************************************
+ *
+ * Estimation
+ *
+ *************************************************************************/
+
+
+void kalmanEstimation(TypeKalman *k, float accVert){
+
+
+ int i,j;
+ float I[3][3]; // matrices temporaires
+ float J[3][2];
+
+ // Calcul de X
+ // X = A*X + B*U
+ k->X[0] = k->Ad[0][0]*k->X[0] + k->Ad[0][1]*k->X[1] + k->Bd[0]*accVert;
+ k->X[1] = k->Ad[1][1]*k->X[1] + k->Bd[1]*accVert;
+ k->X[2] = k->Ad[2][2]*k->X[2] + k->Bd[2]*accVert;
+
+ // Calcul de P
+ for(i=0;i<3;i++){
+ for (j=0;j<3;j++){
+ I[i][j] = k->Ad[i][0]*k->P[0][j] + k->Ad[i][1]*k->P[1][j] + k->Ad[i][2]*k->P[2][j];
+ }
+ }
+
+ for(i=0;i<3;i++){
+ for (j=0;j<3;j++){
+ k->P[i][j] = I[i][0]*k->Ad[j][0] + I[i][1]*k->Ad[j][1] + I[i][2]*k->Ad[j][2];
+ }
+ }
+
+ for(i=0;i<3;i++){
+ for (j=0;j<2;j++){
+ J[i][j] = k->Md[i][0]*k->W[0][j] + k->Md[i][1]*k->W[1][j];
+ }
+ }
+
+ for(i=0;i<3;i++){
+ for (j=0;j<3;j++){
+ k->P[i][j] = k->P[i][j] + J[i][0]*k->Md[j][0] + J[i][1]*k->Md[j][1];
+ }
+ }
+
+}
+
+/*************************************************************************
+ *
+ * Correction GPS
+ *
+ *************************************************************************/
+
+void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS
+
+ int i, j, div;
+
+ float Kf[3] = { 0., 0., 0. };
+
+ // calcul de Kf
+ // C = [1 0 0]
+ // div = C*P*C' + R
+ div = k->P[0][0] + SigAltiGPS*SigAltiGPS;
+
+ if (fabs(div) > 1e-5) {
+ // K = P*C'*inv(div)
+ Kf[0] = k->P[0][0] / div;
+ Kf[1] = k->P[1][0] / div;
+ Kf[2] = k->P[2][0] / div;
+
+ // calcul de X
+ // X = X + K*(meas - C*X)
+ float constante = k->X[0];
+ for(i=0;i<3;i++){
+ k->X[i] = k->X[i]+Kf[i]*(altitude_gps-constante);
+ }
+
+ // calcul de P
+ // P = P - K*C*P
+ constante = Kf[0];
+ for(i=0;i<3;i++){
+ for (j=0;j<3;j++){
+ k->P[i][j] = k->P[i][j] - constante*k->P[i][j];
+ }
+ }
+ }
+
+}
+
+/*************************************************************************
+ *
+ * Correction altimètre
+ *
+ *************************************************************************/
+
+void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){
+
+ int i, j, div;
+
+ float Kf[3] = { 0., 0., 0. };
+
+ // calcul de Kf
+ // C = [1 0 1]
+ // div = C*P*C' + R
+ div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre*SigAltiAltimetre;
+ if (fabs(div) > 1e-5) {
+ // K = P*C'*inv(div)
+ Kf[0] = (k->P[0][0] + k->P[0][2]) / div;
+ Kf[1] = (k->P[1][0] + k->P[1][2]) / div;
+ Kf[2] = (k->P[2][0] + k->P[2][2]) / div;
+
+ // calcul de X
+ // X = X + K*(meas - C*X)
+ float constante = k->X[0] + k->X[2]; //C[0]*k->X[0]+C[1]*k->X[1]+C[2]*k->X[2];
+ for(i=0;i<3;i++){
+ k->X[i] = k->X[i]+Kf[i]*(altitude_altimetre-constante);
+ }
+
+ // calcul de P
+ // P = P - K*C*P
+ constante = Kf[0] + Kf[2]; //Kf[0]*C[0]+Kf[1]*C[1]+Kf[2]*C[2];
+ for(i=0;i<3;i++){
+ for (j=0;j<3;j++){
+ k->P[i][j] = k->P[i][j] - constante*k->P[i][j];
+ }
+ }
+ }
+
+}
+
diff --git a/sw/airborne/modules/ins/alt_filter.h b/sw/airborne/modules/ins/alt_filter.h
new file mode 100644
index 00000000000..476b9eea397
--- /dev/null
+++ b/sw/airborne/modules/ins/alt_filter.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2010 ENAC
+ *
+ * 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.
+ *
+ */
+
+#ifndef ALT_FILTER_H
+#define ALT_FILTER_H
+
+
+/*************************************************************************/
+/*************************************************************************/
+
+/* Kalman 3 states Filter for GPS / barometer and IMU fusion
+ */
+
+/*************************************************************************/
+/*************************************************************************/
+
+#define KALT_N_ETAT 3
+
+typedef struct
+{
+ float Te;
+ float P[KALT_N_ETAT][KALT_N_ETAT];
+ float W[KALT_N_ETAT-1][KALT_N_ETAT-1];
+ float X[KALT_N_ETAT];
+ float Bd[KALT_N_ETAT];
+ float Ad[KALT_N_ETAT][KALT_N_ETAT];
+ float Md[KALT_N_ETAT][KALT_N_ETAT-1];
+} TypeKalman;
+
+extern TypeKalman alt_filter;
+
+extern void alt_filter_init(void);
+extern void alt_filter_periodic(void);
+
+#endif
diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c
index 520eac4f2fa..3eaf16fd90e 100644
--- a/sw/airborne/modules/sensors/baro_ets.c
+++ b/sw/airborne/modules/sensors/baro_ets.c
@@ -104,6 +104,18 @@ void baro_ets_read_periodic( void ) {
#endif
}
+#ifdef BARO_ETS_TELEMETRY
+
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "downlink.h"
+
+#endif
+
void baro_ets_read_event( void ) {
// Get raw altimeter from buffer
baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]);
@@ -138,6 +150,9 @@ void baro_ets_read_event( void ) {
baro_ets_altitude = ground_alt + BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
// New value available
EstimatorSetAlt(baro_ets_altitude);
+#ifdef BARO_ETS_TELEMETRY
+ DOWNLINK_SEND_BARO_ETS(DefaultChannel, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude);
+#endif
} else {
baro_ets_altitude = 0.0;
}