Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

first build

Browse source
This commit is contained in:
breadoven 2023-09-06 12:40:11 +01:00
parent 0d8e7889ca
commit 1c76c0b3d0
10 changed files with 49 additions and 63 deletions
Download patch file Download diff file Expand all files Collapse all files

43
src/main/fc/fc_core.c
View file

@ -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]);

12
src/main/fc/rc_controls.c
View file

@ -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

1
src/main/fc/rc_controls.h
View file

@ -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);

8
src/main/flight/failsafe.c
View file

@ -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;

1
src/main/flight/failsafe.h
View file

@ -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);

6
src/main/flight/mixer.c
View file

@ -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;
}

5
src/main/navigation/navigation_fixedwing.c
View file

@ -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

23
src/main/navigation/navigation_fw_launch.c
View file

@ -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]);

7
src/main/navigation/navigation_multicopter.c
View file

@ -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)) {

2
src/main/navigation/navigation_rover_boat.c
View file

@ -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);