diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index eb7cd7f81..fc6932899 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -66,6 +66,23 @@ #include "sensors/gyro.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_SHUTDOWN_TIMEOUT_MILLIS 200 #define SLOW_FRAME_INTERVAL 4096 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1bdba3e79..d61327824 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -235,8 +235,8 @@ void resetGpsProfile(gpsProfile_t *gpsProfile) } #endif -#ifdef BARO #ifndef USE_PARAMETER_GROUPS +#ifdef BARO void resetBarometerConfig(barometerConfig_t *barometerConfig) { barometerConfig->baro_sample_count = 21; @@ -245,7 +245,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig) barometerConfig->baro_cf_alt = 0.965f; } #endif -#endif #ifdef LED_STRIP void resetLedStripConfig(ledStripConfig_t *ledStripConfig) @@ -266,7 +265,9 @@ void resetLedStripConfig(ledStripConfig_t *ledStripConfig) ledStripConfig->ioTag = IO_TAG_NONE; } #endif +#endif +#ifndef USE_PARAMETER_GROUPS #ifdef USE_SERVOS void resetServoConfig(servoConfig_t *servoConfig) { @@ -285,7 +286,6 @@ void resetServoConfig(servoConfig_t *servoConfig) } #endif -#ifndef USE_PARAMETER_GROUPS void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS @@ -961,21 +961,19 @@ void createDefaultConfig(master_t *config) config->airplaneConfig.fixedwing_althold_dir = 1; - // Motor/ESC/Servo #ifndef USE_PARAMETER_GROUPS + // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); -#endif #ifdef USE_SERVOS resetServoConfig(&config->servoConfig); #endif -#ifndef USE_PARAMETER_GROUPS resetFlight3DConfig(&config->flight3DConfig); -#endif #ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); #endif +#endif #ifdef GPS // gps/nav stuff @@ -1030,6 +1028,7 @@ void createDefaultConfig(master_t *config) #endif #ifdef USE_SERVOS +#ifndef USE_PARAMETER_GROUPS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { 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].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } +#endif // gimbal config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; @@ -1070,6 +1070,7 @@ void createDefaultConfig(master_t *config) memcpy(config->transponderConfig.data, &defaultTransponderData, sizeof(defaultTransponderData)); #endif +#ifndef USE_PARAMETER_GROUPS #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); @@ -1080,11 +1081,11 @@ void createDefaultConfig(master_t *config) #else config->blackboxConfig.device = BLACKBOX_DEVICE_SERIAL; #endif - config->blackboxConfig.rate_num = 1; config->blackboxConfig.rate_denom = 1; config->blackboxConfig.on_motor_test = 0; // default off #endif // BLACKBOX +#endif #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { @@ -1143,7 +1144,7 @@ void activateConfig(void) setAccelerationFilter(accelerometerConfig()->acc_lpf_hz); #ifdef USE_SERVOS - servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig); + servoUseConfigs(&masterConfig.channelForwardingConfig); #endif imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 73712a879..9c2f60fe7 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -31,6 +31,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "drivers/pwm_output.h" #include "drivers/system.h" @@ -52,11 +53,47 @@ 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 servoMixer_t currentServoMixer[MAX_SERVO_RULES]; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; -static servoParam_t *servoConf; static channelForwardingConfig_t *channelForwardingConfig; @@ -147,27 +184,26 @@ const mixerRules_t servoMixers[] = { { 0, NULL }, }; -void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse) +void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse) { - servoConf = servoParamsToUse; channelForwardingConfig = channelForwardingConfigToUse; } 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) { return rcData[channelToForwardFrom]; } - return servoConf[servoIndex].middle; + return servoParams(servoIndex)->middle; } int servoDirection(int servoIndex, int inputSource) { // 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; else 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)) { uint8_t target = currentServoMixer[i].targetChannel; 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 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++) { - servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; + servo[i] = ((int32_t)servoParams(i)->rate * servo[i]) / 100L; servo[i] += determineServoMiddleOrForwardFromChannel(i); } } @@ -430,8 +466,8 @@ static void servoTable(void) /* 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_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); + 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)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ @@ -447,18 +483,18 @@ static void servoTable(void) if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { 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; + 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)servoParams(SERVO_GIMBAL_PITCH)->rate) * attitude.values.pitch / 50 + (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; } else { - servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[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_PITCH] += (int32_t)servoParams(SERVO_GIMBAL_PITCH)->rate * attitude.values.pitch / 50; + servo[SERVO_GIMBAL_ROLL] += (int32_t)servoParams(SERVO_GIMBAL_ROLL)->rate * attitude.values.roll / 50; } } } // constrain servos 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])); // 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) diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 25db91ba7..f47df8b47 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -126,7 +126,7 @@ bool isMixerUsingServos(void); void writeServos(void); void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); void loadCustomServoMixer(void); -void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse); +void servoUseConfigs(struct channelForwardingConfig_s *channelForwardingConfigToUse); int servoDirection(int servoIndex, int fromChannel); void servoConfigureOutput(void); void servosInit(void); diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 37e989954..664ad6135 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -27,24 +27,11 @@ #include "build/build_config.h" +#include "common/axis.h" #include "common/color.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/axis.h" +#include "common/typeconversion.h" #include "common/utils.h" #include "config/config_profile.h" @@ -52,41 +39,39 @@ #include "config/parameter_group.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/rc_controls.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/mixer.h" -#include "flight/pid.h" #include "flight/imu.h" +#include "flight/mixer.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 "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" -/* -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); -*/ +PG_REGISTER_WITH_RESET_FN(ledStripConfig_t, ledStripConfig, PG_LED_STRIP_CONFIG, 0); static bool ledStripInitialised = false; 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 scaledAux; @@ -245,9 +281,9 @@ void reevaluateLedConfig(void) } // 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' }; @@ -449,7 +485,7 @@ static void applyLedFixedLayers() case LED_FUNCTION_FLIGHT_MODE: for (unsigned i = 0; i < ARRAYLEN(flightModeToLed); i++) 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) { color = *directionalColor; } @@ -578,7 +614,7 @@ static void applyLedBatteryLayer(bool updateNow, timeUs_t *timer) *timer += timerDelayUs; 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); } } @@ -607,7 +643,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer) *timer += timerDelay; 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); } } @@ -1032,57 +1068,6 @@ bool setModeColor(ledModeIndex_e modeIndex, int modeColorIndex, int colorIndex) 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() { colors = ledStripConfigMutable()->colors;