Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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"
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 
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 AIRSPEED_SETPOINT_SLEW
149 #define AIRSPEED_SETPOINT_SLEW 1
150 #endif
151 
153 // Automatically found airplane characteristics
154 
155 float ac_char_climb_pitch = 0.0f;
156 float ac_char_climb_max = 0.0f;
159 float ac_char_descend_max = 0.0f;
162 float ac_char_cruise_pitch = 0.0f;
164 
165 static void ac_char_average(float* last, float new, int count)
166 {
167  *last = (((*last) * (((float)count) - 1.0f)) + new) / ((float) count);
168 }
169 
170 static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
171 {
172  if ((accelerate > -0.02) && (accelerate < 0.02))
173  {
174  if (throttle >= 1.0f)
175  {
179  }
180  else if (throttle <= 0.0f)
181  {
185  }
186  else if ((climb > -0.125) && (climb < 0.125))
187  {
191  }
192  }
193 }
194 
195 
196 void v_ctl_init( void ) {
197  /* mode */
199 
200  /* outer loop */
203  v_ctl_altitude_pgain = V_CTL_ALTITUDE_PGAIN;
204 
205 #ifdef V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH
206  v_ctl_auto_throttle_nominal_cruise_pitch = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_PITCH;
207 #else
209 #endif
210 
211  v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
213  v_ctl_airspeed_pgain = V_CTL_AIRSPEED_PGAIN;
214 
215  /* inner loops */
217 
218  /* "auto throttle" inner loop parameters */
219  v_ctl_auto_throttle_nominal_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
220  v_ctl_auto_throttle_climb_throttle_increment = V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT;
221  v_ctl_auto_throttle_pitch_of_vz_pgain = V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN;
222  v_ctl_auto_throttle_of_airspeed_pgain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_PGAIN;
223  v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
224 
225  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
226  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
227  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
228 
229 
230 #ifdef V_CTL_ENERGY_TOT_PGAIN
231  v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
232  v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
233  v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
234  v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
235 #else
240 #warning "V_CTL_ENERGY_TOT GAINS are not defined and set to 0"
241 #endif
242 
243 #ifdef V_CTL_ALTITUDE_MAX_CLIMB
245 #else
246  v_ctl_max_climb = 2;
247 #warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
248 #endif
249 
250 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
251  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
252  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
253  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
255 #endif
256 
258 }
259 
260 const float dt_attidude = 1.0 / ((float)CONTROL_FREQUENCY);
261 const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY);
262 
269 {
270  // Imput Checks
272 
273  // Altitude Controller
276  BoundAbs(sp, v_ctl_max_climb);
277 
278  float incr = sp - v_ctl_climb_setpoint;
279  BoundAbs(incr, 2 * dt_navigation);
280  v_ctl_climb_setpoint += incr;
281 }
282 
283 
284 
285 float lp_vdot[5];
286 
287 static float low_pass_vdot(float v);
288 static float low_pass_vdot(float v)
289 {
290  lp_vdot[4] += (v - lp_vdot[4]) / 3;
291  lp_vdot[3] += (lp_vdot[4] - lp_vdot[3]) / 3;
292  lp_vdot[2] += (lp_vdot[3] - lp_vdot[2]) / 3;
293  lp_vdot[1] += (lp_vdot[2] - lp_vdot[1]) / 3;
294  lp_vdot[0] += (lp_vdot[1] - lp_vdot[0]) / 3;
295 
296  return lp_vdot[0];
297 }
298 
303 void v_ctl_climb_loop( void )
304 {
305  // airspeed_setpoint ratelimiter:
306  // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
308  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
309  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
310 
311 #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
312  // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
313  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f()));
314  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
317 
318  // Do not allow controlled airspeed below the setpoint
319  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
321  // reset integrator of ground speed loop
323  }
324 #else
326 #endif
327 
328  // Airspeed outerloop: positive means we need to accelerate
329  float speed_error = v_ctl_auto_airspeed_controlled - (*stateGetAirspeed_f());
330 
331  // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
332  v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
333  BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);
334 
335  // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
336 #ifndef SITL
337  struct Int32Vect3 accel_meas_body;
339  float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
340 #else
341  float vdot = 0;
342 #endif
343 
344  // Acceleration Error: positive means UAV needs to accelerate: needs extra energy
345  float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot );
346 
347  // Flight Path Outerloop: positive means needs to climb more: needs extra energy
349 
350  // Total Energy Error: positive means energy should be added
351  float en_tot_err = gamma_err + vdot_err;
352 
353  // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
354  float en_dis_err = gamma_err - vdot_err;
355 
356  // Auto Cruise Throttle
358  {
360  v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
361  + en_tot_err * v_ctl_energy_total_igain * dt_attidude;
363  }
364 
365  // Total Controller
369  + v_ctl_energy_total_pgain * en_tot_err;
370 
371  if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle==1))
372  {
373  // If your energy supply is not sufficient, then neglect the climb requirement
374  en_dis_err = -vdot_err;
375 
376  // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
379  }
380 
381 
382  /* pitch pre-command */
384  {
386  + v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
387  Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
388  }
389  float v_ctl_pitch_of_vz =
390  + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
391  - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
393  + v_ctl_energy_diff_pgain * en_dis_err
395 
396  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
397  Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
398 
399  ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
400 
401  v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
402 }
403 
404 
405 #ifdef V_CTL_THROTTLE_SLEW_LIMITER
406 #define V_CTL_THROTTLE_SLEW (1./CONTROL_FREQUENCY/(V_CTL_THROTTLE_SLEW_LIMITER))
407 #endif
408 
409 #ifndef V_CTL_THROTTLE_SLEW
410 #define V_CTL_THROTTLE_SLEW 1.
411 #endif
412 
416 void v_ctl_throttle_slew( void ) {
418  BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
419  v_ctl_throttle_slewed += diff_throttle;
420 }
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:40
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:156
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
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
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
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
int ac_char_cruise_count
Definition: energy_ctrl.c:163
float v_ctl_energy_total_igain
Definition: energy_ctrl.c:119
int ac_char_descend_count
Definition: energy_ctrl.c:160
void v_ctl_throttle_slew(void)
Computes slewed throttle from throttle setpoint called at 20Hz.
Definition: energy_ctrl.c:416
void v_ctl_climb_loop(void)
auto throttle inner loop
Definition: energy_ctrl.c:303
float ac_char_descend_max
Definition: energy_ctrl.c:159
float v_ctl_auto_throttle_sum_err
Definition: energy_ctrl.c:80
int ac_char_climb_count
Definition: energy_ctrl.c:157
Vertical control using total energy control for fixed wing vehicles.
#define V_CTL_THROTTLE_SLEW
Definition: energy_ctrl.c:410
float ac_char_cruise_throttle
Definition: energy_ctrl.c:161
float v_ctl_auto_pitch_of_airspeed_dgain
Definition: energy_ctrl.c:116
static float low_pass_vdot(float v)
Definition: energy_ctrl.c:288
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
int32_t z
Up.
#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:165
float v_ctl_max_climb
Definition: energy_ctrl.c:97
const float dt_navigation
Definition: energy_ctrl.c:261
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
#define STALL_AIRSPEED
Definition: energy_ctrl.c:146
float ac_char_climb_pitch
Definition: energy_ctrl.c:155
bool_t kill_throttle
Definition: autopilot.c:32
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:170
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:149
#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
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:162
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:285
float v_ctl_auto_throttle_nominal_cruise_pitch
Definition: energy_ctrl.c:108
void v_ctl_init(void)
Definition: energy_ctrl.c:196
float v_ctl_auto_airspeed_setpoint_slew
Definition: energy_ctrl.c:125
#define NAVIGATION_FREQUENCY
Definition: autopilot.h:135
#define CONTROL_FREQUENCY
Definition: autopilot.h:130
#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:158
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:260
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:268
Fixedwing autopilot modes.