mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-21 15:25:29 +03:00
Merge pull request #6104 from Airwide/aw-fw-nav-smooth-throttle
Smooth throttle in navigation modes on FW
This commit is contained in:
Konstantin Sharlaimov
GitHub
commit
efeefd9eb8
7 changed files with 71 additions and 26 deletions
|
|
@ -272,6 +272,8 @@
|
|||
| nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes |
|
||||
| nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes |
|
||||
| nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) |
|
||||
| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by nav_fw_pitch2thr_threshold |
|
||||
| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
|
||||
| nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
|
||||
| nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
|
||||
| nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
|
||||
|
|
@ -486,4 +488,4 @@
|
|||
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
|
||||
| yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
|
||||
|
||||
> Note: this table is autogenerated. Do not edit it manually.
|
||||
> Note: this table is autogenerated. Do not edit it manually.
|
||||
|
|
|
|||
|
|
@ -109,6 +109,8 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] =
|
|||
OSD_SETTING_ENTRY("PITCH TO THR RATIO", SETTING_NAV_FW_PITCH2THR),
|
||||
OSD_SETTING_ENTRY("LOITER RADIUS", SETTING_NAV_FW_LOITER_RADIUS),
|
||||
OSD_SETTING_ENTRY("CONTROL SMOOTHNESS", SETTING_NAV_FW_CONTROL_SMOOTHNESS),
|
||||
OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING),
|
||||
OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD),
|
||||
|
||||
OSD_BACK_AND_END_ENTRY,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -2284,6 +2284,18 @@ groups:
|
|||
field: fw.pitch_to_throttle
|
||||
min: 0
|
||||
max: 100
|
||||
- name: nav_fw_pitch2thr_smoothing
|
||||
description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh."
|
||||
default_value: "0"
|
||||
field: fw.pitch_to_throttle_smooth
|
||||
min: 0
|
||||
max: 9
|
||||
- name: nav_fw_pitch2thr_threshold
|
||||
description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]"
|
||||
default_value: "0"
|
||||
field: fw.pitch_to_throttle_thresh
|
||||
min: 0
|
||||
max: 900
|
||||
- name: nav_fw_loiter_radius
|
||||
description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]"
|
||||
default_value: "5000"
|
||||
|
|
|
|||
|
|
@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
|||
// pitch in degrees
|
||||
// output in Watt
|
||||
static float estimatePitchPower(float pitch) {
|
||||
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
||||
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
|
||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||
|
|
|
|||
|
|
@ -156,6 +156,8 @@ 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)
|
||||
.pitch_to_throttle_smooth = 0,
|
||||
.pitch_to_throttle_thresh = 0,
|
||||
.loiter_radius = 5000, // 50m
|
||||
|
||||
//Fixed wing landing
|
||||
|
|
@ -1886,13 +1888,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
|
|||
// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
|
||||
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
|
||||
float navPidApply3(
|
||||
pidController_t *pid,
|
||||
const float setpoint,
|
||||
const float measurement,
|
||||
const float dt,
|
||||
const float outMin,
|
||||
const float outMax,
|
||||
const pidControllerFlags_e pidFlags,
|
||||
pidController_t *pid,
|
||||
const float setpoint,
|
||||
const float measurement,
|
||||
const float dt,
|
||||
const float outMin,
|
||||
const float outMax,
|
||||
const pidControllerFlags_e pidFlags,
|
||||
const float gainScaler,
|
||||
const float dTermScaler
|
||||
) {
|
||||
|
|
@ -2418,7 +2420,7 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void)
|
|||
/*******************************************************
|
||||
* Is a safehome being used instead of the arming point?
|
||||
*******************************************************/
|
||||
bool isSafeHomeInUse(void)
|
||||
bool isSafeHomeInUse(void)
|
||||
{
|
||||
return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES);
|
||||
}
|
||||
|
|
@ -2429,7 +2431,7 @@ bool isSafeHomeInUse(void)
|
|||
**********************************************************/
|
||||
bool foundNearbySafeHome(void)
|
||||
{
|
||||
safehome_used = -1;
|
||||
safehome_used = -1;
|
||||
fpVector3_t safeHome;
|
||||
gpsLocation_t shLLH;
|
||||
shLLH.alt = 0;
|
||||
|
|
@ -2437,7 +2439,7 @@ bool foundNearbySafeHome(void)
|
|||
shLLH.lat = safeHomeConfig(i)->lat;
|
||||
shLLH.lon = safeHomeConfig(i)->lon;
|
||||
geoConvertGeodeticToLocal(&safeHome, &posControl.gpsOrigin, &shLLH, GEO_ALT_RELATIVE);
|
||||
safehome_distance = calculateDistanceToDestination(&safeHome);
|
||||
safehome_distance = calculateDistanceToDestination(&safeHome);
|
||||
if (safehome_distance < 20000) { // 200 m
|
||||
safehome_used = i;
|
||||
setHomePosition(&safeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity());
|
||||
|
|
@ -2937,7 +2939,7 @@ bool saveNonVolatileWaypointList(void)
|
|||
|
||||
#if defined(USE_SAFE_HOME)
|
||||
|
||||
void resetSafeHomes(void)
|
||||
void resetSafeHomes(void)
|
||||
{
|
||||
memset(safeHomeConfigMutable(0), 0, sizeof(navSafeHome_t) * MAX_SAFE_HOMES);
|
||||
}
|
||||
|
|
@ -3237,7 +3239,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
|
||||
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
|
|
|
|||
|
|
@ -228,6 +228,8 @@ 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)
|
||||
uint16_t pitch_to_throttle_smooth; // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
|
||||
uint8_t pitch_to_throttle_thresh; // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
|
||||
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
|
||||
int8_t land_dive_angle;
|
||||
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
|
||||
|
|
@ -468,7 +470,7 @@ bool loadNonVolatileWaypointList(void);
|
|||
bool saveNonVolatileWaypointList(void);
|
||||
|
||||
float getFinalRTHAltitude(void);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
|
||||
|
||||
/* Geodetic functions */
|
||||
typedef enum {
|
||||
|
|
|
|||
|
|
@ -51,8 +51,8 @@
|
|||
|
||||
#include "rx/rx.h"
|
||||
|
||||
// Base frequencies for smoothing pitch and roll
|
||||
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
|
||||
// Base frequencies for smoothing pitch and roll
|
||||
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
|
||||
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
|
||||
|
||||
// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
|
||||
|
|
@ -70,12 +70,21 @@ static bool isAutoThrottleManuallyIncreased = false;
|
|||
static int32_t navHeadingError;
|
||||
static int8_t loiterDirYaw = 1;
|
||||
|
||||
// Calculates the cutoff frequency for smoothing out roll/pitch commands
|
||||
// Calculates the cutoff frequency for smoothing out roll/pitch commands
|
||||
// control_smoothness valid range from 0 to 9
|
||||
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
|
||||
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
|
||||
static float getSmoothnessCutoffFreq(float baseFreq)
|
||||
{
|
||||
uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
|
||||
return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.01f;
|
||||
}
|
||||
|
||||
// Calculates the cutoff frequency for smoothing out pitchToThrottleCorrection
|
||||
// pitch_to_throttle_smooth valid range from 0 to 9
|
||||
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
|
||||
static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq)
|
||||
{
|
||||
uint16_t smoothness = 10 - navConfig()->fw.pitch_to_throttle_smooth;
|
||||
return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
|
||||
}
|
||||
|
||||
|
|
@ -205,7 +214,7 @@ static fpVector3_t virtualDesiredPosition;
|
|||
static pt1Filter_t fwPosControllerCorrectionFilterState;
|
||||
|
||||
/*
|
||||
* TODO Currently this function resets both FixedWing and Rover & Boat position controller
|
||||
* TODO Currently this function resets both FixedWing and Rover & Boat position controller
|
||||
*/
|
||||
void resetFixedWingPositionController(void)
|
||||
{
|
||||
|
|
@ -300,9 +309,9 @@ float processHeadingYawController(timeDelta_t deltaMicros, int32_t navHeadingErr
|
|||
const pidControllerFlags_e yawPidFlags = errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0;
|
||||
|
||||
const float yawAdjustment = navPidApply2(
|
||||
&posControl.pids.fw_heading,
|
||||
0,
|
||||
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
|
||||
&posControl.pids.fw_heading,
|
||||
0,
|
||||
applyDeadband(navHeadingError, navConfig()->fw.yawControlDeadband * 100),
|
||||
US2S(deltaMicros),
|
||||
-limit,
|
||||
limit,
|
||||
|
|
@ -461,9 +470,25 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
|||
return throttleSpeedAdjustment;
|
||||
}
|
||||
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs)
|
||||
{
|
||||
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
|
||||
static timeUs_t previousTimePitchToThrCorr = 0;
|
||||
const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr;
|
||||
previousTimePitchToThrCorr = currentTimeUs;
|
||||
|
||||
static pt1Filter_t pitchToThrFilterState;
|
||||
|
||||
// Apply low-pass filter to pitch angle to smooth throttle correction
|
||||
int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
|
||||
|
||||
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
|
||||
// Unfiltered throttle correction outside of pitch deadband
|
||||
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
|
||||
}
|
||||
else {
|
||||
// Filtered throttle correction inside of pitch deadband
|
||||
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
|
||||
}
|
||||
}
|
||||
|
||||
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
|
||||
|
|
@ -485,7 +510,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
// PITCH >0 dive, <0 climb
|
||||
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));
|
||||
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
|
||||
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection);
|
||||
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
|
||||
|
||||
#ifdef NAV_FIXED_WING_LANDING
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue