Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 14:55:21 +03:00
Code Issues Wiki Activity

Added PG config definitions 5

Browse source
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
Download patch file Download diff file Expand all files Collapse all files

68
src/main/flight/servos.c
View file

@ -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)