Added PG config definitions 5

This commit is contained in:
Martin Budden 2017-02-26 06:29:55 +00:00
parent 9515088a98
commit df14abc5c2
5 changed files with 159 additions and 120 deletions

View File

@ -66,6 +66,23 @@
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/sonar.h" #include "sensors/sonar.h"
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_FLASH
#elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT)
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SDCARD
#else
#define DEFAULT_BLACKBOX_DEVICE BLACKBOX_DEVICE_SERIAL
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE,
.rate_num = 1,
.rate_denom = 1,
.on_motor_test = 0 // default off
);
#define BLACKBOX_I_INTERVAL 32 #define BLACKBOX_I_INTERVAL 32
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
#define SLOW_FRAME_INTERVAL 4096 #define SLOW_FRAME_INTERVAL 4096

View File

@ -235,8 +235,8 @@ void resetGpsProfile(gpsProfile_t *gpsProfile)
} }
#endif #endif
#ifdef BARO
#ifndef USE_PARAMETER_GROUPS #ifndef USE_PARAMETER_GROUPS
#ifdef BARO
void resetBarometerConfig(barometerConfig_t *barometerConfig) void resetBarometerConfig(barometerConfig_t *barometerConfig)
{ {
barometerConfig->baro_sample_count = 21; barometerConfig->baro_sample_count = 21;
@ -245,7 +245,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig)
barometerConfig->baro_cf_alt = 0.965f; barometerConfig->baro_cf_alt = 0.965f;
} }
#endif #endif
#endif
#ifdef LED_STRIP #ifdef LED_STRIP
void resetLedStripConfig(ledStripConfig_t *ledStripConfig) void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
@ -266,7 +265,9 @@ void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
ledStripConfig->ioTag = IO_TAG_NONE; ledStripConfig->ioTag = IO_TAG_NONE;
} }
#endif #endif
#endif
#ifndef USE_PARAMETER_GROUPS
#ifdef USE_SERVOS #ifdef USE_SERVOS
void resetServoConfig(servoConfig_t *servoConfig) void resetServoConfig(servoConfig_t *servoConfig)
{ {
@ -285,7 +286,6 @@ void resetServoConfig(servoConfig_t *servoConfig)
} }
#endif #endif
#ifndef USE_PARAMETER_GROUPS
void resetMotorConfig(motorConfig_t *motorConfig) void resetMotorConfig(motorConfig_t *motorConfig)
{ {
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
@ -961,21 +961,19 @@ void createDefaultConfig(master_t *config)
config->airplaneConfig.fixedwing_althold_dir = 1; config->airplaneConfig.fixedwing_althold_dir = 1;
// Motor/ESC/Servo
#ifndef USE_PARAMETER_GROUPS #ifndef USE_PARAMETER_GROUPS
// Motor/ESC/Servo
resetMixerConfig(&config->mixerConfig); resetMixerConfig(&config->mixerConfig);
resetMotorConfig(&config->motorConfig); resetMotorConfig(&config->motorConfig);
#endif
#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);
#endif #endif
#endif
#ifdef GPS #ifdef GPS
// gps/nav stuff // gps/nav stuff
@ -1030,6 +1028,7 @@ void createDefaultConfig(master_t *config)
#endif #endif
#ifdef USE_SERVOS #ifdef USE_SERVOS
#ifndef USE_PARAMETER_GROUPS
// servos // servos
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
config->servoProfile.servoConf[i].min = DEFAULT_SERVO_MIN; config->servoProfile.servoConf[i].min = DEFAULT_SERVO_MIN;
@ -1040,6 +1039,7 @@ void createDefaultConfig(master_t *config)
config->servoProfile.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; config->servoProfile.servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
config->servoProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; config->servoProfile.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
} }
#endif
// gimbal // gimbal
config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; config->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
@ -1070,6 +1070,7 @@ void createDefaultConfig(master_t *config)
memcpy(config->transponderConfig.data, &defaultTransponderData, sizeof(defaultTransponderData)); memcpy(config->transponderConfig.data, &defaultTransponderData, sizeof(defaultTransponderData));
#endif #endif
#ifndef USE_PARAMETER_GROUPS
#ifdef BLACKBOX #ifdef BLACKBOX
#if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT)
intFeatureSet(FEATURE_BLACKBOX, featuresPtr); intFeatureSet(FEATURE_BLACKBOX, featuresPtr);
@ -1080,11 +1081,11 @@ void createDefaultConfig(master_t *config)
#else #else
config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL;
#endif #endif
config->blackboxConfig.rate_num = 1; config->blackboxConfig.rate_num = 1;
config->blackboxConfig.rate_denom = 1; config->blackboxConfig.rate_denom = 1;
config->blackboxConfig.on_motor_test = 0; // default off config->blackboxConfig.on_motor_test = 0; // default off
#endif // BLACKBOX #endif // BLACKBOX
#endif
#ifdef SERIALRX_UART #ifdef SERIALRX_UART
if (featureConfigured(FEATURE_RX_SERIAL)) { if (featureConfigured(FEATURE_RX_SERIAL)) {
@ -1143,7 +1144,7 @@ void activateConfig(void)
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
#ifdef USE_SERVOS #ifdef USE_SERVOS
servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig); servoUseConfigs(&masterConfig.channelForwardingConfig);
#endif #endif
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);

