From 8dcd9c7f409df1d8cae7d01c1be38dadc40fea79 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 22 Mar 2011 10:30:49 +0100 Subject: [PATCH] update the adaptive control for fixedwing and split some of the settings files --- .../subsystems/fixedwing/control_new.makefile | 2 +- conf/settings/flight_params.xml | 34 ++++++++++ conf/settings/fw_ctl_n.xml | 48 ++----------- conf/settings/infrared.xml | 20 ++++++ conf/settings/ins_basic.xml | 10 +++ conf/settings/ir_i2c.xml | 11 +++ .../fixedwing/guidance/guidance_v_n.c | 38 ++++++++--- .../stabilization/stabilization_adaptive.c | 67 ++++++++++++++----- .../stabilization/stabilization_adaptive.h | 8 +-- 9 files changed, 164 insertions(+), 74 deletions(-) create mode 100644 conf/settings/flight_params.xml create mode 100644 conf/settings/infrared.xml create mode 100644 conf/settings/ins_basic.xml diff --git a/conf/autopilot/subsystems/fixedwing/control_new.makefile b/conf/autopilot/subsystems/fixedwing/control_new.makefile index 267b9eff8f..db21505cbc 100644 --- a/conf/autopilot/subsystems/fixedwing/control_new.makefile +++ b/conf/autopilot/subsystems/fixedwing/control_new.makefile @@ -1,4 +1,4 @@ # new fixed wing control loops with merged auto pitch and auto throttle, adaptive horizontal control +$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c -$(TARGET).srcs += $(SRC_FIXEDWING)/fw_h_ctl_a.c $(SRC_FIXEDWING)/fw_v_ctl_n.c diff --git a/conf/settings/flight_params.xml b/conf/settings/flight_params.xml new file mode 100644 index 0000000000..1fcf980283 --- /dev/null +++ b/conf/settings/flight_params.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/fw_ctl_n.xml b/conf/settings/fw_ctl_n.xml index 5f2fdab3fa..df393b0711 100644 --- a/conf/settings/fw_ctl_n.xml +++ b/conf/settings/fw_ctl_n.xml @@ -4,53 +4,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + @@ -94,8 +60,6 @@ - - diff --git a/conf/settings/infrared.xml b/conf/settings/infrared.xml new file mode 100644 index 0000000000..ee2d434796 --- /dev/null +++ b/conf/settings/infrared.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/ins_basic.xml b/conf/settings/ins_basic.xml new file mode 100644 index 0000000000..386a15c7db --- /dev/null +++ b/conf/settings/ins_basic.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/conf/settings/ir_i2c.xml b/conf/settings/ir_i2c.xml index 40304d0c2a..b386b4c6d9 100644 --- a/conf/settings/ir_i2c.xml +++ b/conf/settings/ir_i2c.xml @@ -3,6 +3,17 @@ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index 839a0da20b..02ecaaf826 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -57,7 +57,7 @@ float v_ctl_auto_throttle_pgain; float v_ctl_auto_throttle_igain; float v_ctl_auto_throttle_dgain; float v_ctl_auto_throttle_sum_err; -#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 150 +#define V_CTL_AUTO_THROTTLE_MAX_SUM_ERR 0.3 float v_ctl_auto_throttle_pitch_of_vz_pgain; float v_ctl_auto_throttle_pitch_of_vz_dgain; @@ -70,14 +70,21 @@ float v_ctl_auto_pitch_pgain; float v_ctl_auto_pitch_dgain; float v_ctl_auto_pitch_igain; float v_ctl_auto_pitch_sum_err; -#define V_CTL_AUTO_PITCH_MAX_SUM_ERR 100 +#define V_CTL_AUTO_PITCH_MAX_SUM_ERR (RadOfDeg(10.)) + +#ifndef V_CTL_AUTO_PITCH_DGAIN +#define V_CTL_AUTO_PITCH_DGAIN 0. +#endif +#ifndef V_CTL_AUTO_PITCH_IGAIN +#define V_CTL_AUTO_PITCH_IGAIN 0. +#endif float controlled_throttle; pprz_t v_ctl_throttle_setpoint; pprz_t v_ctl_throttle_slewed; // Set higher than 2*V_CTL_ALTITUDE_MAX_CLIMB to disable -#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s +#define V_CTL_AUTO_CLIMB_LIMIT (0.5/4.0) // m/s/s uint8_t v_ctl_speed_mode; @@ -181,16 +188,21 @@ void v_ctl_altitude_loop( void ) { static inline void v_ctl_set_pitch ( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_pitch_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_pitch_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); + if (v_ctl_auto_pitch_igain < 0.) { + v_ctl_auto_pitch_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); + } // PI loop + feedforward ctl - nav_pitch = nav_pitch + nav_pitch = 0. //nav_pitch FIXME it really sucks ! + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint + v_ctl_auto_pitch_pgain * err + v_ctl_auto_pitch_dgain * d_err @@ -201,13 +213,18 @@ static inline void v_ctl_set_pitch ( void ) { static inline void v_ctl_set_throttle( void ) { static float last_err = 0.; + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + v_ctl_auto_throttle_sum_err = 0; + // Compute errors float err = estimator_z_dot - v_ctl_climb_setpoint; float d_err = err - last_err; last_err = err; - v_ctl_auto_throttle_sum_err += err*(1./60.); - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); + if (v_ctl_auto_throttle_igain < 0.) { + v_ctl_auto_throttle_sum_err += err*(1./60.); + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); + } // PID loop + feedforward ctl controlled_throttle = v_ctl_auto_throttle_cruise_throttle @@ -269,8 +286,7 @@ void v_ctl_climb_loop ( void ) { } // Set Throttle output - float f_throttle = controlled_throttle; - v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); + v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); } @@ -279,7 +295,7 @@ void v_ctl_climb_loop ( void ) { #endif #ifndef V_CTL_THROTTLE_SLEW -#define V_CTL_THROTTLE_SLEW 1. +#define V_CTL_THROTTLE_SLEW (1.) #endif /** \brief Computes slewed throttle from throttle setpoint called at 20Hz diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 7aff370af2..29b3489cc5 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -107,17 +107,44 @@ float v_ctl_pitch_dash_trim; inline static void h_ctl_roll_loop( void ); inline static void h_ctl_pitch_loop( void ); +// Some default course gains +// H_CTL_COURSE_PGAIN needs to be define in airframe #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION #define H_CTL_COURSE_PRE_BANK_CORRECTION 1. #endif - #ifndef H_CTL_COURSE_DGAIN #define H_CTL_COURSE_DGAIN 0. #endif +// Some default roll gains +// H_CTL_ROLL_ATTITUDE_GAIN needs to be define in airframe #ifndef H_CTL_ROLL_RATE_GAIN #define H_CTL_ROLL_RATE_GAIN 0. #endif +#ifndef H_CTL_ROLL_IGAIN +#define H_CTL_ROLL_IGAIN 0. +#endif +#ifndef H_CTL_ROLL_KFFA +#define H_CTL_ROLL_KFFA 0. +#endif +#ifndef H_CTL_ROLL_KFFD +#define H_CTL_ROLL_KFFD 0. +#endif + +// Some default pitch gains +// H_CTL_PITCH_PGAIN needs to be define in airframe +#ifndef H_CTL_PITCH_DGAIN +#define H_CTL_PITCH_DGAIN 0. +#endif +#ifndef H_CTL_PITCH_IGAIN +#define H_CTL_PITCH_IGAIN 0. +#endif +#ifndef H_CTL_PITCH_KFFA +#define H_CTL_PITCH_KFFA 0. +#endif +#ifndef H_CTL_PITCH_KFFD +#define H_CTL_PITCH_KFFD 0. +#endif void h_ctl_init( void ) { h_ctl_course_setpoint = 0.; @@ -158,7 +185,7 @@ void h_ctl_init( void ) { use_airspeed_ratio = FALSE; airspeed_ratio2 = 1.; -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM v_ctl_pitch_loiter_trim = V_CTL_PITCH_LOITER_TRIM; v_ctl_pitch_dash_trim = V_CTL_PITCH_DASH_TRIM; #else @@ -227,9 +254,6 @@ inline static void h_ctl_roll_loop( void ) { static float cmd_fb = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_roll_sum_err = 0; - #ifdef USE_ANGLE_REF // Update reference setpoints for roll h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT; @@ -271,9 +295,16 @@ inline static void h_ctl_roll_loop( void ) { #endif float d_err = estimator_p - h_ctl_ref_roll_rate; - if (h_ctl_roll_igain < 0.) { - h_ctl_roll_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_roll_sum_err = 0.; + } + else { + if (h_ctl_roll_igain < 0.) { + h_ctl_roll_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain)); + } else { + h_ctl_roll_sum_err = 0.; + } } cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err; @@ -303,7 +334,7 @@ float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM; float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; #endif -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM inline static void loiter(void) { float pitch_trim; @@ -335,11 +366,8 @@ inline static void h_ctl_pitch_loop( void ) { if (h_ctl_pitch_of_roll <0.) h_ctl_pitch_of_roll = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL) - h_ctl_pitch_sum_err = 0; - h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi); -#ifdef PITCH_TRIM +#ifdef USE_PITCH_TRIM loiter(); #endif @@ -369,9 +397,16 @@ inline static void h_ctl_pitch_loop( void ) { float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate; last_err = err; - if (h_ctl_roll_igain < 0.) { - h_ctl_pitch_sum_err += err * H_CTL_REF_DT; - BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain); + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { + h_ctl_pitch_sum_err = 0.; + } + else { + if (h_ctl_pitch_igain < 0.) { + h_ctl_pitch_sum_err += err * H_CTL_REF_DT; + BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain)); + } else { + h_ctl_pitch_sum_err = 0.; + } } float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index 37e1a55638..4a7c082f22 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -46,16 +46,16 @@ extern float h_ctl_pitch_Kffa; extern float h_ctl_pitch_Kffd; extern float h_ctl_pitch_of_roll; -#define H_CTL_ROLL_SUM_ERR_MAX 100. -#define H_CTL_PITCH_SUM_ERR_MAX 100. +#define H_CTL_ROLL_SUM_ERR_MAX (MAX_PPRZ/2.) +#define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.) #define fw_h_ctl_a_SetRollIGain(_gain) { \ - h_ctl_roll_sum_err = 0; \ + h_ctl_roll_sum_err = 0.; \ h_ctl_roll_igain = _gain; \ } #define fw_h_ctl_a_SetPitchIGain(_gain) { \ - h_ctl_pitch_sum_err = 0; \ + h_ctl_pitch_sum_err = 0.; \ h_ctl_pitch_igain = _gain; \ }