Added PG config definitions 3

This commit is contained in:
Martin Budden 2017-02-25 09:21:05 +00:00
parent ff40e8c844
commit a94318a75f
7 changed files with 147 additions and 10 deletions

View File

@ -94,6 +94,9 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#ifndef DEFAULT_FEATURES
#define DEFAULT_FEATURES 0
#endif
#ifndef DEFAULT_RX_FEATURE #ifndef DEFAULT_RX_FEATURE
#define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM #define DEFAULT_RX_FEATURE FEATURE_RX_PARALLEL_PWM
#endif #endif
@ -104,6 +107,25 @@
#define BRUSHED_MOTORS_PWM_RATE 16000 #define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 480 #define BRUSHLESS_MOTORS_PWM_RATE 480
PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t, featureConfig, PG_FEATURE_CONFIG, 0);
PG_RESET_TEMPLATE(featureConfig_t, featureConfig,
.enabledFeatures = DEFAULT_FEATURES | DEFAULT_RX_FEATURE | FEATURE_FAILSAFE
);
PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.current_profile_index = 0,
//!!TODO.activeRateProfile = 0,
.debug_mode = DEBUG_MODE,
.task_statistics = true,
.name = { 0 }
);
PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0);
master_t masterConfig; // master config struct with data independent from profiles master_t masterConfig; // master config struct with data independent from profiles
profile_t *currentProfile; profile_t *currentProfile;
@ -358,7 +380,7 @@ void resetAdcConfig(adcConfig_t *adcConfig)
#ifdef BEEPER #ifdef BEEPER
void resetBeeperConfig(beeperDevConfig_t *beeperDevConfig) void resetBeeperDevConfig(beeperDevConfig_t *beeperDevConfig)
{ {
#ifdef BEEPER_INVERTED #ifdef BEEPER_INVERTED
beeperDevConfig->isOpenDrain = false; beeperDevConfig->isOpenDrain = false;
@ -400,6 +422,7 @@ void resetPwmConfig(pwmConfig_t *pwmConfig)
} }
#endif #endif
#ifndef USE_PARAMETER_GROUPS
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
{ {
flight3DConfig->deadband3d_low = 1406; flight3DConfig->deadband3d_low = 1406;
@ -407,7 +430,9 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
flight3DConfig->neutral3d = 1460; flight3DConfig->neutral3d = 1460;
flight3DConfig->deadband3d_throttle = 50; flight3DConfig->deadband3d_throttle = 50;
} }
#endif
#ifndef USE_PARAMETER_GROUPS
#ifdef TELEMETRY #ifdef TELEMETRY
void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
{ {
@ -427,6 +452,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
#endif #endif
} }
#endif #endif
#endif
#ifndef USE_PARAMETER_GROUPS #ifndef USE_PARAMETER_GROUPS
void resetBatteryConfig(batteryConfig_t *batteryConfig) void resetBatteryConfig(batteryConfig_t *batteryConfig)
@ -627,6 +653,7 @@ void resetSerialPinConfig(serialPinConfig_t *pSerialPinConfig)
#define SECOND_PORT_INDEX 1 #define SECOND_PORT_INDEX 1
#endif #endif
#ifndef USE_PARAMETER_GROUPS
void resetSerialConfig(serialConfig_t *serialConfig) void resetSerialConfig(serialConfig_t *serialConfig)
{ {
memset(serialConfig, 0, sizeof(serialConfig_t)); memset(serialConfig, 0, sizeof(serialConfig_t));
@ -647,6 +674,7 @@ void resetSerialConfig(serialConfig_t *serialConfig)
serialConfig->portConfigs[1].functionMask = FUNCTION_MSP; serialConfig->portConfigs[1].functionMask = FUNCTION_MSP;
#endif #endif
} }
#endif
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
{ {
@ -656,6 +684,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig)
rcControlsConfig->alt_hold_fast_change = 1; rcControlsConfig->alt_hold_fast_change = 1;
} }
#ifndef USE_PARAMETER_GROUPS
void resetMixerConfig(mixerConfig_t *mixerConfig) void resetMixerConfig(mixerConfig_t *mixerConfig)
{ {
#ifdef TARGET_DEFAULT_MIXER #ifdef TARGET_DEFAULT_MIXER
@ -665,6 +694,7 @@ void resetMixerConfig(mixerConfig_t *mixerConfig)
#endif #endif
mixerConfig->yaw_motor_direction = 1; mixerConfig->yaw_motor_direction = 1;
} }
#endif
#ifdef USE_MAX7456 #ifdef USE_MAX7456
void resetMax7456Config(vcdProfile_t *pVcdProfile) void resetMax7456Config(vcdProfile_t *pVcdProfile)
@ -795,7 +825,13 @@ void createDefaultConfig(master_t *config)
config->version = EEPROM_CONF_VERSION; config->version = EEPROM_CONF_VERSION;
// global settings // global settings
#ifndef USE_PARAMETER_GROUPS
config->systemConfig.current_profile_index = 0; // default profile config->systemConfig.current_profile_index = 0; // default profile
config->systemConfig.debug_mode = DEBUG_MODE;
config->systemConfig.task_statistics = true;
#endif
config->imuConfig.dcm_kp = 2500; // 1.0 * 10000 config->imuConfig.dcm_kp = 2500; // 1.0 * 10000
config->imuConfig.dcm_ki = 0; // 0.003 * 10000 config->imuConfig.dcm_ki = 0; // 0.003 * 10000
#ifndef USE_PARAMETER_GROUPS #ifndef USE_PARAMETER_GROUPS
@ -820,11 +856,6 @@ void createDefaultConfig(master_t *config)
config->gyroConfig.gyroMovementCalibrationThreshold = 48; config->gyroConfig.gyroMovementCalibrationThreshold = 48;
#endif #endif
config->systemConfig.debug_mode = DEBUG_MODE;
config->systemConfig.task_statistics = true;
#ifndef USE_PARAMETER_GROUPS #ifndef USE_PARAMETER_GROUPS
resetCompassConfig(&config->compassConfig); resetCompassConfig(&config->compassConfig);
#endif #endif
@ -836,9 +867,11 @@ void createDefaultConfig(master_t *config)
resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims); resetRollAndPitchTrims(&config->accelerometerConfig.accelerometerTrims);
config->accelerometerConfig.acc_lpf_hz = 10.0f; config->accelerometerConfig.acc_lpf_hz = 10.0f;
#endif #endif
#ifndef USE_PARAMETER_GROUPS
config->boardAlignment.rollDegrees = 0; config->boardAlignment.rollDegrees = 0;
config->boardAlignment.pitchDegrees = 0; config->boardAlignment.pitchDegrees = 0;
config->boardAlignment.yawDegrees = 0; config->boardAlignment.yawDegrees = 0;
#endif
config->rcControlsConfig.yaw_control_direction = 1; config->rcControlsConfig.yaw_control_direction = 1;
// xxx_hardware: 0:default/autodetect, 1: disable // xxx_hardware: 0:default/autodetect, 1: disable
@ -864,7 +897,7 @@ void createDefaultConfig(master_t *config)
#endif #endif
#ifdef BEEPER #ifdef BEEPER
resetBeeperConfig(&config->beeperDevConfig); resetBeeperDevConfig(&config->beeperDevConfig);
#endif #endif
#ifdef SONAR #ifdef SONAR
@ -916,20 +949,27 @@ void createDefaultConfig(master_t *config)
config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; config->pwmConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
#endif #endif
#ifndef USE_PARAMETER_GROUPS
config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support config->armingConfig.gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support
config->armingConfig.disarm_kill_switch = 1; config->armingConfig.disarm_kill_switch = 1;
config->armingConfig.auto_disarm_delay = 5; config->armingConfig.auto_disarm_delay = 5;
#endif
config->imuConfig.small_angle = 25; config->imuConfig.small_angle = 25;
config->airplaneConfig.fixedwing_althold_dir = 1; config->airplaneConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo // Motor/ESC/Servo
#ifndef USE_PARAMETER_GROUPS
resetMixerConfig(&config->mixerConfig); resetMixerConfig(&config->mixerConfig);
#endif
resetMotorConfig(&config->motorConfig); resetMotorConfig(&config->motorConfig);
#ifdef USE_SERVOS #ifdef USE_SERVOS
resetServoConfig(&config->servoConfig); resetServoConfig(&config->servoConfig);
#endif #endif
#ifndef USE_PARAMETER_GROUPS
resetFlight3DConfig(&config->flight3DConfig); resetFlight3DConfig(&config->flight3DConfig);
#endif
#ifdef LED_STRIP #ifdef LED_STRIP
resetLedStripConfig(&config->ledStripConfig); resetLedStripConfig(&config->ledStripConfig);
@ -945,7 +985,9 @@ void createDefaultConfig(master_t *config)
resetSerialPinConfig(&config->serialPinConfig); resetSerialPinConfig(&config->serialPinConfig);
#ifndef USE_PARAMETER_GROUPS
resetSerialConfig(&config->serialConfig); resetSerialConfig(&config->serialConfig);
#endif
resetProfile(&config->profile[0]); resetProfile(&config->profile[0]);
@ -973,6 +1015,7 @@ void createDefaultConfig(master_t *config)
config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv config->throttleCorrectionConfig.throttle_correction_value = 0; // could 10 with althold or 40 for fpv
config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv config->throttleCorrectionConfig.throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv
#ifndef USE_PARAMETER_GROUPS
// Failsafe Variables // Failsafe Variables
config->failsafeConfig.failsafe_delay = 10; // 1sec config->failsafeConfig.failsafe_delay = 10; // 1sec
config->failsafeConfig.failsafe_off_delay = 10; // 1sec config->failsafeConfig.failsafe_off_delay = 10; // 1sec
@ -980,6 +1023,7 @@ void createDefaultConfig(master_t *config)
config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss
config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition
config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing
#endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
// servos // servos
@ -1165,11 +1209,12 @@ void validateAndFixConfig(void)
} }
#endif #endif
#ifndef USE_PARAMETER_GROUPS
serialConfig_t *serialConfig = &masterConfig.serialConfig; serialConfig_t *serialConfig = &masterConfig.serialConfig;
if (!isSerialConfigValid(serialConfig)) { if (!isSerialConfigValid(serialConfig)) {
resetSerialConfig(serialConfig); resetSerialConfig(serialConfig);
} }
#endif
validateAndFixGyroConfig(); validateAndFixGyroConfig();
@ -1267,7 +1312,7 @@ void readEEPROM(void)
// setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex); // setControlRateProfile(rateProfileSelection()->defaultRateProfileIndex);
if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check if (systemConfig()->current_profile_index > MAX_PROFILE_COUNT - 1) {// sanity check
systemConfig()->current_profile_index = 0; systemConfigMutable()->current_profile_index = 0;
} }
setProfile(systemConfig()->current_profile_index); setProfile(systemConfig()->current_profile_index);
@ -1313,7 +1358,7 @@ void changeProfile(uint8_t profileIndex)
if (profileIndex >= MAX_PROFILE_COUNT) { if (profileIndex >= MAX_PROFILE_COUNT) {
profileIndex = MAX_PROFILE_COUNT - 1; profileIndex = MAX_PROFILE_COUNT - 1;
} }
systemConfig()->current_profile_index = profileIndex; systemConfigMutable()->current_profile_index = profileIndex;
writeEEPROM(); writeEEPROM();
readEEPROM(); readEEPROM();
beeperConfirmationBeeps(profileIndex + 1); beeperConfirmationBeeps(profileIndex + 1);

