Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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 
29 #include "state.h"
33 
34 #ifndef RC_UPDATE_FREQ
35 #define RC_UPDATE_FREQ 40
36 #endif
37 
39 
43 }
44 
45 /* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch.
46  However, when rolling more then 90 degrees in combination with pitch it switches. For a
47  transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */
49  struct Int32Eulers* att = stateGetNedToBodyEulers_i();
50 
52 
53  if(abs(att->phi) < INT32_ANGLE_PI_2) {
54  int32_t sin_theta;
55  PPRZ_ITRIG_SIN(sin_theta, att->theta);
56  heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC);
57  }
58  else if(ANGLE_FLOAT_OF_BFP(att->theta) > 0)
59  heading = att->psi - att->phi;
60  else
61  heading = att->psi + att->phi;
62 
63  return heading;
64 }
65 
67  struct FloatEulers* att = stateGetNedToBodyEulers_f();
68 
69  float heading;
70 
71  if(abs(att->phi) < M_PI/2) {
72  heading = att->psi - sinf(att->theta)*att->phi;
73  }
74  else if(att->theta > 0)
75  heading = att->psi - att->phi;
76  else
77  heading = att->psi + att->phi;
78 
79  return heading;
80 }
81 
86 void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
87  const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
88  const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
89  const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
90 
91  sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
92  sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
93 
94  if (in_flight) {
95  if (YAW_DEADBAND_EXCEEDED()) {
96  sp->psi += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
98  }
100  //Coordinated turn
101  //feedforward estimate angular rotation omega = g*tan(phi)/v
102  //Take v = 9.81/1.3 m/s
103  int32_t omega;
104  const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
105  if(abs(sp->phi) < max_phi)
106  omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
107  else //max 60 degrees roll, then take constant omega
108  omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)));
109 
110  sp->psi += omega/RC_UPDATE_FREQ;
111  }
112 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
113  // Make sure the yaw setpoint does not differ too much from the real yaw
114  // to prevent a sudden switch at 180 deg
115  const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
116 
118 
119  int32_t delta_psi = sp->psi - heading;
120  INT32_ANGLE_NORMALIZE(delta_psi);
121  if (delta_psi > delta_limit){
122  sp->psi = heading + delta_limit;
123  }
124  else if (delta_psi < -delta_limit){
125  sp->psi = heading - delta_limit;
126  }
128 #endif
129  //Care Free mode
131  //care_free_heading has been set to current psi when entering care free mode.
132  int32_t cos_psi;
133  int32_t sin_psi;
134  int32_t temp_theta;
135  int32_t care_free_delta_psi_i;
136 
137  care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
138 
139  INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
140 
141  PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
142  PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
143 
144  temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
145  sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
146 
147  sp->theta = temp_theta;
148  }
149  }
150  else { /* if not flying, use current yaw as setpoint */
152  }
153 }
154 
155 
157  sp->phi = (radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
158  sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ);
159 
160  if (in_flight) {
161  if (YAW_DEADBAND_EXCEEDED()) {
162  sp->psi += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
164  }
166  //Coordinated turn
167  //feedforward estimate angular rotation omega = g*tan(phi)/v
168  //Take v = 9.81/1.3 m/s
169  float omega;
170  const float max_phi = RadOfDeg(85.0);
171  if(abs(sp->phi) < max_phi)
172  omega = 1.3*tanf(sp->phi);
173  else //max 60 degrees roll, then take constant omega
174  omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0));
175 
176  sp->psi += omega/RC_UPDATE_FREQ;
177  }
178 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
179  // Make sure the yaw setpoint does not differ too much from the real yaw
180  // to prevent a sudden switch at 180 deg
182 
183  float delta_psi = sp->psi - heading;
184  FLOAT_ANGLE_NORMALIZE(delta_psi);
185  if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
186  sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
187  }
188  else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
189  sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
190  }
192 #endif
193  //Care Free mode
195  //care_free_heading has been set to current psi when entering care free mode.
196  float cos_psi;
197  float sin_psi;
198  float temp_theta;
199 
200  float care_free_delta_psi_f = sp->psi - care_free_heading;
201 
202  FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
203 
204  sin_psi = sinf(care_free_delta_psi_f);
205  cos_psi = cosf(care_free_delta_psi_f);
206 
207  temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
208  sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;
209 
210  sp->theta = temp_theta;
211  }
212  }
213  else { /* if not flying, use current yaw as setpoint */
215  }
216 }
217 
218 
224  q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2;
225  q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2;
226  q->qz = 0.0;
227 
228  /* normalize */
229  float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy));
230  q->qi = 1.0 / norm;
231  q->qx /= norm;
232  q->qy /= norm;
233 }
234 
240  /* only non-zero entries for roll quaternion */
241  float roll2 = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2;
242  float qx_roll = sinf(roll2);
243  float qi_roll = cosf(roll2);
244 
245  //An offset is added if in forward mode
246  /* only non-zero entries for pitch quaternion */
247  float pitch2 = (ANGLE_FLOAT_OF_BFP(transition_theta_offset) + radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ) / 2;
248  float qy_pitch = sinf(pitch2);
249  float qi_pitch = cosf(pitch2);
250 
251  /* only multiply non-zero entries of FLOAT_QUAT_COMP(*q, q_roll, q_pitch) */
252  q->qi = qi_roll * qi_pitch;
253  q->qx = qx_roll * qi_pitch;
254  q->qy = qi_roll * qy_pitch;
255  q->qz = qx_roll * qy_pitch;
256 }
257 
258 void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) {
259 
260  // FIXME: remove me, do in quaternion directly
261  // is currently still needed, since the yaw setpoint integration is done in eulers
262 #if defined STABILIZATION_ATTITUDE_TYPE_INT
264 #else
266 #endif
267 
268  struct FloatQuat q_rp_cmd;
270 
271  /* get current heading */
272  const struct FloatVect3 zaxis = {0., 0., 1.};
273  struct FloatQuat q_yaw;
274 
275  //Care Free mode
277  //care_free_heading has been set to current psi when entering care free mode.
279  }
280  else {
282  }
283 
284  /* roll/pitch commands applied to to current heading */
285  struct FloatQuat q_rp_sp;
286  FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd);
287  FLOAT_QUAT_NORMALIZE(q_rp_sp);
288 
289  if (in_flight)
290  {
291  /* get current heading setpoint */
292  struct FloatQuat q_yaw_sp;
293 #if defined STABILIZATION_ATTITUDE_TYPE_INT
295 #else
297 #endif
298 
299  /* rotation between current yaw and yaw setpoint */
300  struct FloatQuat q_yaw_diff;
301  FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
302 
303  /* compute final setpoint with yaw */
304  FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff);
305  } else {
306  QUAT_COPY(*q_sp, q_rp_sp);
307  }
308 }
309 
310 //Function that reads the rc setpoint in an earth bound frame
312  // FIXME: remove me, do in quaternion directly
313  // is currently still needed, since the yaw setpoint integration is done in eulers
314  #if defined STABILIZATION_ATTITUDE_TYPE_INT
316  #else
318  #endif
319 
320  const struct FloatVect3 zaxis = {0., 0., 1.};
321 
322  struct FloatQuat q_rp_cmd;
324 
325  if (in_flight) {
326  /* get current heading setpoint */
327  struct FloatQuat q_yaw_sp;
328 
329  #if defined STABILIZATION_ATTITUDE_TYPE_INT
331  #else
333  #endif
334 
335  FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd);
336  }
337  else {
338  struct FloatQuat q_yaw;
340 
341  /* roll/pitch commands applied to to current heading */
342  struct FloatQuat q_rp_sp;
343  FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd);
344  FLOAT_QUAT_NORMALIZE(q_rp_sp);
345 
346  QUAT_COPY(*q_sp, q_rp_sp);
347  }
348 }
int32_t phi
in rad with INT32_ANGLE_FRAC
#define FLOAT_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
#define ANGLE_FLOAT_OF_BFP(_ai)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define GUIDANCE_H_MODE_CARE_FREE
Definition: guidance_h.h:50
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1027
#define ANGLE_BFP_OF_REAL(_af)
uint8_t autopilot_mode
Definition: autopilot.c:39
General attitude stabilization interface for rotorcrafts.
float psi
in radians
int32_t stabilization_attitude_get_heading_i(void)
#define FLOAT_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
#define AP_MODE_FORWARD
Definition: autopilot.h:54
Read an attitude setpoint from the RC.
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight)
int32_t theta
in rad with INT32_ANGLE_FRAC
Autopilot modes.
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight)
#define YAW_DEADBAND_EXCEEDED()
float theta
in radians
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 RADIO_PITCH
Definition: spektrum_arch.h:40
Roation quaternion.
#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
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight)
Read attitude setpoint from RC as euler angles.
struct Int32Eulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define FLOAT_ANGLE_NORMALIZE(_a)
struct RadioControl radio_control
Definition: radio_control.c:25
#define INT32_ANGLE_NORMALIZE(_a)
uint8_t guidance_h_mode
Definition: guidance_h.c:59
#define RADIO_YAW
Definition: spektrum_arch.h:41
signed long int32_t
Definition: types.h:19
#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:476
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t transition_theta_offset
Definition: guidance_h.c:83
Horizontal guidance for rotorcrafts.
#define FLOAT_QUAT_COMP_INV(_a2b, _a2c, _b2c)
#define MAX_PPRZ
Definition: paparazzi.h:8
#define RADIO_ROLL
Definition: spektrum_arch.h:39
float stabilization_attitude_get_heading_f(void)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight)
#define SQUARE(_a)
Definition: pprz_algebra.h:30
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
euler angles