Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 "state.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 accept 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 
137 
138 
140 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
141 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
142 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
143 #endif
144 #ifndef STALL_AIRSPEED
145 #warning "No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED"
146 #define STALL_AIRSPEED NOMINAL_AIRSPEED
147 #endif
148 #ifndef V_CTL_GLIDE_RATIO
149 #define V_CTL_GLIDE_RATIO 8.
150 #warning "V_CTL_GLIDE_RATIO not defined - default is 8."
151 #endif
152 #ifndef AIRSPEED_SETPOINT_SLEW
153 #define AIRSPEED_SETPOINT_SLEW 1
154 #endif
155 #ifndef V_CTL_MAX_ACCELERATION
156 #define V_CTL_MAX_ACCELERATION 0.5
157 #endif
158 // Automatically found airplane characteristics
160 
161 float ac_char_climb_pitch = 0.0f;
162 float ac_char_climb_max = 0.0f;
165 float ac_char_descend_max = 0.0f;
168 float ac_char_cruise_pitch = 0.0f;
170 
171 static void ac_char_average(float* last_v, float new_v, int count)
172 {
173  *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count);
174 }
175 
176 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
177 {
178  if ((accelerate > -0.02) && (accelerate < 0.02))
179  {
180  if (throttle >= 1.0f)
181  {
185  }
186  else if (throttle <= 0.0f)
187  {
191  }
192  else if ((climb > -0.125) && (climb < 0.125))
193  {
197  }
198  }
199 }
200 
201 
202 void v_ctl_init( void ) {
203  /* mode */
205 
206  /* outer loop */
209  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
210 
211 #ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
212  v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
213 #else
215 #endif
216 
217  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
219  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
220 
222 
223  /* inner loops */
225 
226  /* "auto throttle" inner loop parameters */
227  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
228  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
229  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
230  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
231  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
232 
233  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
234  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
235  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
236 
237 
238 #ifdef V_CTL_ENERGY_TOT_PGAIN
239  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
240  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
241  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
242  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
243 #else
248 #warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
249 #endif
250 
251 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
253 #else
254  v_ctl_max_climb = 2;
255 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
256 #endif
257 
258 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
259  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
260  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
261  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
263 #endif
264 
266 }
267 
268 const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
269 const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
270 
277 {
278  // Airspeed Command Saturation
280 
281  // Altitude Controller
284 
285  // Vertical Speed Limiter
286  BoundAbs(sp, v_ctl_max_climb);
287 
288  // Vertical Acceleration Limiter
289  float incr = sp - v_ctl_climb_setpoint;
290  BoundAbs(incr, 2 * dt_navigation);
291  v_ctl_climb_setpoint += incr;
292 }
293 
294 
295 // Running Average Filter
296 float lp_vdot[5];
297 
298 static float low_pass_vdot(float v);
299 static float low_pass_vdot(float v)
300 {
301  lp_vdot[4] += (v - lp_vdot[4]) / 3;
302  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
303  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
304  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
305  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
306 
307  return lp_vdot[0];
308 }
309 
314 void v_ctl_climb_loop( void )
315 {
316  // Airspeed setpoint rate limiter:
317  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
319  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
320  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
321 
322 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
323  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
324  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f()));
325  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
328 
329  // Do not allow controlled airspeed below the setpoint
330  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
332  // reset integrator of ground speed loop
334  }
335 #else
337 #endif
338 
339  // Airspeed outerloop: positive means we need to accelerate
340  float speed_error = v_ctl_auto_airspeed_controlled - (*stateGetAirspeed_f());
341 
342  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
343  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
344  BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);
345 
346  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
347 #ifndef SITL
348  struct Int32Vect3 accel_meas_body;
349  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
350  INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
351  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
352 #else
353  float vdot = 0;
354 #endif
355 
356  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
357  float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
358 
359  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
361 
362  // Total Energy Error: positive means energy should be added
363  float en_tot_err = gamma_err + vdot_err;
364 
365  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
366  float en_dis_err = gamma_err - vdot_err;
367 
368  // Auto Cruise Throttle
370  {
372  v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
373  + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
375  }
376 
377  // Total Controller
381  + v_ctl_energy_total_pgain * en_tot_err;
382 
383  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle==1))
384  {
385  // If your energy supply is not sufficient, then neglect the climb requirement
386  en_dis_err = -vdot_err;
387 
388  // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
391  }
392 
393 
394  /* pitch pre-command */
396  {
398  + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
399  Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
400  }
401  float v_ctl_pitch_of_vz =
402  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
403  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
405  + v_ctl_energy_diff_pgain * en_dis_err
407 if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO;
408 
409  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
410  Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
411 
412  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
413 
414  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
415 }
416 
417 
418 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
419 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
420 #endif
421 
422 #ifndef V_CTL_THROTTLE_SLEW
423 #define V_CTL_THROTTLE_SLEW 1.
424 #endif
425 
428 void v_ctl_throttle_slew( void ) {
430  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
431  v_ctl_throttle_slewed += diff_throttle;
432 }
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
static void ac_char_average(float *last_v, float new_v, int count)
Definition: energy_ctrl.c:171
bool_t launch
Definition: sim_ap.c:40
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
Attitude and Heading Reference System interface.
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:91
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
float ac_char_climb_max
Definition: energy_ctrl.c:162
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:134
float v_ctl_desired_acceleration
Definition: energy_ctrl.c:104
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
#define V_CTL_MAX_ACCELERATION
Definition: energy_ctrl.c:156
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
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:141
float v_ctl_altitude_pre_climb
Path Angle.
Definition: energy_ctrl.c:92
float z
in meters
float controlled_throttle
Definition: guidance_v_n.c:85
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
int ac_char_cruise_count
Definition: energy_ctrl.c:169
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:119
int ac_char_descend_count
Definition: energy_ctrl.c:166
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:428
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:314
float ac_char_descend_max
Definition: energy_ctrl.c:165
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:80
int ac_char_climb_count
Definition: energy_ctrl.c:163
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:423
float ac_char_cruise_throttle
Definition: energy_ctrl.c:167
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:116
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:299
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:136
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
static float * stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1199
#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
float v_ctl_max_climb
Definition: energy_ctrl.c:97
const float dt_navigation
Definition: energy_ctrl.c:269
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:47
#define STALL_AIRSPEED
Definition: energy_ctrl.c:146
float ac_char_climb_pitch
Definition: energy_ctrl.c:161
bool_t kill_throttle
Definition: autopilot.c:41
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:176
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:153
#define V_CTL_MODE_AUTO_CLIMB
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:93
#define V_CTL_GLIDE_RATIO
Definition: energy_ctrl.c:149
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:78
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:106
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
float ac_char_cruise_pitch
Definition: energy_ctrl.c:168
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:840
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: energy_ctrl.c:110
float lp_vdot[5]
Definition: energy_ctrl.c:296
rotation matrix
float v_ctl_auto_throttle_nominal_cruise_pitch
Definition: energy_ctrl.c:108
void v_ctl_init(void)
Definition: energy_ctrl.c:202
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:125
#define NAVIGATION_FREQUENCY
Definition: autopilot.h:145
#define CONTROL_FREQUENCY
Definition: autopilot.h:140
#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:164
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:129
float alt
in meters above WGS84 reference ellipsoid
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:135
const float dt_attidude
Definition: energy_ctrl.c:268
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
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:276
Fixedwing autopilot modes.