Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/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 
137 static struct Int32Vect3 accel_imu_meas;
138 
140 
142 #ifndef V_CTL_ALTITUDE_MAX_CLIMB
143 #define V_CTL_ALTITUDE_MAX_CLIMB 2;
144 INFO("V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2 , indicating 2 m/s")
145 #endif
146 #ifndef STALL_AIRSPEED
147 INFO("No STALL_AIRSPEED defined. Using NOMINAL_AIRSPEED")
148 #define STALL_AIRSPEED NOMINAL_AIRSPEED
149 #endif
150 #ifndef V_CTL_GLIDE_RATIO
151 #define V_CTL_GLIDE_RATIO 8.
152 INFO("V_CTL_GLIDE_RATIO not defined - default is 8.")
153 #endif
154 #ifndef AIRSPEED_SETPOINT_SLEW
155 #define AIRSPEED_SETPOINT_SLEW 1
156 #endif
157 #ifndef V_CTL_MAX_ACCELERATION
158 #define V_CTL_MAX_ACCELERATION 0.5 //G
159 #endif
160 
161 #ifndef V_CTL_ENERGY_IMU_ID
162 #define V_CTL_ENERGY_IMU_ID ABI_BROADCAST
163 #endif
165 // Automatically found airplane characteristics
166 
167 float ac_char_climb_pitch = 0.0f;
168 float ac_char_climb_max = 0.0f;
171 float ac_char_descend_max = 0.0f;
174 float ac_char_cruise_pitch = 0.0f;
176 
177 static void ac_char_average(float *last_v, float new_v, int count)
178 {
179  *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count);
180 }
181 
182 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
183 {
184  if ((accelerate > -0.02) && (accelerate < 0.02)) {
185  if (throttle >= 1.0f) {
189  } else if (throttle <= 0.0f) {
193  } else if ((climb > -0.125) && (climb < 0.125)) {
197  }
198  }
199 }
200 
201 static void accel_cb(uint8_t sender_id __attribute__((unused)),
202  uint32_t stamp __attribute__((unused)),
203  struct Int32Vect3 *accel)
204 {
205  accel_imu_meas = *accel;
206 }
207 
208 void v_ctl_init(void)
209 {
210  /* mode */
212  v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; //There is only one, added here to be universal in flightplan for different control modes
213 
214  /* outer loop */
217  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
218 
219 #ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
220  v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
221 #else
223 #endif
224 
225  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
227  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
228 
230 
231  /* inner loops */
233 
234  /* "auto throttle" inner loop parameters */
235  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
236  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
237  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
238  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
239  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
240 
241  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
242  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
243  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
244 
245 
246 #ifdef V_CTL_ENERGY_TOT_PGAIN
247  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
248  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
249  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
250  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
251 #else
256 #warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
257 #endif
258 
259 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
261 #else
262  v_ctl_max_climb = 2;
263 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
264 #endif
265 
266 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
267  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
268  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
269  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
271 #endif
272 
274 
275  AbiBindMsgIMU_ACCEL(V_CTL_ENERGY_IMU_ID, &accel_ev, accel_cb);
276 }
277 
278 static const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
279 static const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
280 
287 {
288  // Airspeed Command Saturation
290 
291  // Altitude Controller
294 
295  // Vertical Speed Limiter
296  BoundAbs(sp, v_ctl_max_climb);
297 
298  // Vertical Acceleration Limiter
299  float incr = sp - v_ctl_climb_setpoint;
300  BoundAbs(incr, 2 * dt_navigation);
301  v_ctl_climb_setpoint += incr;
302 }
303 
304 
305 // Running Average Filter
306 float lp_vdot[5];
307 
308 static float low_pass_vdot(float v);
309 static float low_pass_vdot(float v)
310 {
311  lp_vdot[4] += (v - lp_vdot[4]) / 3;
312  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
313  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
314  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
315  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
316 
317  return lp_vdot[0];
318 }
319 
325 {
326  // Airspeed setpoint rate limiter:
327  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
329  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
330  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
331 
332 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
333 // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
335  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
339 
340  // Do not allow controlled airspeed below the setpoint
343  // reset integrator of ground speed loop
346  }
347 #else
349 #endif
350 
351  // Airspeed outerloop: positive means we need to accelerate
352  float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();
353 
354  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
355  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
357 
358  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
359 #ifndef SITL
360  /* convert last imu accel measurement to float */
361  struct FloatVect3 accel_meas_body;
362  ACCELS_FLOAT_OF_BFP(accel_meas_body, accel_imu_meas);
363  float vdot = accel_meas_body.x / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
364 #else
365  float vdot = 0;
366 #endif
367 
368  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
369  float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);
370 
371  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
373 
374  // Total Energy Error: positive means energy should be added
375  float en_tot_err = gamma_err + vdot_err;
376 
377  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
378  float en_dis_err = gamma_err - vdot_err;
379 
380  // Auto Cruise Throttle
384  + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
386  }
387 
388  // Total Controller
392  + v_ctl_energy_total_pgain * en_tot_err;
393 
394  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (autopilot_throttle_killed() == 1)) {
395  // If your energy supply is not sufficient, then neglect the climb requirement
396  en_dis_err = -vdot_err;
397 
398  // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
399  if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }
401  }
402 
403 
404  /* pitch pre-command */
407  + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
408  Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
409  }
410  float v_ctl_pitch_of_vz =
411  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
412  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
414  + v_ctl_energy_diff_pgain * en_dis_err
416  if (autopilot_throttle_killed()) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
417 
418  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
419  Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)
420 
422 
424 }
425 
426 
427 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
428 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
429 #endif
430 
431 #ifndef V_CTL_THROTTLE_SLEW
432 #define V_CTL_THROTTLE_SLEW 1.
433 #endif
438 {
440  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ));
441  v_ctl_throttle_slewed += diff_throttle;
442 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:313
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
float ac_char_climb_max
Definition: energy_ctrl.c:168
float v_ctl_energy_total_pgain
Definition: energy_ctrl.c:115
float v_ctl_auto_pitch_of_airspeed_igain
Definition: energy_ctrl.c:112
pprz_t v_ctl_throttle_setpoint
Definition: energy_ctrl.c:131
int ac_char_cruise_count
Definition: energy_ctrl.c:175
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
Definition: energy_ctrl.c:182
float v_ctl_desired_acceleration
Definition: energy_ctrl.c:101
float v_ctl_auto_groundspeed_sum_err
Definition: energy_ctrl.c:128
float v_ctl_auto_throttle_climb_throttle_increment
Definition: energy_ctrl.c:106
int ac_char_descend_count
Definition: energy_ctrl.c:172
void v_ctl_climb_loop(void)
Auto-throttle inner loop.
Definition: energy_ctrl.c:324
float ac_char_climb_pitch
Definition: energy_ctrl.c:167
float v_ctl_auto_airspeed_controlled
Definition: energy_ctrl.c:123
float v_ctl_auto_throttle_nominal_cruise_pitch
Definition: energy_ctrl.c:105
static abi_event accel_ev
Definition: energy_ctrl.c:139
float v_ctl_max_climb
Definition: energy_ctrl.c:94
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:75
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:116
static struct Int32Vect3 accel_imu_meas
Definition: energy_ctrl.c:137
uint8_t v_ctl_auto_throttle_submode
Definition: energy_ctrl.c:76
float v_ctl_auto_groundspeed_igain
Definition: energy_ctrl.c:127
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:113
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:90
float ac_char_cruise_throttle
Definition: energy_ctrl.c:173
float v_ctl_pitch_setpoint
Definition: energy_ctrl.c:133
float ac_char_descend_max
Definition: energy_ctrl.c:171
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:437
float v_ctl_energy_diff_pgain
Definition: energy_ctrl.c:118
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:77
float v_ctl_max_acceleration
Definition: energy_ctrl.c:95
float v_ctl_altitude_setpoint
in meters above MSL
Definition: energy_ctrl.c:88
float ac_char_cruise_pitch
Definition: energy_ctrl.c:174
pprz_t v_ctl_throttle_slewed
Definition: energy_ctrl.c:132
static const float dt_navigation
Definition: energy_ctrl.c:279
static const float dt_attidude
Definition: energy_ctrl.c:278
#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR
Definition: energy_ctrl.c:129
float v_ctl_auto_throttle_nominal_cruise_throttle
Definition: energy_ctrl.c:104
float v_ctl_auto_throttle_of_airspeed_igain
Definition: energy_ctrl.c:110
static void ac_char_average(float *last_v, float new_v, int count)
Definition: energy_ctrl.c:177
#define V_CTL_ALTITUDE_MAX_CLIMB
Definition: energy_ctrl.c:143
uint8_t v_ctl_speed_mode
Definition: energy_ctrl.c:135
#define STALL_AIRSPEED
Definition: energy_ctrl.c:148
#define V_CTL_ENERGY_IMU_ID
Definition: energy_ctrl.c:162
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: energy_ctrl.c:201
float lp_vdot[5]
Definition: energy_ctrl.c:306
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:122
float v_ctl_airspeed_pgain
Definition: energy_ctrl.c:91
float v_ctl_auto_throttle_pitch_of_vz_pgain
Definition: energy_ctrl.c:107
#define AIRSPEED_SETPOINT_SLEW
Definition: energy_ctrl.c:155
void v_ctl_altitude_loop(void)
outer loop
Definition: energy_ctrl.c:286
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:432
float v_ctl_auto_throttle_cruise_throttle
Definition: energy_ctrl.c:103
void v_ctl_init(void)
Definition: energy_ctrl.c:208
float v_ctl_altitude_pre_climb
Path Angle.
Definition: energy_ctrl.c:89
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:309
float v_ctl_altitude_error
in meters, (setpoint - alt) -> positive = too low
Definition: energy_ctrl.c:92
float v_ctl_auto_pitch_of_airspeed_pgain
Definition: energy_ctrl.c:111
float v_ctl_auto_groundspeed_pgain
Definition: energy_ctrl.c:126
float ac_char_descend_pitch
Definition: energy_ctrl.c:170
int ac_char_climb_count
Definition: energy_ctrl.c:169
float v_ctl_auto_groundspeed_setpoint
in meters per second
Definition: energy_ctrl.c:125
#define V_CTL_GLIDE_RATIO
Definition: energy_ctrl.c:151
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:98
float v_ctl_auto_throttle_of_airspeed_pgain
Definition: energy_ctrl.c:109
#define V_CTL_MAX_ACCELERATION
Definition: energy_ctrl.c:158
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
float v_ctl_energy_diff_igain
Definition: energy_ctrl.c:119
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_SPEED_THROTTLE
Definition: energy_ctrl.h:36
#define CONTROL_FREQUENCY
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
#define V_CTL_MODE_MANUAL
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
#define V_CTL_MODE_AUTO_CLIMB
float controlled_throttle
Definition: guidance_v_n.c:85
float nav_pitch
Definition: nav.c:310
Fixedwing Navigation library.
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
#define TRIM_UPPRZ(pprz)
Definition: paparazzi.h:14
int16_t pprz_t
Definition: paparazzi.h:6
#define MAX_PPRZ
Definition: paparazzi.h:8
#define TRIM_PPRZ(pprz)
Definition: paparazzi.h:11
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float z
in meters
API to get/set the generic vehicle states.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98