View File

@ -31,6 +31,7 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "config/config_reset.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "drivers/system.h" #include "drivers/system.h"
@ -52,11 +53,47 @@
extern mixerMode_e currentMixerMode; extern mixerMode_e currentMixerMode;
PG_REGISTER_WITH_RESET_FN(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
void pgResetFn_servoConfig(servoConfig_t *servoConfig)
{
servoConfig->dev.servoCenterPulse = 1500;
servoConfig->dev.servoPwmRate = 50;
servoConfig->tri_unarmed_servo = 1;
servoConfig->servo_lowpass_freq = 0;
int servoIndex = 0;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) {
if (timerHardware[i].usageFlags & TIM_USE_SERVO) {
servoConfig->dev.ioTags[servoIndex] = timerHardware[i].tag;
servoIndex++;
}
}
}
PG_REGISTER_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 0);
PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 0);
void pgResetFn_servoParams(servoParam_t *instance)
{
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
RESET_CONFIG(const servoParam_t, &instance[i],
.min = DEFAULT_SERVO_MIN,
.max = DEFAULT_SERVO_MAX,
.middle = DEFAULT_SERVO_MIDDLE,
.rate = 100,
.angleAtMin = DEFAULT_SERVO_MIN_ANGLE,
.angleAtMax = DEFAULT_SERVO_MAX_ANGLE,
.forwardFromChannel = CHANNEL_FORWARDING_DISABLED
);
}
}
static uint8_t servoRuleCount = 0; static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
int16_t servo[MAX_SUPPORTED_SERVOS]; int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo; static int useServo;
static servoParam_t *servoConf;
static channelForwardingConfig_t *channelForwardingConfig; static channelForwardingConfig_t *channelForwardingConfig;
@ -147,27 +184,26 @@ const mixerRules_t servoMixers[] = {
{ 0, NULL }, { 0, NULL },
}; };
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse) void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse)
{ {
servoConf = servoParamsToUse;
channelForwardingConfig = channelForwardingConfigToUse; channelForwardingConfig = channelForwardingConfigToUse;
} }
int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
{ {
const uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; const uint8_t channelToForwardFrom = servoParams(servoIndex)->forwardFromChannel;
if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) {
return rcData[channelToForwardFrom]; return rcData[channelToForwardFrom];
} }
return servoConf[servoIndex].middle; return servoParams(servoIndex)->middle;
} }
int servoDirection(int servoIndex, int inputSource) int servoDirection(int servoIndex, int inputSource)
{ {
// determine the direction (reversed or not) from the direction bitfield of the servo // determine the direction (reversed or not) from the direction bitfield of the servo
if (servoConf[servoIndex].reversedSources & (1 << inputSource)) if (servoParams(servoIndex)->reversedSources & (1 << inputSource))
return -1; return -1;
else else
return 1; return 1;
@ -386,7 +422,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) {
uint8_t target = currentServoMixer[i].targetChannel; uint8_t target = currentServoMixer[i].targetChannel;
uint8_t from = currentServoMixer[i].inputSource; uint8_t from = currentServoMixer[i].inputSource;
uint16_t servo_width = servoConf[target].max - servoConf[target].min; uint16_t servo_width = servoParams(target)->max - servoParams(target)->min;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
@ -406,7 +442,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
} }
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; servo[i] = ((int32_t)servoParams(i)->rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(i); servo[i] += determineServoMiddleOrForwardFromChannel(i);
} }
} }
@ -430,8 +466,8 @@ static void servoTable(void)
/* /*
case MIXER_GIMBAL: case MIXER_GIMBAL:
servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH);
servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL);
break; break;
*/ */
@ -447,18 +483,18 @@ static void servoTable(void)
if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { 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_PITCH] -= (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 - (int32_t)servoParams(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; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 + (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50;
} else { } else {
servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_PITCH] += (int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch / 50;
servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50;
} }
} }
} }
// constrain servos // constrain servos
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); // limit the values
} }
} }
@ -486,7 +522,7 @@ static void filterServos(void)
servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
// Sanity check // Sanity check
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
} }
} }
#if defined(MIXER_DEBUG) #if defined(MIXER_DEBUG)

