rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2204 from martinbudden/bf_pg_preparation 

Preparation for conversion to parameter groups
This commit is contained in:
Martin Budden 2017-02-01 09:18:36 +00:00 committed by GitHub
parent 2ce712658b 79d4b2146d
commit 9828b7c444
90 changed files with 1150 additions and 507 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/main/blackbox/blackbox.c
View File

@ -24,6 +24,9 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/debug.h"
#include "build/version.h"
@ -31,8 +34,10 @@
#include "common/encoding.h"
#include "common/utils.h"
#include "blackbox.h"
#include "blackbox_io.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/compass.h"
@ -43,18 +48,22 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/failsafe.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/serial.h"
#include "sensors/sensors.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sonar.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096
@ -761,16 +770,16 @@ void validateBlackboxConfig()
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);
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
@ -786,7 +795,7 @@ void validateBlackboxConfig()
break;
default:
blackboxConfig()->device = BLACKBOX_DEVICE_SERIAL;
blackboxConfigMutable()->device = BLACKBOX_DEVICE_SERIAL;
}
}
@ -820,7 +829,7 @@ void startBlackbox(void)
*/
blackboxBuildConditionCache();
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationProfile()->modeActivationConditions, BOXBLACKBOX);
blackboxModeActivationConditionPresent = isModeActivationConditionPresent(modeActivationConditions(0), BOXBLACKBOX);
blackboxIteration = 0;
blackboxPFrameIndex = 0;

4
src/main/blackbox/blackbox.h
View File

@ -21,6 +21,8 @@
#include "common/time.h"
#include "config/parameter_group.h"
typedef struct blackboxConfig_s {
uint8_t rate_num;
uint8_t rate_denom;
@ -28,6 +30,8 @@ typedef struct blackboxConfig_s {
uint8_t on_motor_test;
} blackboxConfig_t;
PG_DECLARE(blackboxConfig_t, blackboxConfig);
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
void initBlackbox(void);

9
src/main/blackbox/blackbox_io.c
View File

@ -8,6 +8,7 @@
#ifdef BLACKBOX
#include "blackbox.h"
#include "blackbox_io.h"
#include "build/version.h"
@ -16,6 +17,10 @@
#include "common/encoding.h"
#include "common/printf.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -23,12 +28,10 @@
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/serial.h"
#include "msp/msp_serial.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#define BLACKBOX_SERIAL_PORT_MODE MODE_TX
// How many bytes can we transmit per loop iteration when writing headers?

15
src/main/cms/cms.c
View File

@ -44,19 +44,24 @@
#include "drivers/system.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
// For 'ARM' related
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
// For rcData, stopAllMotors, stopPwmAllMotors
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "flight/mixer.h"
// For VISIBLE* (Actually, included by config_master.h)
// For VISIBLE*
#include "io/osd.h"
#include "rx/rx.h"
// DisplayPort management
#ifndef CMS_MAX_DEVICE

7
src/main/cms/cms_menu_blackbox.c
View File

@ -27,10 +27,10 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "build/version.h"
#include "drivers/system.h"
#include "cms/cms.h"
@ -40,8 +40,9 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"

3
src/main/cms/cms_menu_imu.c
View File

@ -44,8 +44,9 @@
#include "flight/pid.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
//
// PID

7
src/main/cms/cms_menu_ledstrip.c
View File

@ -22,15 +22,16 @@
#include "platform.h"
#include "build/version.h"
#ifdef CMS
#include "build/version.h"
#include "drivers/system.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"

9
src/main/cms/cms_menu_misc.c
View File

@ -28,14 +28,15 @@
#include "drivers/system.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "cms/cms_menu_ledstrip.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
//
// Misc
//

7
src/main/cms/cms_menu_osd.c
View File

@ -21,6 +21,8 @@
#include "platform.h"
#if defined(OSD) && defined(CMS)
#include "build/version.h"
#include "cms/cms.h"
@ -28,10 +30,9 @@
#include "cms/cms_menu_osd.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#if defined(OSD) && defined(CMS)
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
OSD_UINT8_t entryAlarmRssi = {&osdProfile()->rssi_alarm, 5, 90, 5};
OSD_UINT16_t entryAlarmCapacity = {&osdProfile()->cap_alarm, 50, 30000, 50};

11
src/main/cms/cms_menu_vtx.c
View File

@ -21,6 +21,10 @@
#include "platform.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "build/version.h"
#include "cms/cms.h"
@ -30,12 +34,9 @@
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef CMS
#if defined(VTX) || defined(USE_RTC6705)
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static bool featureRead = false;
static uint8_t cmsx_featureVtx = 0, cmsx_vtxBand, cmsx_vtxChannel;

65
src/main/config/config_master.h
View File

@ -42,10 +42,10 @@
#include "flight/servos.h"
#include "flight/imu.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h"
#include "io/osd.h"
@ -64,6 +64,7 @@
#include "sensors/battery.h"
#include "sensors/compass.h"
#ifndef USE_PARAMETER_GROUPS
#define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig)
#define servoConfig(x) (&masterConfig.servoConfig)
@ -105,11 +106,71 @@
#define modeActivationProfile(x) (&masterConfig.modeActivationProfile)
#define servoProfile(x) (&masterConfig.servoProfile)
#define customMotorMixer(i) (&masterConfig.customMotorMixer[i])
#define customServoMixer(i) (&masterConfig.customServoMixer[i])
#define customServoMixers(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMsp(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOled(x) (&masterConfig.displayPortProfileOled)
#define motorConfigMutable(x) (&masterConfig.motorConfig)
#define flight3DConfigMutable(x) (&masterConfig.flight3DConfig)
#define servoConfigMutable(x) (&masterConfig.servoConfig)
#define servoMixerConfigMutable(x) (&masterConfig.servoMixerConfig)
#define gimbalConfigMutable(x) (&masterConfig.gimbalConfig)
#define boardAlignmentMutable(x) (&masterConfig.boardAlignment)
#define imuConfigMutable(x) (&masterConfig.imuConfig)
#define gyroConfigMutable(x) (&masterConfig.gyroConfig)
#define compassConfigMutable(x) (&masterConfig.compassConfig)
#define accelerometerConfigMutable(x) (&masterConfig.accelerometerConfig)
#define barometerConfigMutable(x) (&masterConfig.barometerConfig)
#define throttleCorrectionConfigMutable(x) (&masterConfig.throttleCorrectionConfig)
#define batteryConfigMutable(x) (&masterConfig.batteryConfig)
#define rcControlsConfigMutable(x) (&masterConfig.rcControlsConfig)
#define gpsProfileMutable(x) (&masterConfig.gpsProfile)
#define gpsConfigMutable(x) (&masterConfig.gpsConfig)
#define rxConfigMutable(x) (&masterConfig.rxConfig)
#define armingConfigMutable(x) (&masterConfig.armingConfig)
#define mixerConfigMutable(x) (&masterConfig.mixerConfig)
#define airplaneConfigMutable(x) (&masterConfig.airplaneConfig)
#define failsafeConfigMutable(x) (&masterConfig.failsafeConfig)
#define serialConfigMutable(x) (&masterConfig.serialConfig)
#define telemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ibusTelemetryConfigMutable(x) (&masterConfig.telemetryConfig)
#define ppmConfigMutable(x) (&masterConfig.ppmConfig)
#define pwmConfigMutable(x) (&masterConfig.pwmConfig)
#define adcConfigMutable(x) (&masterConfig.adcConfig)
#define beeperConfigMutable(x) (&masterConfig.beeperConfig)
#define sonarConfigMutable(x) (&masterConfig.sonarConfig)
#define ledStripConfigMutable(x) (&masterConfig.ledStripConfig)
#define statusLedConfigMutable(x) (&masterConfig.statusLedConfig)
#define osdProfileMutable(x) (&masterConfig.osdProfile)
#define vcdProfileMutable(x) (&masterConfig.vcdProfile)
#define sdcardConfigMutable(x) (&masterConfig.sdcardConfig)
#define blackboxConfigMutable(x) (&masterConfig.blackboxConfig)
#define flashConfigMutable(x) (&masterConfig.flashConfig)
#define pidConfigMutable(x) (&masterConfig.pidConfig)
#define adjustmentProfileMutable(x) (&masterConfig.adjustmentProfile)
#define modeActivationProfileMutable(x) (&masterConfig.modeActivationProfile)
#define servoProfileMutable(x) (&masterConfig.servoProfile)
#define customMotorMixerMutable(i) (&masterConfig.customMotorMixer[i])
#define customServoMixersMutable(i) (&masterConfig.customServoMixer[i])
#define displayPortProfileMspMutable(x) (&masterConfig.displayPortProfileMsp)
#define displayPortProfileMax7456Mutable(x) (&masterConfig.displayPortProfileMax7456)
#define displayPortProfileOledMutable(x) (&masterConfig.displayPortProfileOled)
#define servoParams(x) (&servoProfile()->servoConf[i])
#define adjustmentRanges(x) (&adjustmentProfile()->adjustmentRanges[x])
#define rxFailsafeChannelConfigs(x) (&masterConfig.rxConfig.failsafe_channel_configurations[x])
#define osdConfig(x) (&masterConfig.osdProfile)
#define modeActivationConditions(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#define servoParamsMutable(x) (&servoProfile()->servoConf[i])
#define adjustmentRangesMutable(x) (&masterConfig.adjustmentProfile.adjustmentRanges[i])
#define rxFailsafeChannelConfigsMutable(x) (&masterConfig.rxConfig.>failsafe_channel_configurations[x])
#define osdConfigMutable(x) (&masterConfig.osdProfile)
#define modeActivationConditionsMutable(x) (&masterConfig.modeActivationProfile.modeActivationConditions[x])
#endif
// System-wide
typedef struct master_s {
uint8_t version;

3
src/main/config/feature.c
View File

@ -21,8 +21,9 @@
#include "platform.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
static uint32_t activeFeaturesLatch = 0;

10
src/main/config/feature.h
View File

@ -17,6 +17,14 @@
#pragma once
#include "config/parameter_group.h"
typedef struct featureConfig_s {
uint32_t enabledFeatures;
} featureConfig_t;
PG_DECLARE(featureConfig_t, featureConfig);
void latchActiveFeatures(void);
bool featureConfigured(uint32_t mask);
bool feature(uint32_t mask);
@ -27,4 +35,4 @@ uint32_t featureMask(void);
void intFeatureClearAll(uint32_t *features);
void intFeatureSet(uint32_t mask, uint32_t *features);
void intFeatureClear(uint32_t mask, uint32_t *features);
void intFeatureClear(uint32_t mask, uint32_t *features);

3
src/main/config/parameter_group.c
View File

@ -19,6 +19,8 @@
#include <string.h>
#include <stdint.h>
#include "platform.h"
#include "parameter_group.h"
#include "common/maths.h"
@ -122,4 +124,3 @@ void pgActivateProfile(int profileIndex)
}
}
}

15
src/main/config/parameter_group.h
View File

@ -100,6 +100,7 @@ extern const uint8_t __pg_resetdata_end[];
} while(0) \
/**/
#ifdef USE_PARAMETER_GROUPS
// Declare system config
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System; \
@ -125,6 +126,20 @@ extern const uint8_t __pg_resetdata_end[];
struct _dummy \
/**/
#else
#define PG_DECLARE(_type, _name) \
extern _type _name ## _System
#define PG_DECLARE_ARRAY(_type, _size, _name) \
extern _type _name ## _SystemArray[_size]
// Declare profile config
#define PG_DECLARE_PROFILE(_type, _name) \
extern _type *_name ## _ProfileCurrent
#endif // USE_PARAMETER_GROUPS
// Register system config
#define PG_REGISTER_I(_type, _name, _pgn, _version, _reset) \
_type _name ## _System; \

4
src/main/config/parameter_group_ids.h
View File

@ -15,6 +15,10 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef USE_PARAMETER_GROUPS
#include "config/config_master.h"
#endif
// FC configuration
#define PG_FAILSAFE_CONFIG 1 // struct OK
#define PG_BOARD_ALIGNMENT 2 // struct OK

312
src/main/fc/cli.c
View File

