Merge pull request #2424 from martinbudden/bf_pg_preparation10
Preparation for conversion to parameter groups 10
This commit is contained in:
commit
367f62236a
|
@ -197,7 +197,6 @@ typedef struct master_s {
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
servoConfig_t servoConfig;
|
servoConfig_t servoConfig;
|
||||||
servoMixerConfig_t servoMixerConfig;
|
|
||||||
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
servoMixer_t customServoMixer[MAX_SERVO_RULES];
|
||||||
// Servo-related stuff
|
// Servo-related stuff
|
||||||
servoProfile_t servoProfile;
|
servoProfile_t servoProfile;
|
||||||
|
|
|
@ -26,6 +26,9 @@
|
||||||
#include "timer.h"
|
#include "timer.h"
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
|
||||||
|
#include "flight/mixer.h" //!!TODO remove dependency on this
|
||||||
|
#include "flight/servos.h" //!!TODO remove dependency on this
|
||||||
|
|
||||||
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
|
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
|
||||||
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
|
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
|
||||||
|
|
||||||
|
|
|
@ -17,8 +17,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "io/motors.h"
|
|
||||||
#include "io/servos.h"
|
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/dma.h"
|
#include "drivers/dma.h"
|
||||||
|
|
||||||
|
@ -111,8 +109,10 @@ typedef struct {
|
||||||
IO_t io;
|
IO_t io;
|
||||||
} pwmOutputPort_t;
|
} pwmOutputPort_t;
|
||||||
|
|
||||||
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount);
|
struct motorConfig_s;
|
||||||
void servoInit(const servoConfig_t *servoConfig);
|
void motorInit(const struct motorConfig_s *motorConfig, uint16_t idlePulse, uint8_t motorCount);
|
||||||
|
struct servoConfig_s;
|
||||||
|
void servoInit(const struct servoConfig_s *servoConfig);
|
||||||
|
|
||||||
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
||||||
|
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
|
@ -30,7 +32,7 @@
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "rcc.h"
|
#include "rcc.h"
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#include "flight/mixer.h" //!!TODO remove dependency on this
|
||||||
|
|
||||||
static uint8_t dmaMotorTimerCount = 0;
|
static uint8_t dmaMotorTimerCount = 0;
|
||||||
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT
|
||||||
|
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
#include "timer.h"
|
#include "timer.h"
|
||||||
#include "timer_stm32f4xx.h"
|
#include "timer_stm32f4xx.h"
|
||||||
|
@ -29,7 +31,7 @@
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "rcc.h"
|
#include "rcc.h"
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#include "flight/mixer.h" //!!TODO remove dependency on this
|
||||||
|
|
||||||
static uint8_t dmaMotorTimerCount = 0;
|
static uint8_t dmaMotorTimerCount = 0;
|
||||||
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#ifdef USE_DSHOT
|
||||||
|
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
#include "timer.h"
|
#include "timer.h"
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
@ -28,7 +30,7 @@
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "rcc.h"
|
#include "rcc.h"
|
||||||
|
|
||||||
#ifdef USE_DSHOT
|
#include "flight/mixer.h" //!!TODO remove dependency on this
|
||||||
|
|
||||||
static uint8_t dmaMotorTimerCount = 0;
|
static uint8_t dmaMotorTimerCount = 0;
|
||||||
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
|
||||||
|
|
|
@ -36,6 +36,8 @@
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
#include "rx_pwm.h"
|
#include "rx_pwm.h"
|
||||||
|
|
||||||
|
#include "flight/mixer.h" //!!TODO remove dependency on this
|
||||||
|
|
||||||
#define DEBUG_PPM_ISR
|
#define DEBUG_PPM_ISR
|
||||||
|
|
||||||
#define PPM_CAPTURE_COUNT 12
|
#define PPM_CAPTURE_COUNT 12
|
||||||
|
|
|
@ -659,9 +659,8 @@ static const clivalue_t valueTable[] = {
|
||||||
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
|
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
|
||||||
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
|
||||||
{ "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoMixerConfig()->servo_lowpass_freq, .config.minmax = { 10, 400} },
|
{ "servo_lowpass_hz", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servo_lowpass_freq, .config.minmax = { 0, 400} },
|
||||||
{ "servo_lowpass", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } },
|
|
||||||
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } },
|
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoPwmRate, .config.minmax = { 50, 498 } },
|
||||||
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
{ "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gimbalConfig()->mode, .config.lookup = { TABLE_GIMBAL_MODE } },
|
||||||
{ "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &channelForwardingConfig()->startChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
|
{ "channel_forwarding_start", VAR_UINT8 | MASTER_VALUE, &channelForwardingConfig()->startChannel, .config.minmax = { AUX1, MAX_SUPPORTED_RC_CHANNEL_COUNT } },
|
||||||
|
|
|
@ -243,6 +243,8 @@ void resetServoConfig(servoConfig_t *servoConfig)
|
||||||
{
|
{
|
||||||
servoConfig->servoCenterPulse = 1500;
|
servoConfig->servoCenterPulse = 1500;
|
||||||
servoConfig->servoPwmRate = 50;
|
servoConfig->servoPwmRate = 50;
|
||||||
|
servoConfig->tri_unarmed_servo = 1;
|
||||||
|
servoConfig->servo_lowpass_freq = 0;
|
||||||
|
|
||||||
int servoIndex = 0;
|
int servoIndex = 0;
|
||||||
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) {
|
for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && servoIndex < MAX_SUPPORTED_SERVOS; i++) {
|
||||||
|
@ -477,15 +479,6 @@ void resetMixerConfig(mixerConfig_t *mixerConfig)
|
||||||
mixerConfig->yaw_motor_direction = 1;
|
mixerConfig->yaw_motor_direction = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig)
|
|
||||||
{
|
|
||||||
servoMixerConfig->tri_unarmed_servo = 1;
|
|
||||||
servoMixerConfig->servo_lowpass_freq = 400;
|
|
||||||
servoMixerConfig->servo_lowpass_enable = 0;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_MAX7456
|
#ifdef USE_MAX7456
|
||||||
void resetMax7456Config(vcdProfile_t *pVcdProfile)
|
void resetMax7456Config(vcdProfile_t *pVcdProfile)
|
||||||
{
|
{
|
||||||
|
@ -732,7 +725,6 @@ void createDefaultConfig(master_t *config)
|
||||||
resetMixerConfig(&config->mixerConfig);
|
resetMixerConfig(&config->mixerConfig);
|
||||||
resetMotorConfig(&config->motorConfig);
|
resetMotorConfig(&config->motorConfig);
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
resetServoMixerConfig(&config->servoMixerConfig);
|
|
||||||
resetServoConfig(&config->servoConfig);
|
resetServoConfig(&config->servoConfig);
|
||||||
#endif
|
#endif
|
||||||
resetFlight3DConfig(&config->flight3DConfig);
|
resetFlight3DConfig(&config->flight3DConfig);
|
||||||
|
@ -904,7 +896,7 @@ void activateConfig(void)
|
||||||
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
|
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig);
|
servoUseConfigs(masterConfig.servoProfile.servoConf, &masterConfig.channelForwardingConfig);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
|
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
|
||||||
|
|
|
@ -498,7 +498,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
||||||
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
|
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoMixerConfig()->tri_unarmed_servo)
|
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
|
||||||
#endif
|
#endif
|
||||||
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
|
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
|
||||||
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
||||||
|
@ -558,8 +558,6 @@ static void subTaskMotorUpdate(void)
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
|
// motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
|
||||||
if (isMixerUsingServos()) {
|
if (isMixerUsingServos()) {
|
||||||
servoTable();
|
|
||||||
filterServos();
|
|
||||||
writeServos();
|
writeServos();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -102,24 +102,25 @@
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/sonar.h"
|
|
||||||
#include "sensors/barometer.h"
|
|
||||||
#include "sensors/compass.h"
|
|
||||||
#include "sensors/acceleration.h"
|
#include "sensors/acceleration.h"
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/barometer.h"
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
|
#include "sensors/compass.h"
|
||||||
|
#include "sensors/esc_sensor.h"
|
||||||
|
#include "sensors/gyro.h"
|
||||||
#include "sensors/initialisation.h"
|
#include "sensors/initialisation.h"
|
||||||
|
#include "sensors/sensors.h"
|
||||||
|
#include "sensors/sonar.h"
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
#include "telemetry/telemetry.h"
|
||||||
#include "sensors/esc_sensor.h"
|
|
||||||
|
|
||||||
#include "flight/pid.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/failsafe.h"
|
|
||||||
#include "flight/navigation.h"
|
#include "flight/navigation.h"
|
||||||
|
#include "flight/pid.h"
|
||||||
|
#include "flight/servos.h"
|
||||||
|
|
||||||
|
|
||||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||||
|
|
|
@ -38,11 +38,11 @@
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
#include "io/gimbal.h"
|
#include "io/gimbal.h"
|
||||||
#include "io/servos.h"
|
|
||||||
|
|
||||||
|
#include "flight/imu.h"
|
||||||
#include "flight/mixer.h"
|
#include "flight/mixer.h"
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/imu.h"
|
#include "flight/servos.h"
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
#include "fc/rc_controls.h"
|
#include "fc/rc_controls.h"
|
||||||
|
@ -52,8 +52,6 @@
|
||||||
|
|
||||||
extern mixerMode_e currentMixerMode;
|
extern mixerMode_e currentMixerMode;
|
||||||
|
|
||||||
static servoMixerConfig_t *servoMixerConfig;
|
|
||||||
|
|
||||||
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];
|
||||||
|
@ -113,6 +111,12 @@ static const servoMixer_t servoMixerGimbal[] = {
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// Custom mixer configuration
|
||||||
|
typedef struct mixerRules_s {
|
||||||
|
uint8_t servoRuleCount;
|
||||||
|
const servoMixer_t *rule;
|
||||||
|
} mixerRules_t;
|
||||||
|
|
||||||
const mixerRules_t servoMixers[] = {
|
const mixerRules_t servoMixers[] = {
|
||||||
{ 0, NULL }, // entry 0
|
{ 0, NULL }, // entry 0
|
||||||
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
|
{ COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI
|
||||||
|
@ -145,9 +149,8 @@ const mixerRules_t servoMixers[] = {
|
||||||
|
|
||||||
static const servoMixer_t *customServoMixers;
|
static const servoMixer_t *customServoMixers;
|
||||||
|
|
||||||
void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
|
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse)
|
||||||
{
|
{
|
||||||
servoMixerConfig = servoMixerConfigToUse;
|
|
||||||
servoConf = servoParamsToUse;
|
servoConf = servoParamsToUse;
|
||||||
channelForwardingConfig = channelForwardingConfigToUse;
|
channelForwardingConfig = channelForwardingConfigToUse;
|
||||||
}
|
}
|
||||||
|
@ -267,10 +270,15 @@ static void updateGimbalServos(uint8_t firstServoIndex)
|
||||||
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
|
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void servoTable(void);
|
||||||
|
static void filterServos(void);
|
||||||
|
|
||||||
void writeServos(void)
|
void writeServos(void)
|
||||||
{
|
{
|
||||||
uint8_t servoIndex = 0;
|
servoTable();
|
||||||
|
filterServos();
|
||||||
|
|
||||||
|
uint8_t servoIndex = 0;
|
||||||
switch (currentMixerMode) {
|
switch (currentMixerMode) {
|
||||||
case MIXER_BICOPTER:
|
case MIXER_BICOPTER:
|
||||||
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
|
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
|
||||||
|
@ -279,7 +287,7 @@ void writeServos(void)
|
||||||
|
|
||||||
case MIXER_TRI:
|
case MIXER_TRI:
|
||||||
case MIXER_CUSTOM_TRI:
|
case MIXER_CUSTOM_TRI:
|
||||||
if (servoMixerConfig()->tri_unarmed_servo) {
|
if (servoConfig()->tri_unarmed_servo) {
|
||||||
// if unarmed flag set, we always move servo
|
// if unarmed flag set, we always move servo
|
||||||
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||||
} else {
|
} else {
|
||||||
|
@ -409,7 +417,7 @@ STATIC_UNIT_TESTED void servoMixer(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void servoTable(void)
|
static void servoTable(void)
|
||||||
{
|
{
|
||||||
// airplane / servo mixes
|
// airplane / servo mixes
|
||||||
switch (currentMixerMode) {
|
switch (currentMixerMode) {
|
||||||
|
@ -464,7 +472,7 @@ bool isMixerUsingServos(void)
|
||||||
return useServo;
|
return useServo;
|
||||||
}
|
}
|
||||||
|
|
||||||
void filterServos(void)
|
static void filterServos(void)
|
||||||
{
|
{
|
||||||
static int16_t servoIdx;
|
static int16_t servoIdx;
|
||||||
static bool servoFilterIsSet;
|
static bool servoFilterIsSet;
|
||||||
|
@ -474,10 +482,10 @@ void filterServos(void)
|
||||||
uint32_t startTime = micros();
|
uint32_t startTime = micros();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (servoMixerConfig()->servo_lowpass_enable) {
|
if (servoConfig()->servo_lowpass_freq) {
|
||||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||||
if (!servoFilterIsSet) {
|
if (!servoFilterIsSet) {
|
||||||
biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig()->servo_lowpass_freq, targetPidLooptime);
|
biquadFilterInitLPF(&servoFilter[servoIdx], servoConfig()->servo_lowpass_freq, targetPidLooptime);
|
||||||
servoFilterIsSet = true;
|
servoFilterIsSet = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -18,6 +18,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
|
#include "drivers/io_types.h"
|
||||||
|
|
||||||
#define MAX_SUPPORTED_SERVOS 8
|
#define MAX_SUPPORTED_SERVOS 8
|
||||||
|
|
||||||
|
@ -91,12 +92,6 @@ typedef struct servoMixer_s {
|
||||||
|
|
||||||
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
|
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);
|
||||||
|
|
||||||
// Custom mixer configuration
|
|
||||||
typedef struct mixerRules_s {
|
|
||||||
uint8_t servoRuleCount;
|
|
||||||
const servoMixer_t *rule;
|
|
||||||
} mixerRules_t;
|
|
||||||
|
|
||||||
typedef struct servoParam_s {
|
typedef struct servoParam_s {
|
||||||
uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted
|
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 min; // servo min
|
||||||
|
@ -110,13 +105,16 @@ typedef struct servoParam_s {
|
||||||
|
|
||||||
PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
|
PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
|
||||||
|
|
||||||
typedef struct servoMixerConfig_s{
|
typedef struct servoConfig_s {
|
||||||
|
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
|
||||||
|
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
|
||||||
|
uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz)
|
||||||
|
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
||||||
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
|
||||||
uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
|
ioTag_t ioTags[MAX_SUPPORTED_SERVOS];
|
||||||
int8_t servo_lowpass_enable; // enable/disable lowpass filter
|
} servoConfig_t;
|
||||||
} servoMixerConfig_t;
|
|
||||||
|
|
||||||
//!!TODO PG_DECLARE(servoConfig_t, servoConfig);
|
PG_DECLARE(servoConfig_t, servoConfig);
|
||||||
|
|
||||||
typedef struct servoProfile_s {
|
typedef struct servoProfile_s {
|
||||||
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
|
servoParam_t servoConf[MAX_SUPPORTED_SERVOS];
|
||||||
|
@ -128,13 +126,11 @@ typedef struct channelForwardingConfig_s {
|
||||||
|
|
||||||
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
||||||
|
|
||||||
void servoTable(void);
|
|
||||||
bool isMixerUsingServos(void);
|
bool isMixerUsingServos(void);
|
||||||
void writeServos(void);
|
void writeServos(void);
|
||||||
void filterServos(void);
|
|
||||||
|
|
||||||
void servoMixerInit(const servoMixer_t *customServoMixers);
|
void servoMixerInit(const servoMixer_t *customServoMixers);
|
||||||
void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
|
void servoUseConfigs(servoParam_t *servoParamsToUse, struct channelForwardingConfig_s *channelForwardingConfigToUse);
|
||||||
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
void servoMixerLoadMix(int index, servoMixer_t *customServoMixers);
|
||||||
void loadCustomServoMixer(void);
|
void loadCustomServoMixer(void);
|
||||||
void servoConfigureOutput(void);
|
void servoConfigureOutput(void);
|
||||||
|
|
|
@ -17,14 +17,3 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "drivers/io_types.h"
|
|
||||||
#include "flight/servos.h"
|
|
||||||
|
|
||||||
typedef struct servoConfig_s {
|
|
||||||
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
|
|
||||||
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
|
|
||||||
uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz)
|
|
||||||
ioTag_t ioTags[MAX_SUPPORTED_SERVOS];
|
|
||||||
} servoConfig_t;
|
|
||||||
|
|
||||||
PG_DECLARE(servoConfig_t, servoConfig);
|
|
||||||
|
|
Loading…
Reference in New Issue