Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

hybrid quadplanes and tailsitters explicit (#3161)

Browse Source
This commit is contained in:
Christophe De Wagter
2023-11-06 10:49:20 +01:00
committed by GitHub
parent 4d88bb9038
commit 54d2a2b681
19 changed files with 354 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/cube_orange.xml
+1 -1
View File
@@ -109,7 +109,7 @@
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
conf/airframes/tudelft/bebop2_indi_convergence.xml
+1 -1
View File
@@ -34,7 +34,7 @@
<module name="gps" type="ublox"/>
<module name="gps" type="ubx_ucenter"/>
<module name="stabilization" type="indi_simple"/>
<module name="guidance" type="indi_hybrid"/>
<module name="guidance" type="indi_hybrid_tailsitter"/>
<module name="ins" type="ekf2"/>
<!--<module name="ahrs" type="int_cmpl_quat">-->
conf/airframes/tudelft/cyfoam.xml
+1 -1
View File
@@ -292,7 +292,7 @@
<define name="WLS_N_U" value="4" />
<define name="WLS_N_V" value="4" />
</module>
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
conf/airframes/tudelft/disco_rotorcraft_indi.xml
+1 -1
View File
@@ -60,7 +60,7 @@
<define name="WLS_N_V" value="4"/>
<configure name="INDI_NUM_ACT" value="3"/>
</module>
<module name="guidance" type="indi_hybrid"/>
<module name="guidance" type="indi_hybrid_tailsitter"/>
<!--<module name="sonar_bebop">
<define name="USE_SONAR"/>
<define name="SENSOR_SYNC_SEND_SONAR"/>
conf/airframes/tudelft/nederdrone4.xml
+2 -1
View File
@@ -150,7 +150,8 @@
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module>
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_HYBRID_USE_WLS" value="TRUE"/>
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
conf/airframes/tudelft/nederdrone6.xml
+1 -1
View File
@@ -138,7 +138,7 @@
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module>
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
conf/airframes/tudelft/nederdrone7.xml
+1 -1
View File
@@ -138,7 +138,7 @@
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module>
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
conf/airframes/tudelft/nederdrone8.xml
+1 -1
View File
@@ -138,7 +138,7 @@
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module>
<module name="guidance" type="indi_hybrid">
<module name="guidance" type="indi_hybrid_tailsitter">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
conf/airframes/tudelft/rot_wing_25kg.xml
+1 -2
View File
@@ -102,7 +102,7 @@
<module name="ctrl_eff_sched_rot_wing"/>
<module name="guidance" type="indi_hybrid"/>
<module name="guidance" type="indi_hybrid_quadplane"/>
<module name="nav_hybrid"/>
<!-- Other -->
@@ -409,7 +409,6 @@
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="ZERO_AIRSPEED" value="TRUE"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="THRUST_X_EFF" value="0.00055"/>
conf/modules/guidance_indi_hybrid.xml
+4 -3
View File
@@ -3,7 +3,9 @@
<module name="guidance_indi_hybrid" dir="guidance" task="control">
<doc>
<description>
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
</description>
<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_HYBRID_">
<define name="USE_WLS" value="FALSE|TRUE" description="use WLS allocation instead of matrix inversion (default: FALSE)"/>
@@ -31,14 +33,13 @@
</settings>
<dep>
<depends>@navigation,guidance_rotorcraft,wls</depends>
<provides>guidance,attitude_command</provides>
<suggests>guidance_indi_hybrid_tailsitter</suggests>
</dep>
<header>
<file name="guidance_indi_hybrid.h"/>
</header>
<init fun="guidance_indi_init()"/>
<init fun="guidance_indi_enter()"/>
<!--<periodic fun="guidance_indi_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>-->
<makefile target="ap|nps" firmware="rotorcraft">
<file name="guidance_indi_hybrid.c" dir="$(SRC_FIRMWARE)/guidance"/>
<define name="GUIDANCE_INDI_HYBRID" value="TRUE"/>
conf/modules/guidance_indi_hybrid_quadplane.xml
+27
View File
@@ -0,0 +1,27 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="guidance_indi_hybrid_quadplane" dir="guidance" task="control">
<doc>
<description>
Guidance controller for hybrid quadplane vehicles
</description>
</doc>
<dep>
<depends>guidance_indi_hybrid</depends>
<conflicts>guidance_indi_hybrid_tailsitter</conflicts>
<provides>guidance,attitude_command</provides>
</dep>
<header>
<file name="guidance_indi_hybrid_quadplane.h"/>
</header>
<init fun="guidance_indi_quadplane_init()"/>
<periodic fun="guidance_indi_quadplane_propagate_filters()" freq="PERIODIC_FREQUENCY" autorun="TRUE"/>
<makefile target="ap|nps" firmware="rotorcraft">
<define name="GUIDANCE_INDI_HYBRID_USE_WLS" value="1"/>
<!-- Guidance actuators: (roll, pitch, thrust, push) for quadplanes -->
<define name="GUIDANCE_INDI_HYBRID_U" value="4"/>
<!-- Guidance control objectives: ax, ay, ay for quadplanes -->
<define name="GUIDANCE_INDI_HYBRID_V" value="3"/>
<file name="guidance_indi_hybrid_quadplane.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile>
</module>
conf/modules/guidance_indi_hybrid_tailsitter.xml
+24
View File
@@ -0,0 +1,24 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="guidance_indi_hybrid_tailsitter" dir="guidance" task="control">
<doc>
<description>
Guidance controller for hybrid tailsitter vehicles
</description>
</doc>
<dep>
<depends>guidance_indi_hybrid</depends>
<conflicts>guidance_indi_hybrid_quadplane</conflicts>
<provides>guidance,attitude_command</provides>
</dep>
<header>
<file name="guidance_indi_hybrid_tailsitter.h"/>
</header>
<makefile target="ap|nps" firmware="rotorcraft">
<!-- Guidance actuators: (roll, pitch, thrust, push) for quadplanes -->
<define name="GUIDANCE_INDI_HYBRID_U" value="3"/>
<!-- Guidance control objectives: ax, ay, ay for quadplanes -->
<define name="GUIDANCE_INDI_HYBRID_V" value="3"/>
<file name="guidance_indi_hybrid_tailsitter.c" dir="$(SRC_FIRMWARE)/guidance"/>
</makefile>
</module>
conf/modules/nav_hybrid.xml
+2
View File
@@ -40,6 +40,8 @@
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
<define name="GUIDANCE_INDI_HYBRID_U" value="3"/>
<define name="GUIDANCE_INDI_HYBRID_V" value="3"/>
</test>
</makefile>
</module>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
-177
View File
@@ -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.
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+10 -5
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+157
View File
@@ -0,0 +1,157 @@
/*
* Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
*
* 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
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplanes.h → sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.h
+5 -6
View File
@@ -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
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
+110
View File
@@ -0,0 +1,110 @@
/*
* Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
*
* 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
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitters.h → sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.h
+5 -5
View File
@@ -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