Paparazzi UAS  v5.15_devel-46-gd23ed71
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_hybrid.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
3  * This is code for guidance of hybrid UAVs. It needs a simple velocity
4  * model to control the ground velocity of the UAV while estimating the
5  * wind velocity.
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, write to
21  * the Free Software Foundation, 59 Temple Place - Suite 330,
22  * Boston, MA 02111-1307, USA.
23  */
24 
39 
40 /* for guidance_v_thrust_coeff */
42 
43 // max airspeed for quadshot guidance
44 #define MAX_AIRSPEED 15
45 // high res frac for integration of angles
46 #define INT32_ANGLE_HIGH_RES_FRAC 18
47 
48 // Variables used for settings
50 float alt_pitch_gain = 0.3;
56 
57 // Private variables
62 static struct Int32Vect2 wind_estimate;
65 
71 static bool guidance_hovering;
74 
75 #if PERIODIC_TELEMETRY
77 
78 static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
79 {
80  struct NedCoor_i *pos = stateGetPositionNed_i();
81  struct NedCoor_i *speed = stateGetSpeedNed_i();
82  pprz_msg_send_HYBRID_GUIDANCE(trans, dev, AC_ID,
83  &(pos->x), &(pos->y),
84  &(speed->x), &(speed->y),
95 }
96 
97 #endif
98 
100 {
101 
105 
106  high_res_psi = 0;
107  guidance_hovering = true;
110  max_turn_bank = 40.0;
111  turn_bank_gain = 0.8;
112  wind_gain = 64;
117 
118 #if PERIODIC_TELEMETRY
120 #endif
121 
122 }
123 
124 #define INT32_ANGLE_HIGH_RES_NORMALIZE(_a) { \
125  while ((_a) > (INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) -= (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
126  while ((_a) < -(INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)); \
127  }
128 
130 {
135 }
136 
138 {
139  guidance_hybrid_ypr_sp.psi = sp_cmd->psi;
142 }
143 
146 {
147 
148  //notes:
149  //in forward flight, it is preferred to first get to min(airspeed_sp, airspeed_ref) and then change heading and then get to airspeed_sp
150  //in hover, just a gradual change is needed, or maybe not even needed
151  //changes between flight regimes should be handled
152 
153  //determine the heading of the airspeed_sp vector
154  int32_t omega = 0;
155  float airspeed_sp_heading = atan2f((float) POS_FLOAT_OF_BFP(guidance_hybrid_airspeed_sp.y),
157  //only for debugging
158  airspeed_sp_heading_disp = (int32_t)(DegOfRad(airspeed_sp_heading));
159 
160  //The difference of the current heading with the required heading.
161  float heading_diff = airspeed_sp_heading - ANGLE_FLOAT_OF_BFP(ypr_sp->psi);
162  FLOAT_ANGLE_NORMALIZE(heading_diff);
163 
164  //only for debugging
165  heading_diff_disp = (int32_t)(heading_diff / 3.14 * 180.0);
166 
167  //calculate the norm of the airspeed setpoint
168  int32_t norm_sp_airspeed;
169  norm_sp_airspeed = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
170 
171  //reference goes with a steady pace towards the setpoint airspeed
172  //hold ref norm below 4 m/s until heading is aligned
173  if (!((norm_sp_airspeed > (4 << 8)) && (guidance_hybrid_norm_ref_airspeed < (4 << 8))
174  && (guidance_hybrid_norm_ref_airspeed > ((4 << 8) - 10)) && (fabs(heading_diff) > (5.0 / 180.0 * 3.14)))) {
176  guidance_hybrid_norm_ref_airspeed) * 2 - 1) * 3 / 2;
177  }
178 
179  norm_sp_airspeed_disp = norm_sp_airspeed;
180 
181  int32_t psi = ypr_sp->psi;
182  int32_t s_psi, c_psi;
183  PPRZ_ITRIG_SIN(s_psi, psi);
184  PPRZ_ITRIG_COS(c_psi, psi);
185 
188 
189  if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
191  // translate speed_sp into bank angle and heading
192 
193  // change heading to direction of airspeed, faster if the airspeed is higher
194  if (heading_diff > 0.0) {
195  omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / 6;
196  } else if (heading_diff < 0.0) {
197  omega = (norm_sp_airspeed << (INT32_ANGLE_FRAC - INT32_POS_FRAC)) / -6;
198  }
199 
200  if (omega > ANGLE_BFP_OF_REAL(0.8)) { omega = ANGLE_BFP_OF_REAL(0.8); }
201  if (omega < ANGLE_BFP_OF_REAL(-0.8)) { omega = ANGLE_BFP_OF_REAL(-0.8); }
202 
203  // 2) calculate roll/pitch commands
204  struct Int32Vect2 hover_sp;
205  //if the setpoint is beyond 4m/s but the ref is not, the norm of the hover sp will stay at 4m/s
206  if (norm_sp_airspeed > (4 << 8)) {
207  hover_sp.x = (guidance_hybrid_airspeed_sp.x << 8) / norm_sp_airspeed * 4;
208  hover_sp.y = (guidance_hybrid_airspeed_sp.y << 8) / norm_sp_airspeed * 4;
209  } else {
210  hover_sp.x = guidance_hybrid_airspeed_sp.x;
211  hover_sp.y = guidance_hybrid_airspeed_sp.y;
212  }
213 
214  // gain of 10 means that for 4 m/s an angle of 40 degrees is needed
215  ypr_sp->theta = (((- (c_psi * hover_sp.x + s_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
216  ypr_sp->phi = ((((- s_psi * hover_sp.x + c_psi * hover_sp.y)) >> INT32_TRIG_FRAC) * 10 * INT32_ANGLE_PI / 180) >> 8;
217  } else {
219  // translate speed_sp into theta + thrust
220  // coordinated turns to change heading
221 
222  // calculate required pitch angle from airspeed_sp magnitude
223  if (guidance_hybrid_norm_ref_airspeed > (15 << 8)) {
224  ypr_sp->theta = -ANGLE_BFP_OF_REAL(RadOfDeg(78.0));
225  } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) {
226  ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (8 << 8)) * 2 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
227  RadOfDeg(68.0));
228  } else {
229  ypr_sp->theta = -(((guidance_hybrid_norm_ref_airspeed - (4 << 8)) * 7 * INT32_ANGLE_PI / 180) >> 8) - ANGLE_BFP_OF_REAL(
230  RadOfDeg(40.0));
231  }
232 
233  // if the sp_airspeed is within hovering range, don't start a coordinated turn
234  if (norm_sp_airspeed < (4 << 8)) {
235  omega = 0;
236  ypr_sp->phi = 0;
237  } else { // coordinated turn
238  ypr_sp->phi = ANGLE_BFP_OF_REAL(heading_diff * turn_bank_gain);
239  if (ypr_sp->phi > ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(max_turn_bank / 180.0 * M_PI); }
240  if (ypr_sp->phi < ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI)) { ypr_sp->phi = ANGLE_BFP_OF_REAL(-max_turn_bank / 180.0 * M_PI); }
241 
242  //feedforward estimate angular rotation omega = g*tan(phi)/v
244  ypr_sp->phi)));
245 
246  if (omega > ANGLE_BFP_OF_REAL(0.7)) { omega = ANGLE_BFP_OF_REAL(0.7); }
247  if (omega < ANGLE_BFP_OF_REAL(-0.7)) { omega = ANGLE_BFP_OF_REAL(-0.7); }
248  }
249  }
250 
251  //only for debugging purposes
252  omega_disp = omega;
253 
254  //go to higher resolution because else the increment is too small to be added
256 
258 
259  // go back to angle_frac
261  ypr_sp->theta = ypr_sp->theta + v_control_pitch;
262 }
263 
265 {
266  /* compute position error */
268 
269  // Compute ground speed setpoint
270  struct Int32Vect2 guidance_hybrid_groundspeed_sp;
271  VECT2_SDIV(guidance_hybrid_groundspeed_sp, guidance_h_pos_err, horizontal_speed_gain);
272 
273  int32_t norm_groundspeed_sp;
274  norm_groundspeed_sp = int32_vect2_norm(&guidance_hybrid_groundspeed_sp);
275 
276  //create setpoint groundspeed with a norm of 15 m/s
277  if (force_forward_flight) {
278  //scale the groundspeed_sp to 15 m/s if large enough to begin with
279  if (norm_groundspeed_sp > 1 << 8) {
280  guidance_hybrid_groundspeed_sp.x = guidance_hybrid_groundspeed_sp.x * (max_airspeed << 8) / norm_groundspeed_sp;
281  guidance_hybrid_groundspeed_sp.y = guidance_hybrid_groundspeed_sp.y * (max_airspeed << 8) / norm_groundspeed_sp;
282  } else { //groundspeed_sp is very small, so continue with the current heading
284  int32_t s_psi, c_psi;
285  PPRZ_ITRIG_SIN(s_psi, psi);
286  PPRZ_ITRIG_COS(c_psi, psi);
287  guidance_hybrid_groundspeed_sp.x = (15 * c_psi) >> (INT32_TRIG_FRAC - 8);
288  guidance_hybrid_groundspeed_sp.y = (15 * s_psi) >> (INT32_TRIG_FRAC - 8);
289  }
290  }
291 
292  struct Int32Vect2 airspeed_sp;
293  INT_VECT2_ZERO(airspeed_sp);
294  VECT2_ADD(airspeed_sp, guidance_hybrid_groundspeed_sp);
295  VECT2_ADD(airspeed_sp, wind_estimate);
296 
297  int32_t norm_airspeed_sp;
298  norm_airspeed_sp = int32_vect2_norm(&airspeed_sp);
299 
300  //Check if the airspeed_sp is larger than the max airspeed. If so, give the wind cancellatioin priority.
301  if (norm_airspeed_sp > (max_airspeed << 8) && norm_groundspeed_sp > 0) {
302  int32_t av = INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, guidance_hybrid_groundspeed_sp.x,
303  8) + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, guidance_hybrid_groundspeed_sp.y, 8);
304  int32_t bv = 2 * (INT_MULT_RSHIFT(wind_estimate.x, guidance_hybrid_groundspeed_sp.x,
305  8) + INT_MULT_RSHIFT(wind_estimate.y, guidance_hybrid_groundspeed_sp.y, 8));
307  8) - (max_airspeed << 8) * max_airspeed;
308 
309  float dv = POS_FLOAT_OF_BFP(bv) * POS_FLOAT_OF_BFP(bv) - 4.0 * POS_FLOAT_OF_BFP(av) * POS_FLOAT_OF_BFP(cv);
310  float d_sqrt_f = sqrtf(dv);
311  int32_t d_sqrt = POS_BFP_OF_REAL(d_sqrt_f);
312 
313  int32_t result = ((-bv + d_sqrt) << 8) / (2 * av);
314 
315  guidance_hybrid_airspeed_sp.x = wind_estimate.x + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.x, result, 8);
316  guidance_hybrid_airspeed_sp.y = wind_estimate.y + INT_MULT_RSHIFT(guidance_hybrid_groundspeed_sp.y, result, 8);
317  } else {
318  // Add the wind to get the airspeed setpoint
319  guidance_hybrid_airspeed_sp = guidance_hybrid_groundspeed_sp;
321  }
322 
323  // limit the airspeed setpoint to 15 m/s, because else saturation+windup will occur
324  norm_airspeed_sp = int32_vect2_norm(&guidance_hybrid_airspeed_sp);
325  // add check for non-zero airspeed (in case the max_airspeed is ever set to zero
326  if ((norm_airspeed_sp > (max_airspeed << 8)) && (norm_airspeed_sp != 0)) {
329  }
330 }
331 
333 {
334 
335  /* compute speed error */
336  struct Int32Vect2 wind_estimate_measured;
337  struct Int32Vect2 measured_ground_speed;
338  INT32_VECT2_RSHIFT(measured_ground_speed, *stateGetSpeedNed_i(), 11);
339  VECT2_DIFF(wind_estimate_measured, guidance_hybrid_ref_airspeed, measured_ground_speed);
340 
341  //Low pass wind_estimate, because we know the wind usually only changes slowly
342  //But not too slow, because the wind_estimate is also an adaptive element for the airspeed model inaccuracies
343  wind_estimate_high_res.x += (((wind_estimate_measured.x - wind_estimate.x) > 0) * 2 - 1) * wind_gain;
344  wind_estimate_high_res.y += (((wind_estimate_measured.y - wind_estimate.y) > 0) * 2 - 1) * wind_gain;
345 
348 }
349 
351 {
353 
354  /* orientation vector describing simultaneous rotation of roll/pitch */
355  struct FloatVect3 ov;
356  ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
357  ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
358  ov.z = 0.0;
359  /* quaternion from that orientation vector */
360  struct FloatQuat q_rp;
361  float_quat_of_orientation_vect(&q_rp, &ov);
362  struct Int32Quat q_rp_i;
363  QUAT_BFP_OF_REAL(q_rp_i, q_rp);
364 
365  // get the vertical vector to rotate the roll pitch setpoint around
366  struct Int32Vect3 zaxis = {0, 0, 1};
367 
368  /* get current heading setpoint */
369  struct Int32Quat q_yaw_sp;
370  int32_quat_of_axis_angle(&q_yaw_sp, &zaxis, sp_cmd->psi);
371 
372  // first apply the roll/pitch setpoint and then the yaw
373  int32_quat_comp(&stab_att_sp_quat, &q_yaw_sp, &q_rp_i);
374 
376 }
377 
379 {
380  if (guidance_hybrid_norm_ref_airspeed < (4 << 8)) {
381  //if airspeed ref < 4 only thrust
382  stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
383  v_control_pitch = 0;
384  guidance_v_kp = GUIDANCE_V_HOVER_KP;
385  guidance_v_kd = GUIDANCE_V_HOVER_KD;
386  guidance_v_ki = GUIDANCE_V_HOVER_KI;
387  } else if (guidance_hybrid_norm_ref_airspeed > (8 << 8)) { //if airspeed ref > 8 only pitch,
388  //at 15 m/s the thrust has to be 33%
389  stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ / 5 + (((guidance_hybrid_norm_ref_airspeed - (8 << 8)) / 7 *
390  (MAX_PPRZ / 3 - MAX_PPRZ / 5)) >> 8) + (guidance_v_delta_t - MAX_PPRZ / 2) / 10;
391  //stabilization_cmd[COMMAND_THRUST] = MAX_PPRZ/5;
392  // stabilization_cmd[COMMAND_THRUST] = ((guidance_hybrid_norm_ref_airspeed - (8<<8)) / 7 * (MAX_PPRZ/3 - MAX_PPRZ/5))>>8 + 9600/5;
393  //Control altitude with pitch, now only proportional control
394  float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
396  guidance_v_kp = GUIDANCE_V_HOVER_KP / 2;
397  guidance_v_kd = GUIDANCE_V_HOVER_KD / 2;
398  guidance_v_ki = GUIDANCE_V_HOVER_KI / 2;
399  } else { //if airspeed ref > 4 && < 8 both
400  int32_t airspeed_transition = (guidance_hybrid_norm_ref_airspeed - (4 << 8)) / 4; //divide by 4 to scale it to 0-1 (<<8)
401  stabilization_cmd[COMMAND_THRUST] = ((MAX_PPRZ / 5 + (guidance_v_delta_t - MAX_PPRZ / 2) / 10) * airspeed_transition +
402  guidance_v_delta_t * ((1 << 8) - airspeed_transition)) >> 8;
403  float alt_control_pitch = (guidance_v_delta_t - MAX_PPRZ * guidance_v_nominal_throttle) * alt_pitch_gain;
405  guidance_v_nominal_throttle)), airspeed_transition, 8);
406  guidance_v_kp = GUIDANCE_V_HOVER_KP;
407  guidance_v_kd = GUIDANCE_V_HOVER_KD;
408  guidance_v_ki = GUIDANCE_V_HOVER_KI;
409  }
410 }
int32_t psi
in rad with INT32_ANGLE_FRAC
#define FLOAT_ANGLE_NORMALIZE(_a)
#define MAX_AIRSPEED
static int32_t v_control_pitch
int32_t max_airspeed
void guidance_hybrid_vertical(void)
Description.
int32_t guidance_v_kd
vertical control D-gain
Definition: guidance_v.c:137
static struct Int32Vect2 wind_estimate_high_res
void guidance_hybrid_position_to_airspeed(void)
Description.
void guidance_hybrid_airspeed_to_attitude(struct Int32Eulers *ypr_sp)
Convert a required airspeed to a certain attitude for the Quadshot.
struct Int32Vect2 pos
horizontal position setpoint in NED.
Definition: guidance_h.h:74
void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c)
Composition (multiplication) of two quaternions.
#define INT32_ANGLE_HIGH_RES_NORMALIZE(_a)
void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle)
Quaternion from unit vector and angle.
Periodic telemetry system header (includes downlink utility and generated code).
float max_turn_bank
General attitude stabilization interface for rotorcrafts.
#define INT32_ANGLE_HIGH_RES_FRAC
int32_t guidance_v_delta_t
thrust command.
Definition: guidance_v.c:106
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
void guidance_hybrid_init(void)
Hybrid Guidance Initialization function.
int32_t guidance_v_kp
vertical control P-gain
Definition: guidance_v.c:136
#define POS_BFP_OF_REAL(_af)
static struct Int32Eulers guidance_hybrid_ypr_sp
int32_t wind_gain
static int32_t airspeed_sp_heading_disp
float guidance_v_nominal_throttle
nominal throttle for hover.
Definition: guidance_v.c:108
Vertical guidance for rotorcrafts.
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:92
static struct Int32Vect2 guidance_hybrid_ref_airspeed
static struct Int32Vect2 guidance_hybrid_airspeed_ref
static uint32_t int32_vect2_norm(struct Int32Vect2 *v)
return norm of 2D vector
Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.
void guidance_hybrid_run(void)
Runs the Hybrid Guidance main functions.
static void send_hybrid_guidance(struct transport_tx *trans, struct link_device *dev)
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:74
#define ANGLE_FLOAT_OF_BFP(_ai)
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
void guidance_hybrid_determine_wind_estimate(void)
Description.
Roation quaternion.
int32_t y
East.
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
static struct NedCoor_i * stateGetSpeedNed_i(void)
Get ground speed in local NED coordinates (int).
Definition: state.h:863
int32_t horizontal_speed_gain
float alt_pitch_gain
int32_t guidance_v_ki
vertical control I-gain
Definition: guidance_v.c:138
static int32_t heading_diff_disp
static int32_t omega_disp
euler angles
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int32_t x
North.
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
void guidance_hybrid_reset_heading(struct Int32Eulers *sp_cmd)
Description.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
float turn_bank_gain
#define INT32_POS_FRAC
static int32_t norm_sp_airspeed_disp
int32_t guidance_hybrid_norm_ref_airspeed
void guidance_hybrid_set_cmd_i(struct Int32Eulers *sp_cmd)
Creates the attitude set-points from an orientation vector.
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define INT32_TRIG_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define VECT2_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:104
static int32_t high_res_psi
int32_t phi
in rad with INT32_ANGLE_FRAC
#define INT_EULERS_ZERO(_e)
vector in North East Down coordinates
#define INT_MULT_RSHIFT(_a, _b, _r)
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
#define INT32_ANGLE_PI
static struct Int32Vect2 guidance_hybrid_airspeed_sp
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
#define INT_VECT2_ZERO(_v)
#define POS_FLOAT_OF_BFP(_ai)
#define MAX_PPRZ
Definition: paparazzi.h:8
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
static struct Int32Vect2 guidance_h_pos_err
Horizontal guidance for rotorcrafts.
#define INT32_VECT2_RSHIFT(_o, _i, _r)
bool force_forward_flight
Rotation quaternion.
struct HorizontalGuidanceSetpoint sp
setpoints
Definition: guidance_h.h:103
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
static bool guidance_hovering
#define PPRZ_ITRIG_COS(_c, _a)
static struct Int32Vect2 wind_estimate