Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
quat_setpoint_int.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2008-2011 Joby Energy Inc
4  *
5  */
6 
12 #include "subsystems/ahrs.h"
13 
16 #include "stabilization.h"
17 
18 #include "messages.h"
19 #include "mcu_periph/uart.h"
21 
22 #define QUAT_SETPOINT_HOVER_PITCH RadOfDeg(90)
23 
24 #define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ)
25 #define PITCH_COEF (STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ)
26 #define YAW_COEF (STABILIZATION_ATTITUDE_SP_MAX_PSI / MAX_PPRZ)
27 
28 #define DEADBAND_EXCEEDED(VARIABLE, VALUE) ((VARIABLE > VALUE) || (VARIABLE < -VALUE))
29 #define APPLY_DEADBAND(VARIABLE, VALUE) (DEADBAND_EXCEEDED(VARIABLE, VALUE) ? VARIABLE : 0.0)
30 
31 // reset to "hover" setpoint
32 static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial)
33 {
34  int32_t pitch_rotation_angle;
35  struct Int32Quat pitch_axis_quat;
36 
37  struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2;
38 
39  struct Int32Vect3 y_axis = { 0, 1, 0 };
40 
41  struct Int32Eulers rotated_eulers;
42 
43  // compose rotation about Y axis (pitch axis) from hover
44  pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH);
45  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
46  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat);
47 
48  INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat);
49 
50  // reset euler angles
51  rotated_eulers.theta = _theta;
52  rotated_eulers.phi = _psi;
53 
54  INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers);
55 
56  // compose rotation about Y axis (pitch axis) to hover
57  pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH);
58  INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle);
59  INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat);
60 
61  // store result into setpoint
62  QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2);
63 }
64 
66 
67  // FIXME: wtf???
68 #ifdef AIRPLANE_STICKS
69  pprz_t roll = radio_control.values[RADIO_ROLL];
70  pprz_t pitch = radio_control.values[RADIO_PITCH];
71  pprz_t yaw = radio_control.values[RADIO_YAW];
72 #else // QUAD STICKS
73  pprz_t roll = radio_control.values[RADIO_YAW];
74  pprz_t pitch = radio_control.values[RADIO_PITCH];
75  pprz_t yaw = -radio_control.values[RADIO_ROLL];
76 #endif
77  struct Int32Eulers sticks_eulers;
78  struct Int32Quat sticks_quat, prev_sp_quat;
79 
80  // heading hold?
81  if (in_flight) {
82  // compose setpoint based on previous setpoint + pitch/roll sticks
84 
85  // get commanded yaw rate from sticks
86  sticks_eulers.phi = RATE_BFP_OF_REAL(APPLY_DEADBAND(roll, STABILIZATION_ATTITUDE_DEADBAND_A) * ROLL_COEF / RC_UPDATE_FREQ);
87  sticks_eulers.theta = 0;
88  sticks_eulers.psi = 0;
89 
90  // convert yaw rate * dt into quaternion
91  INT32_QUAT_OF_EULERS(sticks_quat, sticks_eulers);
92  QUAT_COPY(prev_sp_quat, stab_att_sp_quat)
93 
94  // update setpoint by rotating by incremental yaw command
95  INT32_QUAT_COMP_NORM_SHORTEST(stab_att_sp_quat, prev_sp_quat, sticks_quat);
96  } else { /* if not flying, use current body position + pitch/yaw from sticks to compose setpoint */
98  }
99 
100  // update euler setpoints for telemetry
102 }
103 
105 {
106  // reset setpoint to "hover"
108 }
int32_t phi
in rad with INT32_ANGLE_FRAC
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
Attitude and Heading Reference System interface.
#define INT32_QUAT_OF_AXIS_ANGLE(_q, _uv, _an)
#define ANGLE_BFP_OF_REAL(_af)
void stabilization_attitude_sp_enter(void)
Rotation quaternion.
int16_t pprz_t
Definition: paparazzi.h:6
struct Ahrs ahrs
global AHRS state (fixed point version)
Definition: ahrs.c:24
int32_t theta
in rad with INT32_ANGLE_FRAC
#define RADIO_PITCH
Definition: spektrum_arch.h:42
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define PITCH_COEF
#define APPLY_DEADBAND(VARIABLE, VALUE)
#define INT32_QUAT_OF_EULERS(_q, _e)
#define YAW_COEF
struct Int32Quat ltp_to_body_quat
Rotation from LocalTangentPlane to body frame as unit quaternion.
Definition: ahrs.h:49
#define INT32_QUAT_COMP_NORM_SHORTEST(_a2c, _a2b, _b2c)
struct RadioControl radio_control
Definition: radio_control.c:27
#define RADIO_YAW
Definition: spektrum_arch.h:43
signed long int32_t
Definition: types.h:19
struct FloatQuat stab_att_sp_quat
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:478
#define QUAT_SETPOINT_HOVER_PITCH
int32_t psi
in rad with INT32_ANGLE_FRAC
General stabilization interface for rotorcrafts.
#define RATE_BFP_OF_REAL(_af)
#define RADIO_ROLL
Definition: spektrum_arch.h:41
static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial)
#define INT32_EULERS_OF_QUAT(_e, _q)
#define ROLL_COEF
void stabilization_attitude_read_rc_absolute(bool_t in_flight)
euler angles