From a320f8e08ea7c618cc9d7b2d4f209fa06bd0e0c7 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 23 Aug 2012 17:56:44 +0200 Subject: [PATCH] [state] finally getting rid of old estimator --- conf/airframes/examples/demo_module.xml | 211 ------------------ .../subsystems/fixedwing/autopilot.makefile | 1 - conf/settings/estimation/fw_baro_kalman.xml | 2 +- sw/airborne/arch/sim/jsbsim_gps.c | 1 - sw/airborne/arch/sim/jsbsim_hw.h | 1 - sw/airborne/arch/sim/jsbsim_ir.c | 1 - sw/airborne/arch/sim/sim_ap.c | 1 - sw/airborne/arch/sim/sim_baro.c | 4 +- sw/airborne/arch/sim/sim_gps.c | 1 - sw/airborne/estimator.c | 37 --- sw/airborne/estimator.h | 41 ---- sw/airborne/firmwares/fixedwing/autopilot.h | 1 - .../fixedwing/guidance/energy_ctrl.c | 10 +- sw/airborne/firmwares/fixedwing/main_ap.c | 5 +- sw/airborne/modules/com/generic_com.c | 1 - sw/airborne/modules/gsm/gsm.c | 1 - sw/airborne/rc_settings.c | 1 - 17 files changed, 11 insertions(+), 309 deletions(-) delete mode 100644 conf/airframes/examples/demo_module.xml delete mode 100644 sw/airborne/estimator.c delete mode 100644 sw/airborne/estimator.h diff --git a/conf/airframes/examples/demo_module.xml b/conf/airframes/examples/demo_module.xml deleted file mode 100644 index 94ef391cea..0000000000 --- a/conf/airframes/examples/demo_module.xml +++ /dev/null @@ -1,211 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - -
- - - - - - - - - - - -
- - -
- -
- - - - -
- -
- - - - - - - - - - - - - - - - - - - - -
- -
- -
- -
- - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - - - - - - -
- -
- - - -
- -
- - - - - - - - -
- -
- - - - - -
- - -CONFIG = \"tiny_2_1_1.h\" - -include $(PAPARAZZI_SRC)/conf/firmwares/tiny.makefile - -FLASH_MODE=IAP - -ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DSYS_TIME_LED=1 -ap.srcs = mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c $(SRC_ARCH)/armVIC.c $(SRC_FIRMWARE)/main_fbw.c $(SRC_FIRMWARE)/main_ap.c $(SRC_FIRMWARE)/main.c - -ap.CFLAGS += -DINTER_MCU -ap.srcs += inter_mcu.c - -ap.srcs += commands.c - -########## RC actuators + radio -ap.CFLAGS += -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -ap.srcs += $(SRC_ARCH)/servos_4017_hw.c actuators.c - -ap.CFLAGS += -DRADIO_CONTROL -ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c - -########## Modem -ap.CFLAGS += -DDOWNLINK -DUSE_UART1 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart1 -DDOWNLINK_AP_DEVICE=Uart1 -DPPRZ_UART=Uart1 -DDATALINK=PPRZ -DUART1_BAUD=B9600 -ap.srcs += subsystems/datalink/downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c subsystems/datalink/pprz_transport.c - -########## ADC -ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -ap.srcs += $(SRC_ARCH)/adc_hw.c - -########## GPS -ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART0 -DGPS_LINK=Uart0 -DUART0_BAUD=B38400 -DGPS_USE_LATLONG -ap.srcs += gps_ubx.c gps.c latlong.c - -########## IR sensors -ap.CFLAGS += -DUSE_INFRARED -DALT_KALMAN -ap.srcs += infrared.c estimator.c - -########## Nav -ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -ap.srcs += subsystems/nav.c fw_h_ctl.c fw_v_ctl.c -ap.srcs += subsystems/navigation/nav_survey_rectangle.c -ap.srcs += subsystems/navigation/nav_line.c - - -# Config for SITL simulation -include $(PAPARAZZI_SRC)/conf/firmwares/sitl.makefile -sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN - -sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c - - - diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index c7ecfc010a..0e91400c10 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -162,7 +162,6 @@ fbw_srcs += $(SRC_FIRMWARE)/fbw_downlink.c ap_CFLAGS += -DAP ap_srcs += $(SRC_FIRMWARE)/main_ap.c -ap_srcs += $(SRC_FIXEDWING)/estimator.c ap_CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_float.h\" ap_srcs += $(SRC_FIXEDWING)/subsystems/ins.c ap_srcs += $(SRC_FIXEDWING)/subsystems/ins/ins_float.c diff --git a/conf/settings/estimation/fw_baro_kalman.xml b/conf/settings/estimation/fw_baro_kalman.xml index 34f11f32de..66012cb48d 100644 --- a/conf/settings/estimation/fw_baro_kalman.xml +++ b/conf/settings/estimation/fw_baro_kalman.xml @@ -6,7 +6,7 @@ - + diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index f489ac35a4..94ca3e4715 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -9,7 +9,6 @@ #include "generated/flight_plan.h" #include "autopilot.h" #include "subsystems/gps.h" -#include "estimator.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_int.h" diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h index 1202dce646..fa32784663 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.h +++ b/sw/airborne/arch/sim/jsbsim_hw.h @@ -31,7 +31,6 @@ #include "std.h" #include "inter_mcu.h" #include "firmwares/fixedwing/autopilot.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/flight_plan.h" diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c index f850dd9574..1259852536 100644 --- a/sw/airborne/arch/sim/jsbsim_ir.c +++ b/sw/airborne/arch/sim/jsbsim_ir.c @@ -7,7 +7,6 @@ #include "jsbsim_hw.h" #include -#include "estimator.h" #ifndef JSBSIM_IR_ROLL_NEUTRAL #define JSBSIM_IR_ROLL_NEUTRAL 0. diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index c0ad122a24..7eaffac836 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -12,7 +12,6 @@ #include "std.h" #include "inter_mcu.h" #include "autopilot.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/navigation/traffic_info.h" #include "generated/settings.h" diff --git a/sw/airborne/arch/sim/sim_baro.c b/sw/airborne/arch/sim/sim_baro.c index 923961727b..cf6221bb29 100644 --- a/sw/airborne/arch/sim/sim_baro.c +++ b/sw/airborne/arch/sim/sim_baro.c @@ -1,5 +1,5 @@ #include -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "subsystems/gps.h" #include "baro_MS5534A.h" @@ -32,6 +32,6 @@ void baro_MS5534A_send(void) { void baro_MS5534A_event_task( void ) { baro_MS5534A_pressure = baro_MS5534A_ground_pressure - (gps.hmsl/1000.-ground_alt) / 0.08 + ((10.*random()) / RAND_MAX); - baro_MS5534A_temp = 10 + estimator_z; + baro_MS5534A_temp = 10 + stateGetPositionUtm_f()->alt; baro_MS5534A_available = TRUE; } diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 9e51ba9330..70a5a8c18e 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -9,7 +9,6 @@ #include "generated/flight_plan.h" #include "autopilot.h" #include "subsystems/gps.h" -#include "estimator.h" #include "math/pprz_geodetic_float.h" #include "math/pprz_geodetic_int.h" diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c deleted file mode 100644 index e4852a9fd1..0000000000 --- a/sw/airborne/estimator.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Paparazzi autopilot $Id$ - * - * Copyright (C) 2004-2010 The Paparazzi Team - * - * 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 estimator.c - * \brief State estimate, fusioning sensors - */ - -#include -#include - -#include "estimator.h" - -void estimator_init( void ) { - -} - diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h deleted file mode 100644 index 55b8e31780..0000000000 --- a/sw/airborne/estimator.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * $Id$ - * - * Copyright (C) 2004-2006 Pascal Brisset, Antoine Drouin - * - * 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 estimator.h - * \brief State estimation, fusioning sensors - * TODO will be removed when the remaining variables are integrated to the state interface - */ - -#ifndef ESTIMATOR_H -#define ESTIMATOR_H - -#include - -#include "std.h" - -#include "state.h" - -void estimator_init( void ); - -#endif /* ESTIMATOR_H */ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 9df70ea617..bd8f20c1dd 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -33,7 +33,6 @@ #include #include "std.h" #include "mcu_periph/sys_time.h" -#include "estimator.h" #define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2) diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index fe930e0d2e..81403c64ae 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -53,7 +53,7 @@ #pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!" #include "firmwares/fixedwing/guidance/energy_ctrl.h" -#include "estimator.h" +#include "state.h" #include "subsystems/nav.h" #include "generated/airframe.h" #include "firmwares/fixedwing/autopilot.h" @@ -145,13 +145,13 @@ static void ac_char_update(float throttle, float pitch, float climb, float accel { ac_char_climb_count++; ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count ); - ac_char_average(&ac_char_climb_max , estimator_z_dot, ac_char_climb_count ); + ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count ); } else if (throttle <= 0.0f) { ac_char_descend_count++; ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count ); - ac_char_average(&ac_char_descend_max , estimator_z_dot , ac_char_descend_count ); + ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count ); } else if ((climb > -0.125) && (climb < 0.125)) { @@ -194,7 +194,7 @@ void v_ctl_altitude_loop( void ) if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; // Altitude Controller - v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z; + v_ctl_altitude_error = v_ctl_altitude_setpoint - stateCalcPositionUtm_f()->alt; float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ; BoundAbs(sp, v_ctl_max_climb); @@ -247,7 +247,7 @@ void v_ctl_climb_loop( void ) float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot ); // Flight Path Outerloop: positive means needs to climb more: needs extra energy - float gamma_err = (v_ctl_climb_setpoint - estimator_z_dot) / v_ctl_auto_airspeed_setpoint; + float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_setpoint; // Total Energy Error: positive means energy should be added float en_tot_err = gamma_err + vdot_err; diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 3de67679c2..ab24f631f4 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -58,8 +58,8 @@ #endif // autopilot & control +#include "state.h" #include "firmwares/fixedwing/autopilot.h" -#include "estimator.h" #include "subsystems/ins.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include CTRL_TYPE_H @@ -184,6 +184,8 @@ void init_ap( void ) { ins_init(); + stateInit(); + /************* Links initialization ***************/ #if defined MCU_SPI_LINK link_mcu_init(); @@ -195,7 +197,6 @@ void init_ap( void ) { /************ Internal status ***************/ h_ctl_init(); v_ctl_init(); - estimator_init(); nav_init(); modules_init(); diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 8d31627e32..b99dc3d174 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -28,7 +28,6 @@ #include "mcu_periph/i2c.h" #include "state.h" -#include "estimator.h" #include "subsystems/gps.h" #include "subsystems/electrical.h" #include "generated/airframe.h" diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index c011c16d23..4389f87100 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -71,7 +71,6 @@ Receiving: #include "ap_subsystems/datalink/downlink.h" #include "subsystems/gps.h" #include "autopilot.h" -#include "estimator.h" //#include "subsystems/navigation/common_nav.h" //why is should this be needed? #include "generated/settings.h" diff --git a/sw/airborne/rc_settings.c b/sw/airborne/rc_settings.c index 15573d398a..494bf6c606 100644 --- a/sw/airborne/rc_settings.c +++ b/sw/airborne/rc_settings.c @@ -30,7 +30,6 @@ #include "autopilot.h" #include "subsystems/nav.h" #include "subsystems/sensors/infrared.h" -#include "estimator.h" #include "inter_mcu.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"