mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 00:05:28 +03:00
first build
This commit is contained in:
parent
0d8e7889ca
commit
1c76c0b3d0
10 changed files with 49 additions and 63 deletions
|
@ -833,13 +833,25 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
|
|||
#endif
|
||||
}
|
||||
|
||||
static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
|
||||
static void applyThrottleTiltCompensation(void)
|
||||
{
|
||||
if (throttleTiltCompensationStrength) {
|
||||
if (STATE(MULTIROTOR)) {
|
||||
int16_t thrTiltCompStrength = 0;
|
||||
|
||||
if (navigationRequiresThrottleTiltCompensation()) {
|
||||
thrTiltCompStrength = 100;
|
||||
}
|
||||
else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
||||
thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
|
||||
}
|
||||
|
||||
if (thrTiltCompStrength) {
|
||||
const int throttleIdleValue = getThrottleIdleValue();
|
||||
float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f); // max tilt about 50 deg
|
||||
return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
|
||||
} else {
|
||||
return 1.0f;
|
||||
tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f);
|
||||
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -891,26 +903,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
|||
applyWaypointNavigationAndAltitudeHold();
|
||||
|
||||
// Apply throttle tilt compensation
|
||||
if (!STATE(FIXED_WING_LEGACY)) {
|
||||
int16_t thrTiltCompStrength = 0;
|
||||
|
||||
if (navigationRequiresThrottleTiltCompensation()) {
|
||||
thrTiltCompStrength = 100;
|
||||
}
|
||||
else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
||||
thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
|
||||
}
|
||||
|
||||
if (thrTiltCompStrength) {
|
||||
rcCommand[THROTTLE] = constrain(getThrottleIdleValue()
|
||||
+ (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
||||
getThrottleIdleValue(),
|
||||
motorConfig()->maxthrottle);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// FIXME: throttle pitch comp for FW
|
||||
}
|
||||
applyThrottleTiltCompensation();
|
||||
|
||||
#ifdef USE_POWER_LIMITS
|
||||
powerLimiterApply(&rcCommand[THROTTLE]);
|
||||
|
|
|
@ -51,6 +51,7 @@
|
|||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
#include "io/beeper.h"
|
||||
|
@ -111,6 +112,17 @@ bool isRollPitchStickDeflected(uint8_t deadband)
|
|||
return (ABS(rcCommand[ROLL]) > deadband) || (ABS(rcCommand[PITCH]) > deadband);
|
||||
}
|
||||
|
||||
uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop)
|
||||
{
|
||||
const uint16_t throttleIdleValue = getThrottleIdleValue();
|
||||
|
||||
if (allowMotorStop && throttle < throttleIdleValue) {
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
return throttle;
|
||||
}
|
||||
return constrain(throttle, throttleIdleValue, motorConfig()->maxthrottle);
|
||||
}
|
||||
|
||||
throttleStatus_e FAST_CODE NOINLINE calculateThrottleStatus(throttleStatusType_e type)
|
||||
{
|
||||
int value = rxGetChannelValue(THROTTLE); // THROTTLE_STATUS_TYPE_RC
|
||||
|
|
|
@ -109,6 +109,7 @@ bool checkStickPosition(stickPositions_e stickPos);
|
|||
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||
bool areSticksDeflected(void);
|
||||
bool isRollPitchStickDeflected(uint8_t deadband);
|
||||
uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop);
|
||||
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
|
||||
int16_t throttleStickMixedValue(void);
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(void);
|
||||
|
|
|
@ -211,14 +211,6 @@ bool failsafeRequiresAngleMode(void)
|
|||
failsafeProcedureLogic[failsafeState.activeProcedure].forceAngleMode;
|
||||
}
|
||||
|
||||
bool failsafeRequiresMotorStop(void)
|
||||
{
|
||||
return failsafeState.active &&
|
||||
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
|
||||
posControl.flags.estAltStatus < EST_USABLE &&
|
||||
currentBatteryProfile->failsafe_throttle < getThrottleIdleValue();
|
||||
}
|
||||
|
||||
void failsafeStartMonitoring(void)
|
||||
{
|
||||
failsafeState.monitoring = true;
|
||||
|
|
|
@ -170,7 +170,6 @@ void failsafeOnRxResume(void);
|
|||
bool failsafeMayRequireNavigationMode(void);
|
||||
void failsafeApplyControlInput(void);
|
||||
bool failsafeRequiresAngleMode(void);
|
||||
bool failsafeRequiresMotorStop(void);
|
||||
bool failsafeShouldApplyControlInput(void);
|
||||
bool failsafeBypassNavigation(void);
|
||||
void failsafeUpdateRcCommandValues(void);
|
||||
|
|
|
@ -627,11 +627,7 @@ int16_t getThrottlePercent(bool useScaled)
|
|||
|
||||
motorStatus_e getMotorStatus(void)
|
||||
{
|
||||
if (failsafeRequiresMotorStop()) {
|
||||
return MOTOR_STOPPED_AUTO;
|
||||
}
|
||||
|
||||
if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) {
|
||||
if (STATE(NAV_MOTOR_STOP_OR_IDLE)) {
|
||||
return MOTOR_STOPPED_AUTO;
|
||||
}
|
||||
|
||||
|
|
|
@ -646,7 +646,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
isAutoThrottleManuallyIncreased = false;
|
||||
}
|
||||
|
||||
rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
|
||||
}
|
||||
|
||||
#ifdef NAV_FIXED_WING_LANDING
|
||||
|
@ -661,7 +661,6 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
(posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
|
||||
|
||||
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
|
||||
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
|
||||
|
@ -761,7 +760,7 @@ bool isFixedWingLandingDetected(void)
|
|||
*-----------------------------------------------------------*/
|
||||
void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
|
||||
{
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
|
||||
|
||||
if (posControl.flags.estAltStatus >= EST_USABLE) {
|
||||
// target min descent rate 10m above takeoff altitude
|
||||
|
|
|
@ -234,19 +234,12 @@ static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTi
|
|||
|
||||
/* Wing control Helpers */
|
||||
|
||||
static bool isThrottleIdleEnabled(void)
|
||||
{
|
||||
return currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue();
|
||||
}
|
||||
|
||||
static void applyThrottleIdleLogic(bool forceMixerIdle)
|
||||
{
|
||||
if (isThrottleIdleEnabled() && !forceMixerIdle) {
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->nav.fw.launch_idle_throttle;
|
||||
}
|
||||
else {
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin given throttle value
|
||||
if (forceMixerIdle) {
|
||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped, otherwise motor will run at idle
|
||||
} else {
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_idle_throttle, true);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -290,7 +283,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs
|
|||
UNUSED(currentTimeUs);
|
||||
|
||||
if (!throttleStickIsLow()) {
|
||||
if (isThrottleIdleEnabled()) {
|
||||
if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) {
|
||||
return FW_LAUNCH_EVENT_SUCCESS;
|
||||
}
|
||||
else {
|
||||
|
@ -404,7 +397,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_
|
|||
|
||||
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
|
||||
const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
|
||||
const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
|
||||
|
||||
if (elapsedTimeMs > motorSpinUpMs) {
|
||||
rcCommand[THROTTLE] = launchThrottle;
|
||||
|
@ -438,7 +431,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
|
|||
}
|
||||
} else {
|
||||
initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
|
||||
rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
|
||||
}
|
||||
|
||||
if (isLaunchMaxAltitudeReached()) {
|
||||
|
@ -468,7 +461,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
|
|||
// Make a smooth transition from the launch state to the current state for pitch angle
|
||||
// Do the same for throttle when manual launch throttle isn't used
|
||||
if (!navConfig()->fw.launch_manual_throttle) {
|
||||
const uint16_t launchThrottle = constrain(currentBatteryProfile->nav.fw.launch_throttle, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||
const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
|
||||
rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
|
||||
}
|
||||
fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
|
||||
|
|
|
@ -273,7 +273,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update throttle controller
|
||||
rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(posControl.rcAdjustment[THROTTLE], false);
|
||||
|
||||
// Save processed throttle for future use
|
||||
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
|
||||
|
@ -914,13 +914,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
rcCommand[YAW] = 0;
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
|
||||
/* Altitude sensors gone haywire, attempt to land regardless */
|
||||
if (posControl.flags.estAltStatus < EST_USABLE) {
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
||||
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||
return;
|
||||
}
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -946,7 +947,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Update throttle
|
||||
rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE];
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(posControl.rcAdjustment[THROTTLE], false);
|
||||
|
||||
// Hold position if possible
|
||||
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
|
|
|
@ -138,7 +138,7 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags,
|
|||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
|
||||
rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
|
||||
} else if (navStateFlags & NAV_CTL_POS) {
|
||||
applyRoverBoatPositionController(currentTimeUs);
|
||||
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue