Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[state] finally getting rid of old estimator

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-08-23 17:56:44 +02:00
parent e9794e8986
commit a320f8e08e
17 changed files with 11 additions and 309 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/demo_module.xml
-211
View File
@@ -1,211 +0,0 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<airframe name="Demo module on Tiny2.1">
<modules>
<load name="demo_module.xml"/>
</modules>
<servos>
<servo name="MOTOR_LEFT" no="3" min="1000" neutral="1000" max="2000"/>
<servo name="MOTOR_RIGHT" no="4" min="1000" neutral="1000" max="2000"/>
<servo name="AILEVON_LEFT" no="6" min="1900" neutral="1500" max="1100"/>
<servo name="AILEVON_RIGHT" no="7" min="1100" neutral="1500" max="1900"/>
</servos>
<commands>
<axis name="THROTTLE" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THROTTLE" value="@THROTTLE"/>
<set command="ROLL" value="@ROLL"/>
<set command="PITCH" value="@PITCH"/>
<set command="YAW" value="@YAW"/>
</rc_commands>
<section name="MIXER">
<define name="AILEVON_AILERON_RATE" value="0.75"/>
<define name="AILEVON_ELEVATOR_RATE" value="0.75"/>
</section>
<command_laws>
<let var="aileron" value="@ROLL * AILEVON_AILERON_RATE"/>
<let var="elevator" value="@PITCH * AILEVON_ELEVATOR_RATE"/>
<let var="yaw" value="@YAW"/>
<set servo="MOTOR_LEFT" value="@THROTTLE - $yaw"/>
<set servo="MOTOR_RIGHT" value="@THROTTLE + $yaw"/>
<set servo="AILEVON_LEFT" value="$elevator + $aileron"/>
<set servo="AILEVON_RIGHT" value="$elevator - $aileron"/>
</command_laws>
<section name="AUTO1" prefix="AUTO1_">
<define name="MAX_ROLL" value="0.85"/>
<define name="MAX_PITCH" value="0.6"/>
</section>
<section name="adc" prefix="ADC_CHANNEL_">
<define name="IR1" value="ADC_1"/>
<define name="IR2" value="ADC_2"/>
<define name="IR_TOP" value="ADC_0"/>
<define name="IR_NB_SAMPLES" value="16"/>
</section>
<section name="INFRARED" prefix="IR_">
<define name="ADC_IR1_NEUTRAL" value="512"/>
<define name="ADC_IR2_NEUTRAL" value="512"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
<define name="LATERAL_CORRECTION" value="1."/>
<define name="LONGITUDINAL_CORRECTION" value="1."/>
<define name="VERTICAL_CORRECTION" value="1."/>
<define name="HORIZ_SENSOR_TILTED" value="1"/>
<define name="IR1_SIGN" value="-1"/>
<define name="IR2_SIGN" value="1"/>
<define name="TOP_SIGN" value="1"/>
<define name="ROLL_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="0." unit="deg"/>
<define name="CORRECTION_UP" value="1."/>
<define name="CORRECTION_DOWN" value="1."/>
<define name="CORRECTION_LEFT" value="1."/>
<define name="CORRECTION_RIGHT" value="1."/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
</section>
<section name="MISC">
<define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
<define name="CARROT" value="5." unit="s"/>
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
<define name="CONTROL_FREQUENCY" value="60" unit="Hz"/>
<define name="ALT_KALMAN_ENABLED" value="TRUE"/>
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
</section>
<section name="VERTICAL CONTROL" prefix="V_CTL_">
<define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
<define name="ALTITUDE_PGAIN" value="0.04"/>
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
<!-- auto throttle inner loop -->
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.35"/>
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.3"/>
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
<define name="AUTO_THROTTLE_DASH_TRIM" value="-1000"/>
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
<define name="AUTO_THROTTLE_PGAIN" value="0.02"/>
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.1"/>
<!-- auto pitch inner loop -->
<define name="AUTO_PITCH_PGAIN" value="0.05"/>
<define name="AUTO_PITCH_IGAIN" value="0.075"/>
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
</section>
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
<define name="COURSE_PGAIN" value="1."/>
<define name="ROLL_MAX_SETPOINT" value="0.7" unit="rad"/>
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
<define name="PITCH_PGAIN" value="6000."/>
<define name="PITCH_DGAIN" value="0."/>
<define name="ELEVATOR_OF_ROLL" value="2500"/>
<define name="ROLL_ATTITUDE_GAIN" value="7500"/>
<define name="ROLL_RATE_GAIN" value="1500"/>
</section>
<section name="NAV">
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
<define name="NAV_CROSS_TRACK_ERROR_IGAIN" value="0."/>
<define name="NAV_CROSS_TRACK_ERROR_MAX" value="10." unit="deg"/>
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_THROTTLE" value="0.7"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_PITCH" value="0.25"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="DESCENT_PITCH" value="-0.15"/><!-- Pitch for Aggressive Decent -->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
<section name="FAILSAFE" prefix="FAILSAFE_">
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
<define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
<define name="DEFAULT_ROLL" value="0.3" unit="rad"/>
<define name="DEFAULT_PITCH" value="0.5" unit="rad"/>
<define name="HOME_RADIUS" value="100" unit="m"/>
</section>
<makefile>
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
</makefile>
</airframe>
conf/firmwares/subsystems/fixedwing/autopilot.makefile
-1
View File
@@ -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
conf/settings/estimation/fw_baro_kalman.xml
+1 -1
View File
@@ -6,7 +6,7 @@
<dl_settings>
<dl_settings NAME="alt_baro">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="estimator"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="alt_kalman_enabled" shortname="alt_kalman" module="subsystems/ins/ins_float"/>
</dl_settings>
</dl_settings>
sw/airborne/arch/sim/jsbsim_gps.c
-1
View File
@@ -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"
sw/airborne/arch/sim/jsbsim_hw.h
-1
View File
@@ -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"
sw/airborne/arch/sim/jsbsim_ir.c
-1
View File
@@ -7,7 +7,6 @@
#include "jsbsim_hw.h"
#include <math.h>
#include "estimator.h"
#ifndef JSBSIM_IR_ROLL_NEUTRAL
#define JSBSIM_IR_ROLL_NEUTRAL 0.
sw/airborne/arch/sim/sim_ap.c
-1
View File
@@ -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"
sw/airborne/arch/sim/sim_baro.c
+2 -2
View File
@@ -1,5 +1,5 @@
#include <stdlib.h>
#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;
}
sw/airborne/arch/sim/sim_gps.c
-1
View File
@@ -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"
sw/airborne/estimator.c
-37
View File
@@ -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 <inttypes.h>
#include <math.h>
#include "estimator.h"
void estimator_init( void ) {
}
sw/airborne/estimator.h
-41
View File
@@ -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 <inttypes.h>
#include "std.h"
#include "state.h"
void estimator_init( void );
#endif /* ESTIMATOR_H */
sw/airborne/firmwares/fixedwing/autopilot.h
-1
View File
@@ -33,7 +33,6 @@
#include <inttypes.h>
#include "std.h"
#include "mcu_periph/sys_time.h"
#include "estimator.h"
#define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+5 -5
View File
@@ -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;
sw/airborne/firmwares/fixedwing/main_ap.c
+3 -2
View File
@@ -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();
sw/airborne/modules/com/generic_com.c
-1
View File
@@ -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"
sw/airborne/modules/gsm/gsm.c
-1
View File
@@ -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"
sw/airborne/rc_settings.c
-1
View File
@@ -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"