27 #include "generated/airframe.h"
35 #ifndef STABILIZATION_ATTITUDE_DEADBAND_A
36 #define STABILIZATION_ATTITUDE_DEADBAND_A 0
39 #ifndef STABILIZATION_ATTITUDE_DEADBAND_E
40 #define STABILIZATION_ATTITUDE_DEADBAND_E 0
49 #ifndef COORDINATED_TURN_AIRSPEED
50 #define COORDINATED_TURN_AIRSPEED 12.0
53 #define YAW_DEADBAND_EXCEEDED() \
54 (radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
55 radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
64 #if STABILIZATION_ATTITUDE_DEADBAND_A
76 #if STABILIZATION_ATTITUDE_DEADBAND_E
80 return pitch * max_rc_theta /
MAX_PPRZ;
88 DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
89 return yaw * max_rc_r / (
MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
95 #if STABILIZATION_ATTITUDE_DEADBAND_A
99 return roll * STABILIZATION_ATTITUDE_SP_MAX_PHI /
MAX_PPRZ;
106 #if STABILIZATION_ATTITUDE_DEADBAND_E
110 return pitch * STABILIZATION_ATTITUDE_SP_MAX_THETA /
MAX_PPRZ;
117 DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R);
118 return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (
MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R);
141 heading = att->
psi - att->
phi;
143 heading = att->
psi + att->
phi;
155 if (fabsf(att->
phi) < M_PI / 2) {
157 }
else if (att->
theta > 0) {
158 heading = att->
psi - att->
phi;
160 heading = att->
psi + att->
phi;
174 bool coordinated_turn)
193 if (coordinated_turn) {
198 if (abs(sp->
phi) < max_phi) {
204 sp->
psi += omega * dt;
206 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
215 if (delta_psi > delta_limit) {
216 sp->
psi = heading + delta_limit;
217 }
else if (delta_psi < -delta_limit) {
218 sp->
psi = heading - delta_limit;
241 sp->
theta = temp_theta;
253 bool coordinated_turn)
272 if (coordinated_turn) {
276 const float max_phi = RadOfDeg(60.0);
277 if (fabsf(sp->
phi) < max_phi) {
283 sp->
psi += omega * dt;
285 #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
292 if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
293 sp->
psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
294 }
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
295 sp->
psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
310 sin_psi = sinf(care_free_delta_psi_f);
311 cos_psi = cosf(care_free_delta_psi_f);
313 temp_theta = cos_psi * sp->
theta - sin_psi * sp->
phi;
316 sp->
theta = temp_theta;
351 float qx_roll = sinf(roll2);
352 float qi_roll = cosf(roll2);
357 float qy_pitch = sinf(pitch2);
358 float qi_pitch = cosf(pitch2);
361 q->
qi = qi_roll * qi_pitch;
362 q->
qx = qx_roll * qi_pitch;
363 q->
qy = qi_roll * qy_pitch;
364 q->
qz = qx_roll * qy_pitch;
375 bool coordinated_turn)
380 #if defined STABILIZATION_ATTITUDE_TYPE_INT
409 #if defined STABILIZATION_ATTITUDE_TYPE_INT
428 bool in_carefree,
bool coordinated_turn)
432 #if defined STABILIZATION_ATTITUDE_TYPE_INT
447 #if defined STABILIZATION_ATTITUDE_TYPE_INT
#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.
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static float get_rc_yaw_f(void)
#define STABILIZATION_ATTITUDE_DEADBAND_A
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
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.
static int32_t get_rc_pitch(void)
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]
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.
#define ANGLE_FLOAT_OF_BFP(_ai)
#define QUAT_COPY(_qo, _qi)
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
#define PPRZ_ITRIG_SIN(_s, _a)
Some helper functions to check RC sticks.
Architecture independent timing functions.
#define INT32_ANGLE_NORMALIZE(_a)
int32_t transition_theta_offset
static void float_quat_normalize(struct FloatQuat *q)
struct RadioControl radio_control
#define STABILIZATION_ATTITUDE_DEADBAND_E
#define ANGLE_BFP_OF_REAL(_af)
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.
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).
#define COORDINATED_TURN_AIRSPEED
Airspeed that will be used in the turning speed calculation (m/s).
#define PPRZ_ITRIG_COS(_c, _a)