1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00

Rename settings previously responsible for reversible motors handling

This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-19 14:52:54 +01:00
parent 7e47809c9a
commit f480bcf463
9 changed files with 50 additions and 50 deletions

View file

@ -42,7 +42,7 @@
//#define PG_PROFILE_SELECTION 23 //#define PG_PROFILE_SELECTION 23
#define PG_RX_CONFIG 24 #define PG_RX_CONFIG 24
#define PG_RC_CONTROLS_CONFIG 25 #define PG_RC_CONTROLS_CONFIG 25
#define PG_MOTOR_3D_CONFIG 26 #define PG_REVERSIBLE_MOTORS_CONFIG 26
#define PG_LED_STRIP_CONFIG 27 #define PG_LED_STRIP_CONFIG 27
//#define PG_COLOR_CONFIG 28 //#define PG_COLOR_CONFIG 28
//#define PG_AIRPLANE_ALT_HOLD_CONFIG 29 //#define PG_AIRPLANE_ALT_HOLD_CONFIG 29

View file

@ -1110,16 +1110,16 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_3D: case MSP_3D:
sbufWriteU16(dst, flight3DConfig()->deadband3d_low); sbufWriteU16(dst, reversibleMotorsConfig()->deadband_low);
sbufWriteU16(dst, flight3DConfig()->deadband3d_high); sbufWriteU16(dst, reversibleMotorsConfig()->deadband_high);
sbufWriteU16(dst, flight3DConfig()->neutral3d); sbufWriteU16(dst, reversibleMotorsConfig()->neutral);
break; break;
case MSP_RC_DEADBAND: case MSP_RC_DEADBAND:
sbufWriteU8(dst, rcControlsConfig()->deadband); sbufWriteU8(dst, rcControlsConfig()->deadband);
sbufWriteU8(dst, rcControlsConfig()->yaw_deadband); sbufWriteU8(dst, rcControlsConfig()->yaw_deadband);
sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband); sbufWriteU8(dst, rcControlsConfig()->alt_hold_deadband);
sbufWriteU16(dst, rcControlsConfig()->deadband3d_throttle); sbufWriteU16(dst, rcControlsConfig()->mid_throttle_deadband);
break; break;
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT:
@ -1982,9 +1982,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_3D: case MSP_SET_3D:
if (dataSize >= 6) { if (dataSize >= 6) {
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src); reversibleMotorsConfigMutable()->deadband_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src); reversibleMotorsConfigMutable()->deadband_high = sbufReadU16(src);
flight3DConfigMutable()->neutral3d = sbufReadU16(src); reversibleMotorsConfigMutable()->neutral = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -1994,7 +1994,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
rcControlsConfigMutable()->deadband = sbufReadU8(src); rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src); rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src); rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
rcControlsConfigMutable()->deadband3d_throttle = sbufReadU16(src); rcControlsConfigMutable()->mid_throttle_deadband = sbufReadU16(src);
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;

View file

@ -76,7 +76,7 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.yaw_deadband = 5, .yaw_deadband = 5,
.pos_hold_deadband = 20, .pos_hold_deadband = 20,
.alt_hold_deadband = 50, .alt_hold_deadband = 50,
.deadband3d_throttle = 50, .mid_throttle_deadband = 50,
.airmodeHandlingType = STICK_CENTER, .airmodeHandlingType = STICK_CENTER,
.airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD,
); );
@ -101,8 +101,8 @@ bool areSticksDeflectedMoreThanPosHoldDeadband(void)
throttleStatus_e calculateThrottleStatus(void) throttleStatus_e calculateThrottleStatus(void)
{ {
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; const uint16_t mid_throttle_deadband = rcControlsConfig()->mid_throttle_deadband;
if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - deadband3d_throttle) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + deadband3d_throttle))) if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) > (PWM_RANGE_MIDDLE - mid_throttle_deadband) && rxGetChannelValue(THROTTLE) < (PWM_RANGE_MIDDLE + mid_throttle_deadband)))
return THROTTLE_LOW; return THROTTLE_LOW;
else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck)) else if (!feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck))
return THROTTLE_LOW; return THROTTLE_LOW;