View File

@ -70,6 +70,14 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0);
PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
.gyro_cal_on_first_arm = 0, // TODO - Cleanup retarded arm support
.disarm_kill_switch = 1,
.auto_disarm_delay = 5
);
bool isAirmodeActive(void) { bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
} }

View File

@ -53,6 +53,17 @@
static failsafeState_t failsafeState; static failsafeState_t failsafeState;
PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_delay = 10, // 1sec
.failsafe_off_delay = 10, // 1sec
.failsafe_throttle = 1000, // default throttle off.
.failsafe_kill_switch = 0, // default failsafe switch action is identical to rc link loss
.failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT // default full failsafe procedure is 0: auto-landing
);
/* /*
* Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected.
*/ */

View File

@ -50,6 +50,25 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
PG_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig,
.deadband3d_low = 1406,
.deadband3d_high = 1514,
.neutral3d = 1460,
.deadband3d_throttle = 50
);
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
#ifndef TARGET_DEFAULT_MIXER
#define TARGET_DEFAULT_MIXER MIXER_QUADX
#endif
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.mixerMode = TARGET_DEFAULT_MIXER,
.yaw_motor_direction = 1,
);
#define EXTERNAL_DSHOT_CONVERSION_FACTOR 2 #define EXTERNAL_DSHOT_CONVERSION_FACTOR 2
// (minimum output value(1001) - (minimum input value(48) / conversion factor(2)) // (minimum output value(1001) - (minimum input value(48) / conversion factor(2))
#define EXTERNAL_DSHOT_CONVERSION_OFFSET 977 #define EXTERNAL_DSHOT_CONVERSION_OFFSET 977

