Added flight3DConfig() macro

This commit is contained in:
Martin Budden 2016-11-30 11:46:20 +00:00
parent 6ccf11d518
commit dc9e773b02
7 changed files with 19 additions and 18 deletions

View File

@ -898,7 +898,7 @@ bool inMotorTestMode(void) {
static uint32_t resetTime = 0; static uint32_t resetTime = 0;
uint16_t inactiveMotorCommand; uint16_t inactiveMotorCommand;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d; inactiveMotorCommand = flight3DConfig()->neutral3d;
#ifdef USE_DSHOT #ifdef USE_DSHOT
} else if (isMotorProtocolDshot()) { } else if (isMotorProtocolDshot()) {
inactiveMotorCommand = DSHOT_DISARM_COMMAND; inactiveMotorCommand = DSHOT_DISARM_COMMAND;

View File

@ -62,6 +62,7 @@
#include "sensors/compass.h" #include "sensors/compass.h"
#define motorConfig(x) (&masterConfig.motorConfig) #define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
// System-wide // System-wide
typedef struct master_s { typedef struct master_s {

View File

@ -1062,16 +1062,16 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_3D: case MSP_3D:
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low); sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high); sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d); sbufWriteU16(dst, flight3DConfig()->neutral3d);
break; break;
case MSP_RC_DEADBAND: case MSP_RC_DEADBAND:
sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband); sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband); sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband); sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle); sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
break; break;
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT:
@ -1410,16 +1410,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_3D: case MSP_SET_3D:
masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src); flight3DConfig()->deadband3d_low = sbufReadU16(src);
masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src); flight3DConfig()->deadband3d_high = sbufReadU16(src);
masterConfig.flight3DConfig.neutral3d = sbufReadU16(src); flight3DConfig()->neutral3d = sbufReadU16(src);
break; break;
case MSP_SET_RC_DEADBAND: case MSP_SET_RC_DEADBAND:
masterConfig.rcControlsConfig.deadband = sbufReadU8(src); masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src); masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src); masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src); flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
break; break;
case MSP_SET_RESET_CURR_PID: case MSP_SET_RESET_CURR_PID:

View File

@ -124,7 +124,7 @@ static void taskUpdateBattery(uint32_t currentTime)
if (ibatTimeSinceLastServiced >= IBATINTERVAL) { if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime; ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
} }
} }
} }
@ -197,7 +197,7 @@ static void taskTelemetry(uint32_t currentTime)
telemetryCheckState(); telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) { if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); telemetryProcess(currentTime, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
} }
} }
#endif #endif

View File

@ -515,7 +515,7 @@ void processRx(uint32_t currentTime)
failsafeUpdateState(); failsafeUpdateState();
} }
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset

View File

@ -707,10 +707,10 @@ const clivalue_t valueTable[] = {
#endif #endif
{ "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } }, { "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxEscThrottleJumpMs, .config.minmax = { 0, 1000 } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
{ "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently,
{ "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },

View File

@ -256,7 +256,7 @@ void init(void)
uint16_t idlePulse = motorConfig()->mincommand; uint16_t idlePulse = motorConfig()->mincommand;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
idlePulse = masterConfig.flight3DConfig.neutral3d; idlePulse = flight3DConfig()->neutral3d;
} }
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
@ -465,7 +465,7 @@ void init(void)
cliInit(&masterConfig.serialConfig); cliInit(&masterConfig.serialConfig);
#endif #endif
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); failsafeInit(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions); rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);