@ -32,6 +32,8 @@ uint8_t cliMode = 0;
#ifdef USE_CLI
#include "blackbox/blackbox.h"
#include "build/build_config.h"
#include "build/debug.h"
#include "build/version.h"
@ -45,9 +47,10 @@ uint8_t cliMode = 0;
#include "common/typeconversion.h"
#include "config/config_eeprom.h"
#include "config/config_master.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
@ -81,6 +84,7 @@ uint8_t cliMode = 0;
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
@ -823,6 +827,71 @@ static const clivalue_t valueTable[] = {
};
#endif
#ifdef USE_PARAMETER_GROUPS
static featureConfig_t featureConfigCopy;
static gyroConfig_t gyroConfigCopy;
static accelerometerConfig_t accelerometerConfigCopy;
#ifdef MAG
static compassConfig_t compassConfigCopy;
#endif
#ifdef BARO
static barometerConfig_t barometerConfigCopy;
#endif
#ifdef PITOT
static pitotmeterConfig_t pitotmeterConfigCopy;
#endif
static featureConfig_t featureConfigCopy;
static rxConfig_t rxConfigCopy;
#ifdef BLACKBOX
static blackboxConfig_t blackboxConfigCopy;
#endif
static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT];
static motorConfig_t motorConfigCopy;
static failsafeConfig_t failsafeConfigCopy;
static boardAlignment_t boardAlignmentCopy;
#ifdef USE_SERVOS
static servoConfig_t servoConfigCopy;
static gimbalConfig_t gimbalConfigCopy;
static servoMixer_t customServoMixersCopy[MAX_SERVO_RULES];
static servoParam_t servoParamsCopy[MAX_SUPPORTED_SERVOS];
#endif
static batteryConfig_t batteryConfigCopy;
static motorMixer_t customMotorMixerCopy[MAX_SUPPORTED_MOTORS];
static mixerConfig_t mixerConfigCopy;
static flight3DConfig_t flight3DConfigCopy;
static serialConfig_t serialConfigCopy;
static imuConfig_t imuConfigCopy;
static armingConfig_t armingConfigCopy;
static rcControlsConfig_t rcControlsConfigCopy;
#ifdef GPS
static gpsConfig_t gpsConfigCopy;
#endif
#ifdef NAV
static positionEstimationConfig_t positionEstimationConfigCopy;
static navConfig_t navConfigCopy;
#endif
#ifdef TELEMETRY
static telemetryConfig_t telemetryConfigCopy;
#endif
static modeActivationCondition_t modeActivationConditionsCopy[MAX_MODE_ACTIVATION_CONDITION_COUNT];
static adjustmentRange_t adjustmentRangesCopy[MAX_ADJUSTMENT_RANGE_COUNT];
#ifdef LED_STRIP
static ledStripConfig_t ledStripConfigCopy;
#endif
#ifdef OSD
static osdConfig_t osdConfigCopy;
#endif
static systemConfig_t systemConfigCopy;
#ifdef BEEPER
static beeperConfig_t beeperConfigCopy;
#endif
static controlRateConfig_t controlRateProfilesCopy[MAX_CONTROL_RATE_PROFILE_COUNT];
static pidProfile_t pidProfileCopy[MAX_PROFILE_COUNT];
static modeActivationOperatorConfig_t modeActivationOperatorConfigCopy;
static beeperConfig_t beeperConfigCopy;
#endif // USE_PARAMETER_GROUPS
static void cliPrint(const char *str)
{
while (*str) {
@ -994,6 +1063,180 @@ static const cliCurrentAndDefaultConfig_t *getCurrentAndDefaultConfigs(pgn_t pgn
static cliCurrentAndDefaultConfig_t ret;
switch (pgn) {
case PG_GYRO_CONFIG:
ret.currentConfig = &gyroConfigCopy;
ret.defaultConfig = gyroConfig();
break;
case PG_ACCELEROMETER_CONFIG:
ret.currentConfig = &accelerometerConfigCopy;
ret.defaultConfig = accelerometerConfig();
break;
#ifdef MAG
case PG_COMPASS_CONFIG:
ret.currentConfig = &compassConfigCopy;
ret.defaultConfig = compassConfig();
break;
#endif
#ifdef BARO
case PG_BAROMETER_CONFIG:
ret.currentConfig = &barometerConfigCopy;
ret.defaultConfig = barometerConfig();
break;
#endif
#ifdef PITOT
case PG_PITOTMETER_CONFIG:
ret.currentConfig = &pitotmeterConfigCopy;
ret.defaultConfig = pitotmeterConfig();
break;
#endif
case PG_FEATURE_CONFIG:
ret.currentConfig = &featureConfigCopy;
ret.defaultConfig = featureConfig();
break;
case PG_RX_CONFIG:
ret.currentConfig = &rxConfigCopy;
ret.defaultConfig = rxConfig();
break;
#ifdef BLACKBOX
case PG_BLACKBOX_CONFIG:
ret.currentConfig = &blackboxConfigCopy;
ret.defaultConfig = blackboxConfig();
break;
#endif
case PG_MOTOR_CONFIG:
ret.currentConfig = &motorConfigCopy;
ret.defaultConfig = motorConfig();
break;
case PG_FAILSAFE_CONFIG:
ret.currentConfig = &failsafeConfigCopy;
ret.defaultConfig = failsafeConfig();
break;
case PG_BOARD_ALIGNMENT:
ret.currentConfig = &boardAlignmentCopy;
ret.defaultConfig = boardAlignment();
break;
case PG_MIXER_CONFIG:
ret.currentConfig = &mixerConfigCopy;
ret.defaultConfig = mixerConfig();
break;
case PG_MOTOR_3D_CONFIG:
ret.currentConfig = &flight3DConfigCopy;
ret.defaultConfig = flight3DConfig();
break;
#ifdef USE_SERVOS
case PG_SERVO_CONFIG:
ret.currentConfig = &servoConfigCopy;
ret.defaultConfig = servoConfig();
break;
case PG_GIMBAL_CONFIG:
ret.currentConfig = &gimbalConfigCopy;
ret.defaultConfig = gimbalConfig();
break;
#endif
case PG_BATTERY_CONFIG:
ret.currentConfig = &batteryConfigCopy;
ret.defaultConfig = batteryConfig();
break;
case PG_SERIAL_CONFIG:
ret.currentConfig = &serialConfigCopy;
ret.defaultConfig = serialConfig();
break;
case PG_IMU_CONFIG:
ret.currentConfig = &imuConfigCopy;
ret.defaultConfig = imuConfig();
break;
case PG_RC_CONTROLS_CONFIG:
ret.currentConfig = &rcControlsConfigCopy;
ret.defaultConfig = rcControlsConfig();
break;
case PG_ARMING_CONFIG:
ret.currentConfig = &armingConfigCopy;
ret.defaultConfig = armingConfig();
break;
#ifdef GPS
case PG_GPS_CONFIG:
ret.currentConfig = &gpsConfigCopy;
ret.defaultConfig = gpsConfig();
break;
#endif
#ifdef NAV
case PG_POSITION_ESTIMATION_CONFIG:
ret.currentConfig = &positionEstimationConfigCopy;
ret.defaultConfig = positionEstimationConfig();
break;
case PG_NAV_CONFIG:
ret.currentConfig = &navConfigCopy;
ret.defaultConfig = navConfig();
break;
#endif
#ifdef TELEMETRY
case PG_TELEMETRY_CONFIG:
ret.currentConfig = &telemetryConfigCopy;
ret.defaultConfig = telemetryConfig();
break;
#endif
#ifdef LED_STRIP
case PG_LED_STRIP_CONFIG:
ret.currentConfig = &ledStripConfigCopy;
ret.defaultConfig = ledStripConfig();
break;
#endif
#ifdef OSD
case PG_OSD_CONFIG:
ret.currentConfig = &osdConfigCopy;
ret.defaultConfig = osdConfig();
break;
#endif
case PG_SYSTEM_CONFIG:
ret.currentConfig = &systemConfigCopy;
ret.defaultConfig = systemConfig();
break;
case PG_MODE_ACTIVATION_OPERATOR_CONFIG:
ret.currentConfig = &modeActivationOperatorConfigCopy;
ret.defaultConfig = modeActivationOperatorConfig();
break;
case PG_CONTROL_RATE_PROFILES:
ret.currentConfig = &controlRateProfilesCopy[0];
ret.defaultConfig = controlRateProfiles(0);
break;
case PG_PID_PROFILE:
ret.currentConfig = &pidProfileCopy[getConfigProfile()];
ret.defaultConfig = pidProfile();
break;
case PG_RX_FAILSAFE_CHANNEL_CONFIG:
ret.currentConfig = &rxFailsafeChannelConfigsCopy[0];
ret.defaultConfig = rxFailsafeChannelConfigs(0);
break;
case PG_RX_CHANNEL_RANGE_CONFIG:
ret.currentConfig = &rxChannelRangeConfigsCopy[0];
ret.defaultConfig = rxChannelRangeConfigs(0);
break;
#ifdef USE_SERVOS
case PG_SERVO_MIXER:
ret.currentConfig = &customServoMixersCopy[0];
ret.defaultConfig = customServoMixers(0);
break;
case PG_SERVO_PARAMS:
ret.currentConfig = &servoParamsCopy[0];
ret.defaultConfig = servoParams(0);
break;
#endif
case PG_MOTOR_MIXER:
ret.currentConfig = &customMotorMixerCopy[0];
ret.defaultConfig = customMotorMixer(0);
break;
case PG_MODE_ACTIVATION_PROFILE:
ret.currentConfig = &modeActivationConditionsCopy[0];
ret.defaultConfig = modeActivationConditions(0);
break;
case PG_ADJUSTMENT_RANGE_CONFIG:
ret.currentConfig = &adjustmentRangesCopy[0];
ret.defaultConfig = adjustmentRanges(0);
break;
case PG_BEEPER_CONFIG:
ret.currentConfig = &beeperConfigCopy;
ret.defaultConfig = beeperConfig();
break;
default:
ret.currentConfig = NULL;
ret.defaultConfig = NULL;
@ -1810,7 +2053,7 @@ static void cliMotorMix(char *cmdline)
} 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;
customMotorMixerMutable(i)->throttle = 0.0f;
} else if (strncasecmp(cmdline, "load", 4) == 0) {
ptr = nextArg(cmdline);
if (ptr) {
@ -1821,7 +2064,7 @@ static void cliMotorMix(char *cmdline)
break;
}
if (strncasecmp(ptr, mixerNames[i], len) == 0) {
mixerLoadMix(i, masterConfig.customMotorMixer);
mixerLoadMix(i, customMotorMixerMutable(0));
cliPrintf("Loaded %s\r\n", mixerNames[i]);
cliMotorMix("");
break;
@ -1834,22 +2077,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) {
@ -2200,7 +2443,7 @@ static void printServoMix(uint8_t dumpMask, const master_t *defaultConfig)
{
const char *format = "smix %d %d %d %d %d %d %d %d\r\n";
for (uint32_t i = 0; i < MAX_SERVO_RULES; i++) {
servoMixer_t customServoMixer = *customServoMixer(i);
const servoMixer_t customServoMixer = *customServoMixers(i);
if (customServoMixer.rate == 0) {
break;
}
@ -2361,13 +2604,13 @@ static void cliServoMix(char *cmdline)
args[MIN] >= 0 && args[MIN] <= 100 &&
args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] &&
args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) {
customServoMixer(i)->targetChannel = args[TARGET];
customServoMixer(i)->inputSource = args[INPUT];
customServoMixer(i)->rate = args[RATE];
customServoMixer(i)->speed = args[SPEED];
customServoMixer(i)->min = args[MIN];
customServoMixer(i)->max = args[MAX];
customServoMixer(i)->box = args[BOX];
customServoMixersMutable(i)->targetChannel = args[TARGET];
customServoMixersMutable(i)->inputSource = args[INPUT];
customServoMixersMutable(i)->rate = args[RATE];
customServoMixersMutable(i)->speed = args[SPEED];
customServoMixersMutable(i)->min = args[MIN];
customServoMixersMutable(i)->max = args[MAX];
customServoMixersMutable(i)->box = args[BOX];
cliServoMix("");
} else {
cliShowParseError();
@ -3023,7 +3266,7 @@ static void cliMixer(char *cmdline)
return;
}
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
mixerConfig()->mixerMode = i + 1;
mixerConfigMutable()->mixerMode = i + 1;
break;
}
}
@ -3675,12 +3918,45 @@ static void cliResource(char *cmdline)
#ifdef USE_PARAMETER_GROUPS
static void backupConfigs(void)
{
// make copies of configs to do differencing
// make copies of configs to do differencing
PG_FOREACH(reg) {
// currentConfig is the copy
const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg));
if (cliCurrentAndDefaultConfig->currentConfig) {
if (pgIsProfile(reg)) {
//memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size * MAX_PROFILE_COUNT);
} else {
memcpy((uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->address, reg->size);
}
#ifdef SERIAL_CLI_DEBUG
} else {
cliPrintf("BACKUP %d SET UP INCORRECTLY\r\n", pgN(reg));
#endif
}
}
const pgRegistry_t* reg = pgFind(PG_PID_PROFILE);
memcpy(&pidProfileCopy[0], reg->address, sizeof(pidProfile_t) * MAX_PROFILE_COUNT);
}
static void restoreConfigs(void)
{
PG_FOREACH(reg) {
// currentConfig is the copy
const cliCurrentAndDefaultConfig_t *cliCurrentAndDefaultConfig = getCurrentAndDefaultConfigs(pgN(reg));
if (cliCurrentAndDefaultConfig->currentConfig) {
if (pgIsProfile(reg)) {
//memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size * MAX_PROFILE_COUNT);
} else {
memcpy(reg->address, (uint8_t *)cliCurrentAndDefaultConfig->currentConfig, reg->size);
}
#ifdef SERIAL_CLI_DEBUG
} else {
cliPrintf("RESTORE %d SET UP INCORRECTLY\r\n", pgN(reg));
#endif
}
}
const pgRegistry_t* reg = pgFind(PG_PID_PROFILE);
memcpy(reg->address, &pidProfileCopy[0], sizeof(pidProfile_t) * MAX_PROFILE_COUNT);
}
#endif

18
src/main/fc/config.c
View File

@ -951,7 +951,7 @@ void activateConfig(void)
void validateAndFixConfig(void)
{
if((motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) && (motorConfig()->mincommand < 1000)){
motorConfig()->mincommand = 1000;
motorConfigMutable()->mincommand = 1000;
}
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP) || featureConfigured(FEATURE_RX_SPI))) {
@ -1064,18 +1064,18 @@ void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
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;
}
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;
}
float samplingTime = 0.000125f;
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfig()->gyro_sync_denom = 1;
gyroConfig()->gyro_use_32khz = false;
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
samplingTime = 0.001f;
}
@ -1094,7 +1094,7 @@ void validateAndFixGyroConfig(void)
}
#if !defined(GYRO_USES_SPI) || !defined(USE_MPU_DATA_READY_SIGNAL)
gyroConfig()->gyro_isr_update = false;
gyroConfigMutable()->gyro_isr_update = false;
#endif
// check for looptime restrictions based on motor protocol. Motor times have safety margin
@ -1123,14 +1123,14 @@ void validateAndFixGyroConfig(void)
}
if(pidLooptime < motorUpdateRestriction)
pidConfig()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom);
pidConfigMutable()->pid_process_denom = motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom);
// Prevent overriding the max rate of motors
if(motorConfig()->useUnsyncedPwm && (motorConfig()->motorPwmProtocol <= PWM_TYPE_BRUSHED)) {
uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
if(motorConfig()->motorPwmRate > maxEscRate)
motorConfig()->motorPwmRate = maxEscRate;
motorConfigMutable()->motorPwmRate = maxEscRate;
}
}

