mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 06:45:14 +03:00
Merge pull request #1512 from iNavFlight/fw-coordinated-turn
Fixed wing coordinated turn
This commit is contained in:
Konstantin Sharlaimov
GitHub
commit
c5616f55c8
10 changed files with 85 additions and 40 deletions
|
|
@ -176,7 +176,6 @@ Re-apply any new defaults as desired.
|
|||
| nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
|
||||
| nav_fw_dive_angle | 15 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
|
||||
| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes |
|
||||
| nav_fw_roll2pitch | 75 | Amount of positive pitch (nose up) when the plane turns by ailerons in GPS assisted modes. With high wind is better lowering this |
|
||||
| nav_fw_loiter_radius | 5000 | PosHold radius in cm. 3000 to 7500 is a good value (30-75m) |
|
||||
| nav_fw_launch_velocity | 300 | Forward velocity threshold for swing-launch detection (cm/s) |
|
||||
| nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch (cm/s/s, 1G = 981 cm/s/s) |
|
||||
|
|
@ -312,6 +311,7 @@ Re-apply any new defaults as desired.
|
|||
| flaperon_throw_offset | 250 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
|
||||
| gimbal_mode | NORMAL | When feature SERVO_TILT is enabled, this can be either NORMAL or MIXTILT |
|
||||
| fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. |
|
||||
| fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. |
|
||||
| mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. |
|
||||
| default_rate_profile | 0 | Default = profile number |
|
||||
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you aquirre valid GPS fix. |
|
||||
|
|
|
|||
|
|
@ -743,6 +743,7 @@ static const clivalue_t valueTable[] = {
|
|||
{ "dterm_setpoint_weight", VAR_FLOAT | PROFILE_VALUE, .config.minmax = {0, 2 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_setpoint_weight) },
|
||||
#ifdef USE_SERVOS
|
||||
{ "fw_iterm_throw_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN, FW_ITERM_THROW_LIMIT_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingItermThrowLimit) },
|
||||
{ "fw_reference_airspeed", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 5000}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingReferenceAirspeed) },
|
||||
#endif
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = {0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_soft_notch_hz) },
|
||||
|
|
@ -853,7 +854,6 @@ static const clivalue_t valueTable[] = {
|
|||
{ "nav_fw_climb_angle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 5, 80 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.max_climb_angle) },
|
||||
{ "nav_fw_dive_angle", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 5, 80 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.max_dive_angle) },
|
||||
{ "nav_fw_pitch2thr", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.pitch_to_throttle) },
|
||||
{ "nav_fw_roll2pitch", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.roll_to_pitch) },
|
||||
{ "nav_fw_loiter_radius", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 10000 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.loiter_radius) },
|
||||
|
||||
{ "nav_fw_launch_velocity", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 10000 }, PG_NAV_CONFIG, offsetof(navConfig_t, fw.launch_velocity_thresh) },
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@
|
|||
#include "sensors/gyro.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/pitotmeter.h"
|
||||
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -106,7 +107,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
|
|||
|
||||
static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
|
||||
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1);
|
||||
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
||||
.bank_mc = {
|
||||
|
|
@ -182,12 +183,14 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
|
||||
.yaw_p_limit = YAW_P_LIMIT_DEFAULT,
|
||||
|
||||
.heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT,
|
||||
|
||||
.max_angle_inclination[FD_ROLL] = 300, // 30 degrees
|
||||
.max_angle_inclination[FD_PITCH] = 300, // 30 degrees
|
||||
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
|
||||
.pidSumLimit = PID_SUM_LIMIT_DEFAULT,
|
||||
|
||||
.heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT,
|
||||
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT,
|
||||
.fixedWingReferenceAirspeed = 1000,
|
||||
);
|
||||
|
||||
void pidInit(void)
|
||||
|
|
@ -629,17 +632,55 @@ float pidHeadingHold(void)
|
|||
static void pidTurnAssistant(pidState_t *pidState)
|
||||
{
|
||||
t_fp_vector targetRates;
|
||||
|
||||
targetRates.V.X = 0.0f;
|
||||
targetRates.V.Y = 0.0f;
|
||||
targetRates.V.Z = pidState[YAW].rateTarget;
|
||||
|
||||
if (STATE(FIXED_WING)) {
|
||||
if (calculateCosTiltAngle() >= 0.173648f) {
|
||||
// Ideal banked turn follow the equations:
|
||||
// forward_vel^2 / radius = Gravity * tan(roll_angle)
|
||||
// yaw_rate = forward_vel / radius
|
||||
// If we solve for roll angle we get:
|
||||
// tan(roll_angle) = forward_vel * yaw_rate / Gravity
|
||||
// If we solve for yaw rate we get:
|
||||
// yaw_rate = tan(roll_angle) * Gravity / forward_vel
|
||||
|
||||
#if defined(PITOT)
|
||||
float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) ?
|
||||
pitot.airSpeed :
|
||||
pidProfile()->fixedWingReferenceAirspeed;
|
||||
#else
|
||||
float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed;
|
||||
#endif
|
||||
|
||||
// Constrain to somewhat sane limits - 10km/h - 216km/h
|
||||
airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
|
||||
|
||||
float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
|
||||
float coordinatedTurnRateOffset = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn;
|
||||
|
||||
targetRates.V.Z = pidState[YAW].rateTarget + RADIANS_TO_DEGREES(coordinatedTurnRateOffset);
|
||||
}
|
||||
else {
|
||||
// Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive
|
||||
return;
|
||||
}
|
||||
}
|
||||
else {
|
||||
targetRates.V.Z = pidState[YAW].rateTarget;
|
||||
}
|
||||
|
||||
// Transform calculated rate offsets into body frame and apply
|
||||
imuTransformVectorEarthToBody(&targetRates);
|
||||
|
||||
// Add in roll and pitch, replace yaw completery
|
||||
pidState[ROLL].rateTarget += targetRates.V.X;
|
||||
pidState[PITCH].rateTarget += targetRates.V.Y;
|
||||
pidState[YAW].rateTarget = targetRates.V.Z;
|
||||
// Add in roll and pitch, replace yaw completely
|
||||
pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.V.X, -currentControlRateProfile->rates[ROLL] * 10.0f, currentControlRateProfile->rates[ROLL] * 10.0f);
|
||||
pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.V.Y, -currentControlRateProfile->rates[PITCH] * 10.0f, currentControlRateProfile->rates[PITCH] * 10.0f);
|
||||
pidState[YAW].rateTarget = constrainf(targetRates.V.Z, -currentControlRateProfile->rates[YAW] * 10.0f, currentControlRateProfile->rates[YAW] * 10.0f);
|
||||
|
||||
debug[0] = pidState[ROLL].rateTarget;
|
||||
debug[1] = pidState[PITCH].rateTarget;
|
||||
debug[2] = pidState[YAW].rateTarget;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -676,7 +717,7 @@ void pidController(void)
|
|||
}
|
||||
|
||||
#ifdef USE_FLM_TURN_ASSIST
|
||||
if (FLIGHT_MODE(TURN_ASSISTANT)) {
|
||||
if (FLIGHT_MODE(TURN_ASSISTANT) || naivationRequiresTurnAssistance()) {
|
||||
pidTurnAssistant(pidState);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -99,9 +99,11 @@ typedef struct pidProfile_s {
|
|||
int16_t max_angle_inclination[ANGLE_INDEX_COUNT]; // Max possible inclination (roll and pitch axis separately
|
||||
|
||||
float dterm_setpoint_weight;
|
||||
|
||||
uint16_t fixedWingItermThrowLimit;
|
||||
uint16_t pidSumLimit;
|
||||
|
||||
// Airplane-specific parameters
|
||||
uint16_t fixedWingItermThrowLimit;
|
||||
float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
|
||||
} pidProfile_t;
|
||||
|
||||
typedef struct pidAutotuneConfig_s {
|
||||
|
|
|
|||
|
|
@ -553,7 +553,7 @@ void dashboardSetNextPageChangeAt(timeUs_t futureMicros)
|
|||
|
||||
void formatTrimDegrees ( char *formattedTrim, int16_t trimValue ) {
|
||||
char trim[6];
|
||||
sprintf(trim, "%d", trimValue);
|
||||
tfp_sprintf(trim, "%d", trimValue);
|
||||
int x = strlen(trim)-1;
|
||||
strncpy(formattedTrim,trim,x);
|
||||
formattedTrim[x] = '\0';
|
||||
|
|
|
|||
|
|
@ -116,7 +116,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.max_throttle = 1700,
|
||||
.min_throttle = 1200,
|
||||
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
|
||||
.roll_to_pitch = 75, // percent of coupling
|
||||
.loiter_radius = 5000, // 50m
|
||||
|
||||
// Fixed wing launch
|
||||
|
|
@ -2424,13 +2423,33 @@ bool naivationRequiresAngleMode(void)
|
|||
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* An indicator that TURN ASSISTANCE is required for navigation
|
||||
*-----------------------------------------------------------*/
|
||||
bool naivationRequiresTurnAssistance(void)
|
||||
{
|
||||
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
|
||||
if (STATE(FIXED_WING)) {
|
||||
// For airplanes turn assistant is always required when controlling position
|
||||
return (currentState & NAV_CTL_POS);
|
||||
}
|
||||
else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
|
||||
*/
|
||||
int8_t naivationGetHeadingControlState(void)
|
||||
{
|
||||
// No explicit MAG_HOLD mode for airplanes
|
||||
if ((navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) && !STATE(FIXED_WING)) {
|
||||
// For airplanes report as manual heading control
|
||||
if (STATE(FIXED_WING)) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
|
||||
// For multirotors it depends on navigation system mode
|
||||
if (navGetStateFlags(posControl.navState) & NAV_REQUIRE_MAGHOLD) {
|
||||
if (posControl.flags.isAdjustingHeading) {
|
||||
return NAV_HEADING_CONTROL_MANUAL;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -141,7 +141,6 @@ typedef struct navConfig_s {
|
|||
uint16_t min_throttle; // Minimum allowed throttle in auto mode
|
||||
uint16_t max_throttle; // Maximum allowed throttle in auto mode
|
||||
uint8_t pitch_to_throttle; // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
|
||||
uint8_t roll_to_pitch; // Roll to pitch compensation (in %)
|
||||
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
|
||||
|
||||
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
|
||||
|
|
@ -258,6 +257,7 @@ void applyWaypointNavigationAndAltitudeHold(void);
|
|||
/* Functions to signal navigation requirements to main loop */
|
||||
bool naivationRequiresAngleMode(void);
|
||||
bool navigationRequiresThrottleTiltCompensation(void);
|
||||
bool naivationRequiresTurnAssistance(void);
|
||||
int8_t naivationGetHeadingControlState(void);
|
||||
bool naivationBlockArming(void);
|
||||
bool navigationPositionEstimateIsHealthy(void);
|
||||
|
|
|
|||
|
|
@ -297,13 +297,6 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
|
|||
|
||||
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
|
||||
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
|
||||
|
||||
// Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
|
||||
posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
|
||||
updateHeadingHoldTarget(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
|
||||
|
||||
// Add pitch compensation
|
||||
//posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
|
||||
}
|
||||
|
||||
void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
||||
|
|
@ -414,7 +407,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
|
||||
// Mix Pitch/Roll/Throttle
|
||||
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
|
||||
pitchCorrection += ABS(posControl.rcAdjustment[ROLL]) * (navConfig()->fw.roll_to_pitch / 100.0f);
|
||||
rollCorrection += posControl.rcAdjustment[ROLL];
|
||||
}
|
||||
|
||||
|
|
@ -440,15 +432,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
|
||||
rollCorrection = constrain(rollCorrection, -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle));
|
||||
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
|
||||
|
||||
// Calculate coordinated turn rate based on velocity and banking angle
|
||||
if (posControl.actualState.velXY >= 300.0f) {
|
||||
float targetYawRateDPS = RADIANS_TO_DEGREES(tan_approx(DECIDEGREES_TO_RADIANS(-rollCorrection)) * GRAVITY_CMSS / posControl.actualState.velXY);
|
||||
rcCommand[YAW] = pidRateToRcCommand(targetYawRateDPS, currentControlRateProfile->rates[FD_YAW]);
|
||||
}
|
||||
else {
|
||||
rcCommand[YAW] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
|
||||
|
|
|
|||
|
|
@ -177,9 +177,9 @@
|
|||
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6
|
||||
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
#define BIND_PIN PA3
|
||||
#undef USE_SERIALRX_SPEKTRUM
|
||||
//#define SPEKTRUM_BIND
|
||||
//#define BIND_PIN PA3
|
||||
|
||||
//#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
|
|
|
|||
|
|
@ -36,9 +36,9 @@
|
|||
#define GPS
|
||||
#define GPS_PROTO_UBLOX
|
||||
#define NAV
|
||||
#define USE_FLM_TURN_ASSIST // This is mandatory for fixed-wing navigation
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_LTM
|
||||
#define USE_FLM_TURN_ASSIST
|
||||
#define TELEMETRY_FRSKY
|
||||
#endif
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue