Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
energy_ctrl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 TUDelft, Tobias Muench
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
64 #pragma message "CAUTION! Using TOTAL ENERGY CONTROLLER: Experimental!"
65 
67 #include "estimator.h"
68 #include "subsystems/nav.h"
69 #include "generated/airframe.h"
71 #include "subsystems/ahrs.h"
72 #include "subsystems/imu.h"
73 
75 
76 /* mode */
81 
82 #ifdef LOITER_TRIM
83 #error "Energy Controller can not accep Loiter Trim"
84 #endif
85 //#ifdef V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE
86 //#error
87 
89 
90 /* outer loop */
96 
99 
100 /* inner loop */
102 
103 /* "auto throttle" inner loop parameters */
105 
111 
117 
120 
123 
127 
132 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
133 
136 
137 
139 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
140 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
141 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
142 #endif
143 #ifndef STALL_AIRSPEED
144 #define STALL_AIRSPEED NOMINAL_AIRSPEED
145 #endif
146 #ifndef AIRSPEED_SETPOINT_SLEW
147 #define AIRSPEED_SETPOINT_SLEW 1
148 #endif
149 
151 // Automatically found airplane characteristics
152 
153 float ac_char_climb_pitch = 0.0f;
154 float ac_char_climb_max = 0.0f;
157 float ac_char_descend_max = 0.0f;
160 float ac_char_cruise_pitch = 0.0f;
162 
163 static void ac_char_average(float* last, float new, int count)
164 {
165  *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count);
166 }
167 
168 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
169 {
170  if ((accelerate > -0.02) && (accelerate < 0.02))
171  {
172  if (throttle >= 1.0f)
173  {
177  }
178  else if (throttle <= 0.0f)
179  {
183  }
184  else if ((climb > -0.125) && (climb < 0.125))
185  {
189  }
190  }
191 }
192 
193 
194 void v_ctl_init( void ) {
195  /* mode */
197 
198  /* outer loop */
200 
201 #ifdef V_CTL_ALTITUDE_PGAIN
202  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
203 #endif
204 #ifdef V_CTL_AIRSPEED_PGAIN
205  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
206 #endif
207 
208  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
210 
211 
212  /* inner loops */
214 
215  /* "auto throttle" inner loop parameters */
216  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
217 
218 #ifdef V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT
219  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
220  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
221 #endif
222 
223 #ifdef V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN
224  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
225  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
226 #endif
227 
228 #ifdef V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN
229  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
230  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
231  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
232 #endif
233 
234 
235 #ifdef V_CTL_ENERGY_TOT_PGAIN
236  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
237  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
238  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
239  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
240 #endif
241 
242 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
244 #else
245  v_ctl_max_climb = 2;
246 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
247 #endif
248 
249 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
250  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
251  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
252  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
254 #endif
255 
257 }
258 
265 {
266  // Imput Checks
268 
269  // Altitude Controller
272  BoundAbs(sp, v_ctl_max_climb);
273 
274  float incr = sp - v_ctl_climb_setpoint;
275  BoundAbs(incr, 2.0 / 4.0);
276  v_ctl_climb_setpoint += incr;
277 }
278 
279 
285 const float dt = 0.01f;
286 
287 float lp_vdot[5];
288 
289 static float low_pass_vdot(float v);
290 static float low_pass_vdot(float v)
291 {
292  lp_vdot[4] += (v - lp_vdot[4]) / 3;
293  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
294  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
295  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
296  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
297 
298  return lp_vdot[0];
299 }
300 
301 void v_ctl_climb_loop( void )
302 {
303 #ifdef AIRSPEED_SETPOINT_SLEW
304  // airspeed_setpoint ratelimiter:
305  float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew; // FIXME
306  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * NOMINAL_AIRSPEED);
307  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
308 #endif
309 
310 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
311  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
312  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod);
313  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
316 
317  // Do not allow controlled airspeed below the setpoint
320  // reset integrator of ground speed loop
322  }
323 #else
325 #endif
326 
327  // Airspeed outerloop: positive means we need to accelerate
329 
330  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
331  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
333 
334  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
335 #ifndef SITL
336  struct Int32Vect3 accel_meas_body;
338  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(ahrs_float.ltp_to_imu_euler.theta);
339 #else
340  float vdot = 0;
341 #endif
342 
343  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
344  float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
345 
346  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
348 
349  // Total Energy Error: positive means energy should be added
350  float en_tot_err = gamma_err + vdot_err;
351 
352  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
353  float en_dis_err = gamma_err - vdot_err;
354 
355  // Auto Cruise Throttle
357  {
360  + en_tot_err * v_ctl_energy_total_igain * dt;
362  }
363 
364  // Total Controller
368  + v_ctl_energy_total_pgain * en_tot_err;
369 
370 
371  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f))
372  {
373  // If your energy supply is not sufficient, then neglect the climb requirement
374  en_dis_err = -vdot_err;
375  }
376 
377  /* pitch pre-command */
379  {
381  + v_ctl_energy_diff_igain * en_dis_err * dt;
382  Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
383  }
384  float v_ctl_pitch_of_vz =
385  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
386  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
388  + v_ctl_energy_diff_pgain * en_dis_err
390 
391  nav_pitch = v_ctl_pitch_of_vz;
392  Bound(nav_pitch,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
393 
394  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
395 
396  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
397 }
398 
399 
400 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
401 #define V_CTL_THROTTLE_SLEW (1./CONTROL_RATE/(V_CTL_THROTTLE_SLEW_LIMITER))
402 #endif
403 
404 #ifndef V_CTL_THROTTLE_SLEW
405 #define V_CTL_THROTTLE_SLEW 1.
406 #endif
407 
411 void v_ctl_throttle_slew( void ) {
413  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
414  v_ctl_throttle_slewed += diff_throttle;
415 }
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:95
float v_ctl_airspeed_pgain
Definition: energy_ctrl.c:94
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:107
bool_t launch
Definition: sim_ap.c:41
Attitude and Heading Reference System interface.
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:91
float ac_char_climb_max
Definition: energy_ctrl.c:154
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:134
float v_ctl_desired_acceleration
Definition: energy_ctrl.c:104
float estimator_z_dot
Definition: estimator.c:46
float v_ctl_energy_diff_pgain
Definition: energy_ctrl.c:121
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:128
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:130
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:101
float v_ctl_auto_pitch_of_airspeed_pgain
Definition: energy_ctrl.c:114
int16_t pprz_t
Definition: paparazzi.h:6
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:140
float v_ctl_altitude_pre_climb
Path Angle.
Definition: energy_ctrl.c:92
float controlled_throttle
Definition: guidance_v_n.c:85
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
int ac_char_cruise_count
Definition: energy_ctrl.c:161
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:119
int ac_char_descend_count
Definition: energy_ctrl.c:158
float theta
in radians
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:411
void v_ctl_climb_loop(void)
Definition: energy_ctrl.c:301
float ac_char_descend_max
Definition: energy_ctrl.c:157
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:80
int ac_char_climb_count
Definition: energy_ctrl.c:155
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:405
float ac_char_cruise_throttle
Definition: energy_ctrl.c:159
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:116
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:290
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:132
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
float v_ctl_auto_throttle_climb_throttle_increment
Definition: energy_ctrl.c:109
float v_ctl_auto_groundspeed_sum_err
Definition: energy_ctrl.c:131
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
float v_ctl_energy_diff_igain
Definition: energy_ctrl.c:122
static void ac_char_average(float *last, float new, int count)
Definition: energy_ctrl.c:163
float v_ctl_max_climb
Definition: energy_ctrl.c:97
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
float estimator_airspeed
m/s
Definition: estimator.c:69
#define STALL_AIRSPEED
Definition: energy_ctrl.c:144
float ac_char_climb_pitch
Definition: energy_ctrl.c:153
float v_ctl_auto_throttle_of_airspeed_igain
Definition: energy_ctrl.c:113
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
Definition: energy_ctrl.c:168
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:79
float v_ctl_energy_total_pgain
Definition: energy_ctrl.c:118
#define ACCEL_FLOAT_OF_BFP(_ai)
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77
Inertial Measurement Unit interface.
#define AIRSPEED_SETPOINT_SLEW
Definition: energy_ctrl.c:147
#define V_CTL_MODE_AUTO_CLIMB
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:93
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:78
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
unsigned char uint8_t
Definition: types.h:14
float estimator_z
altitude above MSL in meters
Definition: estimator.c:44
float ac_char_cruise_pitch
Definition: energy_ctrl.c:160
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: energy_ctrl.c:110
float lp_vdot[5]
Definition: energy_ctrl.c:287
State estimation, fusioning sensors.
const float dt
auto throttle inner loop
Definition: energy_ctrl.c:285
float v_ctl_auto_throttle_nominal_cruise_pitch
Definition: energy_ctrl.c:108
void v_ctl_init(void)
Definition: energy_ctrl.c:194
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:125
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define MAX_PPRZ
Definition: paparazzi.h:8
#define V_CTL_MODE_MANUAL
float v_ctl_auto_throttle_of_airspeed_pgain
Definition: energy_ctrl.c:112
float ac_char_descend_pitch
Definition: energy_ctrl.c:156
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:129
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:135
float v_ctl_auto_pitch_of_airspeed_igain
Definition: energy_ctrl.c:115
float v_ctl_max_acceleration
Definition: energy_ctrl.c:98
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:126
struct FloatEulers ltp_to_imu_euler
Rotation from LocalTangentPlane to IMU frame as Euler angles.
Definition: ahrs.h:60
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:264