diff --git a/conf/firmwares/subsystems/fixedwing/control.makefile b/conf/firmwares/subsystems/fixedwing/control.makefile index 440dbdeda0..44ca3e90e2 100644 --- a/conf/firmwares/subsystems/fixedwing/control.makefile +++ b/conf/firmwares/subsystems/fixedwing/control.makefile @@ -4,3 +4,6 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/guidance_v.c + +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile b/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile index a4d5e1aa9a..b867d966cd 100644 --- a/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile +++ b/conf/firmwares/subsystems/fixedwing/control_adaptive.makefile @@ -5,3 +5,5 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v.c +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_energy.makefile b/conf/firmwares/subsystems/fixedwing/control_energy.makefile new file mode 100644 index 0000000000..84119585f9 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/control_energy.makefile @@ -0,0 +1,9 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Standard fixed wing control loops + + +$(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude.c $(SRC_FIRMWARE)/guidance/energy_ctrl.c + +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/energy_ctrl.h\" + diff --git a/conf/firmwares/subsystems/fixedwing/control_new.makefile b/conf/firmwares/subsystems/fixedwing/control_new.makefile index d10475f361..0df9d74f6f 100644 --- a/conf/firmwares/subsystems/fixedwing/control_new.makefile +++ b/conf/firmwares/subsystems/fixedwing/control_new.makefile @@ -4,3 +4,6 @@ $(TARGET).srcs += $(SRC_FIRMWARE)/stabilization/stabilization_adaptive.c $(SRC_FIRMWARE)/guidance/guidance_v_n.c +$(TARGET).CFLAGS += -DCTRL_TYPE_H=\"firmwares/fixedwing/guidance/guidance_v.h\" + + diff --git a/conf/flight_plans/versatile_airspeed.xml b/conf/flight_plans/versatile_airspeed.xml new file mode 100644 index 0000000000..1a0aeafbc5 --- /dev/null +++ b/conf/flight_plans/versatile_airspeed.xml @@ -0,0 +1,189 @@ + + + +
+#include "firmwares/fixedwing/guidance/energy_ctrl.h" +#include "subsystems/datalink/datalink.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index 36c179be5d..a02ef10a2c 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -110,6 +110,7 @@ + diff --git a/conf/modules/airspeed_adc.xml b/conf/modules/airspeed_adc.xml index 0617cc95aa..bcd84e8d2a 100644 --- a/conf/modules/airspeed_adc.xml +++ b/conf/modules/airspeed_adc.xml @@ -20,7 +20,7 @@ - + diff --git a/conf/settings/control/ctl_energy.xml b/conf/settings/control/ctl_energy.xml new file mode 100644 index 0000000000..4c2e865c52 --- /dev/null +++ b/conf/settings/control/ctl_energy.xml @@ -0,0 +1,107 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/settings/estimation/ac_char.xml b/conf/settings/estimation/ac_char.xml new file mode 100644 index 0000000000..81d4d0f3a5 --- /dev/null +++ b/conf/settings/estimation/ac_char.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/ap_downlink.h b/sw/airborne/firmwares/fixedwing/ap_downlink.h index 90311b1acb..f309f37094 100644 --- a/sw/airborne/firmwares/fixedwing/ap_downlink.h +++ b/sw/airborne/firmwares/fixedwing/ap_downlink.h @@ -90,7 +90,12 @@ }) -#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint); +#ifndef USE_AIRSPEED +#define PERIODIC_SEND_DESIRED(_trans, _dev) { float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint);} +#else +#define PERIODIC_SEND_DESIRED(_trans, _dev) DOWNLINK_SEND_DESIRED(_trans, _dev, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint); +#endif + #if USE_INFRARED #define PERIODIC_SEND_STATE_FILTER_STATUS(_trans, _dev) { uint16_t contrast = abs(infrared.roll) + abs(infrared.pitch) + abs(infrared.top); uint8_t mde = 3; if (contrast < 50) mde = 7; DOWNLINK_SEND_STATE_FILTER_STATUS(_trans, _dev, &mde, &contrast); } diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c new file mode 100644 index 0000000000..f721a9ef1f --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -0,0 +1,318 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin, Michel Gorraz + * + * 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 firmwares/fixedwing/guidance/guidance_v.c + * Vertical control using total energy control for fixed wing vehicles. + * + + + ================================================= + Energy: + ------ + E = mgh + 1/2mV^2 + Edot / V = (gamma + Vdot/g) * W + + equilibrium + + Vdot / g = Thrust/W - Drag/W - sin(gamma) + with: Drag/Weight = (Cl/Cd)^-1 + + -glide angle: Vdot = 0, T=0 ==> gamma = Cd/Cl + -level flight: Vdot = 0, gamma=0 ==> W/T = Cl/Cd + ================================================= + + Strategy: thrust = path + acceleration[g] (total energy) + pitch = path - acceleration[g] (energy balance) + + Pseudo-Control Unit = dimensionless acceleration [g] + + - pitch <-> pseudocontrol: sin(Theta) steers Vdot in [g] + - throttle <-> pseudocontrol: motor characteristic as function of V x throttle steeds VDot + + */ + +#pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!" + +#include "firmwares/fixedwing/guidance/energy_ctrl.h" +#include "estimator.h" +#include "subsystems/nav.h" +#include "generated/airframe.h" +#include "firmwares/fixedwing/autopilot.h" +#include "subsystems/ahrs.h" +#include "subsystems/imu.h" + +/////// DEFAULT GUIDANCE_V NECESSITIES ////// + +/* mode */ +uint8_t v_ctl_mode = V_CTL_MODE_MANUAL; +uint8_t v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; +uint8_t v_ctl_auto_throttle_submode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; +float v_ctl_auto_throttle_sum_err = 0; +float v_ctl_auto_airspeed_controlled = 0; +float v_ctl_auto_groundspeed_setpoint = 0; + +#ifdef LOITER_TRIM +#error "Energy Controller can not accep Loiter Trim" +#endif +//#ifdef V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE +//#error + +/////// ACTUALLY USED STUFF ////// + +/* outer loop */ +float v_ctl_altitude_setpoint; +float v_ctl_altitude_pre_climb; ///< Path Angle +float v_ctl_altitude_pgain; +float v_ctl_airspeed_pgain; +float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low + +float v_ctl_auto_airspeed_setpoint; ///< in meters per second + +float v_ctl_max_climb = 2; +float v_ctl_max_acceleration = 0.5; + +/* inner loop */ +float v_ctl_climb_setpoint; + +/* "auto throttle" inner loop parameters */ +float v_ctl_desired_acceleration; + +float v_ctl_auto_throttle_cruise_throttle; +float v_ctl_auto_throttle_nominal_cruise_throttle; +float v_ctl_auto_throttle_nominal_cruise_pitch; +float v_ctl_auto_throttle_climb_throttle_increment; +float v_ctl_auto_throttle_pitch_of_vz_pgain; + +float v_ctl_auto_throttle_of_airspeed_pgain; +float v_ctl_auto_throttle_of_airspeed_igain; +float v_ctl_auto_pitch_of_airspeed_pgain; +float v_ctl_auto_pitch_of_airspeed_igain; +float v_ctl_auto_pitch_of_airspeed_dgain; + +float v_ctl_energy_total_pgain; +float v_ctl_energy_total_igain; + +float v_ctl_energy_diff_pgain; +float v_ctl_energy_diff_igain; + + +pprz_t v_ctl_throttle_setpoint; +pprz_t v_ctl_throttle_slewed; + + +///////////////////////////////////////////////// +// Automatically found airplane characteristics + +float ac_char_climb_pitch = 0.0f; +float ac_char_climb_max = 0.0f; +int ac_char_climb_count = 0; +float ac_char_descend_pitch = 0.0f; +float ac_char_descend_max = 0.0f; +int ac_char_descend_count = 0; +float ac_char_cruise_throttle = 0.0f; +float ac_char_cruise_pitch = 0.0f; +int ac_char_cruise_count = 0; + +static void ac_char_average(float* last, float new, int count) +{ + *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count); +} + +static void ac_char_update(float throttle, float pitch, float climb, float accelerate) +{ + if ((accelerate > -0.02) && (accelerate < 0.02)) + { + if (throttle >= 1.0f) + { + 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 ); + } + 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 ); + } + else if ((climb > -0.125) && (climb < 0.125)) + { + ac_char_cruise_count++; + ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count ); + ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count ); + } + } +} + + +void v_ctl_init( void ) { + /* mode */ + v_ctl_mode = V_CTL_MODE_MANUAL; + + /* outer loop */ + v_ctl_altitude_setpoint = 0.; + v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN; + v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; + + /* inner loops */ + v_ctl_climb_setpoint = 0.; + + /* "auto throttle" inner loop parameters */ + v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; + v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT; + v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN; + + v_ctl_throttle_setpoint = 0; +} + +/** + * outer loop + * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode + */ + +void v_ctl_altitude_loop( void ) +{ + // Imput Checks + 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; + float sp = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb ; + BoundAbs(sp, v_ctl_max_climb); + + float incr = sp - v_ctl_climb_setpoint; + BoundAbs(incr, 2.0 / 4.0); + v_ctl_climb_setpoint += incr; +} + + +/** + * auto throttle inner loop + * \brief + */ + +const float dt = 0.01f; + +float lp_vdot[5]; + +static float low_pass_vdot(float v); +static float low_pass_vdot(float v) +{ + lp_vdot[4] += (v - lp_vdot[4]) / 3; + lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3; + lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3; + lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3; + lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3; + + return lp_vdot[0]; +} + +void v_ctl_climb_loop( void ) +{ + // Airspeed outerloop: positive means we need to accelerate + float speed_error = v_ctl_auto_airspeed_setpoint - estimator_airspeed; + + // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration + v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f; + BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration); + + // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration +#ifndef SITL + struct FloatVect3 accel_float = {0,0,0}; + ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); + float vdot = ( accel_float.x / 9.81f - sin(ahrs_float.ltp_to_imu_euler.theta) ); +#else + float vdot = 0; +#endif + + // Acceleration Error: positive means UAV needs to accelerate: needs extra energy + 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; + + // Total Energy Error: positive means energy should be added + float en_tot_err = gamma_err + vdot_err; + + // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up + float en_dis_err = gamma_err - vdot_err; + + // Auto Cruise Throttle + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) + { + v_ctl_auto_throttle_nominal_cruise_throttle += + v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt + + en_tot_err * v_ctl_energy_total_igain * dt; + if (v_ctl_auto_throttle_nominal_cruise_throttle < 0.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 0.0f; + else if (v_ctl_auto_throttle_nominal_cruise_throttle > 1.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 1.0f; + } + + // Total Controller + float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + + v_ctl_auto_throttle_of_airspeed_pgain * speed_error + + v_ctl_energy_total_pgain * en_tot_err; + + + if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f)) + { + // If your energy supply is not sufficient, then neglect the climb requirement + en_dis_err = -vdot_err; + } + + /* pitch pre-command */ + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) + { + v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt + + v_ctl_energy_diff_igain * en_dis_err * dt; + Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); + } + float v_ctl_pitch_of_vz = + + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain + - v_ctl_auto_pitch_of_airspeed_pgain * speed_error + + v_ctl_auto_pitch_of_airspeed_dgain * vdot + + v_ctl_energy_diff_pgain * en_dis_err + + v_ctl_auto_throttle_nominal_cruise_pitch; + + nav_pitch = v_ctl_pitch_of_vz; + + ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration); + + v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); +} + + +#ifdef V_CTL_THROTTLE_SLEW_LIMITER +#define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER)) +#endif + +#ifndef V_CTL_THROTTLE_SLEW +#define V_CTL_THROTTLE_SLEW 1. +#endif + +/** \brief Computes slewed throttle from throttle setpoint + called at 20Hz + */ +void v_ctl_throttle_slew( void ) { + pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; + BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); + v_ctl_throttle_slewed += diff_throttle; +} diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h new file mode 100644 index 0000000000..c3bb8b49a6 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2012 TUDelft + * + * 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 firmwares/fixedwing/guidance/energy_ctrl.h + * Vertical control using total energy control for fixed wing vehicles. + * + */ + +#ifndef FW_V_CTL_ENERGY_H +#define FW_V_CTL_ENERGY_H + +#include "firmwares/fixedwing/guidance/guidance_common.h" + +/* outer loop */ +// extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low +extern float v_ctl_altitude_setpoint; ///< in meters above MSL +extern float v_ctl_altitude_pre_climb; ///< Path Angle +extern float v_ctl_altitude_pgain; +extern float v_ctl_airspeed_pgain; + +extern float v_ctl_auto_airspeed_setpoint; ///< in meters per second + +extern float v_ctl_max_climb; +extern float v_ctl_max_acceleration; + +/* "auto throttle" inner loop parameters */ +extern float v_ctl_desired_acceleration; + +extern float v_ctl_auto_throttle_nominal_cruise_throttle; +extern float v_ctl_auto_throttle_nominal_cruise_pitch; +extern float v_ctl_auto_throttle_climb_throttle_increment; +extern float v_ctl_auto_throttle_pitch_of_vz_pgain; + +extern float v_ctl_auto_throttle_of_airspeed_pgain; +extern float v_ctl_auto_throttle_of_airspeed_igain; +extern float v_ctl_auto_pitch_of_airspeed_pgain; +extern float v_ctl_auto_pitch_of_airspeed_igain; +extern float v_ctl_auto_pitch_of_airspeed_dgain; + +extern float v_ctl_energy_total_pgain; +extern float v_ctl_energy_total_igain; + +extern float v_ctl_energy_diff_pgain; +extern float v_ctl_energy_diff_igain; + +///////////////////////////////////////////////// +// Automatically found airplane characteristics + +extern float ac_char_climb_pitch; +extern float ac_char_climb_max; +extern float ac_char_descend_pitch; +extern float ac_char_descend_max; +extern float ac_char_cruise_throttle; +extern float ac_char_cruise_pitch; + +#endif /* FW_V_CTL_H */ diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h new file mode 100644 index 0000000000..78aff51e96 --- /dev/null +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h @@ -0,0 +1,88 @@ +/* + * Copyright (C) 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 firmwares/fixedwing/guidance/guidance_v.h + * Vertical control for fixed wing vehicles. + * + */ + +#ifndef FW_V_CTL_COMMON_H +#define FW_V_CTL_COMMON_H + + +#include +#include "paparazzi.h" + +/* Vertical mode */ +#define V_CTL_MODE_MANUAL 0 +#define V_CTL_MODE_AUTO_THROTTLE 1 +#define V_CTL_MODE_AUTO_CLIMB 2 +#define V_CTL_MODE_AUTO_ALT 3 +#define V_CTL_MODE_NB 4 +extern uint8_t v_ctl_mode; + +extern float v_ctl_climb_setpoint; +extern uint8_t v_ctl_climb_mode; + +#define V_CTL_CLIMB_MODE_AUTO_THROTTLE 0 +#define V_CTL_CLIMB_MODE_AUTO_PITCH 1 + +extern uint8_t v_ctl_auto_throttle_submode; +#define V_CTL_AUTO_THROTTLE_STANDARD 0 +#define V_CTL_AUTO_THROTTLE_AGRESSIVE 1 +#define V_CTL_AUTO_THROTTLE_BLENDED 2 + +// Needed for telemetry +extern float v_ctl_auto_throttle_sum_err; + +// Needed for course loop gain +extern float v_ctl_altitude_error; ///< in meters, (setpoint - alt) -> positive = too low + +// Old airspeed code wants: +extern float v_ctl_auto_airspeed_controlled; +extern float v_ctl_auto_groundspeed_setpoint; + +extern float v_ctl_auto_throttle_cruise_throttle; +extern pprz_t v_ctl_throttle_setpoint; +extern pprz_t v_ctl_throttle_slewed; + +extern void v_ctl_init( void ); +extern void v_ctl_altitude_loop( void ); +extern void v_ctl_climb_loop ( void ); + +/** Computes throttle_slewed from throttle_setpoint */ +extern void v_ctl_throttle_slew( void ); + +#define guidance_v_SetCruiseThrottle(_v) { \ + v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \ + Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \ +} + +#define guidance_v_SetAutoThrottleIgain(_v) { \ + v_ctl_auto_throttle_igain = _v; \ + v_ctl_auto_throttle_sum_err = 0; \ + } + + + +#endif + diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index a4fd061a31..d1f0598c53 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -235,6 +235,7 @@ void v_ctl_climb_loop ( void ) { v_ctl_climb_auto_throttle_loop(); break; #ifdef V_CTL_AUTO_PITCH_PGAIN +#pragma message "AUTO PITCH Enabled!" case V_CTL_CLIMB_MODE_AUTO_PITCH: v_ctl_climb_auto_pitch_loop(); break; @@ -302,7 +303,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { f_throttle = controlled_throttle; v_ctl_auto_throttle_sum_err += err; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - nav_pitch += v_ctl_pitch_of_vz; + nav_pitch = v_ctl_pitch_of_vz; #if defined AGR_CLIMB break; } /* switch submode */ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 65d797ab74..1bd7a548f1 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -58,7 +58,7 @@ #include "firmwares/fixedwing/autopilot.h" #include "estimator.h" #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "subsystems/nav.h" #include "generated/flight_plan.h" #ifdef TRAFFIC_INFO @@ -474,10 +474,33 @@ void navigation_task( void ) { #endif if (lateral_mode >=LATERAL_MODE_COURSE) h_ctl_course_loop(); /* aka compute nav_desired_roll */ - if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) - v_ctl_climb_loop(); + + // climb_loop(); //4Hz + } + energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) +} + + +#if USE_AHRS +#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP +volatile uint8_t new_ins_attitude = 0; +#endif +#endif + +void attitude_loop( void ) { + +#if USE_INFRARED + ahrs_update_infrared(); +#endif /* USE_INFRARED */ + + if (pprz_mode >= PPRZ_MODE_AUTO2) + { if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) v_ctl_throttle_setpoint = nav_throttle_setpoint; + else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) + { + v_ctl_climb_loop(); + } #if defined V_CTL_THROTTLE_IDLE Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ); @@ -495,19 +518,6 @@ void navigation_task( void ) { if (kill_throttle || (!estimator_flight_time && !launch)) v_ctl_throttle_setpoint = 0; } - energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours) -} - - -#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP -volatile uint8_t new_ins_attitude = 0; -#endif - -void attitude_loop( void ) { - -#if USE_INFRARED - ahrs_update_infrared(); -#endif /* USE_INFRARED */ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index cf9a09ff90..f504a9ef53 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -35,7 +35,7 @@ #include "estimator.h" #include "subsystems/nav.h" #include "generated/airframe.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 313ed6cc95..bb238c76d8 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -34,7 +34,7 @@ #include "estimator.h" #include "subsystems/nav.h" #include "generated/airframe.h" -#include "firmwares/fixedwing/guidance/guidance_v.h" +#include CTRL_TYPE_H #include "firmwares/fixedwing/autopilot.h" #pragma message "CAUTION! ALL control gains have to be positive now!"