mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
Made PG config() functions const
This commit is contained in:
parent
9f2c90872e
commit
b1fb59c464
20 changed files with 227 additions and 210 deletions
|
@ -956,20 +956,18 @@ static int gcd(int num, int denom)
|
|||
|
||||
static void validateBlackboxConfig()
|
||||
{
|
||||
int div;
|
||||
|
||||
if (blackboxConfig()->rate_num == 0 || blackboxConfig()->rate_denom == 0
|
||||
|| blackboxConfig()->rate_num >= blackboxConfig()->rate_denom) {
|
||||
blackboxConfig()->rate_num = 1;
|
||||
blackboxConfig()->rate_denom = 1;
|
||||
blackboxConfigMutable()->rate_num = 1;
|
||||
blackboxConfigMutable()->rate_denom = 1;
|
||||
} else {
|
||||
/* Reduce the fraction the user entered as much as possible (makes the recorded/skipped frame pattern repeat
|
||||
* itself more frequently)
|
||||
*/
|
||||
div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
||||
const int div = gcd(blackboxConfig()->rate_num, blackboxConfig()->rate_denom);
|
||||
|
||||
blackboxConfig()->rate_num /= div;
|
||||
blackboxConfig()->rate_denom /= div;
|
||||
blackboxConfigMutable()->rate_num /= div;
|
||||
blackboxConfigMutable()->rate_denom /= div;
|
||||
}
|
||||
|
||||
// If we've chosen an unsupported device, change the device to serial
|
||||
|
@ -985,7 +983,7 @@ static void validateBlackboxConfig()
|
|||
break;
|
||||
|
||||
default:
|
||||
blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
|
||||
blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -441,7 +441,7 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
|
|||
UNUSED(self);
|
||||
|
||||
masterConfig.profile[profileIndex].pidProfile.dterm_lpf_hz = cmsx_dterm_lpf_hz;
|
||||
gyroConfig()->gyro_soft_lpf_hz = cmsx_gyroSoftLpf;
|
||||
gyroConfigMutable()->gyro_soft_lpf_hz = cmsx_gyroSoftLpf;
|
||||
masterConfig.profile[profileIndex].pidProfile.yaw_p_limit = cmsx_yaw_p_limit;
|
||||
masterConfig.profile[profileIndex].pidProfile.yaw_lpf_hz = cmsx_yaw_lpf_hz;
|
||||
|
||||
|
@ -487,9 +487,9 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self)
|
|||
{
|
||||
UNUSED(self);
|
||||
|
||||
gyroConfig()->gyroSync = cmsx_gyroSync;
|
||||
gyroConfig()->gyroSyncDenominator = cmsx_gyroSyncDenom;
|
||||
gyroConfig()->gyro_lpf = cmsx_gyroLpf;
|
||||
gyroConfigMutable()->gyroSync = cmsx_gyroSync;
|
||||
gyroConfigMutable()->gyroSyncDenominator = cmsx_gyroSyncDenom;
|
||||
gyroConfigMutable()->gyro_lpf = cmsx_gyroLpf;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -53,21 +53,37 @@
|
|||
#include "telemetry/telemetry.h"
|
||||
|
||||
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
|
||||
#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig)
|
||||
#define servoConfig(x) (&masterConfig.servoConfig)
|
||||
#define servoConfigMutable(x) (&masterConfig.servoConfig)
|
||||
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
|
||||
#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig)
|
||||
#define imuConfig(x) (&masterConfig.imuConfig)
|
||||
#define imuConfigMutable(x) (&masterConfig.imuConfig)
|
||||
#define batteryConfig(x) (&masterConfig.batteryConfig)
|
||||
#define batteryConfigMutable(x) (&masterConfig.batteryConfig)
|
||||
#define rcControlsConfig(x) (&masterConfig.rcControlsConfig)
|
||||
#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig)
|
||||
#define gpsConfig(x) (&masterConfig.gpsConfig)
|
||||
#define gpsConfigMutable(x) (&masterConfig.gpsConfig)
|
||||
#define navConfig(x) (&masterConfig.navConfig)
|
||||
#define navConfigMutable(x) (&masterConfig.navConfig)
|
||||
#define armingConfig(x) (&masterConfig.armingConfig)
|
||||
#define armingConfigMutable(x) (&masterConfig.armingConfig)
|
||||
#define mixerConfig(x) (&masterConfig.mixerConfig)
|
||||
#define mixerConfigMutable(x) (&masterConfig.mixerConfig)
|
||||
#define serialConfig(x) (&masterConfig.serialConfig)
|
||||
#define serialConfigMutable(x) (&masterConfig.serialConfig)
|
||||
#define telemetryConfig(x) (&masterConfig.telemetryConfig)
|
||||
#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig)
|
||||
#define osdProfile(x) (&masterConfig.osdProfile)
|
||||
#define osdProfileMutable(x) (&masterConfig.osdProfile)
|
||||
#define ledStripConfig(x) (&masterConfig.ledStripConfig)
|
||||
#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig)
|
||||
#define pwmRxConfig(x) (&masterConfig.pwmRxConfig)
|
||||
#define pwmRxConfigMutable(x) (&masterConfig.pwmRxConfig)
|
||||
#define customServoMixer(i) (&masterConfig.customServoMixer[i])
|
||||
#define customServoMixerMutable(i) (&masterConfig.customServoMixer[i])
|
||||
|
||||
|
||||
// System-wide
|
||||
|
|
|
@ -102,14 +102,16 @@ extern const uint8_t __pg_resetdata_end[];
|
|||
// Declare system config
|
||||
#define PG_DECLARE(_type, _name) \
|
||||
extern _type _name ## _System; \
|
||||
static inline _type* _name(void) { return &_name ## _System; } \
|
||||
static inline const _type* _name(void) { return &_name ## _System; }\
|
||||
static inline _type* _name ## Mutable(void) { return &_name ## _System; }\
|
||||
struct _dummy \
|
||||
/**/
|
||||
|
||||
// Declare system config array
|
||||
#define PG_DECLARE_ARR(_type, _size, _name) \
|
||||
extern _type _name ## _SystemArray[_size]; \
|
||||
static inline _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
|
||||
static inline const _type* _name(int _index) { return &_name ## _SystemArray[_index]; } \
|
||||
static inline _type* _name ## Mutable(int _index) { return &_name ## _SystemArray[_index]; } \
|
||||
static inline _type (* _name ## _arr(void))[_size] { return &_name ## _SystemArray; } \
|
||||
struct _dummy \
|
||||
/**/
|
||||
|
@ -117,7 +119,8 @@ extern const uint8_t __pg_resetdata_end[];
|
|||
// Declare profile config
|
||||
#define PG_DECLARE_PROFILE(_type, _name) \
|
||||
extern _type *_name ## _ProfileCurrent; \
|
||||
static inline _type* _name(void) { return _name ## _ProfileCurrent; } \
|
||||
static inline const _type* _name(void) { return _name ## _ProfileCurrent; } \
|
||||
static inline _type* _name ## Mutable(void) { return _name ## _ProfileCurrent; } \
|
||||
struct _dummy \
|
||||
/**/
|
||||
|
||||
|
|
|
@ -671,12 +671,12 @@ void validateAndFixConfig(void)
|
|||
{
|
||||
#ifdef USE_GYRO_NOTCH_1
|
||||
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
|
||||
gyroConfig()->gyro_soft_notch_hz_1 = 0;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_GYRO_NOTCH_2
|
||||
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
|
||||
gyroConfig()->gyro_soft_notch_hz_2 = 0;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
|
@ -726,8 +726,8 @@ void validateAndFixConfig(void)
|
|||
// There is a timer clash between PWM RX pins and motor output pins - this forces us to have same timer tick rate for these timers
|
||||
// which is only possible when using brushless motors w/o oneshot (timer tick rate is PWM_TIMER_MHZ)
|
||||
// On CC3D OneShot is incompatible with PWM RX
|
||||
motorConfig()->motorPwmProtocol = PWM_TYPE_STANDARD;
|
||||
motorConfig()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
|
||||
motorConfigMutable()->motorPwmProtocol = PWM_TYPE_STANDARD;
|
||||
motorConfigMutable()->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
@ -760,7 +760,7 @@ void validateAndFixConfig(void)
|
|||
* When async processing mode is enabled, gyroSync has to be forced to "ON"
|
||||
*/
|
||||
if (getAsyncMode() != ASYNC_MODE_NONE) {
|
||||
gyroConfig()->gyroSync = 1;
|
||||
gyroConfigMutable()->gyroSync = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -777,11 +777,11 @@ void validateAndFixConfig(void)
|
|||
}
|
||||
|
||||
if (gyroConfig()->gyroSyncDenominator < denominatorLimit) {
|
||||
gyroConfig()->gyroSyncDenominator = denominatorLimit;
|
||||
gyroConfigMutable()->gyroSyncDenominator = denominatorLimit;
|
||||
}
|
||||
|
||||
if (gyroConfig()->looptime < 2000) {
|
||||
gyroConfig()->looptime = 2000;
|
||||
gyroConfigMutable()->looptime = 2000;
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -855,7 +855,7 @@ void validateAndFixConfig(void)
|
|||
* If provided predefined mixer setup is disabled, fallback to default one
|
||||
*/
|
||||
if (!isMixerEnabled(mixerConfig()->mixerMode)) {
|
||||
mixerConfig()->mixerMode = DEFAULT_MIXER;
|
||||
mixerConfigMutable()->mixerMode = DEFAULT_MIXER;
|
||||
}
|
||||
|
||||
#if defined(NAV)
|
||||
|
@ -865,26 +865,26 @@ void validateAndFixConfig(void)
|
|||
|
||||
/* Limitations of different protocols */
|
||||
#ifdef BRUSHED_MOTORS
|
||||
motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000);
|
||||
motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000);
|
||||
#else
|
||||
switch (motorConfig()->motorPwmProtocol) {
|
||||
case PWM_TYPE_STANDARD: // Limited to 490 Hz
|
||||
motorConfig()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490);
|
||||
motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 490);
|
||||
break;
|
||||
|
||||
case PWM_TYPE_ONESHOT125: // Limited to 3900 Hz
|
||||
motorConfig()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 3900);
|
||||
motorConfigMutable()->motorPwmRate = MIN(motorConfig()->motorPwmRate, 3900);
|
||||
break;
|
||||
|
||||
case PWM_TYPE_ONESHOT42: // 2-8 kHz
|
||||
motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 8000);
|
||||
motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 8000);
|
||||
break;
|
||||
|
||||
case PWM_TYPE_MULTISHOT: // 2-16 kHz
|
||||
motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 16000);
|
||||
motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 2000, 16000);
|
||||
break;
|
||||
case PWM_TYPE_BRUSHED: // 500Hz - 32kHz
|
||||
motorConfig()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000);
|
||||
motorConfigMutable()->motorPwmRate = constrain(motorConfig()->motorPwmRate, 500, 32000);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -214,7 +214,7 @@ void init(void)
|
|||
// Spektrum satellite binding if enabled on startup.
|
||||
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
|
||||
// The rest of Spektrum initialization will happen later - via spektrumInit()
|
||||
spektrumBind(rxConfig());
|
||||
spektrumBind(rxConfigMutable());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1283,12 +1283,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_ARMING_CONFIG:
|
||||
armingConfig()->auto_disarm_delay = sbufReadU8(src);
|
||||
armingConfig()->disarm_kill_switch = sbufReadU8(src);
|
||||
armingConfigMutable()->auto_disarm_delay = sbufReadU8(src);
|
||||
armingConfigMutable()->disarm_kill_switch = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_LOOP_TIME:
|
||||
gyroConfig()->looptime = sbufReadU16(src);
|
||||
gyroConfigMutable()->looptime = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_PID_CONTROLLER:
|
||||
|
@ -1379,37 +1379,37 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
case MSP_SET_MISC:
|
||||
tmp = sbufReadU16(src);
|
||||
if (tmp < 1600 && tmp > 1400)
|
||||
rxConfig()->midrc = tmp;
|
||||
rxConfigMutable()->midrc = tmp;
|
||||
|
||||
motorConfig()->minthrottle = sbufReadU16(src);
|
||||
motorConfig()->maxthrottle = sbufReadU16(src);
|
||||
motorConfig()->mincommand = sbufReadU16(src);
|
||||
motorConfigMutable()->minthrottle = sbufReadU16(src);
|
||||
motorConfigMutable()->maxthrottle = sbufReadU16(src);
|
||||
motorConfigMutable()->mincommand = sbufReadU16(src);
|
||||
|
||||
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
|
||||
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
|
||||
|
||||
#ifdef GPS
|
||||
gpsConfig()->provider = sbufReadU8(src); // gps_type
|
||||
gpsConfigMutable()->provider = sbufReadU8(src); // gps_type
|
||||
sbufReadU8(src); // gps_baudrate
|
||||
gpsConfig()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
|
||||
gpsConfigMutable()->sbasMode = sbufReadU8(src); // gps_ubx_sbas
|
||||
#else
|
||||
sbufReadU8(src); // gps_type
|
||||
sbufReadU8(src); // gps_baudrate
|
||||
sbufReadU8(src); // gps_ubx_sbas
|
||||
#endif
|
||||
batteryConfig()->multiwiiCurrentMeterOutput = sbufReadU8(src);
|
||||
rxConfig()->rssi_channel = sbufReadU8(src);
|
||||
batteryConfigMutable()->multiwiiCurrentMeterOutput = sbufReadU8(src);
|
||||
rxConfigMutable()->rssi_channel = sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
|
||||
#ifdef MAG
|
||||
compassConfig()->mag_declination = sbufReadU16(src) * 10;
|
||||
compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
|
||||
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
break;
|
||||
|
||||
case MSP_SET_MOTOR:
|
||||
|
@ -1461,16 +1461,16 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_3D:
|
||||
flight3DConfig()->deadband3d_low = sbufReadU16(src);
|
||||
flight3DConfig()->deadband3d_high = sbufReadU16(src);
|
||||
flight3DConfig()->neutral3d = sbufReadU16(src);
|
||||
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
|
||||
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
|
||||
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
|
||||
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
|
||||
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_RC_DEADBAND:
|
||||
rcControlsConfig()->deadband = sbufReadU8(src);
|
||||
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
|
||||
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
|
||||
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_RESET_CURR_PID:
|
||||
|
@ -1478,36 +1478,36 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_ALIGNMENT:
|
||||
gyroConfig()->gyro_align = sbufReadU8(src);
|
||||
accelerometerConfig()->acc_align = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyro_align = sbufReadU8(src);
|
||||
accelerometerConfigMutable()->acc_align = sbufReadU8(src);
|
||||
#ifdef MAG
|
||||
compassConfig()->mag_align = sbufReadU8(src);
|
||||
compassConfigMutable()->mag_align = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG:
|
||||
gyroConfig()->gyroSyncDenominator = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyroSyncDenominator = sbufReadU8(src);
|
||||
sbufReadU8(src); // BF: masterConfig.pid_process_denom
|
||||
sbufReadU8(src); // BF: motorConfig()->useUnsyncedPwm
|
||||
motorConfig()->motorPwmProtocol = sbufReadU8(src);
|
||||
motorConfig()->motorPwmRate = sbufReadU16(src);
|
||||
motorConfigMutable()->motorPwmProtocol = sbufReadU8(src);
|
||||
motorConfigMutable()->motorPwmRate = sbufReadU16(src);
|
||||
#ifdef USE_SERVOS
|
||||
servoConfig()->servoPwmRate = sbufReadU16(src);
|
||||
servoConfigMutable()->servoPwmRate = sbufReadU16(src);
|
||||
#else
|
||||
sbufReadU16(src);
|
||||
#endif
|
||||
gyroConfig()->gyroSync = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyroSync = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_FILTER_CONFIG :
|
||||
gyroConfig()->gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
|
||||
currentProfile->pidProfile.dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
|
||||
currentProfile->pidProfile.yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
|
||||
#ifdef USE_GYRO_NOTCH_1
|
||||
gyroConfig()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
|
||||
gyroConfig()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
|
||||
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
|
||||
#endif
|
||||
#ifdef USE_DTERM_NOTCH
|
||||
currentProfile->pidProfile.dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
|
||||
|
@ -1515,8 +1515,8 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
pidInitFilters(¤tProfile->pidProfile);
|
||||
#endif
|
||||
#ifdef USE_GYRO_NOTCH_2
|
||||
gyroConfig()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
|
||||
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
|
||||
#endif
|
||||
//BF: masterConfig.gyro_soft_notch_hz_1 = read16();
|
||||
//BF: masterConfig.gyro_soft_notch_cutoff_1 = read16();
|
||||
|
@ -1558,14 +1558,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
sbufReadU16(src);
|
||||
#endif
|
||||
#ifdef MAG
|
||||
compassConfig()->mag_hold_rate_limit = sbufReadU8(src);
|
||||
compassConfigMutable()->mag_hold_rate_limit = sbufReadU8(src);
|
||||
sbufReadU8(src); //MAG_HOLD_ERROR_LPF_FREQ
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
mixerConfig()->yaw_jump_prevention_limit = sbufReadU16(src);
|
||||
gyroConfig()->gyro_lpf = sbufReadU8(src);
|
||||
mixerConfigMutable()->yaw_jump_prevention_limit = sbufReadU16(src);
|
||||
gyroConfigMutable()->gyro_lpf = sbufReadU8(src);
|
||||
currentProfile->pidProfile.acc_soft_lpf_hz = sbufReadU8(src);
|
||||
sbufReadU8(src); //reserved
|
||||
sbufReadU8(src); //reserved
|
||||
|
@ -1574,19 +1574,19 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_CONFIG:
|
||||
accelerometerConfig()->acc_hardware = sbufReadU8(src);
|
||||
accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
|
||||
#ifdef BARO
|
||||
barometerConfig()->baro_hardware = sbufReadU8(src);
|
||||
barometerConfigMutable()->baro_hardware = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
#ifdef MAG
|
||||
compassConfig()->mag_hardware = sbufReadU8(src);
|
||||
compassConfigMutable()->mag_hardware = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
#ifdef PITOT
|
||||
pitotmeterConfig()->pitot_hardware = sbufReadU8(src);
|
||||
pitotmeterConfigMutable()->pitot_hardware = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src);
|
||||
#endif
|
||||
|
@ -1596,14 +1596,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
|
||||
#ifdef NAV
|
||||
case MSP_SET_NAV_POSHOLD:
|
||||
navConfig()->general.flags.user_control_mode = sbufReadU8(src);
|
||||
navConfig()->general.max_speed = sbufReadU16(src);
|
||||
navConfig()->general.max_climb_rate = sbufReadU16(src);
|
||||
navConfig()->general.max_manual_speed = sbufReadU16(src);
|
||||
navConfig()->general.max_manual_climb_rate = sbufReadU16(src);
|
||||
navConfig()->mc.max_bank_angle = sbufReadU8(src);
|
||||
navConfig()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
|
||||
navConfig()->mc.hover_throttle = sbufReadU16(src);
|
||||
navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
|
||||
navConfigMutable()->general.max_speed = sbufReadU16(src);
|
||||
navConfigMutable()->general.max_climb_rate = sbufReadU16(src);
|
||||
navConfigMutable()->general.max_manual_speed = sbufReadU16(src);
|
||||
navConfigMutable()->general.max_manual_climb_rate = sbufReadU16(src);
|
||||
navConfigMutable()->mc.max_bank_angle = sbufReadU8(src);
|
||||
navConfigMutable()->general.flags.use_thr_mid_for_althold = sbufReadU8(src);
|
||||
navConfigMutable()->mc.hover_throttle = sbufReadU16(src);
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
@ -1636,9 +1636,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
case MSP_SET_BLACKBOX_CONFIG:
|
||||
// Don't allow config to be updated while Blackbox is logging
|
||||
if (blackboxMayEditConfig()) {
|
||||
blackboxConfig()->device = sbufReadU8(src);
|
||||
blackboxConfig()->rate_num = sbufReadU8(src);
|
||||
blackboxConfig()->rate_denom = sbufReadU8(src);
|
||||
blackboxConfigMutable()->device = sbufReadU8(src);
|
||||
blackboxConfigMutable()->rate_num = sbufReadU8(src);
|
||||
blackboxConfigMutable()->rate_denom = sbufReadU8(src);
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
@ -1743,40 +1743,40 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_BOARD_ALIGNMENT:
|
||||
boardAlignment()->rollDeciDegrees = sbufReadU16(src);
|
||||
boardAlignment()->pitchDeciDegrees = sbufReadU16(src);
|
||||
boardAlignment()->yawDeciDegrees = sbufReadU16(src);
|
||||
boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src);
|
||||
boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src);
|
||||
boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_VOLTAGE_METER_CONFIG:
|
||||
batteryConfig()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfig()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfig()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfig()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
batteryConfigMutable()->vbatscale = sbufReadU8(src); // actual vbatscale as intended
|
||||
batteryConfigMutable()->vbatmincellvoltage = sbufReadU8(src); // vbatlevel_warn1 in MWC2.3 GUI
|
||||
batteryConfigMutable()->vbatmaxcellvoltage = sbufReadU8(src); // vbatlevel_warn2 in MWC2.3 GUI
|
||||
batteryConfigMutable()->vbatwarningcellvoltage = sbufReadU8(src); // vbatlevel when buzzer starts to alert
|
||||
break;
|
||||
|
||||
case MSP_SET_CURRENT_METER_CONFIG:
|
||||
batteryConfig()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterOffset = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterType = sbufReadU8(src);
|
||||
batteryConfig()->batteryCapacity = sbufReadU16(src);
|
||||
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
|
||||
batteryConfigMutable()->currentMeterType = sbufReadU8(src);
|
||||
batteryConfigMutable()->batteryCapacity = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
case MSP_SET_MIXER:
|
||||
mixerConfig()->mixerMode = sbufReadU8(src);
|
||||
mixerConfigMutable()->mixerMode = sbufReadU8(src);
|
||||
break;
|
||||
#endif
|
||||
|
||||
case MSP_SET_RX_CONFIG:
|
||||
rxConfig()->serialrx_provider = sbufReadU8(src);
|
||||
rxConfig()->maxcheck = sbufReadU16(src);
|
||||
rxConfig()->midrc = sbufReadU16(src);
|
||||
rxConfig()->mincheck = sbufReadU16(src);
|
||||
rxConfig()->spektrum_sat_bind = sbufReadU8(src);
|
||||
rxConfigMutable()->serialrx_provider = sbufReadU8(src);
|
||||
rxConfigMutable()->maxcheck = sbufReadU16(src);
|
||||
rxConfigMutable()->midrc = sbufReadU16(src);
|
||||
rxConfigMutable()->mincheck = sbufReadU16(src);
|
||||
rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
|
||||
if (dataSize > 8) {
|
||||
rxConfig()->rx_min_usec = sbufReadU16(src);
|
||||
rxConfig()->rx_max_usec = sbufReadU16(src);
|
||||
rxConfigMutable()->rx_min_usec = sbufReadU16(src);
|
||||
rxConfigMutable()->rx_max_usec = sbufReadU16(src);
|
||||
}
|
||||
if (dataSize > 12) {
|
||||
// for compatibility with betaflight
|
||||
|
@ -1785,45 +1785,45 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
sbufReadU16(src);
|
||||
}
|
||||
if (dataSize > 16) {
|
||||
rxConfig()->rx_spi_protocol = sbufReadU8(src);
|
||||
rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
|
||||
}
|
||||
if (dataSize > 17) {
|
||||
rxConfig()->rx_spi_id = sbufReadU32(src);
|
||||
rxConfigMutable()->rx_spi_id = sbufReadU32(src);
|
||||
}
|
||||
if (dataSize > 21) {
|
||||
rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
|
||||
rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_SET_FAILSAFE_CONFIG:
|
||||
failsafeConfig()->failsafe_delay = sbufReadU8(src);
|
||||
failsafeConfig()->failsafe_off_delay = sbufReadU8(src);
|
||||
failsafeConfig()->failsafe_throttle = sbufReadU16(src);
|
||||
failsafeConfig()->failsafe_kill_switch = sbufReadU8(src);
|
||||
failsafeConfig()->failsafe_throttle_low_delay = sbufReadU16(src);
|
||||
failsafeConfig()->failsafe_procedure = sbufReadU8(src);
|
||||
failsafeConfigMutable()->failsafe_delay = sbufReadU8(src);
|
||||
failsafeConfigMutable()->failsafe_off_delay = sbufReadU8(src);
|
||||
failsafeConfigMutable()->failsafe_throttle = sbufReadU16(src);
|
||||
failsafeConfigMutable()->failsafe_kill_switch = sbufReadU8(src);
|
||||
failsafeConfigMutable()->failsafe_throttle_low_delay = sbufReadU16(src);
|
||||
failsafeConfigMutable()->failsafe_procedure = sbufReadU8(src);
|
||||
if (dataSize > 8) {
|
||||
failsafeConfig()->failsafe_recovery_delay = sbufReadU8(src);
|
||||
failsafeConfigMutable()->failsafe_recovery_delay = sbufReadU8(src);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_SET_RXFAIL_CONFIG:
|
||||
i = sbufReadU8(src);
|
||||
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
|
||||
rxFailsafeChannelConfigs(i)->mode = sbufReadU8(src);
|
||||
rxFailsafeChannelConfigs(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
|
||||
rxFailsafeChannelConfigsMutable(i)->mode = sbufReadU8(src);
|
||||
rxFailsafeChannelConfigsMutable(i)->step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
|
||||
} else {
|
||||
return MSP_RESULT_ERROR;
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_SET_RSSI_CONFIG:
|
||||
rxConfig()->rssi_channel = sbufReadU8(src);
|
||||
rxConfigMutable()->rssi_channel = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_RX_MAP:
|
||||
for (int i = 0; i < MAX_MAPPABLE_RX_INPUTS; i++) {
|
||||
rxConfig()->rcmap[i] = sbufReadU8(src);
|
||||
rxConfigMutable()->rcmap[i] = sbufReadU8(src);
|
||||
}
|
||||
break;
|
||||
|
||||
|
@ -1831,20 +1831,20 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
#ifdef USE_QUAD_MIXER_ONLY
|
||||
sbufReadU8(src); // mixerMode ignored
|
||||
#else
|
||||
mixerConfig()->mixerMode = sbufReadU8(src); // mixerMode
|
||||
mixerConfigMutable()->mixerMode = sbufReadU8(src); // mixerMode
|
||||
#endif
|
||||
|
||||
featureClearAll();
|
||||
featureSet(sbufReadU32(src)); // features bitmap
|
||||
|
||||
rxConfig()->serialrx_provider = sbufReadU8(src); // serialrx_type
|
||||
rxConfigMutable()->serialrx_provider = sbufReadU8(src); // serialrx_type
|
||||
|
||||
boardAlignment()->rollDeciDegrees = sbufReadU16(src); // board_align_roll
|
||||
boardAlignment()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch
|
||||
boardAlignment()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw
|
||||
boardAlignmentMutable()->rollDeciDegrees = sbufReadU16(src); // board_align_roll
|
||||
boardAlignmentMutable()->pitchDeciDegrees = sbufReadU16(src); // board_align_pitch
|
||||
boardAlignmentMutable()->yawDeciDegrees = sbufReadU16(src); // board_align_yaw
|
||||
|
||||
batteryConfig()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfig()->currentMeterOffset = sbufReadU16(src);
|
||||
batteryConfigMutable()->currentMeterScale = sbufReadU16(src);
|
||||
batteryConfigMutable()->currentMeterOffset = sbufReadU16(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_CF_SERIAL_CONFIG:
|
||||
|
|
|
@ -1048,7 +1048,7 @@ static bool valuePtrEqualsDefault(uint8_t type, const void *ptr, const void *ptr
|
|||
}
|
||||
|
||||
#ifdef USE_PARAMETER_GROUPS
|
||||
static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, void *currentConfig, void *defaultConfig)
|
||||
static void dumpPgValues(uint16_t valueSection, uint8_t dumpMask, pgn_t pgn, const void *currentConfig, const void *defaultConfig)
|
||||
{
|
||||
const char *format = "set %s = ";
|
||||
for (uint32_t i = 0; i < ARRAYLEN(valueTable); i++) {
|
||||
|
@ -1129,31 +1129,31 @@ static void backupConfigs(void)
|
|||
|
||||
static void restoreConfigs(void)
|
||||
{
|
||||
*gyroConfig() = gyroConfigCopy;
|
||||
*accelerometerConfig() = accelerometerConfigCopy;
|
||||
*gyroConfigMutable() = gyroConfigCopy;
|
||||
*accelerometerConfigMutable() = accelerometerConfigCopy;
|
||||
#ifdef MAG
|
||||
*compassConfig() = compassConfigCopy;
|
||||
*compassConfigMutable() = compassConfigCopy;
|
||||
#endif
|
||||
#ifdef BARO
|
||||
*barometerConfig() = barometerConfigCopy;
|
||||
*barometerConfigMutable() = barometerConfigCopy;
|
||||
#endif
|
||||
#ifdef PITOT
|
||||
*pitotmeterConfig() = pitotmeterConfigCopy;
|
||||
*pitotmeterConfigMutable() = pitotmeterConfigCopy;
|
||||
#endif
|
||||
*rxConfig() = rxConfigCopy;
|
||||
*rxConfigMutable() = rxConfigCopy;
|
||||
for (int ii = 0; ii < MAX_SUPPORTED_RC_CHANNEL_COUNT; ++ii) {
|
||||
*rxFailsafeChannelConfigs(ii) = rxFailsafeChannelConfigsCopy[ii];
|
||||
*rxFailsafeChannelConfigsMutable(ii) = rxFailsafeChannelConfigsCopy[ii];
|
||||
}
|
||||
for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) {
|
||||
*rxChannelRangeConfigs(ii) = rxChannelRangeConfigsCopy[ii];
|
||||
*rxChannelRangeConfigsMutable(ii) = rxChannelRangeConfigsCopy[ii];
|
||||
}
|
||||
*motorConfig() = motorConfigCopy;
|
||||
*boardAlignment() = boardAlignmentCopy;
|
||||
*motorConfigMutable() = motorConfigCopy;
|
||||
*boardAlignmentMutable() = boardAlignmentCopy;
|
||||
#ifdef USE_SERVOS
|
||||
*gimbalConfig() = gimbalConfigCopy;
|
||||
*gimbalConfigMutable() = gimbalConfigCopy;
|
||||
#endif
|
||||
for (int ii = 0; ii < MAX_SUPPORTED_MOTORS; ++ii) {
|
||||
*customMotorMixer(ii) = customMotorMixerCopy[ii];
|
||||
*customMotorMixerMutable(ii) = customMotorMixerCopy[ii];
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1381,7 +1381,8 @@ static void cliRxFail(char *cmdline)
|
|||
channel = atoi(ptr++);
|
||||
if ((channel < MAX_SUPPORTED_RC_CHANNEL_COUNT)) {
|
||||
|
||||
rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigs(channel);
|
||||
// const cast
|
||||
rxFailsafeChannelConfig_t *channelFailsafeConfig = rxFailsafeChannelConfigsMutable(channel);
|
||||
|
||||
uint16_t value;
|
||||
rxFailsafeChannelType_e type = (channel < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_TYPE_FLIGHT : RX_FAILSAFE_TYPE_AUX;
|
||||
|
@ -1849,8 +1850,9 @@ static void cliMotorMix(char *cmdline)
|
|||
printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL);
|
||||
} else if (strncasecmp(cmdline, "reset", 5) == 0) {
|
||||
// erase custom mixer
|
||||
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
customMotorMixer(i)->throttle = 0.0f;
|
||||
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
customMotorMixerMutable(i)->throttle = 0.0f;
|
||||
}
|
||||
} else if (strncasecmp(cmdline, "load", 4) == 0) {
|
||||
ptr = nextArg(cmdline);
|
||||
if (ptr) {
|
||||
|
@ -1861,7 +1863,7 @@ static void cliMotorMix(char *cmdline)
|
|||
break;
|
||||
}
|
||||
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
|
||||
mixerLoadMix(i, customMotorMixer(0));
|
||||
mixerLoadMix(i, customMotorMixerMutable(0));
|
||||
cliPrintf("Loaded %s\r\n", mixerNames[i]);
|
||||
cliMotorMix("");
|
||||
break;
|
||||
|
@ -1874,22 +1876,22 @@ static void cliMotorMix(char *cmdline)
|
|||
if (i < MAX_SUPPORTED_MOTORS) {
|
||||
ptr = nextArg(ptr);
|
||||
if (ptr) {
|
||||
customMotorMixer(i)->throttle = fastA2F(ptr);
|
||||
customMotorMixerMutable(i)->throttle = fastA2F(ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = nextArg(ptr);
|
||||
if (ptr) {
|
||||
customMotorMixer(i)->roll = fastA2F(ptr);
|
||||
customMotorMixerMutable(i)->roll = fastA2F(ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = nextArg(ptr);
|
||||
if (ptr) {
|
||||
customMotorMixer(i)->pitch = fastA2F(ptr);
|
||||
customMotorMixerMutable(i)->pitch = fastA2F(ptr);
|
||||
check++;
|
||||
}
|
||||
ptr = nextArg(ptr);
|
||||
if (ptr) {
|
||||
customMotorMixer(i)->yaw = fastA2F(ptr);
|
||||
customMotorMixerMutable(i)->yaw = fastA2F(ptr);
|
||||
check++;
|
||||
}
|
||||
if (check != 4) {
|
||||
|
@ -1958,7 +1960,7 @@ static void cliRxRange(char *cmdline)
|
|||
} else if (rangeMin < PWM_PULSE_MIN || rangeMin > PWM_PULSE_MAX || rangeMax < PWM_PULSE_MIN || rangeMax > PWM_PULSE_MAX) {
|
||||
cliShowParseError();
|
||||
} else {
|
||||
rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigs(i);
|
||||
rxChannelRangeConfig_t *channelRangeConfig = rxChannelRangeConfigsMutable(i);
|
||||
channelRangeConfig->min = rangeMin;
|
||||
channelRangeConfig->max = rangeMax;
|
||||
}
|
||||
|
|
|
@ -340,7 +340,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
|
|||
// we're 1-based
|
||||
index++;
|
||||
// clear existing
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++)
|
||||
customMixers[i].throttle = 0.0f;
|
||||
|
||||
// do we have anything here to begin with?
|
||||
|
|
|
@ -2473,32 +2473,32 @@ void updateWaypointsAndNavigationMode(void)
|
|||
/*-----------------------------------------------------------
|
||||
* NAV main control functions
|
||||
*-----------------------------------------------------------*/
|
||||
void navigationUseConfig(navConfig_t *navConfigToUse)
|
||||
void navigationUseConfig(const navConfig_t *navConfigToUse)
|
||||
{
|
||||
posControl.navConfig = navConfigToUse;
|
||||
}
|
||||
|
||||
void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig)
|
||||
void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig)
|
||||
{
|
||||
posControl.rcControlsConfig = initialRcControlsConfig;
|
||||
}
|
||||
|
||||
void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig)
|
||||
void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig)
|
||||
{
|
||||
posControl.flight3DConfig = initialFlight3DConfig;
|
||||
}
|
||||
|
||||
void navigationUseRxConfig(rxConfig_t * initialRxConfig)
|
||||
void navigationUseRxConfig(const rxConfig_t * initialRxConfig)
|
||||
{
|
||||
posControl.rxConfig = initialRxConfig;
|
||||
}
|
||||
|
||||
void navigationUsemotorConfig(motorConfig_t * initialmotorConfig)
|
||||
void navigationUsemotorConfig(const motorConfig_t * initialmotorConfig)
|
||||
{
|
||||
posControl.motorConfig = initialmotorConfig;
|
||||
}
|
||||
|
||||
void navigationUsePIDs(pidProfile_t *initialPidProfile)
|
||||
void navigationUsePIDs(const pidProfile_t *initialPidProfile)
|
||||
{
|
||||
posControl.pidProfile = initialPidProfile;
|
||||
|
||||
|
@ -2539,12 +2539,12 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile)
|
|||
(float)posControl.pidProfile->D8[PIDALT] / 100.0f);
|
||||
}
|
||||
|
||||
void navigationInit(navConfig_t *initialnavConfig,
|
||||
pidProfile_t *initialPidProfile,
|
||||
rcControlsConfig_t *initialRcControlsConfig,
|
||||
rxConfig_t * initialRxConfig,
|
||||
flight3DConfig_t * initialFlight3DConfig,
|
||||
motorConfig_t * initialmotorConfig)
|
||||
void navigationInit(const navConfig_t *initialnavConfig,
|
||||
const pidProfile_t *initialPidProfile,
|
||||
const rcControlsConfig_t *initialRcControlsConfig,
|
||||
const rxConfig_t * initialRxConfig,
|
||||
const flight3DConfig_t * initialFlight3DConfig,
|
||||
const motorConfig_t * initialmotorConfig)
|
||||
{
|
||||
/* Initial state */
|
||||
posControl.navState = NAV_STATE_IDLE;
|
||||
|
|
|
@ -233,18 +233,18 @@ typedef struct {
|
|||
navWaypointActions_e activeWpAction;
|
||||
} navSystemStatus_t;
|
||||
|
||||
void navigationUsePIDs(pidProfile_t *pidProfile);
|
||||
void navigationUseConfig(navConfig_t *navConfigToUse);
|
||||
void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig);
|
||||
void navigationUseRxConfig(rxConfig_t * initialRxConfig);
|
||||
void navigationUsemotorConfig(motorConfig_t * initialmotorConfig);
|
||||
void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig);
|
||||
void navigationInit(navConfig_t *initialnavConfig,
|
||||
pidProfile_t *initialPidProfile,
|
||||
rcControlsConfig_t *initialRcControlsConfig,
|
||||
rxConfig_t * initialRxConfig,
|
||||
flight3DConfig_t * initialFlight3DConfig,
|
||||
motorConfig_t * initialmotorConfig);
|
||||
void navigationUsePIDs(const pidProfile_t *pidProfile);
|
||||
void navigationUseConfig(const navConfig_t *navConfigToUse);
|
||||
void navigationUseRcControlsConfig(const rcControlsConfig_t *initialRcControlsConfig);
|
||||
void navigationUseRxConfig(const rxConfig_t * initialRxConfig);
|
||||
void navigationUsemotorConfig(const motorConfig_t * initialmotorConfig);
|
||||
void navigationUseFlight3DConfig(const flight3DConfig_t * initialFlight3DConfig);
|
||||
void navigationInit(const navConfig_t *initialnavConfig,
|
||||
const pidProfile_t *initialPidProfile,
|
||||
const rcControlsConfig_t *initialRcControlsConfig,
|
||||
const rxConfig_t * initialRxConfig,
|
||||
const flight3DConfig_t * initialFlight3DConfig,
|
||||
const motorConfig_t * initialmotorConfig);
|
||||
|
||||
/* Navigation system updates */
|
||||
void updateWaypointsAndNavigationMode(void);
|
||||
|
|
|
@ -286,12 +286,12 @@ typedef struct {
|
|||
/* Internals */
|
||||
int16_t rcAdjustment[4];
|
||||
|
||||
navConfig_t * navConfig;
|
||||
rcControlsConfig_t * rcControlsConfig;
|
||||
pidProfile_t * pidProfile;
|
||||
rxConfig_t * rxConfig;
|
||||
flight3DConfig_t * flight3DConfig;
|
||||
motorConfig_t * motorConfig;
|
||||
const navConfig_t * navConfig;
|
||||
const rcControlsConfig_t * rcControlsConfig;
|
||||
const pidProfile_t * pidProfile;
|
||||
const rxConfig_t * rxConfig;
|
||||
const flight3DConfig_t * flight3DConfig;
|
||||
const motorConfig_t * motorConfig;
|
||||
} navigationPosControl_t;
|
||||
|
||||
extern navigationPosControl_t posControl;
|
||||
|
|
|
@ -122,8 +122,8 @@ void resetAllRxChannelRangeConfigurations(void)
|
|||
{
|
||||
// set default calibration to full range and 1:1 mapping
|
||||
for (int i = 0; i < NON_AUX_CHANNEL_COUNT; i++) {
|
||||
rxChannelRangeConfigs(i)->min = PWM_RANGE_MIN;
|
||||
rxChannelRangeConfigs(i)->max = PWM_RANGE_MAX;
|
||||
rxChannelRangeConfigsMutable(i)->min = PWM_RANGE_MIN;
|
||||
rxChannelRangeConfigsMutable(i)->max = PWM_RANGE_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -523,7 +523,7 @@ static uint16_t getRxfailValue(uint8_t channel)
|
|||
}
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, rxChannelRangeConfig_t *range)
|
||||
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
|
||||
{
|
||||
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
|
||||
if (sample == PPM_RCVR_TIMEOUT) {
|
||||
|
@ -639,12 +639,10 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
|
|||
|
||||
void parseRcChannels(const char *input)
|
||||
{
|
||||
const char *c, *s;
|
||||
|
||||
for (c = input; *c; c++) {
|
||||
s = strchr(rcChannelLetters, *c);
|
||||
for (const char *c = input; *c; c++) {
|
||||
const char *s = strchr(rcChannelLetters, *c);
|
||||
if (s && (s < rcChannelLetters + MAX_MAPPABLE_RX_INPUTS))
|
||||
rxConfig()->rcmap[s - rcChannelLetters] = c - input;
|
||||
rxConfigMutable()->rcmap[s - rcChannelLetters] = c - input;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -358,7 +358,7 @@ void performAcclerationCalibration(void)
|
|||
sensorCalibrationSolveForOffset(&calState, accTmp);
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
accelerometerConfig()->accZero.raw[axis] = lrintf(accTmp[axis]);
|
||||
accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]);
|
||||
}
|
||||
|
||||
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
|
||||
|
@ -375,7 +375,7 @@ void performAcclerationCalibration(void)
|
|||
sensorCalibrationSolveForScale(&calState, accTmp);
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
accelerometerConfig()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
|
||||
accelerometerConfigMutable()->accGain.raw[axis] = lrintf(accTmp[axis] * 4096);
|
||||
}
|
||||
|
||||
saveConfigAndNotify();
|
||||
|
|
|
@ -63,8 +63,8 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
|
|||
const float sinAlignYaw = sin_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees));
|
||||
const float cosAlignYaw = cos_approx(DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees));
|
||||
|
||||
boardAlignment()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll;
|
||||
boardAlignment()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll;
|
||||
boardAlignmentMutable()->rollDeciDegrees += -sinAlignYaw * pitch + cosAlignYaw * roll;
|
||||
boardAlignmentMutable()->pitchDeciDegrees += cosAlignYaw * pitch + sinAlignYaw * roll;
|
||||
|
||||
initBoardAlignment();
|
||||
}
|
||||
|
|
|
@ -294,7 +294,7 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
calStartedAt = currentTimeUs;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
compassConfig()->magZero.raw[axis] = 0;
|
||||
compassConfigMutable()->magZero.raw[axis] = 0;
|
||||
magPrev[axis] = 0;
|
||||
}
|
||||
|
||||
|
@ -333,7 +333,7 @@ void compassUpdate(timeUs_t currentTimeUs)
|
|||
sensorCalibrationSolveForOffset(&calState, magZerof);
|
||||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
compassConfig()->magZero.raw[axis] = lrintf(magZerof[axis]);
|
||||
compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]);
|
||||
}
|
||||
|
||||
calStartedAt = 0;
|
||||
|
|
|
@ -78,25 +78,25 @@ bool sensorsAutodetect(void)
|
|||
#endif
|
||||
|
||||
if (accelerometerConfig()->acc_hardware == ACC_AUTODETECT) {
|
||||
accelerometerConfig()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC];
|
||||
accelerometerConfigMutable()->acc_hardware = detectedSensors[SENSOR_INDEX_ACC];
|
||||
eepromUpdatePending = true;
|
||||
}
|
||||
|
||||
#ifdef BARO
|
||||
if (barometerConfig()->baro_hardware == BARO_AUTODETECT) {
|
||||
barometerConfig()->baro_hardware = detectedSensors[SENSOR_INDEX_BARO];
|
||||
barometerConfigMutable()->baro_hardware = detectedSensors[SENSOR_INDEX_BARO];
|
||||
eepromUpdatePending = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (compassConfig()->mag_hardware == MAG_AUTODETECT) {
|
||||
compassConfig()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
|
||||
compassConfigMutable()->mag_hardware = detectedSensors[SENSOR_INDEX_MAG];
|
||||
eepromUpdatePending = true;
|
||||
}
|
||||
|
||||
#ifdef PITOT
|
||||
if (pitotmeterConfig()->pitot_hardware == PITOT_AUTODETECT) {
|
||||
pitotmeterConfig()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
|
||||
pitotmeterConfigMutable()->pitot_hardware = detectedSensors[SENSOR_INDEX_PITOT];
|
||||
eepromUpdatePending = true;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -34,7 +34,7 @@ void targetConfiguration(master_t *config)
|
|||
{
|
||||
(void)config;
|
||||
if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) {
|
||||
gyroConfig()->gyro_align = CW180_DEG;
|
||||
accelerometerConfig()->acc_align = CW180_DEG;
|
||||
gyroConfigMutable()->gyro_align = CW180_DEG;
|
||||
accelerometerConfigMutable()->acc_align = CW180_DEG;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -72,10 +72,10 @@
|
|||
|
||||
#include "config/config_master.h"
|
||||
|
||||
void targetConfiguration(struct master_s *config)
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
config->mixerConfig.mixerMode = MIXER_HEX6X;
|
||||
rxConfig()->serialrx_provider = 2;
|
||||
rxConfigMutable()->serialrx_provider = 2;
|
||||
featureSet(FEATURE_RX_SERIAL);
|
||||
config->serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
}
|
||||
|
|
|
@ -72,18 +72,18 @@
|
|||
|
||||
#include "config/config_master.h"
|
||||
|
||||
void targetConfiguration(struct master_s *config)
|
||||
void targetConfiguration(master_t *config)
|
||||
{
|
||||
gyroConfig()->looptime = 1000;
|
||||
gyroConfigMutable()->looptime = 1000;
|
||||
|
||||
rxConfig()->rcmap[0] = 1;
|
||||
rxConfig()->rcmap[1] = 2;
|
||||
rxConfig()->rcmap[2] = 3;
|
||||
rxConfig()->rcmap[3] = 0;
|
||||
rxConfig()->rcmap[4] = 4;
|
||||
rxConfig()->rcmap[5] = 5;
|
||||
rxConfig()->rcmap[6] = 6;
|
||||
rxConfig()->rcmap[7] = 7;
|
||||
rxConfigMutable()->rcmap[0] = 1;
|
||||
rxConfigMutable()->rcmap[1] = 2;
|
||||
rxConfigMutable()->rcmap[2] = 3;
|
||||
rxConfigMutable()->rcmap[3] = 0;
|
||||
rxConfigMutable()->rcmap[4] = 4;
|
||||
rxConfigMutable()->rcmap[5] = 5;
|
||||
rxConfigMutable()->rcmap[6] = 6;
|
||||
rxConfigMutable()->rcmap[7] = 7;
|
||||
|
||||
featureSet(FEATURE_VBAT);
|
||||
featureSet(FEATURE_LED_STRIP);
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue