Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Add battery voltage/weight/power density influenced settings to battery profiles (#7074)

Browse source
This commit is contained in:
Michel Pastor 2021-06-15 17:13:57 +02:00 • committed by GitHub
parent 9565a8a01c
commit 7dad0bfa68
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
23 changed files with 362 additions and 297 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -51,6 +51,8 @@
#include "rx/rx.h"
#include "sensors/battery.h"
// 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
@ -483,18 +485,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
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;
return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
}
else {
// Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * navConfig()->fw.pitch_to_throttle;
return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
}
}
void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
{
int16_t minThrottleCorrection = navConfig()->fw.min_throttle - navConfig()->fw.cruise_throttle;
int16_t maxThrottleCorrection = navConfig()->fw.max_throttle - navConfig()->fw.cruise_throttle;
int16_t minThrottleCorrection = currentBatteryProfile->nav.fw.min_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
int16_t maxThrottleCorrection = currentBatteryProfile->nav.fw.max_throttle - currentBatteryProfile->nav.fw.cruise_throttle;
if (isRollAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
// ROLL >0 right, <0 left
@ -529,15 +531,15 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle + throttleCorrection, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95)
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - navConfig()->fw.cruise_throttle);
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
else
correctedThrottleValue = motorConfig()->maxthrottle;
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > navConfig()->fw.cruise_throttle);
isAutoThrottleManuallyIncreased = (rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle);
} else {
isAutoThrottleManuallyIncreased = false;
}
@ -600,7 +602,7 @@ void applyFixedWingEmergencyLandingController(void)
rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]);
rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]);
rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]);
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle;
}
/*-----------------------------------------------------------