Paparazzi UAS  v5.15_devel-81-gd13dafb
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 uint8_t v_ctl_speed_mode; //To be compatible with universal flightplan, not used for etecs
136 
139 
142 
143 
145 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
146 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
147 INFO("V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2 , indicating 2 m/s")
148 #endif
149 #ifndef STALL_AIRSPEED
150 INFO("No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED")
151 #define STALL_AIRSPEED NOMINAL_AIRSPEED
152 #endif
153 #ifndef V_CTL_GLIDE_RATIO
154 #define V_CTL_GLIDE_RATIO 8.
155 INFO("V_CTL_GLIDE_RATIO not defined - default is 8.")
156 #endif
157 #ifndef AIRSPEED_SETPOINT_SLEW
158 #define AIRSPEED_SETPOINT_SLEW 1
159 #endif
160 #ifndef V_CTL_MAX_ACCELERATION
161 #define V_CTL_MAX_ACCELERATION 0.5 //G
162 #endif
163 
164 #ifndef V_CTL_ENERGY_IMU_ID
165 #define V_CTL_ENERGY_IMU_ID ABI_BROADCAST
166 #endif
167 // Automatically found airplane characteristics
169 
170 float ac_char_climb_pitch = 0.0f;
171 float ac_char_climb_max = 0.0f;
174 float ac_char_descend_max = 0.0f;
177 float ac_char_cruise_pitch = 0.0f;
179 
180 static void ac_char_average(float *last_v, float new_v, int count)
181 {
182  *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count);
183 }
184 
185 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
186 {
187  if ((accelerate > -0.02) && (accelerate < 0.02)) {
188  if (throttle >= 1.0f) {
189  ac_char_climb_count++;
190  ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count);
191  ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count);
192  } else if (throttle <= 0.0f) {
193  ac_char_descend_count++;
194  ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count);
195  ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count);
196  } else if ((climb > -0.125) && (climb < 0.125)) {
197  ac_char_cruise_count++;
198  ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count);
199  ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count);
200  }
201  }
202 }
203 
204 static void accel_cb(uint8_t sender_id __attribute__((unused)),
205  uint32_t stamp __attribute__((unused)),
206  struct Int32Vect3 *accel)
207 {
208  accel_imu_meas = *accel;
209 }
210 
211 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
212  struct FloatQuat *q_b2i_f)
213 {
215 }
216 
217 void v_ctl_init(void)
218 {
219  /* mode */
221  v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; //There is only one, added here to be universal in flightplan for different control modes
222 
223  /* outer loop */
226  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
227 
228 #ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
229  v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
230 #else
232 #endif
233 
234  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
236  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
237 
239 
240  /* inner loops */
242 
243  /* "auto throttle" inner loop parameters */
244  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
245  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
246  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
247  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
248  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
249 
250  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
251  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
252  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
253 
254 
255 #ifdef V_CTL_ENERGY_TOT_PGAIN
256  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
257  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
258  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
259  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
260 #else
265 #warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
266 #endif
267 
268 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
270 #else
271  v_ctl_max_climb = 2;
272 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
273 #endif
274 
275 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
276  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
277  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
278  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
280 #endif
281 
283 
285 
286  AbiBindMsgIMU_ACCEL_INT32(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
287  AbiBindMsgBODY_TO_IMU_QUAT(V_CTL_ENERGY_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
288 }
289 
290 static const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
291 static const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
292 
299 {
300  // Airspeed Command Saturation
302 
303  // Altitude Controller
306 
307  // Vertical Speed Limiter
308  BoundAbs(sp, v_ctl_max_climb);
309 
310  // Vertical Acceleration Limiter
311  float incr = sp - v_ctl_climb_setpoint;
312  BoundAbs(incr, 2 * dt_navigation);
313  v_ctl_climb_setpoint += incr;
314 }
315 
316 
317 // Running Average Filter
318 float lp_vdot[5];
319 
320 static float low_pass_vdot(float v);
321 static float low_pass_vdot(float v)
322 {
323  lp_vdot[4] += (v - lp_vdot[4]) / 3;
324  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
325  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
326  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
327  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
328 
329  return lp_vdot[0];
330 }
331 
337 {
338  // Airspeed setpoint rate limiter:
339  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
341  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
342  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
343 
344 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
345 // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
347  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
351 
352  // Do not allow controlled airspeed below the setpoint
353  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
355  // reset integrator of ground speed loop
358  }
359 #else
361 #endif
362 
363  // Airspeed outerloop: positive means we need to accelerate
364  float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();
365 
366  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
367  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
369 
370  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
371 #ifndef SITL
372  /* convert last imu accel measurement to float */
373  struct FloatVect3 accel_imu_f;
374  ACCELS_FLOAT_OF_BFP(accel_imu_f, accel_imu_meas);
375  /* rotate from imu to body frame */
376  struct FloatVect3 accel_meas_body;
377  float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f);
378  float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
379 #else
380  float vdot = 0;
381 #endif
382 
383  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
384  float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);
385 
386  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
388 
389  // Total Energy Error: positive means energy should be added
390  float en_tot_err = gamma_err + vdot_err;
391 
392  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
393  float en_dis_err = gamma_err - vdot_err;
394 
395  // Auto Cruise Throttle
398  v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
399  + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
401  }
402 
403  // Total Controller
407  + v_ctl_energy_total_pgain * en_tot_err;
408 
409  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (autopilot_throttle_killed() == 1)) {
410  // If your energy supply is not sufficient, then neglect the climb requirement
411  en_dis_err = -vdot_err;
412 
413  // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
414  if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }
416  }
417 
418 
419  /* pitch pre-command */
422  + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
423  Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
424  }
425  float v_ctl_pitch_of_vz =
426  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
427  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
429  + v_ctl_energy_diff_pgain * en_dis_err
431  if (autopilot_throttle_killed()) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
432 
433  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
434  Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)
435 
436  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
437 
438  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
439 }
440 
441 
442 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
443 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
444 #endif
445 
446 #ifndef V_CTL_THROTTLE_SLEW
447 #define V_CTL_THROTTLE_SLEW 1.
448 #endif
449 
453 {
455  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
456  v_ctl_throttle_slewed += diff_throttle;
457 }
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:165
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
bool launch
request launch
Definition: autopilot.h:71
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:180
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:171
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:230
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:161
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:146
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:178
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:116
#define CONTROL_FREQUENCY
int ac_char_descend_count
Definition: energy_ctrl.c:175
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:452
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:336
float ac_char_descend_max
Definition: energy_ctrl.c:174
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
int ac_char_climb_count
Definition: energy_ctrl.c:172
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:447
float ac_char_cruise_throttle
Definition: energy_ctrl.c:176
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:113
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:321
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:140
#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_SPEED_THROTTLE
Definition: energy_ctrl.h:36
#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
static const float dt_navigation
Definition: energy_ctrl.c:291
#define STALL_AIRSPEED
Definition: energy_ctrl.c:151
unsigned long uint32_t
Definition: types.h:18
#define NAVIGATION_FREQUENCY
float ac_char_climb_pitch
Definition: energy_ctrl.c:170
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:185
uint8_t v_ctl_speed_mode
Definition: energy_ctrl.c:135
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:158
#define V_CTL_MODE_AUTO_CLIMB
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:90
#define V_CTL_GLIDE_RATIO
Definition: energy_ctrl.c:154
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:211
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:177
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:138
float lp_vdot[5]
Definition: energy_ctrl.c:318
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:217
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:122
static struct FloatQuat imu_to_body_quat
Definition: energy_ctrl.c:137
#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:173
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:204
static abi_event body_to_imu_ev
Definition: energy_ctrl.c:141
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
static const float dt_attidude
Definition: energy_ctrl.c:290
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:298