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

Correctly handle reversible motors in mixer

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-23 19:31:16 +01:00
parent 9b146470a9
commit 0ffcac2974
11 changed files with 105 additions and 33 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -200,7 +200,7 @@ static void updateArmingStatus(void)
/* CHECK: Throttle */
if (!armingConfig()->fixed_wing_auto_arm) {
// Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
if (calculateThrottleStatus() != THROTTLE_LOW) {
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
} else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
@ -529,7 +529,7 @@ void processRx(timeUs_t currentTimeUs)
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
// When armed and motors aren't spinning, do beeps periodically
if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) {

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

@ -99,12 +99,19 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
}
throttleStatus_e calculateThrottleStatus(void)
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type)
{
int value;
if (type == THROTTLE_STATUS_TYPE_RC) {
value = rxGetChannelValue(THROTTLE);
} else {
value = rcCommand[THROTTLE];
}
const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
if (feature(FEATURE_REVERSIBLE_MOTORS) && (value > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && value < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW;
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (value < rxConfig()->mincheck))
return THROTTLE_LOW;
return THROTTLE_HIGH;

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

@ -45,6 +45,11 @@ typedef enum {
THROTTLE_HIGH
} throttleStatus_e;
typedef enum {
THROTTLE_STATUS_TYPE_RC = 0,
THROTTLE_STATUS_TYPE_COMMAND
} throttleStatusType_e;
typedef enum {
NOT_CENTERED = 0,
CENTERED
@ -100,7 +105,7 @@ bool checkStickPosition(stickPositions_e stickPos);
bool areSticksInApModePosition(uint16_t ap_mode);
bool areSticksDeflectedMoreThanPosHoldDeadband(void);
throttleStatus_e calculateThrottleStatus(void);
throttleStatus_e calculateThrottleStatus(throttleStatusType_e type);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus);

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

@ -397,7 +397,7 @@ void failsafeUpdateState(void)
case FAILSAFE_IDLE:
if (armed) {
// Track throttle command below minimum time
if (THROTTLE_HIGH == calculateThrottleStatus()) {
if (THROTTLE_HIGH == calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC)) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
}
if (!receivingRxDataAndNotFailsafeMode) {

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

@ -63,6 +63,11 @@ static EXTENDED_FASTRAM uint8_t motorCount = 0;
EXTENDED_FASTRAM int mixerThrottleCommand;
static EXTENDED_FASTRAM int throttleIdleValue = 0;
static EXTENDED_FASTRAM int motorValueWhenStopped = 0;
static reversibleMotorsThrottleState_e reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
static EXTENDED_FASTRAM int throttleDeadbandLow = 0;
static EXTENDED_FASTRAM int throttleDeadbandHigh = 0;
static EXTENDED_FASTRAM int throttleRangeMin = 0;
static EXTENDED_FASTRAM int throttleRangeMax = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
@ -238,6 +243,9 @@ void mixerInit(void)
mixerScale = 0.5f;
}
throttleDeadbandLow = PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband;
throttleDeadbandHigh = PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband;
mixerResetDisarmedMotors();
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
@ -249,7 +257,19 @@ void mixerInit(void)
void mixerResetDisarmedMotors(void)
{
const int motorZeroCommand = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
int motorZeroCommand;
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
motorZeroCommand = reversibleMotorsConfig()->neutral;
throttleRangeMin = throttleDeadbandHigh;
throttleRangeMax = motorConfig()->maxthrottle;
} else {
motorZeroCommand = motorConfig()->mincommand;
throttleRangeMin = getThrottleIdleValue();
throttleRangeMax = motorConfig()->maxthrottle;
}
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
if (feature(FEATURE_MOTOR_STOP)) {
motorValueWhenStopped = motorZeroCommand;
@ -296,7 +316,26 @@ void FAST_CODE NOINLINE writeMotors(void)
}
}
else {
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) {
if (motor[i] < throttleRangeMin) {
motorValue = motor[i];
} else {
motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
motorValue = constrain(motorValue, reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
}
} else {
if (motor[i] > throttleRangeMax) {
motorValue = motor[i];
} else {
motorValue = scaleRangef(motor[i], throttleRangeMin, throttleRangeMax, motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low);
motorValue = constrain(motorValue, motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low);
}
}
} else {
motorValue = motor[i];
}
}
#else
// We don't define USE_DSHOT
@ -328,6 +367,19 @@ void stopPwmAllMotors(void)
pwmShutdownPulsesForAllMotors(motorCount);
}
static int getReversibleMotorsThrottleDeadband(void)
{
int directionValue;
if (reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) {
directionValue = reversibleMotorsConfig()->deadband_low;
} else {
directionValue = reversibleMotorsConfig()->deadband_high;
}
return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue;
}
void FAST_CODE NOINLINE mixTable(const float dT)
{
int16_t input[3]; // RPY, range [-500:+500]
@ -363,8 +415,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
int16_t rpyMixRange = rpyMixMax - rpyMixMin;
int16_t throttleRange;
int16_t throttleMin, throttleMax;
int16_t throttleRangeMin, throttleRangeMax;
static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions
// Find min and max throttle based on condition.
#ifdef USE_GLOBAL_FUNCTIONS
@ -375,23 +426,26 @@ void FAST_CODE NOINLINE mixTable(const float dT)
} else
#endif
if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = PWM_RANGE_MIDDLE; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
throttleRangeMax = reversibleMotorsConfig()->deadband_low;
throttleRangeMin = throttleIdleValue;
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (PWM_RANGE_MIDDLE + rcControlsConfig()->mid_throttle_deadband)) { // Positive handling
if (rcCommand[THROTTLE] >= (throttleDeadbandHigh)) {
/*
* Throttle is above deadband, FORWARD direction
*/
reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD;
throttleRangeMax = motorConfig()->maxthrottle;
throttleRangeMin = reversibleMotorsConfig()->deadband_high;
throttlePrevious = mixerThrottleCommand = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Deadband handling from negative to positive
mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
throttleRangeMin = throttleIdleValue;
} else { // Deadband handling from positive to negative
throttleRangeMax = motorConfig()->maxthrottle;
mixerThrottleCommand = throttleMin = reversibleMotorsConfig()->deadband_high;
throttleRangeMin = throttleDeadbandHigh;
} else if (rcCommand[THROTTLE] <= throttleDeadbandLow) {
/*
* Throttle is below deadband, BACKWARD direction
*/
reversibleMotorsThrottleState = MOTOR_DIRECTION_BACKWARD;
throttleRangeMax = throttleDeadbandLow;
throttleRangeMin = motorConfig()->mincommand;
}
motorValueWhenStopped = getReversibleMotorsThrottleDeadband();
mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax);
} else {
mixerThrottleCommand = rcCommand[THROTTLE];
throttleRangeMin = throttleIdleValue;
@ -463,7 +517,7 @@ motorStatus_e getMotorStatus(void)
return MOTOR_STOPPED_AUTO;
}
if (calculateThrottleStatus() == THROTTLE_LOW) {
if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
if ((STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
return MOTOR_STOPPED_USER;
}

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

@ -102,6 +102,12 @@ typedef enum {
MOTOR_RUNNING
} motorStatus_e;
typedef enum {
MOTOR_DIRECTION_FORWARD,
MOTOR_DIRECTION_BACKWARD,
MOTOR_DIRECTION_DEADBAND
} reversibleMotorsThrottleState_e;
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand;

4
src/main/navigation/navigation.c
View file

@ -1552,7 +1552,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if (feature(FEATURE_FW_LAUNCH)) {
throttleStatus_e throttleStatus = calculateThrottleStatus();
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
abortFixedWingLaunch();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@ -2909,7 +2909,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
else {
// If we were in LAUNCH mode - force switch to IDLE only if the throttle is low
if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
throttleStatus_e throttleStatus = calculateThrottleStatus();
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if (throttleStatus != THROTTLE_LOW)
return NAV_FSM_EVENT_NONE;
else

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

@ -146,7 +146,7 @@ static void applyFixedWingLaunchIdleLogic(void)
else
{
static float timeThrottleRaisedMs;
if (calculateThrottleStatus() == THROTTLE_LOW)
if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
{
timeThrottleRaisedMs = millis();
}

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

@ -164,7 +164,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
void setupMulticopterAltitudeController(void)
{
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if (navConfig()->general.flags.use_thr_mid_for_althold) {
altHoldThrottleRCZero = rcLookupThrottleMid();

2
src/main/sensors/battery.c
View file

@ -466,7 +466,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
case CURRENT_SENSOR_VIRTUAL:
amperage = batteryMetersConfig()->current.offset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus();
throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000;
int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000;

2
src/main/telemetry/frsky_d.c
View file

@ -196,7 +196,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
sendDataHead(ID_RPM);
if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC);
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
throttleForRPM = 0;
serialize16(throttleForRPM);