diff --git a/conf/airframes/ENAC/quadrotor/hexa_tilted_motors.xml b/conf/airframes/ENAC/quadrotor/hexa_tilted_motors.xml new file mode 100644 index 0000000000..6224118dd3 --- /dev/null +++ b/conf/airframes/ENAC/quadrotor/hexa_tilted_motors.xml @@ -0,0 +1,264 @@ + + + + + + Fully actuated hexa copter with tilted motors + + * Autopilot: Tawaki + * Telemetry: XBee + * GPS: datalink + * RC: SBUS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + +
+ +
+ + + + +
+ +
+ +
+ +
+ + + +
+ +
+ + + + +
+ +
+ +
+ +
+ + + + +
+ +
+ + + +
+ +
+ + + + + +
+ + + diff --git a/conf/airframes/ENAC/quadrotor/hoops_112_hinf_outdoor.xml b/conf/airframes/ENAC/quadrotor/hoops_112_hinf_outdoor.xml new file mode 100644 index 0000000000..a9f4a6b515 --- /dev/null +++ b/conf/airframes/ENAC/quadrotor/hoops_112_hinf_outdoor.xml @@ -0,0 +1,265 @@ + + + + + + + * Autopilot: TawakiV2 + * Actuators: 4 in 1 + * Telemetry: XBee + * GPS: Ublox M10 + * RC: SBUS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + + + +
+ +
+ + + + + +
+ +
+ + + +
+ +
+ + + + +
+ + + diff --git a/conf/modules/guidance_indi.xml b/conf/modules/guidance_indi.xml index 038ef97457..fd5bf91f08 100644 --- a/conf/modules/guidance_indi.xml +++ b/conf/modules/guidance_indi.xml @@ -3,30 +3,11 @@ - Guidance controller for rotorcraft using INDI + Quadrotor indi guidance (meta-module for backward compatibility) - - - - - - - - - - @navigation,guidance_rotorcraft - guidance,attitude_command + guidance_indi_quadrotor -
- -
- - - - - - - + diff --git a/conf/modules/guidance_indi_base.xml b/conf/modules/guidance_indi_base.xml new file mode 100644 index 0000000000..20ef3ba545 --- /dev/null +++ b/conf/modules/guidance_indi_base.xml @@ -0,0 +1,33 @@ + + + + + + Base guidance controller for rotorcraft using INDI + Requires a model, e.g. quadrotor, fully_actuated, ... + + + + + + + + + + + + + @navigation,guidance_rotorcraft + guidance,attitude_command + +
+ +
+ + + + + + + + diff --git a/conf/modules/guidance_indi_fully_actuated.xml b/conf/modules/guidance_indi_fully_actuated.xml new file mode 100644 index 0000000000..a95fb0c72d --- /dev/null +++ b/conf/modules/guidance_indi_fully_actuated.xml @@ -0,0 +1,29 @@ + + + + + + Fully actuated hexacopter model for INDI guidance + + +
+ + + +
+ + + guidance_indi_base + + + + + + + + + + + + + diff --git a/conf/modules/guidance_indi_hinf.xml b/conf/modules/guidance_indi_hinf.xml new file mode 100644 index 0000000000..e8d07c5d5f --- /dev/null +++ b/conf/modules/guidance_indi_hinf.xml @@ -0,0 +1,35 @@ + + + + + + Hinfinity controller for INDI guidance outer loop + + +
+ + + + + + + + + + + + + + + + +
+ + + guidance_indi + + + + + + diff --git a/conf/modules/guidance_indi_quadrotor.xml b/conf/modules/guidance_indi_quadrotor.xml new file mode 100644 index 0000000000..7615ce3128 --- /dev/null +++ b/conf/modules/guidance_indi_quadrotor.xml @@ -0,0 +1,16 @@ + + + + + + Basic quadrotor model for INDI guidance + + + + guidance_indi_base + + + + + + diff --git a/conf/modules/stabilization_indi_hinf.xml b/conf/modules/stabilization_indi_hinf.xml new file mode 100644 index 0000000000..9a5a02156e --- /dev/null +++ b/conf/modules/stabilization_indi_hinf.xml @@ -0,0 +1,30 @@ + + + + + + Hinfinity controller for INDI stabilization outer loop + + +
+ + + + + + + + +
+ + + stabilization_indi + + + + + + + + + diff --git a/conf/simulator/jsbsim/aircraft/FAHEX.xml b/conf/simulator/jsbsim/aircraft/FAHEX.xml new file mode 100644 index 0000000000..0ce0afa989 --- /dev/null +++ b/conf/simulator/jsbsim/aircraft/FAHEX.xml @@ -0,0 +1,579 @@ + + + + + + Mohamad Hachem + 1-02-2025 + Version 1.0 + + Model for Fully Actauted Hexacopter copied from anton quadrotor Done by Gautier Hattenberge + Simple Quadrotor in X configuration + Motor: T-Motor F40 + prop 6045 + Include rotor dynamic + NE/SW turning CCW, NW/SE CW + + Drag model is linear with speed and correspond to the quadrotor used in + IMAV2021 paper "Estimating wind using a quadrotor" (Hattenberger, Bronz, Condomines) + + + + + 1 + 1 + 1 + 0 + 0 + 0 + 0 + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + 0 + 0 + 0 + + + + + 0.0068 + 0.0068 + 0.0136 + 0. + 0. + 0. + 0.65 + + 0 + 0 + 0 + + + + + + + -0.25 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0.25 + 0 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + 0.25 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + 0. + -0.25 + -0.1 + + 0.8 + 0.5 + 500 + 100 + 1000 + 0.0 + NONE + 0 + + + + + + fcs/motor_lag + fcs/motor_d_filter + fcs/g1_gain + fcs/g2_gain + + + + + + fcs/motor1 + fcs/motor_lag + fcs/motor1_lag + + + fcs/motor2 + fcs/motor_lag + fcs/motor2_lag + + + fcs/motor3 + fcs/motor_lag + fcs/motor3_lag + + + fcs/motor4 + fcs/motor_lag + fcs/motor4_lag + + + fcs/motor5 + fcs/motor_lag + fcs/motor5_lag + + + fcs/motor6 + fcs/motor_lag + fcs/motor6_lag + + + + + fcs/motor1 + fcs/motor_d_filter + fcs/motor1_d + + + fcs/motor2 + fcs/motor_d_filter + fcs/motor2_d + + + fcs/motor3 + fcs/motor_d_filter + fcs/motor3_d + + + fcs/motor4 + fcs/motor_d_filter + fcs/motor4_d + + + fcs/motor5 + fcs/motor_d_filter + fcs/motor5_d + + + fcs/motor6 + fcs/motor_d_filter + fcs/motor6_d + + + + + fcs/motor1_d + fcs/g2_gain + fcs/motor1_g2timesactd + + + fcs/motor1_lag + fcs/g1_gain + fcs/motor1_g1timesact + + + fcs/motor1_g2timesactd + fcs/motor1_g1timesact + fcs/motor1_yawcontrol + + + + fcs/motor2_d + fcs/g2_gain + fcs/motor2_g2timesactd + + + fcs/motor2_lag + fcs/g1_gain + fcs/motor2_g1timesact + + + fcs/motor2_g2timesactd + fcs/motor2_g1timesact + fcs/motor2_yawcontrol + + + + fcs/motor3_d + fcs/g2_gain + fcs/motor3_g2timesactd + + + fcs/motor3_lag + fcs/g1_gain + fcs/motor3_g1timesact + + + fcs/motor3_g2timesactd + fcs/motor3_g1timesact + fcs/motor3_yawcontrol + + + + fcs/motor4_d + fcs/g2_gain + fcs/motor4_g2timesactd + + + fcs/motor4_lag + fcs/g1_gain + fcs/motor4_g1timesact + + + fcs/motor4_g2timesactd + fcs/motor4_g1timesact + fcs/motor4_yawcontrol + + + + fcs/motor5_d + fcs/g2_gain + fcs/motor5_g2timesactd + + + fcs/motor5_lag + fcs/g1_gain + fcs/motor5_g1timesact + + + fcs/motor5_g2timesactd + fcs/motor5_g1timesact + fcs/motor5_yawcontrol + + + + fcs/motor6_d + fcs/g2_gain + fcs/motor6_g2timesactd + + + fcs/motor6_lag + fcs/g1_gain + fcs/motor6_g1timesact + + + fcs/motor6_g2timesactd + fcs/motor6_g1timesact + fcs/motor6_yawcontrol + + + + + + + + fcs/motor1 + fcs/motor2 + fcs/motor3 + fcs/motor4 + fcs/motor5 + fcs/motor6 + fcs/motor1_lag + fcs/motor2_lag + fcs/motor3_lag + fcs/motor4_lag + fcs/motor5_lag + fcs/motor6_lag + + + + + + + fcs/motor1_lag + 4.5 + 0.224808943 + + + + -0.1299 + 0.075 + 0 + + + -0.171 + 0.2962 + -0.9397 + + + + + + + fcs/motor2_lag + 4.5 + 0.224808943 + + + + 0.0 + 0.15 + 0 + + + 0.342 + 0 + -0.9397 + + + + + + + fcs/motor3_lag + 4.5 + 0.224808943 + + + + 0.1299 + 0.075 + 0 + + + -0.171 + -0.2962 + -0.9397 + + + + + + + fcs/motor4_lag + 4.5 + 0.224808943 + + + + 0.1299 + -0.075 + 0 + + + -0.171 + 0.2962 + -0.9397 + + + + + + + fcs/motor5_lag + 4.5 + 0.224808943 + + + + 0 + -0.15 + 0 + + + 0.342 + 0 + -0.9397 + + + + + + + fcs/motor6_lag + 4.5 + 0.224808943 + + + + -0.1299 + -0.075 + 0 + + + -0.1710 + -0.2962 + -0.9397 + + + + + + + + + fcs/motor1_yawcontrol + 0.01 + 0.738 + + + + -0.1299 + 0.075 + 0 + + + 0.171 + -0.2962 + 0.9397 + + + + + + + fcs/motor2_yawcontrol + 0.01 + 0.738 + + + + 0.0 + 0.15 + 0 + + + 0.342 + 0 + -0.9397 + + + + + + + fcs/motor3_yawcontrol + 0.01 + 0.738 + + + + 0.1299 + 0.075 + 0 + + + 0.171 + 0.2962 + 0.9397 + + + + + + + fcs/motor4_yawcontrol + 0.01 + 0.738 + + + + 0.1299 + -0.075 + 0 + + + -0.171 + 0.2962 + -0.9397 + + + + + + + fcs/motor5_yawcontrol + 0.01 + 0.738 + + + + 0 + -0.15 + 0 + + + -0.342 + 0 + 0.9397 + + + + + + + fcs/motor6_yawcontrol + 0.01 + 0.738 + + + + -0.1299 + -0.075 + 0 + + + -0.1710 + -0.2962 + -0.9397 + + + + + + + + + + + + + Drag + + velocities/vtrue-fps + 0.3048 + 0.230292 + 0.224808943 + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c index 729a22dc61..3f05c841bd 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c @@ -14,9 +14,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @@ -107,12 +106,43 @@ float thrust_act = 0.f; Butterworth2LowPass filt_accel_ned[3]; Butterworth2LowPass roll_filt; Butterworth2LowPass pitch_filt; +Butterworth2LowPass yaw_filt; Butterworth2LowPass thrust_filt; +static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU]; + +#if GUIDANCE_INDI_USE_WLS +#include "math/wls/wls_alloc.h" + +#define NU_MAX 6 // Example constant value // [dtheta, dphi, dthrust, dtx , dty] +#define NV_MAX 3 // Example constant value (ax,ay,az) +static float du_guidance[GUIDANCE_INDI_NU]; +static float *Bwls_gi[GUIDANCE_INDI_NV]; + +static struct WLS_t wls_guid_p = { + .nu = GUIDANCE_INDI_NU, + .nv = GUIDANCE_INDI_NV, + .gamma_sq = 1000.0, + .v = {0.0}, + .Wv = {100.f, 100.f, 100.f}, // x,y,z + .Wu = {10.f,10.f,0.f,0.f,0.f,10.f}, // minimize the control input (thetq,phi,Tx,Ty,Tz,psi) + .u_pref = {0.0}, + .u_min = {0.0}, + .u_max = {0.0}, + .PC = 0.0, + .SC = 0.0, + .iter = 0 +}; + +#else + +// inverse matrix directly if not using WLS struct FloatMat33 Ga; struct FloatMat33 Ga_inv; struct FloatVect3 control_increment; // [dtheta, dphi, dthrust] +#endif + float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF; float guidance_indi_max_bank = GUIDANCE_H_MAX_BANK; @@ -122,28 +152,35 @@ float time_of_accel_sp_3d = 0.0; struct FloatEulers guidance_euler_cmd; struct ThrustSetpoint thrust_sp; float thrust_in; +float thrust_vect[3]; static void guidance_indi_propagate_filters(struct FloatEulers *eulers); -static void guidance_indi_calcG(struct FloatMat33 *Gmat); -static void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz); + +//------------------------------------------------------------// #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" static void send_indi_guidance(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_GUIDANCE_INDI_HYBRID(trans, dev, AC_ID, - &sp_accel.x, - &sp_accel.y, - &sp_accel.z, - &control_increment.x, - &control_increment.y, - &control_increment.z, - &filt_accel_ned[0].o[0], - &filt_accel_ned[1].o[0], - &filt_accel_ned[2].o[0], - &speed_sp.x, - &speed_sp.y, - &speed_sp.z); + &sp_accel.x, + &sp_accel.y, + &sp_accel.z, +#if GUIDANCE_INDI_USE_WLS + &du_guidance[0], + &du_guidance[1], + &du_guidance[2], +#else + &control_increment.x, + &control_increment.y, + &control_increment.z, +#endif + &filt_accel_ned[0].o[0], + &filt_accel_ned[1].o[0], + &filt_accel_ned[2].o[0], + &speed_sp.x, + &speed_sp.y, + &speed_sp.z); } #endif @@ -188,6 +225,7 @@ void guidance_indi_enter(void) } init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi); init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta); + init_butterworth_2_low_pass(&yaw_filt, tau, sample_time, stateGetNedToBodyEulers_f()->psi); init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in); } @@ -200,63 +238,21 @@ void guidance_indi_enter(void) */ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, float heading_sp) { - struct FloatEulers eulers_yxz; + struct FloatEulers euler_yxz; struct FloatQuat * statequat = stateGetNedToBodyQuat_f(); - float_eulers_of_quat_yxz(&eulers_yxz, statequat); + float_eulers_of_quat_yxz(&euler_yxz, statequat); // set global accel sp variable FIXME clean this sp_accel = *accel_sp; //filter accel to get rid of noise and filter attitude to synchronize with accel - guidance_indi_propagate_filters(&eulers_yxz); + guidance_indi_propagate_filters(&euler_yxz); - // FIXME the ABI message overwrite the accel setpoint - // it update should be replaced by a call to the run function - // If the acceleration setpoint is set over ABI message - if (indi_accel_sp_set_2d) { - sp_accel.x = indi_accel_sp.x; - sp_accel.y = indi_accel_sp.y; - // In 2D the vertical motion is derived from the flight plan - sp_accel.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain; - float dt = get_sys_time_float() - time_of_accel_sp_2d; - // If the input command is not updated after a timeout, switch back to flight plan control - if (dt > 0.5) { - indi_accel_sp_set_2d = false; - } - } else if (indi_accel_sp_set_3d) { - sp_accel.x = indi_accel_sp.x; - sp_accel.y = indi_accel_sp.y; - sp_accel.z = indi_accel_sp.z; - float dt = get_sys_time_float() - time_of_accel_sp_3d; - // If the input command is not updated after a timeout, switch back to flight plan control - if (dt > 0.5) { - indi_accel_sp_set_3d = false; - } - } - // else, don't change sp_accel - -#if GUIDANCE_INDI_RC_DEBUG -#warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!" - //for rc control horizontal, rotate from body axes to NED - float psi = stateGetNedToBodyEulers_f()->psi; - float rc_x = -(radio_control.values[RADIO_PITCH] / 9600.0) * 8.0; - float rc_y = (radio_control.values[RADIO_ROLL] / 9600.0) * 8.0; - sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y; - sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y; - - //for rc vertical control - sp_accel.z = -(radio_control.values[RADIO_THROTTLE] - 4500) * 8.0 / 9600.0; -#endif - - //Calculate matrix of partial derivatives - guidance_indi_calcG_yxz(&Ga, &eulers_yxz); - - //Invert this matrix - MAT33_INV(Ga_inv, Ga); - - struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned[0].o[0], sp_accel.y - filt_accel_ned[1].o[0], sp_accel.z - filt_accel_ned[2].o[0]}; - - //Bound the acceleration error so that the linearization still holds + struct FloatVect3 a_diff = { + sp_accel.x - filt_accel_ned[0].o[0], + sp_accel.y - filt_accel_ned[1].o[0], + sp_accel.z - filt_accel_ned[2].o[0] + }; Bound(a_diff.x, -6.0, 6.0); Bound(a_diff.y, -6.0, 6.0); Bound(a_diff.z, -9.0, 9.0); @@ -269,30 +265,64 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa #endif #endif - //Calculate roll,pitch and thrust command +#if GUIDANCE_INDI_USE_WLS + float m = GUIDANCE_INDI_MASS; + wls_guid_p.v[0] = m*a_diff.x; + wls_guid_p.v[1] = m*a_diff.y; + wls_guid_p.v[2] = m*a_diff.z; + + guidance_indi_set_wls_settings(&wls_guid_p, &euler_yxz, heading_sp); + guidance_indi_calcG(Gmat, euler_yxz); + for (int i = 0; i < GUIDANCE_INDI_NV; i++) { + Bwls_gi[i] = Gmat[i]; + } + wls_alloc(&wls_guid_p,Bwls_gi, 0, 0, 10); + for (int i = 0; i < GUIDANCE_INDI_NU; i++) { + du_guidance [i] = wls_guid_p.u[i]; + } + + guidance_euler_cmd.phi = roll_filt.o[0] + du_guidance[0]; + guidance_euler_cmd.theta = pitch_filt.o[0] + du_guidance[1]; + guidance_euler_cmd.psi = yaw_filt.o[0] + du_guidance[2]; + thrust_vect[0] = du_guidance[3]; // (TX) + thrust_vect[1] = du_guidance[4]; // (TY) + thrust_vect[2] = du_guidance[5]; // (TZ) + thrust_sp = th_sp_from_incr_vect_f(thrust_vect); + +#else // !USE_WLS + + // Calculate matrix of partial derivatives + guidance_indi_calcG(Gmat, euler_yxz); + + RMAT_ELMT(Ga, 0, 0) = Gmat[0][0]; + RMAT_ELMT(Ga, 1, 0) = Gmat[1][0]; + RMAT_ELMT(Ga, 2, 0) = Gmat[2][0]; + RMAT_ELMT(Ga, 0, 1) = Gmat[0][1]; + RMAT_ELMT(Ga, 1, 1) = Gmat[1][1]; + RMAT_ELMT(Ga, 2, 1) = Gmat[2][1]; + RMAT_ELMT(Ga, 0, 2) = Gmat[0][2]; + RMAT_ELMT(Ga, 1, 2) = Gmat[1][2]; + RMAT_ELMT(Ga, 2, 2) = Gmat[2][2]; + // Invert this matrix // FIXME input format + MAT33_INV(Ga_inv, Ga); + // Calculate roll,pitch and thrust command MAT33_VECT3_MUL(control_increment, Ga_inv, a_diff); guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x; - guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y; - guidance_euler_cmd.psi = heading_sp; + guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y; + guidance_euler_cmd.psi = heading_sp; // Compute and store thust setpoint #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN guidance_indi_filter_thrust(); //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust thrust_in = thrust_filt.o[0] + control_increment.z * guidance_indi_specific_force_gain; - Bound(thrust_in, 0, 9600); -#if GUIDANCE_INDI_RC_DEBUG - if (radio_control.values[RADIO_THROTTLE] < 300) { - thrust_in = 0; - } -#endif - // return required thrust + Bound(thrust_in, 0, MAX_PPRZ); thrust_sp = th_sp_from_thrust_i(thrust_in, THRUST_AXIS_Z); #else - float thrust_vect[3]; - thrust_vect[0] = 0.0f; // Fill for quadplanes + // basic quad, no side force + thrust_vect[0] = 0.0f; thrust_vect[1] = 0.0f; thrust_vect[2] = control_increment.z; @@ -300,25 +330,26 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa thrust_sp = th_sp_from_incr_vect_f(thrust_vect); #endif - //Bound euler angles to prevent flipping +#endif // USE_WLS + + // Bound euler angles to prevent flipping Bound(guidance_euler_cmd.phi, -guidance_indi_max_bank, guidance_indi_max_bank); Bound(guidance_euler_cmd.theta, -guidance_indi_max_bank, guidance_indi_max_bank); - //set the quat setpoint with the calculated roll and pitch + // set the quat setpoint with the calculated roll and pitch struct FloatQuat q_sp; float_quat_of_eulers_yxz(&q_sp, &guidance_euler_cmd); return stab_sp_from_quat_f(&q_sp); } -struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode) +struct FloatVect3 WEAK guidance_indi_controller(bool in_flight UNUSED, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode) { struct FloatVect3 pos_err = { 0 }; struct FloatVect3 accel_sp = { 0 }; struct FloatVect3 speed_fb; - if (h_mode == GUIDANCE_INDI_H_ACCEL) { // Speed feedback is included in the guidance when running in ACCEL mode speed_fb.x = 0.; @@ -360,9 +391,43 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc accel_sp.y = speed_fb.y + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y); accel_sp.z = speed_fb.z + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); + return accel_sp; +} + +struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode) +{ + struct FloatVect3 accel_sp; + + // If set and valid, the ABI message overwrite the accel setpoint + // TODO This is not ideal, guided mode with accel setpoint would be better + if (indi_accel_sp_set_2d) { + accel_sp.x = indi_accel_sp.x; + accel_sp.y = indi_accel_sp.y; + // In 2D the vertical motion is derived from the flight plan + accel_sp.z = (speed_sp.z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain; + float dt = get_sys_time_float() - time_of_accel_sp_2d; + // If the input command is not updated after a timeout, switch back to flight plan control + if (dt > 0.5) { + indi_accel_sp_set_2d = false; + } + } else if (indi_accel_sp_set_3d) { + accel_sp.x = indi_accel_sp.x; + accel_sp.y = indi_accel_sp.y; + accel_sp.z = indi_accel_sp.z; + float dt = get_sys_time_float() - time_of_accel_sp_3d; + // If the input command is not updated after a timeout, switch back to flight plan control + if (dt > 0.5) { + indi_accel_sp_set_3d = false; + } + } + else { + accel_sp = guidance_indi_controller(in_flight, gh, gv, h_mode, v_mode); + } + return guidance_indi_run(&accel_sp, gh->sp.heading); } + #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN /** * Filter the thrust, such that it corresponds to the filtered acceleration @@ -391,66 +456,7 @@ void guidance_indi_propagate_filters(struct FloatEulers *eulers) update_butterworth_2_low_pass(&roll_filt, eulers->phi); update_butterworth_2_low_pass(&pitch_filt, eulers->theta); -} - -/** - * @param Gmat array to write the matrix to [3x3] - * - * Calculate the matrix of partial derivatives of the pitch, roll and thrust. - * w.r.t. the NED accelerations for YXZ eulers - * ddx = G*[dtheta,dphi,dT] - */ -void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz) -{ - - float sphi = sinf(euler_yxz->phi); - float cphi = cosf(euler_yxz->phi); - float stheta = sinf(euler_yxz->theta); - float ctheta = cosf(euler_yxz->theta); - //minus gravity is a guesstimate of the thrust force, thrust measurement would be better - float T = -9.81; - - RMAT_ELMT(*Gmat, 0, 0) = ctheta * cphi * T; - RMAT_ELMT(*Gmat, 1, 0) = 0; - RMAT_ELMT(*Gmat, 2, 0) = -stheta * cphi * T; - RMAT_ELMT(*Gmat, 0, 1) = -stheta * sphi * T; - RMAT_ELMT(*Gmat, 1, 1) = -cphi * T; - RMAT_ELMT(*Gmat, 2, 1) = -ctheta * sphi * T; - RMAT_ELMT(*Gmat, 0, 2) = stheta * cphi; - RMAT_ELMT(*Gmat, 1, 2) = -sphi; - RMAT_ELMT(*Gmat, 2, 2) = ctheta * cphi; -} - -/** - * @param Gmat array to write the matrix to [3x3] - * - * Calculate the matrix of partial derivatives of the roll, pitch and thrust. - * w.r.t. the NED accelerations for ZYX eulers - * ddx = G*[dtheta,dphi,dT] - */ -UNUSED void guidance_indi_calcG(struct FloatMat33 *Gmat) -{ - - struct FloatEulers *euler = stateGetNedToBodyEulers_f(); - - float sphi = sinf(euler->phi); - float cphi = cosf(euler->phi); - float stheta = sinf(euler->theta); - float ctheta = cosf(euler->theta); - float spsi = sinf(euler->psi); - float cpsi = cosf(euler->psi); - //minus gravity is a guesstimate of the thrust force, thrust measurement would be better - float T = -9.81; - - RMAT_ELMT(*Gmat, 0, 0) = (cphi * spsi - sphi * cpsi * stheta) * T; - RMAT_ELMT(*Gmat, 1, 0) = (-sphi * spsi * stheta - cpsi * cphi) * T; - RMAT_ELMT(*Gmat, 2, 0) = -ctheta * sphi * T; - RMAT_ELMT(*Gmat, 0, 1) = (cphi * cpsi * ctheta) * T; - RMAT_ELMT(*Gmat, 1, 1) = (cphi * spsi * ctheta) * T; - RMAT_ELMT(*Gmat, 2, 1) = -stheta * cphi * T; - RMAT_ELMT(*Gmat, 0, 2) = sphi * spsi + cphi * cpsi * stheta; - RMAT_ELMT(*Gmat, 1, 2) = cphi * spsi * stheta - cpsi * sphi; - RMAT_ELMT(*Gmat, 2, 2) = cphi * ctheta; + update_butterworth_2_low_pass(&yaw_filt, eulers->psi); } /** diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h index 7bc68ea027..060f218813 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h @@ -14,9 +14,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @@ -52,6 +51,25 @@ enum GuidanceIndi_VMode { extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp); extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode); +extern struct FloatVect3 guidance_indi_controller(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode); + +// Default number of virtual commands (e.g. [dax, day, daz]) +#ifndef GUIDANCE_INDI_NV +#define GUIDANCE_INDI_NV 3 +#endif + +// Default number of outputs (e.g. [dtheta, dphi, dthrust]) +#ifndef GUIDANCE_INDI_NU +#define GUIDANCE_INDI_NU 3 +#endif + +// Function to compute efficiency matrix G +extern void guidance_indi_calcG(float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU], struct FloatEulers att); +#if GUIDANCE_INDI_USE_WLS +#include "math/wls/wls_alloc.h" +extern void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp); +#endif + extern float guidance_indi_specific_force_gain; // settings for guidance INDI diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c new file mode 100644 index 0000000000..0ef4f70de1 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2025 Gautier Hattenberger + * 20025 Mohamad Hachem + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c + * + * Fully actuated plateform can be achieve with hexa-copter with + * tilted propellers for example + * TODO cite Mohamad Hachem + */ + +#include "firmwares/rotorcraft/guidance/guidance_indi.h" +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "state.h" +#include "generated/modules.h" + +// FIXME should be a function of the mass + +#ifndef GUIDANCE_INDI_MAX_H_THRUST +#define GUIDANCE_INDI_MAX_H_THRUST 1.f +#endif + +#ifndef GUIDANCE_INDI_MAX_V_THRUST +#define GUIDANCE_INDI_MAX_V_THRUST 15.f +#endif + +float guidance_indi_max_h_thrust = GUIDANCE_INDI_MAX_H_THRUST; +float guidance_indi_max_v_thrust = GUIDANCE_INDI_MAX_V_THRUST; + +/** + * @param Gmat array to write the matrix to [3x6] + * + * Calculate the matrix of partial derivatives of the pitch, roll and thrust. + * w.r.t. the NED accelerations for YXZ eulers + * ddx = G*[dphi,dtheta,dpsi,dTx,dTy,dTz] + */ +static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_yxz) +{ + // Euler Angles + float sphi = sinf(euler_yxz.phi); + float cphi = cosf(euler_yxz.phi); + float stheta = sinf(euler_yxz.theta); + float ctheta = cosf(euler_yxz.theta); + float cpsi = cosf(euler_yxz.psi); + float spsi = sinf(euler_yxz.psi); + + // Estimated Thrust + float Tx = stab_thrust_filt.x; + float Ty = stab_thrust_filt.y; + float Tz = stab_thrust_filt.z; + + // dPhi (Roll) + Gmat[0][0] = Tx * (-spsi * cphi * stheta) + Ty * (spsi * sphi) + Tz * (spsi * cphi * ctheta); + Gmat[1][0] = Tx * (cpsi * cphi * stheta) + Ty * (-cpsi * sphi) + Tz * (-cpsi * cphi * ctheta); + Gmat[2][0] = Tx * (sphi * stheta) + Ty * (cphi) + Tz * (-sphi * ctheta); + + // dTheta (Pitch) + Gmat[0][1] = Tx * (-cpsi * stheta - spsi * sphi * ctheta) + Tz * (cpsi * ctheta - spsi * sphi * stheta); + Gmat[1][1] = Tx * (-spsi * stheta + cpsi * sphi * ctheta) + Tz * (spsi * ctheta + cpsi * sphi * stheta); + Gmat[2][1] = Tx * (-cphi * ctheta) + Tz * (-cphi * stheta); + + // dPsi added to add constraints on psi + Gmat[0][2] = 0.f; + Gmat[1][2] = 0.f; + Gmat[2][2] = 0.f; + + // dTx + Gmat[0][3] = cpsi * ctheta - spsi * sphi * stheta; + Gmat[1][3] = spsi * ctheta + cpsi * sphi * stheta; + Gmat[2][3] = -stheta * cphi; + + // dTy + Gmat[0][4] = -spsi * cphi; + Gmat[1][4] = cpsi * cphi; + Gmat[2][4] = sphi; + + // dTz + Gmat[0][5] = cpsi * stheta + spsi * sphi * ctheta; + Gmat[1][5] = spsi * stheta - cpsi * sphi * ctheta; + Gmat[2][5] = cphi * ctheta; +} + +/** Compute effectiveness matrix for guidance + * + * @param Gmat Dynamics matrix + */ +void guidance_indi_calcG(float Gmat[3][6], struct FloatEulers euler) { + guidance_indi_calcG_yxz(Gmat, euler); +} + +void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp UNUSED) +{ + struct FloatEulers euler_yxz_ref = { 0 }; +#if GUIDANCE_INDI_RC_SWITCH_EULER + euler_yxz_ref.phi = (radio_control_get(RADIO_PITCH) / MAX_PPRZ) * guidance_indi_max_bank; + euler_yxz_ref.theta = (radio_control_get(RADIO_ROLL) / MAX_PPRZ) * guidance_indi_max_bank; + euler_yxz_ref.psi = heading_sp // TODO check this one +#endif + + // Set lower limits + wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi; //phi + wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta; //theta + wls->u_min[2] = -M_PI - euler_yxz->psi; //psi FIXME M_PI or a lower bound ? (was 1 in initial code) + wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x; //Tx + wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y; //Ty + wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z; //Tz (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST]) + + // ->r limits limits + wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi; //phi + wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta; //theta + wls->u_max[2] = M_PI - euler_yxz->psi; //psi + wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x; //Tx + wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y; //Ty + wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z; //Tz + + // ->ered states + wls->u_pref[0] = euler_yxz_ref.phi - euler_yxz->phi; // prefered delta phi + wls->u_pref[1] = euler_yxz_ref.theta - euler_yxz->theta; // prefered delta theta + wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi; // prefered Ty + wls->u_pref[3] = stab_thrust_filt.x; // prefred Tx + wls->u_pref[4] = stab_thrust_filt.y; // prefered Ty + wls->u_pref[5] = stab_thrust_filt.z; // prefered Tz +} + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hinf.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hinf.c new file mode 100644 index 0000000000..a2fadc8400 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hinf.c @@ -0,0 +1,130 @@ +/* + * Copyright (C) Gautier Hattenberger, Mohamad Hachem + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +#include "firmwares/rotorcraft/guidance/guidance_indi.h" +#include "math/pprz_algebra_float.h" +#include "state.h" + +// proportional control part (horizontal position) +static float Ap = GUIDANCE_INDI_HINF_Ap; +static float Bp = GUIDANCE_INDI_HINF_Bp; +static float Cp = GUIDANCE_INDI_HINF_Cp; +static float Dp = GUIDANCE_INDI_HINF_Dp; +// derivative control part (horizontal speed) +static float Ad = GUIDANCE_INDI_HINF_Ad; +static float Bd = GUIDANCE_INDI_HINF_Bd; +static float Cd = GUIDANCE_INDI_HINF_Cd; +static float Dd = GUIDANCE_INDI_HINF_Dd; +// proportional control part (vertical position) +static float Apz = GUIDANCE_INDI_HINF_Apz; +static float Bpz = GUIDANCE_INDI_HINF_Bpz; +static float Cpz = GUIDANCE_INDI_HINF_Cpz; +static float Dpz = GUIDANCE_INDI_HINF_Dpz; +// derivative control part (vertical speed) +static float Adz = GUIDANCE_INDI_HINF_Adz; +static float Bdz = GUIDANCE_INDI_HINF_Bdz; +static float Cdz = GUIDANCE_INDI_HINF_Cdz; +static float Ddz = GUIDANCE_INDI_HINF_Ddz; + +static struct FloatVect3 pos_state = { 0 }; +static struct FloatVect3 speed_state = { 0 }; + +/** Acceleration controller based Hinfinity + */ +struct FloatVect3 guidance_indi_controller(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndi_HMode h_mode, enum GuidanceIndi_VMode v_mode) +{ + struct FloatVect3 pos_err = { 0 }; + struct FloatVect3 speed_err = { 0 }; + + struct FloatVect3 accel_sp = { 0 }; + struct FloatVect3 speed_sp = { 0 }; + struct FloatVect3 speed_fb = { 0 }; + + if (!in_flight) { + FLOAT_VECT3_ZERO(pos_state); + FLOAT_VECT3_ZERO(speed_state); + // TODO return with no control ? + } + + if (h_mode == GUIDANCE_INDI_H_ACCEL) { + // Speed feedback is included in the guidance when running in ACCEL mode + speed_fb.x = 0.f; + speed_fb.y = 0.f; + } + else { + // Generate speed feedback for acceleration, as it is estimated + if (h_mode == GUIDANCE_INDI_H_SPEED) { + speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x); + speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y); + } + else { + // H_POS + // speed setpoint from position error + pos_err.x = POS_FLOAT_OF_BFP(gh->ref.pos.x) - stateGetPositionNed_f()->x; + pos_err.y = POS_FLOAT_OF_BFP(gh->ref.pos.y) - stateGetPositionNed_f()->y; + speed_sp.x = Cp * pos_state.x + Dp * pos_err.x; + speed_sp.y = Cp * pos_state.y + Dp * pos_err.y; + pos_state.x = Ap * pos_state.x + Bd * pos_err.x; + pos_state.y = Ap * pos_state.y + Bp * pos_err.y; + + // TODO where should we add the feed-forward ref speed ? + //speed_sp.x += SPEED_FLOAT_OF_BFP(gh->ref.speed.x); + //speed_sp.y += SPEED_FLOAT_OF_BFP(gh->ref.speed.y); + } + speed_err.x = speed_sp.x - stateGetSpeedNed_f()->x; + speed_err.y = speed_sp.y - stateGetSpeedNed_f()->y; + speed_fb.x = Cd * speed_state.x + Dd * speed_err.x; + speed_fb.y = Cd * speed_state.y + Dd * speed_err.y; + speed_state.x = Ad * speed_state.x + Bd * speed_err.x; + speed_state.y = Ad * speed_state.y + Bd * speed_err.y; + } + + if (v_mode == GUIDANCE_INDI_V_ACCEL) { + // Speed feedback is included in the guidance when running in ACCEL mode + speed_fb.z = 0; + } + else { + // Generate speed feedback for acceleration, as it is estimated + if (v_mode == GUIDANCE_INDI_V_SPEED) { + speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref); + } + else { + // V_POS + // vertical speed setpoint from altitude error + pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z; + speed_sp.z = Cpz * pos_state.z + Dpz * pos_err.z; + pos_state.z = Apz * pos_state.z + Bpz * pos_err.z; + // TODO add speed feed-forward ? + // speed_sp.z += SPEED_FLOAT_OF_BFP(gv->zd_ref); + } + speed_err.z = speed_sp.z - stateGetSpeedNed_f()->z; + speed_fb.z = Cdz * speed_state.z + Ddz * speed_err.z; + speed_state.z = Adz * speed_state.z + Bdz * speed_err.z; + } + + // TODO add accel feed-forward term ? + accel_sp.x = speed_fb.x; // + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x); + accel_sp.y = speed_fb.y; // + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y); + accel_sp.z = speed_fb.z; // + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); + + return accel_sp; +} + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_quadrotor.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_quadrotor.c new file mode 100644 index 0000000000..77dace5d9b --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_quadrotor.c @@ -0,0 +1,99 @@ +/* + * Copyright (C) 2025 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_indi_quadrotor.c + * + */ + +#include "firmwares/rotorcraft/guidance/guidance_indi.h" +#include "filters/low_pass_filter.h" +#include "state.h" +#include "autopilot.h" +#include "generated/modules.h" + +/** + * @param Gmat array to write the matrix to [3x3] + * + * Calculate the matrix of partial derivatives of the roll, pitch and thrust. + * w.r.t. the NED accelerations for ZYX eulers + * ddx = G*[dtheta,dphi,dT] + */ +UNUSED static void guidance_indi_calcG_zyx(float Gmat[3][3], struct FloatEulers euler_zyx) +{ + float sphi = sinf(euler_zyx.phi); + float cphi = cosf(euler_zyx.phi); + float stheta = sinf(euler_zyx.theta); + float ctheta = cosf(euler_zyx.theta); + float spsi = sinf(euler_zyx.psi); + float cpsi = cosf(euler_zyx.psi); + // minus gravity is a guesstimate of the thrust force, thrust measurement would be better + float T = -9.81f; + + Gmat[0][0] = (cphi * spsi - sphi * cpsi * stheta) * T; + Gmat[1][0] = (-sphi * spsi * stheta - cpsi * cphi) * T; + Gmat[2][0] = -ctheta * sphi * T; + Gmat[0][1] = (cphi * cpsi * ctheta) * T; + Gmat[1][1] = (cphi * spsi * ctheta) * T; + Gmat[2][1] = -stheta * cphi * T; + Gmat[0][2] = sphi * spsi + cphi * cpsi * stheta; + Gmat[1][2] = cphi * spsi * stheta - cpsi * sphi; + Gmat[2][2] = cphi * ctheta; +} + +/** + * @param Gmat array to write the matrix to [3x3] + * + * Calculate the matrix of partial derivatives of the pitch, roll and thrust. + * w.r.t. the NED accelerations for YXZ eulers + * ddx = G*[dtheta,dphi,dT] + */ +static void guidance_indi_calcG_yxz(float Gmat[3][3], struct FloatEulers euler_yxz) +{ + float sphi = sinf(euler_yxz.phi); + float cphi = cosf(euler_yxz.phi); + float stheta = sinf(euler_yxz.theta); + float ctheta = cosf(euler_yxz.theta); + // minus gravity is a guesstimate of the thrust force, thrust measurement would be better + float T = -9.81f; + + Gmat[0][0] = ctheta * cphi * T; + Gmat[1][0] = 0; + Gmat[2][0] = -stheta * cphi * T; + Gmat[0][1] = -stheta * sphi * T; + Gmat[1][1] = -cphi * T; + Gmat[2][1] = -ctheta * sphi * T; + Gmat[0][2] = stheta * cphi; + Gmat[1][2] = -sphi; + Gmat[2][2] = ctheta * cphi; +} + +/** Compute effectiveness matrix for guidance + * + * @param Gmat Dynamics matrix + */ +void guidance_indi_calcG(float Gmat[3][3], struct FloatEulers att) { +#ifdef GUIDANCE_INDI_CALC_G_ZYX + guidance_indi_calcG_zyx(Gmat, att); +#else + guidance_indi_calcG_yxz(Gmat, att); // default case +#endif +} + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index d5aa15297c..b4f31b9361 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -15,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @file stabilization_attitude_quat_indi.c @@ -80,7 +79,34 @@ #define STABILIZATION_INDI_THROTTLE_LIMIT_AIRSPEED_FWD 8.0 #endif -#if INDI_OUTPUTS > 4 +#ifndef STABILIZATION_INDI_USE_ADAPTIVE +#define STABILIZATION_INDI_USE_ADAPTIVE false +#endif + +#ifndef STABILIZATION_INDI_ADAPTIVE_MU +#define STABILIZATION_INDI_ADAPTIVE_MU 0.001f +#endif + +#ifndef STABILIZATION_INDI_ACT_IS_SERVO +#define STABILIZATION_INDI_ACT_IS_SERVO {0} +#endif + +#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_X +#define STABILIZATION_INDI_ACT_IS_THRUSTER_X {0} +#endif + +#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Y +#define STABILIZATION_INDI_ACT_IS_THRUSTER_Y {0} +#endif + +/** + * Limit the maximum specific moment that can be compensated (units rad/s^2) +*/ +#ifndef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT +#define STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT 99999.f +#endif + +#if INDI_OUTPUTS == 5 #ifndef STABILIZATION_INDI_G1_THRUST_X #error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS" #endif @@ -94,33 +120,42 @@ #warning SetAutoCommandsFromRC not used: STAB_INDI writes actuators directly #endif +#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE // e.g. use WLS + +// Weighting of different control objectives in the cost function +#ifndef STABILIZATION_INDI_WLS_PRIORITIES +#if INDI_OUTPUTS == 5 +#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100, 100} // roll, pitch, yaw, thrust_z, thrust_x +#else +#define STABILIZATION_INDI_WLS_PRIORITIES {1000, 1000, 1, 100} // default: roll, pitch, yaw, thrust_z +#endif +#endif + +// Weighting of different actuators in the cost function +#ifndef STABILIZATION_INDI_WLS_WU +#define STABILIZATION_INDI_WLS_WU {[0 ... INDI_NUM_ACT - 1] = 1.0} +#endif + +// Preferred (neutral, least energy) actuator value +// Assume 0 is neutral +#ifndef STABILIZATION_INDI_ACT_PREF +#define STABILIZATION_INDI_ACT_PREF {0.0} +#endif -#if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE #if INDI_NUM_ACT > WLS_N_U_MAX #error Matrix-WLS_N_U_MAX too small or not defined: define WLS_N_U_MAX >= INDI_NUM_ACT in airframe file #endif #if INDI_OUTPUTS > WLS_N_V_MAX #error Matrix-WLS_N_V_MAX too small or not defined: define WLS_N_V_MAX >= INDI_OUTPUTS in airframe file #endif + struct WLS_t wls_stab_p = { .nu = INDI_NUM_ACT, .nv = INDI_OUTPUTS, .gamma_sq = 10000.0, .v = {0.0}, -#ifdef STABILIZATION_INDI_WLS_PRIORITIES - .Wv = STABILIZATION_INDI_WLS_PRIORITIES, -#else //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST} -#if INDI_OUTPUTS == 5 - .Wv = {1000, 1000, 1, 100, 100}, -#else - .Wv = {1000, 1000, 1, 100}, -#endif -#endif -#ifdef STABILIZATION_INDI_WLS_WU //Weighting of different actuators in the cost function + .Wv = STABILIZATION_INDI_WLS_PRIORITIES, .Wu = STABILIZATION_INDI_WLS_WU, -#else - .Wu = {[0 ... INDI_NUM_ACT - 1] = 1.0}, -#endif .u_pref = {0.0}, .u_min = {0.0}, .u_max = {0.0}, @@ -128,10 +163,15 @@ struct WLS_t wls_stab_p = { .SC = 0.0, .iter = 0 }; -#endif + +float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF; + +#endif // !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE (e.g. use WLS) + float indi_v[INDI_OUTPUTS]; float *Bwls[INDI_OUTPUTS]; + static void lms_estimation(void); static void get_actuator_state(void); static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra); @@ -155,36 +195,29 @@ struct Indi_gains indi_gains = { }, }; -#if STABILIZATION_INDI_USE_ADAPTIVE -bool indi_use_adaptive = true; -#else -bool indi_use_adaptive = false; -#endif +bool indi_use_adaptive = STABILIZATION_INDI_USE_ADAPTIVE; #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT float act_rate_limit[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_RATE_LIMIT; #endif -#ifdef STABILIZATION_INDI_ACT_IS_SERVO +// Vector of boolean telling if an actuator is a servo bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO; + +// [3 x INDI_NUM_ACT] matrix representing the contribution (True/False) of an +// actuator to the total 3D thrust vector +#ifdef STABILIZATION_INDI_ACT_THRUST_MAT +static bool act_thrust_mat[3][INDI_NUM_ACT] = STABILIZATION_INDI_ACT_THRUST_MAT; +#else // backward compatibility +static bool act_thrust_mat[3][INDI_NUM_ACT] = { + STABILIZATION_INDI_ACT_IS_THRUSTER_X, + STABILIZATION_INDI_ACT_IS_THRUSTER_Y, +#ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_Z + STABILIZATION_INDI_ACT_IS_THRUSTER_Z #else -bool act_is_servo[INDI_NUM_ACT] = {0}; + {0} #endif - -#ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_X -bool act_is_thruster_x[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_THRUSTER_X; -#else -bool act_is_thruster_x[INDI_NUM_ACT] = {0}; -#endif - -bool act_is_thruster_z[INDI_NUM_ACT]; - -#ifdef STABILIZATION_INDI_ACT_PREF -// Preferred (neutral, least energy) actuator value -float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF; -#else -// Assume 0 is neutral -float act_pref[INDI_NUM_ACT] = {0.0}; +}; #endif #ifdef STABILIZATION_INDI_ACT_DYN @@ -197,14 +230,7 @@ float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ; float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init #endif -/** - * Limit the maximum specific moment that can be compensated (units rad/s^2) -*/ -#ifdef STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT float stablization_indi_yaw_dist_limit = STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT; -#else // Put a rediculously high limit -float stablization_indi_yaw_dist_limit = 99999.f; -#endif // variables needed for control float actuator_state_filt_vect[INDI_NUM_ACT]; @@ -213,6 +239,7 @@ struct FloatRates angular_rate_ref = {0., 0., 0.}; float angular_acceleration[3] = {0., 0., 0.}; float actuator_state[INDI_NUM_ACT]; float indi_u[INDI_NUM_ACT]; +struct FloatVect3 stab_thrust_filt = { 0.f, 0.f, 0.f }; float q_filt = 0.0; float r_filt = 0.0; @@ -239,7 +266,6 @@ float act_obs[INDI_NUM_ACT]; // Number of actuators used to provide thrust int32_t num_thrusters; -int32_t num_thrusters_x; static struct Int32Eulers stab_att_sp_euler; static struct Int32Quat stab_att_sp_quat; @@ -266,6 +292,7 @@ float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X }; + #else float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST @@ -279,12 +306,14 @@ float g2_est[INDI_NUM_ACT]; float g1_init[INDI_OUTPUTS][INDI_NUM_ACT]; float g2_init[INDI_NUM_ACT]; +// Can be used to directly control each actuator from the control algorithm +int16_t actuators_pprz[INDI_NUM_ACT+1]; + Butterworth2LowPass actuator_lowpass_filters[INDI_NUM_ACT]; Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT]; Butterworth2LowPass measurement_lowpass_filters[3]; Butterworth2LowPass estimation_output_lowpass_filters[3]; Butterworth2LowPass acceleration_lowpass_filter; -Butterworth2LowPass acceleration_body_x_filter; #if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER Butterworth2LowPass rates_filt_so[3]; #else @@ -292,19 +321,20 @@ static struct FirstOrderLowPass rates_filt_fo[3]; #endif struct FloatVect3 body_accel_f; -void init_filters(void); -void sum_g1_g2(void); +static void init_filters(void); +static struct FloatRates filter_rates(struct FloatRates *rates); +static void sum_g1_g2(void); #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE static void send_wls_v_stab(struct transport_tx *trans, struct link_device *dev) { - send_wls_v("stab", &wls_stab_p, trans, dev); + send_wls_v("stab", &wls_stab_p, trans, dev); } static void send_wls_u_stab(struct transport_tx *trans, struct link_device *dev) { - send_wls_u("stab", &wls_stab_p, trans, dev); + send_wls_u("stab", &wls_stab_p, trans, dev); } #endif static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev) @@ -411,16 +441,15 @@ void stabilization_indi_init(void) float_vect_copy(g1_init[0], g1[0], INDI_OUTPUTS * INDI_NUM_ACT); float_vect_copy(g2_init, g2, INDI_NUM_ACT); - // Assume all non-servos are delivering thrust - num_thrusters = INDI_NUM_ACT; - num_thrusters_x = 0; + num_thrusters = 0; // sum of Z thrusters for (i = 0; i < INDI_NUM_ACT; i++) { - num_thrusters -= act_is_servo[i]; - num_thrusters -= act_is_thruster_x[i]; - - num_thrusters_x += act_is_thruster_x[i]; - - act_is_thruster_z[i] = !act_is_servo[i] && !act_is_thruster_x[i]; +#ifndef STABILIZATION_INDI_ACT_IS_THRUSTER_Z + // Assume non-servos and non thrust-x/y motors, not already set to true, are delivering (Z) thrust + act_thrust_mat[2][i] = act_thrust_mat[2][i] || (!act_thrust_mat[0][i] && !act_thrust_mat[1][i] && !act_is_servo[i]); +#endif + if (act_thrust_mat[2][i]) { + num_thrusters++; + } } #if PERIODIC_TELEMETRY @@ -467,7 +496,7 @@ void stabilization_indi_update_filt_freq(float freq) /** * Function that resets the filters to zeros */ -void init_filters(void) +static void init_filters(void) { // tau = 1/(2*pi*Fc) float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF); @@ -486,9 +515,6 @@ void init_filters(void) init_butterworth_2_low_pass(&estimation_input_lowpass_filters[i], tau_est, sample_time, 0.0); } - // Filtering the bodyx acceleration with same cutoff as gyroscope - init_butterworth_2_low_pass(&acceleration_body_x_filter, tau, sample_time, 0.0); - // Filtering of the accel body z init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0); @@ -509,6 +535,39 @@ void init_filters(void) #endif } +static struct FloatRates filter_rates(struct FloatRates *rates) +{ + struct FloatRates rates_filt; +#if STABILIZATION_INDI_FILTER_ROLL_RATE +#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER + rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], rates->p); +#else + rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], rates->p); +#endif +#else + rates_filt.p = rates->p; +#endif +#if STABILIZATION_INDI_FILTER_PITCH_RATE +#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER + rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], rates->q); +#else + rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], rates->q); +#endif +#else + rates_filt.q = rates->q; +#endif +#if STABILIZATION_INDI_FILTER_YAW_RATE +#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER + rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], rates->r); +#else + rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], rates->r); +#endif +#else + rates_filt.r = rates->r; +#endif + return rates_filt; +} + /** * @param in_flight boolean that states if the UAV is in flight or not * @param sp rate setpoint @@ -558,9 +617,7 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]); update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]); - update_butterworth_2_low_pass(&acceleration_body_x_filter, body_accel_f.x); - - //Calculate the angular acceleration via finite difference + // Calculate the angular acceleration via finite difference angular_acceleration[i] = (measurement_lowpass_filters[i].o[0] - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY; @@ -583,43 +640,14 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s float_vect_zero(angular_acc_disturbance_estimate, INDI_OUTPUTS); } - //The rates used for feedback are by default the measured rates. - //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback. - //Note that due to the delay, the PD controller may need relaxed gains. - struct FloatRates rates_filt; -#if STABILIZATION_INDI_FILTER_ROLL_RATE -#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER - rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], body_rates->p); -#else - rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p); -#endif -#else - rates_filt.p = body_rates->p; -#endif -#if STABILIZATION_INDI_FILTER_PITCH_RATE -#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER - rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], body_rates->q); -#else - rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q); -#endif -#else - rates_filt.q = body_rates->q; -#endif -#if STABILIZATION_INDI_FILTER_YAW_RATE -#if STABILIZATION_INDI_FILTER_RATES_SECOND_ORDER - rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], body_rates->r); -#else - rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r); -#endif -#else - rates_filt.r = body_rates->r; -#endif + // The rates used for feedback are by default the measured rates. + // If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback. + // Note that due to the delay, the controller may need relaxed gains. + struct FloatRates rates_filt = filter_rates(body_rates); - // calculate the virtual control (reference acceleration) based on a PD controller - struct FloatRates rate_sp = stab_sp_to_rates_f(sp); - angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p; - angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q; - angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r; + // calculate the virtual control (reference acceleration) based on a controller + struct FloatRates rates_sp = stab_sp_to_rates_f(sp); + angular_accel_ref = stabilization_indi_rate_controller(rates_filt, rates_sp); // compute virtual thrust struct FloatVect3 v_thrust = { 0.f, 0.f, 0.f }; @@ -629,27 +657,50 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s v_thrust.z = th_sp_to_incr_f(thrust, 0, THRUST_AXIS_Z); // Compute estimated thrust - struct FloatVect3 thrust_filt = { 0.f, 0.f, 0.f }; + FLOAT_VECT3_ZERO(stab_thrust_filt); for (i = 0; i < INDI_NUM_ACT; i++) { - thrust_filt.z += Bwls[3][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_z[i]; -#if INDI_OUTPUTS == 5 - thrust_filt.x += Bwls[4][i]* actuator_lowpass_filters[i].o[0] * (int32_t) act_is_thruster_x[i]; +#if INDI_OUTPUTS == 4 + stab_thrust_filt.z += Bwls[3][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[2][i]; +#endif +#if INDI_OUTPUTS == 5 // FIXME change order of Z and X, or better detect that automatically ? + stab_thrust_filt.x += Bwls[4][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[0][i]; + stab_thrust_filt.z += Bwls[3][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[2][i]; +#endif +#if INDI_OUTPUTS == 6 + stab_thrust_filt.x += Bwls[3][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[0][i]; + stab_thrust_filt.y += Bwls[4][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[1][i]; + stab_thrust_filt.z += Bwls[5][i] * actuator_lowpass_filters[i].o[0] * (int32_t) act_thrust_mat[2][i]; #endif } // Add the current estimated thrust to the increment - VECT3_ADD(v_thrust, thrust_filt); + VECT3_ADD(v_thrust, stab_thrust_filt); } else { // build incremental thrust - float th_cmd_z = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z); + struct FloatVect3 th_cmd; + th_cmd.x = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_X); + th_cmd.y = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Y); + th_cmd.z = (float)th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z); +#if (INDI_OUTPUTS == 5) && (defined RADIO_CONTROL_THRUST_X) && (defined COMMAND_THRUST_X) + // TODO set X thrust from RC in the thrust input setpoint + cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X]; + th_cmd.x = (float)cmd[COMMAND_THRUST_X]; +#endif for (i = 0; i < INDI_NUM_ACT; i++) { - v_thrust.z += th_cmd_z * Bwls[3][i]; -#if INDI_OUTPUTS == 5 - // TODO set X thrust from RC in the thrust input setpoint - cmd[COMMAND_THRUST_X] = radio_control.values[RADIO_CONTROL_THRUST_X]; - v_thrust.x += cmd[COMMAND_THRUST_X] * Bwls[4][i]; +#if INDI_OUTPUTS == 4 + v_thrust.z += th_cmd.z * Bwls[3][i] * (int32_t) act_thrust_mat[2][i]; +#endif +#if INDI_OUTPUTS == 5 // FIXME change order of Z and X, or better detect that automatically ? + v_thrust.x += th_cmd.x * Bwls[4][i] * (int32_t) act_thrust_mat[0][i]; + v_thrust.z += th_cmd.z * Bwls[3][i] * (int32_t) act_thrust_mat[2][i]; +#endif +#if INDI_OUTPUTS == 6 + v_thrust.x += th_cmd.x * Bwls[3][i] * (int32_t) act_thrust_mat[0][i]; + v_thrust.y += th_cmd.y * Bwls[4][i] * (int32_t) act_thrust_mat[1][i]; + v_thrust.z += th_cmd.z * Bwls[5][i] * (int32_t) act_thrust_mat[2][i]; #endif } - v_thrust.y = 0.f; + // store estimated thrust + stab_thrust_filt = v_thrust; } // This term compensates for the spinup torque in the yaw axis @@ -667,10 +718,18 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s indi_v[0] = (angular_accel_ref.p - angular_acc_disturbance_estimate[0]); indi_v[1] = (angular_accel_ref.q - angular_acc_disturbance_estimate[1]); indi_v[2] = (angular_accel_ref.r - angular_acc_disturbance_estimate[2]) + g2_times_u; +#if INDI_OUTPUTS == 4 + indi_v[3] = v_thrust.z; +#endif +#if INDI_OUTPUTS == 5 // FIXME order of Z/X is reversed indi_v[3] = v_thrust.z; -#if INDI_OUTPUTS == 5 indi_v[4] = v_thrust.x; #endif +#if INDI_OUTPUTS == 6 + indi_v[3] = v_thrust.x; + indi_v[4] = v_thrust.y; + indi_v[5] = v_thrust.z; +#endif #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE // Calculate the increment for each actuator @@ -689,12 +748,19 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s } wls_alloc(&wls_stab_p, Bwls, 0, 0, 10); for (i = 0; i < INDI_NUM_ACT; i++) { - indi_u [i] = wls_stab_p.u[i]; + indi_u[i] = wls_stab_p.u[i]; } #endif - // Bound the inputs to the actuators + // Use online effectiveness estimation only when flying + if (in_flight && indi_use_adaptive) { + lms_estimation(); + } + + // update actuators and thrust commands + cmd[COMMAND_THRUST] = 0; for (i = 0; i < INDI_NUM_ACT; i++) { + // bound actuator command if (act_is_servo[i]) { BoundAbs(indi_u[i], MAX_PPRZ); } else { @@ -704,27 +770,90 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s indi_u[i] = -MAX_PPRZ; } } - } - // Use online effectiveness estimation only when flying - if (in_flight && indi_use_adaptive) { - lms_estimation(); - } - - /*Commit the actuator command*/ - for (i = 0; i < INDI_NUM_ACT; i++) { + // commit actuator command actuators_pprz[i] = (int16_t) indi_u[i]; - } - //update thrust command such that the current is correctly estimated - cmd[COMMAND_THRUST] = 0; - for (i = 0; i < INDI_NUM_ACT; i++) { - cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i]; + // update thrust command such that the current is correctly estimated + cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_thrust_mat[2][i]; } cmd[COMMAND_THRUST] /= num_thrusters; - } +/** + * @param in_flight enable integrator only in flight + * @param att_sp attitude stabilization setpoint + * @param thrust thrust setpoint + * @param[out] output command vector + * + * Function that should be called to run the INDI controller + */ +void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd) +{ + stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref.. + stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint + + struct FloatQuat quat_att = *stateGetNedToBodyQuat_f(); + struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp); + struct FloatRates rates_ff = stab_sp_to_rates_f(att_sp); + + // calculate the virtual control (reference acceleration) based on a controller + angular_rate_ref = stabilization_indi_attitude_controller(quat_att, quat_sp, rates_ff); + + /* compute the INDI command */ + struct StabilizationSetpoint sp = stab_sp_from_rates_f(&angular_rate_ref); + stabilization_indi_rate_run(in_flight, &sp, thrust, cmd); +} + +/** Default PD angular rate controller. + * + * Takes the current attitude filtered state and setpoint and compute the desired rates. + * Can be redefined elsewhere to use an other control scheme. + */ +struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff) +{ + /* attitude error */ + struct FloatQuat att_err; + float_quat_inv_comp_norm_shortest(&att_err, &att, &att_sp); + + struct FloatVect3 att_fb; +#if TILT_TWIST_CTRL + struct FloatQuat tilt; + struct FloatQuat twist; + float_quat_tilt_twist(&tilt, &twist, &att_err); + att_fb.x = tilt.qx; + att_fb.y = tilt.qy; + att_fb.z = twist.qz; +#else + att_fb.x = att_err.qx; + att_fb.y = att_err.qy; + att_fb.z = att_err.qz; +#endif + + struct FloatRates rates_ref; + rates_ref.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p; + rates_ref.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q; + rates_ref.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r; + // add feed-forward term + RATES_ADD(rates_ref, rates_ff); + return rates_ref; +} + +/** Default PD angular acceleration controller. + * + * Takes the current rates filtered state and setpoint and compute the desired acceleration. + * Can be redefined elsewhere to use an other control scheme. + */ +struct FloatRates WEAK stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp) +{ + struct FloatRates accel_ref; + accel_ref.p = (sp.p - rates.p) * indi_gains.rate.p; + accel_ref.q = (sp.q - rates.q) * indi_gains.rate.q; + accel_ref.r = (sp.r - rates.r) * indi_gains.rate.r; + return accel_ref; +} + + /** * Function that sets the u_min, u_max and u_pref if function not elsewhere defined */ @@ -754,64 +883,6 @@ void WEAK stabilization_indi_set_wls_settings(void) } #endif -/** - * @param in_flight enable integrator only in flight - * @param att_sp attitude stabilization setpoint - * @param thrust thrust setpoint - * @param[out] output command vector - * - * Function that should be called to run the INDI controller - */ -void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd) -{ - stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref.. - stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint - - /* attitude error in float */ - struct FloatQuat att_err; - struct FloatQuat *att_quat = stateGetNedToBodyQuat_f(); - struct FloatQuat quat_sp = stab_sp_to_quat_f(att_sp); - - float_quat_inv_comp_norm_shortest(&att_err, att_quat, &quat_sp); - - struct FloatVect3 att_fb; -#if TILT_TWIST_CTRL - struct FloatQuat tilt; - struct FloatQuat twist; - float_quat_tilt_twist(&tilt, &twist, &att_err); - att_fb.x = tilt.qx; - att_fb.y = tilt.qy; - att_fb.z = twist.qz; -#else - att_fb.x = att_err.qx; - att_fb.y = att_err.qy; - att_fb.z = att_err.qz; -#endif - - // local variable to compute rate setpoints based on attitude error - struct FloatRates rate_sp; - // calculate the virtual control (reference acceleration) based on a PD controller - rate_sp.p = indi_gains.att.p * att_fb.x / indi_gains.rate.p; - rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q; - rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r; - - // Add feed-forward rates to the attitude feedback part - struct FloatRates ff_rates = stab_sp_to_rates_f(att_sp); - RATES_ADD(rate_sp, ff_rates); - - // Store for telemetry - angular_rate_ref.p = rate_sp.p; - angular_rate_ref.q = rate_sp.q; - angular_rate_ref.r = rate_sp.r; - - // Possibly we can use some bounding here - /*BoundAbs(rate_sp.r, 5.0);*/ - - /* compute the INDI command */ - struct StabilizationSetpoint sp = stab_sp_from_rates_f(&rate_sp); - stabilization_indi_rate_run(in_flight, &sp, thrust, cmd); -} - /** * Function that tries to get actuator feedback. * @@ -893,17 +964,9 @@ void lms_estimation(void) float indi_accel_d = (acceleration_lowpass_filter.o[0] - acceleration_lowpass_filter.o[1]) * PERIODIC_FREQUENCY; - // Use xml setting for adaptive mu for lms - // Set default value if not defined -#ifndef STABILIZATION_INDI_ADAPTIVE_MU - float adaptive_mu_lr = 0.001; -#else - float adaptive_mu_lr = STABILIZATION_INDI_ADAPTIVE_MU; -#endif - // scale the inputs to avoid numerical errors - float_vect_smul(du_estimation, actuator_state_filt_vectd, adaptive_mu_lr, INDI_NUM_ACT); - float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, adaptive_mu_lr / PERIODIC_FREQUENCY, INDI_NUM_ACT); + float_vect_smul(du_estimation, actuator_state_filt_vectd, STABILIZATION_INDI_ADAPTIVE_MU, INDI_NUM_ACT); + float_vect_smul(ddu_estimation, actuator_state_filt_vectdd, STABILIZATION_INDI_ADAPTIVE_MU / PERIODIC_FREQUENCY, INDI_NUM_ACT); float ddx_estimation[INDI_OUTPUTS] = {estimation_rate_dd[0], estimation_rate_dd[1], estimation_rate_dd[2], indi_accel_d}; @@ -1081,3 +1144,4 @@ static void bound_g_mat(void) } } } + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index c14986e55d..468edae17a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -15,9 +15,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ #ifndef STABILIZATION_INDI @@ -33,6 +32,16 @@ extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT]; extern float actuator_state_filt_vect[INDI_NUM_ACT]; +extern struct FloatVect3 stab_thrust_filt; + +/** PPRZ command to each actuator + * Can be used to directly control actuators from the control algorithm + * if the command_laws are set up appropriately in the airframe file + * + * FIXME add an extra slot for specific case (e.g. rotwing in simulation) + */ +extern int16_t actuators_pprz[INDI_NUM_ACT+1]; + extern bool act_is_servo[INDI_NUM_ACT]; extern bool indi_use_adaptive; @@ -63,6 +72,11 @@ extern void stabilization_indi_attitude_run(bool in_flight, struct Stabilization extern void stabilization_indi_set_wls_settings(void); extern void stabilization_indi_update_filt_freq(float freq); // setting handler +// outer-loop indi controller, a simple PD by default +// but can be redefined elsewhere +extern struct FloatRates stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff); +extern struct FloatRates stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp); + #if !STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE #include "math/wls/wls_alloc.h" extern struct WLS_t wls_stab_p; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_hinf.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_hinf.c new file mode 100644 index 0000000000..515c6691e1 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_hinf.c @@ -0,0 +1,109 @@ +/* + * Copyright (C) Gautier Hattenberger, Mohamad Hachem + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "math/pprz_algebra_float.h" +#include "autopilot.h" + +static struct FloatRates rate_state; +static struct FloatRates att_state; + +// proportional control part (attitude) +static float Ap = STABILIZATION_INDI_HINF_Ap; +static float Bp = STABILIZATION_INDI_HINF_Bp; +static float Cp = STABILIZATION_INDI_HINF_Cp; +static float Dp = STABILIZATION_INDI_HINF_Dp; +// derivative control part (rates) +static float Ad = STABILIZATION_INDI_HINF_Ad; +static float Bd = STABILIZATION_INDI_HINF_Bd; +static float Cd = STABILIZATION_INDI_HINF_Cd; +static float Dd = STABILIZATION_INDI_HINF_Dd; + +/** Angular acceleration controller based on Hinfinity + * + * Takes the current rates filtered state and setpoint and compute the desired acceleration. + */ +struct FloatRates stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp) +{ + struct FloatRates rate_error; + RATES_DIFF(rate_error, sp, rates); + + struct FloatRates accel_ref; + if (autopilot_in_flight()) { + accel_ref.p = Cd * rate_state.p + Dd * rate_error.p; + rate_state.p = Ad * rate_state.p + Bd * rate_error.p; + + accel_ref.q = Cd * rate_state.q + Dd * rate_error.q; + rate_state.q = Ad * rate_state.q + Bd * rate_error.q; + + accel_ref.r = rate_error.r * indi_gains.rate.r; + } else { + FLOAT_RATES_ZERO(rate_state); + FLOAT_RATES_ZERO(accel_ref); + } + + return accel_ref; +} + +/** Angular rate controller based on Hinfinity + * + * Takes the current attitude filtered state and setpoint and compute the desired rates. + * Can be redefined elsewhere to use an other control scheme. + */ +struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff) +{ + /* attitude error */ + struct FloatQuat att_err; + float_quat_inv_comp_norm_shortest(&att_err, &att, &att_sp); + + struct FloatVect3 att_fb; +#if TILT_TWIST_CTRL + struct FloatQuat tilt; + struct FloatQuat twist; + float_quat_tilt_twist(&tilt, &twist, &att_err); + att_fb.x = tilt.qx; + att_fb.y = tilt.qy; + att_fb.z = twist.qz; +#else + att_fb.x = att_err.qx; + att_fb.y = att_err.qy; + att_fb.z = att_err.qz; +#endif + + struct FloatRates rate_sp; + if (autopilot_in_flight()) { + rate_sp.p = Cp * att_state.p + Dp * att_fb.x; + att_state.p = Ap * att_state.p + Bp * att_fb.x; + + rate_sp.q = Cp * att_state.q + Dp * att_fb.y; + att_state.q = Ap * att_state.q + Bp * att_fb.y; + + rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r; + + // add feed-forward term + RATES_ADD(rate_sp, rates_ff); + } else { + FLOAT_RATES_ZERO(att_state); + FLOAT_RATES_ZERO(rate_sp); + } + + return rate_sp; +} + diff --git a/sw/airborne/modules/actuators/actuators.c b/sw/airborne/modules/actuators/actuators.c index d2f6936329..42d5deef25 100644 --- a/sw/airborne/modules/actuators/actuators.c +++ b/sw/airborne/modules/actuators/actuators.c @@ -63,9 +63,6 @@ static void send_actuators(struct transport_tx *trans, struct link_device *dev) struct actuator_t actuators[ACTUATORS_NB] = ACTUATORS_CONFIG; -// Can be used to directly control each actuator from the control algorithm -int16_t actuators_pprz[ACTUATORS_NB]; - uint32_t actuators_delay_time; bool actuators_delay_done; diff --git a/sw/airborne/modules/actuators/actuators.h b/sw/airborne/modules/actuators/actuators.h index 4b2d43a12a..fcdf5bb5bb 100644 --- a/sw/airborne/modules/actuators/actuators.h +++ b/sw/airborne/modules/actuators/actuators.h @@ -87,12 +87,6 @@ struct actuator_t { * */ extern struct actuator_t actuators[ACTUATORS_NB]; -/** PPRZ command to each actuator - * Can be used to directly control actuators from the control algorithm - * if the command_laws are set up appropriately in the airframe file - */ -extern int16_t actuators_pprz[ACTUATORS_NB]; - /** Set actuators. * @param pprz_idx general actuators index * @param pprz_value actuator's value in pprz unit diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 64614eb142..3565f5e9ca 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -27,6 +27,7 @@ #include "nps_electrical.h" #include "nps_fdm.h" +#include "generated/modules.h" #include "modules/radio_control/radio_control.h" #include "modules/imu/imu.h" #include "mcu_periph/sys_time.h" diff --git a/tests/modules/generated/airframe.h b/tests/modules/generated/airframe.h index 8034925a74..d70ec1f151 100644 --- a/tests/modules/generated/airframe.h +++ b/tests/modules/generated/airframe.h @@ -62,4 +62,32 @@ #define STABILIZATION_ATTITUDE_SP_MAX_R 1.04719755 #define STABILIZATION_ATTITUDE_DEADBAND_R 250 +#define SECTION_STABILIZATION_ATTITUDE_INDI_HINF 1 +#define STABILIZATION_INDI_HINF_Ap 1. +#define STABILIZATION_INDI_HINF_Bp 0.03518 +#define STABILIZATION_INDI_HINF_Cp 0.0191 +#define STABILIZATION_INDI_HINF_Dp 7.643 +#define STABILIZATION_INDI_HINF_Ad 0.9013 +#define STABILIZATION_INDI_HINF_Bd -0.02509 +#define STABILIZATION_INDI_HINF_Cd 91.15 +#define STABILIZATION_INDI_HINF_Dd 44.57 + +#define SECTION_GUIDANCE_INDI_HINF 1 +#define GUIDANCE_INDI_HINF_Ap 0.9985 +#define GUIDANCE_INDI_HINF_Bp 0.04362 +#define GUIDANCE_INDI_HINF_Cp 0.009739 +#define GUIDANCE_INDI_HINF_Dp 0.5602 +#define GUIDANCE_INDI_HINF_Ad 1 +#define GUIDANCE_INDI_HINF_Bd -0.01418 +#define GUIDANCE_INDI_HINF_Cd -0.2814 +#define GUIDANCE_INDI_HINF_Dd 2.397 +#define GUIDANCE_INDI_HINF_Apz 0.9987 +#define GUIDANCE_INDI_HINF_Bpz -0.0003935 +#define GUIDANCE_INDI_HINF_Cpz -1.052 +#define GUIDANCE_INDI_HINF_Dpz 1.084 +#define GUIDANCE_INDI_HINF_Adz 1 +#define GUIDANCE_INDI_HINF_Bdz 0.003449 +#define GUIDANCE_INDI_HINF_Cdz 1.548 +#define GUIDANCE_INDI_HINF_Ddz 3.357 + #endif // AIRFRAME_H