Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 
65 #include "state.h"
67 #include "generated/airframe.h"
68 #include "autopilot.h"
69 #include "subsystems/abi.h"
70 
72 
73 /* mode */
78 
79 #ifdef LOITER_TRIM
80 #error "Energy Controller can not accept Loiter Trim"
81 #endif
82 //#ifdef V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE
83 //#error
84 
86 
87 /* outer loop */
93 
96 
97 /* inner loop */
99 
100 /* "auto throttle" inner loop parameters */
102 
108 
114 
117 
120 
124 
129 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
130 
134 
135 
138 
141 
142 
144 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
145 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
146 INFO("V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s")
147 #endif
148 #ifndef STALL_AIRSPEED
149 INFO("No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED")
150 #define STALL_AIRSPEED NOMINAL_AIRSPEED
151 #endif
152 #ifndef V_CTL_GLIDE_RATIO
153 #define V_CTL_GLIDE_RATIO 8.
154 INFO("V_CTL_GLIDE_RATIO not defined - default is 8.")
155 #endif
156 #ifndef AIRSPEED_SETPOINT_SLEW
157 #define AIRSPEED_SETPOINT_SLEW 1
158 #endif
159 #ifndef V_CTL_MAX_ACCELERATION
160 #define V_CTL_MAX_ACCELERATION 0.5
161 #endif
162 
163 #ifndef V_CTL_ENERGY_IMU_ID
164 #define V_CTL_ENERGY_IMU_ID ABI_BROADCAST
165 #endif
166 // Automatically found airplane characteristics
168 
169 float ac_char_climb_pitch = 0.0f;
170 float ac_char_climb_max = 0.0f;
173 float ac_char_descend_max = 0.0f;
176 float ac_char_cruise_pitch = 0.0f;
178 
179 static void ac_char_average(float *last_v, float new_v, int count)
180 {
181  *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count);
182 }
183 
184 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
185 {
186  if ((accelerate > -0.02) && (accelerate < 0.02)) {
187  if (throttle >= 1.0f) {
188  ac_char_climb_count++;
189  ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count);
190  ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count);
191  } else if (throttle <= 0.0f) {
192  ac_char_descend_count++;
193  ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count);
194  ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count);
195  } else if ((climb > -0.125) && (climb < 0.125)) {
196  ac_char_cruise_count++;
197  ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count);
198  ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count);
199  }
200  }
201 }
202 
203 static void accel_cb(uint8_t sender_id __attribute__((unused)),
204  uint32_t stamp __attribute__((unused)),
205  struct Int32Vect3 *accel)
206 {
207  accel_imu_meas = *accel;
208 }
209 
210 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
211  struct FloatQuat *q_b2i_f)
212 {
214 }
215 
216 void v_ctl_init(void)
217 {
218  /* mode */
220 
221  /* outer loop */
224  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
225 
226 #ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
227  v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
228 #else
230 #endif
231 
232  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
234  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
235 
237 
238  /* inner loops */
240 
241  /* "auto throttle" inner loop parameters */
242  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
243  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
244  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
245  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
246  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
247 
248  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
249  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
250  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
251 
252 
253 #ifdef V_CTL_ENERGY_TOT_PGAIN
254  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
255  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
256  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
257  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
258 #else
263 #warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
264 #endif
265 
266 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
268 #else
269  v_ctl_max_climb = 2;
270 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
271 #endif
272 
273 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
274  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
275  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
276  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
278 #endif
279 
281 
283 
284  AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
285  AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
286 }
287 
288 const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
289 const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
290 
297 {
298  // Airspeed Command Saturation
300 
301  // Altitude Controller
304 
305  // Vertical Speed Limiter
306  BoundAbs(sp, v_ctl_max_climb);
307 
308  // Vertical Acceleration Limiter
309  float incr = sp - v_ctl_climb_setpoint;
310  BoundAbs(incr, 2 * dt_navigation);
311  v_ctl_climb_setpoint += incr;
312 }
313 
314 
315 // Running Average Filter
316 float lp_vdot[5];
317 
318 static float low_pass_vdot(float v);
319 static float low_pass_vdot(float v)
320 {
321  lp_vdot[4] += (v - lp_vdot[4]) / 3;
322  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
323  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
324  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
325  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
326 
327  return lp_vdot[0];
328 }
329 
335 {
336  // Airspeed setpoint rate limiter:
337  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
339  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
340  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
341 
342 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
343 // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
345  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
349 
350  // Do not allow controlled airspeed below the setpoint
351  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
353  // reset integrator of ground speed loop
356  }
357 #else
359 #endif
360 
361  // Airspeed outerloop: positive means we need to accelerate
362  float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();
363 
364  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
365  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
367 
368  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
369 #ifndef SITL
370  /* convert last imu accel measurement to float */
371  struct FloatVect3 accel_imu_f;
372  ACCELS_FLOAT_OF_BFP(accel_imu_f, accel_imu_meas);
373  /* rotate from imu to body frame */
374  struct FloatVect3 accel_meas_body;
375  float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
376  float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
377 #else
378  float vdot = 0;
379 #endif
380 
381  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
382  float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);
383 
384  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
386 
387  // Total Energy Error: positive means energy should be added
388  float en_tot_err = gamma_err + vdot_err;
389 
390  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
391  float en_dis_err = gamma_err - vdot_err;
392 
393  // Auto Cruise Throttle
396  v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
397  + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
399  }
400 
401  // Total Controller
405  + v_ctl_energy_total_pgain * en_tot_err;
406 
407  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (autopilot_throttle_killed() == 1)) {
408  // If your energy supply is not sufficient, then neglect the climb requirement
409  en_dis_err = -vdot_err;
410 
411  // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
412  if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }
414  }
415 
416 
417  /* pitch pre-command */
420  + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
421  Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
422  }
423  float v_ctl_pitch_of_vz =
424  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
425  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
427  + v_ctl_energy_diff_pgain * en_dis_err
429  if (autopilot_throttle_killed()) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
430 
431  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
432  Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)
433 
434  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
435 
436  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
437 }
438 
439 
440 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
441 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
442 #endif
443 
444 #ifndef V_CTL_THROTTLE_SLEW
445 #define V_CTL_THROTTLE_SLEW 1.
446 #endif
447 
451 {
453  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
454  v_ctl_throttle_slewed += diff_throttle;
455 }
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:92
#define V_CTL_ENERGY_IMU_ID
Definition: energy_ctrl.c:164
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
bool launch
request launch
Definition: autopilot.h:65
float v_ctl_airspeed_pgain
Definition: energy_ctrl.c:91
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
static void ac_char_average(float *last_v, float new_v, int count)
Definition: energy_ctrl.c:179
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float ac_char_climb_max
Definition: energy_ctrl.c:170
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
float v_ctl_desired_acceleration
Definition: energy_ctrl.c:101
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:225
float v_ctl_energy_diff_pgain
Definition: energy_ctrl.c:118
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:127
#define V_CTL_MAX_ACCELERATION
Definition: energy_ctrl.c:160
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
float v_ctl_auto_pitch_of_airspeed_pgain
Definition: energy_ctrl.c:111
int16_t pprz_t
Definition: paparazzi.h:6
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:145
float v_ctl_altitude_pre_climb
Path Angle.
Definition: energy_ctrl.c:89
Main include for ABI (AirBorneInterface).
float controlled_throttle
Definition: guidance_v_n.c:85
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
int ac_char_cruise_count
Definition: energy_ctrl.c:177
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:116
#define CONTROL_FREQUENCY
int ac_char_descend_count
Definition: energy_ctrl.c:174
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:450
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:334
float ac_char_descend_max
Definition: energy_ctrl.c:173
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
int ac_char_climb_count
Definition: energy_ctrl.c:171
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:445
float ac_char_cruise_throttle
Definition: energy_ctrl.c:175
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:113
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:319
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
Roation quaternion.
static abi_event accel_ev
Definition: energy_ctrl.c:139
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:129
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
float v_ctl_auto_throttle_climb_throttle_increment
Definition: energy_ctrl.c:106
float v_ctl_auto_groundspeed_sum_err
Definition: energy_ctrl.c:128
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
float v_ctl_energy_diff_igain
Definition: energy_ctrl.c:119
float v_ctl_max_climb
Definition: energy_ctrl.c:94
const float dt_navigation
Definition: energy_ctrl.c:289
#define STALL_AIRSPEED
Definition: energy_ctrl.c:150
unsigned long uint32_t
Definition: types.h:18
#define NAVIGATION_FREQUENCY
float ac_char_climb_pitch
Definition: energy_ctrl.c:169
float v_ctl_auto_throttle_of_airspeed_igain
Definition: energy_ctrl.c:110
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
Definition: energy_ctrl.c:184
void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, const struct FloatVect3 *v_in)
rotate 3D vector by quaternion.
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
static void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi)
float v_ctl_energy_total_pgain
Definition: energy_ctrl.c:115
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
#define AIRSPEED_SETPOINT_SLEW
Definition: energy_ctrl.c:157
#define V_CTL_MODE_AUTO_CLIMB
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:739
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:90
#define V_CTL_GLIDE_RATIO
Definition: energy_ctrl.c:153
Core autopilot interface common to all firmwares.
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:75
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f)
Definition: energy_ctrl.c:210
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
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:176
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: energy_ctrl.c:107
static struct Int32Vect3 accel_imu_meas
Definition: energy_ctrl.c:137
float lp_vdot[5]
Definition: energy_ctrl.c:316
float z
in meters
float v_ctl_auto_throttle_nominal_cruise_pitch
Definition: energy_ctrl.c:105
void v_ctl_init(void)
Definition: energy_ctrl.c:216
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:122
static struct FloatQuat imu_to_body_quat
Definition: energy_ctrl.c:136
#define MAX_PPRZ
Definition: paparazzi.h:8
#define V_CTL_MODE_MANUAL
float v_ctl_auto_throttle_of_airspeed_pgain
Definition: energy_ctrl.c:109
float ac_char_descend_pitch
Definition: energy_ctrl.c:172
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:126
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: energy_ctrl.c:203
static abi_event body_to_imu_ev
Definition: energy_ctrl.c:140
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
const float dt_attidude
Definition: energy_ctrl.c:288
float v_ctl_auto_pitch_of_airspeed_igain
Definition: energy_ctrl.c:112
float v_ctl_max_acceleration
Definition: energy_ctrl.c:95
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:296