View file

@ -80,7 +80,7 @@ typedef struct rcControlsConfig_s {
uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t yaw_deadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero.
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode) uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC uint16_t mid_throttle_deadband; // default throttle deadband from MIDRC
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation uint16_t airmodeThrottleThreshold; // Throttle threshold for airmode initial activation
} rcControlsConfig_t; } rcControlsConfig_t;

View file

@ -704,19 +704,19 @@ groups:
min: 0 min: 0
max: 450 max: 450
- name: PG_MOTOR_3D_CONFIG - name: PG_REVERSIBLE_MOTORS_CONFIG
type: flight3DConfig_t type: reversibleMotorsConfig_t
members: members:
- name: 3d_deadband_low - name: 3d_deadband_low
field: deadband3d_low field: deadband_low
min: PWM_RANGE_MIN min: PWM_RANGE_MIN
max: PWM_RANGE_MAX max: PWM_RANGE_MAX
- name: 3d_deadband_high - name: 3d_deadband_high
field: deadband3d_high field: deadband_high
min: PWM_RANGE_MIN min: PWM_RANGE_MIN
max: PWM_RANGE_MAX max: PWM_RANGE_MAX
- name: 3d_neutral - name: 3d_neutral
field: neutral3d field: neutral
min: PWM_RANGE_MIN min: PWM_RANGE_MIN
max: PWM_RANGE_MAX max: PWM_RANGE_MAX
@ -940,7 +940,7 @@ groups:
min: 10 min: 10
max: 250 max: 250
- name: 3d_deadband_throttle - name: 3d_deadband_throttle
field: deadband3d_throttle field: mid_throttle_deadband
min: 0 min: 0
max: 200 max: 200
- name: mc_airmode_type - name: mc_airmode_type

View file

