Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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 
44 #define YAW_DEADBAND_EXCEEDED() \
45  (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
46  radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
47 
49 
50 
51 static int32_t get_rc_roll(void)
52 {
53  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
55 #if STABILIZATION_ATTITUDE_DEADBAND_A
56  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
57  return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
58 #else
59  return roll * max_rc_phi / MAX_PPRZ;
60 #endif
61 }
62 
63 static int32_t get_rc_pitch(void)
64 {
65  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
67 #if STABILIZATION_ATTITUDE_DEADBAND_E
68  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
69  return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
70 #else
71  return pitch * max_rc_theta / MAX_PPRZ;
72 #endif
73 }
74 
75 static int32_t get_rc_yaw(void)
76 {
77  const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
79  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
80  return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
81 }
82 
83 static float get_rc_roll_f(void)
84 {
86 #if STABILIZATION_ATTITUDE_DEADBAND_A
87  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
88  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
89 #else
90  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ;
91 #endif
92 }
93 
94 static float get_rc_pitch_f(void)
95 {
97 #if STABILIZATION_ATTITUDE_DEADBAND_E
98  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
99  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
100 #else
101  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ;
102 #endif
103 }
104 
105 static inline float get_rc_yaw_f(void)
106 {
108  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
109  return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
110 }
111 
114 {
116 }
117 
118 /* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
119  However, when rolling more then 90 degrees in combination with pitch it switches. For a
120  transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
122 {
123  struct Int32Eulers *att = stateGetNedToBodyEulers_i();
124 
126 
127  if (abs(att->phi) < INT32_ANGLE_PI_2) {
128  int32_t sin_theta;
129  PPRZ_ITRIG_SIN(sin_theta, att->theta);
130  heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
131  } else if (ANGLE_FLOAT_OF_BFP(att->theta) > 0) {
132  heading = att->psi - att->phi;
133  } else {
134  heading = att->psi + att->phi;
135  }
136 
137  return heading;
138 }
139 
141 {
142  struct FloatEulers *att = stateGetNedToBodyEulers_f();
143 
144  float heading;
145 
146  if (fabsf(att->phi) < M_PI / 2) {
147  heading = att->psi - sinf(att->theta) * att->phi;
148  } else if (att->theta > 0) {
149  heading = att->psi - att->phi;
150  } else {
151  heading = att->psi + att->phi;
152  }
153 
154  return heading;
155 }
156 
157 
164 void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree,
165  bool_t coordinated_turn)
166 {
167  /* last time this function was called, used to calculate yaw setpoint update */
168  static float last_ts = 0.f;
169 
170  sp->phi = get_rc_roll();
171  sp->theta = get_rc_pitch();
172 
173  if (in_flight) {
174  /* calculate dt for yaw integration */
175  float dt = get_sys_time_float() - last_ts;
176  /* make sure nothing drastically weird happens, bound dt to 0.5sec */
177  Bound(dt, 0, 0.5);
178 
179  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
181  sp->psi += get_rc_yaw() * dt;
183  }
184  if (coordinated_turn) {
185  //Coordinated turn
186  //feedforward estimate angular rotation omega = g*tan(phi)/v
187  //Take v = 9.81/1.3 m/s
188  int32_t omega;
189  const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
190  if (abs(sp->phi) < max_phi) {
191  omega = ANGLE_BFP_OF_REAL(1.3 * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
192  } else { //max 60 degrees roll, then take constant omega
193  omega = ANGLE_BFP_OF_REAL(1.3 * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
194  }
195 
196  sp->psi += omega * dt;
197  }
198 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
199  // Make sure the yaw setpoint does not differ too much from the real yaw
200  // to prevent a sudden switch at 180 deg
201  const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
202 
204 
205  int32_t delta_psi = sp->psi - heading;
206  INT32_ANGLE_NORMALIZE(delta_psi);
207  if (delta_psi > delta_limit) {
208  sp->psi = heading + delta_limit;
209  } else if (delta_psi < -delta_limit) {
210  sp->psi = heading - delta_limit;
211  }
213 #endif
214  //Care Free mode
215  if (in_carefree) {
216  //care_free_heading has been set to current psi when entering care free mode.
217  int32_t cos_psi;
218  int32_t sin_psi;
219  int32_t temp_theta;
220  int32_t care_free_delta_psi_i;
221 
222  care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
223 
224  INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
225 
226  PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
227  PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
228 
229  temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
231  sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
232 
233  sp->theta = temp_theta;
234  }
235  } else { /* if not flying, use current yaw as setpoint */
237  }
238 
239  /* update timestamp for dt calculation */
240  last_ts = get_sys_time_float();
241 }
242 
243 
244 void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree,
245  bool_t coordinated_turn)
246 {
247  /* last time this function was called, used to calculate yaw setpoint update */
248  static float last_ts = 0.f;
249 
250  sp->phi = get_rc_roll_f();
251  sp->theta = get_rc_pitch_f();
252 
253  if (in_flight) {
254  /* calculate dt for yaw integration */
255  float dt = get_sys_time_float() - last_ts;
256  /* make sure nothing drastically weird happens, bound dt to 0.5sec */
257  Bound(dt, 0, 0.5);
258 
259  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
261  sp->psi += get_rc_yaw_f() * dt;
263  }
264  if (coordinated_turn) {
265  //Coordinated turn
266  //feedforward estimate angular rotation omega = g*tan(phi)/v
267  //Take v = 9.81/1.3 m/s
268  float omega;
269  const float max_phi = RadOfDeg(85.0);
270  if (fabsf(sp->phi) < max_phi) {
271  omega = 1.3 * tanf(sp->phi);
272  } else { //max 60 degrees roll, then take constant omega
273  omega = 1.3 * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
274  }
275 
276  sp->psi += omega * dt;
277  }
278 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
279  // Make sure the yaw setpoint does not differ too much from the real yaw
280  // to prevent a sudden switch at 180 deg
282 
283  float delta_psi = sp->psi - heading;
284  FLOAT_ANGLE_NORMALIZE(delta_psi);
285  if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
286  sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
287  } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
288  sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
289  }
291 #endif
292  //Care Free mode
293  if (in_carefree) {
294  //care_free_heading has been set to current psi when entering care free mode.
295  float cos_psi;
296  float sin_psi;
297  float temp_theta;
298 
299  float care_free_delta_psi_f = sp->psi - care_free_heading;
300 
301  FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
302 
303  sin_psi = sinf(care_free_delta_psi_f);
304  cos_psi = cosf(care_free_delta_psi_f);
305 
306  temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
307  sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
308 
309  sp->theta = temp_theta;
310  }
311  } else { /* if not flying, use current yaw as setpoint */
313  }
314 
315  /* update timestamp for dt calculation */
316  last_ts = get_sys_time_float();
317 }
318 
319 
325 {
326  /* orientation vector describing simultaneous rotation of roll/pitch */
327  struct FloatVect3 ov;
328  ov.x = get_rc_roll_f();
329  ov.y = get_rc_pitch_f();
330  ov.z = 0.0;
331 
332  /* quaternion from that orientation vector */
334 }
335 
341 {
342  /* only non-zero entries for roll quaternion */
343  float roll2 = get_rc_roll_f() / 2.0f;
344  float qx_roll = sinf(roll2);
345  float qi_roll = cosf(roll2);
346 
347  //An offset is added if in forward mode
348  /* only non-zero entries for pitch quaternion */
349  float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + get_rc_pitch_f()) / 2.0f;
350  float qy_pitch = sinf(pitch2);
351  float qi_pitch = cosf(pitch2);
352 
353  /* only multiply non-zero entries of float_quat_comp(q, &q_roll, &q_pitch) */
354  q->qi = qi_roll * qi_pitch;
355  q->qx = qx_roll * qi_pitch;
356  q->qy = qi_roll * qy_pitch;
357  q->qz = qx_roll * qy_pitch;
358 }
359 
367 void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree,
368  bool_t coordinated_turn)
369 {
370 
371  // FIXME: remove me, do in quaternion directly
372  // is currently still needed, since the yaw setpoint integration is done in eulers
373 #if defined STABILIZATION_ATTITUDE_TYPE_INT
374  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
375 #else
376  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
377 #endif
378 
379  struct FloatQuat q_rp_cmd;
381 
382  /* get current heading */
383  const struct FloatVect3 zaxis = {0., 0., 1.};
384  struct FloatQuat q_yaw;
385 
386  //Care Free mode
387  if (in_carefree) {
388  //care_free_heading has been set to current psi when entering care free mode.
390  } else {
391  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
392  }
393 
394  /* roll/pitch commands applied to to current heading */
395  struct FloatQuat q_rp_sp;
396  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
397  float_quat_normalize(&q_rp_sp);
398 
399  if (in_flight) {
400  /* get current heading setpoint */
401  struct FloatQuat q_yaw_sp;
402 #if defined STABILIZATION_ATTITUDE_TYPE_INT
404 #else
405  float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
406 #endif
407 
408  /* rotation between current yaw and yaw setpoint */
409  struct FloatQuat q_yaw_diff;
410  float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);
411 
412  /* compute final setpoint with yaw */
413  float_quat_comp_norm_shortest(q_sp, &q_rp_sp, &q_yaw_diff);
414  } else {
415  QUAT_COPY(*q_sp, q_rp_sp);
416  }
417 }
418 
419 //Function that reads the rc setpoint in an earth bound frame
421  bool_t in_carefree, bool_t coordinated_turn)
422 {
423  // FIXME: remove me, do in quaternion directly
424  // is currently still needed, since the yaw setpoint integration is done in eulers
425 #if defined STABILIZATION_ATTITUDE_TYPE_INT
426  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
427 #else
428  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
429 #endif
430 
431  const struct FloatVect3 zaxis = {0., 0., 1.};
432 
433  struct FloatQuat q_rp_cmd;
435 
436  if (in_flight) {
437  /* get current heading setpoint */
438  struct FloatQuat q_yaw_sp;
439 
440 #if defined STABILIZATION_ATTITUDE_TYPE_INT
442 #else
443  float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi);
444 #endif
445 
446  float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd);
447  } else {
448  struct FloatQuat q_yaw;
449  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);
450 
451  /* roll/pitch commands applied to to current heading */
452  struct FloatQuat q_rp_sp;
453  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
454  float_quat_normalize(&q_rp_sp);
455 
456  QUAT_COPY(*q_sp, q_rp_sp);
457  }
458 }
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()
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:1114
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.
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
float psi
in radians
static int32_t get_rc_pitch(void)
#define INT32_ANGLE_PI_2
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as euler angles.
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.
#define THROTTLE_STICK_DOWN()
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:124
#define ANGLE_FLOAT_OF_BFP(_ai)
euler angles
Roation quaternion.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:532
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
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
#define STABILIZATION_ATTITUDE_DEADBAND_E
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
Some helper functions to check RC sticks.
#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.
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
Read attitude setpoint from RC as quaternion Interprets the stick positions as axes.
#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.
int32_t transition_theta_offset
Definition: guidance_h.c:82
Horizontal guidance for rotorcrafts.
static float get_rc_roll_f(void)
#define MAX_PPRZ
Definition: paparazzi.h:8
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:1096
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn)
#define PPRZ_ITRIG_COS(_c, _a)