View File

@ -99,6 +99,34 @@ const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 2500
#define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0])) #define BAUD_RATE_COUNT (sizeof(baudRates) / sizeof(baudRates[0]))
PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 0);
void pgResetFn_serialConfig(serialConfig_t *serialConfig)
{
memset(serialConfig, 0, sizeof(serialConfig_t));
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
serialConfig->portConfigs[i].identifier = serialPortIdentifiers[i];
serialConfig->portConfigs[i].msp_baudrateIndex = BAUD_115200;
serialConfig->portConfigs[i].gps_baudrateIndex = BAUD_38400;
serialConfig->portConfigs[i].telemetry_baudrateIndex = BAUD_AUTO;
serialConfig->portConfigs[i].blackbox_baudrateIndex = BAUD_115200;
}
serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
#ifdef USE_VCP
if (serialConfig->portConfigs[0].identifier == SERIAL_PORT_USB_VCP) {
serialPortConfig_t * uart1Config = serialFindPortConfiguration(SERIAL_PORT_USART1);
if (uart1Config) {
uart1Config->functionMask = FUNCTION_MSP;
}
}
#endif
serialConfig->reboot_character = 'R';
}
baudRate_e lookupBaudRateIndex(uint32_t baudRate) baudRate_e lookupBaudRateIndex(uint32_t baudRate)
{ {
uint8_t index; uint8_t index;

View File

@ -33,6 +33,9 @@
static bool standardBoardAlignment = true; // board orientation correction static bool standardBoardAlignment = true; // board orientation correction
static float boardRotation[3][3]; // matrix static float boardRotation[3][3]; // matrix
// no template required since defaults are zero
PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment) static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
{ {
return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees; return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees;

View File

@ -54,6 +54,29 @@
#include "telemetry/ibus.h" #include "telemetry/ibus.h"
PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
#if defined(STM32F3)
#define TELEMETRY_DEFAULT_INVERSION 1
#else
#define TELEMETRY_DEFAULT_INVERSION 0
#endif
PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
.telemetry_inversion = TELEMETRY_DEFAULT_INVERSION,
.sportHalfDuplex = 1,
.telemetry_switch = 0,
.gpsNoFixLatitude = 0,
.gpsNoFixLongitude = 0,
.frsky_coordinate_format = FRSKY_FORMAT_DMS,
.frsky_unit = FRSKY_UNIT_METRICS,
.frsky_vfas_precision = 0,
.frsky_vfas_cell_voltage = 0,
.hottAlarmSoundInterval = 5,
.pidValuesAsTelemetry = 0,
.report_cell_voltage = false
);
void telemetryInit(void) void telemetryInit(void)
{ {
#ifdef TELEMETRY_FRSKY #ifdef TELEMETRY_FRSKY