Added PG config definitions 5
This commit is contained in:
parent
9515088a98
commit
df14abc5c2
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue