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