Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_rc_setpoint.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2013 Felix Ruess <felix.ruess@gmail.com>
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 
27 #include "generated/airframe.h"
28 
30 #include "state.h"
33 #include "mcu_periph/sys_time.h"
34 
35 #ifndef STABILIZATION_ATTITUDE_DEADBAND_A
36 #define STABILIZATION_ATTITUDE_DEADBAND_A 0
37 #endif
38 
39 #ifndef STABILIZATION_ATTITUDE_DEADBAND_E
40 #define STABILIZATION_ATTITUDE_DEADBAND_E 0
41 #endif
42 
49 #ifndef COORDINATED_TURN_AIRSPEED
50 #define COORDINATED_TURN_AIRSPEED 12.0
51 #endif
52 
53 #define YAW_DEADBAND_EXCEEDED() \
54  (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
55  radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
56 
59 
60 static int32_t get_rc_roll(void)
61 {
62  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
64 #if STABILIZATION_ATTITUDE_DEADBAND_A
65  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
66  return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
67 #else
68  return roll * max_rc_phi / MAX_PPRZ;
69 #endif
70 }
71 
72 static int32_t get_rc_pitch(void)
73 {
74  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
76 #if STABILIZATION_ATTITUDE_DEADBAND_E
77  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
78  return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
79 #else
80  return pitch * max_rc_theta / MAX_PPRZ;
81 #endif
82 }
83 
84 static int32_t get_rc_yaw(void)
85 {
86  const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
88  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
89  return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
90 }
91 
92 static float get_rc_roll_f(void)
93 {
95 #if STABILIZATION_ATTITUDE_DEADBAND_A
96  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
97  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
98 #else
99  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ;
100 #endif
101 }
102 
103 static float get_rc_pitch_f(void)
104 {
106 #if STABILIZATION_ATTITUDE_DEADBAND_E
107  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
108  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
109 #else
110  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ;
111 #endif
112 }
113 
114 static inline float get_rc_yaw_f(void)
115 {
117  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
118  return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
119 }
120 
123 {
125 }
126 
127 /* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
128  However, when rolling more then 90 degrees in combination with pitch it switches. For a
129  transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
131 {
132  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
133 
135 
136  if (abs(att->phi) < INT32_ANGLE_PI_2) {
137  int32_t sin_theta;
138  PPRZ_ITRIG_SIN(sin_theta, att->theta);
139  heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
140  } else if (ANGLE_FLOAT_OF_BFP(att->theta) > 0) {
141  heading = att->psi - att->phi;
142  } else {
143  heading = att->psi + att->phi;
144  }
145 
146  return heading;
147 }
148 
150 {
151  struct FloatEulers *att = stateGetNedToBodyEulers_f();
152 
153  float heading;
154 
155  if (fabsf(att->phi) < M_PI / 2) {
156  heading = att->psi - sinf(att->theta) * att->phi;
157  } else if (att->theta > 0) {
158  heading = att->psi - att->phi;
159  } else {
160  heading = att->psi + att->phi;
161  }
162 
163  return heading;
164 }
165 
166 
173 void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
174  bool coordinated_turn)
175 {
176  /* last time this function was called, used to calculate yaw setpoint update */
177  static float last_ts = 0.f;
178 
179  sp->phi = get_rc_roll();
180  sp->theta = get_rc_pitch();
181 
182  if (in_flight) {
183  /* calculate dt for yaw integration */
184  float dt = get_sys_time_float() - last_ts;
185  /* make sure nothing drastically weird happens, bound dt to 0.5sec */
186  Bound(dt, 0, 0.5);
187 
188  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
190  sp->psi += get_rc_yaw() * dt;
192  }
193  if (coordinated_turn) {
194  //Coordinated turn
195  //feedforward estimate angular rotation omega = g*tan(phi)/v
196  int32_t omega;
197  const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
198  if (abs(sp->phi) < max_phi) {
200  } else { //max 60 degrees roll
201  omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
202  }
203 
204  sp->psi += omega * dt;
205  }
206 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
207  // Make sure the yaw setpoint does not differ too much from the real yaw
208  // to prevent a sudden switch at 180 deg
209  const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
210 
212 
213  int32_t delta_psi = sp->psi - heading;
214  INT32_ANGLE_NORMALIZE(delta_psi);
215  if (delta_psi > delta_limit) {
216  sp->psi = heading + delta_limit;
217  } else if (delta_psi < -delta_limit) {
218  sp->psi = heading - delta_limit;
219  }
221 #endif
222  //Care Free mode
223  if (in_carefree) {
224  //care_free_heading has been set to current psi when entering care free mode.
225  int32_t cos_psi;
226  int32_t sin_psi;
227  int32_t temp_theta;
228  int32_t care_free_delta_psi_i;
229 
230  care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
231 
232  INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
233 
234  PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
235  PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
236 
237  temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
239  sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
240 
241  sp->theta = temp_theta;
242  }
243  } else { /* if not flying, use current yaw as setpoint */
245  }
246 
247  /* update timestamp for dt calculation */
249 }
250 
251 
252 void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
253  bool coordinated_turn)
254 {
255  /* last time this function was called, used to calculate yaw setpoint update */
256  static float last_ts = 0.f;
257 
258  sp->phi = get_rc_roll_f();
259  sp->theta = get_rc_pitch_f();
260 
261  if (in_flight) {
262  /* calculate dt for yaw integration */
263  float dt = get_sys_time_float() - last_ts;
264  /* make sure nothing drastically weird happens, bound dt to 0.5sec */
265  Bound(dt, 0, 0.5);
266 
267  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
269  sp->psi += get_rc_yaw_f() * dt;
271  }
272  if (coordinated_turn) {
273  //Coordinated turn
274  //feedforward estimate angular rotation omega = g*tan(phi)/v
275  float omega;
276  const float max_phi = RadOfDeg(60.0);
277  if (fabsf(sp->phi) < max_phi) {
278  omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
279  } else { //max 60 degrees roll
280  omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
281  }
282 
283  sp->psi += omega * dt;
284  }
285 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
286  // Make sure the yaw setpoint does not differ too much from the real yaw
287  // to prevent a sudden switch at 180 deg
289 
290  float delta_psi = sp->psi - heading;
291  FLOAT_ANGLE_NORMALIZE(delta_psi);
292  if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
293  sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
294  } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
295  sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
296  }
298 #endif
299  //Care Free mode
300  if (in_carefree) {
301  //care_free_heading has been set to current psi when entering care free mode.
302  float cos_psi;
303  float sin_psi;
304  float temp_theta;
305 
306  float care_free_delta_psi_f = sp->psi - care_free_heading;
307 
308  FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
309 
310  sin_psi = sinf(care_free_delta_psi_f);
311  cos_psi = cosf(care_free_delta_psi_f);
312 
313  temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
314  sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
315 
316  sp->theta = temp_theta;
317  }
318  } else { /* if not flying, use current yaw as setpoint */
320  }
321 
322  /* update timestamp for dt calculation */
324 }
325 
326 
332 {
333  /* orientation vector describing simultaneous rotation of roll/pitch */
334  struct FloatVect3 ov;
335  ov.x = get_rc_roll_f();
336  ov.y = get_rc_pitch_f();
337  ov.z = 0.0;
338 
339  /* quaternion from that orientation vector */
341 }
342 
348 {
349  /* only non-zero entries for roll quaternion */
350  float roll2 = get_rc_roll_f() / 2.0f;
351  float qx_roll = sinf(roll2);
352  float qi_roll = cosf(roll2);
353 
354  //An offset is added if in forward mode
355  /* only non-zero entries for pitch quaternion */
356  float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + get_rc_pitch_f()) / 2.0f;
357  float qy_pitch = sinf(pitch2);
358  float qi_pitch = cosf(pitch2);
359 
360  /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */
361  q->qi = qi_roll * qi_pitch;
362  q->qx = qx_roll * qi_pitch;
363  q->qy = qi_roll * qy_pitch;
364  q->qz = qx_roll * qy_pitch;
365 }
366 
374 void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
375  bool coordinated_turn)
376 {
377 
378  // FIXME: remove me, do in quaternion directly
379  // is currently still needed, since the yaw setpoint integration is done in eulers
380 #if defined STABILIZATION_ATTITUDE_TYPE_INT
381  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
382 #else
383  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
384 #endif
385 
386  struct FloatQuat q_rp_cmd;
388 
389  /* get current heading */
390  const struct FloatVect3 zaxis = {0., 0., 1.};
391  struct FloatQuat q_yaw;
392 
393  //Care Free mode
394  if (in_carefree) {
395  //care_free_heading has been set to current psi when entering care free mode.
397  } else {
398  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
399  }
400 
401  /* roll/pitch commands applied to to current heading */
402  struct FloatQuat q_rp_sp;
403  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
404  float_quat_normalize(&q_rp_sp);
405 
406  if (in_flight) {
407  /* get current heading setpoint */
408  struct FloatQuat q_yaw_sp;
409 #if defined STABILIZATION_ATTITUDE_TYPE_INT
411 #else
412  float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
413 #endif
414 
415  /* rotation between current yaw and yaw setpoint */
416  struct FloatQuat q_yaw_diff;
417  float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
418 
419  /* compute final setpoint with yaw */
420  float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff);
421  } else {
422  QUAT_COPY(*q_sp, q_rp_sp);
423  }
424 }
425 
426 //Function that reads the rc setpoint in an earth bound frame
428  bool in_carefree, bool coordinated_turn)
429 {
430  // FIXME: remove me, do in quaternion directly
431  // is currently still needed, since the yaw setpoint integration is done in eulers
432 #if defined STABILIZATION_ATTITUDE_TYPE_INT
433  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
434 #else
435  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
436 #endif
437 
438  const struct FloatVect3 zaxis = {0., 0., 1.};
439 
440  struct FloatQuat q_rp_cmd;
442 
443  if (in_flight) {
444  /* get current heading setpoint */
445  struct FloatQuat q_yaw_sp;
446 
447 #if defined STABILIZATION_ATTITUDE_TYPE_INT
449 #else
450  float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
451 #endif
452 
453  float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd);
454  } else {
455  struct FloatQuat q_yaw;
456  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
457 
458  /* roll/pitch commands applied to to current heading */
459  struct FloatQuat q_rp_sp;
460  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
461  float_quat_normalize(&q_rp_sp);
462 
463  QUAT_COPY(*q_sp, q_rp_sp);
464  }
465 }
uint32_t last_ts
Definition: bluegiga.c:131
float phi
in radians
float theta
in radians
float psi
in radians
static void float_quat_normalize(struct FloatQuat *q)
#define FLOAT_ANGLE_NORMALIZE(_a)
void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle)
Quaternion from unit vector and angle.
void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions with normalization.
void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov)
Quaternion from orientation vector.
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
euler angles
Roation quaternion.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INT_MULT_RSHIFT(_a, _b, _r)
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_TRIG_FRAC
#define INT32_ANGLE_PI_2
#define INT32_ANGLE_FRAC
#define ANGLE_FLOAT_OF_BFP(_ai)
#define INT32_ANGLE_NORMALIZE(_a)
euler angles
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
#define MAX_PPRZ
Definition: paparazzi.h:8
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct RadioControl radio_control
Definition: radio_control.c:33
Generic interface for radio control modules.
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:67
Some helper functions to check RC sticks.
#define THROTTLE_STICK_DOWN()
General attitude stabilization interface for rotorcrafts.
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q)
Read roll/pitch command from RC as quaternion.
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as euler angles.
int32_t stabilization_attitude_get_heading_i(void)
void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q)
Read roll/pitch command from RC as quaternion.
static float get_rc_roll_f(void)
#define COORDINATED_TURN_AIRSPEED
Airspeed that will be used in the turning speed calculation (m/s).
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
static float get_rc_yaw_f(void)
#define YAW_DEADBAND_EXCEEDED()
static int32_t get_rc_pitch(void)
#define STABILIZATION_ATTITUDE_DEADBAND_A
static int32_t get_rc_roll(void)
#define STABILIZATION_ATTITUDE_DEADBAND_E
static float get_rc_pitch_f(void)
void stabilization_attitude_reset_care_free_heading(void)
reset the heading for care-free mode to current heading
int32_t transition_theta_offset
static int32_t get_rc_yaw(void)
float stabilization_attitude_get_heading_f(void)
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree, bool coordinated_turn)
Read an attitude setpoint from the RC.
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
float heading
Definition: wedgebug.c:258