From 8c5d5669f1ed2f6bcad8198d2864af6112270567 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Mon, 8 Apr 2024 18:10:10 +0200 Subject: [PATCH 01/14] initial commit --- .../tudelft/rot_wing_v3c_oneloop.xml | 310 +++++ ...ot_wing_v3c_oneloop_optitrack_ext_pose.xml | 314 +++++ .../rot_wing_v3c_oneloop_simulation.xml | 278 +++++ conf/autopilot/rotorcraft_oneloop.xml | 2 +- .../rotorcraft_oneloop_andi_indi.xml | 191 +++ .../rotorcraft_oneloop_with_backup.xml | 14 +- .../tudelft/oneloop_valkenburg.xml | 10 +- conf/modules/ins_ext_pose.xml | 1 + conf/modules/oneloop_andi.xml | 37 +- .../jsbsim/aircraft/rotwingv3c_SI.xml | 629 ++++++++++ .../rotorcraft/guidance/guidance_oneloop.c | 2 +- .../rotorcraft/oneloop/oneloop_andi.c | 1043 ++++++++++++----- .../rotorcraft/oneloop/oneloop_andi.h | 20 +- .../stabilization/stabilization_oneloop.c | 2 +- .../modules/nav/nav_rotorcraft_hybrid.c | 21 +- .../modules/nav/nav_rotorcraft_hybrid.h | 2 +- 16 files changed, 2551 insertions(+), 325 deletions(-) create mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop.xml create mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml create mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml create mode 100644 conf/autopilot/rotorcraft_oneloop_andi_indi.xml create mode 100644 conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml new file mode 100644 index 00000000000..b20dd4d0864 --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -0,0 +1,310 @@ + + + + RotatingWingV3C for outdoor flight with INS EKF2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + +
+ diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml new file mode 100644 index 00000000000..368b7ed4716 --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml @@ -0,0 +1,314 @@ + + + + RotatingWingV3C with optitrack and INS ext pose + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + +
+ diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml new file mode 100644 index 00000000000..4de976a5aac --- /dev/null +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml @@ -0,0 +1,278 @@ + + + + RotatingWingV3C for simulation + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + +
+
+ + + + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + +
+ +
+ + + + + + +
+ diff --git a/conf/autopilot/rotorcraft_oneloop.xml b/conf/autopilot/rotorcraft_oneloop.xml index 1b8efa78224..25b86c59db7 100644 --- a/conf/autopilot/rotorcraft_oneloop.xml +++ b/conf/autopilot/rotorcraft_oneloop.xml @@ -116,7 +116,7 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + + + diff --git a/conf/flight_plans/tudelft/oneloop_valkenburg.xml b/conf/flight_plans/tudelft/oneloop_valkenburg.xml index 42721971f34..65ce2e0a348 100644 --- a/conf/flight_plans/tudelft/oneloop_valkenburg.xml +++ b/conf/flight_plans/tudelft/oneloop_valkenburg.xml @@ -1,6 +1,6 @@ - +
#include "autopilot.h" #include "modules/datalink/datalink.h" @@ -9,15 +9,15 @@ #include "modules/ahrs/ahrs.h"
- + - + - - + + diff --git a/conf/modules/ins_ext_pose.xml b/conf/modules/ins_ext_pose.xml index b2df677b49c..b59d48be4b8 100644 --- a/conf/modules/ins_ext_pose.xml +++ b/conf/modules/ins_ext_pose.xml @@ -27,6 +27,7 @@ + diff --git a/conf/modules/oneloop_andi.xml b/conf/modules/oneloop_andi.xml index 5650557a907..41287dd6fb9 100644 --- a/conf/modules/oneloop_andi.xml +++ b/conf/modules/oneloop_andi.xml @@ -6,15 +6,34 @@ - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml new file mode 100644 index 00000000000..9038387245f --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml @@ -0,0 +1,629 @@ + + + + + + Tomaso De Ponti + 07-03-2017 + Version 0.9 - beta + RW3C with actuator dynamics + + + + 0 + 0 + 0 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.126323 + 1.064109 + 1.00000 + 0.0 + 0.0 + 0.0 + 7.97 + + 0 + 0 + 0 + + + + + + + -0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.15 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.15 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + + + + fcs/front_motor + 10.1 + fcs/front_motor_lag + + + fcs/right_motor + 10.1 + fcs/right_motor_lag + + + fcs/back_motor + 10.1 + fcs/back_motor_lag + + + fcs/left_motor + 10.1 + fcs/left_motor_lag + + + fcs/pusher + 24.07 + fcs/pusher_lag + + + + + fcs/front_motor + 10.1 + fcs/front_motor_d + + + fcs/right_motor + 10.1 + fcs/right_motor_d + + + fcs/back_motor + 10.1 + fcs/back_motor_d + + + fcs/left_motor + 10.1 + fcs/left_motor_d + + + + + + + fcs/front_motor + fcs/front_motor_lag + fcs/front_motor_d + fcs/right_motor + fcs/right_motor_lag + fcs/right_motor_d + fcs/back_motor + fcs/back_motor_lag + fcs/back_motor_d + fcs/left_motor + fcs/left_motor_lag + fcs/left_motor_d + + fcs/pusher + fcs/pusher_lag + + + + + + + + fcs/front_motor_lag + 8.69 + + + + -0.423 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/right_motor_lag + 8.69 + + + + 0 + 0.408 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/back_motor_lag + 8.69 + + + + 0.423 + 0 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/left_motor_lag + 8.69 + + + + 0 + -0.408 + 0 + + + 0 + 0 + -1 + + + + + + + fcs/pusher_lag + 36.96 + + + + 0 + 0 + 0 + + + 1 + 0 + 0 + + + + + + + + fcs/front_motor_lag + 10.0 + + + + 0 + 0.192 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/front_motor_lag + 10.0 + + + + 0 + -0.192 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/right_motor_lag + 10.0 + + + + 0 + -0.192 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/right_motor_lag + 10.0 + + + + 0 + 0.192 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/back_motor_lag + 10.0 + + + + 0 + 0.192 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/back_motor_lag + 10.0 + + + + 0 + -0.192 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/left_motor_lag + 10.0 + + + + 0 + -0.192 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/left_motor_lag + 10.0 + + + + 0 + 0.192 + 0 + + + -1 + 0 + 0 + + + + + + + + fcs/front_motor_d + 10.0 + + + + 0 + 0.0096 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/front_motor_d + 10.0 + + + + 0 + -0.0096 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/right_motor_d + 10.0 + + + + 0 + -0.0096 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/right_motor_d + 10.0 + + + + 0 + 0.0096 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/back_motor_d + 10.0 + + + + 0 + 0.0096 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/back_motor_d + 10.0 + + + + 0 + -0.0096 + 0 + + + -1 + 0 + 0 + + + + + + + fcs/left_motor_d + 10.0 + + + + 0 + -0.0096 + 0 + + + 1 + 0 + 0 + + + + + + + fcs/left_motor_d + 10.0 + + + + 0 + 0.0096 + 0 + + + -1 + 0 + 0 + + + + + + + + + + + + Drag + + aero/qbar-psf + 47.9 + 0.0151 + 0.224808943 + + + + + + + Side force due to beta + + aero/qbar-psf + metrics/Sw-sqft + aero/beta-rad + -4 + + + + + + + \ No newline at end of file diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c index 9106e42b4d9..f88af1b7279 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c @@ -33,7 +33,7 @@ void guidance_h_run_enter(void) { - oneloop_andi_enter(false); + oneloop_andi_enter(false, CTRL_ANDI); } void guidance_v_run_enter(void) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index 8ed77e304f6..e1d12f39c65 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -82,17 +82,17 @@ #include "generated/airframe.h" #include "modules/radio_control/radio_control.h" #include "modules/actuators/actuators.h" -#ifdef STABILIZATION_ANDI_ROTWING_V3A -#include "modules/rot_wing_drone/wing_rotation_controller_v3a.h" -#endif #include "modules/core/abi.h" #include "filters/low_pass_filter.h" #include "math/wls/wls_alloc.h" #include "modules/nav/nav_rotorcraft_hybrid.h" #include "firmwares/rotorcraft/navigation.h" - - +#include "modules/rot_wing_drone/rotwing_state.h" #include +#if INS_EXT_POSE +#include "modules/ins/ins_ext_pose.h" +#endif +//#include "nps/nps_fdm.h" // Number of real actuators (e.g. motors, servos) #ifndef ANDI_NUM_ACT @@ -123,6 +123,10 @@ float num_thrusters_oneloop = 4.0; // Number of motors used for thrust float num_thrusters_oneloop = ONELOOP_ANDI_NUM_THRUSTERS; #endif +#ifndef ONELOOP_ANDI_SCHEDULING +#define ONELOOP_ANDI_SCHEDULING FALSE +#endif + #ifdef ONELOOP_ANDI_FILT_CUTOFF float oneloop_andi_filt_cutoff = ONELOOP_ANDI_FILT_CUTOFF; #else @@ -142,27 +146,36 @@ float oneloop_andi_filt_cutoff_v = 2.0; #endif #ifdef ONELOOP_ANDI_FILT_CUTOFF_POS -float oneloop_andi_filt_cutoff_p = ONELOOP_ANDI_FILT_CUTOFF_POS; +float oneloop_andi_filt_cutoff_pos = ONELOOP_ANDI_FILT_CUTOFF_POS; #else -float oneloop_andi_filt_cutoff_p = 2.0; +float oneloop_andi_filt_cutoff_pos = 2.0; #endif #ifdef ONELOOP_ANDI_FILT_CUTOFF_P #define ONELOOP_ANDI_FILTER_ROLL_RATE TRUE +float oneloop_andi_filt_cutoff_p = ONELOOP_ANDI_FILT_CUTOFF_P; #else -#define ONELOOP_ANDI_FILT_CUTOFF_P 20.0 +float oneloop_andi_filt_cutoff_p = 20.0; #endif #ifdef ONELOOP_ANDI_FILT_CUTOFF_Q #define ONELOOP_ANDI_FILTER_PITCH_RATE TRUE +float oneloop_andi_filt_cutoff_q = ONELOOP_ANDI_FILT_CUTOFF_Q; #else -#define ONELOOP_ANDI_FILT_CUTOFF_Q 20.0 +float oneloop_andi_filt_cutoff_q = 20.0; #endif #ifdef ONELOOP_ANDI_FILT_CUTOFF_R #define ONELOOP_ANDI_FILTER_YAW_RATE TRUE +float oneloop_andi_filt_cutoff_r = ONELOOP_ANDI_FILT_CUTOFF_R; +#else +float oneloop_andi_filt_cutoff_r = 20.0; +#endif + +#ifndef MAX_R +float max_r = RadOfDeg(120.0); #else -#define ONELOOP_ANDI_FILT_CUTOFF_R 20.0 +float max_r = RadOfDeg(MAX_R); #endif #ifdef ONELOOP_ANDI_ACT_IS_SERVO @@ -241,6 +254,26 @@ static float u_pref[ANDI_NUM_ACT_TOT] = {0.0}; #define ONELOOP_ANDI_PUSHER_IDX 4 #endif +// Assume phi and theta are the first actuators after the real ones unless otherwise specified +#ifndef ONELOOP_ANDI_PHI_IDX +#define ONELOOP_ANDI_PHI_IDX ANDI_NUM_ACT +#endif + +#define ONELOOP_ANDI_MAX_BANK act_max[ONELOOP_ANDI_PHI_IDX] // assuming abs of max and min is the same +#define ONELOOP_ANDI_MAX_PHI act_max[ONELOOP_ANDI_PHI_IDX] // assuming abs of max and min is the same + +#ifndef ONELOOP_ANDI_THETA_IDX +#define ONELOOP_ANDI_THETA_IDX ANDI_NUM_ACT+1 +#endif + +#define ONELOOP_ANDI_MAX_THETA act_max[ONELOOP_ANDI_THETA_IDX] // assuming abs of max and min is the same + +#ifndef ONELOOP_THETA_PREF_MAX +float theta_pref_max = RadOfDeg(20.0); +#else +float theta_pref_max = RadOfDeg(ONELOOP_THETA_PREF_MAX); +#endif + #if ANDI_NUM_ACT_TOT != WLS_N_U #error Matrix-WLS_N_U is not equal to the number of actuators: define WLS_N_U == ANDI_NUM_ACT_TOT in airframe file #define WLS_N_U == ANDI_NUM_ACT_TOT @@ -250,10 +283,9 @@ static float u_pref[ANDI_NUM_ACT_TOT] = {0.0}; #define WLS_N_V == ANDI_OUTPUTS #endif - /* Declaration of Navigation Variables*/ -#ifdef ONELOOP_MAX_ACC -float max_a_nav = ONELOOP_MAX_ACC; +#ifdef NAV_HYBRID_MAX_DECELERATION +float max_a_nav = NAV_HYBRID_MAX_DECELERATION; #else float max_a_nav = 4.0; // (35[N]/6.5[Kg]) = 5.38[m/s2] [0.8 SF] #endif @@ -264,56 +296,22 @@ float max_j_nav = ONELOOP_MAX_JERK; float max_j_nav = 500.0; // Pusher Test shows erros above 2[Hz] ramp commands [0.6 SF] #endif -#ifdef ONELOOP_MAX_AIRSPEED -float max_v_nav = ONELOOP_MAX_AIRSPEED; // Consider implications of difference Ground speed and airspeed +#ifdef NAV_HYBRID_MAX_AIRSPEED +float max_v_nav = NAV_HYBRID_MAX_AIRSPEED; // Consider implications of difference Ground speed and airspeed #else float max_v_nav = 5.0; #endif - -// DELETE ONCE NAV HYBRID IS MADE COMPATIBLE WITH ANY GUIDANCE/////////////////////////////////////////// -#ifndef GUIDANCE_INDI_SPEED_GAIN -#define GUIDANCE_INDI_SPEED_GAIN 1.8 -#define GUIDANCE_INDI_SPEED_GAINZ 1.8 -#endif - -#ifndef GUIDANCE_INDI_POS_GAIN -#define GUIDANCE_INDI_POS_GAIN 0.5 -#define GUIDANCE_INDI_POS_GAINZ 0.5 -#endif - -#ifndef GUIDANCE_INDI_LIFTD_ASQ -#define GUIDANCE_INDI_LIFTD_ASQ 0.20 -#endif - -/* If lift effectiveness at low airspeed not defined, - * just make one interpolation segment that connects to - * the quadratic part from 12 m/s onward - */ -#ifndef GUIDANCE_INDI_LIFTD_P50 -#define GUIDANCE_INDI_LIFTD_P80 (GUIDANCE_INDI_LIFTD_ASQ*12*12) -#define GUIDANCE_INDI_LIFTD_P50 (GUIDANCE_INDI_LIFTD_P80/2) +#ifndef FWD_SIDESLIP_GAIN +float fwd_sideslip_gain = 1.0; +#else +float fwd_sideslip_gain = FWD_SIDESLIP_GAIN; #endif -struct guidance_indi_hybrid_params gih_params = { - .pos_gain = GUIDANCE_INDI_POS_GAIN, - .pos_gainz = GUIDANCE_INDI_POS_GAINZ, - - .speed_gain = GUIDANCE_INDI_SPEED_GAIN, - .speed_gainz = GUIDANCE_INDI_SPEED_GAINZ, - - .heading_bank_gain = 5.0, - .liftd_asq = GUIDANCE_INDI_LIFTD_ASQ, // coefficient of airspeed squared - .liftd_p80 = GUIDANCE_INDI_LIFTD_P80, - .liftd_p50 = GUIDANCE_INDI_LIFTD_P50, -}; -bool force_forward = false; -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - /* Define Section of the functions used in this module*/ void init_poles(void); void calc_normalization(void); -void sum_g1g2_1l(void); +void sum_g1g2_1l(int ctrl_type); void get_act_state_oneloop(void); void oneloop_andi_propagate_filters(void); void init_filter(void); @@ -331,7 +329,105 @@ void rm_3rd_pos(float dt, float x_ref[], float x_d_ref[], float x_2d_ref[], flo 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); 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); void ec_3rd_att(float y_4d[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]); -void calc_model(void); +void calc_model(int ctrl_type); +float oneloop_andi_sideslip(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[]); + +/*Define general struct of the Oneloop ANDI controller*/ +struct OneloopGeneral oneloop_andi; + +/* Oneloop Misc variables*/ +static float use_increment = 0.0; +static float nav_target[3]; // Can be a position, speed or acceleration depending on the guidance H mode +static float dt_1l = 1./PERIODIC_FREQUENCY; +static float g = 9.81; // [m/s^2] Gravitational Acceleration + +/* Oneloop Control Variables*/ +float andi_u[ANDI_NUM_ACT_TOT]; +float andi_du[ANDI_NUM_ACT_TOT]; +static float andi_du_n[ANDI_NUM_ACT_TOT]; +float nu[ANDI_OUTPUTS]; +static float act_dynamics_d[ANDI_NUM_ACT_TOT]; +float actuator_state_1l[ANDI_NUM_ACT]; +static float a_thrust = 0.0; +static float g2_ff= 0.0; + +/*Attitude related variables*/ +struct Int32Eulers stab_att_sp_euler_1l;// here for now to correct warning, can be better eploited in the future +struct Int32Quat stab_att_sp_quat_1l; // here for now to correct warning, can be better eploited in the future +struct FloatEulers eulers_zxy_des; +struct FloatEulers eulers_zxy; +//static float psi_des_rad = 0.0; +float psi_des_rad = 0.0; +float psi_des_deg = 0.0; +static float psi_vec[4] = {0.0, 0.0, 0.0, 0.0}; +bool heading_manual = true; +bool yaw_stick_in_auto = false; + +/*WLS Settings*/ +static float gamma_wls = 1000.0; +static float du_min_1l[ANDI_NUM_ACT_TOT]; +static float du_max_1l[ANDI_NUM_ACT_TOT]; +static float du_pref_1l[ANDI_NUM_ACT_TOT]; +static float pitch_pref = 0; +static int number_iter = 0; + +/*Complementary Filter Variables*/ +static float model_pred[ANDI_OUTPUTS]; +static float ang_acc[3]; +static float lin_acc[3]; + +/*Chirp test Variables*/ +bool chirp_on = false; +bool chirp_first_call = true; +float time_elapsed_chirp = 0.0; +float t_0_chirp = 0.0; +float f0_chirp = 0.8 / (2.0 * M_PI); +float f1_chirp = 0.8 / (2.0 * M_PI); +float t_chirp = 45.0; +float A_chirp = 1.0; +int8_t chirp_axis = 0; +float p_ref_0[3] = {0.0, 0.0, 0.0}; + +/*Declaration of Reference Model and Error Controller Gains*/ +struct PolePlacement p_att_e; +struct PolePlacement p_att_rm; +/*Position Loop*/ +struct PolePlacement p_pos_e; +struct PolePlacement p_pos_rm; +/*Altitude Loop*/ +struct PolePlacement p_alt_e; +struct PolePlacement p_alt_rm; +/*Heading Loop*/ +struct PolePlacement p_head_e; +struct PolePlacement p_head_rm; +/*Gains of EC and RM ANDI*/ +struct Gains3rdOrder k_att_e; +struct Gains3rdOrder k_att_rm; +struct Gains3rdOrder k_pos_e; +struct Gains3rdOrder k_pos_rm; +/*Gains of EC and RM INDI*/ +struct Gains3rdOrder k_att_e_indi; +struct Gains3rdOrder k_pos_e_indi; + +/* Effectiveness Matrix definition */ +float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2; //scaled by ANDI_G_SCALING +float g1_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}; +float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; +float *bwls_1l[ANDI_OUTPUTS]; +float ratio_u_un[ANDI_NUM_ACT_TOT]; +float ratio_vn_v[ANDI_NUM_ACT_TOT]; + +/*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 att_dot_meas_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 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) /* Define messages of the module*/ #if PERIODIC_TELEMETRY @@ -400,83 +496,88 @@ static void send_guidance_oneloop_andi(struct transport_tx *trans, struct link_d &oneloop_andi.gui_ref.jer[1], &oneloop_andi.gui_ref.jer[2]); } -#endif - -/*Define general struct of the Oneloop ANDI controller*/ -struct OneloopGeneral oneloop_andi; - -/* Oneloop Misc variables*/ -static float use_increment = 0.0; -static float nav_target[3]; // Can be a position, speed or acceleration depending on the guidance H mode -static float dt_1l = 1./PERIODIC_FREQUENCY; -static float g = 9.81; // [m/s^2] Gravitational Acceleration - -/* Oneloop Control Variables*/ -float andi_u[ANDI_NUM_ACT_TOT]; -float andi_du[ANDI_NUM_ACT_TOT]; -static float andi_du_n[ANDI_NUM_ACT_TOT]; -float nu[ANDI_OUTPUTS]; -static float act_dynamics_d[ANDI_NUM_ACT_TOT]; -float actuator_state_1l[ANDI_NUM_ACT]; -static float a_thrust = 0.0; - -/*Attitude related variables*/ -struct Int32Eulers stab_att_sp_euler_1l;// here for now to correct warning, can be better eploited in the future -struct Int32Quat stab_att_sp_quat_1l; // here for now to correct warning, can be better eploited in the future -struct FloatEulers eulers_zxy_des; -struct FloatEulers eulers_zxy; -static float psi_des_rad = 0.0; -float psi_des_deg = 0.0; -static float psi_vec[4] = {0.0, 0.0, 0.0, 0.0}; - -/*WLS Settings*/ -static float gamma_wls = 1000.0; -static float du_min_1l[ANDI_NUM_ACT_TOT]; -static float du_max_1l[ANDI_NUM_ACT_TOT]; -static float du_pref_1l[ANDI_NUM_ACT_TOT]; -static int number_iter = 0; - -/*Complementary Filter Variables*/ -static float model_pred[ANDI_OUTPUTS]; -static float ang_acc[3]; -static float lin_acc[3]; +static void debug_vect(struct transport_tx *trans, struct link_device *dev, char *name, float *data, int datasize) +{ + pprz_msg_send_DEBUG_VECT(trans, dev, AC_ID, + strlen(name), name, + datasize, data); +} -/*Declaration of Reference Model and Error Controller Gains*/ -struct PolePlacement p_att_e; -struct PolePlacement p_att_rm; -/*Position Loop*/ -struct PolePlacement p_pos_e; -struct PolePlacement p_pos_rm; -/*Altitude Loop*/ -struct PolePlacement p_alt_e; -struct PolePlacement p_alt_rm; -/*Heading Loop*/ -struct PolePlacement p_head_e; -struct PolePlacement p_head_rm; -/*Gains of EC and RM*/ -struct Gains3rdOrder k_att_e; -struct Gains3rdOrder k_att_rm; -struct Gains2ndOrder k_head_e; -struct Gains2ndOrder k_head_rm; -struct Gains3rdOrder k_pos_e; -struct Gains3rdOrder k_pos_rm; +static void send_oneloop_debug(struct transport_tx *trans, struct link_device *dev) +{ + // float temp_att_des[3]; + // temp_att_des[0] = eulers_zxy_des.phi; + // temp_att_des[1] = eulers_zxy_des.theta; + // temp_att_des[2] = eulers_zxy_des.psi; + // debug_vect(trans, dev, "att_des", temp_att_des, 3); + // float temp_debug_vect[ANDI_NUM_ACT_TOT+1]; + // for (int i = 0; i < ANDI_NUM_ACT_TOT; i++) { + // temp_debug_vect[i] = andi_u[i]; + // } + // temp_debug_vect[ANDI_NUM_ACT_TOT] = pitch_pref; + // debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, ANDI_NUM_ACT_TOT+1); + float temp_debug_vect[2]; + temp_debug_vect[0] = andi_u[ONELOOP_ANDI_THETA_IDX]; + temp_debug_vect[1] = pitch_pref; + debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, 2); + // debug_vect(trans, dev, "andi_u", andi_u, ANDI_NUM_ACT); + // float temp_pref_vect[1]={pitch_pref}; + // debug_vect(trans, dev, "pitch_pref", temp_pref_vect, 1); + // float rate_vect_temp[3] = {stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r}; + // debug_vect(trans, dev, "att_rate", rate_vect_temp, 3); +} -/* Effectiveness Matrix definition */ -float g2_1l[ANDI_NUM_ACT_TOT] = ONELOOP_ANDI_G2; //scaled by INDI_G_SCALING -float g1_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT] = {ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_ZERO, ONELOOP_ANDI_G1_THRUST, ONELOOP_ANDI_G1_ROLL, ONELOOP_ANDI_G1_PITCH, ONELOOP_ANDI_G1_YAW}; -float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; -float *bwls_1l[ANDI_OUTPUTS]; -float ratio_u_un[ANDI_NUM_ACT_TOT]; -float ratio_vn_v[ANDI_NUM_ACT_TOT]; +static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Quat quat_ext_pose; + #ifdef INS_EXT_POSE + quat_ext_pose.qi = ins_ext_pos.ev_quat.qi/0.0000305;//(int32_t) + quat_ext_pose.qx = ins_ext_pos.ev_quat.qx/0.0000305;//(int32_t) + quat_ext_pose.qy = ins_ext_pos.ev_quat.qy/0.0000305;//(int32_t) + quat_ext_pose.qz = ins_ext_pos.ev_quat.qz/0.0000305;//(int32_t) + #else + quat_ext_pose.qi = 0; + quat_ext_pose.qx = 0; + quat_ext_pose.qy = 0; + quat_ext_pose.qz = 0; + #endif -/*Filters Initialization*/ -static struct FirstOrderLowPass filt_accel_ned[3]; -static struct FirstOrderLowPass rates_filt_fo[3]; -static struct FirstOrderLowPass model_pred_a_filt[3]; -static Butterworth2LowPass att_dot_meas_lowpass_filters[3]; -static Butterworth2LowPass model_pred_filt[ANDI_OUTPUTS]; + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); + pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID, + &quat_ext_pose.qi,// In the future also stream setpoint in quat &stab_att_sp_quat.qi, + &quat_ext_pose.qx,// In the future also stream setpoint in quat &stab_att_sp_quat.qx, + &quat_ext_pose.qy,// In the future also stream setpoint in quat &stab_att_sp_quat.qy, + &quat_ext_pose.qz,// In the future also stream setpoint in quat &stab_att_sp_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); +} +// static void send_oneloop_nps(struct transport_tx *trans, struct link_device *dev) +// { +// float nps_acc_N = (float) fdm.ltpprz_ecef_accel.x; +// float nps_acc_E = (float) fdm.ltpprz_ecef_accel.y; +// float nps_acc_D = (float) fdm.ltpprz_ecef_accel.z; +// float nps_vel_N = (float) fdm.ltpprz_ecef_vel.x ; +// float nps_vel_E = (float) fdm.ltpprz_ecef_vel.y ; +// float nps_vel_D = (float) fdm.ltpprz_ecef_vel.z ; +// float nps_pos_N = (float) fdm.ltpprz_pos.x ; +// float nps_pos_E = (float) fdm.ltpprz_pos.y ; +// float nps_pos_D = (float) fdm.ltpprz_pos.z ; +// pprz_msg_send_NPS_SPEED_POS(trans, dev, AC_ID, +// &nps_acc_N, +// &nps_acc_E, +// &nps_acc_D, +// &nps_vel_N, +// &nps_vel_E, +// &nps_vel_D, +// &nps_pos_N, +// &nps_pos_E, +// &nps_pos_D); +// } +#endif /** @brief Function to make sure that inputs are positive non zero vaues*/ static float positive_non_zero(float input) @@ -522,6 +623,18 @@ static float k_e_2_2_f(float p1, float p2) { return (p1 + p2); } +static float k_e_1_2_f_v2(float omega, float zeta) { + omega = positive_non_zero(omega); + zeta = positive_non_zero(zeta); + return (omega * omega); +} + +static float k_e_2_2_f_v2(float omega, float zeta) { + omega = positive_non_zero(omega); + zeta = positive_non_zero(zeta); + return (2* zeta * omega); +} + static float k_e_1_3_f_v2(float omega_n, UNUSED float zeta, float p1) { omega_n = positive_non_zero(omega_n); p1 = positive_non_zero(p1); @@ -560,8 +673,8 @@ static float k_rm_2_3_f(float omega_n, float zeta, float p1) { static float k_rm_3_3_f(float omega_n, float zeta, float p1) { omega_n = positive_non_zero(omega_n); - zeta = positive_non_zero(p1); - p1 = positive_non_zero(zeta); + zeta = positive_non_zero(zeta); + p1 = positive_non_zero(p1); return p1 + omega_n * zeta * 2.0; } @@ -880,10 +993,10 @@ void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[ * @param k1_e Error Controller Gain 1st order signal * @param k2_e Error Controller Gain 2nd order signal */ -static float ec_2rd(float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e){ - float y_3d = k1_e*(x_ref-x)+k2_e*(x_d_ref-x_d)+x_2d_ref; - return y_3d; -} +// static float ec_2rd(float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e){ +// float y_3d = k1_e*(x_ref-x)+k2_e*(x_d_ref-x_d)+x_2d_ref; +// return y_3d; +// } /** @@ -902,38 +1015,65 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ tao = positive_non_zero(tao); return 1.0/tao; } + +/** + * @brief 3 Coincident poles to 2 coincident and act dynamics Approximation + * @param p3 Pole + * @param omega Dynamics Slowest actuator + */ +static float P3_2_P2Omega(float p3, float omega){ + p3 = positive_non_zero(p3); + omega = positive_non_zero(omega); + float p2 = -omega + sqrt(pow(omega,2) + 3 * pow(p3,2)); + p2 = positive_non_zero(p2); + return p2; +} /** * @brief Initialize Position of Poles * */ void init_poles(void){ - p_att_e.omega_n = 7.68; + + // Attitude Controller Poles---------------------------------------------------------- + float slow_pole = 10.1; // Pole of the slowest dynamics used in the attitude controller + + p_att_e.omega_n = 4.50;//sqrt(pow(7.68,3)/slow_pole); p_att_e.zeta = 1.0; - p_att_e.p3 = p_att_e.omega_n * p_att_e.zeta; + p_att_e.p3 = slow_pole; //p_att_e.omega_n * p_att_e.zeta; - p_att_rm.omega_n = 6.14; + p_att_rm.omega_n = 4.71;//6.14; p_att_rm.zeta = 1.0; p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; - p_pos_e.omega_n = 1.41; - p_pos_e.zeta = 0.85; - p_pos_e.p3 = 6.0; + p_head_e.omega_n = 1.80;//sqrt(pow(3.4,3)/slow_pole);//P3_2_P2Omega(1.5,slow_pole);// sqrt(pow(1.5,3)/slow_pole);//1.5;// + p_head_e.zeta = 1.0; + p_head_e.p3 = slow_pole;//p_head_e.omega_n * p_head_e.zeta;// + + p_head_rm.omega_n = 2.56;//1.5; + p_head_rm.zeta = 1.0; + p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; + + act_dynamics[ONELOOP_ANDI_PHI_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[ONELOOP_ANDI_THETA_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - p_pos_rm.omega_n = 0.6; + // Position Controller Poles---------------------------------------------------------- + slow_pole = act_dynamics[ONELOOP_ANDI_PHI_IDX]; // Pole of the slowest dynamics used in the position controller + + p_pos_e.omega_n = 1.0;//sqrt(pow(1.41,3)/slow_pole); + p_pos_e.zeta = 1.0; + p_pos_e.p3 = slow_pole; //p_pos_e.omega_n * p_pos_e.zeta; + + p_pos_rm.omega_n = 0.93;//0.6; p_pos_rm.zeta = 1.0; - p_pos_rm.p3 = 6.0; + p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; - p_alt_e.omega_n = 3.54; - p_alt_e.zeta = 0.85; - p_alt_e.p3 = 6.0; + p_alt_e.omega_n = 3.0;//sqrt(pow(3.54,3)/slow_pole); + p_alt_e.zeta = 1.0; + p_alt_e.p3 = slow_pole; //p_alt_e.omega_n * p_alt_e.zeta; // - p_alt_rm.omega_n = 2.0; + p_alt_rm.omega_n = 1.93;//2.0; p_alt_rm.zeta = 1.0; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; - - p_head_e.omega_n = 1.5; - p_head_e.zeta = 1.0; - p_head_e.p3 = p_head_e.omega_n * p_head_e.zeta; } /** * @brief Initialize Controller Gains @@ -943,17 +1083,30 @@ void init_controller(void){ /*Register a variable from nav_hybrid. SHould be improved when nav hybrid is final.*/ max_v_nav = nav_max_speed; /*Some calculations in case new poles have been specified*/ - p_att_e.p3 = p_att_e.omega_n * p_att_e.zeta; - p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; - p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; - p_head_e.p3 = p_head_e.omega_n * p_head_e.zeta; + //p_att_e.p3 = p_att_e.omega_n * p_att_e.zeta; + p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; + //p_pos_e.p3 = p_pos_e.omega_n * p_pos_e.zeta; + p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; + //p_alt_e.p3 = p_alt_e.omega_n * p_alt_e.zeta; + p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; + //p_head_e.p3 = p_head_e.omega_n * p_head_e.zeta; + p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; + + + //printf("p_att_e.omega_n = %f \n", p_att_e.omega_n); + //printf("p_head_e.omega_n = %f\n", p_head_e.omega_n); + //printf("p_pos_e.omega_n = %f\n", p_pos_e.omega_n); + //printf("p_alt_e.omega_n = %f\n", p_alt_e.omega_n); + + //--ANDI Controller gains -------------------------------------------------------------------------------- /*Attitude Loop*/ - k_att_e.k1[0] = k_e_1_3_f(p_att_e.p3, p_att_e.p3, p_att_e.p3); - k_att_e.k2[0] = k_e_2_3_f(p_att_e.p3, p_att_e.p3, p_att_e.p3); - k_att_e.k3[0] = k_e_3_3_f(p_att_e.p3, p_att_e.p3, p_att_e.p3); + k_att_e.k1[0] = k_e_1_3_f_v2(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); + k_att_e.k2[0] = k_e_2_3_f_v2(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); + k_att_e.k3[0] = k_e_3_3_f_v2(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); k_att_e.k1[1] = k_att_e.k1[0]; k_att_e.k2[1] = k_att_e.k2[0]; k_att_e.k3[1] = k_att_e.k3[0]; + k_att_rm.k1[0] = k_rm_1_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3); k_att_rm.k2[0] = k_rm_2_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3); k_att_rm.k3[0] = k_rm_3_3_f(p_att_rm.omega_n, p_att_rm.zeta, p_att_rm.p3); @@ -961,23 +1114,31 @@ void init_controller(void){ k_att_rm.k2[1] = k_att_rm.k2[0]; k_att_rm.k3[1] = k_att_rm.k3[0]; - /*Position Loop*/ + /*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); + k_att_e.k3[2] = k_e_3_3_f_v2(p_head_e.omega_n, p_head_e.zeta, p_head_e.p3); + k_att_rm.k1[2] = k_rm_1_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); + k_att_rm.k2[2] = k_rm_2_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); + k_att_rm.k3[2] = k_rm_3_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); + + /*Position Loop*/ k_pos_e.k1[0] = k_e_1_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); k_pos_e.k2[0] = k_e_2_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); k_pos_e.k3[0] = k_e_3_3_f_v2(p_pos_e.omega_n, p_pos_e.zeta, p_pos_e.p3); k_pos_e.k1[1] = k_pos_e.k1[0]; k_pos_e.k2[1] = k_pos_e.k2[0]; - k_pos_e.k3[1] = k_pos_e.k3[0]; - k_pos_rm.k1[0] = k_e_1_3_f_v2(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); - k_pos_rm.k2[0] = k_e_2_3_f_v2(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); - k_pos_rm.k3[0] = k_e_3_3_f_v2(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); + k_pos_e.k3[1] = k_pos_e.k3[0]; + + k_pos_rm.k1[0] = k_rm_1_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); + k_pos_rm.k2[0] = k_rm_2_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); + k_pos_rm.k3[0] = k_rm_3_3_f(p_pos_rm.omega_n, p_pos_rm.zeta, p_pos_rm.p3); k_pos_rm.k1[1] = k_pos_rm.k1[0]; k_pos_rm.k2[1] = k_pos_rm.k2[0]; k_pos_rm.k3[1] = k_pos_rm.k3[0]; - - gih_params.pos_gain = k_pos_rm.k1[0]; //delete once nav hybrid is fixed - gih_params.speed_gain = k_pos_rm.k2[0]; //delete once nav hybrid is fixed + nav_hybrid_pos_gain = k_pos_rm.k1[0]; + nav_hybrid_max_bank = ONELOOP_ANDI_MAX_BANK; /*Altitude Loop*/ k_pos_e.k1[2] = k_e_1_3_f_v2(p_alt_e.omega_n, p_alt_e.zeta, p_alt_e.p3); @@ -986,90 +1147,127 @@ void init_controller(void){ k_pos_rm.k1[2] = k_rm_1_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k2[2] = k_rm_2_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k3[2] = k_rm_3_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); - gih_params.pos_gainz = k_pos_rm.k1[2]; //delete once nav hybrid is fixed - gih_params.speed_gainz = k_pos_rm.k2[2]; //delete once nav hybrid is fixed - /*Heading Loop Manual*/ - k_head_e.k2 = k_e_1_2_f(p_head_e.p3, p_head_e.p3); - k_head_e.k3 = k_e_2_2_f(p_head_e.p3, p_head_e.p3); - k_head_rm.k2 = k_rm_1_2_f(p_head_rm.p3, p_head_rm.p3); - k_head_rm.k3 = k_rm_2_2_f(p_head_rm.p3, p_head_rm.p3); + // ANDI Gains check + printf("k_att_e [k1,k2,k3] = [%f,%f,%f]\n",k_att_e.k1[0],k_att_e.k2[0],k_att_e.k3[0]); + printf("k_att_rm [k1,k2,k3] = [%f,%f,%f]\n",k_att_rm.k1[0],k_att_rm.k2[0],k_att_rm.k3[0]); + printf("k_att_head [k1,k2,k3] = [%f,%f,%f]\n",k_att_e.k1[2],k_att_e.k2[2],k_att_e.k3[2]); + printf("k_att_head_rm [k1,k2,k3] = [%f,%f,%f]\n",k_att_rm.k1[2],k_att_rm.k2[2],k_att_rm.k3[2]); + printf("k_pos_e [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e.k1[0],k_pos_e.k2[0],k_pos_e.k3[0]); + printf("k_pos_rm [k1,k2,k3] = [%f,%f,%f]\n",k_pos_rm.k1[0],k_pos_rm.k2[0],k_pos_rm.k3[0]); + printf("k_alt_e [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e.k1[2],k_pos_e.k2[2],k_pos_e.k3[2]); + printf("k_alt_rm [k1,k2,k3] = [%f,%f,%f]\n",k_pos_rm.k1[2],k_pos_rm.k2[2],k_pos_rm.k3[2]); + + //--INDI Controller gains -------------------------------------------------------------------------------- + /*Attitude Loop*/ + k_att_e_indi.k1[0] = k_e_1_2_f_v2(p_att_e.omega_n, p_att_e.zeta); + k_att_e_indi.k2[0] = k_e_2_2_f_v2(p_att_e.omega_n, p_att_e.zeta); + k_att_e_indi.k3[0] = 1.0; + k_att_e_indi.k1[1] = k_att_e_indi.k1[0]; + k_att_e_indi.k2[1] = k_att_e_indi.k2[0]; + k_att_e_indi.k3[1] = k_att_e_indi.k3[0]; /*Heading Loop NAV*/ - k_att_e.k1[2] = k_e_1_3_f(p_head_e.p3, p_head_e.p3, p_head_e.p3); - k_att_e.k2[2] = k_e_2_3_f(p_head_e.p3, p_head_e.p3, p_head_e.p3); - k_att_e.k3[2] = k_e_3_3_f(p_head_e.p3, p_head_e.p3, p_head_e.p3); - k_att_rm.k1[2] = k_rm_1_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); - k_att_rm.k2[2] = k_rm_2_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); - k_att_rm.k3[2] = k_rm_3_3_f(p_head_rm.omega_n, p_head_rm.zeta, p_head_rm.p3); + k_att_e_indi.k1[2] = k_e_1_2_f_v2(p_head_e.omega_n, p_head_e.zeta); + k_att_e_indi.k2[2] = k_e_2_2_f_v2(p_head_e.omega_n, p_head_e.zeta); + k_att_e_indi.k3[2] = 1.0; + /*Position Loop*/ + k_pos_e_indi.k1[0] = k_e_1_2_f_v2(p_pos_e.omega_n, p_pos_e.zeta); + k_pos_e_indi.k2[0] = k_e_2_2_f_v2(p_pos_e.omega_n, p_pos_e.zeta); + k_pos_e_indi.k3[0] = 1.0; + k_pos_e_indi.k1[1] = k_pos_e_indi.k1[0]; + k_pos_e_indi.k2[1] = k_pos_e_indi.k2[0]; + k_pos_e_indi.k3[1] = k_pos_e_indi.k3[0]; + + /*Altitude Loop*/ + k_pos_e_indi.k1[2] = k_e_1_2_f_v2(p_alt_e.omega_n, p_alt_e.zeta); + k_pos_e_indi.k2[2] = k_e_2_2_f_v2(p_alt_e.omega_n, p_alt_e.zeta); + k_pos_e_indi.k3[2] = 1.0; + + // INDI Gains check + printf("k_att_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_att_e_indi.k1[0],k_att_e_indi.k2[0],k_att_e_indi.k3[0]); + printf("k_att_head_indi [k1,k2,k3] = [%f,%f,%f]\n",k_att_e_indi.k1[2],k_att_e_indi.k2[2],k_att_e_indi.k3[2]); + printf("k_pos_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e_indi.k1[0],k_pos_e_indi.k2[0],k_pos_e_indi.k3[0]); + printf("k_alt_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e_indi.k1[2],k_pos_e_indi.k2[2],k_pos_e_indi.k3[2]); + + //------------------------------------------------------------------------------------------ /*Approximated Dynamics*/ - act_dynamics[ANDI_NUM_ACT] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - act_dynamics[ANDI_NUM_ACT+1] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[ONELOOP_ANDI_PHI_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + act_dynamics[ONELOOP_ANDI_THETA_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); + + printf("act_dynamics[ONELOOP_ANDI_PHI_IDX] = %f\n",act_dynamics[ONELOOP_ANDI_PHI_IDX]); + printf("act_dynamics[ONELOOP_ANDI_THETA_IDX] = %f\n",act_dynamics[ONELOOP_ANDI_THETA_IDX]); } /** @brief Initialize the filters */ void init_filter(void) { - float tau = 1.0 / (2.0 * M_PI *oneloop_andi_filt_cutoff); + //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); + 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(&att_dot_meas_lowpass_filters[i], tau, sample_time, 0.0); - init_first_order_low_pass(&filt_accel_ned[i], tau_a, 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_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p); - init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q); - init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r); + 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); - // Remember to change the time constant if you provide different P Q R filters - for (i = 0; i < ANDI_OUTPUTS; i++){ - if (i < 3){ - init_butterworth_2_low_pass(&model_pred_filt[i], tau_a, sample_time, 0.0); - init_first_order_low_pass(&model_pred_a_filt[i], tau_a, sample_time, 0.0); - } else { - init_butterworth_2_low_pass(&model_pred_filt[i], tau, 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); } /** @brief Propagate the filters */ void oneloop_andi_propagate_filters(void) { struct NedCoor_f *accel = stateGetAccelNed_f(); + struct NedCoor_f *veloc = stateGetSpeedNed_f(); struct FloatRates *body_rates = stateGetBodyRates_f(); float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r}; - update_first_order_low_pass(&filt_accel_ned[0], accel->x); - update_first_order_low_pass(&filt_accel_ned[1], accel->y); - update_first_order_low_pass(&filt_accel_ned[2], accel->z); - - - calc_model(); + 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); + calc_model(oneloop_andi.ctrl_type); int8_t i; - for (i = 0; i < ANDI_OUTPUTS; i++){ - update_butterworth_2_low_pass(&model_pred_filt[i], model_pred[i]); - } - + for (i = 0; i < 3; i++) { - update_first_order_low_pass(&model_pred_a_filt[i], model_pred[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(&att_dot_meas_lowpass_filters[i], rate_vect[i]); - update_first_order_low_pass(&rates_filt_fo[i], rate_vect[i]); - - ang_acc[i] = (att_dot_meas_lowpass_filters[i].o[0]- att_dot_meas_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY + model_pred[3+i] - model_pred_filt[3+i].o[0]; - lin_acc[i] = filt_accel_ned[i].last_out + model_pred[i] - model_pred_a_filt[i].last_out; -}} + update_butterworth_2_low_pass(&rates_filt_bt[i], rate_vect[i]); + + ang_acc[i] = (att_dot_meas_lowpass_filters[i].o[0]- att_dot_meas_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY + model_pred[3+i] - model_pred_aa_filt[i].o[0]; + lin_acc[i] = filt_accel_ned[i].o[0] + model_pred[i] - model_pred_la_filt[i].o[0]; + } + // Propagate filter for sideslip correction + float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y); + update_butterworth_2_low_pass(&accely_filt, accely); + float airspeed_meas = stateGetAirspeed_f(); + update_butterworth_2_low_pass(&airspeed_filt, airspeed_meas); +} /** @brief Init function of Oneloop ANDI controller */ void oneloop_andi_init(void) { oneloop_andi.half_loop = true; + oneloop_andi.ctrl_type = CTRL_ANDI; init_poles(); // Make sure that the dynamics are positive and non-zero int8_t i; @@ -1078,7 +1276,7 @@ void oneloop_andi_init(void) } // Initialize Effectiveness matrix calc_normalization(); - sum_g1g2_1l(); + sum_g1g2_1l(oneloop_andi.ctrl_type); for (i = 0; i < ANDI_OUTPUTS; i++) { bwls_1l[i] = g1g2_1l[i]; } @@ -1108,6 +1306,9 @@ void oneloop_andi_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_oneloop_andi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_oneloop_andi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE, send_guidance_oneloop_andi); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_oneloop_debug); + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); + //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NPS_SPEED_POS, send_oneloop_nps); #endif } @@ -1117,13 +1318,14 @@ void oneloop_andi_init(void) * and there are multiple modes that use (the same) stabilization. Resetting the controller * is not so nice when you are flying. */ -void oneloop_andi_enter(bool half_loop_sp) +void oneloop_andi_enter(bool half_loop_sp, int ctrl_type) { oneloop_andi.half_loop = half_loop_sp; + oneloop_andi.ctrl_type = ctrl_type; psi_des_rad = eulers_zxy.psi; - psi_des_deg = eulers_zxy.psi * 180.0 / M_PI; + psi_des_deg = DegOfRad(eulers_zxy.psi); calc_normalization(); - sum_g1g2_1l(); + sum_g1g2_1l(oneloop_andi.ctrl_type); init_filter(); init_controller(); /* Stabilization Reset */ @@ -1136,7 +1338,7 @@ void oneloop_andi_enter(bool half_loop_sp) float_vect_zero(nav_target,3); eulers_zxy_des.phi = 0.0; eulers_zxy_des.theta = 0.0; - eulers_zxy_des.psi = 0.0; + eulers_zxy_des.psi = psi_des_rad; float_vect_zero(model_pred,ANDI_OUTPUTS); /*Guidance Reset*/ } @@ -1168,10 +1370,22 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, float_vect_copy(oneloop_andi.gui_ref.acc,oneloop_andi.gui_state.acc,3); float_vect_zero(oneloop_andi.gui_ref.jer,3); // Set desired attitude with stick input - eulers_zxy_des.phi = (float) (radio_control_get(RADIO_ROLL))/MAX_PPRZ*45.0*M_PI/180.0; - eulers_zxy_des.theta = (float) (radio_control_get(RADIO_PITCH))/MAX_PPRZ*45.0*M_PI/180.0; - eulers_zxy_des.psi = eulers_zxy.psi; - psi_des_rad = eulers_zxy.psi; + eulers_zxy_des.phi = (float) (radio_control_get(RADIO_ROLL)) /MAX_PPRZ * ONELOOP_ANDI_MAX_PHI ; + eulers_zxy_des.theta = (float) (radio_control_get(RADIO_PITCH))/MAX_PPRZ * ONELOOP_ANDI_MAX_THETA; + // Set desired Yaw rate with stick input + des_r = (float) (radio_control_get(RADIO_YAW))/MAX_PPRZ*max_r; // Get yaw rate from stick + BoundAbs(des_r,max_r); // Bound yaw rate + float delta_psi_des_rad = des_r * dt_1l; // Integrate desired Yaw rate to get desired change in yaw + float delta_psi_rad = eulers_zxy_des.psi-eulers_zxy.psi; // Calculate current yaw difference between des and actual + NormRadAngle(delta_psi_rad); // Normalize the difference + if (fabs(delta_psi_rad) > RadOfDeg(10.0)){ // If difference is bigger than 10 deg do not further increment desired + delta_psi_des_rad = 0.0; + } + psi_des_rad += delta_psi_des_rad; // Incrementdesired yaw + NormRadAngle(psi_des_rad); + eulers_zxy_des.psi = psi_des_rad; + // Register Attitude Setpoints from previous loop + float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, eulers_zxy_des.psi}; // Create commands adhoc to get actuators to the wanted level thrust_cmd_1l = (float) radio_control_get(RADIO_THROTTLE); Bound(thrust_cmd_1l,0.0,MAX_PPRZ); @@ -1179,21 +1393,11 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, for (i = 0; i < ANDI_NUM_ACT; i++) { a_thrust +=(thrust_cmd_1l - use_increment*actuator_state_1l[i]) * bwls_1l[2][i] / (ratio_u_un[i] * ratio_vn_v[i]); } - // Overwrite Yaw Ref with desired Yaw setting (for consistent plotting) - oneloop_andi.sta_ref.att[2] = psi_des_rad; - // Set desired Yaw rate with stick input - des_r = (float) (radio_control_get(RADIO_YAW))/MAX_PPRZ*3.0; - BoundAbs(des_r,3.0); - // Generate reference signals - rm_3rd(dt_1l, &oneloop_andi.sta_ref.att[0], &oneloop_andi.sta_ref.att_d[0], &oneloop_andi.sta_ref.att_2d[0], &oneloop_andi.sta_ref.att_3d[0], eulers_zxy_des.phi, k_att_rm.k1[0], k_att_rm.k2[0], k_att_rm.k3[0]); - rm_3rd(dt_1l, &oneloop_andi.sta_ref.att[1], &oneloop_andi.sta_ref.att_d[1], &oneloop_andi.sta_ref.att_2d[1], &oneloop_andi.sta_ref.att_3d[1], eulers_zxy_des.theta, k_att_rm.k1[1], k_att_rm.k2[1], k_att_rm.k3[1]); - rm_2nd(dt_1l, &oneloop_andi.sta_ref.att_d[2], &oneloop_andi.sta_ref.att_2d[2], &oneloop_andi.sta_ref.att_3d[2], des_r, k_head_rm.k2, k_head_rm.k3); + 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); }else{ // Make sure X and Y jerk objectives are active Wv_wls[0] = Wv[0]; Wv_wls[1] = Wv[1]; - // Register Attitude Setpoints from previous loop - float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, psi_des_rad}; // Generate Reference signals for positioning using RM if (rm_order_h == 3){ 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_nav, 2); @@ -1205,7 +1409,22 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, float_vect_copy(oneloop_andi.gui_ref.vel, oneloop_andi.gui_state.vel,2); rm_1st_pos(dt_1l, oneloop_andi.gui_ref.acc, oneloop_andi.gui_ref.jer, nav_target, k_pos_rm.k3, max_j_nav, 2); } - + // Update desired Heading (psi_des_rad) based on previous loop or changed setting + if (heading_manual){ + psi_des_rad = RadOfDeg(psi_des_deg); + if (yaw_stick_in_auto){ + psi_des_rad += (float) (radio_control_get(RADIO_YAW))/MAX_PPRZ*max_r * dt_1l; + } + } else { + float ref_mag_vel = float_vect_norm(oneloop_andi.gui_ref.vel,2); + if (ref_mag_vel > 3.0){ + psi_des_rad = atan2f(oneloop_andi.gui_ref.vel[1],oneloop_andi.gui_ref.vel[0]); + } + psi_des_rad += oneloop_andi_sideslip() * dt_1l; + NormRadAngle(psi_des_rad); + } + // Register Attitude Setpoints from previous loop + float att_des[3] = {eulers_zxy_des.phi, eulers_zxy_des.theta, psi_des_rad}; // The RM functions want an array as input. Create a single entry array and write the vertical guidance entries. float single_value_ref[1] = {oneloop_andi.gui_ref.pos[2]}; float single_value_d_ref[1] = {oneloop_andi.gui_ref.vel[2]}; @@ -1235,11 +1454,13 @@ void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, oneloop_andi.gui_ref.acc[2] = single_value_2d_ref[0]; oneloop_andi.gui_ref.jer[2] = single_value_3d_ref[0]; } + // Run chirp test if turnerd on (overwrite the guidance references) + 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); // Generate Reference signals for attitude using RM // FIX ME ow not yet defined, will be useful in the future to have accurate psi tracking in NAV functions bool ow_psi = false; 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); -} + } } /** @@ -1257,7 +1478,7 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, init_controller(); calc_normalization(); get_act_state_oneloop(); - sum_g1g2_1l(); + sum_g1g2_1l(oneloop_andi.ctrl_type); // If drone is not on the ground use incremental law use_increment = 0.0; @@ -1270,28 +1491,25 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, in_flight_oneloop = false; } - // Update desired Heading (psi_des_rad) based on previous loop or changed setting - psi_des_rad = psi_des_deg * M_PI / 180.0; - // Register the state of the drone in the variables used in RM and EC // (1) Attitude related - oneloop_andi.sta_state.att[0] = eulers_zxy.phi * use_increment; - oneloop_andi.sta_state.att[1] = eulers_zxy.theta * use_increment; - oneloop_andi.sta_state.att[2] = eulers_zxy.psi * use_increment; + oneloop_andi.sta_state.att[0] = eulers_zxy.phi * use_increment; + oneloop_andi.sta_state.att[1] = eulers_zxy.theta * use_increment; + oneloop_andi.sta_state.att[2] = eulers_zxy.psi * use_increment; oneloop_andi_propagate_filters(); //needs to be after update of attitude vector - oneloop_andi.sta_state.att_d[0] = rates_filt_fo[0].last_out * use_increment; - oneloop_andi.sta_state.att_d[1] = rates_filt_fo[1].last_out * use_increment; - oneloop_andi.sta_state.att_d[2] = rates_filt_fo[2].last_out * use_increment; - oneloop_andi.sta_state.att_2d[0] = ang_acc[0] * use_increment; - oneloop_andi.sta_state.att_2d[1] = ang_acc[1] * use_increment; - oneloop_andi.sta_state.att_2d[2] = ang_acc[2] * use_increment; + oneloop_andi.sta_state.att_d[0] = rates_filt_bt[0].o[0] * use_increment; + oneloop_andi.sta_state.att_d[1] = rates_filt_bt[1].o[0] * use_increment; + oneloop_andi.sta_state.att_d[2] = rates_filt_bt[2].o[0] * use_increment; + oneloop_andi.sta_state.att_2d[0] = ang_acc[0] * use_increment; + oneloop_andi.sta_state.att_2d[1] = ang_acc[1] * use_increment; + oneloop_andi.sta_state.att_2d[2] = ang_acc[2] * use_increment; // (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] = stateGetSpeedNed_f()->x; - oneloop_andi.gui_state.vel[1] = stateGetSpeedNed_f()->y; - oneloop_andi.gui_state.vel[2] = stateGetSpeedNed_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]; @@ -1301,28 +1519,54 @@ void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, for (i = 0; i < ANDI_OUTPUTS; i++) { bwls_1l[i] = g1g2_1l[i]; } - + // Calculated feedforward signal for yaW CONTROL + g2_ff = 0.0; + for (i = 0; i < ANDI_NUM_ACT; i++) { + if (oneloop_andi.ctrl_type == CTRL_ANDI){ + g2_ff += g2_1l[i] * act_dynamics[i] * andi_du[i]; + } else if (oneloop_andi.ctrl_type == CTRL_INDI){ + g2_ff += g2_1l[i]* andi_du_n[i]; + } + } + //G2 is scaled by ANDI_G_SCALING to make it readable + g2_ff = g2_ff / ANDI_G_SCALING; // Run the Reference Model (RM) oneloop_andi_RM(half_loop, PSA_des, rm_order_h, rm_order_v); - // Generate pseudo control for stabilization vector (nu) based on error controller + + // Guidance Pseudo Control Vector (nu) based on error controller if(half_loop){ nu[0] = 0.0; nu[1] = 0.0; nu[2] = a_thrust; - nu[3] = ec_3rd(oneloop_andi.sta_ref.att[0], oneloop_andi.sta_ref.att_d[0], oneloop_andi.sta_ref.att_2d[0], oneloop_andi.sta_ref.att_3d[0], oneloop_andi.sta_state.att[0], oneloop_andi.sta_state.att_d[0], oneloop_andi.sta_state.att_2d[0], k_att_e.k1[0], k_att_e.k2[0], k_att_e.k3[0]); - nu[4] = ec_3rd(oneloop_andi.sta_ref.att[1], oneloop_andi.sta_ref.att_d[1], oneloop_andi.sta_ref.att_2d[1], oneloop_andi.sta_ref.att_3d[1], oneloop_andi.sta_state.att[1], oneloop_andi.sta_state.att_d[1], oneloop_andi.sta_state.att_2d[1], k_att_e.k1[1], k_att_e.k2[1], k_att_e.k3[1]); - nu[5] = ec_2rd(oneloop_andi.sta_ref.att_d[2], oneloop_andi.sta_ref.att_2d[2], oneloop_andi.sta_ref.att_3d[2], oneloop_andi.sta_state.att_d[2], oneloop_andi.sta_state.att_2d[2], k_head_e.k2, k_head_e.k3); }else{ - float y_4d_att[3]; - ec_3rd_att(y_4d_att, 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); - nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], oneloop_andi.gui_ref.jer[0], oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e.k1[0], k_pos_e.k2[0], k_pos_e.k3[0]); - nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], oneloop_andi.gui_ref.jer[1], oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e.k1[1], k_pos_e.k2[1], k_pos_e.k3[1]); - nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], oneloop_andi.gui_ref.jer[2], oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e.k1[2], k_pos_e.k2[2], k_pos_e.k3[2]); - nu[3] = y_4d_att[0]; - nu[4] = y_4d_att[1]; - nu[5] = y_4d_att[2]; + if(oneloop_andi.ctrl_type == CTRL_ANDI){ + nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], oneloop_andi.gui_ref.jer[0], oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e.k1[0], k_pos_e.k2[0], k_pos_e.k3[0]); + nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], oneloop_andi.gui_ref.jer[1], oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e.k1[1], k_pos_e.k2[1], k_pos_e.k3[1]); + nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], oneloop_andi.gui_ref.jer[2], oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e.k1[2], k_pos_e.k2[2], k_pos_e.k3[2]); + } else if (oneloop_andi.ctrl_type == CTRL_INDI){ + nu[0] = ec_3rd(oneloop_andi.gui_ref.pos[0], oneloop_andi.gui_ref.vel[0], oneloop_andi.gui_ref.acc[0], 0.0, oneloop_andi.gui_state.pos[0], oneloop_andi.gui_state.vel[0], oneloop_andi.gui_state.acc[0], k_pos_e_indi.k1[0], k_pos_e_indi.k2[0], k_pos_e_indi.k3[0]); + nu[1] = ec_3rd(oneloop_andi.gui_ref.pos[1], oneloop_andi.gui_ref.vel[1], oneloop_andi.gui_ref.acc[1], 0.0, oneloop_andi.gui_state.pos[1], oneloop_andi.gui_state.vel[1], oneloop_andi.gui_state.acc[1], k_pos_e_indi.k1[1], k_pos_e_indi.k2[1], k_pos_e_indi.k3[1]); + nu[2] = ec_3rd(oneloop_andi.gui_ref.pos[2], oneloop_andi.gui_ref.vel[2], oneloop_andi.gui_ref.acc[2], 0.0, oneloop_andi.gui_state.pos[2], oneloop_andi.gui_state.vel[2], oneloop_andi.gui_state.acc[2], k_pos_e_indi.k1[2], k_pos_e_indi.k2[2], k_pos_e_indi.k3[2]); + } } + // Attitude Pseudo Control Vector (nu) based on error controller + float y_4d_att[3]; + if(oneloop_andi.ctrl_type == CTRL_ANDI){ + ec_3rd_att(y_4d_att, 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); + } else if (oneloop_andi.ctrl_type == CTRL_INDI){ + float dummy0[3] = {0.0, 0.0, 0.0}; + ec_3rd_att(y_4d_att, 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); + } + nu[3] = y_4d_att[0]; + nu[4] = y_4d_att[1]; + nu[5] = y_4d_att[2] + g2_ff; + if (!chirp_on){ + pitch_pref = radio_control.values[RADIO_AUX5]; + pitch_pref = pitch_pref / MAX_PPRZ * theta_pref_max; + Bound(pitch_pref,0.0,theta_pref_max); + } + u_pref[ONELOOP_ANDI_THETA_IDX] = pitch_pref; // Calculate the min and max increments for (i = 0; i < ANDI_NUM_ACT_TOT; i++) { if(ix; @@ -1512,8 +1822,8 @@ void oneloop_from_nav(bool in_flight) // Oneloop controller wants desired targets and handles reference generation internally switch (nav.setpoint_mode) { case NAV_SETPOINT_MODE_POS: - PSA_des.x = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.carrot.y)); - PSA_des.y = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.carrot.x)); + PSA_des.x = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.y)); + PSA_des.y = POS_FLOAT_OF_BFP(POS_BFP_OF_REAL(nav.target.x)); rm_order_h = 3; break; case NAV_SETPOINT_MODE_SPEED: @@ -1535,4 +1845,145 @@ void oneloop_from_nav(bool in_flight) oneloop_andi_run(in_flight, false, PSA_des, rm_order_h, rm_order_v); } +/** @brief Function to calculate corrections for sideslip*/ +float oneloop_andi_sideslip(void) +{ + // Coordinated turn + // feedforward estimate angular rotation omega = g*tan(phi)/v + float omega; + const float max_phi = RadOfDeg(ONELOOP_ANDI_MAX_BANK); + float airspeed_turn = airspeed_filt.o[0]; + Bound(airspeed_turn, 10.0f, 30.0f); + // Use the current roll angle to determine the corresponding heading rate of change. + float coordinated_turn_roll = eulers_zxy.phi; + // Prevent flipping + if( (andi_u[ONELOOP_ANDI_THETA_IDX] > 0.0f) && ( fabs(andi_u[ONELOOP_ANDI_PHI_IDX]) < andi_u[ONELOOP_ANDI_THETA_IDX])) { + coordinated_turn_roll = ((andi_u[ONELOOP_ANDI_PHI_IDX] > 0.0f) - (andi_u[ONELOOP_ANDI_PHI_IDX] < 0.0f)) * andi_u[ONELOOP_ANDI_THETA_IDX]; + } + BoundAbs(coordinated_turn_roll, max_phi); + omega = g / airspeed_turn * tanf(coordinated_turn_roll); + #ifdef FWD_SIDESLIP_GAIN + // Add sideslip correction + omega -= accely_filt.o[0]*fwd_sideslip_gain; + #endif + return omega; +} + +/** @brief Function to calculate the position reference during the chirp*/ +static float chirp_pos_p_ref(float delta_t, float f0, float k, float A){ + float p_ref_fun = sinf(delta_t * M_PI * (f0 + k * delta_t) * 2.0); + return (A * p_ref_fun); +} +/** @brief Function to calculate the velocity reference during the chirp*/ +static float chirp_pos_v_ref(float delta_t, float f0, float k, float A){ + 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); + return (A * v_ref_fun); +} +/** @brief Function to calculate the acceleration reference during the chirp*/ +static float chirp_pos_a_ref(float delta_t, float f0, float k, float A){ + 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; + return (A * a_ref_fun); +} +/** @brief Function to calculate the jerk reference during the chirp*/ +static float chirp_pos_j_ref(float delta_t, float f0, float k, float A){ + 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; + return (A * j_ref_fun); +} + +/** + * @brief Reference Model Definition for 3rd order system specific to positioning with bounds + * @param dt [s] time passed since start of the chirp + * @param f0 [Hz] initial frequency of the chirp + * @param f1 [Hz] final frequency of the chirp + * @param t_chirp [s] duration of the chirp + * @param p_ref [m] position reference + * @param v_ref [m/s] velocity reference + * @param a_ref [m/s2] acceleration reference + * @param j_ref [m/s3] jerk reference + */ +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[]) { + f0 = positive_non_zero(f0); + f1 = positive_non_zero(f1); + t_chirp = positive_non_zero(t_chirp); + A = positive_non_zero(A); + if ((f1-f0) < -FLT_EPSILON){ + f1 = f0; + } + // 0 body x, 1 body y, 2 body z, 3 pitch pref + if (n > 3){ + n = 0; + } + if (n < 0){ + n = 0; + } + // 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); + float a_ref_chirp = chirp_pos_a_ref(time_elapsed, f0, k, A); + float j_ref_chirp = chirp_pos_j_ref(time_elapsed, f0, k, A); + + float spsi = sinf(psi); + float cpsi = cosf(psi); + float mult_0 = 0.0; + float mult_1 = 0.0; + float mult_2 = 0.0; + if (n == 0){ + mult_0 = cpsi; + mult_1 = spsi; + mult_2 = 0.0; + }else if(n==1){ + mult_0 = -spsi; + mult_1 = cpsi; + mult_2 = 0.0; + }else if(n==2){ + mult_0 = 0.0; + mult_1 = 0.0; + mult_2 = 1.0; + } + // Do not overwrite the reference if chirp is not on that axis + if (n == 2){ + p_ref[2] = p_ref_0[2] + p_ref_chirp * mult_2; + v_ref[2] = v_ref_chirp * mult_2; + a_ref[2] = a_ref_chirp * mult_2; + j_ref[2] = j_ref_chirp * mult_2; + } else if (n < 2){ + p_ref[0] = p_ref_0[0] + p_ref_chirp * mult_0; + p_ref[1] = p_ref_0[1] + p_ref_chirp * mult_1; + v_ref[0] = v_ref_chirp * mult_0; + v_ref[1] = v_ref_chirp * mult_1; + a_ref[0] = a_ref_chirp * mult_0; + a_ref[1] = a_ref_chirp * mult_1; + j_ref[0] = j_ref_chirp * mult_0; + j_ref[1] = j_ref_chirp * mult_1; + } else { //Pitch preferred chirp, for now a little bit hacked in... + pitch_pref = p_ref_chirp; + pitch_pref = (pitch_pref / A + 1.0) * (theta_pref_max / 2.0); + float pitch_offset = RadOfDeg(5.0); + pitch_pref = pitch_pref + pitch_offset; + Bound(pitch_pref,0.0,25.0); + } +} +void 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[]){ + if (*chirp_on){ + if (*chirp_first_call){ + *time_elapsed = 0.0; + *chirp_first_call = false; + *t_0 = get_sys_time_float(); + p_ref_0[0] = p_ref[0]; + p_ref_0[1] = p_ref[1]; + p_ref_0[2] = p_ref[2]; + } + if (*time_elapsed < t_chirp){ + *time_elapsed = get_sys_time_float() - *t_0; + chirp_pos(*time_elapsed, f0, f1, t_chirp, A, n, psi, p_ref, v_ref, a_ref, j_ref, p_ref_0); + } else { + *chirp_on = false; + *chirp_first_call = true; + *time_elapsed = 0.0; + *t_0 = 0.0; + oneloop_andi_enter(false, oneloop_andi.ctrl_type); + } + } +} \ No newline at end of file diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h index 430be73ed7a..f57b5093085 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h @@ -32,6 +32,10 @@ #define ANDI_G_SCALING 1000.0f +/** Control types.*/ +#define CTRL_ANDI 0 +#define CTRL_INDI 1 + extern float act_state_filt_vect_1l[ANDI_NUM_ACT]; extern float actuator_state_1l[ANDI_NUM_ACT]; extern float nu[6]; @@ -39,6 +43,19 @@ extern float g1g2_1l[ANDI_OUTPUTS][ANDI_NUM_ACT_TOT]; extern float andi_u[ANDI_NUM_ACT_TOT]; extern float andi_du[ANDI_NUM_ACT_TOT]; extern float psi_des_deg; +extern bool heading_manual; +extern bool yaw_stick_in_auto; +extern float fwd_sideslip_gain; +extern struct FloatEulers eulers_zxy_des; +extern float psi_des_rad; + +/*Chirp test Variables*/ +extern bool chirp_on; +extern float f0_chirp; +extern float f1_chirp; +extern float t_chirp; +extern float A_chirp; +extern int8_t chirp_axis; // Delete once hybrid nav is fixed ////////////////////////////////////////////////////////////////////////////////// struct guidance_indi_hybrid_params { @@ -81,6 +98,7 @@ struct OneloopStabilizationState { }; struct OneloopGeneral { bool half_loop; + int ctrl_type; struct OneloopGuidanceRef gui_ref; // Guidance References struct OneloopGuidanceState gui_state; // Guidance State struct OneloopStabilizationRef sta_ref; // Stabilization References @@ -125,7 +143,7 @@ extern struct Gains2ndOrder k_head_rm; extern struct Gains3rdOrder k_pos_e; extern struct Gains3rdOrder k_pos_rm; extern void oneloop_andi_init(void); -extern void oneloop_andi_enter(bool half_loop_sp); +extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type); extern void oneloop_andi_set_failsafe_setpoint(void); extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c index c19554dcd09..bc098ee32c2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_oneloop.c @@ -48,7 +48,7 @@ void stabilization_attitude_init(void) void stabilization_attitude_enter(void) { - oneloop_andi_enter(true); + oneloop_andi_enter(true, CTRL_ANDI); } void stabilization_attitude_set_failsafe_setpoint(void) diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c index 1c82552cf3b..87a63955d29 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c @@ -30,7 +30,9 @@ // if NAV_HYBRID_MAX_BANK is not defined, set it to 30 degrees. #ifndef NAV_HYBRID_MAX_BANK -#define NAV_HYBRID_MAX_BANK 0.52f +float nav_hybrid_max_bank = 0.52f; +#else +float nav_hybrid_max_bank = NAV_HYBRID_MAX_BANK; #endif // Max ground speed that will be commanded @@ -74,13 +76,16 @@ float nav_hybrid_line_gain = 1.0f; #ifdef NAV_HYBRID_POS_GAIN float nav_hybrid_pos_gain = NAV_HYBRID_POS_GAIN; #else -float nav_hybrid_pos_gain = 1.0; +float nav_hybrid_pos_gain = 1.0f; #endif #ifndef GUIDANCE_INDI_HYBRID -bool force_forward = 0; +bool force_forward = 0.0f; #endif +#ifndef NAV_OPTITRACK +#define NAV_OPTITRACK FALSE +#endif /** Implement basic nav function for the hybrid case */ @@ -118,7 +123,13 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp) VECT2_COPY(nav.speed, speed_sp); nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT; - nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED; + + // In optitrack tests use position mode for more precise hovering + if (NAV_OPTITRACK){ + nav.setpoint_mode = NAV_SETPOINT_MODE_POS; + }else{ + nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED; + } } static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end) @@ -252,7 +263,7 @@ static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius) } else { // close to circle, speed function of radius for a feasible turn // 0.8 * MAX_BANK gives some margins for the turns - desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * NAV_HYBRID_MAX_BANK)); + desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(0.8f * nav_hybrid_max_bank)); } Bound(desired_speed, 0.0f, nav_max_speed); } diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h index 5063d237bc9..b2fe0c8ba51 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.h @@ -37,7 +37,7 @@ extern float nav_goto_max_speed; // max speed in goto/stay mode extern float nav_max_deceleration_sp; extern float nav_hybrid_line_gain; extern float nav_hybrid_pos_gain; - +extern float nav_hybrid_max_bank; #ifndef GUIDANCE_INDI_HYBRID extern bool force_forward; #endif From 46572ed66963eac59e52b1e58827ce193a4956a4 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Mon, 8 Apr 2024 18:16:15 +0200 Subject: [PATCH 02/14] merge airframes --- conf/airframes/tudelft/rot_wing_v3c_oneloop.xml | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml index b20dd4d0864..429434b5270 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -7,7 +7,7 @@ --> - RotatingWingV3C for outdoor flight with INS EKF2 + RotatingWingV3C for outdoor flight and simulation with INS EKF2 @@ -85,7 +85,10 @@ - + + + + @@ -104,7 +107,7 @@ - + @@ -278,7 +281,7 @@ - + @@ -300,10 +303,10 @@
- - + + - +
From cee23260e3cb987b94abeefd96258d0cd602bea3 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Mon, 8 Apr 2024 18:17:34 +0200 Subject: [PATCH 03/14] Delete unused airframe --- .../rot_wing_v3c_oneloop_simulation.xml | 278 ------------------ 1 file changed, 278 deletions(-) delete mode 100644 conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml deleted file mode 100644 index 4de976a5aac..00000000000 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml +++ /dev/null @@ -1,278 +0,0 @@ - - - - RotatingWingV3C for simulation - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- - - - - - - - - - - - - - - - - - - -
-
- - - - - - - -
- -
- - - - - - -
- -
- - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
- -
- - - - - -
- -
- - - - - - -
- -
- - - - - - -
- From 206702fb06caa15247610c49a8325c4499d607ea Mon Sep 17 00:00:00 2001 From: Tomaso Date: Mon, 8 Apr 2024 18:45:22 +0200 Subject: [PATCH 04/14] code clean up --- .../rotorcraft/oneloop/oneloop_andi.c | 60 +++---------------- 1 file changed, 7 insertions(+), 53 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index e1d12f39c65..f779e378f3b 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -506,36 +506,20 @@ 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_att_des[3]; - // temp_att_des[0] = eulers_zxy_des.phi; - // temp_att_des[1] = eulers_zxy_des.theta; - // temp_att_des[2] = eulers_zxy_des.psi; - // debug_vect(trans, dev, "att_des", temp_att_des, 3); - // float temp_debug_vect[ANDI_NUM_ACT_TOT+1]; - // for (int i = 0; i < ANDI_NUM_ACT_TOT; i++) { - // temp_debug_vect[i] = andi_u[i]; - // } - // temp_debug_vect[ANDI_NUM_ACT_TOT] = pitch_pref; - // debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, ANDI_NUM_ACT_TOT+1); float temp_debug_vect[2]; temp_debug_vect[0] = andi_u[ONELOOP_ANDI_THETA_IDX]; temp_debug_vect[1] = pitch_pref; debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, 2); - // debug_vect(trans, dev, "andi_u", andi_u, ANDI_NUM_ACT); - // float temp_pref_vect[1]={pitch_pref}; - // debug_vect(trans, dev, "pitch_pref", temp_pref_vect, 1); - // float rate_vect_temp[3] = {stateGetBodyRates_f()->p, stateGetBodyRates_f()->q, stateGetBodyRates_f()->r}; - // debug_vect(trans, dev, "att_rate", rate_vect_temp, 3); } static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) { struct Int32Quat quat_ext_pose; #ifdef INS_EXT_POSE - quat_ext_pose.qi = ins_ext_pos.ev_quat.qi/0.0000305;//(int32_t) - quat_ext_pose.qx = ins_ext_pos.ev_quat.qx/0.0000305;//(int32_t) - quat_ext_pose.qy = ins_ext_pos.ev_quat.qy/0.0000305;//(int32_t) - quat_ext_pose.qz = ins_ext_pos.ev_quat.qz/0.0000305;//(int32_t) + quat_ext_pose.qi = ins_ext_pos.ev_quat.qi/0.0000305; + quat_ext_pose.qx = ins_ext_pos.ev_quat.qx/0.0000305; + quat_ext_pose.qy = ins_ext_pos.ev_quat.qy/0.0000305; + quat_ext_pose.qz = ins_ext_pos.ev_quat.qz/0.0000305; #else quat_ext_pose.qi = 0; quat_ext_pose.qx = 0; @@ -1083,21 +1067,11 @@ void init_controller(void){ /*Register a variable from nav_hybrid. SHould be improved when nav hybrid is final.*/ max_v_nav = nav_max_speed; /*Some calculations in case new poles have been specified*/ - //p_att_e.p3 = p_att_e.omega_n * p_att_e.zeta; p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; - //p_pos_e.p3 = p_pos_e.omega_n * p_pos_e.zeta; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; - //p_alt_e.p3 = p_alt_e.omega_n * p_alt_e.zeta; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; - //p_head_e.p3 = p_head_e.omega_n * p_head_e.zeta; p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; - - //printf("p_att_e.omega_n = %f \n", p_att_e.omega_n); - //printf("p_head_e.omega_n = %f\n", p_head_e.omega_n); - //printf("p_pos_e.omega_n = %f\n", p_pos_e.omega_n); - //printf("p_alt_e.omega_n = %f\n", p_alt_e.omega_n); - //--ANDI Controller gains -------------------------------------------------------------------------------- /*Attitude Loop*/ k_att_e.k1[0] = k_e_1_3_f_v2(p_att_e.omega_n, p_att_e.zeta, p_att_e.p3); @@ -1147,16 +1121,6 @@ void init_controller(void){ k_pos_rm.k1[2] = k_rm_1_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k2[2] = k_rm_2_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); k_pos_rm.k3[2] = k_rm_3_3_f(p_alt_rm.omega_n, p_alt_rm.zeta, p_alt_rm.p3); - - // ANDI Gains check - printf("k_att_e [k1,k2,k3] = [%f,%f,%f]\n",k_att_e.k1[0],k_att_e.k2[0],k_att_e.k3[0]); - printf("k_att_rm [k1,k2,k3] = [%f,%f,%f]\n",k_att_rm.k1[0],k_att_rm.k2[0],k_att_rm.k3[0]); - printf("k_att_head [k1,k2,k3] = [%f,%f,%f]\n",k_att_e.k1[2],k_att_e.k2[2],k_att_e.k3[2]); - printf("k_att_head_rm [k1,k2,k3] = [%f,%f,%f]\n",k_att_rm.k1[2],k_att_rm.k2[2],k_att_rm.k3[2]); - printf("k_pos_e [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e.k1[0],k_pos_e.k2[0],k_pos_e.k3[0]); - printf("k_pos_rm [k1,k2,k3] = [%f,%f,%f]\n",k_pos_rm.k1[0],k_pos_rm.k2[0],k_pos_rm.k3[0]); - printf("k_alt_e [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e.k1[2],k_pos_e.k2[2],k_pos_e.k3[2]); - printf("k_alt_rm [k1,k2,k3] = [%f,%f,%f]\n",k_pos_rm.k1[2],k_pos_rm.k2[2],k_pos_rm.k3[2]); //--INDI Controller gains -------------------------------------------------------------------------------- /*Attitude Loop*/ @@ -1672,17 +1636,11 @@ void sum_g1g2_1l(int ctrl_type) { float s3skew = sskew * s2skew; float c3skew = cskew * c2skew; - // float T = 0.0; - // for (i = 0; i < 4; i++){ - // T += actuator_state_1l[i] * g1_1l[2][i]; - // } - // T = T / num_thrusters_oneloop; - // Thrust and Pusher force estimation - float T = -g/(cphi*ctheta);//-9.81; //minus gravity is a guesstimate of the thrust force, thrust measurement would be better + float T = -g/(cphi*ctheta);//minus gravity is a guesstimate of the thrust force, thrust measurement would be better float P = 0.0; if (ONELOOP_ANDI_AC_HAS_PUSHER){ - P = actuator_state_1l[ONELOOP_ANDI_PUSHER_IDX] * g1_1l[2][ONELOOP_ANDI_PUSHER_IDX] / ANDI_G_SCALING; // DIVIDE BY MASS!!!!!!! + P = actuator_state_1l[ONELOOP_ANDI_PUSHER_IDX] * g1_1l[2][ONELOOP_ANDI_PUSHER_IDX] / ANDI_G_SCALING; } float scaler = 1.0; float sched_p = 1.0; // Scheduler variable for roll axis @@ -1781,11 +1739,7 @@ void calc_model(int ctrl_type){ float ctheta = cosf(eulers_zxy.theta); float spsi = sinf(eulers_zxy.psi); float cpsi = cosf(eulers_zxy.psi); - // float T = 0.0; - // for (i = 0; i < 4; i++){ - // T += actuator_state_1l[i] * g1_1l[2][i]; - // } - // T = T / num_thrusters_oneloop; + float T = -g/(cphi*ctheta); // -9.81; float P = 0.0; if (ONELOOP_ANDI_AC_HAS_PUSHER){ From fefb2264c1562ad334cae33122fbd125a9b9aba6 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Tue, 9 Apr 2024 11:49:05 +0200 Subject: [PATCH 05/14] removed unused functions and some extra clean up --- .../rotorcraft/oneloop/oneloop_andi.c | 69 ++++--------------- 1 file changed, 14 insertions(+), 55 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index f779e378f3b..b5196020293 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -538,29 +538,6 @@ static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *d &(quat->qy), &(quat->qz)); } - -// static void send_oneloop_nps(struct transport_tx *trans, struct link_device *dev) -// { -// float nps_acc_N = (float) fdm.ltpprz_ecef_accel.x; -// float nps_acc_E = (float) fdm.ltpprz_ecef_accel.y; -// float nps_acc_D = (float) fdm.ltpprz_ecef_accel.z; -// float nps_vel_N = (float) fdm.ltpprz_ecef_vel.x ; -// float nps_vel_E = (float) fdm.ltpprz_ecef_vel.y ; -// float nps_vel_D = (float) fdm.ltpprz_ecef_vel.z ; -// float nps_pos_N = (float) fdm.ltpprz_pos.x ; -// float nps_pos_E = (float) fdm.ltpprz_pos.y ; -// float nps_pos_D = (float) fdm.ltpprz_pos.z ; -// pprz_msg_send_NPS_SPEED_POS(trans, dev, AC_ID, -// &nps_acc_N, -// &nps_acc_E, -// &nps_acc_D, -// &nps_vel_N, -// &nps_vel_E, -// &nps_vel_D, -// &nps_pos_N, -// &nps_pos_E, -// &nps_pos_D); -// } #endif /** @brief Function to make sure that inputs are positive non zero vaues*/ @@ -965,24 +942,6 @@ void ec_3rd_att(float y_4d[3], float x_ref[3], float x_d_ref[3], float x_2d_ref[ float_vect_sum(y_4d, y_4d, y_4d_3, 3); } -/** - * @brief Error Controller Definition for 2rd order system - * @param dt Delta time [s] - * @param x_ref Reference signal 1st order - * @param x_d_ref Reference signal 2nd order - * @param x_2d_ref Reference signal 3rd order - * @param x_des Desired 1st order signal - * @param x Current 1st order signal - * @param x_d Current 2nd order signal - * @param k1_e Error Controller Gain 1st order signal - * @param k2_e Error Controller Gain 2nd order signal - */ -// static float ec_2rd(float x_ref, float x_d_ref, float x_2d_ref, float x, float x_d, float k1_e, float k2_e){ -// float y_3d = k1_e*(x_ref-x)+k2_e*(x_d_ref-x_d)+x_2d_ref; -// return y_3d; -// } - - /** * @brief Third Order to First Order Dynamics Approximation * @param p1 Pole 1 @@ -996,7 +955,7 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ p3 = positive_non_zero(p3); rm_k = positive_non_zero(rm_k); float tao = (p1*p2+p1*p3+p2*p3)/(p1*p2*p3)/(rm_k); - tao = positive_non_zero(tao); + tao = positive_non_zero(tao); return 1.0/tao; } @@ -1021,19 +980,19 @@ void init_poles(void){ // Attitude Controller Poles---------------------------------------------------------- float slow_pole = 10.1; // Pole of the slowest dynamics used in the attitude controller - p_att_e.omega_n = 4.50;//sqrt(pow(7.68,3)/slow_pole); + p_att_e.omega_n = 4.50; p_att_e.zeta = 1.0; - p_att_e.p3 = slow_pole; //p_att_e.omega_n * p_att_e.zeta; + p_att_e.p3 = slow_pole; - p_att_rm.omega_n = 4.71;//6.14; + p_att_rm.omega_n = 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;//sqrt(pow(3.4,3)/slow_pole);//P3_2_P2Omega(1.5,slow_pole);// sqrt(pow(1.5,3)/slow_pole);//1.5;// + p_head_e.omega_n = 1.80; p_head_e.zeta = 1.0; - p_head_e.p3 = slow_pole;//p_head_e.omega_n * p_head_e.zeta;// + p_head_e.p3 = slow_pole; - p_head_rm.omega_n = 2.56;//1.5; + p_head_rm.omega_n = 2.56; p_head_rm.zeta = 1.0; p_head_rm.p3 = p_head_rm.omega_n * p_head_rm.zeta; @@ -1043,19 +1002,19 @@ void init_poles(void){ // Position Controller Poles---------------------------------------------------------- slow_pole = act_dynamics[ONELOOP_ANDI_PHI_IDX]; // Pole of the slowest dynamics used in the position controller - p_pos_e.omega_n = 1.0;//sqrt(pow(1.41,3)/slow_pole); + p_pos_e.omega_n = 1.0; p_pos_e.zeta = 1.0; - p_pos_e.p3 = slow_pole; //p_pos_e.omega_n * p_pos_e.zeta; + p_pos_e.p3 = slow_pole; - p_pos_rm.omega_n = 0.93;//0.6; + p_pos_rm.omega_n = 0.93; p_pos_rm.zeta = 1.0; p_pos_rm.p3 = p_pos_rm.omega_n * p_pos_rm.zeta; - p_alt_e.omega_n = 3.0;//sqrt(pow(3.54,3)/slow_pole); + p_alt_e.omega_n = 3.0; p_alt_e.zeta = 1.0; - p_alt_e.p3 = slow_pole; //p_alt_e.omega_n * p_alt_e.zeta; // + p_alt_e.p3 = slow_pole; - p_alt_rm.omega_n = 1.93;//2.0; + p_alt_rm.omega_n = 1.93; p_alt_rm.zeta = 1.0; p_alt_rm.p3 = p_alt_rm.omega_n * p_alt_rm.zeta; } @@ -1064,7 +1023,7 @@ void init_poles(void){ * FIXME: Calculate the gains dynamically for transition */ void init_controller(void){ - /*Register a variable from nav_hybrid. SHould be improved when nav hybrid is final.*/ + /*Register a variable from nav_hybrid. Should be improved when nav hybrid is final.*/ max_v_nav = nav_max_speed; /*Some calculations in case new poles have been specified*/ p_att_rm.p3 = p_att_rm.omega_n * p_att_rm.zeta; From 9ef5660880369b00498cba90ad10270b869f8538 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Tue, 9 Apr 2024 13:04:45 +0200 Subject: [PATCH 06/14] Removed other unused functions --- .../rotorcraft/oneloop/oneloop_andi.c | 79 +++---------------- 1 file changed, 11 insertions(+), 68 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index b5196020293..116056d797a 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -551,39 +551,6 @@ static float positive_non_zero(float input) /** @brief Error Controller Gain Design */ -static float k_e_1_3_f(float p1, float p2, float p3) { - p1 = positive_non_zero(p1); - p2 = positive_non_zero(p2); - p3 = positive_non_zero(p3); - return (p1 * p2 * p3); -} - -static float k_e_2_3_f(float p1, float p2, float p3) { - p1 = positive_non_zero(p1); - p2 = positive_non_zero(p2); - p3 = positive_non_zero(p3); - return (p1 * p2 + p1 * p3 + p2 * p3); -} - -static float k_e_3_3_f(float p1, float p2, float p3) { - p1 = positive_non_zero(p1); - p2 = positive_non_zero(p2); - p3 = positive_non_zero(p3); - return (p1 + p2 + p3); -} - -static float k_e_1_2_f(float p1, float p2) { - p1 = positive_non_zero(p1); - p2 = positive_non_zero(p2); - return (p1 * p2); -} - -static float k_e_2_2_f(float p1, float p2) { - p1 = positive_non_zero(p1); - p2 = positive_non_zero(p2); - return (p1 + p2); -} - static float k_e_1_2_f_v2(float omega, float zeta) { omega = positive_non_zero(omega); zeta = positive_non_zero(zeta); @@ -639,18 +606,6 @@ static float k_rm_3_3_f(float omega_n, float zeta, float p1) { return p1 + omega_n * zeta * 2.0; } -static float k_rm_1_2_f(float omega_n, float zeta) { - omega_n = positive_non_zero(omega_n); - zeta = positive_non_zero(zeta); - return omega_n / (2.0 * zeta); -} - -static float k_rm_2_2_f(float omega_n, float zeta) { - omega_n = positive_non_zero(omega_n); - zeta = positive_non_zero(zeta); - return 2.0 * zeta * omega_n; -} - /** @brief Attitude Rates to Euler Conversion Function */ void float_rates_of_euler_dot_vec(float r[3], float e[3], float edot[3]) { @@ -959,18 +914,6 @@ static float w_approx(float p1, float p2, float p3, float rm_k){ return 1.0/tao; } -/** - * @brief 3 Coincident poles to 2 coincident and act dynamics Approximation - * @param p3 Pole - * @param omega Dynamics Slowest actuator - */ -static float P3_2_P2Omega(float p3, float omega){ - p3 = positive_non_zero(p3); - omega = positive_non_zero(omega); - float p2 = -omega + sqrt(pow(omega,2) + 3 * pow(p3,2)); - p2 = positive_non_zero(p2); - return p2; -} /** * @brief Initialize Position of Poles * @@ -1584,16 +1527,16 @@ void sum_g1g2_1l(int ctrl_type) { float spsi = sinf(eulers_zxy.psi); float cpsi = cosf(eulers_zxy.psi); // Trig of skew - float skew = 0.0; - if (ONELOOP_ANDI_SCHEDULING){ - skew = rotwing_state_skewing.wing_angle_deg; - } - float sskew = sinf(skew); - float cskew = cosf(skew); - float s2skew = sskew * sskew; - float c2skew = cskew * cskew; - float s3skew = sskew * s2skew; - float c3skew = cskew * c2skew; + // float skew = 0.0; + // if (ONELOOP_ANDI_SCHEDULING){ + // skew = rotwing_state_skewing.wing_angle_deg; + // } + // float sskew = sinf(skew); + // float cskew = cosf(skew); + // float s2skew = sskew * sskew; + // float c2skew = cskew * cskew; + // float s3skew = sskew * s2skew; + // float c3skew = cskew * c2skew; // Thrust and Pusher force estimation float T = -g/(cphi*ctheta);//minus gravity is a guesstimate of the thrust force, thrust measurement would be better @@ -1804,7 +1747,7 @@ static float chirp_pos_j_ref(float delta_t, float f0, float k, float A){ } /** - * @brief Reference Model Definition for 3rd order system specific to positioning with bounds + * @brief Function to perform position and attitude chirps * @param dt [s] time passed since start of the chirp * @param f0 [Hz] initial frequency of the chirp * @param f1 [Hz] final frequency of the chirp From 4bb59da9a2835cce3c0230b82f0d03d25bbc8f49 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:00:11 +0200 Subject: [PATCH 07/14] Leave out rotwing state fr now, will be addressed in the next PR --- conf/airframes/tudelft/rot_wing_v3c_oneloop.xml | 7 ++++--- .../tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml | 9 +++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml index 429434b5270..240e870ff44 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -131,11 +131,11 @@ - + @@ -200,7 +200,8 @@ - + + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml index 368b7ed4716..e8d7e3ae19f 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml @@ -113,7 +113,7 @@ - + @@ -129,11 +129,11 @@ - + @@ -198,7 +198,8 @@ - + + From b5a4d99cedb99fad18e3e1e035a1b67e168b5bbe Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:00:57 +0200 Subject: [PATCH 08/14] Removed ins ext pose quaternion messages from oneloop controller --- .../rotorcraft/oneloop/oneloop_andi.c | 29 ------------------- 1 file changed, 29 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index 116056d797a..f61a02e9cff 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -511,33 +511,6 @@ static void send_oneloop_debug(struct transport_tx *trans, struct link_device *d temp_debug_vect[1] = pitch_pref; debug_vect(trans, dev, "andi_u_pitch_pref", temp_debug_vect, 2); } - -static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) -{ - struct Int32Quat quat_ext_pose; - #ifdef INS_EXT_POSE - quat_ext_pose.qi = ins_ext_pos.ev_quat.qi/0.0000305; - quat_ext_pose.qx = ins_ext_pos.ev_quat.qx/0.0000305; - quat_ext_pose.qy = ins_ext_pos.ev_quat.qy/0.0000305; - quat_ext_pose.qz = ins_ext_pos.ev_quat.qz/0.0000305; - #else - quat_ext_pose.qi = 0; - quat_ext_pose.qx = 0; - quat_ext_pose.qy = 0; - quat_ext_pose.qz = 0; - #endif - - struct Int32Quat *quat = stateGetNedToBodyQuat_i(); - pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID, - &quat_ext_pose.qi,// In the future also stream setpoint in quat &stab_att_sp_quat.qi, - &quat_ext_pose.qx,// In the future also stream setpoint in quat &stab_att_sp_quat.qx, - &quat_ext_pose.qy,// In the future also stream setpoint in quat &stab_att_sp_quat.qy, - &quat_ext_pose.qz,// In the future also stream setpoint in quat &stab_att_sp_quat.qz, - &(quat->qi), - &(quat->qx), - &(quat->qy), - &(quat->qz)); -} #endif /** @brief Function to make sure that inputs are positive non zero vaues*/ @@ -1173,8 +1146,6 @@ void oneloop_andi_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_oneloop_andi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE, send_guidance_oneloop_andi); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DEBUG_VECT, send_oneloop_debug); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_REF_QUAT, send_ahrs_ref_quat); - //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_NPS_SPEED_POS, send_oneloop_nps); #endif } From 7b626f0f6f637fbd301c406af63a0c21fbdb1048 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:01:26 +0200 Subject: [PATCH 09/14] Conf for RW3C --- conf/userconf/tudelft/conf.xml | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index 39e5a8fca2c..a8d75922763 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -7,7 +7,7 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml" + settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml" gui_color="#ffffc457c457" /> + From d7df0246a7e7a2fef7f4f03cc378692a7f6ee95d Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:02:01 +0200 Subject: [PATCH 10/14] Added NAV_HYBRID_GOTO_MODE as suggested --- sw/airborne/modules/nav/nav_rotorcraft_hybrid.c | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c index 87a63955d29..08f9931dc53 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c @@ -83,9 +83,10 @@ float nav_hybrid_pos_gain = 1.0f; bool force_forward = 0.0f; #endif -#ifndef NAV_OPTITRACK -#define NAV_OPTITRACK FALSE +#ifndef NAV_HYBRID_GOTO_MODE +#define NAV_HYBRID_GOTO_MODE NAV_SETPOINT_MODE_SPEED #endif + /** Implement basic nav function for the hybrid case */ @@ -123,13 +124,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp) VECT2_COPY(nav.speed, speed_sp); nav.horizontal_mode = NAV_HORIZONTAL_MODE_WAYPOINT; - - // In optitrack tests use position mode for more precise hovering - if (NAV_OPTITRACK){ - nav.setpoint_mode = NAV_SETPOINT_MODE_POS; - }else{ - nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED; - } + nav.setpoint_mode = NAV_HYBRID_GOTO_MODE; } static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end) From fe4bfafed879d85cc5f6bba3cd6ab483fb4b95f5 Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:06:15 +0200 Subject: [PATCH 11/14] Use aux 5 to set preferred pitch for the rotating wing drone --- sw/airborne/modules/radio_control/rc_datalink.h | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/modules/radio_control/rc_datalink.h b/sw/airborne/modules/radio_control/rc_datalink.h index 71994decafb..73d30957533 100644 --- a/sw/airborne/modules/radio_control/rc_datalink.h +++ b/sw/airborne/modules/radio_control/rc_datalink.h @@ -44,6 +44,7 @@ #define RADIO_AUX2 6 #define RADIO_AUX4 7 #define RADIO_AUX6 8 +#define RADIO_AUX5 9 extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ]; extern volatile bool rc_dl_frame_available; From 01a5a0280b18dff34baef0b8ddd97af5afe7ca9d Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:16:58 +0200 Subject: [PATCH 12/14] adjusted name --- conf/userconf/tudelft/conf.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml index a8d75922763..8e68b182dbf 100644 --- a/conf/userconf/tudelft/conf.xml +++ b/conf/userconf/tudelft/conf.xml @@ -627,7 +627,7 @@ gui_color="red" /> Date: Thu, 11 Apr 2024 17:17:22 +0200 Subject: [PATCH 13/14] removed printf --- .../firmwares/rotorcraft/oneloop/oneloop_andi.c | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index f61a02e9cff..4645ba03316 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -1024,26 +1024,16 @@ void init_controller(void){ k_pos_e_indi.k2[2] = k_e_2_2_f_v2(p_alt_e.omega_n, p_alt_e.zeta); k_pos_e_indi.k3[2] = 1.0; - // INDI Gains check - printf("k_att_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_att_e_indi.k1[0],k_att_e_indi.k2[0],k_att_e_indi.k3[0]); - printf("k_att_head_indi [k1,k2,k3] = [%f,%f,%f]\n",k_att_e_indi.k1[2],k_att_e_indi.k2[2],k_att_e_indi.k3[2]); - printf("k_pos_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e_indi.k1[0],k_pos_e_indi.k2[0],k_pos_e_indi.k3[0]); - printf("k_alt_e_indi [k1,k2,k3] = [%f,%f,%f]\n",k_pos_e_indi.k1[2],k_pos_e_indi.k2[2],k_pos_e_indi.k3[2]); - //------------------------------------------------------------------------------------------ /*Approximated Dynamics*/ act_dynamics[ONELOOP_ANDI_PHI_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); act_dynamics[ONELOOP_ANDI_THETA_IDX] = w_approx(p_att_rm.p3, p_att_rm.p3, p_att_rm.p3, 1.0); - - printf("act_dynamics[ONELOOP_ANDI_PHI_IDX] = %f\n",act_dynamics[ONELOOP_ANDI_PHI_IDX]); - printf("act_dynamics[ONELOOP_ANDI_THETA_IDX] = %f\n",act_dynamics[ONELOOP_ANDI_THETA_IDX]); } /** @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); 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); From 2ab3add328b719b87291436812fa70b8c2753a2e Mon Sep 17 00:00:00 2001 From: Tomaso Date: Thu, 11 Apr 2024 17:37:36 +0200 Subject: [PATCH 14/14] Use moment defnition in JSBSIM as suggested --- .../jsbsim/aircraft/rotwingv3c_SI.xml | 244 ++++-------------- 1 file changed, 50 insertions(+), 194 deletions(-) diff --git a/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml index 9038387245f..90ba41f02c7 100644 --- a/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml +++ b/conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml @@ -289,310 +289,166 @@ - + fcs/front_motor_lag 10.0 + 0.192 - 0 - 0.192 - 0 - - - 1 + -0.423 0 0 - - - - - - - fcs/front_motor_lag - 10.0 - - - - 0 - -0.192 - 0 - -1 - 0 - 0 - - - - - - - fcs/right_motor_lag - 10.0 - - - 0 - -0.192 - 0 - - - 1 0 - 0 + -1 - + - + fcs/right_motor_lag 10.0 + 0.192 0 - 0.192 + 0.408 0 - -1 - 0 - 0 - - - - - - - fcs/back_motor_lag - 10.0 - - - 0 - 0.192 - 0 - - - 1 0 - 0 + 1 - + - + - fcs/back_motor_lag + fcs/front_motor_lag 10.0 + 0.192 - 0 - -0.192 - 0 - - - -1 + 0.423 0 0 - - - - - - - fcs/left_motor_lag - 10.0 - - - - 0 - -0.192 - 0 - 1 + 0 0 - 0 + -1 - + - + - fcs/left_motor_lag + fcs/right_motor_lag 10.0 + 0.192 0 - 0.192 + -0.408 0 - -1 + 0 0 - 0 + 1 - - + + - + fcs/front_motor_d 10.0 + 0.0096 - 0 - 0.0096 - 0 - - - 1 + -0.423 0 0 - - - - - - - fcs/front_motor_d - 10.0 - - - - 0 - -0.0096 - 0 - -1 - 0 - 0 - - - - - - - fcs/right_motor_d - 10.0 - - - 0 - -0.0096 - 0 - - - 1 0 - 0 + -1 - + - + - fcs/right_motor_d + fcs/right_motor_lag 10.0 + 0.0096 0 - 0.0096 + 0.408 0 - -1 - 0 - 0 - - - - - - - fcs/back_motor_d - 10.0 - - - 0 - 0.0096 - 0 - - - 1 0 - 0 + 1 - + - + - fcs/back_motor_d + fcs/front_motor_lag 10.0 + 0.0096 - 0 - -0.0096 - 0 - - - -1 + 0.423 0 0 - - - - - - - fcs/left_motor_d - 10.0 - - - - 0 - -0.0096 - 0 - 1 + 0 0 - 0 + -1 - + - + - fcs/left_motor_d + fcs/right_motor_lag 10.0 + 0.0096 0 - 0.0096 + -0.408 0 - -1 + 0 0 - 0 + 1 - +