diff --git a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml index 60bc43d668..86f3f1699a 100644 --- a/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml +++ b/conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml @@ -219,7 +219,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -280,8 +297,8 @@ - - + + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml index 1cf9ae7201..5004266791 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop.xml @@ -221,7 +221,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -286,8 +303,8 @@ - - + + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml index 38044f59e2..4a77196bb0 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml @@ -49,7 +49,6 @@ - @@ -223,7 +222,24 @@ +
+ + + + + + + + + + + + + + + +
@@ -275,6 +291,8 @@
+ + @@ -291,8 +309,8 @@ - - + + diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml index c084012f24..47d8628160 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml @@ -44,6 +44,7 @@ + @@ -252,7 +253,6 @@ -
diff --git a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml index 19b1a6a441..70bbd141c3 100644 --- a/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml +++ b/conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml @@ -256,8 +256,8 @@ - - + + diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop.xml new file mode 100644 index 0000000000..1ba5906812 --- /dev/null +++ b/conf/airframes/tudelft/rotwing_v3g_oneloop.xml @@ -0,0 +1,390 @@ + + + + RotatingWingV3G for outdoor flight with INS EKF2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + Enter the PIC + Enter the PAC + Enter the GCS op + Goal of the flight + Location, airspace and weather + Check the RC battery + Check tail connection + Check wings taped and secured + Inspect airframe condition + Check attitude and heading + Airspeed sensor calibration + Put UAV on take-off location + Check flight plan + Switch to correct flight block + Switch on drone tag + Switch on camera + Announce flight to other airspace users + + +
+ + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + +
+ diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml new file mode 100644 index 0000000000..4cc3f48ccd --- /dev/null +++ b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack.xml @@ -0,0 +1,394 @@ + + + + RotatingWingV3G with optitrack and INS EKF2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + +
+
+ + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + Enter the PIC + Enter the PAC + Enter the GCS op + Goal of the flight + Location, airspace and weather + Check the RC battery + Check tail connection + Check wings taped and secured + Inspect airframe condition + Check attitude and heading + Airspeed sensor calibration + Put UAV on take-off location + Check flight plan + Switch to correct flight block + Switch on drone tag + Switch on camera + Announce flight to other airspace users + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + +
+ diff --git a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml index 57d4538da2..e75070c353 100644 --- a/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml @@ -36,12 +36,12 @@ - + @@ -194,25 +194,25 @@ - - - - + + + + - - - - + + + + - - - - - + + + + +
@@ -252,7 +252,6 @@ -
@@ -308,6 +307,8 @@
+ + @@ -322,10 +323,10 @@ - + - - + + diff --git a/conf/flight_plans/tudelft/oneloop_valkenburg.xml b/conf/flight_plans/tudelft/oneloop_valkenburg.xml index 7879814cd1..938b158f55 100644 --- a/conf/flight_plans/tudelft/oneloop_valkenburg.xml +++ b/conf/flight_plans/tudelft/oneloop_valkenburg.xml @@ -72,11 +72,11 @@ - - = IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Landed"/> diff --git a/conf/modules/eff_scheduling_rotwing_V2.xml b/conf/modules/eff_scheduling_rotwing_V2.xml index b54e7c3786..7e6abcbb74 100644 --- a/conf/modules/eff_scheduling_rotwing_V2.xml +++ b/conf/modules/eff_scheduling_rotwing_V2.xml @@ -22,6 +22,8 @@ + + diff --git a/conf/modules/oneloop_andi.xml b/conf/modules/oneloop_andi.xml index 5ba8d08d38..4909adfb0f 100644 --- a/conf/modules/oneloop_andi.xml +++ b/conf/modules/oneloop_andi.xml @@ -6,38 +6,47 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/telemetry/oneloop_telemetry.xml b/conf/telemetry/oneloop_telemetry.xml index 69add177da..4ec6d3422d 100644 --- a/conf/telemetry/oneloop_telemetry.xml +++ b/conf/telemetry/oneloop_telemetry.xml @@ -63,7 +63,8 @@ - + + @@ -124,7 +125,8 @@ - + + @@ -330,7 +332,8 @@ - + + diff --git a/conf/userconf/tudelft/RW_control_panel.xml b/conf/userconf/tudelft/RW_control_panel.xml index 5d8c7db428..620212c705 100644 --- a/conf/userconf/tudelft/RW_control_panel.xml +++ b/conf/userconf/tudelft/RW_control_panel.xml @@ -1124,6 +1124,16 @@ + + + + + + + + + + @@ -1448,7 +1458,8 @@ - + + @@ -1456,7 +1467,8 @@ - + + @@ -1466,7 +1478,8 @@ - + + @@ -1497,8 +1510,8 @@ - - + + @@ -1506,15 +1519,5 @@ - - - - - - - - - -
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 3f6f68c3b0..b35742abab 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -585,12 +585,12 @@ @@ -126,7 +127,7 @@ float oneloop_andi_filt_cutoff_v = ONELOOP_ANDI_FILT_CUTOFF_VEL; #else float oneloop_andi_filt_cutoff_v = 2.0; #endif - +PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_VEL) #ifdef ONELOOP_ANDI_FILT_CUTOFF_POS float oneloop_andi_filt_cutoff_pos = ONELOOP_ANDI_FILT_CUTOFF_POS; #else @@ -154,6 +155,10 @@ float oneloop_andi_filt_cutoff_r = ONELOOP_ANDI_FILT_CUTOFF_R; float oneloop_andi_filt_cutoff_r = 20.0; #endif +PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF); +PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_Q); +PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_P); +PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF_R); #ifndef MAX_R float max_r = RadOfDeg(120.0); #else @@ -238,10 +243,6 @@ float theta_pref_max = RadOfDeg(ONELOOP_THETA_PREF_MAX); #define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0 #endif -#ifndef ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD -#define ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD 10.0 -#endif - /* Declaration of Navigation Variables*/ #ifdef NAV_HYBRID_MAX_DECELERATION float max_a_nav = NAV_HYBRID_MAX_DECELERATION; @@ -267,20 +268,31 @@ float max_v_nav = NAV_HYBRID_MAX_AIRSPEED; // Consider implications of differenc float max_v_nav = 5.0; #endif float max_as = 19.0f; +float min_as = 0.0f; #ifdef NAV_HYBRID_MAX_SPEED_V float max_v_nav_v = NAV_HYBRID_MAX_SPEED_V; #else float max_v_nav_v = 1.5; #endif + #ifndef FWD_SIDESLIP_GAIN float fwd_sideslip_gain = 0.2; #else float fwd_sideslip_gain = FWD_SIDESLIP_GAIN; #endif +#ifndef ONELOOP_ANDI_WU_QUAD_MOTORS_FWD +float Wu_quad_motors_fwd = 6.0; +#else +float Wu_quad_motors_fwd = ONELOOP_ANDI_WU_QUAD_MOTORS_FWD; +#endif + + /* Define Section of the functions used in this module*/ void init_poles(void); +void init_poles_att(void); +void init_poles_pos(void); void calc_normalization(void); void normalize_nu(void); void G1G2_oneloop(int ctrl_type); @@ -310,6 +322,12 @@ float oneloop_andi_sideslip(void); void reshape_wind(void); 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[]); 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 init_cf2(struct CF2_t *cf, float fc); +void init_cf4(struct CF4_t *cf, float fc); +void init_all_cf(void); +void reinit_cf2(struct CF2_t *cf, bool reinit); +void reinit_cf4(struct CF4_t *cf, bool reinit); +void reinit_all_cf(bool reinit); /*Define general struct of the Oneloop ANDI controller*/ struct OneloopGeneral oneloop_andi; @@ -322,6 +340,7 @@ static float dt_1l = 1./PERIODIC_FREQUENCY; static float g = 9.81; // [m/s^2] Gravitational Acceleration float k_as = 2.0; int16_t temp_pitch = 0; +float gi_unbounded_airspeed_sp = 0.0; /* Oneloop Control Variables*/ float andi_u[ANDI_NUM_ACT_TOT]; @@ -386,12 +405,16 @@ static float Wv_backup[ANDI_OUTPUTS] = ONELOOP_ANDI_WV; #else static float Wv_backup[ANDI_OUTPUTS] = {1.0}; #endif + +#ifdef ONELOOP_ANDI_WU // {mF,mR,mB,mL,mP,de,dr,da,df,phi,theta} +static float Wu_backup[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_WU; +#else +static float Wu_backup[ANDI_NUM_ACT_TOT] = {1.0}; +#endif + /*Complementary Filter Variables*/ -static float model_pred[ANDI_OUTPUTS]; -static float model_pred_ar[3]; -static float ang_acc[3]; -static float ang_rate[3]; -static float lin_acc[3]; +struct Oneloop_CF_t cf; +static struct Oneloop_notch_t oneloop_notch; /*Chirp test Variables*/ bool chirp_on = false; @@ -433,19 +456,34 @@ float ratio_u_un[ANDI_NUM_ACT_TOT]; float ratio_vn_v[ANDI_OUTPUTS]; /*Filters Initialization*/ -static Butterworth2LowPass filt_accel_ned[3]; // Low pass filter for acceleration NED (1) - oneloop_andi_filt_cutoff_a (tau_a) -static Butterworth2LowPass filt_veloc_ned[3]; // Low pass filter for velocity NED - oneloop_andi_filt_cutoff_a (tau_a) -static Butterworth2LowPass rates_filt_bt[3]; // Low pass filter for angular rates - ONELOOP_ANDI_FILT_CUTOFF_P/Q/R -static Butterworth2LowPass model_pred_la_filt[3]; // Low pass filter for model prediction linear acceleration (1) - oneloop_andi_filt_cutoff_a (tau_a) -static Butterworth2LowPass ang_rate_lowpass_filters[3]; // Low pass filter for attitude derivative measurements - oneloop_andi_filt_cutoff (tau) -static Butterworth2LowPass model_pred_aa_filt[3]; // Low pass filter for model prediction angular acceleration - oneloop_andi_filt_cutoff (tau) -static Butterworth2LowPass model_pred_ar_filt[3]; // Low pass filter for model prediction angular rates - ONELOOP_ANDI_FILT_CUTOFF_P/Q/R +static Butterworth2LowPass filt_veloc_N; // Low pass filter for velocity NED - oneloop_andi_filt_cutoff_a (tau_a) +static Butterworth2LowPass filt_veloc_E; +static Butterworth2LowPass filt_veloc_D; static Butterworth2LowPass accely_filt; // Low pass filter for acceleration in y direction - oneloop_andi_filt_cutoff (tau) -static Butterworth2LowPass airspeed_filt; // Low pass filter for airspeed - oneloop_andi_filt_cutoff (tau) - +static Butterworth2LowPass airspeed_filt; // Low pass filter for airspeed - oneloop_andi_filt_cutoff (tau) /* Define messages of the module*/ #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" +// static void send_cf_oneloop(struct transport_tx *trans, struct link_device *dev) +// { +// float temp_cf_p[5] = {cf.p.model,cf.p.model_filt.o[0],cf.p.feedback,cf.p.feedback_filt.o[0],cf.p.out}; +// float temp_cf_q[5] = {cf.q.model,cf.q.model_filt.o[0],cf.q.feedback,cf.q.feedback_filt.o[0],cf.q.out}; +// float temp_cf_r[5] = {cf.r.model,cf.r.model_filt.o[0],cf.r.feedback,cf.r.feedback_filt.o[0],cf.r.out}; +// float temp_cf_p_dot[5] = {cf.p_dot.model,cf.p_dot.model_filt.lp2.o[0],cf.p_dot.feedback,cf.p_dot.feedback_filt.lp2.o[0],cf.p_dot.out}; +// float temp_cf_q_dot[5] = {cf.q_dot.model,cf.q_dot.model_filt.lp2.o[0],cf.q_dot.feedback,cf.q_dot.feedback_filt.lp2.o[0],cf.q_dot.out}; +// float temp_cf_r_dot[5] = {cf.r_dot.model,cf.r_dot.model_filt.o[0],cf.r_dot.feedback,cf.r_dot.feedback_filt.o[0],cf.r_dot.out}; +// //float temp_cf_ax[5] = {cf.ax.model,cf.ax.model_filt.o[0],cf.ax.feedback,cf.ax.feedback_filt.o[0],cf.ax.out}; +// //float temp_cf_ay[5] = {cf.ay.model,cf.ay.model_filt.o[0],cf.ay.feedback,cf.ay.feedback_filt.o[0],cf.ay.out}; +// float temp_cf_az[5] = {cf.az.model,cf.az.model_filt.o[0],cf.az.feedback,cf.az.feedback_filt.o[0],cf.az.out}; +// pprz_msg_send_COMPLEMENTARY_FILTER(trans, dev, AC_ID, +// 5, temp_cf_p, +// 5, temp_cf_q, +// 5, temp_cf_r, +// 5, temp_cf_p_dot, +// 5, temp_cf_q_dot, +// 5, temp_cf_r_dot, +// 5, temp_cf_az); +// } static void send_wls_v_oneloop(struct transport_tx *trans, struct link_device *dev) { send_wls_v("one", &WLS_one_p, trans, dev); @@ -458,9 +496,9 @@ static void send_eff_mat_stab_oneloop_andi(struct transport_tx *trans, struct li { float zero = 0.0; pprz_msg_send_EFF_MAT_STAB(trans, dev, AC_ID, - ANDI_NUM_ACT, EFF_MAT_G[3], - ANDI_NUM_ACT, EFF_MAT_G[4], - ANDI_NUM_ACT, EFF_MAT_G[5], + ANDI_NUM_ACT, EFF_MAT_RW[3], + ANDI_NUM_ACT, EFF_MAT_RW[4], + ANDI_NUM_ACT, EFF_MAT_RW[5], 1, &zero, 1, &zero); } @@ -468,9 +506,9 @@ static void send_eff_mat_stab_oneloop_andi(struct transport_tx *trans, struct li static void send_eff_mat_guid_oneloop_andi(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_EFF_MAT_GUID(trans, dev, AC_ID, - ANDI_NUM_ACT_TOT, EFF_MAT_G[0], - ANDI_NUM_ACT_TOT, EFF_MAT_G[1], - ANDI_NUM_ACT_TOT, EFF_MAT_G[2]); + ANDI_NUM_ACT_TOT, EFF_MAT_RW[0], + ANDI_NUM_ACT_TOT, EFF_MAT_RW[1], + ANDI_NUM_ACT_TOT, EFF_MAT_RW[2]); } static void send_oneloop_andi(struct transport_tx *trans, struct link_device *dev) { @@ -527,15 +565,17 @@ static void debug_vect(struct transport_tx *trans, struct link_device *dev, char static void send_oneloop_debug(struct transport_tx *trans, struct link_device *dev) { - float temp_debug_vect[7]; - temp_debug_vect[0] = model_pred[0]; - temp_debug_vect[1] = model_pred[1]; - temp_debug_vect[2] = model_pred[2]; - temp_debug_vect[3] = model_pred[3]; - temp_debug_vect[4] = model_pred[4]; - temp_debug_vect[5] = model_pred[5]; - temp_debug_vect[6] = RW.as; - debug_vect(trans, dev, "model_pred_as", temp_debug_vect, 7); + float temp_debug_vect[9]; + temp_debug_vect[0] = cf.ax.model; + temp_debug_vect[1] = cf.ay.model; + temp_debug_vect[2] = cf.az.model; + temp_debug_vect[3] = cf.p_dot.model; + temp_debug_vect[4] = cf.q_dot.model; + temp_debug_vect[5] = cf.r_dot.model; + temp_debug_vect[6] = cf.p.model; + temp_debug_vect[7] = cf.q.model; + temp_debug_vect[8] = cf.r.model; + debug_vect(trans, dev, "model_cf", temp_debug_vect, 9); } #endif @@ -677,7 +717,11 @@ void vect_bound_nd(float vect[], float bound, int n) { void acc_body_bound(struct FloatVect2* vect, float bound) { int n = 2; float v[2] = {vect->x, vect->y}; + float sign_v0 = (v[0] > 0.f) ? 1.f : (v[0] < 0.f) ? -1.f : 0.f; + float sign_v1 = (v[1] > 0.f) ? 1.f : (v[1] < 0.f) ? -1.f : 0.f; float norm = float_vect_norm(v,n); + v[0] = fabsf(v[0]); + v[1] = fabsf(v[1]); norm = positive_non_zero(norm); if((norm-bound) > FLT_EPSILON) { v[0] = Min(v[0], bound); @@ -685,8 +729,8 @@ void acc_body_bound(struct FloatVect2* vect, float bound) { acc_b_y_2 = positive_non_zero(acc_b_y_2); v[1] = sqrtf(acc_b_y_2); } - vect->x = v[0]; - vect->y = v[1]; + vect->x = sign_v0*v[0]; + vect->y = sign_v1*v[1]; } /** @brief Calculate velocity limit based on acceleration limit */ @@ -965,6 +1009,38 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ return 1.0/tao; } +/** + * @brief Calculate EC poles given RM poles + * @param p_rm Reference Model Pole (3 coincident poles) + * @param slow_pole Pole of the slowest dynamics + * @param k EC / RM ratio + * @param omega_n Natural Frequency + */ +static float ec_poles(float p_rm, float slow_pole, float k){ + p_rm = positive_non_zero(p_rm); + slow_pole = positive_non_zero(slow_pole); + k = positive_non_zero(k); + float omega_n = (2*p_rm*slow_pole*k)/(3*slow_pole-p_rm); + return omega_n; +} + +/** + * @brief Initialize Position of Poles + * + */ +void init_poles_att(void){ + float slow_pole = 22.0; // Pole of the slowest dynamics used in the attitude controller + p_att_e.omega_n = ec_poles(p_att_rm.omega_n, slow_pole, 1.28); + p_head_e.omega_n = ec_poles(p_head_rm.omega_n, slow_pole, 1.28); +} +void init_poles_pos(void){ + act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + float slow_pole = act_dynamics[COMMAND_ROLL]; // Pole of the slowest dynamics used in the position controller + p_pos_e.omega_n = ec_poles(p_pos_rm.omega_n,slow_pole,1.28);//1.0; + p_alt_e.omega_n = ec_poles(p_alt_rm.omega_n,slow_pole,1.28);//1.0;// 3.0; +} + /** * @brief Initialize Position of Poles * @@ -972,46 +1048,53 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ void init_poles(void){ // Attitude Controller Poles---------------------------------------------------------- - float slow_pole = 15.1; // Pole of the slowest dynamics used in the attitude controller + float slow_pole = 22.0; // Pole of the slowest dynamics used in the attitude controller - p_att_e.omega_n = 4.50; - p_att_e.zeta = 1.0; - p_att_e.p3 = slow_pole; - - p_att_rm.omega_n = 4.71; + p_att_rm.omega_n = 10.0; // 10.0 4.71 p_att_rm.zeta = 1.0; p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; - p_head_e.omega_n = 1.80; - p_head_e.zeta = 1.0; - p_head_e.p3 = slow_pole; + p_att_e.omega_n = ec_poles(p_att_rm.omega_n, slow_pole, 1.28); //4.50; + p_att_e.zeta = 1.0; + p_att_e.p3 = slow_pole; - p_head_rm.omega_n = 2.56; + p_head_rm.omega_n = 7.0; // 7.0 2.56 p_head_rm.zeta = 1.0; p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; + p_head_e.omega_n = ec_poles(p_head_rm.omega_n, slow_pole, 1.28); //1.80; + p_head_e.zeta = 1.0; + p_head_e.p3 = slow_pole; + act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); // Position Controller Poles---------------------------------------------------------- slow_pole = act_dynamics[COMMAND_ROLL]; // Pole of the slowest dynamics used in the position controller - - p_pos_e.omega_n = 1.0; - p_pos_e.zeta = 1.0; - p_pos_e.p3 = slow_pole; - - p_pos_rm.omega_n = 0.93; +#ifdef ONELOOP_ANDI_POLES_POS_OMEGA_N + p_pos_rm.omega_n = ONELOOP_ANDI_POLES_POS_OMEGA_N; +#else + p_pos_rm.omega_n = 0.93; //2.2; +#endif p_pos_rm.zeta = 1.0; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; - p_alt_e.omega_n = 1.0;// 3.0; - p_alt_e.zeta = 1.0; - p_alt_e.p3 = slow_pole; - - p_alt_rm.omega_n = 0.93; //1.93; + p_pos_e.omega_n = ec_poles(p_pos_rm.omega_n,slow_pole,1.28);//1.0; + p_pos_e.zeta = 1.0; + p_pos_e.p3 = slow_pole; +#ifdef ONELOOP_ANDI_POLES_ALT_OMEGA_N + p_alt_rm.omega_n = ONELOOP_ANDI_POLES_ALT_OMEGA_N; +#else + p_alt_rm.omega_n = 0.93; //2.2; +#endif p_alt_rm.zeta = 1.0; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; + + p_alt_e.omega_n = ec_poles(p_alt_rm.omega_n,slow_pole,1.28);//1.0;// 3.0; + p_alt_e.zeta = 1.0; + p_alt_e.p3 = slow_pole; } + /** * @brief Initialize Controller Gains * FIXME: Calculate the gains dynamically for transition @@ -1022,6 +1105,8 @@ void init_controller(void){ max_v_nav = nav_max_speed + max_wind; max_a_nav = nav_max_acceleration_sp; /*Some calculations in case new poles have been specified*/ + init_poles_att(); + init_poles_pos(); p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; @@ -1045,6 +1130,7 @@ void init_controller(void){ //printf("Attitude RM Gains: %f %f %f\n", k_att_rm.k1[0], k_att_rm.k2[0], k_att_rm.k3[0]); //printf("Attitude E Gains: %f %f %f\n", k_att_e.k1[0], k_att_e.k2[0], k_att_e.k3[0]); + //printf("Heading E Gains: %f %f %f\n", k_att_e.k1[2], k_att_e.k2[2], k_att_e.k3[2]); /*Heading Loop NAV*/ k_att_e.k1[2] = k_e_1_3_f_v2(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3); k_att_e.k2[2] = k_e_2_3_f_v2(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3); @@ -1116,70 +1202,193 @@ void init_controller(void){ //------------------------------------------------------------------------------------------ /*Approximated Dynamics*/ act_dynamics[COMMAND_ROLL] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[COMMAND_PITCH] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); } - - +// Complementary Filters Functions ----------------------------------------------------------- +/** @brief Initialize the Complementary Filters 2nd Order Butterworth */ +void init_cf2(struct CF2_t *cf, float fc){ + cf->freq = fc; + cf->freq_set = fc; + cf->tau = 1/(2*M_PI*cf->freq); + init_butterworth_2_low_pass(&cf->model_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_2_low_pass(&cf->feedback_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, 0.0); + cf->model = 0.0; + cf->feedback = 0.0; + cf->out = 0.0; +} +/** @brief Initialize the Complementary Filters 4th Order Butterworth*/ +void init_cf4(struct CF4_t *cf, float fc){ + cf->freq = fc; + cf->freq_set = fc; + cf->tau = 1/(2*M_PI*cf->freq); + init_butterworth_4_low_pass(&cf->model_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_4_low_pass(&cf->feedback_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, 0.0); + cf->model = 0.0; + cf->feedback = 0.0; + cf->out = 0.0; +} +/** @brief Initialize all the Complementary Filters */ +void init_all_cf(void){ + init_cf2(&cf.ax, oneloop_andi_filt_cutoff_a); + init_cf2(&cf.ay, oneloop_andi_filt_cutoff_a); + init_cf2(&cf.az, oneloop_andi_filt_cutoff_a); + init_cf4(&cf.p_dot, 2.0); + init_cf4(&cf.q_dot, 2.0); + init_cf2(&cf.r_dot, oneloop_andi_filt_cutoff); + init_cf2(&cf.p, oneloop_andi_filt_cutoff_p); + init_cf2(&cf.q, oneloop_andi_filt_cutoff_q); + init_cf2(&cf.r, oneloop_andi_filt_cutoff_r); +} +/** @brief Reinitialize 2nd Order CF if new frequency setting or if forced */ +void reinit_cf2(struct CF2_t *cf, bool reinit){ + if(cf->freq != cf->freq_set || reinit){ + cf->freq = cf->freq_set; + cf->tau = 1/(2*M_PI*cf->freq); + init_butterworth_2_low_pass(&cf->model_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, get_butterworth_2_low_pass(&cf->model_filt)); + init_butterworth_2_low_pass(&cf->feedback_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, get_butterworth_2_low_pass(&cf->feedback_filt)); + } +} +/** @brief Reinitialize 4th Order CF if new frequency setting or if forced */ +void reinit_cf4(struct CF4_t *cf, bool reinit){ + if(cf->freq != cf->freq_set || reinit){ + cf->freq = cf->freq_set; + cf->tau = 1/(2*M_PI*cf->freq); + init_butterworth_4_low_pass(&cf->model_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, get_butterworth_4_low_pass(&cf->model_filt)); + init_butterworth_4_low_pass(&cf->feedback_filt, cf->tau, 1.0 / PERIODIC_FREQUENCY, get_butterworth_4_low_pass(&cf->feedback_filt)); + } +} +/** @brief Reinitialize all the Complementary Filters */ +void reinit_all_cf(bool reinit){ + reinit_cf2(&cf.ax, reinit); + reinit_cf2(&cf.ay, reinit); + reinit_cf2(&cf.az, reinit); + reinit_cf4(&cf.p_dot, reinit); + reinit_cf4(&cf.q_dot, reinit); + reinit_cf2(&cf.r_dot, reinit); + reinit_cf2(&cf.p, reinit); + reinit_cf2(&cf.q, reinit); + reinit_cf2(&cf.r, reinit); +} +//------------------------------------------------------------------------------------------ /** @brief Initialize the filters */ void init_filter(void) { - //printf("oneloop andi filt cutoff PQR: %f %f %f\n", oneloop_andi_filt_cutoff_p,oneloop_andi_filt_cutoff_q,oneloop_andi_filt_cutoff_r); + // Store Notch filter values +#ifdef ONELOOP_ANDI_ROLL_STRUCTURAL_MODE_FREQ + oneloop_notch.roll.freq = ONELOOP_ANDI_ROLL_STRUCTURAL_MODE_FREQ, +#else + oneloop_notch.roll.freq = 8.46, +#endif +#ifdef ONELOOP_ANDI_PITCH_STRUCTURAL_MODE_FREQ + oneloop_notch.pitch.freq = ONELOOP_ANDI_PITCH_STRUCTURAL_MODE_FREQ, +#else + oneloop_notch.pitch.freq = 6.44, +#endif +#ifdef ONELOOP_ANDI_YAW_STRUCTURAL_MODE_FREQ + oneloop_notch.yaw.freq = ONELOOP_ANDI_YAW_STRUCTURAL_MODE_FREQ, +#else + oneloop_notch.yaw.freq = 17.90, +#endif + oneloop_notch.roll.bandwidth = 2.0; + oneloop_notch.pitch.bandwidth = 1.0; + oneloop_notch.yaw.bandwidth = 4.0; + // Initialize Notch filters + notch_filter_init(&oneloop_notch.roll.filter, oneloop_notch.roll.freq, oneloop_notch.roll.bandwidth, PERIODIC_FREQUENCY); + notch_filter_init(&oneloop_notch.pitch.filter, oneloop_notch.pitch.freq, oneloop_notch.pitch.bandwidth, PERIODIC_FREQUENCY); + notch_filter_init(&oneloop_notch.yaw.filter, oneloop_notch.yaw.freq, oneloop_notch.yaw.bandwidth, PERIODIC_FREQUENCY); + // Filtering of the velocities float tau = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff); - float tau_a = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff_a); float tau_v = 1.0 / (2.0 * M_PI * oneloop_andi_filt_cutoff_v); - float sample_time = 1.0 / PERIODIC_FREQUENCY; - - // Filtering of the Inputs with 3 dimensions (e.g. rates and accelerations) - int8_t i; - for (i = 0; i < 3; i++) { - init_butterworth_2_low_pass(&ang_rate_lowpass_filters[i], tau, sample_time, 0.0); - init_butterworth_2_low_pass(&model_pred_aa_filt[i], tau, sample_time, 0.0); - init_butterworth_2_low_pass(&filt_accel_ned[i], tau_a, sample_time, 0.0 ); - init_butterworth_2_low_pass(&filt_veloc_ned[i], tau_v, sample_time, 0.0 ); - init_butterworth_2_low_pass(&model_pred_la_filt[i], tau_a, sample_time, 0.0); - } - - // Init rate filter for feedback - float time_constants[3] = {1.0 / (2 * M_PI * oneloop_andi_filt_cutoff_p), 1.0 / (2 * M_PI * oneloop_andi_filt_cutoff_q), 1.0 / (2 * M_PI * oneloop_andi_filt_cutoff_r)}; - init_butterworth_2_low_pass(&rates_filt_bt[0], time_constants[0], sample_time, stateGetBodyRates_f()->p); - init_butterworth_2_low_pass(&rates_filt_bt[1], time_constants[1], sample_time, stateGetBodyRates_f()->q); - init_butterworth_2_low_pass(&rates_filt_bt[2], time_constants[2], sample_time, stateGetBodyRates_f()->r); - init_butterworth_2_low_pass(&model_pred_ar_filt[0], time_constants[0], sample_time, 0.0); - init_butterworth_2_low_pass(&model_pred_ar_filt[1], time_constants[1], sample_time, 0.0); - init_butterworth_2_low_pass(&model_pred_ar_filt[2], time_constants[2], sample_time, 0.0); - - // Some other filters - init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0); - init_butterworth_2_low_pass(&airspeed_filt, tau, sample_time, 0.0); + //printf("tau: %f tau_v: %f\n", tau, tau_v); + //printf("initializing filters\n"); + init_butterworth_2_low_pass(&filt_veloc_N, tau_v, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_2_low_pass(&filt_veloc_E, tau_v, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_2_low_pass(&filt_veloc_D, tau_v, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_2_low_pass(&accely_filt, tau, 1.0 / PERIODIC_FREQUENCY, 0.0); + init_butterworth_2_low_pass(&airspeed_filt, tau, 1.0 / PERIODIC_FREQUENCY, 0.0); } /** @brief Propagate the filters */ void oneloop_andi_propagate_filters(void) { + reinit_all_cf(false); struct NedCoor_f *accel = stateGetAccelNed_f(); struct NedCoor_f *veloc = stateGetSpeedNed_f(); + //printf("veloc: %f %f %f\n", veloc->x, veloc->y, veloc->z); struct FloatRates *body_rates = stateGetBodyRates_f(); - float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r}; - update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x); - update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y); - update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z); - update_butterworth_2_low_pass(&filt_veloc_ned[0], veloc->x); - update_butterworth_2_low_pass(&filt_veloc_ned[1], veloc->y); - update_butterworth_2_low_pass(&filt_veloc_ned[2], veloc->z); + // Store Feedbacks in the Complementary Filters + cf.ax.feedback = accel->x; + cf.ay.feedback = accel->y; + cf.az.feedback = accel->z; +//#define USE_ROLL_NOTCH +//#define USE_PITCH_NOTCH +#define USE_YAW_NOTCH + float temp_p_dot = (body_rates->p-cf.p.feedback)*PERIODIC_FREQUENCY; + float temp_q_dot = (body_rates->q-cf.q.feedback)*PERIODIC_FREQUENCY; + float temp_r_dot = (body_rates->r-cf.r.feedback)*PERIODIC_FREQUENCY; +#ifdef USE_ROLL_NOTCH + notch_filter_update(&oneloop_notch.roll.filter, &temp_p_dot, &cf.p_dot.feedback); +#else + cf.p_dot.feedback = temp_p_dot; +#endif +#ifdef USE_PITCH_NOTCH + notch_filter_update(&oneloop_notch.pitch.filter, &temp_q_dot, &cf.q_dot.feedback); +#else + cf.q_dot.feedback = temp_q_dot; +#endif +#ifdef USE_YAW_NOTCH + notch_filter_update(&oneloop_notch.yaw.filter, &temp_r_dot, &cf.r_dot.feedback); +#else + cf.r_dot.feedback = temp_r_dot; +#endif + cf.p.feedback = body_rates->p; + cf.q.feedback = body_rates->q; + cf.r.feedback = body_rates->r; + // Update Filters of Feedbacks + update_butterworth_2_low_pass(&cf.ax.feedback_filt, cf.ax.feedback); + update_butterworth_2_low_pass(&cf.ay.feedback_filt, cf.ay.feedback); + update_butterworth_2_low_pass(&cf.az.feedback_filt, cf.az.feedback); + update_butterworth_4_low_pass(&cf.p_dot.feedback_filt, cf.p_dot.feedback); + update_butterworth_4_low_pass(&cf.q_dot.feedback_filt, cf.q_dot.feedback); + update_butterworth_2_low_pass(&cf.r_dot.feedback_filt, cf.r_dot.feedback); + update_butterworth_2_low_pass(&cf.p.feedback_filt, cf.p.feedback); + update_butterworth_2_low_pass(&cf.q.feedback_filt, cf.q.feedback); + update_butterworth_2_low_pass(&cf.r.feedback_filt, cf.r.feedback); + //printf("veloc: %f %f %f\n", veloc->x, veloc->y, veloc->z); + update_butterworth_2_low_pass(&filt_veloc_N, veloc->x); + update_butterworth_2_low_pass(&filt_veloc_E, veloc->y); + update_butterworth_2_low_pass(&filt_veloc_D, veloc->z); + //printf("veloc_filt: %f %f %f\n", filt_veloc_N.o[0], filt_veloc_E.o[0], filt_veloc_D.o[0]); + // Calculate Model Predictions for Linear and Angular Accelerations Using the Effectiveness Matrix calc_model(); - int8_t i; - - for (i = 0; i < 3; i++) { - update_butterworth_2_low_pass(&model_pred_aa_filt[i], model_pred[3+i]); - update_butterworth_2_low_pass(&model_pred_la_filt[i], model_pred[i]); - update_butterworth_2_low_pass(&ang_rate_lowpass_filters[i], rate_vect[i]); - update_butterworth_2_low_pass(&rates_filt_bt[i], rate_vect[i]); - lin_acc[i] = filt_accel_ned[i].o[0] + model_pred[i] - model_pred_la_filt[i].o[0]; - ang_acc[i] = (ang_rate_lowpass_filters[i].o[0]- ang_rate_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY + model_pred[3+i] - model_pred_aa_filt[i].o[0]; - model_pred_ar[i] = model_pred_ar[i] + ang_acc[i] / PERIODIC_FREQUENCY; - update_butterworth_2_low_pass(&model_pred_ar_filt[i], model_pred_ar[i]); - ang_rate[i] = rates_filt_bt[i].o[0] + model_pred_ar[i] - model_pred_ar_filt[i].o[0]; - } + // Update Filters of Model Predictions for Linear and Angular Accelerations + update_butterworth_2_low_pass(&cf.ax.model_filt, cf.ax.model); + update_butterworth_2_low_pass(&cf.ay.model_filt, cf.ay.model); + update_butterworth_2_low_pass(&cf.az.model_filt, cf.az.model); + update_butterworth_4_low_pass(&cf.p_dot.model_filt, cf.p_dot.model); + update_butterworth_4_low_pass(&cf.q_dot.model_filt, cf.q_dot.model); + update_butterworth_2_low_pass(&cf.r_dot.model_filt, cf.r_dot.model); + // Calculate Complementary Filter outputs for Linear and Angular Accelerations + cf.ax.out = cf.ax.feedback_filt.o[0] + cf.ax.model - cf.ax.model_filt.o[0]; + cf.ay.out = cf.ay.feedback_filt.o[0] + cf.ay.model - cf.ay.model_filt.o[0]; + cf.az.out = cf.az.feedback_filt.o[0] + cf.az.model - cf.az.model_filt.o[0]; + //cf.p_dot.out = cf.p_dot.feedback_filt.o[0] + cf.p_dot.model - cf.p_dot.model_filt.o[0]; + cf.p_dot.out = cf.p_dot.feedback_filt.lp2.o[0] + cf.p_dot.model - cf.p_dot.model_filt.lp2.o[0]; + //cf.q_dot.out = cf.q_dot.feedback_filt.o[0] + cf.q_dot.model - cf.q_dot.model_filt.o[0]; + cf.q_dot.out = cf.q_dot.feedback_filt.lp2.o[0] + cf.q_dot.model - cf.q_dot.model_filt.lp2.o[0]; + cf.r_dot.out = cf.r_dot.feedback_filt.o[0] + cf.r_dot.model - cf.r_dot.model_filt.o[0]; + // Calculate Model Predictions for Angular Rates Using the output of the angular acceleration Complementary Filter + cf.p.model = cf.p.model + cf.p_dot.out / PERIODIC_FREQUENCY; + cf.q.model = cf.q.model + cf.q_dot.out / PERIODIC_FREQUENCY; + cf.r.model = cf.r.model + cf.r_dot.out / PERIODIC_FREQUENCY; + // Update Filters of Model Predictions for Angular Rates + update_butterworth_2_low_pass(&cf.p.model_filt, cf.p.model); + update_butterworth_2_low_pass(&cf.q.model_filt, cf.q.model); + update_butterworth_2_low_pass(&cf.r.model_filt, cf.r.model); + // Calculate Complementary Filter outputs for Angular Rates + cf.p.out = cf.p.feedback_filt.o[0] + cf.p.model - cf.p.model_filt.o[0]; + cf.q.out = cf.q.feedback_filt.o[0] + cf.q.model - cf.q.model_filt.o[0]; + cf.r.out = cf.r.feedback_filt.o[0] + cf.r.model - cf.r.model_filt.o[0]; // Propagate filter for sideslip correction float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y); update_butterworth_2_low_pass(&accely_filt, accely); @@ -1206,6 +1415,7 @@ void oneloop_andi_init(void) bwls_1l[i] = EFF_MAT_G[i]; } // Initialize filters and other variables + init_all_cf(); init_filter(); init_controller(); float_vect_zero(andi_u, ANDI_NUM_ACT_TOT); @@ -1218,16 +1428,11 @@ void oneloop_andi_init(void) float_vect_zero(oneloop_andi.sta_ref.att_3d,3); float_vect_zero(nu, ANDI_OUTPUTS); float_vect_zero(nu_n, ANDI_OUTPUTS); - float_vect_zero(ang_acc,3); - float_vect_zero(lin_acc,3); float_vect_zero(nav_target,3); float_vect_zero(nav_target_new,3); eulers_zxy_des.phi = 0.0; eulers_zxy_des.theta = 0.0; eulers_zxy_des.psi = 0.0; - float_vect_zero(model_pred,ANDI_OUTPUTS); - float_vect_zero(model_pred_ar,3); - // Start telemetry #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_oneloop_andi); @@ -1238,6 +1443,7 @@ void oneloop_andi_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_oneloop_debug); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_V, send_wls_v_oneloop); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WLS_U, send_wls_u_oneloop); + // register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_COMPLEMENTARY_FILTER, send_cf_oneloop); #endif } @@ -1260,6 +1466,7 @@ void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) for (i = 0; i < ANDI_OUTPUTS; i++) { bwls_1l[i] = EFF_MAT_G[i]; } + reinit_all_cf(true); init_filter(); init_controller(); /* Stabilization Reset */ @@ -1267,15 +1474,11 @@ void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) float_vect_zero(oneloop_andi.sta_ref.att_d,3); float_vect_zero(oneloop_andi.sta_ref.att_2d,3); float_vect_zero(oneloop_andi.sta_ref.att_3d,3); - float_vect_zero(ang_acc,3); - float_vect_zero(lin_acc,3); float_vect_zero(nav_target,3); float_vect_zero(nav_target_new,3); eulers_zxy_des.phi = 0.0; eulers_zxy_des.theta = 0.0; eulers_zxy_des.psi = psi_des_rad; - float_vect_zero(model_pred,ANDI_OUTPUTS); - float_vect_zero(model_pred_ar,3); /*Guidance Reset*/ } @@ -1453,24 +1656,23 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, oneloop_andi.sta_state.att[1] = eulers_zxy.theta; oneloop_andi.sta_state.att[2] = eulers_zxy.psi ; oneloop_andi_propagate_filters(); //needs to be after update of attitude vector - oneloop_andi.sta_state.att_d[0] = ang_rate[0];//rates_filt_bt[0].o[0]; - oneloop_andi.sta_state.att_d[1] = ang_rate[1];//rates_filt_bt[1].o[0]; - oneloop_andi.sta_state.att_d[2] = ang_rate[2];//rates_filt_bt[2].o[0]; - oneloop_andi.sta_state.att_2d[0] = ang_acc[0] ; - oneloop_andi.sta_state.att_2d[1] = ang_acc[1] ; - oneloop_andi.sta_state.att_2d[2] = ang_acc[2] ; + oneloop_andi.sta_state.att_d[0] = cf.p.out; + oneloop_andi.sta_state.att_d[1] = cf.q.out; + oneloop_andi.sta_state.att_d[2] = cf.r.out; + oneloop_andi.sta_state.att_2d[0] = cf.p_dot.out; + oneloop_andi.sta_state.att_2d[1] = cf.q_dot.out; + oneloop_andi.sta_state.att_2d[2] = cf.r_dot.out; // (2) Position related oneloop_andi.gui_state.pos[0] = stateGetPositionNed_f()->x; oneloop_andi.gui_state.pos[1] = stateGetPositionNed_f()->y; oneloop_andi.gui_state.pos[2] = stateGetPositionNed_f()->z; - oneloop_andi.gui_state.vel[0] = filt_veloc_ned[0].o[0]; - oneloop_andi.gui_state.vel[1] = filt_veloc_ned[1].o[0]; - oneloop_andi.gui_state.vel[2] = filt_veloc_ned[2].o[0]; - oneloop_andi.gui_state.acc[0] = lin_acc[0]; - oneloop_andi.gui_state.acc[1] = lin_acc[1]; - oneloop_andi.gui_state.acc[2] = lin_acc[2]; - - // Calculated feedforward signal for yaW CONTROL + oneloop_andi.gui_state.vel[0] = filt_veloc_N.o[0]; + oneloop_andi.gui_state.vel[1] = filt_veloc_E.o[0]; + oneloop_andi.gui_state.vel[2] = filt_veloc_D.o[0]; + oneloop_andi.gui_state.acc[0] = cf.ax.out; + oneloop_andi.gui_state.acc[1] = cf.ay.out; + oneloop_andi.gui_state.acc[2] = cf.az.out; + // Calculated feedforward signal for yaw control g2_ff = 0.0; for (i = 0; i < ANDI_NUM_ACT; i++) { @@ -1525,17 +1727,22 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, case COMMAND_MOTOR_FRONT: case COMMAND_MOTOR_RIGHT: case COMMAND_MOTOR_BACK: - case COMMAND_MOTOR_LEFT: + case COMMAND_MOTOR_LEFT: { + float skew_bound = RW.skew.deg; + Bound(skew_bound,70.0,90.0); + float Wu_sched = (Wu_quad_motors_fwd-Wu_backup[i])/(90.0-70.0)*(skew_bound-70)+Wu_backup[i]; + WLS_one_p.Wu[i] = Wu_sched; WLS_one_p.u_min[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i]; WLS_one_p.u_pref[i] = (u_pref[i] - actuator_state_1l[i])/ratio_u_un[i]; WLS_one_p.u_max[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i]; - if (rotwing_state_settings.hover_motors_active){ + if (rotwing_state.hover_motors_enabled){ WLS_one_p.u_max[i] = (act_max[i] - actuator_state_1l[i])/ratio_u_un[i]; } else { WLS_one_p.u_max[i] = (0.0 - actuator_state_1l[i])/ratio_u_un[i]; } break; + } case COMMAND_MOTOR_PUSHER: case COMMAND_RUDDER: WLS_one_p.u_min[i] = (act_min[i] - actuator_state_1l[i])/ratio_u_un[i]; @@ -1574,7 +1781,7 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, WLS_one_p.u_pref[i] = (u_pref[i] - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; break; case COMMAND_PITCH: - if (rotwing_state_settings.stall_protection){ + if (RW.skew.deg > 70.0){ WLS_one_p.u_min[i] = (RadOfDeg(-17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; WLS_one_p.u_max[i] = (RadOfDeg(17.0) - oneloop_andi.sta_state.att[i-ANDI_NUM_ACT])/ratio_u_un[i]; } else { @@ -1585,7 +1792,6 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, break; } } - // WLS Control Allocator normalize_nu(); wls_alloc(&WLS_one_p, bwls_1l, 0, 0, 10); @@ -1605,7 +1811,6 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, andi_u[COMMAND_ROLL] = andi_du[COMMAND_ROLL]; andi_u[COMMAND_PITCH] = andi_du[COMMAND_PITCH]; } - #ifdef COMMAND_MOTOR_PUSHER if ((half_loop)){ andi_u[COMMAND_MOTOR_PUSHER] = radio_control.values[RADIO_AUX4]; @@ -1619,9 +1824,11 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, /*Commit the actuator command*/ for (i = 0; i < ANDI_NUM_ACT; i++) { - //actuators_pprz[i] = (int16_t) andi_u[i]; commands[i] = (int16_t) andi_u[i]; } + if (rotwing_state.fail_pusher_motor){ + commands[COMMAND_MOTOR_PUSHER] = 0;//Min(1000,andi_u[COMMAND_MOTOR_PUSHER]); + } commands[COMMAND_THRUST] = (commands[COMMAND_MOTOR_FRONT] + commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_BACK] + commands[COMMAND_MOTOR_LEFT])/num_thrusters_oneloop; autopilot.throttle = commands[COMMAND_THRUST]; stabilization.cmd[COMMAND_THRUST] = commands[COMMAND_THRUST]; @@ -1693,12 +1900,13 @@ void G1G2_oneloop(int ctrl_type) { break; } int j = 0; + bool turn_quad_off = ((!rotwing_state.hover_motors_enabled || !rotwing_state_hover_motors_running()) && rotwing_state.state != ROTWING_STATE_FORCE_HOVER); for (j = 0; j < ANDI_OUTPUTS; j++) { EFF_MAT_G[j][i] = EFF_MAT_RW[j][i] * scaler * ratio_vn_v[j]; if (airspeed_filt.o[0] < ELE_MIN_AS && i == COMMAND_ELEVATOR){ EFF_MAT_G[j][i] = 0.0; } - if (!rotwing_state_settings.hover_motors_active && i < 4){ + if (turn_quad_off && i < 4){ EFF_MAT_G[j][i] = 0.0; } if (ctrl_off && i < 4 && j == 5){ //hack test @@ -1774,19 +1982,22 @@ void calc_model(void){ float T = RW.T / RW.m; // Thrust specific force. Minus gravity is a guesstimate. float P = RW.P / RW.m; // Pusher specific force - model_pred[0] = -(cpsi * stheta + ctheta * sphi * spsi) * T + (cpsi * ctheta - sphi * spsi * stheta) * P - sphi * spsi * L; - model_pred[1] = -(spsi * stheta - cpsi * ctheta * sphi) * T + (ctheta * spsi + cpsi * sphi * stheta) * P + cpsi * sphi * L; - model_pred[2] = g - cphi * ctheta * T - cphi * stheta * P - cphi * L; - for (i = 3; i < ANDI_OUTPUTS; i++){ // For loop for prediction of angular acceleration - model_pred[i] = 0.0; // + cf.ax.model = -(cpsi * stheta + ctheta * sphi * spsi) * T + (cpsi * ctheta - sphi * spsi * stheta) * P - sphi * spsi * L; + cf.ay.model = -(spsi * stheta - cpsi * ctheta * sphi) * T + (ctheta * spsi + cpsi * sphi * stheta) * P + cpsi * sphi * L; + cf.az.model = g - cphi * ctheta * T - cphi * stheta * P - cphi * L; + float model_pqr_dot[3] = {0.0, 0.0, 0.0}; + for (i = 0; i < 3; i++){ // For loop for prediction of angular acceleration for (j = 0; j < ANDI_NUM_ACT; j++){ if(j == COMMAND_ELEVATOR){ - model_pred[i] = model_pred[i] + (actuator_state_1l[j] - RW.ele_pref) * EFF_MAT_RW[i][j]; // Ele pref is incidence angle + model_pqr_dot[i] = model_pqr_dot[i] + (actuator_state_1l[j] - RW.ele_pref) * EFF_MAT_RW[i+3][j]; // Ele pref is incidence angle } else { - model_pred[i] = model_pred[i] + actuator_state_1l[j] * EFF_MAT_RW[i][j]; + model_pqr_dot[i] = model_pqr_dot[i] + actuator_state_1l[j] * EFF_MAT_RW[i+3][j]; } } } + cf.p_dot.model = model_pqr_dot[0]; + cf.q_dot.model = model_pqr_dot[1]; + cf.r_dot.model = model_pqr_dot[2]; } /** @brief Function that maps navigation inputs to the oneloop controller for the generated autopilot. */ @@ -1898,7 +2109,7 @@ void chirp_pos(float time_elapsed, float f0, float f1, float t_chirp, float A, i if (n < 0){ n = 0; } - // i think there should not be a problem with f1 being equal to f0 + // I think there should not be a problem with f1 being equal to f0 float k = (f1 - f0) / t_chirp; float p_ref_chirp = chirp_pos_p_ref(time_elapsed, f0, k, A); float v_ref_chirp = chirp_pos_v_ref(time_elapsed, f0, k, A); @@ -1983,7 +2194,6 @@ void reshape_wind(void) float spsi = sinf(psi); float airspeed = airspeed_filt.o[0]; struct FloatVect2 NT_v_NE = {nav_target[0], nav_target[1]}; // Nav target in North and East frame - //struct FloatVect2 NT_v_B = {0.0, 0.0}; // Nav target in body frame (Overwritten later) struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed }; struct FloatVect2 windspeed; struct FloatVect2 groundspeed = { oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.vel[1] }; @@ -1993,14 +2203,17 @@ void reshape_wind(void) VECT2_DIFF(windspeed, groundspeed, airspeed_v); // Wind speed in North and East frame VECT2_DIFF(des_as_NE, NT_v_NE, windspeed); // Desired airspeed in North and East frame float norm_des_as = FLOAT_VECT2_NORM(des_as_NE); + gi_unbounded_airspeed_sp = norm_des_as; + //Check if some minimum airspeed is desired (e.g. to prevent stall) + if (norm_des_as < min_as) { + norm_des_as = min_as; + } nav_target_new[0] = NT_v_NE.x; nav_target_new[1] = NT_v_NE.y; // if the desired airspeed is larger than the max airspeed or we are in force forward reshape gs des to cancel wind and fly at max airspeed - if ((norm_des_as > max_as)||(rotwing_state_settings.force_forward)){ - //printf("reshaping wind\n"); + if ((norm_des_as > max_as)||(force_forward)){ float groundspeed_factor = 0.0f; if (FLOAT_VECT2_NORM(windspeed) < max_as) { - //printf("we can achieve wind cancellation\n"); float av = NT_v_NE.x * NT_v_NE.x + NT_v_NE.y * NT_v_NE.y; // norm squared of nav target float bv = -2.f * (windspeed.x * NT_v_NE.x + windspeed.y * NT_v_NE.y); float cv = windspeed.x * windspeed.x + windspeed.y * windspeed.y - max_as * max_as; @@ -2020,7 +2233,7 @@ void reshape_wind(void) norm_des_as = FLOAT_VECT2_NORM(des_as_NE); // Recalculate norm of desired airspeed des_as_B.x = norm_des_as; // Desired airspeed in body x frame des_as_B.y = 0.0; // Desired airspeed in body y frame - if (((airspeed > ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD) && (norm_des_as > (ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD+2.0f)))|| (rotwing_state_settings.force_forward)){ + if (((airspeed > ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD) && (norm_des_as > (ONELOOP_ANDI_AIRSPEED_SWITCH_THRESHOLD+2.0f)))|| (force_forward)){ float delta_psi = atan2f(des_as_NE.y, des_as_NE.x) - psi; FLOAT_ANGLE_NORMALIZE(delta_psi); des_acc_B.y = delta_psi * 5.0;//gih_params.heading_bank_gain; @@ -2035,3 +2248,8 @@ void reshape_wind(void) vect_bound_nd(nav_target_new, max_a_nav, 2); } +void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed) { + min_as = min_airspeed; + max_as = max_airspeed; +} + diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h index 9a76793245..f38f61891c 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h @@ -30,6 +30,8 @@ #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h" #include "generated/airframe.h" +#include "filters/low_pass_filter.h" +#include "filters/notch_filter_float.h" #ifndef ANDI_NUM_ACT #define ANDI_NUM_ACT COMMANDS_NB_REAL @@ -72,6 +74,7 @@ extern struct FloatEulers eulers_zxy_des; extern float psi_des_rad; extern float k_as; extern float max_as; +extern float gi_unbounded_airspeed_sp; /*Chirp test Variables*/ extern bool chirp_on; @@ -147,6 +150,50 @@ struct Gains2ndOrder{ float k3; }; +struct CF4_t { + float tau; + float freq; + float freq_set; + float model; + Butterworth4LowPass model_filt; + float feedback; + Butterworth4LowPass feedback_filt; + float out; +}; +struct CF2_t { + float tau; + float freq; + float freq_set; + float model; + Butterworth2LowPass model_filt; + float feedback; + Butterworth2LowPass feedback_filt; + float out; +}; + +struct Oneloop_CF_t { + struct CF2_t p; + struct CF2_t q; + struct CF2_t r; + struct CF4_t p_dot; + struct CF4_t q_dot; + struct CF2_t r_dot; + struct CF2_t ax; + struct CF2_t ay; + struct CF2_t az; +}; +extern struct Oneloop_CF_t cf; +struct notch_axis_t{ + struct SecondOrderNotchFilter filter; + float freq; + float bandwidth; +}; +struct Oneloop_notch_t{ + struct notch_axis_t roll; + struct notch_axis_t pitch; + struct notch_axis_t yaw; +}; + extern int16_t temp_pitch; /*Declaration of Reference Model and Error Controller Gains*/ extern struct PolePlacement p_att_e; @@ -174,4 +221,5 @@ extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 P extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop); extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn); extern void oneloop_from_nav(bool in_flight); +extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed); #endif // ONELOOP_ANDI_H diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c index 93d09b01c1..918b76bfd1 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c +++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.c @@ -67,7 +67,8 @@ float actuator_state_filt_vect[EFF_MAT_COLS_NB] = {0}; float G2_RW[EFF_MAT_COLS_NB] = {0};//ROTWING_EFF_SCHED_G2; //scaled by RW_G_SCALE float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0};//{ROTWING_EFF_SCHED_G1_ZERO, ROTWING_EFF_SCHED_G1_ZERO, ROTWING_EFF_SCHED_G1_THRUST, ROTWING_EFF_SCHED_G1_ROLL, ROTWING_EFF_SCHED_G1_PITCH, ROTWING_EFF_SCHED_G1_YAW}; //scaled by RW_G_SCALE float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0}; -static float flt_cut = 1.0e-4; +static float flt_cut_ap = 2.0e-3; +static float flt_cut = 1.0e-4; struct FloatEulers eulers_zxy_RW_EFF; static Butterworth2LowPass skew_filt; @@ -75,6 +76,8 @@ static Butterworth2LowPass skew_filt; bool airspeed_fake_on = false; float airspeed_fake = 0.0; float ele_eff = 19.36; // (0.88*22.0); +float roll_eff = 5.5 ;//3.835; +float yaw_eff = 0.390; float ele_min = 0.0; /* Define Forces and Moments tructs for each actuator*/ struct RW_Model RW; @@ -107,6 +110,8 @@ static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *po } } +#include "generated/modules.h" +PRINT_CONFIG_VAR(EFF_SCHEDULING_ROTWING_PERIODIC_FREQ) void eff_scheduling_rotwing_init(void) { init_RW_Model(); @@ -130,22 +135,22 @@ void init_RW_Model(void) RW.m = 6.670; // [kg] // Motor Front RW.mF.dFdu = 3.835 / RW_G_SCALE; // [N / pprz] - RW.mF.dMdu = 0.390 / RW_G_SCALE; // [Nm / pprz] + RW.mF.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz] RW.mF.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz] - RW.mF.l = 0.423 ; // [m] + RW.mF.l = 0.423 ; // [m] 435 // Motor Right - RW.mR.dFdu = 3.835 / RW_G_SCALE; // [N / pprz] - RW.mR.dMdu = 0.390 / RW_G_SCALE; // [Nm / pprz] + RW.mR.dFdu = roll_eff / RW_G_SCALE; // [N / pprz] + RW.mR.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz] RW.mR.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz] - RW.mR.l = 0.408 ; // [m] + RW.mR.l = 0.408 ; // [m] 375 // Motor Back RW.mB.dFdu = 3.835 / RW_G_SCALE; // [N / pprz] - RW.mB.dMdu = 0.390 / RW_G_SCALE; // [Nm / pprz] + RW.mB.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz] RW.mB.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz] RW.mB.l = 0.423 ; // [m] // Motor Left - RW.mL.dFdu = 3.835 / RW_G_SCALE; // [N / pprz] - RW.mL.dMdu = 0.390 / RW_G_SCALE; // [Nm / pprz] + RW.mL.dFdu = roll_eff / RW_G_SCALE; // [N / pprz] + RW.mL.dMdu = yaw_eff / RW_G_SCALE; // [Nm / pprz] RW.mL.dMdud = 0.020 / RW_G_SCALE; // [Nm / pprz] RW.mL.l = 0.408 ; // [m] // Motor Pusher @@ -353,8 +358,21 @@ void sum_EFF_MAT_RW(void) { for (i = 0; i < EFF_MAT_ROWS_NB; i++) { for(j = 0; j < EFF_MAT_COLS_NB; j++) { float abs = fabs(EFF_MAT_RW[i][j]); - if (abs < flt_cut) { - EFF_MAT_RW[i][j] = 0.0; + switch (i) { + case (RW_aN): + case (RW_aE): + case (RW_aD): + case (RW_aq): + case (RW_ar): + if (abs < flt_cut) { + EFF_MAT_RW[i][j] = 0.0; + } + break; + case (RW_ap): + if (abs < flt_cut_ap) { + EFF_MAT_RW[i][j] = 0.0; + } + break; } } } diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.h b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.h index 2490895e79..bc0bda5b79 100644 --- a/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.h +++ b/sw/airborne/modules/ctrl/eff_scheduling_rotwing_V2.h @@ -187,6 +187,8 @@ struct RW_Model{ extern bool airspeed_fake_on; extern float airspeed_fake; extern float ele_eff; +extern float roll_eff; +extern float yaw_eff; extern float ele_min; extern float rotation_angle_setpoint_deg; diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c index 115b23087c..054304aac8 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c @@ -265,7 +265,7 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius) VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center); // direction of rotation - float sign_radius = radius > 0.f ? 1.f : -1.f; + float sign_radius = (radius > 0.f) ? 1.f : (radius < 0.f) ? -1.f : 0.f; // absolute radius float abs_radius = fabsf(radius); #if NAV_HYBRID_LIMIT_CIRCLE_RADIUS @@ -315,7 +315,6 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius) VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f()); float_vect2_normalize(&speed_unit); VECT2_SMUL(nav.speed, speed_unit, desired_speed); - nav_rotorcraft_base.circle.center = *wp_center; nav_rotorcraft_base.circle.radius = sign_radius * abs_radius; nav.horizontal_mode = NAV_HORIZONTAL_MODE_CIRCLE; diff --git a/sw/airborne/modules/rotwing_drone/wing_rotation_adc_sensor.h b/sw/airborne/modules/rotwing_drone/wing_rotation_adc_sensor.h index ac5d6d0657..70d782b055 100644 --- a/sw/airborne/modules/rotwing_drone/wing_rotation_adc_sensor.h +++ b/sw/airborne/modules/rotwing_drone/wing_rotation_adc_sensor.h @@ -26,7 +26,6 @@ #ifndef WING_ROTATION_ADC_SENSOR_H #define WING_ROTATION_ADC_SENSOR_H -extern float adc_wing_rotation_extern; extern void wing_rotation_adc_init(void); extern void wing_rotation_adc_to_deg(void); diff --git a/sw/include/std.h b/sw/include/std.h index 30174444ac..16523b6fbf 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -65,6 +65,10 @@ typedef uint8_t unit_t; #define M_PI 3.14159265358979323846 #endif +#ifndef M_PI_6 +#define M_PI_6 (M_PI/6) +#endif + #ifndef M_PI_4 #define M_PI_4 (M_PI/4) #endif