Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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 
35 #ifndef RC_UPDATE_FREQ
36 #define RC_UPDATE_FREQ 40
37 #endif
38 
39 #ifndef STABILIZATION_ATTITUDE_DEADBAND_A
40 #define STABILIZATION_ATTITUDE_DEADBAND_A 0
41 #endif
42 
43 #ifndef STABILIZATION_ATTITUDE_DEADBAND_E
44 #define STABILIZATION_ATTITUDE_DEADBAND_E 0
45 #endif
46 
47 #define YAW_DEADBAND_EXCEEDED() \
48  (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
49  radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
50 
52 
53 
54 static int32_t get_rc_roll(void) {
55  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
57 #if STABILIZATION_ATTITUDE_DEADBAND_A
58  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
59  return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
60 #else
61  return roll * max_rc_phi / MAX_PPRZ;
62 #endif
63 }
64 
65 static int32_t get_rc_pitch(void) {
66  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
68 #if STABILIZATION_ATTITUDE_DEADBAND_E
69  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
70  return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
71 #else
72  return pitch * max_rc_theta / MAX_PPRZ;
73 #endif
74 }
75 
76 static int32_t get_rc_yaw(void) {
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) {
85 #if STABILIZATION_ATTITUDE_DEADBAND_A
86  DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A);
87  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A);
88 #else
89  return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ;
90 #endif
91 }
92 
93 static float get_rc_pitch_f(void) {
95 #if STABILIZATION_ATTITUDE_DEADBAND_E
96  DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E);
97  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E);
98 #else
99  return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ;
100 #endif
101 }
102 
103 static inline float get_rc_yaw_f(void) {
105  DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
106  return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
107 }
108 
112 }
113 
114 /* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
115  However, when rolling more then 90 degrees in combination with pitch it switches. For a
116  transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
118  struct Int32Eulers* att = stateGetNedToBodyEulers_i();
119 
121 
122  if(abs(att->phi) < INT32_ANGLE_PI_2) {
123  int32_t sin_theta;
124  PPRZ_ITRIG_SIN(sin_theta, att->theta);
125  heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
126  }
127  else if(ANGLE_FLOAT_OF_BFP(att->theta) > 0)
128  heading = att->psi - att->phi;
129  else
130  heading = att->psi + att->phi;
131 
132  return heading;
133 }
134 
136  struct FloatEulers* att = stateGetNedToBodyEulers_f();
137 
138  float heading;
139 
140  if(abs(att->phi) < M_PI/2) {
141  heading = att->psi - sinf(att->theta)*att->phi;
142  }
143  else if(att->theta > 0)
144  heading = att->psi - att->phi;
145  else
146  heading = att->psi + att->phi;
147 
148  return heading;
149 }
150 
151 
158 void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
159  sp->phi = get_rc_roll();
160  sp->theta = get_rc_pitch();
161 
162  if (in_flight) {
163  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
165  sp->psi += get_rc_yaw() / RC_UPDATE_FREQ;
167  }
168  if (coordinated_turn) {
169  //Coordinated turn
170  //feedforward estimate angular rotation omega = g*tan(phi)/v
171  //Take v = 9.81/1.3 m/s
172  int32_t omega;
173  const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
174  if (abs(sp->phi) < max_phi)
175  omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
176  else //max 60 degrees roll, then take constant omega
177  omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
178 
179  sp->psi += omega/RC_UPDATE_FREQ;
180  }
181 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
182  // Make sure the yaw setpoint does not differ too much from the real yaw
183  // to prevent a sudden switch at 180 deg
184  const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
185 
187 
188  int32_t delta_psi = sp->psi - heading;
189  INT32_ANGLE_NORMALIZE(delta_psi);
190  if (delta_psi > delta_limit){
191  sp->psi = heading + delta_limit;
192  }
193  else if (delta_psi < -delta_limit){
194  sp->psi = heading - delta_limit;
195  }
197 #endif
198  //Care Free mode
199  if (in_carefree) {
200  //care_free_heading has been set to current psi when entering care free mode.
201  int32_t cos_psi;
202  int32_t sin_psi;
203  int32_t temp_theta;
204  int32_t care_free_delta_psi_i;
205 
206  care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
207 
208  INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
209 
210  PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
211  PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
212 
213  temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
214  sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
215 
216  sp->theta = temp_theta;
217  }
218  }
219  else { /* if not flying, use current yaw as setpoint */
221  }
222 }
223 
224 
225 void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
226  sp->phi = get_rc_roll_f();
227  sp->theta = get_rc_pitch_f();
228 
229  if (in_flight) {
230  /* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
232  sp->psi += get_rc_yaw_f() / RC_UPDATE_FREQ;
234  }
235  if (coordinated_turn) {
236  //Coordinated turn
237  //feedforward estimate angular rotation omega = g*tan(phi)/v
238  //Take v = 9.81/1.3 m/s
239  float omega;
240  const float max_phi = RadOfDeg(85.0);
241  if(abs(sp->phi) < max_phi)
242  omega = 1.3*tanf(sp->phi);
243  else //max 60 degrees roll, then take constant omega
244  omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0));
245 
246  sp->psi += omega/RC_UPDATE_FREQ;
247  }
248 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
249  // Make sure the yaw setpoint does not differ too much from the real yaw
250  // to prevent a sudden switch at 180 deg
252 
253  float delta_psi = sp->psi - heading;
254  FLOAT_ANGLE_NORMALIZE(delta_psi);
255  if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
256  sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
257  }
258  else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
259  sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
260  }
262 #endif
263  //Care Free mode
264  if (in_carefree) {
265  //care_free_heading has been set to current psi when entering care free mode.
266  float cos_psi;
267  float sin_psi;
268  float temp_theta;
269 
270  float care_free_delta_psi_f = sp->psi - care_free_heading;
271 
272  FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
273 
274  sin_psi = sinf(care_free_delta_psi_f);
275  cos_psi = cosf(care_free_delta_psi_f);
276 
277  temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
278  sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;
279 
280  sp->theta = temp_theta;
281  }
282  }
283  else { /* if not flying, use current yaw as setpoint */
285  }
286 }
287 
288 
294  /* orientation vector describing simultaneous rotation of roll/pitch */
295  struct FloatVect3 ov;
296  ov.x = get_rc_roll_f();
297  ov.y = get_rc_pitch_f();
298  ov.z = 0.0;
299 
300  /* quaternion from that orientation vector */
302 }
303 
309  /* only non-zero entries for roll quaternion */
310  float roll2 = get_rc_roll_f() / 2.0f;
311  float qx_roll = sinf(roll2);
312  float qi_roll = cosf(roll2);
313 
314  //An offset is added if in forward mode
315  /* only non-zero entries for pitch quaternion */
316  float pitch2 = ANGLE_FLOAT_OF_BFP(transition_theta_offset) + get_rc_pitch_f() / 2.0f;
317  float qy_pitch = sinf(pitch2);
318  float qi_pitch = cosf(pitch2);
319 
320  /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */
321  q->qi = qi_roll * qi_pitch;
322  q->qx = qx_roll * qi_pitch;
323  q->qy = qi_roll * qy_pitch;
324  q->qz = qx_roll * qy_pitch;
325 }
326 
334 void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
335 
336  // FIXME: remove me, do in quaternion directly
337  // is currently still needed, since the yaw setpoint integration is done in eulers
338 #if defined STABILIZATION_ATTITUDE_TYPE_INT
339  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
340 #else
341  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
342 #endif
343 
344  struct FloatQuat q_rp_cmd;
346 
347  /* get current heading */
348  const struct FloatVect3 zaxis = {0., 0., 1.};
349  struct FloatQuat q_yaw;
350 
351  //Care Free mode
352  if (in_carefree) {
353  //care_free_heading has been set to current psi when entering care free mode.
355  }
356  else {
358  }
359 
360  /* roll/pitch commands applied to to current heading */
361  struct FloatQuat q_rp_sp;
362  FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd);
363  FLOAT_QUAT_NORMALIZE(q_rp_sp);
364 
365  if (in_flight)
366  {
367  /* get current heading setpoint */
368  struct FloatQuat q_yaw_sp;
369 #if defined STABILIZATION_ATTITUDE_TYPE_INT
371 #else
373 #endif
374 
375  /* rotation between current yaw and yaw setpoint */
376  struct FloatQuat q_yaw_diff;
377  FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
378 
379  /* compute final setpoint with yaw */
380  FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff);
381  } else {
382  QUAT_COPY(*q_sp, q_rp_sp);
383  }
384 }
385 
386 //Function that reads the rc setpoint in an earth bound frame
387 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) {
388  // FIXME: remove me, do in quaternion directly
389  // is currently still needed, since the yaw setpoint integration is done in eulers
390  #if defined STABILIZATION_ATTITUDE_TYPE_INT
391  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
392  #else
393  stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
394  #endif
395 
396  const struct FloatVect3 zaxis = {0., 0., 1.};
397 
398  struct FloatQuat q_rp_cmd;
400 
401  if (in_flight) {
402  /* get current heading setpoint */
403  struct FloatQuat q_yaw_sp;
404 
405  #if defined STABILIZATION_ATTITUDE_TYPE_INT
407  #else
409  #endif
410 
411  FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd);
412  }
413  else {
414  struct FloatQuat q_yaw;
416 
417  /* roll/pitch commands applied to to current heading */
418  struct FloatQuat q_rp_sp;
419  FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd);
420  FLOAT_QUAT_NORMALIZE(q_rp_sp);
421 
422  QUAT_COPY(*q_sp, q_rp_sp);
423  }
424 }
int32_t phi
in rad with INT32_ANGLE_FRAC
#define YAW_DEADBAND_EXCEEDED()
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
#define ANGLE_FLOAT_OF_BFP(_ai)
#define INT_MULT_RSHIFT(_a, _b, _r)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
static float get_rc_yaw_f(void)
#define ANGLE_BFP_OF_REAL(_af)
#define STABILIZATION_ATTITUDE_DEADBAND_A
General attitude stabilization interface for rotorcrafts.
static float get_rc_pitch_f(void)
float psi
in radians
static int32_t get_rc_roll(void)
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
int32_t stabilization_attitude_get_heading_i(void)
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
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)
static int32_t get_rc_pitch(void)
int32_t theta
in rad with INT32_ANGLE_FRAC
float theta
in radians
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
#define INT32_TRIG_FRAC
euler angles
void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q)
Read roll/pitch command from RC as quaternion.
#define THROTTLE_STICK_DOWN()
#define RADIO_PITCH
Definition: spektrum_arch.h:45
Roation quaternion.
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov)
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
float heading
Definition: ahrs_infrared.c:40
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:55
#define INT32_ANGLE_FRAC
#define INT32_ANGLE_PI_2
float phi
in radians
#define FLOAT_ANGLE_NORMALIZE(_a)
struct RadioControl radio_control
Definition: radio_control.c:25
#define INT32_ANGLE_NORMALIZE(_a)
#define STABILIZATION_ATTITUDE_DEADBAND_E
#define RADIO_YAW
Definition: spektrum_arch.h:48
signed long int32_t
Definition: types.h:19
Some helper functions to check RC sticks.
#define FLOAT_QUAT_NORMALIZE(_q)
void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q)
Read roll/pitch command from RC as quaternion.
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
API to get/set the generic vehicle states.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:512
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.
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t transition_theta_offset
Definition: guidance_h.c:95
Horizontal guidance for rotorcrafts.
static float get_rc_roll_f(void)
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define MAX_PPRZ
Definition: paparazzi.h:8
#define RADIO_ROLL
Definition: spektrum_arch.h:42
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:1012
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)
Definition: pprz_trig_int.h:50
euler angles