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:
parent
7e47809c9a
commit
f480bcf463
9 changed files with 50 additions and 50 deletions
|
@ -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
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue