diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml index ee2a0d930d..a235f9c9c8 100644 --- a/conf/airframes/examples/cube_orange.xml +++ b/conf/airframes/examples/cube_orange.xml @@ -109,7 +109,7 @@ - + diff --git a/conf/airframes/tudelft/bebop2_indi_convergence.xml b/conf/airframes/tudelft/bebop2_indi_convergence.xml index 1629e3cf67..8841cf854e 100644 --- a/conf/airframes/tudelft/bebop2_indi_convergence.xml +++ b/conf/airframes/tudelft/bebop2_indi_convergence.xml @@ -34,7 +34,7 @@ - + diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml index 7fe2d6ce22..db3dab7afb 100644 --- a/conf/airframes/tudelft/cyfoam.xml +++ b/conf/airframes/tudelft/cyfoam.xml @@ -292,7 +292,7 @@ - + diff --git a/conf/airframes/tudelft/disco_rotorcraft_indi.xml b/conf/airframes/tudelft/disco_rotorcraft_indi.xml index 50e92066b2..c759e36508 100644 --- a/conf/airframes/tudelft/disco_rotorcraft_indi.xml +++ b/conf/airframes/tudelft/disco_rotorcraft_indi.xml @@ -60,7 +60,7 @@ - + @@ -409,7 +409,6 @@ - diff --git a/conf/modules/guidance_indi_hybrid.xml b/conf/modules/guidance_indi_hybrid.xml index 1535377fa5..c083ed67ad 100644 --- a/conf/modules/guidance_indi_hybrid.xml +++ b/conf/modules/guidance_indi_hybrid.xml @@ -3,7 +3,9 @@ - Guidance controller for hybrids using INDI + Common guidance controller code for hybrids using INDI: in your airframe file include one of the options: + - guidance_indi_hybid_tailsitter + - guidance_indi_hybid_quadplane
@@ -31,14 +33,13 @@ @navigation,guidance_rotorcraft,wls - guidance,attitude_command + guidance_indi_hybrid_tailsitter
- diff --git a/conf/modules/guidance_indi_hybrid_quadplane.xml b/conf/modules/guidance_indi_hybrid_quadplane.xml new file mode 100644 index 0000000000..5afcf0d6f8 --- /dev/null +++ b/conf/modules/guidance_indi_hybrid_quadplane.xml @@ -0,0 +1,27 @@ + + + + + + Guidance controller for hybrid quadplane vehicles + + + + guidance_indi_hybrid + guidance_indi_hybrid_tailsitter + guidance,attitude_command + +
+ +
+ + + + + + + + + + + diff --git a/conf/modules/guidance_indi_hybrid_tailsitter.xml b/conf/modules/guidance_indi_hybrid_tailsitter.xml new file mode 100644 index 0000000000..3e6c10cac8 --- /dev/null +++ b/conf/modules/guidance_indi_hybrid_tailsitter.xml @@ -0,0 +1,24 @@ + + + + + + Guidance controller for hybrid tailsitter vehicles + + + + guidance_indi_hybrid + guidance_indi_hybrid_quadplane + guidance,attitude_command + +
+ +
+ + + + + + + + diff --git a/conf/modules/nav_hybrid.xml b/conf/modules/nav_hybrid.xml index c5e2ccc2ad..15ed23187b 100644 --- a/conf/modules/nav_hybrid.xml +++ b/conf/modules/nav_hybrid.xml @@ -40,6 +40,8 @@ + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index f7fd05a723..c5a6b1e730 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -90,20 +90,6 @@ float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED; float guidance_indi_pitch_pref_deg = 0; -#if GUIDANCE_INDI_QUADPLANE -#ifndef GUIDANCE_INDI_THRUST_Z_EFF -#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF" -#else -float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF; -#endif - -#ifndef GUIDANCE_INDI_THRUST_X_EFF -#error "You need to define GUIDANCE_INDI_THRUST_X_EFF" -#else -float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF; -#endif -#endif - // If using WLS, check that the matrix size is sufficient #if GUIDANCE_INDI_HYBRID_USE_WLS #if GUIDANCE_INDI_HYBRID_U > WLS_N_U @@ -181,7 +167,6 @@ Butterworth2LowPass roll_filt; Butterworth2LowPass pitch_filt; Butterworth2LowPass thrust_filt; Butterworth2LowPass accely_filt; -Butterworth2LowPass accel_bodyz_filt; struct FloatVect2 desired_airspeed; @@ -207,7 +192,6 @@ float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f }; #endif float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF; -float bodyz_filter_cutoff = 0.2; float guidance_indi_hybrid_heading_sp = 0.f; struct FloatEulers guidance_euler_cmd; @@ -224,7 +208,6 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0}; float time_of_vel_sp = 0.0; void guidance_indi_propagate_filters(void); -void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]); #if PERIODIC_TELEMETRY #include "modules/datalink/telemetry.h" @@ -289,7 +272,6 @@ void guidance_indi_enter(void) { guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi; float tau = 1.0 / (2.0 * M_PI * filter_cutoff); - float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff); float sample_time = 1.0 / PERIODIC_FREQUENCY; for (int8_t i = 0; i < 3; i++) { init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0); @@ -302,7 +284,6 @@ void guidance_indi_enter(void) { init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta); init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in); init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0); - init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81); } #include "firmwares/rotorcraft/navigation.h" @@ -735,166 +716,8 @@ void guidance_indi_propagate_filters(void) { // Propagate filter for sideslip correction float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y); update_butterworth_2_low_pass(&accely_filt, accely); - - // Propagate filter for thrust/lift estimate - float accelz = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->z); - update_butterworth_2_low_pass(&accel_bodyz_filt, accelz); } -/** - * Calculate the matrix of partial derivatives of the roll, pitch and thrust - * w.r.t. the NED accelerations, taking into account the lift of a wing that is - * horizontal at -90 degrees pitch - * - * @param Gmat Dynamics matrix - * @param a_diff acceleration errors in earth frame - * @param body_v 3D vector to write the control objective v - */ -#if defined(GUIDANCE_INDI_QUADPLANE) -/** - * Perform WLS - * - * @param Gmat Dynamics matrix - * @param a_diff acceleration errors in earth frame - * @param body_v 3D vector to write the control objective v - */ -void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V]) { - /*Pre-calculate sines and cosines*/ - float sphi = sinf(eulers_zxy.phi); - float cphi = cosf(eulers_zxy.phi); - float stheta = sinf(eulers_zxy.theta); - float ctheta = cosf(eulers_zxy.theta); - float spsi = sinf(eulers_zxy.psi); - float cpsi = cosf(eulers_zxy.psi); - -#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING -#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0 -#endif - - /*Amount of lift produced by the wing*/ - float lift_thrust_bz = accel_bodyz_filt.o[0]; // Sum of lift and thrust in boxy z axis (level flight) - - // get the derivative of the lift wrt to theta - float liftd = guidance_indi_get_liftd(0.0f, 0.0f); - - Gmat[0][0] = -sphi*stheta*lift_thrust_bz; - Gmat[1][0] = -cphi*lift_thrust_bz; - Gmat[2][0] = -sphi*ctheta*lift_thrust_bz; - - Gmat[0][1] = cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING; - Gmat[1][1] = sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd; - Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd; - - Gmat[0][2] = cphi*stheta; - Gmat[1][2] = -sphi; - Gmat[2][2] = cphi*ctheta; - - Gmat[0][3] = ctheta; - Gmat[1][3] = 0; - Gmat[2][3] = -stheta; - - // Convert acceleration error to body axis system - body_v[0] = cpsi * a_diff.x + spsi * a_diff.y; - body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y; - body_v[2] = a_diff.z; -} -#else -void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) { - - /*Pre-calculate sines and cosines*/ - float sphi = sinf(eulers_zxy.phi); - float cphi = cosf(eulers_zxy.phi); - float stheta = sinf(eulers_zxy.theta); - float ctheta = cosf(eulers_zxy.theta); - float spsi = sinf(eulers_zxy.psi); - float cpsi = cosf(eulers_zxy.psi); - //minus gravity is a guesstimate of the thrust force, thrust measurement would be better - -#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING -#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0 -#endif - - /*Amount of lift produced by the wing*/ - float pitch_lift = eulers_zxy.theta; - Bound(pitch_lift,-M_PI_2,0); - float lift = sinf(pitch_lift)*9.81; - float T = cosf(pitch_lift)*-9.81; - - // get the derivative of the lift wrt to theta - float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta); - - Gmat[0][0] = cphi*ctheta*spsi*T + cphi*spsi*lift; - Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift; - Gmat[2][0] = -sphi*ctheta*T -sphi*lift; - Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd; - Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd; - Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd; - Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi; - Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi; - Gmat[2][2] = cphi*ctheta; - - v_gih[0] = a_diff.x; - v_gih[1] = a_diff.y; - v_gih[2] = a_diff.z; -} -#endif -/** - * - * @param body_v - * - * WEAK function to set the quadplane wls settings - */ -#if GUIDANCE_INDI_HYBRID_USE_WLS -void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle, float pitch_angle) -{ - // Set lower limits - du_min_gih[0] = -guidance_indi_max_bank - roll_angle; // roll - du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // pitch - du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]; - - // Set upper limits limits - du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll - du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // pitch - du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]); - - // Set prefered states - du_pref_gih[0] = -roll_angle; // prefered delta roll angle - du_pref_gih[1] = -pitch_angle; // prefered delta pitch angle - du_pref_gih[2] = du_max_gih[2]; -} -#elif defined(GUIDANCE_INDI_QUADPLANE) -#warning We have GUIDANCE_INDI_QUADPLANE -void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) -{ - float roll_limit_rad = GUIDANCE_H_MAX_BANK; - float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); - float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); - - float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); - - // Set lower limits - du_min_gih[0] = -roll_limit_rad - roll_angle; //roll - du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch - du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff; - du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff; - - // Set upper limits limits - du_max_gih[0] = roll_limit_rad - roll_angle; //roll - du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch - du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; - du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff; - - // Set prefered states - du_pref_gih[0] = -roll_angle; // prefered delta roll angle - du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle - du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency - du_pref_gih[3] = body_v[0]; // solve the body acceleration -} -#else -void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle UNUSED, float pitch_angle UNUSED) { - -} -#endif /** * @brief Get the derivative of lift w.r.t. pitch. diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h index c687b96f89..4d46131beb 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h @@ -37,11 +37,9 @@ #include "firmwares/rotorcraft/guidance.h" #include "firmwares/rotorcraft/stabilization.h" -#ifdef GUIDANCE_INDI_QUADPLANE -#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h" -#define GUIDANCE_INDI_HYBRID_USE_WLS true -#else -#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h" + +#ifndef GUIDANCE_INDI_HYBRID_U +#error Please use guidance_indi_hybrid_tailsitter or guidance_indi_hybrid_quadplane in your airframe file. #endif @@ -50,7 +48,11 @@ extern void guidance_indi_init(void); extern void guidance_indi_enter(void); extern float guidance_indi_get_liftd(float pitch, float theta); +extern void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]); + +#if GUIDANCE_INDI_HYBRID_USE_WLS extern void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle); +#endif enum GuidanceIndiHybrid_HMode { GUIDANCE_INDI_HYBRID_H_POS, @@ -83,11 +85,14 @@ extern struct FloatVect3 gi_speed_sp; extern float guidance_indi_pitch_pref_deg; +#if GUIDANCE_INDI_HYBRID_USE_WLS extern float Wu_gih[GUIDANCE_INDI_HYBRID_U]; extern float Wv_gih[GUIDANCE_INDI_HYBRID_V]; extern float du_min_gih[GUIDANCE_INDI_HYBRID_U]; extern float du_max_gih[GUIDANCE_INDI_HYBRID_U]; extern float du_pref_gih[GUIDANCE_INDI_HYBRID_U]; +#endif + extern float guidance_indi_thrust_z_eff; extern float guidance_indi_thrust_x_eff; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c new file mode 100644 index 0000000000..c7bdac2220 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2015 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c + * + */ + +#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" +#include "stabilization/stabilization_attitude_ref_quat_int.h" +#include "filters/low_pass_filter.h" +#include "state.h" +#include "generated/modules.h" + + +#ifndef GUIDANCE_INDI_THRUST_Z_EFF +#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF" +#else +float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF; +#endif + +#ifndef GUIDANCE_INDI_THRUST_X_EFF +#error "You need to define GUIDANCE_INDI_THRUST_X_EFF" +#else +float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF; +#endif + + +float bodyz_filter_cutoff = 0.2; + +Butterworth2LowPass accel_bodyz_filt; + + + +/** + * + * Call upon entering indi guidance + */ +void guidance_indi_quadplane_init(void) { + float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff); + float sample_time = 1.0 / PERIODIC_FREQUENCY; + init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81); +} + + +/** + * Low pass the accelerometer measurements to remove noise from vibrations. + * The roll and pitch also need to be filtered to synchronize them with the + * acceleration + * Called as a periodic function with PERIODIC_FREQ + */ +void guidance_indi_quadplane_propagate_filters(void) { + // Propagate filter for thrust/lift estimate + float accelz = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->z); + update_butterworth_2_low_pass(&accel_bodyz_filt, accelz); +} + + + + +/** + * Perform WLS + * + * @param Gmat Dynamics matrix + * @param a_diff acceleration errors in earth frame + * @param body_v 3D vector to write the control objective v + */ +void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V]) { + // Get attitude + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + /*Pre-calculate sines and cosines*/ + float sphi = sinf(eulers_zxy.phi); + float cphi = cosf(eulers_zxy.phi); + float stheta = sinf(eulers_zxy.theta); + float ctheta = cosf(eulers_zxy.theta); + float spsi = sinf(eulers_zxy.psi); + float cpsi = cosf(eulers_zxy.psi); + +#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING +#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0 +#endif + + /*Amount of lift produced by the wing*/ + float lift_thrust_bz = accel_bodyz_filt.o[0]; // Sum of lift and thrust in boxy z axis (level flight) + + // get the derivative of the lift wrt to theta + float liftd = guidance_indi_get_liftd(0.0f, 0.0f); + + Gmat[0][0] = -sphi*stheta*lift_thrust_bz; + Gmat[1][0] = -cphi*lift_thrust_bz; + Gmat[2][0] = -sphi*ctheta*lift_thrust_bz; + + Gmat[0][1] = cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING; + Gmat[1][1] = sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd; + Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd; + + Gmat[0][2] = cphi*stheta; + Gmat[1][2] = -sphi; + Gmat[2][2] = cphi*ctheta; + + Gmat[0][3] = ctheta; + Gmat[1][3] = 0; + Gmat[2][3] = -stheta; + + // Convert acceleration error to body axis system + body_v[0] = cpsi * a_diff.x + spsi * a_diff.y; + body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y; + body_v[2] = a_diff.z; +} + + +void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle) +{ + float roll_limit_rad = GUIDANCE_H_MAX_BANK; + float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH); + float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH); + + float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg); + + // Set lower limits + du_min_gih[0] = -roll_limit_rad - roll_angle; //roll + du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch + du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff; + du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff; + + // Set upper limits limits + du_max_gih[0] = roll_limit_rad - roll_angle; //roll + du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch + du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff; + du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff; + + // Set prefered states + du_pref_gih[0] = -roll_angle; // prefered delta roll angle + du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle + du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency + du_pref_gih[3] = body_v[0]; // solve the body acceleration +} diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.h similarity index 79% rename from sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h rename to sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.h index 793e04f8f9..a1145e7247 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.h @@ -19,14 +19,12 @@ * Boston, MA 02111-1307, USA. */ +#ifndef GUIDANCE_INDI_HYBRID_QUADPLANE +#define GUIDANCE_INDI_HYBRID_QUADPLANE -// Guidance actuators: (roll, pitch, thrust, push) for quadplanes -#define GUIDANCE_INDI_HYBRID_U 4 - -// Guidance control objectives: ax, ay, ay for quadplanes -#define GUIDANCE_INDI_HYBRID_V 3 - +extern void guidance_indi_quadplane_init(void); +extern void guidance_indi_quadplane_propagate_filters(void); #ifndef GUIDANCE_INDI_MIN_PITCH @@ -36,3 +34,4 @@ +#endif // GUIDANCE_INDI_HYBRID_QUADPLANE \ No newline at end of file diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c new file mode 100644 index 0000000000..c4abe22813 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2015 Ewoud Smeur + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter + */ + +#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" +#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.h" +#include "state.h" +#include "generated/modules.h" + +/** + * Calculate the matrix of partial derivatives of the roll, pitch and thrust + * w.r.t. the NED accelerations, taking into account the lift of a wing that is + * horizontal at -90 degrees pitch + * + * @param Gmat Dynamics matrix + * @param a_diff acceleration errors in earth frame + * @param body_v 3D vector to write the control objective v + */ + +void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) { + // Get attitude + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + + /*Pre-calculate sines and cosines*/ + float sphi = sinf(eulers_zxy.phi); + float cphi = cosf(eulers_zxy.phi); + float stheta = sinf(eulers_zxy.theta); + float ctheta = cosf(eulers_zxy.theta); + float spsi = sinf(eulers_zxy.psi); + float cpsi = cosf(eulers_zxy.psi); + //minus gravity is a guesstimate of the thrust force, thrust measurement would be better + +#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING +#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0 +#endif + + /*Amount of lift produced by the wing*/ + float pitch_lift = eulers_zxy.theta; + Bound(pitch_lift,-M_PI_2,0); + float lift = sinf(pitch_lift)*9.81; + float T = cosf(pitch_lift)*-9.81; + + // get the derivative of the lift wrt to theta + float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta); + + Gmat[0][0] = cphi*ctheta*spsi*T + cphi*spsi*lift; + Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift; + Gmat[2][0] = -sphi*ctheta*T -sphi*lift; + Gmat[0][1] = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd; + Gmat[1][1] = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd; + Gmat[2][1] = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd; + Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi; + Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi; + Gmat[2][2] = cphi*ctheta; + + v_gih[0] = a_diff.x; + v_gih[1] = a_diff.y; + v_gih[2] = a_diff.z; +} + + +#if GUIDANCE_INDI_HYBRID_USE_WLS + + + +void guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle, float pitch_angle) +{ + // Set lower limits + du_min_gih[0] = -guidance_indi_max_bank - roll_angle; // roll + du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // pitch + du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]; + + // Set upper limits limits + du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll + du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // pitch + du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]); + + // Set prefered states + du_pref_gih[0] = -roll_angle; // prefered delta roll angle + du_pref_gih[1] = -pitch_angle; // prefered delta pitch angle + du_pref_gih[2] = du_max_gih[2]; +} + + +#endif + + diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.h similarity index 82% rename from sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h rename to sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.h index e0f4165de6..3d33ee125c 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.h @@ -19,15 +19,15 @@ * Boston, MA 02111-1307, USA. */ +#ifndef GUIDANCE_INDI_HYBRID_TAILSITTER +#define GUIDANCE_INDI_HYBRID_TAILSITTER -// Guidance actuators: (roll, pitch, thrust) for tailsitters -#define GUIDANCE_INDI_HYBRID_U 3 - -// Guidance control objectives: ax, ay, ay for tailsitters -#define GUIDANCE_INDI_HYBRID_V 3 #ifndef GUIDANCE_INDI_MIN_PITCH #define GUIDANCE_INDI_MIN_PITCH -120 #define GUIDANCE_INDI_MAX_PITCH 25 #endif + + +#endif // GUIDANCE_INDI_HYBRID_TAILSITTER \ No newline at end of file