From 56ea7cc7fa8c040c42364f1fe43aed9cf9572653 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 23 May 2011 10:35:07 +0200 Subject: [PATCH 01/45] airframe and utilities for corsica --- conf/airframes/ENAC/fixed-wing/spocIII-1.xml | 212 ++++++++++++++++ conf/airframes/ENAC/fixed-wing/spocIII-2.xml | 213 ++++++++++++++++ conf/flight_plans/corsica.xml | 95 +++++++ conf/flight_plans/corsica_sectors.xml | 37 +++ conf/modules/alt_filter.xml | 13 + conf/modules/generic_com.xml | 19 ++ sw/airborne/modules/com/generic_com.c | 104 ++++++++ sw/airborne/modules/com/generic_com.h | 34 +++ sw/airborne/modules/ins/alt_filter.c | 248 +++++++++++++++++++ sw/airborne/modules/ins/alt_filter.h | 54 ++++ sw/airborne/modules/sensors/baro_ets.c | 15 ++ 11 files changed, 1044 insertions(+) create mode 100644 conf/airframes/ENAC/fixed-wing/spocIII-1.xml create mode 100644 conf/airframes/ENAC/fixed-wing/spocIII-2.xml create mode 100644 conf/flight_plans/corsica.xml create mode 100644 conf/flight_plans/corsica_sectors.xml create mode 100644 conf/modules/alt_filter.xml create mode 100644 conf/modules/generic_com.xml create mode 100644 sw/airborne/modules/com/generic_com.c create mode 100644 sw/airborne/modules/com/generic_com.h create mode 100644 sw/airborne/modules/ins/alt_filter.c create mode 100644 sw/airborne/modules/ins/alt_filter.h 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; } From 0f73cd4efaf2e33e7892e1f7899c64ddec6483fd Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 23 May 2011 10:41:51 +0200 Subject: [PATCH 02/45] try to compile jsbsim with arduimu --- conf/airframes/ENAC/fixed-wing/funjet3.xml | 5 +++-- conf/autopilot/subsystems/fixedwing/autopilot.makefile | 2 +- sw/airborne/arch/sim/jsbsim_ir.c | 8 ++++++-- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/funjet3.xml b/conf/airframes/ENAC/fixed-wing/funjet3.xml index 842c8b60d8c..11a3016f28b 100644 --- a/conf/airframes/ENAC/fixed-wing/funjet3.xml +++ b/conf/airframes/ENAC/fixed-wing/funjet3.xml @@ -22,6 +22,7 @@ + @@ -43,7 +44,7 @@ - + @@ -208,7 +209,7 @@ - + diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 7d491dd3382..44b395950eb 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -211,7 +211,7 @@ jsbsim.CFLAGS += -I$(SIMDIR) -I/usr/include -I$(JSBSIM_INC) -I$(OCAMLLIBDIR) ` jsbsim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy -L/usr/lib -lJSBSim jsbsim.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -jsbsim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c +jsbsim.srcs += downlink.c $(SRC_FIRMWARE)/datalink.c $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_gps.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/jsbsim_transport.c jsbsim.srcs += subsystems/settings.c jsbsim.srcs += $(SRC_ARCH)/subsystems/settings_arch.c diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c index 9d448d486b4..5341bb3b916 100644 --- a/sw/airborne/arch/sim/jsbsim_ir.c +++ b/sw/airborne/arch/sim/jsbsim_ir.c @@ -7,6 +7,7 @@ #include "jsbsim_hw.h" #include +#include "estimator.h" #ifndef JSBSIM_IR_ROLL_NEUTRAL #define JSBSIM_IR_ROLL_NEUTRAL 0. @@ -15,15 +16,18 @@ #define JSBSIM_IR_PITCH_NEUTRAL 0. #endif -void set_ir(double roll, double pitch) { +void set_ir(double roll __attribute__ ((unused)), double pitch __attribute__ ((unused))) { +#ifdef USE_INFRARED double ir_contrast = 150; //FIXME double roll_sensor = roll + JSBSIM_IR_ROLL_NEUTRAL; // ir_roll_neutral; double pitch_sensor = pitch + JSBSIM_IR_PITCH_NEUTRAL; // ir_pitch_neutral; -#ifdef USE_INFRARED infrared.roll = sin(roll_sensor) * ir_contrast; infrared.pitch = sin(pitch_sensor) * ir_contrast; infrared.top = cos(roll_sensor) * cos(pitch_sensor) * ir_contrast; +#else + estimator_phi = roll; // - ins_roll_neutral; + estimator_theta = pitch; // - ins_pitch_neutral; #endif } From 837c6ce2495e573d088999d6e738a50eefec356f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 May 2011 10:34:47 +0200 Subject: [PATCH 03/45] add an option to reverse airspeed measure when you stupidely mix static and dynamic pressure --- conf/airframes/ENAC/fixed-wing/spocIII-2.xml | 30 +++++++++++--------- sw/airborne/modules/sensors/airspeed_ets.c | 5 ++++ 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml index ebe2cde45e3..2983b7f7c3c 100644 --- a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml +++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml @@ -13,8 +13,10 @@ - + + + @@ -35,7 +37,7 @@ - + @@ -54,10 +56,10 @@ - - - - + + + + @@ -101,7 +103,7 @@
- + @@ -109,7 +111,7 @@ - + @@ -130,6 +132,8 @@ + + @@ -175,18 +179,18 @@ - + - - - + + + - +
diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 4921e9c6a23..1859cf5ca20 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -146,8 +146,13 @@ void airspeed_ets_read_event( void ) { airspeed_ets_offset_tmp += airspeed_ets_raw; } // Convert raw to m/s +#ifdef AIRSPEED_ETS_REVERSE + if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET; +#else if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET; +#endif else airspeed_tmp = 0.0; // Airspeed should always be positive From 1b2228ed67b7296591125bc9fa30ad12ed22cb99 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 May 2011 10:37:17 +0200 Subject: [PATCH 04/45] fix number of bytes to send --- conf/modules/generic_com.xml | 2 +- sw/airborne/modules/com/generic_com.c | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/conf/modules/generic_com.xml b/conf/modules/generic_com.xml index 6a141a38f08..3cb12df9eb2 100644 --- a/conf/modules/generic_com.xml +++ b/conf/modules/generic_com.xml @@ -10,7 +10,7 @@ - + diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 52ce9af13f4..0c32c97d1aa 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -35,7 +35,7 @@ #include "firmwares/fixedwing/autopilot.h" #include "subsystems/navigation/common_nav.h" -#define NB_DATA 21 +#define NB_DATA 22 #ifndef GENERIC_COM_I2C_DEV #define GENERIC_COM_I2C_DEV i2c0 @@ -94,6 +94,7 @@ void generic_com_event( void ) { void start_com( void ) { active_com = TRUE; + com_trans.status = I2CTransDone; } void stop_com( void ) { From df76d5e80e8840abf6130439fbbf1c96f4e92845 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 25 May 2011 10:38:10 +0200 Subject: [PATCH 05/45] fix bugs in altitude filter --- sw/airborne/modules/ins/alt_filter.c | 70 +++++++++++++++++----------- 1 file changed, 42 insertions(+), 28 deletions(-) diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c index cf461919614..00b898f7253 100644 --- a/sw/airborne/modules/ins/alt_filter.c +++ b/sw/airborne/modules/ins/alt_filter.c @@ -36,6 +36,8 @@ TypeKalman alt_filter; /* Kalman parameters */ float SigAltiGPS; float SigAltiAltimetre; +float MarcheAleaBiaisAltimetre; +float MarcheAleaAccelerometre; /* Function declaration */ void kalmanInit(TypeKalman *k); @@ -48,8 +50,10 @@ float last_gps_alt; float last_baro_alt; void alt_filter_init(void) { - SigAltiGPS = 20.; - SigAltiAltimetre = 0.5; + SigAltiGPS = 5.; + SigAltiAltimetre = 5.; + MarcheAleaBiaisAltimetre = 0.1; + MarcheAleaAccelerometre = 0.5; last_gps_alt = 0.; last_baro_alt = 0.; @@ -62,14 +66,14 @@ void alt_filter_periodic(void) { // 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; } + if (ga != last_gps_alt && GpsFixValid()) { + kalmanCorrectionGPS(&alt_filter, ga); + last_gps_alt = ga; + } RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, &baro_ets_altitude, &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]), @@ -87,8 +91,8 @@ void alt_filter_periodic(void) { 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->W[0][0] = MarcheAleaAccelerometre*MarcheAleaAccelerometre; k->W[0][1] = 0; + k->W[1][0] = 0; k->W[1][1] = MarcheAleaBiaisAltimetre*MarcheAleaBiaisAltimetre; k->X[0] = 0; k->X[1] = 0; @@ -96,7 +100,7 @@ void kalmanInit(TypeKalman *k){ 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->P[2][0] = 0; k->P[2][1] = 0; k->P[2][2] = 0.0001; k->Te = (1./60.); @@ -110,8 +114,8 @@ void kalmanInit(TypeKalman *k){ 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[0][0] = pow(k->Te, 1.5)/2; k->Md[0][1] = 0; + k->Md[1][0] = pow(k->Te, 0.5); k->Md[1][1] = 0; k->Md[2][0] = 0; k->Md[2][1] = pow(k->Te, 0.5); } @@ -131,12 +135,13 @@ void kalmanEstimation(TypeKalman *k, float accVert){ 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; + // X(k+1) = Ad*X(k) + Bd*U(k) + k->X[0] = k->Ad[0][0]*k->X[0] + k->Ad[0][1]*k->X[1] + k->Ad[0][2]*k->X[2] + k->Bd[0]*accVert; + k->X[1] = k->Ad[1][0]*k->X[0] + k->Ad[1][1]*k->X[1] + k->Ad[1][2]*k->X[2] + k->Bd[1]*accVert; + k->X[2] = k->Ad[2][0]*k->X[0] + k->Ad[2][1]*k->X[1] + k->Ad[2][2]*k->X[2] + k->Bd[2]*accVert; // Calcul de P + // P(k+1) = Ad*P(k)*Ad' + Md*W*Md' 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]; @@ -172,6 +177,7 @@ void kalmanEstimation(TypeKalman *k, float accVert){ 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 I[3][3]; // matrice temporaire float Kf[3] = { 0., 0., 0. }; @@ -181,24 +187,27 @@ void kalmanEstimation(TypeKalman *k, float accVert){ div = k->P[0][0] + SigAltiGPS*SigAltiGPS; if (fabs(div) > 1e-5) { - // K = P*C'*inv(div) + // Kf = 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) + // X = X + Kf*(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); + k->X[i] = k->X[i]+Kf[i]*(altitude_gps - constante); } // calcul de P - // P = P - K*C*P - constante = Kf[0]; + // P = P - Kf*C*P + I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = 0; + I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0; + I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 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]; + k->P[i][j] = k->P[i][j] - I[i][0]*k->P[0][j] - I[i][1]*k->P[1][j] - I[i][2]*k->P[2][j]; } } } @@ -214,6 +223,7 @@ void kalmanEstimation(TypeKalman *k, float accVert){ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ int i, j, div; + float I[3][3]; // matrice temporaire float Kf[3] = { 0., 0., 0. }; @@ -221,25 +231,29 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ // 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 = 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]; + // X = X + Kf*(meas - C*X) + float constante = k->X[0] + k->X[2]; for(i=0;i<3;i++){ - k->X[i] = k->X[i]+Kf[i]*(altitude_altimetre-constante); + 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]; + // P = P - Kf*C*P + I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = Kf[0]; + I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = Kf[1]; + I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = Kf[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]; + k->P[i][j] = k->P[i][j] - I[i][0]*k->P[0][j] - I[i][1]*k->P[1][j] - I[i][2]*k->P[2][j]; } } } From dd9e8b4c5bc3d553a47c5f3f5d2d499175ce7f66 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 27 May 2011 13:17:35 +0200 Subject: [PATCH 06/45] add joystich and airframe files --- conf/airframes/ENAC/fixed-wing/spocIII-2.xml | 22 +- conf/airframes/ENAC/fixed-wing/spocIII-3.xml | 219 +++++++++++++++++++ conf/joystick/gp_interlink.xml | 21 ++ 3 files changed, 252 insertions(+), 10 deletions(-) create mode 100644 conf/airframes/ENAC/fixed-wing/spocIII-3.xml create mode 100644 conf/joystick/gp_interlink.xml diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml index 2983b7f7c3c..95541bb2062 100644 --- a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml +++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml @@ -23,6 +23,7 @@ + @@ -38,7 +39,7 @@ - + @@ -88,7 +89,7 @@
- +
@@ -98,7 +99,7 @@
- +
@@ -111,11 +112,12 @@ - + +
@@ -136,13 +138,13 @@ - - + + - + - + @@ -167,7 +169,7 @@ - + @@ -184,7 +186,7 @@ - + diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-3.xml b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml new file mode 100644 index 00000000000..a3a7ac3f168 --- /dev/null +++ b/conf/airframes/ENAC/fixed-wing/spocIII-3.xml @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + +
+ + +
+ +
+ + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + + +
+ +
+ + + +
+ + diff --git a/conf/joystick/gp_interlink.xml b/conf/joystick/gp_interlink.xml new file mode 100644 index 00000000000..e431724c48b --- /dev/null +++ b/conf/joystick/gp_interlink.xml @@ -0,0 +1,21 @@ + + + + + + +