6
src/main/fc/config.h
View File

@ -61,6 +61,12 @@ typedef enum {
FEATURE_ESC_SENSOR = 1 << 27,
} features_e;
typedef struct systemConfig_s {
uint8_t debug_mode;
} systemConfig_t;
//!!TODOPG_DECLARE(systemConfig_t, systemConfig);
void beeperOffSet(uint32_t mask);
void beeperOffSetAll(uint8_t beeperCount);
void beeperOffClear(uint32_t mask);

21
src/main/fc/fc_core.c
View File

@ -29,15 +29,21 @@
#include "common/utils.h"
#include "common/filter.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
@ -60,15 +66,14 @@
#include "scheduler/scheduler.h"
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/altitudehold.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
// June 2013 V2.2-dev
@ -96,8 +101,8 @@ static bool armingCalibrationWasInitialised;
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfig()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
saveConfigAndNotify();
}

10
src/main/fc/fc_init.c
View File

@ -28,6 +28,12 @@
#include "common/maths.h"
#include "common/printf.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
@ -115,10 +121,6 @@
#include "flight/failsafe.h"
#include "flight/navigation.h"
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"

274
src/main/fc/fc_msp.c
View File

@ -36,9 +36,11 @@
#include "config/config_eeprom.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/accgyro.h"
#include "drivers/bus_i2c.h"
#include "drivers/compass.h"
@ -673,25 +675,25 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SERVO_CONFIGURATIONS:
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
sbufWriteU16(dst, servoProfile()->servoConf[i].min);
sbufWriteU16(dst, servoProfile()->servoConf[i].max);
sbufWriteU16(dst, servoProfile()->servoConf[i].middle);
sbufWriteU8(dst, servoProfile()->servoConf[i].rate);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMin);
sbufWriteU8(dst, servoProfile()->servoConf[i].angleAtMax);
sbufWriteU8(dst, servoProfile()->servoConf[i].forwardFromChannel);
sbufWriteU32(dst, servoProfile()->servoConf[i].reversedSources);
sbufWriteU16(dst, servoParams(i)->min);
sbufWriteU16(dst, servoParams(i)->max);
sbufWriteU16(dst, servoParams(i)->middle);
sbufWriteU8(dst, servoParams(i)->rate);
sbufWriteU8(dst, servoParams(i)->angleAtMin);
sbufWriteU8(dst, servoParams(i)->angleAtMax);
sbufWriteU8(dst, servoParams(i)->forwardFromChannel);
sbufWriteU32(dst, servoParams(i)->reversedSources);
}
break;
case MSP_SERVO_MIX_RULES:
for (int i = 0; i < MAX_SERVO_RULES; i++) {
sbufWriteU8(dst, customServoMixer(i)->targetChannel);
sbufWriteU8(dst, customServoMixer(i)->inputSource);
sbufWriteU8(dst, customServoMixer(i)->rate);
sbufWriteU8(dst, customServoMixer(i)->speed);
sbufWriteU8(dst, customServoMixer(i)->min);
sbufWriteU8(dst, customServoMixer(i)->max);
sbufWriteU8(dst, customServoMixer(i)->box);
sbufWriteU8(dst, customServoMixers(i)->targetChannel);
sbufWriteU8(dst, customServoMixers(i)->inputSource);
sbufWriteU8(dst, customServoMixers(i)->rate);
sbufWriteU8(dst, customServoMixers(i)->speed);
sbufWriteU8(dst, customServoMixers(i)->min);
sbufWriteU8(dst, customServoMixers(i)->max);
sbufWriteU8(dst, customServoMixers(i)->box);
}
break;
#endif
@ -798,7 +800,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_ADJUSTMENT_RANGES:
for (int i = 0; i < MAX_ADJUSTMENT_RANGE_COUNT; i++) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
const adjustmentRange_t *adjRange = adjustmentRanges(i);
sbufWriteU8(dst, adjRange->adjustmentIndex);
sbufWriteU8(dst, adjRange->auxChannelIndex);
sbufWriteU8(dst, adjRange->range.startStep);
@ -965,8 +967,8 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RXFAIL_CONFIG:
for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
sbufWriteU8(dst, rxConfig()->failsafe_channel_configurations[i].mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig()->failsafe_channel_configurations[i].step));
sbufWriteU8(dst, rxFailsafeChannelConfigs(i)->mode);
sbufWriteU16(dst, RXFAIL_STEP_TO_CHANNEL_VALUE(rxFailsafeChannelConfigs(i)->step));
}
break;
@ -1012,7 +1014,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#ifdef LED_STRIP
case MSP_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
const hsvColor_t *color = &ledStripConfig()->colors[i];
sbufWriteU16(dst, color->h);
sbufWriteU8(dst, color->s);
sbufWriteU8(dst, color->v);
@ -1021,7 +1023,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_LED_STRIP_CONFIG:
for (int i = 0; i < LED_MAX_STRIP_LENGTH; i++) {
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
const ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
sbufWriteU32(dst, *ledConfig);
}
break;
@ -1089,13 +1091,13 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, osdProfile()->units);
sbufWriteU8(dst, osdProfile()->rssi_alarm);
sbufWriteU16(dst, osdProfile()->cap_alarm);
sbufWriteU16(dst, osdProfile()->time_alarm);
sbufWriteU16(dst, osdProfile()->alt_alarm);
sbufWriteU8(dst, osdConfig()->units);
sbufWriteU8(dst, osdConfig()->rssi_alarm);
sbufWriteU16(dst, osdConfig()->cap_alarm);
sbufWriteU16(dst, osdConfig()->time_alarm);
sbufWriteU16(dst, osdConfig()->alt_alarm);
for (int i = 0; i < OSD_ITEM_COUNT; i++) {
sbufWriteU16(dst, osdProfile()->item_pos[i]);
sbufWriteU16(dst, osdConfig()->item_pos[i]);
}
#else
sbufWriteU8(dst, 0); // OSD not supported
@ -1315,12 +1317,12 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#endif
break;
case MSP_SET_ACC_TRIM:
accelerometerConfig()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfig()->accelerometerTrims.values.roll = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.pitch = sbufReadU16(src);
accelerometerConfigMutable()->accelerometerTrims.values.roll = sbufReadU16(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:
@ -1342,7 +1344,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_MODE_RANGE:
i = sbufReadU8(src);
if (i < MAX_MODE_ACTIVATION_CONDITION_COUNT) {
modeActivationCondition_t *mac = &modeActivationProfile()->modeActivationConditions[i];
modeActivationCondition_t *mac = modeActivationConditionsMutable(i);
i = sbufReadU8(src);
const box_t *box = findBoxByPermenantId(i);
if (box) {
@ -1351,7 +1353,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
mac->range.startStep = sbufReadU8(src);
mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
useRcControlsConfig(modeActivationConditions(0), motorConfig(), &currentProfile->pidProfile);
} else {
return MSP_RESULT_ERROR;
}
@ -1363,7 +1365,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_ADJUSTMENT_RANGE:
i = sbufReadU8(src);
if (i < MAX_ADJUSTMENT_RANGE_COUNT) {
adjustmentRange_t *adjRange = &adjustmentProfile()->adjustmentRanges[i];
adjustmentRange_t *adjRange = adjustmentRangesMutable(i);
i = sbufReadU8(src);
if (i < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT) {
adjRange->adjustmentIndex = i;
@ -1405,32 +1407,32 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_MISC:
rxConfig()->midrc = sbufReadU16(src);
motorConfig()->minthrottle = sbufReadU16(src);
motorConfig()->maxthrottle = sbufReadU16(src);
motorConfig()->mincommand = sbufReadU16(src);
rxConfigMutable()->midrc = 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);
compassConfig()->mag_declination = sbufReadU16(src) * 10;
compassConfigMutable()->mag_declination = sbufReadU16(src) * 10;
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:
@ -1448,14 +1450,14 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SUPPORTED_SERVOS) {
return MSP_RESULT_ERROR;
} else {
servoProfile()->servoConf[i].min = sbufReadU16(src);
servoProfile()->servoConf[i].max = sbufReadU16(src);
servoProfile()->servoConf[i].middle = sbufReadU16(src);
servoProfile()->servoConf[i].rate = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMin = sbufReadU8(src);
servoProfile()->servoConf[i].angleAtMax = sbufReadU8(src);
servoProfile()->servoConf[i].forwardFromChannel = sbufReadU8(src);
servoProfile()->servoConf[i].reversedSources = sbufReadU32(src);
servoParamsMutable(i)->min = sbufReadU16(src);
servoParamsMutable(i)->max = sbufReadU16(src);
servoParamsMutable(i)->middle = sbufReadU16(src);
servoParamsMutable(i)->rate = sbufReadU8(src);
servoParamsMutable(i)->angleAtMin = sbufReadU8(src);
servoParamsMutable(i)->angleAtMax = sbufReadU8(src);
servoParamsMutable(i)->forwardFromChannel = sbufReadU8(src);
servoParamsMutable(i)->reversedSources = sbufReadU32(src);
}
#endif
break;
@ -1466,59 +1468,59 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= MAX_SERVO_RULES) {
return MSP_RESULT_ERROR;
} else {
customServoMixer(i)->targetChannel = sbufReadU8(src);
customServoMixer(i)->inputSource = sbufReadU8(src);
customServoMixer(i)->rate = sbufReadU8(src);
customServoMixer(i)->speed = sbufReadU8(src);
customServoMixer(i)->min = sbufReadU8(src);
customServoMixer(i)->max = sbufReadU8(src);
customServoMixer(i)->box = sbufReadU8(src);
customServoMixersMutable(i)->targetChannel = sbufReadU8(src);
customServoMixersMutable(i)->inputSource = sbufReadU8(src);
customServoMixersMutable(i)->rate = sbufReadU8(src);
customServoMixersMutable(i)->speed = sbufReadU8(src);
customServoMixersMutable(i)->min = sbufReadU8(src);
customServoMixersMutable(i)->max = sbufReadU8(src);
customServoMixersMutable(i)->box = sbufReadU8(src);
loadCustomServoMixer();
}
#endif
break;
case MSP_SET_3D:
flight3DConfig()->deadband3d_low = sbufReadU16(src);
flight3DConfig()->deadband3d_high = sbufReadU16(src);
flight3DConfig()->neutral3d = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_low = sbufReadU16(src);
flight3DConfigMutable()->deadband3d_high = sbufReadU16(src);
flight3DConfigMutable()->neutral3d = sbufReadU16(src);
break;
case MSP_SET_RC_DEADBAND:
rcControlsConfig()->deadband = sbufReadU8(src);
rcControlsConfig()->yaw_deadband = sbufReadU8(src);
rcControlsConfig()->alt_hold_deadband = sbufReadU8(src);
flight3DConfig()->deadband3d_throttle = sbufReadU16(src);
rcControlsConfigMutable()->deadband = sbufReadU8(src);
rcControlsConfigMutable()->yaw_deadband = sbufReadU8(src);
rcControlsConfigMutable()->alt_hold_deadband = sbufReadU8(src);
flight3DConfigMutable()->deadband3d_throttle = sbufReadU16(src);
break;
case MSP_SET_RESET_CURR_PID:
resetProfile(currentProfile);
break;
case MSP_SET_SENSOR_ALIGNMENT:
gyroConfig()->gyro_align = sbufReadU8(src);
accelerometerConfig()->acc_align = sbufReadU8(src);
compassConfig()->mag_align = sbufReadU8(src);
gyroConfigMutable()->gyro_align = sbufReadU8(src);
accelerometerConfigMutable()->acc_align = sbufReadU8(src);
compassConfigMutable()->mag_align = sbufReadU8(src);
break;
case MSP_SET_ADVANCED_CONFIG:
gyroConfig()->gyro_sync_denom = sbufReadU8(src);
pidConfig()->pid_process_denom = sbufReadU8(src);
motorConfig()->useUnsyncedPwm = sbufReadU8(src);
gyroConfigMutable()->gyro_sync_denom = sbufReadU8(src);
pidConfigMutable()->pid_process_denom = sbufReadU8(src);
motorConfigMutable()->useUnsyncedPwm = sbufReadU8(src);
#ifdef USE_DSHOT
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1);
#else
motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
motorConfigMutable()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_BRUSHED);
#endif
motorConfig()->motorPwmRate = sbufReadU16(src);
if (sbufBytesRemaining(src) >= 2) {
motorConfig()->digitalIdleOffsetPercent = sbufReadU16(src) / 100.0f;
}
if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_use_32khz = sbufReadU8(src);
gyroConfigMutable()->gyro_use_32khz = sbufReadU8(src);
}
//!!TODO gyro_isr_update to be added pending decision
/*if (sbufBytesRemaining(src)) {
gyroConfig()->gyro_isr_update = sbufReadU8(src);
gyroConfigMutable()->gyro_isr_update = sbufReadU8(src);
}*/
validateAndFixGyroConfig();
@ -1528,18 +1530,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *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 = sbufReadU16(src);
currentProfile->pidProfile.yaw_lpf_hz = sbufReadU16(src);
if (dataSize > 5) {
gyroConfig()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_1 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_hz = sbufReadU16(src);
currentProfile->pidProfile.dterm_notch_cutoff = sbufReadU16(src);
}
if (dataSize > 13) {
gyroConfig()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfig()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_hz_2 = sbufReadU16(src);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = sbufReadU16(src);
}
// reinitialize the gyro filters with the new values
validateAndFixGyroConfig();
@ -1569,9 +1571,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_CONFIG:
accelerometerConfig()->acc_hardware = sbufReadU8(src);
barometerConfig()->baro_hardware = sbufReadU8(src);
compassConfig()->mag_hardware = sbufReadU8(src);
accelerometerConfigMutable()->acc_hardware = sbufReadU8(src);
barometerConfigMutable()->baro_hardware = sbufReadU8(src);
compassConfigMutable()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:
@ -1603,9 +1605,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
@ -1761,53 +1763,53 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_BOARD_ALIGNMENT:
boardAlignment()->rollDegrees = sbufReadU16(src);
boardAlignment()->pitchDegrees = sbufReadU16(src);
boardAlignment()->yawDegrees = sbufReadU16(src);
boardAlignmentMutable()->rollDegrees = sbufReadU16(src);
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src);
boardAlignmentMutable()->yawDegrees = 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
if (dataSize > 4) {
batteryConfig()->batteryMeterType = sbufReadU8(src);
batteryConfigMutable()->batteryMeterType = sbufReadU8(src);
}
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) {
rxConfig()->rcInterpolation = sbufReadU8(src);
rxConfig()->rcInterpolationInterval = sbufReadU8(src);
rxConfig()->airModeActivateThreshold = sbufReadU16(src);
rxConfigMutable()->rcInterpolation = sbufReadU8(src);
rxConfigMutable()->rcInterpolationInterval = sbufReadU8(src);
rxConfigMutable()->airModeActivateThreshold = sbufReadU16(src);
}
if (dataSize > 16) {
rxConfig()->rx_spi_protocol = sbufReadU8(src);
rxConfig()->rx_spi_id = sbufReadU32(src);
rxConfig()->rx_spi_rf_channel_count = sbufReadU8(src);
rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
rxConfigMutable()->rx_spi_id = sbufReadU32(src);
rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
}
if (dataSize > 22) {
rxConfig()->fpvCamAngleDegrees = sbufReadU8(src);
@ -1815,31 +1817,31 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *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);
break;
case MSP_SET_RXFAIL_CONFIG:
i = sbufReadU8(src);
if (i < MAX_SUPPORTED_RC_CHANNEL_COUNT) {
rxConfig()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfig()->failsafe_channel_configurations[i].step = CHANNEL_VALUE_TO_RXFAIL_STEP(sbufReadU16(src));
rxConfigMutable()->failsafe_channel_configurations[i].mode = sbufReadU8(src);
rxConfigMutable()->failsafe_channel_configurations[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;
@ -1847,20 +1849,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()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignment()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignment()->yawDegrees = sbufReadU16(src); // board_align_yaw
boardAlignmentMutable()->rollDegrees = sbufReadU16(src); // board_align_roll
boardAlignmentMutable()->pitchDegrees = sbufReadU16(src); // board_align_pitch
boardAlignmentMutable()->yawDegrees = 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:
@ -1894,7 +1896,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef LED_STRIP
case MSP_SET_LED_COLORS:
for (int i = 0; i < LED_CONFIGURABLE_COLOR_COUNT; i++) {
hsvColor_t *color = &ledStripConfig()->colors[i];
hsvColor_t *color = &ledStripConfigMutable()->colors[i];
color->h = sbufReadU16(src);
color->s = sbufReadU8(src);
color->v = sbufReadU8(src);
@ -1907,7 +1909,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
if (i >= LED_MAX_STRIP_LENGTH || dataSize != (1 + 4)) {
return MSP_RESULT_ERROR;
}
ledConfig_t *ledConfig = &ledStripConfig()->ledConfigs[i];
ledConfig_t *ledConfig = &ledStripConfigMutable()->ledConfigs[i];
*ledConfig = sbufReadU32(src);
reevaluateLedConfig();
}

19
src/main/fc/fc_tasks.c
View File

@ -27,6 +27,11 @@
#include "common/color.h"
#include "common/utils.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/compass.h"
@ -43,8 +48,10 @@
#include "fc/cli.h"
#include "fc/fc_dispatch.h"
#include "flight/pid.h"
#include "flight/altitudehold.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
#include "io/dashboard.h"
@ -72,10 +79,6 @@
#include "telemetry/telemetry.h"
#include "config/feature.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#ifdef USE_BST
void taskBstMasterProcess(timeUs_t currentTimeUs);
#endif
@ -94,7 +97,7 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
accUpdate(&accelerometerConfig()->accelerometerTrims);
accUpdate(&accelerometerConfigMutable()->accelerometerTrims);
}
static void taskHandleSerial(timeUs_t currentTimeUs)
@ -128,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTimeUs;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle);
}
}
}
@ -201,7 +204,7 @@ static void taskTelemetry(timeUs_t currentTimeUs)
telemetryCheckState();
if (!cliMode && feature(FEATURE_TELEMETRY)) {
telemetryProcess(currentTimeUs, &masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
telemetryProcess(currentTimeUs, rxConfig(), flight3DConfig()->deadband3d_throttle);
}
}
#endif

