mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-18 22:05:15 +03:00
Drop min_throttle in favor of throttle IDLE percent
This commit is contained in:
parent
419d4e48e0
commit
4fd2149e2b
20 changed files with 63 additions and 61 deletions
|
@ -755,7 +755,7 @@ static void writeIntraframe(void)
|
||||||
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
|
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
|
||||||
* Throttle lies in range [minthrottle..maxthrottle]:
|
* Throttle lies in range [minthrottle..maxthrottle]:
|
||||||
*/
|
*/
|
||||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
|
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - getThrottleIdleValue());
|
||||||
|
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||||
/*
|
/*
|
||||||
|
@ -809,7 +809,7 @@ static void writeIntraframe(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
//Motors can be below minthrottle when disarmed, but that doesn't happen much
|
//Motors can be below minthrottle when disarmed, but that doesn't happen much
|
||||||
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
|
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - getThrottleIdleValue());
|
||||||
|
|
||||||
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
|
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
|
||||||
const int motorCount = getMotorCount();
|
const int motorCount = getMotorCount();
|
||||||
|
@ -1618,10 +1618,10 @@ static bool blackboxWriteSysinfo(void)
|
||||||
BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
|
BLACKBOX_PRINT_HEADER_LINE("Log start datetime", "%s", blackboxGetStartDateTime(buf));
|
||||||
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
|
BLACKBOX_PRINT_HEADER_LINE("Craft name", "%s", systemConfig()->name);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
BLACKBOX_PRINT_HEADER_LINE("P interval", "%u/%u", blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", motorConfig()->minthrottle);
|
BLACKBOX_PRINT_HEADER_LINE("minthrottle", "%d", getThrottleIdleValue());
|
||||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
|
BLACKBOX_PRINT_HEADER_LINE("maxthrottle", "%d", motorConfig()->maxthrottle);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f));
|
BLACKBOX_PRINT_HEADER_LINE("gyro_scale", "0x%x", castFloatBytesToInt(1.0f));
|
||||||
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", motorConfig()->minthrottle,motorConfig()->maxthrottle);
|
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
|
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||||
|
|
|
@ -42,7 +42,7 @@ static const OSD_Entry menuMiscEntries[]=
|
||||||
{
|
{
|
||||||
OSD_LABEL_ENTRY("-- MISC --"),
|
OSD_LABEL_ENTRY("-- MISC --"),
|
||||||
|
|
||||||
OSD_SETTING_ENTRY("MIN THR", SETTING_MIN_THROTTLE),
|
OSD_SETTING_ENTRY("THR IDLE", SETTING_THROTTLE_IDLE),
|
||||||
#if defined(USE_OSD) && defined(USE_ADC)
|
#if defined(USE_OSD) && defined(USE_ADC)
|
||||||
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
|
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
|
||||||
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
|
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
|
||||||
|
|
|
@ -793,9 +793,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (thrTiltCompStrength) {
|
if (thrTiltCompStrength) {
|
||||||
rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle
|
rcCommand[THROTTLE] = constrain(getThrottleIdleValue()
|
||||||
+ (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
+ (rcCommand[THROTTLE] - getThrottleIdleValue()) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
||||||
motorConfig()->minthrottle,
|
getThrottleIdleValue(),
|
||||||
motorConfig()->maxthrottle);
|
motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -702,7 +702,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
case MSP_MISC:
|
case MSP_MISC:
|
||||||
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
||||||
|
|
||||||
sbufWriteU16(dst, motorConfig()->minthrottle);
|
sbufWriteU16(dst, 0); // Was min_throttle
|
||||||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||||
|
|
||||||
|
@ -732,7 +732,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
||||||
case MSP2_INAV_MISC:
|
case MSP2_INAV_MISC:
|
||||||
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
sbufWriteU16(dst, PWM_RANGE_MIDDLE);
|
||||||
|
|
||||||
sbufWriteU16(dst, motorConfig()->minthrottle);
|
sbufWriteU16(dst, 0); //Was min_throttle
|
||||||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||||
|
|
||||||
|
@ -1712,7 +1712,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
if (dataSize >= 22) {
|
if (dataSize >= 22) {
|
||||||
sbufReadU16(src); // midrc
|
sbufReadU16(src); // midrc
|
||||||
|
|
||||||
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
sbufReadU16(src); //Was min_throttle
|
||||||
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||||
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
||||||
|
|
||||||
|
@ -1753,7 +1753,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
|
||||||
if (dataSize == 41) {
|
if (dataSize == 41) {
|
||||||
sbufReadU16(src); // midrc
|
sbufReadU16(src); // midrc
|
||||||
|
|
||||||
motorConfigMutable()->minthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
sbufReadU16(src); // was min_throttle
|
||||||
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
motorConfigMutable()->maxthrottle = constrain(sbufReadU16(src), PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||||
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
motorConfigMutable()->mincommand = constrain(sbufReadU16(src), 0, PWM_RANGE_MAX);
|
||||||
|
|
||||||
|
|
|
@ -41,7 +41,7 @@ int16_t lookupThrottleRCMid; // THROTTLE curve mid point
|
||||||
|
|
||||||
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
|
void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
|
||||||
{
|
{
|
||||||
lookupThrottleRCMid = motorConfig()->minthrottle + (int32_t)(motorConfig()->maxthrottle - motorConfig()->minthrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
|
lookupThrottleRCMid = getThrottleIdleValue() + (int32_t)(motorConfig()->maxthrottle - getThrottleIdleValue()) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
|
||||||
|
|
||||||
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
|
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
|
||||||
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
|
const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8;
|
||||||
|
@ -51,7 +51,7 @@ void generateThrottleCurve(const controlRateConfig_t *controlRateConfig)
|
||||||
if (tmp < 0)
|
if (tmp < 0)
|
||||||
y = controlRateConfig->throttle.rcMid8;
|
y = controlRateConfig->throttle.rcMid8;
|
||||||
lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
|
lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10;
|
||||||
lookupThrottleRC[i] = motorConfig()->minthrottle + (int32_t) (motorConfig()->maxthrottle - motorConfig()->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
|
lookupThrottleRC[i] = getThrottleIdleValue() + (int32_t) (motorConfig()->maxthrottle - getThrottleIdleValue()) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -478,10 +478,6 @@ groups:
|
||||||
type: motorConfig_t
|
type: motorConfig_t
|
||||||
headers: ["flight/mixer.h"]
|
headers: ["flight/mixer.h"]
|
||||||
members:
|
members:
|
||||||
- name: min_throttle
|
|
||||||
field: minthrottle
|
|
||||||
min: PWM_RANGE_MIN
|
|
||||||
max: PWM_RANGE_MAX
|
|
||||||
- name: max_throttle
|
- name: max_throttle
|
||||||
field: maxthrottle
|
field: maxthrottle
|
||||||
min: PWM_RANGE_MIN
|
min: PWM_RANGE_MIN
|
||||||
|
@ -509,6 +505,10 @@ groups:
|
||||||
field: throttleScale
|
field: throttleScale
|
||||||
min: 0
|
min: 0
|
||||||
max: 1
|
max: 1
|
||||||
|
- name: throttle_idle
|
||||||
|
field: throttleIdle
|
||||||
|
min: 4
|
||||||
|
max: 30
|
||||||
- name: motor_poles
|
- name: motor_poles
|
||||||
field: motorPoleCount
|
field: motorPoleCount
|
||||||
min: 4
|
min: 4
|
||||||
|
|
|
@ -216,7 +216,7 @@ bool failsafeRequiresMotorStop(void)
|
||||||
{
|
{
|
||||||
return failsafeState.active &&
|
return failsafeState.active &&
|
||||||
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
|
failsafeState.activeProcedure == FAILSAFE_PROCEDURE_AUTO_LANDING &&
|
||||||
failsafeConfig()->failsafe_throttle < motorConfig()->minthrottle;
|
failsafeConfig()->failsafe_throttle < getThrottleIdleValue();
|
||||||
}
|
}
|
||||||
|
|
||||||
void failsafeStartMonitoring(void)
|
void failsafeStartMonitoring(void)
|
||||||
|
@ -287,7 +287,7 @@ void failsafeApplyControlInput(void)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case THROTTLE:
|
case THROTTLE:
|
||||||
rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->minthrottle;
|
rcCommand[idx] = feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : getThrottleIdleValue();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -61,6 +61,7 @@ static float mixerScale = 1.0f;
|
||||||
static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||||
static EXTENDED_FASTRAM uint8_t motorCount = 0;
|
static EXTENDED_FASTRAM uint8_t motorCount = 0;
|
||||||
EXTENDED_FASTRAM int mixerThrottleCommand;
|
EXTENDED_FASTRAM int mixerThrottleCommand;
|
||||||
|
static EXTENDED_FASTRAM int throttleIdleValue = 1150;
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
|
||||||
|
|
||||||
|
@ -96,7 +97,6 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 4);
|
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 4);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
||||||
.minthrottle = DEFAULT_MIN_THROTTLE,
|
|
||||||
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
|
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
|
||||||
.motorPwmRate = DEFAULT_PWM_RATE,
|
.motorPwmRate = DEFAULT_PWM_RATE,
|
||||||
.maxthrottle = DEFAULT_MAX_THROTTLE,
|
.maxthrottle = DEFAULT_MAX_THROTTLE,
|
||||||
|
@ -104,6 +104,7 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
|
||||||
.motorAccelTimeMs = 0,
|
.motorAccelTimeMs = 0,
|
||||||
.motorDecelTimeMs = 0,
|
.motorDecelTimeMs = 0,
|
||||||
.digitalIdleOffsetValue = 450, // Same scale as in Betaflight
|
.digitalIdleOffsetValue = 450, // Same scale as in Betaflight
|
||||||
|
.throttleIdle = 15.0f,
|
||||||
.throttleScale = 1.0f,
|
.throttleScale = 1.0f,
|
||||||
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles
|
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles
|
||||||
);
|
);
|
||||||
|
@ -113,6 +114,11 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTO
|
||||||
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
|
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
|
||||||
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
|
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
|
||||||
|
|
||||||
|
int getThrottleIdleValue(void)
|
||||||
|
{
|
||||||
|
return throttleIdleValue;
|
||||||
|
}
|
||||||
|
|
||||||
static void computeMotorCount(void)
|
static void computeMotorCount(void)
|
||||||
{
|
{
|
||||||
motorCount = 0;
|
motorCount = 0;
|
||||||
|
@ -174,7 +180,7 @@ void applyMotorRateLimiting(const float dT)
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
// Calculate max motor step
|
// Calculate max motor step
|
||||||
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
|
const uint16_t motorRange = motorConfig()->maxthrottle - throttleIdleValue;
|
||||||
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
|
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
|
||||||
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
|
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
|
||||||
|
|
||||||
|
@ -183,12 +189,12 @@ void applyMotorRateLimiting(const float dT)
|
||||||
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
|
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
|
||||||
|
|
||||||
// Handle throttle below min_throttle (motor start/stop)
|
// Handle throttle below min_throttle (motor start/stop)
|
||||||
if (motorPrevious[i] < motorConfig()->minthrottle) {
|
if (motorPrevious[i] < throttleIdleValue) {
|
||||||
if (motor[i] < motorConfig()->minthrottle) {
|
if (motor[i] < throttleIdleValue) {
|
||||||
motorPrevious[i] = motor[i];
|
motorPrevious[i] = motor[i];
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
motorPrevious[i] = motorConfig()->minthrottle;
|
motorPrevious[i] = throttleIdleValue;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -202,6 +208,7 @@ void applyMotorRateLimiting(const float dT)
|
||||||
|
|
||||||
void mixerInit(void)
|
void mixerInit(void)
|
||||||
{
|
{
|
||||||
|
throttleIdleValue = motorConfig()->mincommand + (((motorConfig()->maxthrottle - motorConfig()->mincommand) / 100.0f) * motorConfig()->throttleIdle);
|
||||||
computeMotorCount();
|
computeMotorCount();
|
||||||
loadPrimaryMotorMixer();
|
loadPrimaryMotorMixer();
|
||||||
// in 3D mode, mixer gain has to be halved
|
// in 3D mode, mixer gain has to be halved
|
||||||
|
@ -237,8 +244,8 @@ void FAST_CODE NOINLINE writeMotors(void)
|
||||||
const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue;
|
const float dshotMinThrottleOffset = (DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 10000.0f * motorConfig()->digitalIdleOffsetValue;
|
||||||
|
|
||||||
if (feature(FEATURE_3D)) {
|
if (feature(FEATURE_3D)) {
|
||||||
if (motor[i] >= motorConfig()->minthrottle && motor[i] <= flight3DConfig()->deadband3d_low) {
|
if (motor[i] >= throttleIdleValue && motor[i] <= flight3DConfig()->deadband3d_low) {
|
||||||
motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE);
|
motorValue = scaleRangef(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, dshotMinThrottleOffset + DSHOT_MIN_THROTTLE);
|
||||||
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
|
motorValue = constrain(motorValue, DSHOT_MIN_THROTTLE, DSHOT_3D_DEADBAND_LOW);
|
||||||
}
|
}
|
||||||
else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) {
|
else if (motor[i] >= flight3DConfig()->deadband3d_high && motor[i] <= motorConfig()->maxthrottle) {
|
||||||
|
@ -250,11 +257,11 @@ void FAST_CODE NOINLINE writeMotors(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
if (motor[i] < motorConfig()->minthrottle) { // motor disarmed
|
if (motor[i] < throttleIdleValue) { // motor disarmed
|
||||||
motorValue = DSHOT_DISARM_COMMAND;
|
motorValue = DSHOT_DISARM_COMMAND;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
motorValue = scaleRangef(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
motorValue = scaleRangef(motor[i], throttleIdleValue, motorConfig()->maxthrottle, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
||||||
motorValue = constrain(motorValue, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
motorValue = constrain(motorValue, (dshotMinThrottleOffset + DSHOT_MIN_THROTTLE), DSHOT_MAX_THROTTLE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -337,7 +344,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
||||||
// Find min and max throttle based on condition.
|
// Find min and max throttle based on condition.
|
||||||
#ifdef USE_GLOBAL_FUNCTIONS
|
#ifdef USE_GLOBAL_FUNCTIONS
|
||||||
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
|
if (GLOBAL_FUNCTION_FLAG(GLOBAL_FUNCTION_FLAG_OVERRIDE_THROTTLE)) {
|
||||||
throttleMin = motorConfig()->minthrottle;
|
throttleMin = throttleIdleValue;
|
||||||
throttleMax = motorConfig()->maxthrottle;
|
throttleMax = motorConfig()->maxthrottle;
|
||||||
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
|
mixerThrottleCommand = constrain(globalFunctionValues[GLOBAL_FUNCTION_ACTION_OVERRIDE_THROTTLE], throttleMin, throttleMax);
|
||||||
} else
|
} else
|
||||||
|
@ -347,7 +354,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
||||||
|
|
||||||
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
|
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Out of band handling
|
||||||
throttleMax = flight3DConfig()->deadband3d_low;
|
throttleMax = flight3DConfig()->deadband3d_low;
|
||||||
throttleMin = motorConfig()->minthrottle;
|
throttleMin = throttleIdleValue;
|
||||||
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
||||||
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
|
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->deadband3d_throttle)) { // Positive handling
|
||||||
throttleMax = motorConfig()->maxthrottle;
|
throttleMax = motorConfig()->maxthrottle;
|
||||||
|
@ -355,14 +362,14 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
||||||
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
|
||||||
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
|
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
|
||||||
mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low;
|
mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low;
|
||||||
throttleMin = motorConfig()->minthrottle;
|
throttleMin = throttleIdleValue;
|
||||||
} else { // Deadband handling from positive to negative
|
} else { // Deadband handling from positive to negative
|
||||||
throttleMax = motorConfig()->maxthrottle;
|
throttleMax = motorConfig()->maxthrottle;
|
||||||
mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high;
|
mixerThrottleCommand = throttleMin = flight3DConfig()->deadband3d_high;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
mixerThrottleCommand = rcCommand[THROTTLE];
|
mixerThrottleCommand = rcCommand[THROTTLE];
|
||||||
throttleMin = motorConfig()->minthrottle;
|
throttleMin = throttleIdleValue;
|
||||||
throttleMax = motorConfig()->maxthrottle;
|
throttleMax = motorConfig()->maxthrottle;
|
||||||
|
|
||||||
// Throttle scaling to limit max throttle when battery is full
|
// Throttle scaling to limit max throttle when battery is full
|
||||||
|
@ -404,12 +411,12 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
||||||
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
|
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
|
||||||
} else if (feature(FEATURE_3D)) {
|
} else if (feature(FEATURE_3D)) {
|
||||||
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) {
|
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) {
|
||||||
motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig()->deadband3d_low);
|
motor[i] = constrain(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low);
|
||||||
} else {
|
} else {
|
||||||
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
|
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
motor[i] = constrain(motor[i], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Motor stop handling
|
// Motor stop handling
|
||||||
|
@ -417,7 +424,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
|
||||||
if (feature(FEATURE_MOTOR_STOP)) {
|
if (feature(FEATURE_MOTOR_STOP)) {
|
||||||
motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
|
motor[i] = (feature(FEATURE_3D) ? PWM_RANGE_MIDDLE : motorConfig()->mincommand);
|
||||||
} else {
|
} else {
|
||||||
motor[i] = motorConfig()->minthrottle;
|
motor[i] = throttleIdleValue;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -83,7 +83,6 @@ PG_DECLARE(flight3DConfig_t, flight3DConfig);
|
||||||
|
|
||||||
typedef struct motorConfig_s {
|
typedef struct motorConfig_s {
|
||||||
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
|
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
|
||||||
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
|
|
||||||
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
||||||
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
|
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
|
||||||
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
|
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
|
||||||
|
@ -91,6 +90,7 @@ typedef struct motorConfig_s {
|
||||||
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
|
uint16_t motorAccelTimeMs; // Time limit for motor to accelerate from 0 to 100% throttle [ms]
|
||||||
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
|
uint16_t motorDecelTimeMs; // Time limit for motor to decelerate from 0 to 100% throttle [ms]
|
||||||
uint16_t digitalIdleOffsetValue;
|
uint16_t digitalIdleOffsetValue;
|
||||||
|
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
|
||||||
float throttleScale; // Scaling factor for throttle.
|
float throttleScale; // Scaling factor for throttle.
|
||||||
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
|
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
|
||||||
} motorConfig_t;
|
} motorConfig_t;
|
||||||
|
@ -107,6 +107,7 @@ extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||||
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
|
||||||
extern int mixerThrottleCommand;
|
extern int mixerThrottleCommand;
|
||||||
|
|
||||||
|
int getThrottleIdleValue(void);
|
||||||
uint8_t getMotorCount(void);
|
uint8_t getMotorCount(void);
|
||||||
float getMotorMixRange(void);
|
float getMotorMixRange(void);
|
||||||
bool mixerIsOutputSaturated(void);
|
bool mixerIsOutputSaturated(void);
|
||||||
|
|
|
@ -322,7 +322,7 @@ void pidResetTPAFilter(void)
|
||||||
{
|
{
|
||||||
if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
|
if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
|
||||||
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
|
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
|
||||||
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
|
pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -370,10 +370,10 @@ static float calculateFixedWingTPAFactor(uint16_t throttle)
|
||||||
|
|
||||||
// tpa_rate is amount of curve TPA applied to PIDs
|
// tpa_rate is amount of curve TPA applied to PIDs
|
||||||
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
|
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
|
||||||
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > motorConfig()->minthrottle) {
|
if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) {
|
||||||
if (throttle > motorConfig()->minthrottle) {
|
if (throttle > getThrottleIdleValue()) {
|
||||||
// Calculate TPA according to throttle
|
// Calculate TPA according to throttle
|
||||||
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - motorConfig()->minthrottle) / (throttle - motorConfig()->minthrottle) / 2.0f);
|
tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
|
||||||
|
|
||||||
// Limit to [0.5; 2] range
|
// Limit to [0.5; 2] range
|
||||||
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
|
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
|
||||||
|
|
|
@ -79,7 +79,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
||||||
static float estimatePitchPower(float pitch) {
|
static float estimatePitchPower(float pitch) {
|
||||||
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
||||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - motorConfig()->minthrottle) / (navConfig()->fw.cruise_throttle - motorConfig()->minthrottle);
|
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
|
||||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -484,7 +484,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
||||||
isAutoThrottleManuallyIncreased = false;
|
isAutoThrottleManuallyIncreased = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
rcCommand[THROTTLE] = constrain(correctedThrottleValue, motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
rcCommand[THROTTLE] = constrain(correctedThrottleValue, getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef NAV_FIXED_WING_LANDING
|
#ifdef NAV_FIXED_WING_LANDING
|
||||||
|
@ -497,7 +497,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
||||||
((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) ) {
|
((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
|
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||||
rcCommand[THROTTLE] = motorConfig()->minthrottle;
|
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||||
|
|
||||||
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
|
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
|
||||||
|
|
|
@ -138,10 +138,10 @@ static void applyFixedWingLaunchIdleLogic(void)
|
||||||
pidResetTPAFilter();
|
pidResetTPAFilter();
|
||||||
|
|
||||||
// Throttle control logic
|
// Throttle control logic
|
||||||
if (navConfig()->fw.launch_idle_throttle <= motorConfig()->minthrottle)
|
if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue())
|
||||||
{
|
{
|
||||||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
|
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); // If MOTOR_STOP is enabled mixer will keep motor stopped
|
||||||
rcCommand[THROTTLE] = motorConfig()->minthrottle; // If MOTOR_STOP is disabled, motors will spin at minthrottle
|
rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -155,7 +155,7 @@ static void applyFixedWingLaunchIdleLogic(void)
|
||||||
const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
|
const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
|
||||||
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
|
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
|
||||||
0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
|
0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
|
||||||
motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
|
getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -200,7 +200,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
|
||||||
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
|
// Increase throttle gradually over `launch_motor_spinup_time` milliseconds
|
||||||
if (navConfig()->fw.launch_motor_spinup_time > 0) {
|
if (navConfig()->fw.launch_motor_spinup_time > 0) {
|
||||||
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
|
const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
|
||||||
const uint16_t minIdleThrottle = MAX(motorConfig()->minthrottle, navConfig()->fw.launch_idle_throttle);
|
const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
|
||||||
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
|
rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
|
||||||
0.0f, navConfig()->fw.launch_motor_spinup_time,
|
0.0f, navConfig()->fw.launch_motor_spinup_time,
|
||||||
minIdleThrottle, navConfig()->fw.launch_throttle);
|
minIdleThrottle, navConfig()->fw.launch_throttle);
|
||||||
|
|
|
@ -104,7 +104,7 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
|
||||||
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
||||||
{
|
{
|
||||||
// Calculate min and max throttle boundaries (to compensate for integral windup)
|
// Calculate min and max throttle boundaries (to compensate for integral windup)
|
||||||
const int16_t thrAdjustmentMin = (int16_t)motorConfig()->minthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)navConfig()->mc.hover_throttle;
|
||||||
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)navConfig()->mc.hover_throttle;
|
||||||
|
|
||||||
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
posControl.rcAdjustment[THROTTLE] = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0);
|
||||||
|
@ -116,7 +116,7 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
|
||||||
bool adjustMulticopterAltitudeFromRCInput(void)
|
bool adjustMulticopterAltitudeFromRCInput(void)
|
||||||
{
|
{
|
||||||
if (posControl.flags.isTerrainFollowEnabled) {
|
if (posControl.flags.isTerrainFollowEnabled) {
|
||||||
const float altTarget = scaleRangef(rcCommand[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude);
|
const float altTarget = scaleRangef(rcCommand[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle, 0, navConfig()->general.max_terrain_follow_altitude);
|
||||||
|
|
||||||
// In terrain follow mode we apply different logic for terrain control
|
// In terrain follow mode we apply different logic for terrain control
|
||||||
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
|
if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) {
|
||||||
|
@ -144,7 +144,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
// Scaling from minthrottle to altHoldThrottleRCZero
|
// Scaling from minthrottle to altHoldThrottleRCZero
|
||||||
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - motorConfig()->minthrottle - rcControlsConfig()->alt_hold_deadband);
|
rcClimbRate = rcThrottleAdjustment * navConfig()->general.max_manual_climb_rate / (float)(altHoldThrottleRCZero - getThrottleIdleValue() - rcControlsConfig()->alt_hold_deadband);
|
||||||
}
|
}
|
||||||
|
|
||||||
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
|
updateClimbRateToAltitudeController(rcClimbRate, ROC_TO_ALT_NORMAL);
|
||||||
|
@ -181,7 +181,7 @@ void setupMulticopterAltitudeController(void)
|
||||||
|
|
||||||
// Make sure we are able to satisfy the deadband
|
// Make sure we are able to satisfy the deadband
|
||||||
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
|
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
|
||||||
motorConfig()->minthrottle + rcControlsConfig()->alt_hold_deadband + 10,
|
getThrottleIdleValue() + rcControlsConfig()->alt_hold_deadband + 10,
|
||||||
motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10);
|
motorConfig()->maxthrottle - rcControlsConfig()->alt_hold_deadband - 10);
|
||||||
|
|
||||||
// Force AH controller to initialize althold integral for pending takeoff on reset
|
// Force AH controller to initialize althold integral for pending takeoff on reset
|
||||||
|
@ -249,7 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update throttle controller
|
// Update throttle controller
|
||||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||||
|
|
||||||
// Save processed throttle for future use
|
// Save processed throttle for future use
|
||||||
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
|
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
|
||||||
|
@ -721,12 +721,12 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Update throttle controller
|
// Update throttle controller
|
||||||
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
rcCommand[THROTTLE] = constrain((int16_t)navConfig()->mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
/* Sensors has gone haywire, attempt to land regardless */
|
/* Sensors has gone haywire, attempt to land regardless */
|
||||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_DROP_IT) {
|
||||||
rcCommand[THROTTLE] = motorConfig()->minthrottle;
|
rcCommand[THROTTLE] = getThrottleIdleValue();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||||
|
|
|
@ -57,7 +57,6 @@ void targetConfiguration(void)
|
||||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||||
motorConfigMutable()->minthrottle = 1000;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
pidProfileMutable()->bank_mc.pid[ROLL].P = 36;
|
pidProfileMutable()->bank_mc.pid[ROLL].P = 36;
|
||||||
|
|
|
@ -63,7 +63,6 @@ void targetConfiguration(void)
|
||||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_BRUSHED;
|
||||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||||
motorConfigMutable()->minthrottle = 1000;
|
|
||||||
}
|
}
|
||||||
if (hardwareRevision == AFF4_REV_1) {
|
if (hardwareRevision == AFF4_REV_1) {
|
||||||
rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048;
|
rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048;
|
||||||
|
|
|
@ -64,7 +64,6 @@ void targetConfiguration(void)
|
||||||
|
|
||||||
if (hardwareMotorType == MOTOR_BRUSHED) {
|
if (hardwareMotorType == MOTOR_BRUSHED) {
|
||||||
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
motorConfigMutable()->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
|
||||||
motorConfigMutable()->minthrottle = 1000;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hardwareRevision == AFF7_REV_1) {
|
if (hardwareRevision == AFF7_REV_1) {
|
||||||
|
|
|
@ -37,6 +37,5 @@ void targetConfiguration(void)
|
||||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
||||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
||||||
motorConfigMutable()->motorPwmRate = 4000;
|
motorConfigMutable()->motorPwmRate = 4000;
|
||||||
motorConfigMutable()->minthrottle = 1075;
|
|
||||||
motorConfigMutable()->maxthrottle = 1950;
|
motorConfigMutable()->maxthrottle = 1950;
|
||||||
}
|
}
|
||||||
|
|
|
@ -37,6 +37,5 @@ void targetConfiguration(void)
|
||||||
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT;
|
||||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_MULTISHOT;
|
||||||
motorConfigMutable()->motorPwmRate = 4000;
|
motorConfigMutable()->motorPwmRate = 4000;
|
||||||
motorConfigMutable()->minthrottle = 1075;
|
|
||||||
motorConfigMutable()->maxthrottle = 1950;
|
motorConfigMutable()->maxthrottle = 1950;
|
||||||
}
|
}
|
||||||
|
|
|
@ -82,7 +82,6 @@ void targetConfiguration(void)
|
||||||
blackboxConfigMutable()->rate_num = 1;
|
blackboxConfigMutable()->rate_num = 1;
|
||||||
blackboxConfigMutable()->rate_denom = 4;
|
blackboxConfigMutable()->rate_denom = 4;
|
||||||
|
|
||||||
motorConfigMutable()->minthrottle = 1015;
|
|
||||||
motorConfigMutable()->maxthrottle = 2000;
|
motorConfigMutable()->maxthrottle = 2000;
|
||||||
motorConfigMutable()->mincommand = 980;
|
motorConfigMutable()->mincommand = 980;
|
||||||
motorConfigMutable()->motorPwmRate = 2000;
|
motorConfigMutable()->motorPwmRate = 2000;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue