Merge pull request #1711 from martinbudden/bf_pg_style
Use parameter group style config() macros
This commit is contained in:
commit
16eed3c27c
|
@ -495,7 +495,7 @@ static void writeIntraframe(void)
|
|||
* 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]:
|
||||
*/
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle);
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle);
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||
/*
|
||||
|
@ -539,7 +539,7 @@ static void writeIntraframe(void)
|
|||
blackboxWriteSigned16VBArray(blackboxCurrent->debug, 4);
|
||||
|
||||
//Motors can be below minthrottle when disarmed, but that doesn't happen much
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle);
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - motorConfig()->minthrottle);
|
||||
|
||||
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others
|
||||
for (x = 1; x < motorCount; x++) {
|
||||
|
@ -898,13 +898,13 @@ bool inMotorTestMode(void) {
|
|||
static uint32_t resetTime = 0;
|
||||
uint16_t inactiveMotorCommand;
|
||||
if (feature(FEATURE_3D)) {
|
||||
inactiveMotorCommand = masterConfig.flight3DConfig.neutral3d;
|
||||
inactiveMotorCommand = flight3DConfig()->neutral3d;
|
||||
#ifdef USE_DSHOT
|
||||
} else if (isMotorProtocolDshot()) {
|
||||
inactiveMotorCommand = DSHOT_DISARM_COMMAND;
|
||||
#endif
|
||||
} else {
|
||||
inactiveMotorCommand = masterConfig.motorConfig.mincommand;
|
||||
inactiveMotorCommand = motorConfig()->mincommand;
|
||||
}
|
||||
|
||||
int i;
|
||||
|
@ -1173,8 +1173,8 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("Craft name:%s", masterConfig.name);
|
||||
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackboxConfig.rate_num, masterConfig.blackboxConfig.rate_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", motorConfig()->minthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", motorConfig()->maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G);
|
||||
|
||||
|
@ -1199,7 +1199,7 @@ static bool blackboxWriteSysinfo()
|
|||
);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", masterConfig.gyroConfig.gyro_sync_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
|
||||
|
@ -1262,25 +1262,25 @@ static bool blackboxWriteSysinfo()
|
|||
|
||||
BLACKBOX_PRINT_HEADER_LINE("deadband:%d", masterConfig.rcControlsConfig.deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", masterConfig.gyroConfig.gyro_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", masterConfig.gyroConfig.gyro_soft_lpf_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", masterConfig.gyroConfig.gyro_soft_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_hz_1,
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", masterConfig.gyroConfig.gyro_soft_notch_cutoff_1,
|
||||
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(masterConfig.accelerometerConfig.acc_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", masterConfig.sensorSelectionConfig.acc_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", masterConfig.sensorSelectionConfig.baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", masterConfig.sensorSelectionConfig.mag_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf:%d", gyroConfig()->gyro_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_soft_type:%d", gyroConfig()->gyro_soft_lpf_type);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz:%d", gyroConfig()->gyro_soft_lpf_hz);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz:%d,%d", gyroConfig()->gyro_soft_notch_hz_1,
|
||||
gyroConfig()->gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", sensorSelectionConfig()->acc_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", sensorSelectionConfig()->baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", sensorSelectionConfig()->mag_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", masterConfig.armingConfig.gyro_cal_on_first_arm);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", masterConfig.rxConfig.rcInterpolation);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", masterConfig.rxConfig.rcInterpolationInterval);
|
||||
BLACKBOX_PRINT_HEADER_LINE("airmode_activate_throttle:%d", masterConfig.rxConfig.airModeActivateThreshold);
|
||||
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider:%d", masterConfig.rxConfig.serialrx_provider);
|
||||
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", masterConfig.motorConfig.useUnsyncedPwm);
|
||||
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", masterConfig.motorConfig.motorPwmProtocol);
|
||||
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", masterConfig.motorConfig.motorPwmRate);
|
||||
BLACKBOX_PRINT_HEADER_LINE("unsynced_fast_pwm:%d", motorConfig()->useUnsyncedPwm);
|
||||
BLACKBOX_PRINT_HEADER_LINE("fast_pwm_protocol:%d", motorConfig()->motorPwmProtocol);
|
||||
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate:%d", motorConfig()->motorPwmRate);
|
||||
BLACKBOX_PRINT_HEADER_LINE("debug_mode:%d", masterConfig.debug_mode);
|
||||
BLACKBOX_PRINT_HEADER_LINE("features:%d", masterConfig.enabledFeatures);
|
||||
|
||||
|
|
|
@ -282,11 +282,11 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] =
|
|||
{
|
||||
{ "-- FILTER GLB --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.gyroConfig.gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
|
||||
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
|
||||
{ "GYRO LPF", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig()->gyro_soft_lpf_hz, 0, 255, 1 }, 0 },
|
||||
{ "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_1, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_hz_2, 0, 500, 1 }, 0 },
|
||||
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig()->gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 },
|
||||
|
||||
{ "BACK", OME_Back, NULL, NULL, 0 },
|
||||
{ NULL, OME_END, NULL, NULL, 0 }
|
||||
|
|
|
@ -83,7 +83,7 @@ static OSD_Entry menuMiscEntries[]=
|
|||
{
|
||||
{ "-- MISC --", OME_Label, NULL, NULL, 0 },
|
||||
|
||||
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &masterConfig.motorConfig.minthrottle, 1000, 2000, 1 }, 0 },
|
||||
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 },
|
||||
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatscale, 1, 250, 1 }, 0 },
|
||||
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50, 1 }, 0 },
|
||||
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},
|
||||
|
|
|
@ -61,6 +61,21 @@
|
|||
#include "sensors/battery.h"
|
||||
#include "sensors/compass.h"
|
||||
|
||||
#define motorConfig(x) (&masterConfig.motorConfig)
|
||||
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
|
||||
#define servoConfig(x) (&masterConfig.servoConfig)
|
||||
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
|
||||
#define gimbalConfig(x) (&masterConfig.gimbalConfig)
|
||||
#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig)
|
||||
#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig)
|
||||
#define sensorTrims(x) (&masterConfig.sensorTrims)
|
||||
#define boardAlignment(x) (&masterConfig.boardAlignment)
|
||||
#define imuConfig(x) (&masterConfig.imuConfig)
|
||||
#define gyroConfig(x) (&masterConfig.gyroConfig)
|
||||
#define compassConfig(x) (&masterConfig.compassConfig)
|
||||
#define accelerometerConfig(x) (&masterConfig.accelerometerConfig)
|
||||
#define barometerConfig(x) (&masterConfig.barometerConfig)
|
||||
|
||||
// System-wide
|
||||
typedef struct master_s {
|
||||
uint8_t version;
|
||||
|
|
|
@ -528,7 +528,7 @@ controlRateConfig_t *getControlRateConfig(uint8_t profileIndex)
|
|||
|
||||
uint16_t getCurrentMinthrottle(void)
|
||||
{
|
||||
return masterConfig.motorConfig.minthrottle;
|
||||
return motorConfig()->minthrottle;
|
||||
}
|
||||
|
||||
|
||||
|
@ -842,8 +842,8 @@ void activateConfig(void)
|
|||
#endif
|
||||
|
||||
useFailsafeConfig(&masterConfig.failsafeConfig);
|
||||
setAccelerationTrims(&masterConfig.sensorTrims.accZero);
|
||||
setAccelerationFilter(masterConfig.accelerometerConfig.acc_lpf_hz);
|
||||
setAccelerationTrims(&sensorTrims()->accZero);
|
||||
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
|
||||
|
||||
mixerUseConfigs(
|
||||
&masterConfig.flight3DConfig,
|
||||
|
@ -878,8 +878,8 @@ void activateConfig(void)
|
|||
|
||||
void validateAndFixConfig(void)
|
||||
{
|
||||
if((masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) && (masterConfig.motorConfig.mincommand < 1000)){
|
||||
masterConfig.motorConfig.mincommand = 1000;
|
||||
if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
|
||||
motorConfig()->mincommand = 1000;
|
||||
}
|
||||
|
||||
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
|
||||
|
@ -991,16 +991,16 @@ void validateAndFixConfig(void)
|
|||
void validateAndFixGyroConfig(void)
|
||||
{
|
||||
// Prevent invalid notch cutoff
|
||||
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 >= masterConfig.gyroConfig.gyro_soft_notch_hz_1) {
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = 0;
|
||||
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
|
||||
gyroConfig()->gyro_soft_notch_hz_1 = 0;
|
||||
}
|
||||
if (masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 >= masterConfig.gyroConfig.gyro_soft_notch_hz_2) {
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = 0;
|
||||
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
|
||||
gyroConfig()->gyro_soft_notch_hz_2 = 0;
|
||||
}
|
||||
|
||||
if (masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_256HZ && masterConfig.gyroConfig.gyro_lpf != GYRO_LPF_NONE) {
|
||||
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
|
||||
masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
|
||||
masterConfig.gyroConfig.gyro_sync_denom = 1;
|
||||
gyroConfig()->gyro_sync_denom = 1;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -778,9 +778,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
case MSP_MISC:
|
||||
sbufWriteU16(dst, masterConfig.rxConfig.midrc);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.motorConfig.minthrottle);
|
||||
sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle);
|
||||
sbufWriteU16(dst, masterConfig.motorConfig.mincommand);
|
||||
sbufWriteU16(dst, motorConfig()->minthrottle);
|
||||
sbufWriteU16(dst, motorConfig()->maxthrottle);
|
||||
sbufWriteU16(dst, motorConfig()->mincommand);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
|
||||
|
||||
|
@ -797,7 +797,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
sbufWriteU8(dst, masterConfig.rxConfig.rssi_channel);
|
||||
sbufWriteU8(dst, 0);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.compassConfig.mag_declination / 10);
|
||||
sbufWriteU16(dst, compassConfig()->mag_declination / 10);
|
||||
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatscale);
|
||||
sbufWriteU8(dst, masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
|
@ -866,9 +866,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_BOARD_ALIGNMENT:
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->rollDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->yawDegrees);
|
||||
break;
|
||||
|
||||
case MSP_VOLTAGE_METER_CONFIG:
|
||||
|
@ -938,9 +938,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
|
||||
sbufWriteU8(dst, masterConfig.rxConfig.serialrx_provider);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.rollDegrees);
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.pitchDegrees);
|
||||
sbufWriteU16(dst, masterConfig.boardAlignment.yawDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->rollDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->pitchDegrees);
|
||||
sbufWriteU16(dst, boardAlignment()->yawDegrees);
|
||||
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterScale);
|
||||
sbufWriteU16(dst, masterConfig.batteryConfig.currentMeterOffset);
|
||||
|
@ -1062,47 +1062,47 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_3D:
|
||||
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_low);
|
||||
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_high);
|
||||
sbufWriteU16(dst, masterConfig.flight3DConfig.neutral3d);
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_low);
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_high);
|
||||
sbufWriteU16(dst, flight3DConfig()->neutral3d);
|
||||
break;
|
||||
|
||||
case MSP_RC_DEADBAND:
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.deadband);
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.yaw_deadband);
|
||||
sbufWriteU8(dst, masterConfig.rcControlsConfig.alt_hold_deadband);
|
||||
sbufWriteU16(dst, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
sbufWriteU16(dst, flight3DConfig()->deadband3d_throttle);
|
||||
break;
|
||||
|
||||
case MSP_SENSOR_ALIGNMENT:
|
||||
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.gyro_align);
|
||||
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.acc_align);
|
||||
sbufWriteU8(dst, masterConfig.sensorAlignmentConfig.mag_align);
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align);
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->acc_align);
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->mag_align);
|
||||
break;
|
||||
|
||||
case MSP_ADVANCED_CONFIG:
|
||||
if (masterConfig.gyroConfig.gyro_lpf) {
|
||||
if (gyroConfig()->gyro_lpf) {
|
||||
sbufWriteU8(dst, 8); // If gyro_lpf != OFF then looptime is set to 1000
|
||||
sbufWriteU8(dst, 1);
|
||||
} else {
|
||||
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_sync_denom);
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_sync_denom);
|
||||
sbufWriteU8(dst, masterConfig.pid_process_denom);
|
||||
}
|
||||
sbufWriteU8(dst, masterConfig.motorConfig.useUnsyncedPwm);
|
||||
sbufWriteU8(dst, masterConfig.motorConfig.motorPwmProtocol);
|
||||
sbufWriteU16(dst, masterConfig.motorConfig.motorPwmRate);
|
||||
sbufWriteU8(dst, motorConfig()->useUnsyncedPwm);
|
||||
sbufWriteU8(dst, motorConfig()->motorPwmProtocol);
|
||||
sbufWriteU16(dst, motorConfig()->motorPwmRate);
|
||||
break;
|
||||
|
||||
case MSP_FILTER_CONFIG :
|
||||
sbufWriteU8(dst, masterConfig.gyroConfig.gyro_soft_lpf_hz);
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_lpf_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.yaw_lpf_hz);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_1);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_1);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_hz);
|
||||
sbufWriteU16(dst, currentProfile->pidProfile.dterm_notch_cutoff);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_hz_2);
|
||||
sbufWriteU16(dst, masterConfig.gyroConfig.gyro_soft_notch_cutoff_2);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2);
|
||||
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
break;
|
||||
|
||||
case MSP_PID_ADVANCED:
|
||||
|
@ -1121,9 +1121,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_SENSOR_CONFIG:
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.acc_hardware);
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.baro_hardware);
|
||||
sbufWriteU8(dst, masterConfig.sensorSelectionConfig.mag_hardware);
|
||||
sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware);
|
||||
sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware);
|
||||
sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware);
|
||||
break;
|
||||
|
||||
case MSP_REBOOT:
|
||||
|
@ -1336,9 +1336,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
if (tmp < 1600 && tmp > 1400)
|
||||
masterConfig.rxConfig.midrc = tmp;
|
||||
|
||||
masterConfig.motorConfig.minthrottle = sbufReadU16(src);
|
||||
masterConfig.motorConfig.maxthrottle = sbufReadU16(src);
|
||||
masterConfig.motorConfig.mincommand = sbufReadU16(src);
|
||||
motorConfig()->minthrottle = sbufReadU16(src);
|
||||
motorConfig()->maxthrottle = sbufReadU16(src);
|
||||
motorConfig()->mincommand = sbufReadU16(src);
|
||||
|
||||
masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);
|
||||
|
||||
|
@ -1355,7 +1355,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
masterConfig.rxConfig.rssi_channel = sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
|
||||
masterConfig.compassConfig.mag_declination = sbufReadU16(src) * 10;
|
||||
compassConfig()->mag_declination = sbufReadU16(src) * 10;
|
||||
|
||||
masterConfig.batteryConfig.vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
|
@ -1410,16 +1410,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_3D:
|
||||
masterConfig.flight3DConfig.deadband3d_low = sbufReadU16(src);
|
||||
masterConfig.flight3DConfig.deadband3d_high = sbufReadU16(src);
|
||||
masterConfig.flight3DConfig.neutral3d = sbufReadU16(src);
|
||||
flight3DConfig()->deadband3d_low = sbufReadU16(src);
|
||||
flight3DConfig()->deadband3d_high = sbufReadU16(src);
|
||||
flight3DConfig()->neutral3d = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_RC_DEADBAND:
|
||||
masterConfig.rcControlsConfig.deadband = sbufReadU8(src);
|
||||
masterConfig.rcControlsConfig.yaw_deadband = sbufReadU8(src);
|
||||
masterConfig.rcControlsConfig.alt_hold_deadband = sbufReadU8(src);
|
||||
masterConfig.flight3DConfig.deadband3d_throttle = sbufReadU16(src);
|
||||
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_RESET_CURR_PID:
|
||||
|
@ -1427,36 +1427,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_ALIGNMENT:
|
||||
masterConfig.sensorAlignmentConfig.gyro_align = sbufReadU8(src);
|
||||
masterConfig.sensorAlignmentConfig.acc_align = sbufReadU8(src);
|
||||
masterConfig.sensorAlignmentConfig.mag_align = sbufReadU8(src);
|
||||
sensorAlignmentConfig()->gyro_align = sbufReadU8(src);
|
||||
sensorAlignmentConfig()->acc_align = sbufReadU8(src);
|
||||
sensorAlignmentConfig()->mag_align = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG:
|
||||
masterConfig.gyroConfig.gyro_sync_denom = sbufReadU8(src);
|
||||
gyroConfig()->gyro_sync_denom = sbufReadU8(src);
|
||||
masterConfig.pid_process_denom = sbufReadU8(src);
|
||||
masterConfig.motorConfig.useUnsyncedPwm = sbufReadU8(src);
|
||||
motorConfig()->useUnsyncedPwm = sbufReadU8(src);
|
||||
#ifdef USE_DSHOT
|
||||
masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
|
||||
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
|
||||
#else
|
||||
masterConfig.motorConfig.motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
|
||||
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
|
||||
#endif
|
||||
masterConfig.motorConfig.motorPwmRate = sbufReadU16(src);
|
||||
motorConfig()->motorPwmRate = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_FILTER_CONFIG:
|
||||
masterConfig.gyroConfig.gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
currentProfile->pidProfile.dterm_lpf_hz = sbufReadU16(src);
|
||||
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
|
||||
if (dataSize > 5) {
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_1 = sbufReadU16(src);
|
||||
masterConfig.gyroConfig.gyro_soft_notch_cutoff_1 = sbufReadU16(src);
|
||||
gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
|
||||
gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
|
||||
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
|
||||
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
|
||||
}
|
||||
if (dataSize > 13) {
|
||||
masterConfig.gyroConfig.gyro_soft_notch_hz_2 = sbufReadU16(src);
|
||||
masterConfig.gyroConfig.gyro_soft_notch_cutoff_2 = sbufReadU16(src);
|
||||
gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
|
||||
}
|
||||
// reinitialize the gyro filters with the new values
|
||||
validateAndFixGyroConfig();
|
||||
|
@ -1481,9 +1481,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_CONFIG:
|
||||
masterConfig.sensorSelectionConfig.acc_hardware = sbufReadU8(src);
|
||||
masterConfig.sensorSelectionConfig.baro_hardware = sbufReadU8(src);
|
||||
masterConfig.sensorSelectionConfig.mag_hardware = sbufReadU8(src);
|
||||
sensorSelectionConfig()->acc_hardware = sbufReadU8(src);
|
||||
sensorSelectionConfig()->baro_hardware = sbufReadU8(src);
|
||||
sensorSelectionConfig()->mag_hardware = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_RESET_CONF:
|
||||
|
@ -1641,9 +1641,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_BOARD_ALIGNMENT:
|
||||
masterConfig.boardAlignment.rollDegrees = sbufReadU16(src);
|
||||
masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src);
|
||||
masterConfig.boardAlignment.yawDegrees = sbufReadU16(src);
|
||||
boardAlignment()->rollDegrees = sbufReadU16(src);
|
||||
boardAlignment()->pitchDegrees = sbufReadU16(src);
|
||||
boardAlignment()->yawDegrees = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_VOLTAGE_METER_CONFIG:
|
||||
|
@ -1729,9 +1729,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
|
||||
masterConfig.rxConfig.serialrx_provider = sbufReadU8(src); // serialrx_type
|
||||
|
||||
masterConfig.boardAlignment.rollDegrees = sbufReadU16(src); // board_align_roll
|
||||
masterConfig.boardAlignment.pitchDegrees = sbufReadU16(src); // board_align_pitch
|
||||
masterConfig.boardAlignment.yawDegrees = sbufReadU16(src); // board_align_yaw
|
||||
boardAlignment()->rollDegrees = sbufReadU16(src); // board_align_roll
|
||||
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
|
||||
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
|
||||
|
||||
masterConfig.batteryConfig.currentMeterScale = sbufReadU16(src);
|
||||
masterConfig.batteryConfig.currentMeterOffset = sbufReadU16(src);
|
||||
|
|
|
@ -124,7 +124,7 @@ static void taskUpdateBattery(uint32_t currentTime)
|
|||
|
||||
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
|
||||
ibatLastServiced = currentTime;
|
||||
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -157,7 +157,7 @@ static void taskUpdateRxMain(uint32_t currentTime)
|
|||
static void taskUpdateCompass(uint32_t currentTime)
|
||||
{
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
compassUpdate(currentTime, &masterConfig.sensorTrims.magZero);
|
||||
compassUpdate(currentTime, &sensorTrims()->magZero);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -197,7 +197,7 @@ static void taskTelemetry(uint32_t currentTime)
|
|||
telemetryCheckState();
|
||||
|
||||
if (!cliMode && feature(FEATURE_TELEMETRY)) {
|
||||
telemetryProcess(currentTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
telemetryProcess(currentTime, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -515,7 +515,7 @@ void processRx(uint32_t currentTime)
|
|||
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 (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
|
||||
|
@ -733,7 +733,7 @@ void subTaskMainSubprocesses(void)
|
|||
if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
#ifdef USE_SERVOS
|
||||
&& !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo)
|
||||
&& !((masterConfig.mixerConfig.mixerMode == MIXER_TRI || masterConfig.mixerConfig.mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
|
||||
#endif
|
||||
&& masterConfig.mixerConfig.mixerMode != MIXER_AIRPLANE
|
||||
&& masterConfig.mixerConfig.mixerMode != MIXER_FLYING_WING
|
||||
|
@ -801,7 +801,7 @@ void subTaskMotorUpdate(void)
|
|||
|
||||
uint8_t setPidUpdateCountDown(void)
|
||||
{
|
||||
if (masterConfig.gyroConfig.gyro_soft_lpf_hz) {
|
||||
if (gyroConfig()->gyro_soft_lpf_hz) {
|
||||
return masterConfig.pid_process_denom - 1;
|
||||
} else {
|
||||
return 1;
|
||||
|
|
|
@ -699,27 +699,27 @@ const clivalue_t valueTable[] = {
|
|||
{ "max_aux_channels", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.max_aux_channel, .config.minmax = { 0, 13 } },
|
||||
{ "debug_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.debug_mode, .config.lookup = { TABLE_DEBUG } },
|
||||
|
||||
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "min_command", VAR_UINT16 | MASTER_VALUE, &motorConfig()->mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
#ifdef USE_DSHOT
|
||||
{ "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, &masterConfig.motorConfig.digitalIdleOffsetPercent, .config.minmax = { 0, 20} },
|
||||
{ "digital_idle_percent", VAR_FLOAT | MASTER_VALUE, &motorConfig()->digitalIdleOffsetPercent, .config.minmax = { 0, 20} },
|
||||
#endif
|
||||
{ "max_esc_throttle_jump", VAR_UINT16 | MASTER_VALUE, &masterConfig.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_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_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.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_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, &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, &flight3DConfig()->neutral3d, .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, &masterConfig.motorConfig.useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
|
||||
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } },
|
||||
{ "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_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 200, 32000 } },
|
||||
|
||||
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "gyro_cal_on_first_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.armingConfig.gyro_cal_on_first_arm, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.armingConfig.auto_disarm_delay, .config.minmax = { 0, 60 } },
|
||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.small_angle, .config.minmax = { 0, 180 } },
|
||||
{ "small_angle", VAR_UINT8 | MASTER_VALUE, &imuConfig()->small_angle, .config.minmax = { 0, 180 } },
|
||||
|
||||
{ "fixedwing_althold_dir", VAR_INT8 | MASTER_VALUE, &masterConfig.airplaneConfig.fixedwing_althold_dir, .config.minmax = { -1, 1 } },
|
||||
|
||||
|
@ -802,26 +802,26 @@ const clivalue_t valueTable[] = {
|
|||
{ "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.batteryConfig.useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.consumptionWarningPercentage, .config.minmax = { 0, 100 } },
|
||||
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.acc_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorAlignmentConfig.mag_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
|
||||
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.rollDegrees, .config.minmax = { -180, 360 } },
|
||||
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.pitchDegrees, .config.minmax = { -180, 360 } },
|
||||
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &masterConfig.boardAlignment.yawDegrees, .config.minmax = { -180, 360 } },
|
||||
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDegrees, .config.minmax = { -180, 360 } },
|
||||
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } },
|
||||
{ "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } },
|
||||
|
||||
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyroConfig.gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
|
||||
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
|
||||
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyroConfig.gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
|
||||
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
|
||||
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_kp, .config.minmax = { 0, 50000 } },
|
||||
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.imuConfig.dcm_ki, .config.minmax = { 0, 50000 } },
|
||||
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
|
||||
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } },
|
||||
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
|
||||
{ "gyro_notch1_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_1, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_1, .config.minmax = { 1, 1000 } },
|
||||
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_hz_2, .config.minmax = { 0, 1000 } },
|
||||
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, &gyroConfig()->gyro_soft_notch_cutoff_2, .config.minmax = { 1, 1000 } },
|
||||
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
|
||||
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_kp, .config.minmax = { 0, 50000 } },
|
||||
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &imuConfig()->dcm_ki, .config.minmax = { 0, 50000 } },
|
||||
|
||||
{ "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.rcControlsConfig.alt_hold_deadband, .config.minmax = { 1, 250 } },
|
||||
{ "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rcControlsConfig.alt_hold_fast_change, .config.lookup = { TABLE_OFF_ON } },
|
||||
|
@ -837,12 +837,12 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
||||
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
|
||||
#ifdef USE_SERVOS
|
||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 } },
|
||||
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} },
|
||||
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } },
|
||||
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
||||
#endif
|
||||
|
||||
{ "rc_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcRate8, .config.minmax = { 0, 255 } },
|
||||
|
@ -868,25 +868,25 @@ const clivalue_t valueTable[] = {
|
|||
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
|
||||
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
|
||||
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
|
||||
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.accelerometerConfig.acc_lpf_hz, .config.minmax = { 0, 400 } },
|
||||
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.xy, .config.minmax = { 0, 100 } },
|
||||
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &masterConfig.imuConfig.accDeadband.z, .config.minmax = { 0, 100 } },
|
||||
{ "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.imuConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
|
||||
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &accelerometerConfig()->acc_lpf_hz, .config.minmax = { 0, 400 } },
|
||||
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.xy, .config.minmax = { 0, 100 } },
|
||||
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.z, .config.minmax = { 0, 100 } },
|
||||
{ "acc_unarmedcal", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &imuConfig()->acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.pitch, .config.minmax = { -300, 300 } },
|
||||
{ "acc_trim_roll", VAR_INT16 | MASTER_VALUE, &masterConfig.accelerometerTrims.values.roll, .config.minmax = { -300, 300 } },
|
||||
|
||||
#ifdef BARO
|
||||
{ "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &masterConfig.barometerConfig.baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } },
|
||||
{ "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_noise_lpf, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_vel, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &masterConfig.barometerConfig.baro_cf_alt, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
|
||||
{ "baro_tab_size", VAR_UINT8 | MASTER_VALUE, &barometerConfig()->baro_sample_count, .config.minmax = { 0, BARO_SAMPLE_COUNT_MAX } },
|
||||
{ "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_noise_lpf, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_vel, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_alt, .config.minmax = { 0 , 1 } },
|
||||
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
|
||||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.sensorSelectionConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.compassConfig.mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
#endif
|
||||
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
|
||||
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
|
||||
|
@ -944,9 +944,9 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[X], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Y], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &masterConfig.sensorTrims.magZero.raw[Z], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[X], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Y], .config.minmax = { -32768, 32767 } },
|
||||
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Z], .config.minmax = { -32768, 32767 } },
|
||||
#endif
|
||||
#ifdef LED_STRIP
|
||||
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.ledStripConfig.ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },
|
||||
|
@ -3790,9 +3790,9 @@ const cliResourceValue_t resourceTable[] = {
|
|||
#ifdef BEEPER
|
||||
{ OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 },
|
||||
#endif
|
||||
{ OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS },
|
||||
{ OWNER_MOTOR, &motorConfig()->ioTags[0], MAX_SUPPORTED_MOTORS },
|
||||
#ifdef USE_SERVOS
|
||||
{ OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS },
|
||||
{ OWNER_SERVO, &servoConfig()->ioTags[0], MAX_SUPPORTED_SERVOS },
|
||||
#endif
|
||||
#if defined(USE_PWM) || defined(USE_PPM)
|
||||
{ OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 },
|
||||
|
|
|
@ -254,12 +254,12 @@ void init(void)
|
|||
servoMixerInit(masterConfig.customServoMixer);
|
||||
#endif
|
||||
|
||||
uint16_t idlePulse = masterConfig.motorConfig.mincommand;
|
||||
uint16_t idlePulse = motorConfig()->mincommand;
|
||||
if (feature(FEATURE_3D)) {
|
||||
idlePulse = masterConfig.flight3DConfig.neutral3d;
|
||||
idlePulse = flight3DConfig()->neutral3d;
|
||||
}
|
||||
|
||||
if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
featureClear(FEATURE_3D);
|
||||
idlePulse = 0; // brushed motors
|
||||
}
|
||||
|
@ -284,7 +284,7 @@ void init(void)
|
|||
|
||||
#if defined(USE_PWM) || defined(USE_PPM)
|
||||
if (feature(FEATURE_RX_PPM)) {
|
||||
ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol);
|
||||
ppmRxInit(&masterConfig.ppmConfig, motorConfig()->motorPwmProtocol);
|
||||
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
pwmRxInit(&masterConfig.pwmConfig);
|
||||
}
|
||||
|
@ -424,7 +424,7 @@ void init(void)
|
|||
#endif
|
||||
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
|
||||
&masterConfig.sensorSelectionConfig,
|
||||
masterConfig.compassConfig.mag_declination,
|
||||
compassConfig()->mag_declination,
|
||||
&masterConfig.gyroConfig,
|
||||
sonarConfig)) {
|
||||
// if gyro was not detected due to whatever reason, we give up now.
|
||||
|
@ -465,7 +465,7 @@ void init(void)
|
|||
cliInit(&masterConfig.serialConfig);
|
||||
#endif
|
||||
|
||||
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
failsafeInit(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
|
||||
|
||||
rxInit(&masterConfig.rxConfig, masterConfig.modeActivationConditions);
|
||||
|
||||
|
|
|
@ -750,9 +750,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
case BST_MISC:
|
||||
bstWrite16(masterConfig.rxConfig.midrc);
|
||||
|
||||
bstWrite16(masterConfig.motorConfig.minthrottle);
|
||||
bstWrite16(masterConfig.motorConfig.maxthrottle);
|
||||
bstWrite16(masterConfig.motorConfig.mincommand);
|
||||
bstWrite16(motorConfig()->minthrottle);
|
||||
bstWrite16(motorConfig()->maxthrottle);
|
||||
bstWrite16(motorConfig()->mincommand);
|
||||
|
||||
bstWrite16(masterConfig.failsafeConfig.failsafe_throttle);
|
||||
|
||||
|
@ -769,7 +769,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(masterConfig.rxConfig.rssi_channel);
|
||||
bstWrite8(0);
|
||||
|
||||
bstWrite16(masterConfig.compassConfig.mag_declination / 10);
|
||||
bstWrite16(compassConfig()->mag_declination / 10);
|
||||
|
||||
bstWrite8(masterConfig.batteryConfig.vbatscale);
|
||||
bstWrite8(masterConfig.batteryConfig.vbatmincellvoltage);
|
||||
|
@ -848,9 +848,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
break;
|
||||
|
||||
case BST_BOARD_ALIGNMENT:
|
||||
bstWrite16(masterConfig.boardAlignment.rollDegrees);
|
||||
bstWrite16(masterConfig.boardAlignment.pitchDegrees);
|
||||
bstWrite16(masterConfig.boardAlignment.yawDegrees);
|
||||
bstWrite16(boardAlignment()->rollDegrees);
|
||||
bstWrite16(boardAlignment()->pitchDegrees);
|
||||
bstWrite16(boardAlignment()->yawDegrees);
|
||||
break;
|
||||
|
||||
case BST_VOLTAGE_METER_CONFIG:
|
||||
|
@ -910,9 +910,9 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
|
||||
bstWrite8(masterConfig.rxConfig.serialrx_provider);
|
||||
|
||||
bstWrite16(masterConfig.boardAlignment.rollDegrees);
|
||||
bstWrite16(masterConfig.boardAlignment.pitchDegrees);
|
||||
bstWrite16(masterConfig.boardAlignment.yawDegrees);
|
||||
bstWrite16(boardAlignment()->rollDegrees);
|
||||
bstWrite16(boardAlignment()->pitchDegrees);
|
||||
bstWrite16(boardAlignment()->yawDegrees);
|
||||
|
||||
bstWrite16(masterConfig.batteryConfig.currentMeterScale);
|
||||
bstWrite16(masterConfig.batteryConfig.currentMeterOffset);
|
||||
|
@ -977,7 +977,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
bstWrite8(masterConfig.rcControlsConfig.yaw_deadband);
|
||||
break;
|
||||
case BST_FC_FILTERS:
|
||||
bstWrite16(constrain(masterConfig.gyroConfig.gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
|
||||
bstWrite16(constrain(gyroConfig()->gyro_lpf, 0, 1)); // Extra safety to prevent OSD setting corrupt values
|
||||
break;
|
||||
default:
|
||||
// we do not know how to handle the (valid) message, indicate error BST
|
||||
|
@ -1113,9 +1113,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
if (tmp < 1600 && tmp > 1400)
|
||||
masterConfig.rxConfig.midrc = tmp;
|
||||
|
||||
masterConfig.motorConfig.minthrottle = bstRead16();
|
||||
masterConfig.motorConfig.maxthrottle = bstRead16();
|
||||
masterConfig.motorConfig.mincommand = bstRead16();
|
||||
motorConfig()->minthrottle = bstRead16();
|
||||
motorConfig()->maxthrottle = bstRead16();
|
||||
motorConfig()->mincommand = bstRead16();
|
||||
|
||||
masterConfig.failsafeConfig.failsafe_throttle = bstRead16();
|
||||
|
||||
|
@ -1132,7 +1132,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
masterConfig.rxConfig.rssi_channel = bstRead8();
|
||||
bstRead8();
|
||||
|
||||
masterConfig.compassConfig.mag_declination = bstRead16() * 10;
|
||||
compassConfig()->mag_declination = bstRead16() * 10;
|
||||
|
||||
masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended
|
||||
masterConfig.batteryConfig.vbatmincellvoltage = bstRead8(); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
|
@ -1254,9 +1254,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
featureSet(bstRead32()); // features bitmap
|
||||
break;
|
||||
case BST_SET_BOARD_ALIGNMENT:
|
||||
masterConfig.boardAlignment.rollDegrees = bstRead16();
|
||||
masterConfig.boardAlignment.pitchDegrees = bstRead16();
|
||||
masterConfig.boardAlignment.yawDegrees = bstRead16();
|
||||
boardAlignment()->rollDegrees = bstRead16();
|
||||
boardAlignment()->pitchDegrees = bstRead16();
|
||||
boardAlignment()->yawDegrees = bstRead16();
|
||||
break;
|
||||
case BST_SET_VOLTAGE_METER_CONFIG:
|
||||
masterConfig.batteryConfig.vbatscale = bstRead8(); // actual vbatscale as intended
|
||||
|
@ -1327,9 +1327,9 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
|
||||
masterConfig.rxConfig.serialrx_provider = bstRead8(); // serialrx_type
|
||||
|
||||
masterConfig.boardAlignment.rollDegrees = bstRead16(); // board_align_roll
|
||||
masterConfig.boardAlignment.pitchDegrees = bstRead16(); // board_align_pitch
|
||||
masterConfig.boardAlignment.yawDegrees = bstRead16(); // board_align_yaw
|
||||
boardAlignment()->rollDegrees = bstRead16(); // board_align_roll
|
||||
boardAlignment()->pitchDegrees = bstRead16(); // board_align_pitch
|
||||
boardAlignment()->yawDegrees = bstRead16(); // board_align_yaw
|
||||
|
||||
masterConfig.batteryConfig.currentMeterScale = bstRead16();
|
||||
masterConfig.batteryConfig.currentMeterOffset = bstRead16();
|
||||
|
@ -1404,7 +1404,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
masterConfig.rcControlsConfig.yaw_deadband = bstRead8();
|
||||
break;
|
||||
case BST_SET_FC_FILTERS:
|
||||
masterConfig.gyroConfig.gyro_lpf = bstRead16();
|
||||
gyroConfig()->gyro_lpf = bstRead16();
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
Loading…
Reference in New Issue