@ -63,12 +63,12 @@ static EXTENDED_FASTRAM uint8_t motorCount = 0;
EXTENDED_FASTRAM int mixerThrottleCommand; EXTENDED_FASTRAM int mixerThrottleCommand;
static EXTENDED_FASTRAM int throttleIdleValue = 0; static EXTENDED_FASTRAM int throttleIdleValue = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.deadband3d_low = 1406, .deadband_low = 1406,
.deadband3d_high = 1514, .deadband_high = 1514,
.neutral3d = 1460 .neutral = 1460
); );
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 2);
@ -250,7 +250,7 @@ void mixerResetDisarmedMotors(void)
{ {
// set disarmed motor values // set disarmed motor values
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
motor_disarmed[i] = feature(FEATURE_REVERSIBLE_MOTORS) ? flight3DConfig()->neutral3d : motorConfig()->mincommand; motor_disarmed[i] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
} }
} }
@ -264,12 +264,12 @@ void FAST_CODE NOINLINE writeMotors(void)
if (isMotorProtocolDigital()) { if (isMotorProtocolDigital()) {
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (motor[i] >= throttleIdleValue && motor[i] <= flight3DConfig()->deadband3d_low) { if (motor[i] >= throttleIdleValue && motor[i] <= reversibleMotorsConfig()->deadband_low) {
motorValue = scaleRangef(motor[i], motorConfig()->mincommand, flight3DConfig()->deadband3d_low, DSHOT_3D_DEADBAND_LOW, DSHOT_MIN_THROTTLE); motorValue = scaleRangef(motor[i], motorConfig()->mincommand, reversibleMotorsConfig()->deadband_low, DSHOT_3D_DEADBAND_LOW, 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] >= reversibleMotorsConfig()->deadband_high && motor[i] <= motorConfig()->maxthrottle) {
motorValue = scaleRangef(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); motorValue = scaleRangef(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE); motorValue = constrain(motorValue, DSHOT_3D_DEADBAND_HIGH, DSHOT_MAX_THROTTLE);
} }
else { else {
@ -309,7 +309,7 @@ void writeAllMotors(int16_t mc)
void stopMotors(void) void stopMotors(void)
{ {
writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? flight3DConfig()->neutral3d : motorConfig()->mincommand); writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
delay(50); // give the timers and ESCs a chance to react. delay(50); // give the timers and ESCs a chance to react.
} }
@ -367,20 +367,20 @@ void FAST_CODE NOINLINE mixTable(const float dT)
if (feature(FEATURE_REVERSIBLE_MOTORS)) { 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 (!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()->deadband3d_throttle))) { // Out of band handling if ((rcCommand[THROTTLE] <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband))) { // Out of band handling
throttleMax = flight3DConfig()->deadband3d_low; throttleMax = reversibleMotorsConfig()->deadband_low;
throttleMin = throttleIdleValue; 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()->mid_throttle_deadband)) { // Positive handling
throttleMax = motorConfig()->maxthrottle; throttleMax = motorConfig()->maxthrottle;
throttleMin = flight3DConfig()->deadband3d_high; throttleMin = reversibleMotorsConfig()->deadband_high;
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()->mid_throttle_deadband))) { // Deadband handling from negative to positive
mixerThrottleCommand = throttleMax = flight3DConfig()->deadband3d_low; mixerThrottleCommand = throttleMax = reversibleMotorsConfig()->deadband_low;
throttleMin = throttleIdleValue; 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 = reversibleMotorsConfig()->deadband_high;
} }
} else { } else {
mixerThrottleCommand = rcCommand[THROTTLE]; mixerThrottleCommand = rcCommand[THROTTLE];
@ -425,10 +425,10 @@ void FAST_CODE NOINLINE mixTable(const float dT)
if (failsafeIsActive()) { if (failsafeIsActive()) {
motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], motorConfig()->mincommand, motorConfig()->maxthrottle);
} else if (feature(FEATURE_REVERSIBLE_MOTORS)) { } else if (feature(FEATURE_REVERSIBLE_MOTORS)) {
if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->deadband3d_throttle)) { if (throttlePrevious <= (PWM_RANGE_MIDDLE - rcControlsConfig()->mid_throttle_deadband)) {
motor[i] = constrain(motor[i], throttleIdleValue, flight3DConfig()->deadband3d_low); motor[i] = constrain(motor[i], throttleIdleValue, reversibleMotorsConfig()->deadband_low);
} else { } else {
motor[i] = constrain(motor[i], flight3DConfig()->deadband3d_high, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], reversibleMotorsConfig()->deadband_high, motorConfig()->maxthrottle);
} }
} else { } else {
motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle); motor[i] = constrain(motor[i], throttleIdleValue, motorConfig()->maxthrottle);

View file

@ -72,13 +72,13 @@ typedef struct mixerConfig_s {
PG_DECLARE(mixerConfig_t, mixerConfig); PG_DECLARE(mixerConfig_t, mixerConfig);
typedef struct flight3DConfig_s { typedef struct reversibleMotorsConfig_s {
uint16_t deadband3d_low; // min 3d value uint16_t deadband_low; // min 3d value
uint16_t deadband3d_high; // max 3d value uint16_t deadband_high; // max 3d value
uint16_t neutral3d; // center 3d value uint16_t neutral; // center 3d value
} flight3DConfig_t; } reversibleMotorsConfig_t;
PG_DECLARE(flight3DConfig_t, flight3DConfig); PG_DECLARE(reversibleMotorsConfig_t, reversibleMotorsConfig);
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)

View file

@ -281,10 +281,10 @@ bool feature(uint32_t mask)
return false; return false;
} }
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband)
{ {
UNUSED(*rxConfig); UNUSED(*rxConfig);
UNUSED(deadband3d_throttle); UNUSED(mid_throttle_deadband);
return THROTTLE_HIGH; return THROTTLE_HIGH;
} }

View file

@ -420,10 +420,10 @@ uint32_t millis(void)
return sysTickUptime; return sysTickUptime;
} }
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t mid_throttle_deadband)
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
UNUSED(deadband3d_throttle); UNUSED(mid_throttle_deadband);
return throttleStatus; return throttleStatus;
} }