83#include "generated/airframe.h"
92#ifndef ONELOOP_ANDI_SIMPLE_QUAD
97#include "generated/modules.h"
98#ifdef MODULE_SYS_ID_DOUBLET_ID
106#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
119static void oneloop_andi_constant_eff(
void);
128#ifndef ONELOOP_ANDI_DEBUG_MODE
129#define ONELOOP_ANDI_DEBUG_MODE FALSE
133static float dt_1l = 1./PERIODIC_FREQUENCY;
134static float g = 9.81;
140#ifdef ONELOOP_ANDI_FILT_CUTOFF
146#ifdef ONELOOP_ANDI_FILT_CUTOFF_ACC
152#ifdef ONELOOP_ANDI_FILT_CUTOFF_VEL
158#ifdef ONELOOP_ANDI_FILT_CUTOFF_P
159#define ONELOOP_ANDI_FILTER_ROLL_RATE TRUE
165#ifdef ONELOOP_ANDI_FILT_CUTOFF_Q
166#define ONELOOP_ANDI_FILTER_PITCH_RATE TRUE
172#ifdef ONELOOP_ANDI_FILT_CUTOFF_R
173#define ONELOOP_ANDI_FILTER_YAW_RATE TRUE
179PRINT_CONFIG_MSG(
"============================================================")
187PRINT_CONFIG_MSG(
"============================================================")
198#define USE_PID_NUMDIFF_DERIVATIVE
199#ifdef USE_PID_NUMDIFF_DERIVATIVE
208#ifndef ONELOOP_ANDI_NUM_THRUSTERS
214#ifdef ONELOOP_ANDI_ACT_IS_SERVO
220#ifdef ONELOOP_ANDI_ACT_DYN
224#error "You must specify the actuator dynamics"
229#ifdef ONELOOP_ANDI_ACT_MAX
235#ifdef ONELOOP_ANDI_ACT_MIN
241#ifdef ONELOOP_ANDI_ACT_MAX_NORM
247#ifdef ONELOOP_ANDI_ACT_MIN_NORM
253#ifdef ONELOOP_ANDI_NU_NORM_MAX
259#ifdef ONELOOP_ANDI_U_PREF
265#define ONELOOP_ANDI_MAX_BANK act_max[COMMAND_ROLL]
266#define ONELOOP_ANDI_MAX_PHI act_max[COMMAND_ROLL]
267#define ONELOOP_ANDI_MAX_THETA act_max[COMMAND_PITCH]
269#ifndef ONELOOP_THETA_PREF_MAX
275#if ANDI_NUM_ACT_TOT != WLS_N_U_MAX
276#error Matrix-WLS_N_U_MAX is not equal to the number of actuators: define WLS_N_U_MAX == ANDI_NUM_ACT_TOT in airframe file
277#define WLS_N_U_MAX == ANDI_NUM_ACT_TOT
279#if ANDI_OUTPUTS != WLS_N_V_MAX
280#error Matrix-WLS_N_V_MAX is not equal to the number of controlled axis: define WLS_N_V_MAX == ANDI_OUTPUTS in airframe file
281#define WLS_N_V_MAX == ANDI_OUTPUTS
297float max_r = RadOfDeg(MAX_R);
300#ifdef ONELOOP_ANDI_YAW_DISTURBANCE_LIMIT
306#if ONELOOP_ANDI_HEADING_MANUAL
312#if ONELOOP_ANDI_YAW_STICK_IN_AUTO
319 #ifdef ONELOOP_ANDI_MAX_ANGULAR_JERK
320 .att_3d[0] = ONELOOP_ANDI_MAX_ANGULAR_JERK,
321 .att_3d[1] = ONELOOP_ANDI_MAX_ANGULAR_JERK,
323 .att_3d[0] = RadOfDeg(100000.0),
324 .att_3d[1] = RadOfDeg(100000.0),
327#ifdef ONELOOP_ANDI_MAX_ANGULAR_JERK_YAW
328 .att_3d[2] = ONELOOP_ANDI_MAX_ANGULAR_JERK_YAW,
330 .att_3d[2] = RadOfDeg(1520.0),
333 #ifdef ONELOOP_ANDI_MAX_ANGULAR_ACCEL
334 .att_2d[0] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL,
335 .att_2d[1] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL,
337 .att_2d[0] = RadOfDeg(700.0),
338 .att_2d[1] = RadOfDeg(480.0),
341#ifdef ONELOOP_ANDI_MAX_ANGULAR_ACCEL_YAW
342 .att_2d[2] = ONELOOP_ANDI_MAX_ANGULAR_ACCEL_YAW,
344 .att_2d[2] = RadOfDeg(130.0),
347#ifdef ONELOOP_ANDI_MAX_ANGULAR_VEL
348 .att_d[0] = ONELOOP_ANDI_MAX_ANGULAR_VEL,
349 .att_d[1] = ONELOOP_ANDI_MAX_ANGULAR_VEL,
351 .att_d[0] = RadOfDeg(10000.0),
352 .att_d[1] = RadOfDeg(10000.0),
355#ifdef ONELOOP_ANDI_MAX_ANGULAR_VEL_YAW
356 .att_d[2] = ONELOOP_ANDI_MAX_ANGULAR_VEL_YAW,
358 .att_d[2] = RadOfDeg(30.0),
366static float psi_vec[4] = {0.0, 0.0, 0.0, 0.0};
367static float phi_vec[4] = {0.0, 0.0, 0.0, 0.0};
371#ifndef ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD
372#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0
375#ifndef FWD_SIDESLIP_GAIN
381#ifdef NAV_HYBRID_MAX_DECELERATION
387#ifdef ONELOOP_ANDI_MAX_LINEAR_JERK
388float max_j_lin = ONELOOP_ANDI_MAX_LINEAR_JERK;
393#ifdef NAV_HYBRID_MAX_AIRSPEED
399#ifdef NAV_HYBRID_MAX_SPEED_V
405#define USE_ND_VELOCITIES
417#ifndef ONELOOP_ANDI_WU_QUAD_MOTORS_FWD
428#ifdef ONELOOP_ANDI_WV
429 .Wv = ONELOOP_ANDI_WV,
433#ifdef ONELOOP_ANDI_WU
434 .Wu = ONELOOP_ANDI_WU,
446#ifdef ONELOOP_ANDI_WV
452#ifdef ONELOOP_ANDI_WU
546void err_nd(
float err[],
float a[],
float b[],
float k[],
int n);
547void err_sum_nd(
float err[],
float a[],
float b[],
float k[],
float c[],
int n);
549void integrate_nd(
float dt,
float a[],
float a_dot[],
int n);
554void rm_3rd_attitude(
float dt,
float x_ref[3],
float x_d_ref[3],
float x_2d_ref[3],
float x_3d_ref[3],
float x_des[3],
bool ow_psi,
float psi_overwrite[4],
float k1_rm[3],
float k2_rm[3],
float k3_rm[3],
struct OneloopStabilizationRef bounds);
555void rm_3rd_pos(
float dt,
float x_ref[],
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x_des[],
float k1_rm[],
float k2_rm[],
float k3_rm[],
float x_d_bound,
float x_2d_bound,
float x_3d_bound,
int n);
556void rm_2nd_pos(
float dt,
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x_d_des[],
float k2_rm[],
float k3_rm[],
float x_2d_bound,
float x_3d_bound,
int n);
557void rm_1st_pos(
float dt,
float x_2d_ref[],
float x_3d_ref[],
float x_2d_des[],
float k3_rm[],
float x_3d_bound,
int n);
558void ec_3rd_att(
float y_4d[3],
float x_des[3],
float x_ref[3],
float x_d_ref[3],
float x_2d_ref[3],
float x_3d_ref[3],
float x[3],
float x_d[3],
float x_2d[3],
float k1_e[3],
float k2_e[3],
float k3_e[3],
struct OneloopStabilizationRef bounds,
float fb[3]);
559void ec_3rd_pos(
float y_4d[],
float x_des[],
float x_ref[],
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x[],
float x_d[],
float x_2d[],
float k1_e[],
float k2_e[],
float k3_e[],
float x_d_bound,
float x_2d_bound,
float fb[],
int n);
562void chirp_pos(
float time_elapsed,
float f0,
float f1,
float t_chirp,
float A,
int8_t n,
float psi,
float p_ref[],
float v_ref[],
float a_ref[],
float j_ref[],
float p_ref_0[]);
563void chirp_call(
bool*
chirp_on,
bool*
chirp_first_call,
float*
t_0_chirp,
float* time_elapsed,
float f0,
float f1,
float t_chirp,
float A,
int8_t n,
float psi,
float p_ref[],
float v_ref[],
float a_ref[],
float j_ref[],
float p_ref_0[]);
572#if PERIODIC_TELEMETRY
604 pprz_msg_send_EFF_MAT_STAB(trans,
dev, AC_ID,
612 pprz_msg_send_EFF_MAT_GUID(trans,
dev, AC_ID,
620 pprz_msg_send_STAB_ATTITUDE(trans,
dev, AC_ID,
621 3, temp_eulers_zxy_des,
633 pprz_msg_send_GUIDANCE(trans,
dev, AC_ID,
656static void debug_vect(
struct transport_tx *trans,
struct link_device *
dev,
char *name,
float *data,
int datasize)
658 pprz_msg_send_DEBUG_VECT(trans,
dev, AC_ID,
664 float temp_debug_vect[9];
690 if (input < FLT_EPSILON)
697void err_nd(
float err[],
float a[],
float b[],
float k[],
int n)
700 for (i = 0; i < n; i++) {
701 err[i] = k[i] * (a[i] -
b[i]);
705void err_sum_nd(
float err[],
float a[],
float b[],
float k[],
float c[],
int n)
708 for (i = 0; i < n; i++) {
709 err[i] = k[i] * (a[i] -
b[i]);
716 err[0] = (a[0] -
b[0]);
717 err[1] = (a[1] -
b[1]);
718 err[2] = (a[2] -
b[2]);
719 NormRadAngle(err[2]);
725 for (i = 0; i < n; i++) {
726 a[i] = a[i] + dt * a_dot[i];
733 if((norm-bound) > FLT_EPSILON) {
734 float scale = bound/norm;
736 for(i = 0; i < n; i++) {
744static float k_rm_1_3_f(
float omega_n,
float zeta,
float p1) {
748 return (omega_n * omega_n * p1) / (omega_n * omega_n + omega_n * p1 * zeta * 2.0);
750static float k_rm_2_3_f(
float omega_n,
float zeta,
float p1) {
754 return (omega_n * omega_n + omega_n * p1 * zeta * 2.0) / (p1 + omega_n * zeta * 2.0);
756static float k_rm_3_3_f(
float omega_n,
float zeta,
float p1) {
760 return p1 + omega_n * zeta * 2.0;
765static void Pos_PID(
float x_des[3],
float x[3],
float x_dot[3],
float k_P,
float k_I,
float k_D,
float a[3])
767 static float prev_pos[3] = {0.0f, 0.0f, 0.0f};
768 static float integral[3] = {0.0f, 0.0f, 0.0f};
769 for (
int i = 0; i < 3; i++)
771 float err = x_des[i] - x[i];
772 integral[i] += err *
dt_1l;
774 const float I_MAX = 0.4f;
775 if (integral[i] > I_MAX) integral[i] = I_MAX;
776 if (integral[i] < -I_MAX) integral[i] = -I_MAX;
777#ifdef USE_PID_NUMDIFF_DERIVATIVE
783 float derivative_nd = (x[i] - prev_pos[i]) /
dt_1l;
788 float derivative = -x_dot[i];
795 a[i] =
k_P * err+
k_I * integral[i]+
k_D * derivative;
807 float spsi = sinf(psi);
808 float cpsi = cosf(psi);
810 float aX = cpsi*a[0]+spsi*a[1];
811 float aY =-spsi*a[0]+cpsi*a[1];
813 theta_des = asinf(-aX);
814 float ctheta_des = cosf(theta_des);
815 if (fabs(ctheta_des) < FLT_EPSILON){ctheta_des = FLT_EPSILON;}
816 phi_des = asinf(aY/ctheta_des);
818 BoundAbs(phi_des, M_PI_6);
819 BoundAbs(theta_des, M_PI_6);
840 float sphi = sinf(e[0]);
841 float cphi = cosf(e[0]);
842 float stheta = sinf(e[1]);
843 float ctheta = cosf(e[1]);
844 r[0] = ctheta * edot[0] - stheta * cphi * edot[2];
845 r[1] = edot[1] + sphi * edot[2];
846 r[2] = stheta * edot[0] + ctheta *cphi * edot[2];
868 float sphi = sinf(e[0]);
869 float cphi = cosf(e[0]);
870 float stheta = sinf(e[1]);
871 float ctheta = cosf(e[1]);
872 float spsi = sinf(e[2]);
873 float cpsi = cosf(e[2]);
875 if (fabs(ctheta) < FLT_EPSILON)
877 ctheta = FLT_EPSILON;
879 edot[0] = r[0] + spsi * stheta / ctheta * r[1] + cpsi * stheta / ctheta * r[2];
880 edot[1] = cphi * r[1] - sphi * r[2];
881 edot[2] = sphi * r[1] + cphi * r[2];
889 float v[2] = {vect->
x, vect->
y};
890 float sign_v0 = (v[0] > 0.f) ? 1.f : (v[0] < 0.f) ? -1.f : 0.f;
891 float sign_v1 = (v[1] > 0.f) ? 1.f : (v[1] < 0.f) ? -1.f : 0.f;
896 if ((norm - bound) > FLT_EPSILON)
898 v[0] =
Min(v[0], bound);
899 float acc_b_y_2 = bound * bound - v[0] * v[0];
901 v[1] = sqrtf(acc_b_y_2);
903 vect->
x = sign_v0 * v[0];
904 vect->
y = sign_v1 * v[1];
918 norm = fmaxf(norm, 1.0);
919 float v_bound_a = sqrtf(fabs(2.0 * a_bound * norm *
xdot_lim_sf));
920 return fminf(v_bound, v_bound_a);
936 float v_bound_a = sqrtf(fabs(2.0 * a_bound * norm *
xdot_lim_sf));
937 return fminf(v_bound, v_bound_a);
956void rm_3rd_attitude(
float dt,
float x_ref[3],
float x_d_ref[3],
float x_2d_ref[3],
float x_3d_ref[3],
float x_des[3],
bool ow_psi,
float psi_overwrite[4],
float k1_rm[3],
float k2_rm[3],
float k3_rm[3],
struct OneloopStabilizationRef bounds){
960 float x_d_fw_rates[3];
963 float bounds_att_d[3];
964 float x_d_eul_ref[3];
967 x_d_fw[0] = e_x[0] * k1_rm[0];
968 x_d_fw[1] = e_x[1] * k1_rm[1];
969 x_d_fw[2] = e_x[2] * k1_rm[2];
974 BoundAbs(x_d_fw_rates[0], bounds_att_d[0]);
975 BoundAbs(x_d_fw_rates[1], bounds_att_d[1]);
976 BoundAbs(x_d_fw_rates[2], bounds_att_d[2]);
978 err_nd(x_2d_fw, x_d_fw_rates, x_d_ref, k2_rm, 3);
979 BoundAbs(x_2d_fw[0], bounds.
att_2d[0]);
980 BoundAbs(x_2d_fw[1], bounds.
att_2d[1]);
981 BoundAbs(x_2d_fw[2], bounds.
att_2d[2]);
983 err_nd(x_3d_fw, x_2d_fw, x_2d_ref, k3_rm, 3);
984 BoundAbs(x_3d_fw[0], bounds.
att_3d[0]);
985 BoundAbs(x_3d_fw[1], bounds.
att_3d[1]);
986 BoundAbs(x_3d_fw[2], bounds.
att_3d[2]);
989 if (ow_psi){x_3d_ref[2] = psi_overwrite[3];}
992 if (ow_psi){x_2d_ref[2] = psi_overwrite[2];}
995 if (ow_psi){x_d_ref[2] = psi_overwrite[1];}
999 if (ow_psi){x_ref[2] = psi_overwrite[0];}
1000 NormRadAngle(x_ref[2]);
1020void rm_3rd_pos(
float dt,
float x_ref[],
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x_des[],
float k1_rm[],
float k2_rm[],
float k3_rm[],
float x_d_bound,
float x_2d_bound,
float x_3d_bound,
int n)
1025 err_nd(e_x, x_des, x_ref, k1_rm, n);
1028 err_nd(e_x_d, e_x, x_d_ref, k2_rm, n);
1030 err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, n);
1051void rm_2nd_pos(
float dt,
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x_d_des[],
float k2_rm[],
float k3_rm[],
float x_2d_bound,
float x_3d_bound,
int n)
1055 err_nd(e_x_d, x_d_des, x_d_ref, k2_rm, n);
1057 err_nd(e_x_2d, e_x_d, x_2d_ref, k3_rm, n);
1074void rm_1st_pos(
float dt,
float x_2d_ref[],
float x_3d_ref[],
float x_2d_des[],
float k3_rm[],
float x_3d_bound,
int n)
1077 err_nd(e_x_2d, x_2d_des, x_2d_ref, k3_rm, n);
1099void ec_3rd_pos(
float y_4d[],
float x_des[],
float x_ref[],
float x_d_ref[],
float x_2d_ref[],
float x_3d_ref[],
float x[],
float x_d[],
float x_2d[],
float k1_e[],
float k2_e[],
float k3_e[],
float x_d_bound,
float x_2d_bound,
float fb[],
int n)
1106 err_sum_nd(e_x_d, x_ref, x, k1_e, x_d_ref, n);
1108 err_sum_nd(e_x_2d, e_x_d, x_d, k2_e, x_2d_ref, n);
1113 err_sum_nd(y_4d, e_x_2d, dist, k3_e, x_3d_ref, n);
1131void ec_3rd_att(
float y_4d[3],
float x_des[3],
float x_ref[3],
float x_d_ref[3],
float x_2d_ref[3],
float x_3d_ref[3],
float x[3],
float x_d[3],
float x_2d[3],
float k1_e[3],
float k2_e[3],
float k3_e[3],
struct OneloopStabilizationRef bounds,
float fb[3])
1136 float x_d_fw_rates[3];
1137 float x_d_fw_rates_rm[3];
1139 float bounds_att_d[3];
1142 x_d_fw[0] = e_x[0]*k1_e[0];
1143 x_d_fw[1] = e_x[1]*k1_e[1];
1144 x_d_fw[2] = e_x[2]*k1_e[2];
1151 BoundAbs(x_d_fw_rates_rm[0], bounds_att_d[0]);
1152 BoundAbs(x_d_fw_rates_rm[1], bounds_att_d[1]);
1153 BoundAbs(x_d_fw_rates_rm[2], bounds_att_d[2]);
1155 x_2d_fw[0] = (x_d_fw_rates_rm[0] - x_d[0]) * k2_e[0] + x_2d_ref[0];
1156 x_2d_fw[1] = (x_d_fw_rates_rm[1] - x_d[1]) * k2_e[1] + x_2d_ref[1];
1157 x_2d_fw[2] = (x_d_fw_rates_rm[2] - x_d[2]) * k2_e[2] + x_2d_ref[2];
1166 err_sum_nd(y_4d, x_2d_fw, dist, k3_e, x_3d_ref, 3);
1176static float w_approx(
float p1,
float p2,
float p3,
float rm_k)
1182 float tao = (p1 * p2 + p1 * p3 + p2 * p3) / (p1 * p2 * p3) / (rm_k);
1247 float max_wind = 20.0;
1444 LP->tau = 1 / (2 * M_PI *
LP->freq);
1447#elif defined(USE_BW2)
1449#elif defined(USE_YAW_LP4)
1455 LP->meas_prev = 0.0;
1461 if (
LP->freq !=
LP->freq_set || reinit)
1463 LP->freq =
LP->freq_set;
1464 LP->tau = 1 / (2 * M_PI *
LP->freq);
1471 if (
LP->freq !=
LP->freq_set || reinit)
1473 LP->freq =
LP->freq_set;
1474 LP->tau = 1 / (2 * M_PI *
LP->freq);
1521 float tau_2 = 1.0 / (2.0 * M_PI * 2.0);
1526#ifdef USE_PID_NUMDIFF_DERIVATIVE
1527 for (
int i = 0; i < 3; i++){
1543#ifdef USE_ND_VELOCITIES
1544 static bool vel_inited =
false;
1546 if (!vel_inited) { veloc_prev = *veloc; vel_inited =
true; }
1547 LP.ax.meas = (veloc->
x - veloc_prev.
x) * PERIODIC_FREQUENCY;
1548 LP.ay.meas = (veloc->
y - veloc_prev.
y) * PERIODIC_FREQUENCY;
1549 LP.az.meas = (veloc->
z - veloc_prev.
z) * PERIODIC_FREQUENCY;
1550 veloc_prev = *veloc;
1553 LP.ax.meas = accel->
x;
1554 LP.ay.meas = accel->
y;
1555 LP.az.meas = accel->
z;
1557 LP.p.meas_prev =
LP.p.meas;
1558 LP.q.meas_prev =
LP.q.meas;
1559 LP.r.meas_prev =
LP.r.meas;
1560 LP.p.meas = body_rates->
p;
1561 LP.q.meas = body_rates->
q;
1562 LP.r.meas = body_rates->
r;
1563 LP.p_dot.meas = (
LP.p.meas -
LP.p.meas_prev) * PERIODIC_FREQUENCY;
1564 LP.q_dot.meas = (
LP.q.meas -
LP.q.meas_prev) * PERIODIC_FREQUENCY;
1565 LP.r_dot.meas = (
LP.r.meas -
LP.r.meas_prev) * PERIODIC_FREQUENCY;
1580 Bound(airspeed_meas, 0.0, 30.0);
1662#if PERIODIC_TELEMETRY
1682#ifndef ONELOOP_ANDI_SIMPLE_QUAD
1731 float thrust_cmd_1l = 0.0;
1757 BoundAbs(des_r,
max_r);
1758 float delta_psi_des_rad = des_r *
dt_1l;
1760 NormRadAngle(delta_psi_rad);
1761 if (fabs(delta_psi_rad) > RadOfDeg(30.0))
1763 delta_psi_des_rad = 0.0;
1768 if (!in_flight_oneloop)
1776 Bound(thrust_cmd_1l, 0.0,
MAX_PPRZ);
1791 rm_3rd_attitude(
dt_1l,
oneloop_andi.sta_ref.att,
oneloop_andi.sta_ref.att_d,
oneloop_andi.sta_ref.att_2d,
oneloop_andi.sta_ref.att_3d, att_des,
false,
psi_vec,
k_att_rm.k1,
k_att_rm.k2,
k_att_rm.k3,
sta_bounds);
1799 if (rm_order_h == 3)
1801 rm_3rd_pos(
dt_1l,
oneloop_andi.gui_ref.pos,
oneloop_andi.gui_ref.vel,
oneloop_andi.gui_ref.acc,
oneloop_andi.gui_ref.jer,
nav_target,
k_pos_rm.k1,
k_pos_rm.k2,
k_pos_rm.k3,
max_v_nav,
max_a_nav,
max_j_lin, 2);
1803 else if (rm_order_h == 2)
1811 else if (rm_order_h == 1)
1832 if (!in_flight_oneloop)
1839 float single_value_ref[1] = {
oneloop_andi.gui_ref.pos[2]};
1840 float single_value_d_ref[1] = {
oneloop_andi.gui_ref.vel[2]};
1841 float single_value_2d_ref[1] = {
oneloop_andi.gui_ref.acc[2]};
1842 float single_value_3d_ref[1] = {
oneloop_andi.gui_ref.jer[2]};
1843 float single_value_nav_target[1] = {
nav_target[2]};
1844 float single_value_k1_rm[1] = {
k_pos_rm.k1[2]};
1845 float single_value_k2_rm[1] = {
k_pos_rm.k2[2]};
1846 float single_value_k3_rm[1] = {
k_pos_rm.k3[2]};
1847 if (rm_order_v == 3)
1849 rm_3rd_pos(
dt_1l, single_value_ref, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k1_rm, single_value_k2_rm, single_value_k3_rm,
max_v_nav_v,
max_a_nav,
max_j_lin, 1);
1855 else if (rm_order_v == 2)
1857 rm_2nd_pos(
dt_1l, single_value_d_ref, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k2_rm, single_value_k3_rm,
max_a_nav,
max_j_lin, 1);
1863 else if (rm_order_v == 1)
1865 rm_1st_pos(
dt_1l, single_value_2d_ref, single_value_3d_ref, single_value_nav_target, single_value_k3_rm,
max_j_lin, 1);
1883 float trigger_acc = 0.0;
1918 chirp_call(&
chirp_on, &
chirp_first_call, &
t_0_chirp, &
time_elapsed_chirp,
f0_chirp,
f1_chirp,
t_chirp,
A_chirp,
chirp_axis, att_des[2],
oneloop_andi.gui_ref.pos,
oneloop_andi.gui_ref.vel,
oneloop_andi.gui_ref.acc,
oneloop_andi.gui_ref.jer,
p_ref_0);
1921 bool ow_psi =
false;
1934 bool ow_phi =
false;
1944 rm_3rd_attitude(
dt_1l,
oneloop_andi.sta_ref.att,
oneloop_andi.sta_ref.att_d,
oneloop_andi.sta_ref.att_2d,
oneloop_andi.sta_ref.att_3d, att_des, ow_psi,
psi_vec,
k_att_rm.k1,
k_att_rm.k2,
k_att_rm.k3,
sta_bounds);
1989 bool in_flight_oneloop =
false;
1991 in_flight_oneloop =
true;
1995 in_flight_oneloop =
false;
2041 oneloop_andi_RM(half_loop, PSA_des, rm_order_h, rm_order_v, in_flight_oneloop);
2058 float temp_dist_bound_gui[3] = {0.0, 0.0, 0.0};
2059 ec_3rd_pos(
nu, pos_des,
oneloop_andi.gui_ref.pos,
oneloop_andi.gui_ref.vel,
oneloop_andi.gui_ref.acc,
oneloop_andi.gui_ref.jer,
oneloop_andi.gui_state.pos,
oneloop_andi.gui_state.vel,
oneloop_andi.gui_state.acc,
k_pos_e.k1,
k_pos_e.k2,
k_pos_e.k3,
max_v_nav,
max_a_nav, temp_dist_bound_gui, 3);
2063 float dummy1[3] = {0.0, 0.0, 0.0};
2064 ec_3rd_pos(
nu, pos_des,
oneloop_andi.gui_ref.pos,
oneloop_andi.gui_ref.vel,
oneloop_andi.gui_ref.acc, dummy1,
oneloop_andi.gui_state.pos,
oneloop_andi.gui_state.vel,
oneloop_andi.gui_state.acc,
k_pos_e_indi.k1,
k_pos_e_indi.k2,
k_pos_e_indi.k3,
max_v_nav,
max_a_nav, dummy1, 3);
2069#define USE_PID_OUTERLOOP
2070#ifdef USE_PID_OUTERLOOP
2088 ec_3rd_att(y_4d_att, att_des_4_ec,
oneloop_andi.sta_ref.att,
oneloop_andi.sta_ref.att_d,
oneloop_andi.sta_ref.att_2d,
oneloop_andi.sta_ref.att_3d,
oneloop_andi.sta_state.att,
oneloop_andi.sta_state.att_d,
oneloop_andi.sta_state.att_2d,
k_att_e.k1,
k_att_e.k2,
k_att_e.k3,
sta_bounds, temp_dist_bound_sta);
2092 float dummy0[3] = {0.0, 0.0, 0.0};
2094 ec_3rd_att(y_4d_att, att_des_4_ec,
oneloop_andi.sta_ref.att,
oneloop_andi.sta_ref.att_d,
oneloop_andi.sta_ref.att_2d, dummy0,
oneloop_andi.sta_state.att,
oneloop_andi.sta_state.att_d,
oneloop_andi.sta_state.att_2d,
k_att_e_indi.k1,
k_att_e_indi.k2,
k_att_e_indi.k3,
sta_bounds, temp_dist_bound_sta_INDI);
2104 nu[3] = y_4d_att[0];
2105 nu[4] = y_4d_att[1];
2162 case COMMAND_MOTOR_FRONT:
2163 case COMMAND_MOTOR_RIGHT:
2164 case COMMAND_MOTOR_BACK:
2165 case COMMAND_MOTOR_LEFT:
2167#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2168 float skew_bound =
RW.skew.deg;
2169 Bound(skew_bound, 70.0, 90.0);
2178#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2193#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2194 case COMMAND_MOTOR_PUSHER:
2195 case COMMAND_RUDDER:
2200 case COMMAND_AILERONS:
2201 if (
RW.skew.deg > 25.0)
2214 if (
RW.skew.deg > 50.0)
2226 case COMMAND_ELEVATOR:
2239#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2240 if (
RW.skew.deg > 50.0)
2264#ifdef MODULE_SYS_ID_DOUBLET_ID
2267 bool doublet_running =
false;
2269 if (!doublet_running)
2281#ifdef COMMAND_MOTOR_PUSHER
2306#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2309 commands[COMMAND_MOTOR_PUSHER] = -9600;
2328#ifndef USE_PID_OUTERLOOP
2351 float prev_actuator_state_1l;
2371#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
2377static void oneloop_andi_constant_eff(
void)
2385 float sphi = sinf(eul->
phi), cphi = cosf(eul->
phi);
2386 float stheta = sinf(eul->
theta), ctheta = cosf(eul->
theta);
2387 float spsi = sinf(eul->
psi), cpsi = cosf(eul->
psi);
2423#ifdef ONELOOP_ANDI_CONSTANT_EFFECTIVENESS
2424 oneloop_andi_constant_eff();
2431 case (COMMAND_MOTOR_FRONT):
2432 case (COMMAND_MOTOR_RIGHT):
2433 case (COMMAND_MOTOR_BACK):
2434 case (COMMAND_MOTOR_LEFT):
2435#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2436 case (COMMAND_MOTOR_PUSHER):
2437 case (COMMAND_ELEVATOR):
2438 case (COMMAND_RUDDER):
2439 case (COMMAND_AILERONS):
2440 case (COMMAND_FLAPS):
2466#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2469 bool turn_quad_off =
false;
2478#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2484 if (turn_quad_off && i < 4)
2522 ratio_u_un[i] = ratio_numerator / ratio_denominator;
2529 float ratio_denominator = 1.0;
2536 ratio_vn_v[i] = ratio_numerator / ratio_denominator;
2540 ratio_vn_v[i] = ratio_numerator / ratio_denominator;
2544 ratio_vn_v[i] = ratio_numerator / ratio_denominator;
2548 ratio_vn_v[i] = ratio_numerator / ratio_denominator;
2581 switch (
nav.setpoint_mode)
2594 switch (
nav.vertical_mode)
2617 Bound(airspeed_turn, 1.0f, 30.0f);
2619 float coordinated_turn_roll =
eulers_zxy.phi;
2626 BoundAbs(coordinated_turn_roll,
max_phi);
2627 omega =
g / airspeed_turn * tanf(coordinated_turn_roll);
2632#ifdef FWD_SIDESLIP_GAIN
2643 float p_ref_fun = sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0);
2644 return (
A * p_ref_fun);
2649 float v_ref_fun = cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * (M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0);
2650 return (
A * v_ref_fun);
2655 float a_ref_fun = -sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * pow((M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0), 2) + k * M_PI * cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * 4.0;
2656 return (
A * a_ref_fun);
2661 float j_ref_fun = -cosf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * pow((M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0), 3) - k * M_PI * sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0) * (M_PI * (f0 + k * delta_t) * 2.0 + k * delta_t * M_PI * 2.0) * 1.2e+1;
2662 return (
A * j_ref_fun);
2676void chirp_pos(
float time_elapsed,
float f0,
float f1,
float t_chirp,
float A,
int8_t n,
float psi,
float p_ref[],
float v_ref[],
float a_ref[],
float j_ref[],
float p_ref_0[])
2683 if ((f1 - f0) < -FLT_EPSILON)
2697 float k = (f1 - f0) /
t_chirp;
2703 float spsi = sinf(psi);
2704 float cpsi = cosf(psi);
2715 p_ref[0] =
p_ref_0[0] + p_ref_chirp * mult_0;
2716 p_ref[1] =
p_ref_0[1] + p_ref_chirp * mult_1;
2717 v_ref[0] = v_ref_chirp * mult_0;
2718 v_ref[1] = v_ref_chirp * mult_1;
2719 a_ref[0] = a_ref_chirp * mult_0;
2720 a_ref[1] = a_ref_chirp * mult_1;
2721 j_ref[0] = j_ref_chirp * mult_0;
2722 j_ref[1] = j_ref_chirp * mult_1;
2729 p_ref[0] =
p_ref_0[0] + p_ref_chirp * mult_0;
2730 p_ref[1] =
p_ref_0[1] + p_ref_chirp * mult_1;
2731 v_ref[0] = v_ref_chirp * mult_0;
2732 v_ref[1] = v_ref_chirp * mult_1;
2733 a_ref[0] = a_ref_chirp * mult_0;
2734 a_ref[1] = a_ref_chirp * mult_1;
2735 j_ref[0] = j_ref_chirp * mult_0;
2736 j_ref[1] = j_ref_chirp * mult_1;
2742 p_ref[2] =
p_ref_0[2] + p_ref_chirp * mult_2;
2743 v_ref[2] = v_ref_chirp * mult_2;
2744 a_ref[2] = a_ref_chirp * mult_2;
2745 j_ref[2] = j_ref_chirp * mult_2;
2751 float pitch_offset = RadOfDeg(5.0);
2759 oneloop_andi.sta_ref.att_d[2] = v_ref_chirp * M_PI / 180.0;
2760 oneloop_andi.sta_ref.att_2d[2] = a_ref_chirp * M_PI / 180.0;
2761 oneloop_andi.sta_ref.att_3d[2] = j_ref_chirp * M_PI / 180.0;
2769 if (time_elapsed < (
t_chirp / 4.0) || (time_elapsed > (3.0 *
t_chirp / 4.0)))
2775 oneloop_andi.sta_ref.att_2d[2] = A_backup * M_PI / 180.0;
2779 BoundAbs(S_psi, 0.9 * M_PI_2)
2795 if (time_elapsed < (0.5))
2810void chirp_call(
bool *
chirp_on,
bool *
chirp_first_call,
float *t_0,
float *time_elapsed,
float f0,
float f1,
float t_chirp,
float A,
int8_t n,
float psi,
float p_ref[],
float v_ref[],
float a_ref[],
float j_ref[],
float p_ref_0[])
2816 *time_elapsed = 0.0;
2826 chirp_pos(*time_elapsed, f0, f1,
t_chirp,
A, n, psi, p_ref, v_ref, a_ref, j_ref,
p_ref_0);
2832 *time_elapsed = 0.0;
2860 float cpsi = cosf(psi);
2861 float spsi = sinf(psi);
2864 struct FloatVect2 airspeed_v = {cpsi * airspeed, spsi * airspeed};
2870 VECT2_DIFF(windspeed, groundspeed, airspeed_v);
2875 if (norm_des_as <
min_as)
2884 float groundspeed_factor = 0.0f;
2887 float av = NT_v_NE.
x * NT_v_NE.
x + NT_v_NE.
y * NT_v_NE.
y;
2888 float bv = -2.f * (windspeed.
x * NT_v_NE.
x + windspeed.
y * NT_v_NE.
y);
2889 float cv = windspeed.
x * windspeed.
x + windspeed.
y * windspeed.
y -
max_as *
max_as;
2890 float dv = bv * bv - 4.0f * av * cv;
2896 float d_sqrt = sqrtf(dv);
2897 groundspeed_factor = (-bv + d_sqrt) / (2.0f * av);
2899 des_as_NE.
x = groundspeed_factor * NT_v_NE.
x - windspeed.
x;
2900 des_as_NE.
y = groundspeed_factor * NT_v_NE.
y - windspeed.
y;
2901 NT_v_NE.
x = groundspeed_factor * NT_v_NE.
x;
2902 NT_v_NE.
y = groundspeed_factor * NT_v_NE.
y;
2905 des_as_B.
x = norm_des_as;
2909 float delta_psi = atan2f(des_as_NE.
y, des_as_NE.
x) - psi;
2911 des_acc_B.
y = delta_psi * 5.0;
2912 des_acc_B.
x = (des_as_B.
x - airspeed) *
k_pos_rm.k2[0];
2938 u_trim[COMMAND_MOTOR_FRONT] = 4800.0;
2939 u_trim[COMMAND_MOTOR_RIGHT] = 4800.0;
2940 u_trim[COMMAND_MOTOR_BACK] = 4800.0;
2941 u_trim[COMMAND_MOTOR_LEFT] = 4800.0;
2942#ifndef ONELOOP_ANDI_SIMPLE_QUAD
2943 u_trim[COMMAND_MOTOR_PUSHER] = 0.0;
2944 u_trim[COMMAND_ELEVATOR] =
RW.ele_pref;
2945 u_trim[COMMAND_RUDDER] = 0.0;
2946 u_trim[COMMAND_AILERONS] = 0.0;
2947 u_trim[COMMAND_FLAPS] = 0.0;
2958 float u_range =
MAX_PPRZ - u_trim[j];
2959 eff = (eff / den) * u_range;
2970 float u_range =
MAX_PPRZ - u_trim[j];
2971 eff = (eff / den) * u_range;
2972 float eff2 = eff * eff;
3085 float exp_term = expf(-mu->
sigma / mu->
fs);
3088 float c_mu_c_0 = exp_term;
3095 mu->
fs = (float)PERIODIC_FREQUENCY;
3115 (void)max_climb_speed_quad;
3116 (void)max_climb_speed_fwd;
3121 (void)max_descend_speed_quad;
3122 (void)max_descend_speed_fwd;
Main include for ABI (AirBorneInterface).
struct pprz_autopilot autopilot
Global autopilot structure.
bool autopilot_get_motors_on(void)
get motors status
Hardware independent code for commands handling.
static struct uart_periph * dev
float G2_RW[EFF_MAT_COLS_NB]
float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]
#define FLOAT_ANGLE_NORMALIZE(_a)
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_zero(float *a, const int n)
a = 0
static void float_vect_copy(float *a, const float *b, const int n)
a = b
static void float_vect_diff(float *o, const float *a, const float *b, const int n)
o = a - b
static float float_vect_norm(const float *a, const int n)
||a||
#define FLOAT_VECT2_NORM(_v)
#define VECT2_DIFF(_c, _a, _b)
#define POS_FLOAT_OF_BFP(_ai)
#define SPEED_FLOAT_OF_BFP(_ai)
#define POS_BFP_OF_REAL(_af)
#define ACCEL_FLOAT_OF_BFP(_ai)
#define SPEED_BFP_OF_REAL(_af)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
static struct FloatEulers * stateGetNedToBodyEulersZxy_f(void)
Get vehicle body attitude ZXY euler angles (float).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static struct Int32Vect3 * stateGetAccelBody_i(void)
Get acceleration in Body coordinates (int).
static float stateGetAirspeed_f(void)
Get airspeed (float).
bool force_forward
forward flight for hybrid nav
Butterworth2LowPass accely_filt
struct FloatEulers eulers_zxy
state eulers in zxy order
float gi_unbounded_airspeed_sp
Integrated Navigation System interface.
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Simple first order low pass filter with bilinear transform.
struct SecondOrderLowPass Butterworth2LowPass
Second order Butterworth low pass filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
Init first order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, const float tau, const float sample_time, const float value)
Init a second order Butterworth filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, const float value)
Update second order Butterworth low pass filter state with a new value.
static float update_butterworth_4_low_pass(Butterworth4LowPass *filter, const float value)
Update fourth order Butterworth low pass filter state with a new value.
static void init_butterworth_4_low_pass(Butterworth4LowPass *filter, const float tau, const float sample_time, const float value)
Init a fourth order Butterworth filter.
Hardware independent API for actuators (servos, motor controllers).
#define NAV_HYBRID_MAX_AIRSPEED
float nav_hybrid_max_bank
float nav_max_acceleration_sp
float nav_hybrid_pos_gain
#define NAV_HYBRID_MAX_DECELERATION
Specific navigation functions for hybrid aircraft.
static void notch_filter_update(struct SecondOrderNotchFilter *filter, int32_t *input_signal, int32_t *output_signal)
Notch filter propagate.
static void notch_filter_init(struct SecondOrderNotchFilter *filter, float cutoff_frequency, float bandwidth, uint16_t sample_frequency)
Initialize second order notch filter.
static float w_approx(float p1, float p2, float p3, float rm_k)
Third Order to First Order Dynamics Approximation.
#define ONELOOP_ANDI_MAX_THETA
void init_poles(void)
Initialize Position of Poles.
struct PolePlacement p_pos_e
static float Wv_backup[ANDI_OUTPUTS]
static void reinit_LP(struct LP_t *LP, bool reinit)
void chirp_pos(float time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[])
Reference Model Definition for 3rd order system specific to positioning with bounds.
float act_dynamics[ANDI_NUM_ACT_TOT]
float ratio_u_un[ANDI_NUM_ACT_TOT]
static float Wu_backup[ANDI_NUM_ACT_TOT]
void init_filter(void)
Initialize the filters.
void reinit_controller(void)
Re-Init function of controller variables.
struct PolePlacement p_head_rm
void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop)
Function to generate the reference signals for the oneloop controller.
void oneloop_calc_model_disturbance(bool in_flight)
Function that calculates the model disturbance (Control Effort) The control effort is equalt to : EFF...
static void update_filter_on_type(struct LP_t *filter, float input)
Update a filter based on its type.
void oneloop_andi_enter(bool half_loop_sp, int ctrl_type)
Function that resets important values upon engaging Oneloop ANDI.
static Butterworth2LowPass airspeed_filt
static Butterworth2LowPass filt_veloc[3]
static void eul_of_acc(float a[3], float psi)
float n_array[ANDI_OUTPUTS]
static void init_all_LP(void)
Initialize all Low Pass Filters.
float act_max_norm[ANDI_NUM_ACT_TOT]
struct Gains3rdOrder k_att_rm
void float_euler_dot_of_rates_vec(float r[3], float e[3], float edot[3])
Attitude Euler to Rates Conversion Function ZYX.
float oneloop_andi_filt_cutoff_v
void oneloop_andi_bound_disturbance(void)
static Butterworth2LowPass u_filt[ANDI_NUM_ACT_TOT]
float act_dyn_ctrl[ANDI_NUM_ACT_TOT]
struct FloatEulers eulers_zxy_des
struct PolePlacement p_att_rm
static float k_rm_3_3_f(float omega_n, float zeta, float p1)
struct Oneloop_DynFilt_t mu_mR
void guidance_set_max_bank_angle(float max_bank)
Guidance limit setters called by rotwing_state (also implemented by guidance_indi_hybrid).
struct Gains3rdOrder k_att_e
float EFF_MAT_G[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]
void rm_1st_pos(float dt, float x_2d_ref[], float x_3d_ref[], float x_2d_des[], float k3_rm[], float x_3d_bound, int n)
Reference Model Definition for 3rd order system specific to positioning with bounds.
void oneloop_andi_propagate_filters(void)
Propagate the filters.
void guidance_set_max_climb_speed(float max_climb_speed_quad, float max_climb_speed_fwd)
struct Gains3rdOrder k_pos_e
bool actuator_is_servo[ANDI_NUM_ACT_TOT]
void float_vect_diff_euler(float err[3], float a[3], float b[3])
Calculate Error between two 3D euler arrays with the psi normalized.
float oneloop_andi_sideslip(void)
Function to calculate corrections for sideslip.
float num_thrusters_oneloop
static float act_dynamics_d[ANDI_NUM_ACT_TOT]
static float andi_u_n[ANDI_NUM_ACT_TOT]
float act_min_norm[ANDI_NUM_ACT_TOT]
#define ONELOOP_ANDI_DEBUG_MODE
float oneloop_andi_filt_cutoff_q
void dynFilter_init(struct Oneloop_DynFilt_t *mu, float varepsilon, float sigma)
void rm_3rd_attitude(float dt, float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x_des[3], bool ow_psi, float psi_overwrite[4], float k1_rm[3], float k2_rm[3], float k3_rm[3], struct OneloopStabilizationRef bounds)
Reference Model Definition for 3rd order system with attitude conversion functions.
float oneloop_andi_filt_cutoff_a
void oneloop_axis_effectiveness_calc(void)
static void send_wls_u_oneloop(struct transport_tx *trans, struct link_device *dev)
static float chirp_pos_j_ref(float delta_t, float f0, float k, float A)
Function to calculate the jerk reference during the chirp.
#define ONELOOP_ANDI_MAX_PHI
float oneloop_andi_sigma_min
void oneloop_from_nav(bool in_flight)
Function that maps navigation inputs to the oneloop controller for the generated autopilot.
float bound_v_from_a(float e_x, float v_bound, float a_bound)
Calculate velocity limit based on acceleration limit for a float From SUVAT we have that v^2 = u^2 + ...
struct PolePlacement p_alt_rm
void init_controller_gains(void)
Initialize Controller Gains FIXME: Calculate the gains dynamically for transition.
static void reinit_all_LP(bool reinit)
Reinitialize all the Low Pass Filters.
float oneloop_andi_sigma_max
static float positive_non_zero(float input)
Function to make sure that inputs are positive non zero vaues.
static void send_eff_mat_stab_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
float oneloop_andi_yaw_dist_limit
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed)
float andi_u[ANDI_NUM_ACT_TOT]
void calc_normalization(void)
Calculate Normalization of actuators and discrete actuator dynamics.
struct Gains3rdOrder k_pos_e_indi
void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v)
Main function that runs the controller and performs control allocation.
struct PolePlacement p_att_e
static void send_eff_mat_guid_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize)
struct PolePlacement p_alt_e
float bound_v_from_a_vect(float e_x[], float v_bound, float a_bound, int n)
Calculate velocity limit based on acceleration limit for a vector From SUVAT we have that v^2 = u^2 +...
void rm_2nd_pos(float dt, float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_d_des[], float k2_rm[], float k3_rm[], float x_2d_bound, float x_3d_bound, int n)
Reference Model Definition for 3rd order system specific to positioning with bounds.
void get_act_state_oneloop(void)
Function to reconstruct actuator state using first order dynamics.
struct PolePlacement p_roll_e
void ec_3rd_pos(float y_4d[], float x_des[], float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x[], float x_d[], float x_2d[], float k1_e[], float k2_e[], float k3_e[], float x_d_bound, float x_2d_bound, float fb[], int n)
Error Controller Definition for 3rd order system.
struct PolePlacement p_head_e
static float chirp_pos_p_ref(float delta_t, float f0, float k, float A)
Function to calculate the position reference during the chirp.
struct Oneloop_LP_t oneloop_andi_model_filt
static void init_LP(struct LP_t *LP, float fc)
Initialize the Low Pass Filter Struct.
float andi_du[ANDI_NUM_ACT_TOT]
static float chirp_pos_v_ref(float delta_t, float f0, float k, float A)
Function to calculate the velocity reference during the chirp.
static void send_wls_v_oneloop(struct transport_tx *trans, struct link_device *dev)
static float nav_target[3]
void err_nd(float err[], float a[], float b[], float k[], int n)
Calculate Scaled Error between two 3D arrays.
static void Pos_PID(float x_des[3], float x[3], float x_dot[3], float k_P, float k_I, float k_D, float a[3])
#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD
float oneloop_andi_dist_bound[ANDI_OUTPUTS]
void dynFilter_run(struct Oneloop_DynFilt_t *mu, float u_c, float sigma)
static void init_filter_on_type(struct LP_t *filter, float x0)
Initialize a filter based on its type.
struct OneloopGeneral oneloop_andi
struct Gains3rdOrder k_pos_rm
bool autopilot_in_flight_end_detection(bool motors_on UNUSED)
Quadplanes can still be in-flight with COMMAND_THRUST==0 and can even soar not descending in updrafts...
void guidance_set_max_descend_speed(float max_descend_speed_quad, float max_descend_speed_fwd)
static void reinit_LP_synchronous(struct LP_t *LP, struct LP_t *f, bool reinit)
Reinitialize Low Pass filter if new frequency setting or if forced.
float oneloop_andi_model[ANDI_OUTPUTS]
float m_array[ANDI_NUM_ACT_TOT]
void chirp_call(bool *chirp_on, bool *chirp_first_call, float *t_0_chirp, float *time_elapsed, float f0, float f1, float t_chirp, float A, int8_t n, float psi, float p_ref[], float v_ref[], float a_ref[], float j_ref[], float p_ref_0[])
void G1G2_oneloop(int ctrl_type)
Function that samples and scales the effectiveness matrix FIXME: make this function into a for loop t...
static void send_guidance_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
float act_max[ANDI_NUM_ACT_TOT]
void normalize_nu(void)
Function to normalize the pseudo control vector.
void rm_3rd_pos(float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], float x_3d_ref[], float x_des[], float k1_rm[], float k2_rm[], float k3_rm[], float x_d_bound, float x_2d_bound, float x_3d_bound, int n)
Reference Model Definition for 3rd order system specific to positioning with bounds.
void oneloop_andi_init(void)
Init function of Oneloop ANDI controller.
void float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3])
Attitude Rates to Euler Conversion Function ZYX.
float oneloop_andi_filt_cutoff_p
static float k_rm_1_3_f(float omega_n, float zeta, float p1)
Reference Model Gain Design.
struct OneloopStabilizationRef sta_bounds
float oneloop_andi_filt_cutoff_r
struct PolePlacement p_pos_rm
float ratio_vn_v[ANDI_OUTPUTS]
static float chirp_pos_a_ref(float delta_t, float f0, float k, float A)
Function to calculate the acceleration reference during the chirp.
#define ONELOOP_ANDI_MAX_BANK
float actuator_state_1l[ANDI_NUM_ACT_TOT]
void vect_bound_nd(float vect[], float bound, int n)
Scale a 3D array to within a 3D bound.
void err_sum_nd(float err[], float a[], float b[], float k[], float c[], int n)
Calculate Scaled Error between two 3D arrays.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
void integrate_nd(float dt, float a[], float a_dot[], int n)
Integrate in time 3D array.
static void send_oneloop_andi(struct transport_tx *trans, struct link_device *dev)
struct Gains3rdOrder k_att_e_indi
static void send_oneloop_debug(struct transport_tx *trans, struct link_device *dev)
float act_min[ANDI_NUM_ACT_TOT]
struct Oneloop_DynFilt_t mu_mL
void acc_body_bound(struct FloatVect2 *vect, float bound)
Scale a 3D array to within a 3D bound.
float * bwls_1l[ANDI_OUTPUTS]
static float k_rm_2_3_f(float omega_n, float zeta, float p1)
static float nav_target_new[3]
void ec_3rd_att(float y_4d[3], float x_des[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[3], float x_3d_ref[3], float x[3], float x_d[3], float x_2d[3], float k1_e[3], float k2_e[3], float k3_e[3], struct OneloopStabilizationRef bounds, float fb[3])
Error Controller Definition for 3rd order system specific to attitude.
static float u_pref[ANDI_NUM_ACT_TOT]
static void shape_vector(float a[3])
float oneloop_andi_filt_cutoff
float coupling_factor[ANDI_OUTPUTS]
#define CTRL_ANDI
Control types.
union LP_t::@324202153354356061211053020005360346147215120230 meas_filt
enum FilterType filter_type
Paparazzi floating point algebra.
vector in North East Down coordinates Units: meters
struct RadioControl radio_control
Generic interface for radio control modules.
static pprz_t radio_control_get(uint8_t idx)
Get a radio control channel value.
#define RADIO_ROLL
Redefining RADIO_* Do not use with radio.h (ppm rc).
struct RotorcraftNavigation nav
Rotorcraft navigation functions.
#define NAV_SETPOINT_MODE_SPEED
#define NAV_VERTICAL_MODE_CLIMB
#define NAV_VERTICAL_MODE_ALT
#define NAV_SETPOINT_MODE_POS
Nav setpoint modes these modes correspond to submodes defined by navigation routines to tell which se...
bool rotwing_state_hover_motors_running(void)
struct rotwing_state_t rotwing_state
@ ROTWING_STATE_FORCE_HOVER
static const ShellCommand commands[]
struct Stabilization stabilization
API to get/set the generic vehicle states.
uint8_t sys_id_doublet_running(void)
static float get_sys_time_float(void)
Get the time in seconds since startup.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
static const float scale[]
short int16_t
Typedef defining 16 bit short type.
signed char int8_t
Typedef defining 8 bit char type.
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup).
void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)