17
src/main/fc/rc_controls.c
View File

@ -31,6 +31,8 @@
#include "common/maths.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
@ -59,7 +61,7 @@
#include "flight/failsafe.h"
static motorConfig_t *motorConfig;
static const motorConfig_t *motorConfig;
static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
@ -115,7 +117,7 @@ bool areSticksInApModePosition(uint16_t ap_mode)
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
}
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
throttleStatus_e calculateThrottleStatus(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
@ -128,7 +130,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
return THROTTLE_HIGH;
}
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
@ -318,12 +320,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
}
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId)
{
uint8_t index;
for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
const modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
if (modeActivationCondition->modeId == modeId && IS_RANGE_USABLE(&modeActivationCondition->range)) {
return true;
@ -647,7 +649,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position)
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig)
{
uint8_t adjustmentIndex;
uint32_t now = millis();
@ -728,7 +730,7 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500);
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
{
motorConfig = motorConfigToUse;
pidProfile = pidProfileToUse;
@ -740,4 +742,3 @@ void resetAdjustmentStates(void)
{
memset(adjustmentStates, 0, sizeof(adjustmentStates));
}

22
src/main/fc/rc_controls.h
View File

@ -19,6 +19,8 @@
#include <stdbool.h>
#include "config/parameter_group.h"
typedef enum {
BOXARM = 0,
BOXANGLE,
@ -140,6 +142,8 @@ typedef struct modeActivationCondition_s {
channelRange_t range;
} modeActivationCondition_t;
PG_DECLARE_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions);
typedef struct modeActivationProfile_s {
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
} modeActivationProfile_t;
@ -158,6 +162,8 @@ typedef struct controlRateConfig_s {
uint16_t tpa_breakpoint; // Breakpoint where TPA is activated
} controlRateConfig_t;
//!!TODO PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);
extern int16_t rcCommand[4];
extern int16_t rcCommandSmooth[4];
@ -169,18 +175,22 @@ typedef struct rcControlsConfig_s {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
} rcControlsConfig_t;
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
typedef struct armingConfig_s {
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
} armingConfig_t;
PG_DECLARE(armingConfig_t, armingConfig);
bool areUsingSticksToArm(void);
bool areSticksInApModePosition(uint16_t ap_mode);
struct rxConfig_s;
throttleStatus_e calculateThrottleStatus(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
@ -267,6 +277,8 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15
PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges);
typedef struct adjustmentProfile_s {
adjustmentRange_t adjustmentRanges[MAX_ADJUSTMENT_RANGE_COUNT];
} adjustmentProfile_t;
@ -274,12 +286,12 @@ typedef struct adjustmentProfile_s {
bool isAirmodeActive(void);
void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig);
bool isUsingSticksForArming(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
struct pidProfile_s;
struct motorConfig_s;
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);

2
src/main/fc/rc_curves.c
View File

@ -34,7 +34,7 @@
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig)
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig)
{
uint8_t i;
uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : motorConfig->minthrottle);

2
src/main/fc/rc_curves.h
View File

@ -19,7 +19,7 @@
struct controlRateConfig_s;
struct motorConfig_s;
void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig);
void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig);
int16_t rcLookupThrottle(int32_t tmp);

22
src/main/flight/failsafe.c
View File