View File

@ -126,7 +126,7 @@ bool isMixerUsingServos(void);
void writeServos(void); void writeServos(void);
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
void loadCustomServoMixer(void); void loadCustomServoMixer(void);
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse); void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse);
int servoDirection(int servoIndex, int fromChannel); int servoDirection(int servoIndex, int fromChannel);
void servoConfigureOutput(void); void servoConfigureOutput(void);
void servosInit(void); void servosInit(void);

View File

@ -27,24 +27,11 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "common/axis.h"
#include "common/color.h" #include "common/color.h"
#include "common/maths.h" #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"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/rx_pwm.h"
#include "common/printf.h" #include "common/printf.h"
#include "common/axis.h" #include "common/typeconversion.h"
#include "common/utils.h" #include "common/utils.h"
#include "config/config_profile.h" #include "config/config_profile.h"
@ -52,41 +39,39 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "drivers/light_ws2811strip.h"
#include "drivers/serial.h"
#include "drivers/system.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "sensors/battery.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "io/ledstrip.h"
#include "io/beeper.h"
#include "io/motors.h"
#include "io/servos.h"
#include "io/gimbal.h"
#include "io/serial.h"
#include "io/gps.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/gimbal.h"
#include "io/gps.h"
#include "io/ledstrip.h"
#include "io/serial.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
/* PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0);
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);
PG_REGISTER_ARR_WITH_RESET_FN(modeColorIndexes_t, LED_MODE_COUNT, modeColors, PG_MODE_COLOR_CONFIG, 0);
PG_REGISTER_WITH_RESET_FN(specialColorIndexes_t, specialColors, PG_SPECIAL_COLOR_CONFIG, 0);
*/
static bool ledStripInitialised = false; static bool ledStripInitialised = false;
static bool ledStripEnabled = true; static bool ledStripEnabled = true;
@ -171,6 +156,57 @@ static const specialColorIndexes_t defaultSpecialColors[] = {
}} }}
}; };
#ifndef USE_PARAMETER_GROUPS
void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
{
memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t));
}
void applyDefaultColors(hsvColor_t *colors)
{
// copy hsv colors as default
memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
*colors++ = hsv[colorIndex];
}
}
void applyDefaultModeColors(modeColorIndexes_t *modeColors)
{
memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors));
}
void applyDefaultSpecialColors(specialColorIndexes_t *specialColors)
{
memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
}
#else
void pgResetFn_ledStripConfig(ledStripConfig_t *ledStripConfig)
{
memset(ledStripConfig->ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t));
// copy hsv colors as default
memset(ledStripConfig->colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
BUILD_BUG_ON(LED_CONFIGURABLE_COLOR_COUNT < ARRAYLEN(hsv));
for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
ledStripConfig->colors[colorIndex] = hsv[colorIndex];
}
memcpy_fn(&ledStripConfig->modeColors, &defaultModeColors, sizeof(defaultModeColors));
memcpy_fn(&ledStripConfig->specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
ledStripConfig->ledstrip_visual_beeper = 0;
ledStripConfig->ledstrip_aux_channel = THROTTLE;
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
if (timerHardware[i].usageFlags & TIM_USE_LED) {
ledStripConfig->ioTag = timerHardware[i].tag;
return;
}
}
ledStripConfig->ioTag = IO_TAG_NONE;
}
#endif
static int scaledThrottle; static int scaledThrottle;
static int scaledAux; static int scaledAux;
@ -245,9 +281,9 @@ void reevaluateLedConfig(void)
} }
// get specialColor by index // get specialColor by index
static hsvColor_t* getSC(ledSpecialColorIds_e index) static const hsvColor_t* getSC(ledSpecialColorIds_e index)
{ {
return &ledStripConfigMutable()->colors[ledStripConfig()->specialColors.color[index]]; return &ledStripConfig()->colors[ledStripConfig()->specialColors.color[index]];
} }
static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' }; static const char directionCodes[LED_DIRECTION_COUNT] = { 'N', 'E', 'S', 'W', 'U', 'D' };
@ -449,7 +485,7 @@ static void applyLedFixedLayers()
case LED_FUNCTION_FLIGHT_MODE: case LED_FUNCTION_FLIGHT_MODE:
for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++)
if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) { if (!flightModeToLed[i].flightMode || FLIGHT_MODE(flightModeToLed[i].flightMode)) {
hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]); const hsvColor_t *directionalColor = getDirectionalModeColor(ledIndex, &ledStripConfig()->modeColors[flightModeToLed[i].ledMode]);
if (directionalColor) { if (directionalColor) {
color = *directionalColor; color = *directionalColor;
} }
@ -578,7 +614,7 @@ static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer)
*timer += timerDelayUs; *timer += timerDelayUs;
if (!flash) { if (!flash) {
hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc); applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_BATTERY), bgc);
} }
} }
@ -607,7 +643,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer)
*timer += timerDelay; *timer += timerDelay;
if (!flash) { if (!flash) {
hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND); const hsvColor_t *bgc = getSC(LED_SCOLOR_BACKGROUND);
applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc); applyLedHsv(LED_MOV_FUNCTION(LED_FUNCTION_RSSI), bgc);
} }
} }
@ -1032,57 +1068,6 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex)
return true; return true;
} }
/*
void pgResetFn_ledConfigs(ledConfig_t *instance)
{
memcpy_fn(instance, &defaultLedStripConfig, sizeof(defaultLedStripConfig));
}
void pgResetFn_colors(hsvColor_t *instance)
{
// copy hsv colors as default
BUILD_BUG_ON(ARRAYLEN(*colors_arr()) < ARRAYLEN(hsv));
for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
*instance++ = hsv[colorIndex];
}
}
void pgResetFn_modeColors(modeColorIndexes_t *instance)
{
memcpy_fn(instance, &defaultModeColors, sizeof(defaultModeColors));
}
void pgResetFn_specialColors(specialColorIndexes_t *instance)
{
memcpy_fn(instance, &defaultSpecialColors, sizeof(defaultSpecialColors));
}
*/
void applyDefaultLedStripConfig(ledConfig_t *ledConfigs)
{
memset(ledConfigs, 0, LED_MAX_STRIP_LENGTH * sizeof(ledConfig_t));
}
void applyDefaultColors(hsvColor_t *colors)
{
// copy hsv colors as default
memset(colors, 0, ARRAYLEN(hsv) * sizeof(hsvColor_t));
for (unsigned colorIndex = 0; colorIndex < ARRAYLEN(hsv); colorIndex++) {
*colors++ = hsv[colorIndex];
}
}
void applyDefaultModeColors(modeColorIndexes_t *modeColors)
{
memcpy_fn(modeColors, &defaultModeColors, sizeof(defaultModeColors));
}
void applyDefaultSpecialColors(specialColorIndexes_t *specialColors)
{
memcpy_fn(specialColors, &defaultSpecialColors, sizeof(defaultSpecialColors));
}
void ledStripInit() void ledStripInit()
{ {
colors = ledStripConfigMutable()->colors; colors = ledStripConfigMutable()->colors;