@ -24,6 +24,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"
@ -50,9 +53,10 @@
static failsafeState_t failsafeState;
static failsafeConfig_t *failsafeConfig;
static rxConfig_t *rxConfig;
#ifndef USE_PARAMETER_GROPUS
static const failsafeConfig_t *failsafeConfig;
static const rxConfig_t *rxConfig;
#endif
static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC
@ -72,15 +76,23 @@ static void failsafeReset(void)
/*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse)
void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse)
{
#ifdef USE_PARAMETER_GROUPS
(void)(failsafeConfigToUse);
#else
failsafeConfig = failsafeConfigToUse;
#endif
failsafeReset();
}
void failsafeInit(rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
void failsafeInit(const rxConfig_t *intialRxConfig, uint16_t deadband3d_throttle)
{
#ifdef USE_PARAMETER_GROUPS
(void)(intialRxConfig);
#else
rxConfig = intialRxConfig;
#endif
deadband3dThrottle = deadband3d_throttle;
failsafeState.events = 0;

8
src/main/flight/failsafe.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#define FAILSAFE_POWER_ON_DELAY_US (1000 * 1000 * 5)
#define MILLIS_PER_TENTH_SECOND 100
#define MILLIS_PER_SECOND 1000
@ -36,6 +38,8 @@ typedef struct failsafeConfig_s {
uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it
} failsafeConfig_t;
PG_DECLARE(failsafeConfig_t, failsafeConfig);
typedef enum {
FAILSAFE_IDLE = 0,
FAILSAFE_RX_LOSS_DETECTED,
@ -71,8 +75,8 @@ typedef struct failsafeState_s {
} failsafeState_t;
struct rxConfig_s;
void failsafeInit(struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle);
void useFailsafeConfig(failsafeConfig_t *failsafeConfigToUse);
void failsafeInit(const struct rxConfig_s *intialRxConfig, uint16_t deadband3d_throttle);
void useFailsafeConfig(const failsafeConfig_t *failsafeConfigToUse);
void failsafeStartMonitoring(void);
void failsafeUpdateState(void);

3
src/main/flight/imu.c
View File

@ -30,6 +30,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "sensors/sensors.h"

4
src/main/flight/imu.h
View File

@ -21,6 +21,8 @@
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "sensors/acceleration.h"
// Exported symbols
@ -65,6 +67,8 @@ typedef struct imuConfig_s {
accDeadband_t accDeadband;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s {
float dcm_ki;
float dcm_kp;

70
src/main/flight/mixer.c
View File

@ -28,6 +28,10 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
@ -46,9 +50,6 @@
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "config/feature.h"
#include "config/config_master.h"
#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
// (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977
@ -61,11 +62,13 @@ static float motorMixRange;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig;
static motorConfig_t *motorConfig;
static airplaneConfig_t *airplaneConfig;
rxConfig_t *rxConfig;
#ifndef USE_PARAMETER_GROUPS
static const mixerConfig_t *mixerConfig;
static const flight3DConfig_t *flight3DConfig;
static const motorConfig_t *motorConfig;
const rxConfig_t *rxConfig;
#endif
mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -253,7 +256,7 @@ float getMotorMixRange()
bool isMotorProtocolDshot(void) {
#ifdef USE_DSHOT
switch(motorConfig->motorPwmProtocol) {
switch(motorConfig()->motorPwmProtocol) {
case PWM_TYPE_DSHOT1200:
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
@ -273,25 +276,25 @@ void initEscEndpoints(void) {
if (isMotorProtocolDshot()) {
disarmMotorOutput = DSHOT_DISARM_COMMAND;
if (feature(FEATURE_3D))
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_3D_DEADBAND_LOW - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
else
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig->digitalIdleOffsetPercent);
motorOutputLow = DSHOT_MIN_THROTTLE + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_MIN_THROTTLE) / 100.0f) * motorConfig()->digitalIdleOffsetPercent);
motorOutputHigh = DSHOT_MAX_THROTTLE;
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dHigh = DSHOT_3D_DEADBAND_HIGH + lrintf(((DSHOT_MAX_THROTTLE - DSHOT_3D_DEADBAND_HIGH) / 100.0f) * motorConfig()->digitalIdleOffsetPercent); // TODO - Not working yet !! Mixer requires some throttle rescaling changes
deadbandMotor3dLow = DSHOT_3D_DEADBAND_LOW;
} else
#endif
{
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig->neutral3d : motorConfig->mincommand;
motorOutputLow = motorConfig->minthrottle;
motorOutputHigh = motorConfig->maxthrottle;
deadbandMotor3dHigh = flight3DConfig->deadband3d_high;
deadbandMotor3dLow = flight3DConfig->deadband3d_low;
disarmMotorOutput = (feature(FEATURE_3D)) ? flight3DConfig()->neutral3d : motorConfig()->mincommand;
motorOutputLow = motorConfig()->minthrottle;
motorOutputHigh = motorConfig()->maxthrottle;
deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
}
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig->mincheck);
rcCommandThrottleRange3dLow = rxConfig->midrc - rxConfig->mincheck - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
rcCommandThrottleRange = (PWM_RANGE_MAX - rxConfig()->mincheck);
rcCommandThrottleRange3dLow = rxConfig()->midrc - rxConfig()->mincheck - flight3DConfig()->deadband3d_throttle;
rcCommandThrottleRange3dHigh = PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
}
void mixerUseConfigs(
@ -301,11 +304,18 @@ void mixerUseConfigs(
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfigToUse)
{
#ifdef USE_PARAMETER_GROUPS
(void)(flight3DConfigToUse);
(void)(motorConfigToUse);
(void)(mixerConfigToUse);
(void)(rxConfigToUse);
#else
flight3DConfig = flight3DConfigToUse;
motorConfig = motorConfigToUse;
mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse;
#endif
airplaneConfig = airplaneConfigToUse;
}
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@ -440,25 +450,25 @@ void mixTable(pidProfile_t *pidProfile)
// Find min and max throttle based on condition.
if (feature(FEATURE_3D)) {
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig()->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming.
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
if ((rcCommand[THROTTLE] <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Out of band handling
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
} else if (rcCommand[THROTTLE] >= (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { // Positive handling
motorOutputMax = motorOutputHigh;
motorOutputMin = deadbandMotor3dHigh;
throttlePrevious = rcCommand[THROTTLE];
throttle = rcCommand[THROTTLE] - rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rcCommand[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
} else if ((throttlePrevious <= (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle))) { // Deadband handling from negative to positive
motorOutputMax = deadbandMotor3dLow;
motorOutputMin = motorOutputLow;
throttle = rxConfig->midrc - flight3DConfig->deadband3d_throttle;
throttle = rxConfig()->midrc - flight3DConfig()->deadband3d_throttle;
currentThrottleInputRange = rcCommandThrottleRange3dLow;
if(isMotorProtocolDshot()) mixerInversion = true;
} else { // Deadband handling from positive to negative
@ -468,7 +478,7 @@ void mixTable(pidProfile_t *pidProfile)
currentThrottleInputRange = rcCommandThrottleRange3dHigh;
}
} else {
throttle = rcCommand[THROTTLE] - rxConfig->mincheck;
throttle = rcCommand[THROTTLE] - rxConfig()->mincheck;
currentThrottleInputRange = rcCommandThrottleRange;
motorOutputMin = motorOutputLow;
motorOutputMax = motorOutputHigh;
@ -493,7 +503,7 @@ void mixTable(pidProfile_t *pidProfile)
motorMix[i] =
scaledAxisPIDf[PITCH] * currentMixer[i].pitch +
scaledAxisPIDf[ROLL] * currentMixer[i].roll +
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig->yaw_motor_direction);
scaledAxisPIDf[YAW] * currentMixer[i].yaw * (-mixerConfig()->yaw_motor_direction);
if (vbatCompensationFactor > 1.0f) {
motorMix[i] *= vbatCompensationFactor; // Add voltage compensation
@ -541,7 +551,7 @@ void mixTable(pidProfile_t *pidProfile)
// Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck)) {
motor[i] = disarmMotorOutput;
}
}

27
src/main/flight/mixer.h
View File

@ -17,6 +17,9 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define MAX_SUPPORTED_MOTORS 12
#define QUAD_MOTOR_COUNT 4
@ -85,6 +88,8 @@ typedef struct motorMixer_s {
float yaw;
} motorMixer_t;
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer);
// Custom mixer configuration
typedef struct mixer_s {
uint8_t motorCount;
@ -97,6 +102,8 @@ typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
} mixerConfig_t;
PG_DECLARE(mixerConfig_t, mixerConfig);
typedef struct flight3DConfig_s {
uint16_t deadband3d_low; // min 3d value
uint16_t deadband3d_high; // max 3d value
@ -104,6 +111,22 @@ typedef struct flight3DConfig_s {
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
} flight3DConfig_t;
PG_DECLARE(flight3DConfig_t, flight3DConfig);
typedef struct motorConfig_s {
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedPwm;
float digitalIdleOffsetPercent;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
} motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);
typedef struct airplaneConfig_s {
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
@ -113,8 +136,6 @@ typedef struct airplaneConfig_s {
extern const mixer_t mixers[];
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct motorConfig_s;
struct rxConfig_s;
uint8_t getMotorCount();
@ -122,7 +143,7 @@ float getMotorMixRange();
void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse,
struct motorConfig_s *motorConfigToUse,
motorConfig_t *motorConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);

3
src/main/flight/navigation.c
View File

@ -31,6 +31,9 @@
#include "common/maths.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "fc/config.h"

3
src/main/flight/pid.c
View File

@ -28,6 +28,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/fc_core.h"
#include "fc/fc_rc.h"

4
src/main/flight/pid.h
View File

@ -19,6 +19,8 @@
#include <stdbool.h>
#include "config/parameter_group.h"
#define PID_CONTROLLER_BETAFLIGHT 1
#define PID_MIXER_SCALING 1000.0f
#define PID_SERVO_MIXER_SCALING 0.7f
@ -85,6 +87,8 @@ typedef struct pidProfile_s {
float rateAccelLimit; // accel limiter roll/pitch deg/sec/ms
} pidProfile_t;
PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
} pidConfig_t;

30
src/main/flight/servos.c
View File

@ -28,6 +28,9 @@
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pwm_output.h"
#include "drivers/system.h"
@ -47,7 +50,6 @@
#include "config/feature.h"
extern mixerMode_e currentMixerMode;
extern rxConfig_t *rxConfig;
static servoMixerConfig_t *servoMixerConfig;
@ -276,7 +278,7 @@ void writeServos(void)
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (servoMixerConfig->tri_unarmed_servo) {
if (servoMixerConfig()->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} else {
@ -346,7 +348,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
@ -362,14 +364,14 @@ STATIC_UNIT_TESTED void servoMixer(void)
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig()->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig()->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig()->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig()->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig()->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig()->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig()->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig()->midrc;
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;
@ -440,7 +442,7 @@ void servoTable(void)
servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) {
if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) {
if (gimbalConfig()->mode == GIMBAL_MODE_MIXTILT) {
servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50;
} else {
@ -471,10 +473,10 @@ void filterServos(void)
uint32_t startTime = micros();
#endif
if (servoMixerConfig->servo_lowpass_enable) {
if (servoMixerConfig()->servo_lowpass_enable) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
if (!servoFilterIsSet) {
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime);
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime);
servoFilterIsSet = true;
}

12
src/main/flight/servos.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#define MAX_SUPPORTED_SERVOS 8
// These must be consecutive, see 'reversedSources'
@ -87,6 +89,8 @@ typedef struct servoMixer_s {
#define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
// Custom mixer configuration
typedef struct mixerRules_s {
uint8_t servoRuleCount;
@ -94,6 +98,7 @@ typedef struct mixerRules_s {
} mixerRules_t;
typedef struct servoParam_s {
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
int16_t min; // servo min
int16_t max; // servo max
int16_t middle; // servo middle
@ -101,8 +106,9 @@ typedef struct servoParam_s {
uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value.
uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value.
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
} __attribute__ ((__packed__)) servoParam_t;
} servoParam_t;
PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
typedef struct servoMixerConfig_s{
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
@ -110,6 +116,8 @@ typedef struct servoMixerConfig_s{
int8_t servo_lowpass_enable; // enable/disable lowpass filter
} servoMixerConfig_t;
//!!TODO PG_DECLARE(servoConfig_t, servoConfig);
typedef struct servoProfile_s {
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
} servoProfile_t;

3
src/main/io/displayport_max7456.c
View File

@ -24,7 +24,8 @@
#include "common/utils.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/display.h"
#include "drivers/max7456.h"

4
src/main/io/gimbal.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "config/parameter_group.h"
typedef enum {
GIMBAL_MODE_NORMAL = 0,
GIMBAL_MODE_MIXTILT = 1
@ -27,3 +29,5 @@ typedef enum {
typedef struct gimbalConfig_s {
uint8_t mode;
} gimbalConfig_t;
PG_DECLARE(gimbalConfig_t, gimbalConfig);

5
src/main/io/gps.c
View File

@ -35,6 +35,11 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/light_led.h"
#include "drivers/light_led.h"
#include "drivers/system.h"

4
src/main/io/gps.h
View File

@ -19,6 +19,8 @@
#include "common/time.h"
#include "config/parameter_group.h"
#define LAT 0
#define LON 1
@ -68,6 +70,8 @@ typedef struct gpsConfig_s {
gpsAutoBaud_e autoBaud;
} gpsConfig_t;
PG_DECLARE(gpsConfig_t, gpsConfig);
typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t dddmm;
int16_t mmmm;

12
src/main/io/ledstrip.c
View File

@ -31,6 +31,9 @@
#include "common/maths.h"
#include "common/typeconversion.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/system.h"
#include "drivers/serial.h"
@ -44,6 +47,11 @@
#include "common/axis.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
@ -73,10 +81,6 @@
#include "telemetry/telemetry.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
/*
PG_REGISTER_ARR_WITH_RESET_FN(ledConfig_t, LED_MAX_STRIP_LENGTH, ledConfigs, PG_LED_STRIP_CONFIG, 0);
PG_REGISTER_ARR_WITH_RESET_FN(hsvColor_t, LED_CONFIGURABLE_COLOR_COUNT, colors, PG_COLOR_CONFIG, 0);

3
src/main/io/ledstrip.h
View File

@ -19,6 +19,7 @@
#include "common/color.h"
#include "common/time.h"
#include "config/parameter_group.h"
#include "drivers/io_types.h"
#define LED_MAX_STRIP_LENGTH 32
@ -147,6 +148,8 @@ typedef struct ledStripConfig_s {
ioTag_t ioTag;
} ledStripConfig_t;
PG_DECLARE(ledStripConfig_t, ledStripConfig);
ledConfig_t *ledConfigs;
hsvColor_t *colors;
modeColorIndexes_t *modeColors;

12
src/main/io/motors.h
View File

@ -20,14 +20,4 @@
#include "drivers/io_types.h"
#include "flight/mixer.h"
typedef struct motorConfig_s {
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t motorPwmRate; // The update rate of motor outputs
uint8_t motorPwmProtocol; // Pwm Protocol
uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation
uint8_t useUnsyncedPwm;
float digitalIdleOffsetPercent;
ioTag_t ioTags[MAX_SUPPORTED_MOTORS];
} motorConfig_t;

11
src/main/io/osd.c
View File

@ -40,6 +40,11 @@
#include "common/printf.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/max7456_symbols.h"
#include "drivers/display.h"
#include "drivers/system.h"
@ -51,19 +56,15 @@
#include "cms/cms_types.h"
#include "cms/cms_menu_osd.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/flashfs.h"
#include "io/osd.h"
#include "io/vtx.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif

4
src/main/io/osd.h
View File

@ -18,6 +18,7 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#define VISIBLE_FLAG 0x0800
#define VISIBLE(x) (x & VISIBLE_FLAG)
@ -67,6 +68,9 @@ typedef struct osd_profile_s {
osd_unit_e units;
} osd_profile_t;
// !!TODO change to osdConfig_t
PG_DECLARE(osd_profile_t, osdConfig);
struct displayPort_s;
void osdInit(struct displayPort_s *osdDisplayPort);
void osdResetConfig(osd_profile_t *osdProfile);

3
src/main/io/serial.c
View File

@ -25,6 +25,9 @@
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)

3
src/main/io/serial.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/serial.h"
typedef enum {
@ -111,6 +112,8 @@ typedef struct serialConfig_s {
uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else.
} serialConfig_t;
PG_DECLARE(serialConfig_t, serialConfig);
typedef void serialConsumer(uint8_t);
//

2
src/main/io/servos.h
View File

@ -26,3 +26,5 @@ typedef struct servoConfig_s {
uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz)
ioTag_t ioTags[MAX_SUPPORTED_SERVOS];
} servoConfig_t;
PG_DECLARE(servoConfig_t, servoConfig);

6
src/main/io/vtx.c
View File

@ -26,10 +26,14 @@
#include "io/osd.h"
//External dependencies
#include "config/config_master.h"
#include "config/config_eeprom.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/vtx_rtc6705.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"

16
src/main/io/vtx_smartaudio.c
View File

@ -19,32 +19,36 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <ctype.h>
#include "platform.h"
#if defined(VTX_SMARTAUDIO) && defined(VTX_CONTROL)
#include "build/build_config.h"
#include "cms/cms.h"
#include "cms/cms_types.h"
#include "string.h"
#include "common/printf.h"
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/vtx_common.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_string.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "flight/pid.h"
#include "config/config_master.h"
#include "build/build_config.h"
#include "io/serial.h"
#include "io/vtx_smartaudio.h"
#include "io/vtx_string.h"
//#define SMARTAUDIO_DPRINTF
//#define SMARTAUDIO_DEBUG_MONITOR

47
src/main/rx/rx.c
View File

@ -30,6 +30,8 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/adc.h"
#include "drivers/rx_pwm.h"
@ -90,7 +92,6 @@ uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
rxRuntimeConfig_t rxRuntimeConfig;
static const rxConfig_t *rxConfig;
static uint8_t rcSampleIndex = 0;
static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
@ -108,7 +109,7 @@ static uint8_t nullFrameStatus(void)
void useRxConfig(const rxConfig_t *rxConfigToUse)
{
rxConfig = rxConfigToUse;
(void)(rxConfigToUse);
}
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
@ -126,8 +127,8 @@ STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void)
STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration)
{
return pulseDuration >= rxConfig->rx_min_usec &&
pulseDuration <= rxConfig->rx_max_usec;
return pulseDuration >= rxConfig()->rx_min_usec &&
pulseDuration <= rxConfig()->rx_max_usec;
}
// pulse duration is in micro seconds (usec)
@ -204,20 +205,20 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
}
#endif
void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeActivationConditions)
void rxInit(const rxConfig_t *initialRxConfig, const modeActivationCondition_t *modeActivationConditions)
{
useRxConfig(rxConfig);
useRxConfig(initialRxConfig);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
rcSampleIndex = 0;
needRxSignalMaxDelayUs = DELAY_10_HZ;
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig->midrc;
rcData[i] = rxConfig()->midrc;
rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
}
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec;
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
// Initialize ARM switch to OFF position when arming via switch is defined
for (int i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
@ -237,7 +238,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
const bool enabled = serialRxInit(rxConfig, &rxRuntimeConfig);
const bool enabled = serialRxInit(rxConfig(), &rxRuntimeConfig);
if (!enabled) {
featureClear(FEATURE_RX_SERIAL);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
@ -248,14 +249,14 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#ifdef USE_RX_MSP
if (feature(FEATURE_RX_MSP)) {
rxMspInit(rxConfig, &rxRuntimeConfig);
rxMspInit(rxConfig(), &rxRuntimeConfig);
needRxSignalMaxDelayUs = DELAY_5_HZ;
}
#endif
#ifdef USE_RX_SPI
if (feature(FEATURE_RX_SPI)) {
const bool enabled = rxSpiInit(rxConfig, &rxRuntimeConfig);
const bool enabled = rxSpiInit(rxConfig(), &rxRuntimeConfig);
if (!enabled) {
featureClear(FEATURE_RX_SPI);
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
@ -266,7 +267,7 @@ void rxInit(const rxConfig_t *rxConfig, const modeActivationCondition_t *modeAct
#if defined(USE_PWM) || defined(USE_PPM)
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxPwmInit(rxConfig, &rxRuntimeConfig);
rxPwmInit(rxConfig(), &rxRuntimeConfig);
}
#endif
}
@ -376,7 +377,7 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
static uint16_t getRxfailValue(uint8_t channel)
{
const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig->failsafe_channel_configurations[channel];
const rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &rxConfig()->failsafe_channel_configurations[channel];
switch(channelFailsafeConfiguration->mode) {
case RX_FAILSAFE_MODE_AUTO:
@ -384,12 +385,12 @@ static uint16_t getRxfailValue(uint8_t channel)
case ROLL:
case PITCH:
case YAW:
return rxConfig->midrc;
return rxConfig()->midrc;
case THROTTLE:
if (feature(FEATURE_3D))
return rxConfig->midrc;
return rxConfig()->midrc;
else
return rxConfig->rx_min_usec;
return rxConfig()->rx_min_usec;
}
/* no break */
@ -420,7 +421,7 @@ static uint8_t getRxChannelCount(void) {
static uint8_t maxChannelsAllowed;
if (!maxChannelsAllowed) {
uint8_t maxChannels = rxConfig->max_aux_channel + NON_AUX_CHANNEL_COUNT;
uint8_t maxChannels = rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT;
if (maxChannels > rxRuntimeConfig.channelCount) {
maxChannelsAllowed = rxRuntimeConfig.channelCount;
} else {
@ -436,14 +437,14 @@ static void readRxChannelsApplyRanges(void)
const int channelCount = getRxChannelCount();
for (int channel = 0; channel < channelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel
uint16_t sample = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, rawChannel);
// apply the rx calibration
if (channel < NON_AUX_CHANNEL_COUNT) {
sample = applyRxChannelRangeConfiguraton(sample, rxConfig->channelRanges[channel]);
sample = applyRxChannelRangeConfiguraton(sample, rxConfig()->channelRanges[channel]);
}
rcRaw[channel] = sample;
@ -548,10 +549,10 @@ static void updateRSSIPWM(void)
{
int16_t pwmRssi = 0;
// Read value of AUX channel as rssi
pwmRssi = rcData[rxConfig->rssi_channel - 1];
pwmRssi = rcData[rxConfig()->rssi_channel - 1];
// RSSI_Invert option
if (rxConfig->rssi_ppm_invert) {
if (rxConfig()->rssi_ppm_invert) {
pwmRssi = ((2000 - pwmRssi) + 1000);
}
@ -578,7 +579,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
int16_t adcRssiMean = 0;
uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint8_t rssiPercentage = adcRssiSample / rxConfig->rssi_scale;
uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale;
adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT;
@ -599,7 +600,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
void updateRSSI(timeUs_t currentTimeUs)
{
if (rxConfig->rssi_channel > 0) {
if (rxConfig()->rssi_channel > 0) {
updateRSSIPWM();
} else if (feature(FEATURE_RSSI_ADC)) {
updateRSSIADC(currentTimeUs);

9
src/main/rx/rx.h
View File

@ -18,6 +18,7 @@
#pragma once
#include "common/time.h"
#include "config/parameter_group.h"
#define STICK_CHANNEL_COUNT 4
@ -105,11 +106,17 @@ typedef struct rxFailsafeChannelConfiguration_s {
uint8_t step;
} rxFailsafeChannelConfiguration_t;
//!!TODO change to rxFailsafeChannelConfig_t
PG_DECLARE_ARRAY(rxFailsafeChannelConfiguration_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs);
typedef struct rxChannelRangeConfiguration_s {
uint16_t min;
uint16_t max;
} rxChannelRangeConfiguration_t;
//!!TODO change to rxChannelRangeConfig_t
PG_DECLARE_ARRAY(rxChannelRangeConfiguration_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
typedef struct rxConfig_s {
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
@ -139,6 +146,8 @@ typedef struct rxConfig_s {
rxChannelRangeConfiguration_t channelRanges[NON_AUX_CHANNEL_COUNT];
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);
#define REMAPPABLE_CHANNEL_COUNT (sizeof(((rxConfig_t *)0)->rcmap) / sizeof(((rxConfig_t *)0)->rcmap[0]))
struct rxRuntimeConfig_s;

3
src/main/sensors/acceleration.c
View File

@ -27,6 +27,9 @@
#include "common/axis.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_adxl345.h"
#include "drivers/accgyro_bma280.h"

3
src/main/sensors/acceleration.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/accgyro.h"
#include "sensors/sensors.h"
@ -66,6 +67,8 @@ typedef struct accelerometerConfig_s {
rollAndPitchTrims_t accelerometerTrims;
} accelerometerConfig_t;
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroTargetLooptime);
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);

15
src/main/sensors/barometer.c
View File

@ -23,6 +23,9 @@
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
#include "drivers/barometer_bmp280.h"
@ -51,7 +54,9 @@ static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
static uint32_t baroPressureSum = 0;
#ifndef USE_PARAMETER_GROUPS
static const barometerConfig_t *barometerConfig;
#endif
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
@ -121,7 +126,11 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
void useBarometerConfig(const barometerConfig_t *barometerConfigToUse)
{
#ifdef USE_PARAMETER_GROUPS
(void)(barometerConfigToUse);
#else
barometerConfig = barometerConfigToUse;
#endif
}
bool isBaroCalibrationComplete(void)
@ -160,7 +169,7 @@ static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
return newPressureReading;
}
#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)
#define PRESSURE_SAMPLE_COUNT (barometerConfig()->baro_sample_count - 1)
static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
{
@ -213,7 +222,7 @@ uint32_t baroUpdate(void)
baro.dev.get_up();
baro.dev.start_ut();
baro.dev.calculate(&baroPressure, &baroTemperature);
baroPressureSum = recalculateBarometerTotal(barometerConfig->baro_sample_count, baroPressureSum, baroPressure);
baroPressureSum = recalculateBarometerTotal(barometerConfig()->baro_sample_count, baroPressureSum, baroPressure);
state = BAROMETER_NEEDS_SAMPLES;
return baro.dev.ut_delay;
break;
@ -228,7 +237,7 @@ int32_t baroCalculateAltitude(void)
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / PRESSURE_SAMPLE_COUNT) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude;
baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig->baro_noise_lpf)); // additional LPF to reduce baro noise
baro.BaroAlt = lrintf((float)baro.BaroAlt * barometerConfig()->baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - barometerConfig()->baro_noise_lpf)); // additional LPF to reduce baro noise
return baro.BaroAlt;
}

3
src/main/sensors/barometer.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/barometer.h"
typedef enum {
@ -37,6 +38,8 @@ typedef struct barometerConfig_s {
float baro_cf_alt; // apply CF to use ACC for height estimation
} barometerConfig_t;
PG_DECLARE(barometerConfig_t, barometerConfig);
typedef struct baro_s {
baroDev_t dev;
int32_t BaroAlt;

64
src/main/sensors/battery.c
View File

@ -25,6 +25,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/adc.h"
#include "drivers/system.h"
@ -65,6 +68,10 @@ int32_t amperageLatest = 0; // most recent value
int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start
#ifndef USE_PARAMETER_GROUPS
batteryConfig_t *batteryConfig;
#endif
static batteryState_e vBatState;
static batteryState_e consumptionState;
@ -72,7 +79,7 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
{
// calculate battery voltage based on ADC reading
// result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V
return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier);
return ((((uint32_t)src * batteryConfig()->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig()->vbatresdivval))/batteryConfig()->vbatresdivmultiplier);
}
static void updateBatteryVoltage(void)
@ -86,7 +93,7 @@ static void updateBatteryVoltage(void)
}
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
if (feature(FEATURE_ESC_SENSOR) && batteryConfig()->batteryMeterType == BATTERY_SENSOR_ESC) {
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
vbatLatest = escData->dataAge <= MAX_ESC_BATTERY_AGE ? escData->voltage / 10 : 0;
if (debugMode == DEBUG_BATTERY) {
@ -134,20 +141,20 @@ void updateBattery(void)
uint16_t vBatMeasured = vbatLatest;
/* battery has just been connected*/
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
if (vBatState == BATTERY_NOT_PRESENT && (ARMING_FLAG(ARMED) || (vbat > batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA))) {
/* Actual battery state is calculated below, this is really BATTERY_PRESENT */
vBatState = BATTERY_OK;
unsigned cells = (vBatMeasured / batteryConfig->vbatmaxcellvoltage) + 1;
unsigned cells = (vBatMeasured / batteryConfig()->vbatmaxcellvoltage) + 1;
if (cells > 8) {
// something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells)
cells = 8;
}
batteryCellCount = cells;
batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig->batterynotpresentlevel */
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage;
batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage;
/* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->batterynotpresentlevel */
} else if (vBatState != BATTERY_NOT_PRESENT && !ARMING_FLAG(ARMED) && vbat <= batteryConfig()->batterynotpresentlevel && ABS(vBatMeasured - vBatPrevious) <= VBAT_STABLE_MAX_DELTA) {
vBatState = BATTERY_NOT_PRESENT;
batteryCellCount = 0;
batteryWarningVoltage = 0;
@ -159,16 +166,16 @@ void updateBattery(void)
debug[3] = batteryCellCount;
}
if (batteryConfig->useVBatAlerts) {
if (batteryConfig()->useVBatAlerts) {
switch(vBatState) {
case BATTERY_OK:
if (vbat <= (batteryWarningVoltage - batteryConfig->vbathysteresis)) {
if (vbat <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_WARNING;
}
break;
case BATTERY_WARNING:
if (vbat <= (batteryCriticalVoltage - batteryConfig->vbathysteresis)) {
if (vbat <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
vBatState = BATTERY_CRITICAL;
} else if (vbat > batteryWarningVoltage) {
vBatState = BATTERY_OK;
@ -208,7 +215,11 @@ const char * getBatteryStateString(void)
void batteryInit(batteryConfig_t *initialBatteryConfig)
{
#ifndef USE_PARAMETER_GROUPS
(void)initialBatteryConfig;
#else
batteryConfig = initialBatteryConfig;
#endif
vBatState = BATTERY_NOT_PRESENT;
consumptionState = BATTERY_OK;
batteryCellCount = 0;
@ -221,9 +232,9 @@ static int32_t currentSensorToCentiamps(uint16_t src)
int32_t millivolts;
millivolts = ((uint32_t)src * ADCVREF) / 4096;
millivolts -= batteryConfig->currentMeterOffset;
millivolts -= batteryConfig()->currentMeterOffset;
return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps
return (millivolts * 1000) / (int32_t)batteryConfig()->currentMeterScale; // current in 0.01A steps
}
static void updateBatteryCurrent(void)
@ -251,10 +262,10 @@ static void updateCurrentDrawn(int32_t lastUpdateAt)
void updateConsumptionWarning(void)
{
if (batteryConfig->useConsumptionAlerts && batteryConfig->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) {
if (batteryConfig()->useConsumptionAlerts && batteryConfig()->batteryCapacity > 0 && getBatteryState() != BATTERY_NOT_PRESENT) {
if (calculateBatteryPercentage() == 0) {
vBatState = BATTERY_CRITICAL;
} else if (calculateBatteryPercentage() <= batteryConfig->consumptionWarningPercentage) {
} else if (calculateBatteryPercentage() <= batteryConfig()->consumptionWarningPercentage) {
consumptionState = BATTERY_WARNING;
} else {
consumptionState = BATTERY_OK;
@ -264,35 +275,30 @@ void updateConsumptionWarning(void)
}
}
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
(void)(rxConfig);
if (getBatteryState() != BATTERY_NOT_PRESENT) {
switch(batteryConfig->currentMeterType) {
switch(batteryConfig()->currentMeterType) {
case CURRENT_SENSOR_ADC:
updateBatteryCurrent();
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_VIRTUAL:
amperageLatest = (int32_t)batteryConfig->currentMeterOffset;
amperageLatest = (int32_t)batteryConfig()->currentMeterOffset;
if (ARMING_FLAG(ARMED)) {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle);
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50);
amperageLatest += throttleFactor * (int32_t)batteryConfig->currentMeterScale / 1000;
amperageLatest += throttleFactor * (int32_t)batteryConfig()->currentMeterScale / 1000;
}
amperage = amperageLatest;
updateCurrentDrawn(lastUpdateAt);
updateConsumptionWarning();
break;
case CURRENT_SENSOR_ESC:
#ifdef USE_ESC_SENSOR
@ -328,7 +334,7 @@ float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
// Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) vbat), 1.0f, 1.33f);
}
return batteryScaler;
}
@ -337,11 +343,11 @@ uint8_t calculateBatteryPercentage(void)
{
uint8_t batteryPercentage = 0;
if (batteryCellCount > 0) {
uint16_t batteryCapacity = batteryConfig->batteryCapacity;
uint16_t batteryCapacity = batteryConfig()->batteryCapacity;
if (batteryCapacity > 0) {
batteryPercentage = constrain(((float)batteryCapacity - mAhDrawn) * 100 / batteryCapacity, 0, 100);
} else {
batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig->vbatmaxcellvoltage - batteryConfig->vbatmincellvoltage) * batteryCellCount), 0, 100);
batteryPercentage = constrain((((uint32_t)vbat - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
}
}

9
src/main/sensors/battery.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "common/maths.h" // for fix12_t
#ifndef VBAT_SCALE_DEFAULT
@ -63,6 +64,8 @@ typedef struct batteryConfig_s {
uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning
} batteryConfig_t;
PG_DECLARE(batteryConfig_t, batteryConfig);
typedef enum {
BATTERY_OK = 0,
BATTERY_WARNING,
@ -77,15 +80,17 @@ extern uint16_t batteryWarningVoltage;
extern int32_t amperageLatest;
extern int32_t amperage;
extern int32_t mAhDrawn;
#ifndef USE_PARAMETER_GROUPS
extern batteryConfig_t *batteryConfig;
#endif
batteryState_e getBatteryState(void);
const char * getBatteryStateString(void);
void updateBattery(void);
void batteryInit(batteryConfig_t *initialBatteryConfig);
batteryConfig_t *batteryConfig;
struct rxConfig_s;
void updateCurrentMeter(int32_t lastUpdateAt, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
int32_t currentMeterToCentiamps(uint16_t src);
float calculateVbatPidCompensation(void);

3
src/main/sensors/boardalignment.c
View File

@ -23,6 +23,9 @@
#include "common/maths.h"
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "boardalignment.h"

4
src/main/sensors/boardalignment.h
View File

@ -17,11 +17,15 @@
#pragma once
#include "config/parameter_group.h"
typedef struct boardAlignment_s {
int32_t rollDegrees;
int32_t pitchDegrees;
int32_t yawDegrees;
} boardAlignment_t;
PG_DECLARE(boardAlignment_t, boardAlignment);
void alignSensors(int32_t *dest, uint8_t rotation);
void initBoardAlignment(const boardAlignment_t *boardAlignment);

3
src/main/sensors/compass.c
View File

@ -22,6 +22,9 @@
#include "common/axis.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/compass.h"
#include "drivers/compass_ak8975.h"
#include "drivers/compass_ak8963.h"

4
src/main/sensors/compass.h
View File

@ -17,8 +17,8 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/compass.h"
#include "sensors/sensors.h"
@ -47,6 +47,8 @@ typedef struct compassConfig_s {
flightDynamicsTrims_t magZero;
} compassConfig_t;
PG_DECLARE(compassConfig_t, compassConfig);
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
void compassInit(const compassConfig_t *compassConfig);
union flightDynamicsTrims_u;

18
src/main/sensors/esc_sensor.c
View File

@ -21,26 +21,28 @@
#include <platform.h>
#include "fc/config.h"
#include "build/debug.h"
#include "config/feature.h"
#include "config/config_master.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/pwm_output.h"
#include "drivers/serial.h"
#include "drivers/serial_uart.h"
#include "drivers/pwm_output.h"
#include "drivers/system.h"
#include "io/serial.h"
#include "esc_sensor.h"
#include "fc/config.h"
#include "flight/mixer.h"
#include "sensors/battery.h"
#include "build/debug.h"
#include "esc_sensor.h"
#include "io/serial.h"
/*
KISS ESC TELEMETRY PROTOCOL

41
src/main/sensors/gyro.c
View File

@ -28,6 +28,9 @@
#include "common/maths.h"
#include "common/filter.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_adxl345.h"
#include "drivers/accgyro_bma280.h"
@ -69,7 +72,9 @@ gyro_t gyro; // gyro access functions
static int32_t gyroADC[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
#ifndef USE_PARAMETER_GROUPS
static const gyroConfig_t *gyroConfig;
#endif
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
@ -233,7 +238,11 @@ static bool gyroDetect(gyroDev_t *dev)
bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{
#ifdef USE_PARAMETER_GROUPS
(void)(gyroConfigToUse);
#else
gyroConfig = gyroConfigToUse;
#endif
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
@ -261,8 +270,8 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
}
// Must set gyro sample rate before initialisation
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom, gyroConfig->gyro_use_32khz);
gyro.dev.lpf = gyroConfig->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
gyro.dev.lpf = gyroConfig()->gyro_lpf;
gyro.dev.init(&gyro.dev);
gyroInitFilters();
return true;
@ -280,43 +289,43 @@ void gyroInitFilters(void)
notchFilter1ApplyFn = nullFilterApply;
notchFilter2ApplyFn = nullFilterApply;
if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
if (gyroConfig()->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known
if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterLPF[axis];
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
biquadFilterInitLPF(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime);
}
} else if (gyroConfig->gyro_soft_lpf_type == FILTER_PT1) {
} else if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) {
softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroFilterPt1[axis];
pt1FilterInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyroDt);
pt1FilterInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyroDt);
}
} else {
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) {
softLpfFilter[axis] = &gyroDenoiseState[axis];
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
firFilterDenoiseInit(softLpfFilter[axis], gyroConfig()->gyro_soft_lpf_hz, gyro.targetLooptime);
}
}
}
if (gyroConfig->gyro_soft_notch_hz_1) {
if (gyroConfig()->gyro_soft_notch_hz_1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
for (int axis = 0; axis < 3; axis++) {
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(notchFilter1[axis], gyroConfig()->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
}
}
if (gyroConfig->gyro_soft_notch_hz_2) {
if (gyroConfig()->gyro_soft_notch_hz_2) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
for (int axis = 0; axis < 3; axis++) {
notchFilter2[axis] = &gyroFilterNotch_2[axis];
biquadFilterInit(notchFilter2[axis], gyroConfig->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}
@ -441,7 +450,7 @@ void gyroUpdate(void)
if (calibrationComplete) {
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
// SPI-based gyro so can read and update in ISR
if (gyroConfig->gyro_isr_update) {
if (gyroConfig()->gyro_isr_update) {
mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
return;
}
@ -450,7 +459,7 @@ void gyroUpdate(void)
debug[3] = (uint16_t)(micros() & 0xffff);
#endif
} else {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {

3
src/main/sensors/gyro.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/accgyro.h"
#include "common/axis.h"
@ -59,6 +60,8 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_cutoff_2;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);
void gyroSetCalibrationCycles(void);
bool gyroInit(const gyroConfig_t *gyroConfigToUse);
void gyroInitFilters(void);

2
src/main/sensors/sonar.c
View File

@ -29,6 +29,8 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/runtime_config.h"

3
src/main/sensors/sonar.h
View File

@ -17,10 +17,9 @@
#pragma once
#include "config/parameter_group.h"
#include "common/time.h"
#include "drivers/sonar_hcsr04.h"
#include "sensors/battery.h"
#define SONAR_OUT_OF_RANGE (-1)

30
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -77,8 +77,6 @@
#include "bus_bst.h"
#include "i2c_bst.h"
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse);
#define BST_PROTOCOL_VERSION 0
#define API_VERSION_MAJOR 1 // increment when major changes are made
@ -640,13 +638,13 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
break;
case BST_SERVO_MIX_RULES:
for (i = 0; i < MAX_SERVO_RULES; i++) {
bstWrite8(customServoMixer(i)->targetChannel);
bstWrite8(customServoMixer(i)->inputSource);
bstWrite8(customServoMixer(i)->rate);
bstWrite8(customServoMixer(i)->speed);
bstWrite8(customServoMixer(i)->min);
bstWrite8(customServoMixer(i)->max);
bstWrite8(customServoMixer(i)->box);
bstWrite8(customServoMixers(i)->targetChannel);
bstWrite8(customServoMixers(i)->inputSource);
bstWrite8(customServoMixers(i)->rate);
bstWrite8(customServoMixers(i)->speed);
bstWrite8(customServoMixers(i)->min);
bstWrite8(customServoMixers(i)->max);
bstWrite8(customServoMixers(i)->box);
}
break;
#endif
@ -1179,13 +1177,13 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
if (i >= MAX_SERVO_RULES) {
ret = BST_FAILED;
} else {
customServoMixer(i)->targetChannel = bstRead8();
customServoMixer(i)->inputSource = bstRead8();
customServoMixer(i)->rate = bstRead8();
customServoMixer(i)->speed = bstRead8();
customServoMixer(i)->min = bstRead8();
customServoMixer(i)->max = bstRead8();
customServoMixer(i)->box = bstRead8();
customServoMixers(i)->targetChannel = bstRead8();
customServoMixers(i)->inputSource = bstRead8();
customServoMixers(i)->rate = bstRead8();
customServoMixers(i)->speed = bstRead8();
customServoMixers(i)->min = bstRead8();
customServoMixers(i)->max = bstRead8();
customServoMixers(i)->box = bstRead8();
loadCustomServoMixer();
}
#endif

1
src/main/target/common.h
View File

@ -17,6 +17,7 @@
#pragma once
//#define USE_PARAMETER_GROUPS
// type conversion warnings.
// -Wconversion can be turned on to enable the process of eliminating these warnings
//#pragma GCC diagnostic warning "-Wconversion"

4
src/main/telemetry/crsf.c
View File

@ -32,10 +32,8 @@
#define CLEANFLIGHT
#endif
#ifdef CLEANFLIGHT
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#endif
#include "common/streambuf.h"
#include "common/utils.h"
@ -184,7 +182,7 @@ void crsfFrameBatterySensor(sbuf_t *dst)
const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage();
#else
crsfSerialize16(dst, amperage / 10);
const uint32_t batteryCapacity = batteryConfig->batteryCapacity;
const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
#endif
crsfSerialize8(dst, (batteryCapacity >> 16));

36
src/main/telemetry/frsky.c
View File

@ -33,6 +33,8 @@
#include "common/utils.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
@ -72,13 +74,13 @@ static serialPortConfig_t *portConfig;
#define FRSKY_BAUDRATE 9600
#define FRSKY_INITIAL_PORT_MODE MODE_TX
static telemetryConfig_t *telemetryConfig;
#ifndef USE_PARAMETER_GROUPS
static const telemetryConfig_t *telemetryConfig;
#endif
static bool frskyTelemetryEnabled = false;
static portSharing_e frskyPortSharing;
extern batteryConfig_t *batteryConfig;
#define CYCLETIME 125
#define PROTOCOL_HEADER 0x5E
@ -196,7 +198,7 @@ static void sendGpsAltitude(void)
}
#endif
static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
sendDataHead(ID_RPM);
#ifdef USE_ESC_SENSOR
@ -213,7 +215,7 @@ static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadba
throttleForRPM = 0;
serialize16(throttleForRPM);
} else {
serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
serialize16((batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER));
}
#endif
}
@ -240,7 +242,7 @@ static void sendSatalliteSignalQualityAsTemperature2(void)
}
sendDataHead(ID_TEMPRATURE2);
if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
if (telemetryConfig()->frsky_unit == FRSKY_UNIT_METRICS) {
serialize16(satellite);
} else {
float tmp = (satellite - 32) / 1.8f;
@ -286,7 +288,7 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
min = absgps / GPS_DEGREES_DIVIDER; // minutes left
if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
result->dddmm = deg * 100 + min;
} else {
result->dddmm = deg * 60 + min;
@ -332,8 +334,8 @@ static void sendFakeLatLong(void)
// Heading is only displayed on OpenTX if non-zero lat/long is also sent
int32_t coord[2] = {0,0};
coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
sendLatLong(coord);
}
@ -409,7 +411,7 @@ static void sendVoltage(void)
*/
static void sendVoltageAmp(void)
{
if (telemetryConfig->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
/*
* Use new ID 0x39 to send voltage directly in 0.1 volts resolution
*/
@ -418,7 +420,7 @@ static void sendVoltageAmp(void)
} else {
uint16_t voltage = (getVbat() * 110) / 21;
uint16_t vfasVoltage;
if (telemetryConfig->frsky_vfas_cell_voltage) {
if (telemetryConfig()->frsky_vfas_cell_voltage) {
vfasVoltage = voltage / batteryCellCount;
} else {
vfasVoltage = voltage;
@ -440,7 +442,7 @@ static void sendFuelLevel(void)
{
sendDataHead(ID_FUEL_LEVEL);
if (batteryConfig->batteryCapacity > 0) {
if (batteryConfig()->batteryCapacity > 0) {
serialize16((uint16_t)calculateBatteryPercentage());
} else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
@ -455,9 +457,13 @@ static void sendHeading(void)
serialize16(0);
}
void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
void initFrSkyTelemetry(const telemetryConfig_t *initialTelemetryConfig)
{
#ifdef USE_PARAMETER_GROUPS
UNUSED(initialTelemetryConfig);
#else
telemetryConfig = initialTelemetryConfig;
#endif
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY);
frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY);
}
@ -475,7 +481,7 @@ void configureFrSkyTelemetryPort(void)
return;
}
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
if (!frskyPort) {
return;
}
@ -509,7 +515,7 @@ void checkFrSkyTelemetryState(void)
}
}
void handleFrSkyTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
void handleFrSkyTelemetry(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
if (!frskyTelemetryEnabled) {
return;

4
src/main/telemetry/frsky.h
View File

@ -23,11 +23,11 @@ typedef enum {
} frskyVFasPrecision_e;
struct rxConfig_s;
void handleFrSkyTelemetry(struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void handleFrSkyTelemetry(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void checkFrSkyTelemetryState(void);
struct telemetryConfig_s;
void initFrSkyTelemetry(struct telemetryConfig_s *telemetryConfig);
void initFrSkyTelemetry(const struct telemetryConfig_s *telemetryConfig);
void configureFrSkyTelemetryPort(void);
void freeFrSkyTelemetryPort(void);

16
src/main/telemetry/hott.c
View File

@ -65,6 +65,10 @@
#include "common/axis.h"
#include "common/time.h"
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
@ -108,7 +112,9 @@ static uint8_t hottMsgCrc;
static serialPort_t *hottPort = NULL;
static serialPortConfig_t *portConfig;
static telemetryConfig_t *telemetryConfig;
#ifndef USE_PARAMETER_GROUPS
static const telemetryConfig_t *telemetryConfig;
#endif
static bool hottTelemetryEnabled = false;
static portSharing_e hottPortSharing;
@ -219,7 +225,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
static bool shouldTriggerBatteryAlarmNow(void)
{
return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND));
return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND));
}
static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
@ -289,9 +295,13 @@ void freeHoTTTelemetryPort(void)
hottTelemetryEnabled = false;
}
void initHoTTTelemetry(telemetryConfig_t *initialTelemetryConfig)
void initHoTTTelemetry(const telemetryConfig_t *initialTelemetryConfig)
{
#ifdef USE_PARAMETER_GROUPS
UNUSED(initialTelemetryConfig);
#else
telemetryConfig = initialTelemetryConfig;
#endif
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT);
hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT);

2
src/main/telemetry/hott.h
View File

@ -490,7 +490,7 @@ typedef struct HOTT_AIRESC_MSG_s {
void handleHoTTTelemetry(timeUs_t currentTimeUs);
void checkHoTTTelemetryState(void);
void initHoTTTelemetry(telemetryConfig_t *telemetryConfig);
void initHoTTTelemetry(const telemetryConfig_t *telemetryConfig);
void configureHoTTTelemetryPort(void);
void freeHoTTTelemetryPort(void);

3
src/main/telemetry/ibus.c
View File

@ -32,7 +32,8 @@
#if defined(TELEMETRY) && defined(TELEMETRY_IBUS)
#include "config/config_master.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "common/axis.h"

4
src/main/telemetry/ibus.h
View File

@ -17,13 +17,11 @@
#pragma once
/*
typedef struct ibusTelemetryConfig_s {
uint8_t report_cell_voltage; // report vbatt divided with cellcount
} ibusTelemetryConfig_t;
PG_DECLARE(ibusTelemetryConfig_t, ibusTelemetryConfig);
*/
void initIbusTelemetry(void);
@ -31,4 +29,4 @@ void handleIbusTelemetry(void);
bool checkIbusTelemetryState(void);
void configureIbusTelemetryPort(void);
void freeIbusTelemetryPort(void);
void freeIbusTelemetryPort(void);

11
src/main/telemetry/ltm.c
View File

@ -40,6 +40,7 @@
#include "common/maths.h"
#include "common/axis.h"
#include "common/color.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
@ -80,7 +81,9 @@
static serialPort_t *ltmPort;
static serialPortConfig_t *portConfig;
static telemetryConfig_t *telemetryConfig;
#ifndef USE_PARAMETER_GROUPS
static const telemetryConfig_t *telemetryConfig;
#endif
static bool ltmEnabled;
static portSharing_e ltmPortSharing;
static uint8_t ltm_crc;
@ -268,9 +271,13 @@ void freeLtmTelemetryPort(void)
ltmEnabled = false;
}
void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig)
void initLtmTelemetry(const telemetryConfig_t *initialTelemetryConfig)
{
#ifdef USE_PARAMETER_GROUPS
UNUSED(initialTelemetryConfig);
#else
telemetryConfig = initialTelemetryConfig;
#endif
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
}

2
src/main/telemetry/ltm.h
View File

@ -20,7 +20,7 @@
#pragma once
struct telemetryConfig_s;
void initLtmTelemetry(struct telemetryConfig_s *initialTelemetryConfig);
void initLtmTelemetry(const struct telemetryConfig_s *initialTelemetryConfig);
void handleLtmTelemetry(void);
void checkLtmTelemetryState(void);

44
src/main/telemetry/mavlink.c
View File

@ -32,26 +32,18 @@
#include "common/axis.h"
#include "common/color.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "rx/rx.h"
#include "fc/runtime_config.h"
#include "flight/mixer.h"
#include "flight/pid.h"
@ -60,16 +52,24 @@
#include "flight/navigation.h"
#include "flight/altitudehold.h"
#include "io/serial.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/motors.h"
#include "rx/rx.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "telemetry/telemetry.h"
#include "telemetry/mavlink.h"
#include "fc/config.h"
#include "config/config_profile.h"
#include "config/config_master.h"
#include "config/feature.h"
#include "fc/runtime_config.h"
// mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used
// until this is resolved in mavlink library - ignore -Wpedantic for mavlink code
#pragma GCC diagnostic push

26
src/main/telemetry/smartport.c
View File

@ -16,6 +16,11 @@
#include "common/maths.h"
#include "common/utils.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
@ -52,9 +57,6 @@
#include "telemetry/telemetry.h"
#include "telemetry/smartport.h"
#include "config/config_profile.h"
#include "config/feature.h"
#include "msp/msp.h"
extern profile_t *currentProfile;
@ -148,7 +150,9 @@ const uint16_t frSkyDataIdTable[] = {
static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
static serialPortConfig_t *portConfig;
static telemetryConfig_t *telemetryConfig;
#ifndef USE_PARAMETER_GROUPS
static const telemetryConfig_t *telemetryConfig;
#endif
static bool smartPortTelemetryEnabled = false;
static portSharing_e smartPortPortSharing;
@ -302,9 +306,13 @@ static void smartPortSendPackage(uint16_t id, uint32_t val)
smartPortSendPackageEx(FSSP_DATA_FRAME,payload);
}
void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
void initSmartPortTelemetry(const telemetryConfig_t *initialTelemetryConfig)
{
#ifdef USE_PARAMETER_GROUPS
UNUSED(initialTelemetryConfig);
#else
telemetryConfig = initialTelemetryConfig;
#endif
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
}
@ -326,11 +334,11 @@ void configureSmartPortTelemetryPort(void)
portOptions_t portOptions = 0;
if (telemetryConfig->sportHalfDuplex) {
if (telemetryConfig()->sportHalfDuplex) {
portOptions |= SERIAL_BIDIR;
}
if (telemetryConfig->telemetry_inversion) {
if (telemetryConfig()->telemetry_inversion) {
portOptions |= SERIAL_INVERTED;
}
@ -622,7 +630,7 @@ void handleSmartPortTelemetry(void)
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT) && batteryCellCount > 0) {
uint16_t vfasVoltage;
if (telemetryConfig->frsky_vfas_cell_voltage) {
if (telemetryConfig()->frsky_vfas_cell_voltage) {
vfasVoltage = getVbat() / batteryCellCount;
} else {
vfasVoltage = getVbat();
@ -752,7 +760,7 @@ void handleSmartPortTelemetry(void)
} else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
smartPortHasRequest = 0;
} else if (telemetryConfig->pidValuesAsTelemetry){
} else if (telemetryConfig()->pidValuesAsTelemetry){
switch (t2Cnt) {
case 0:
tmp2 = currentProfile->pidProfile.P8[ROLL];

2
src/main/telemetry/smartport.h
View File

@ -7,7 +7,7 @@
#pragma once
void initSmartPortTelemetry(telemetryConfig_t *);
void initSmartPortTelemetry(const telemetryConfig_t *);
void handleSmartPortTelemetry(void);
void checkSmartPortTelemetryState(void);

23
src/main/telemetry/telemetry.c
View File

@ -25,6 +25,9 @@
#include "common/utils.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "drivers/serial_softserial.h"
@ -50,29 +53,35 @@
#include "telemetry/srxl.h"
#include "telemetry/ibus.h"
#ifndef USE_PARAMETER_GROUPS
static telemetryConfig_t *telemetryConfig;
#endif
void telemetryUseConfig(telemetryConfig_t *telemetryConfigToUse)
{
#ifdef USE_PARAMETER_GROUPS
UNUSED(telemetryConfigToUse);
#else
telemetryConfig = telemetryConfigToUse;
#endif
}
void telemetryInit(void)
{
#ifdef TELEMETRY_FRSKY
initFrSkyTelemetry(telemetryConfig);
initFrSkyTelemetry(telemetryConfig());
#endif
#ifdef TELEMETRY_HOTT
initHoTTTelemetry(telemetryConfig);
initHoTTTelemetry(telemetryConfig());
#endif
#ifdef TELEMETRY_SMARTPORT
initSmartPortTelemetry(telemetryConfig);
initSmartPortTelemetry(telemetryConfig());
#endif
#ifdef TELEMETRY_LTM
initLtmTelemetry(telemetryConfig);
initLtmTelemetry(telemetryConfig());
#endif
#ifdef TELEMETRY_JETIEXBUS
initJetiExBusTelemetry(telemetryConfig);
initJetiExBusTelemetry(telemetryConfig());
#endif
#ifdef TELEMETRY_MAVLINK
initMAVLinkTelemetry();
@ -95,7 +104,7 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
bool enabled = portSharing == PORTSHARING_NOT_SHARED;
if (portSharing == PORTSHARING_SHARED) {
if (telemetryConfig->telemetry_switch)
if (telemetryConfig()->telemetry_switch)
enabled = IS_RC_MODE_ACTIVE(BOXTELEMETRY);
else
enabled = ARMING_FLAG(ARMED);
@ -142,7 +151,7 @@ void telemetryCheckState(void)
#endif
}
void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
void telemetryProcess(uint32_t currentTime, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
#ifdef TELEMETRY_FRSKY
handleFrSkyTelemetry(rxConfig, deadband3d_throttle);

7
src/main/telemetry/telemetry.h
View File

@ -24,6 +24,9 @@
#pragma once
#include "config/parameter_group.h"
#include "io/serial.h"
typedef enum {
FRSKY_FORMAT_DMS = 0,
FRSKY_FORMAT_NMEA
@ -49,13 +52,15 @@ typedef struct telemetryConfig_s {
uint8_t report_cell_voltage;
} telemetryConfig_t;
PG_DECLARE(telemetryConfig_t, telemetryConfig);
void telemetryInit(void);
bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig);
extern serialPort_t *telemetrySharedPort;
void telemetryCheckState(void);
struct rxConfig_s;
void telemetryProcess(uint32_t currentTime, struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
void telemetryProcess(uint32_t currentTime, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
bool telemetryDetermineEnabledState(portSharing_e portSharing);

1
src/test/unit/cms_unittest.cc
View File

@ -26,6 +26,7 @@
#define BARO
extern "C" {
#include "platform.h""
#include "target.h"
#include "drivers/display.h"
#include "cms/cms.h"

2
src/test/unit/parameter_groups_unittest.cc
View File

@ -29,7 +29,7 @@ extern "C" {
#include "io/motors.h"
PG_DECLARE(motorConfig_t, motorConfig);
//PG_DECLARE(motorConfig_t, motorConfig);
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);

4
src/test/unit/platform.h
View File

@ -17,6 +17,8 @@
#pragma once
#define USE_PARAMETER_GROUPS
#define U_ID_0 0
#define U_ID_1 1
#define U_ID_2 2
@ -30,8 +32,6 @@
#define USE_SERVOS
#define TRANSPONDER
#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6
typedef enum
{
Mode_TEST = 0x0,

11
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -22,10 +22,10 @@
#include <limits.h>
extern "C" {
#include "build/debug.h"
#include <platform.h>
#include "build/debug.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/gps_conversion.h"
@ -56,6 +56,8 @@ extern "C" {
bool airMode;
uint16_t vbat;
serialPort_t *telemetrySharedPort;
PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
}
#include "unittest_macros.h"
@ -135,10 +137,7 @@ TEST(TelemetryCrsfTest, TestGPS)
TEST(TelemetryCrsfTest, TestBattery)
{
uint8_t frame[CRSF_FRAME_SIZE_MAX];
batteryConfig_t workingBatteryConfig;
batteryConfig = &workingBatteryConfig;
memset(batteryConfig, 0, sizeof(batteryConfig_t));
vbat = 0; // 0.1V units
int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
@ -157,7 +156,7 @@ TEST(TelemetryCrsfTest, TestBattery)
vbat = 33; // 3.3V = 3300 mv
amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps
batteryConfig->batteryCapacity = 1234;
batteryConfigMutable()->batteryCapacity = 1234;
frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
voltage = frame[3] << 8 | frame[4]; // mV * 100
EXPECT_EQ(33, voltage);

6
src/test/unit/telemetry_hott_unittest.cc
View File

@ -29,6 +29,10 @@ extern "C" {
#include "common/axis.h"
#include "common/gps_conversion.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/system.h"
#include "drivers/serial.h"
#include "drivers/system.h"
@ -45,6 +49,8 @@ extern "C" {
#include "telemetry/telemetry.h"
#include "telemetry/hott.h"
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
}
#include "unittest_macros.h"