Skip to content

Commit

Permalink
Dynamic YAW output + motor acceleration correction
Browse files Browse the repository at this point in the history 
Yaw PID output bypasses the servo mixer and is passed directly to tricopter
mixer.

New CLI params:
tri_motor_acceleration (float) - Time in seconds it takes for motor to
accelerate from min_throttle to max_throttle.

tri_dynamic_yaw_maxthrottle - Dynamic yaw gain (0-500%) when motor is at
max_throttle

tri_dynamic_yaw_minthrottle - Dynamic yaw gain (0-500%) when motor is at
in_throttle

The gains in between are linearly interpolated.

tri_motor_acc_yaw_correction - The correction that is applied as expected
yaw DPS error when motor is decelerating or accelerating.

More detailed description will follow in release notes.
  • Loading branch information
@lkaino
lkaino committed May 17, 2016
1 parent bdaeec1 commit efec142
Show file tree
Hide file tree
Showing 11 changed files with 295 additions and 131 deletions.
17 changes: 12 additions & 5 deletions src/main/config/config.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;

static const uint8_t EEPROM_CONF_VERSION = 112;
static const uint8_t EEPROM_CONF_VERSION = 113;

void resetPidProfile(pidProfile_t *pidProfile)
{
Expand Down Expand Up @@ -300,10 +300,10 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcExpo8 = 85;
controlRateConfig->thrMid8 = 50;
controlRateConfig->dynThrPID = 0;
controlRateConfig->tpa_yaw_rate = 90;
controlRateConfig->rcYawExpo8 = 83;
controlRateConfig->tpa_breakpoint = 1500;
controlRateConfig->tpa_yaw_breakpoint = 1500;
controlRateConfig->tri_dynamic_yaw_minthrottle = 300; // 3x YAW gain at min throttle
controlRateConfig->tri_dynamic_yaw_maxthrottle = 100; // no reduction by default

controlRateConfig->rates[FD_PITCH] = 55;
controlRateConfig->rates[FD_ROLL] = 55;
Expand Down Expand Up @@ -335,8 +335,10 @@ static void resetMixerConfig(mixerConfig_t *mixerConfig) {
mixerConfig->tri_servo_feedback = TRI_SERVO_FB_RSSI;
#else
mixerConfig->tri_servo_feedback = TRI_SERVO_FB_VIRTUAL;
#endif
#endif
#endif //RCE
mixerConfig->tri_motor_acc_yaw_correction = 6;
mixerConfig->tri_motor_acceleration = 0.20f;
#endif //USE_SERVOS
}

uint8_t getCurrentProfile(void)
Expand Down Expand Up @@ -369,6 +371,11 @@ uint16_t getCurrentMinthrottle(void)
return masterConfig.escAndServoConfig.minthrottle;
}

uint16_t getCurrentMaxthrottle(void)
{
return masterConfig.escAndServoConfig.maxthrottle;
}

// Default settings
STATIC_UNIT_TESTED void resetConf(void)
{
Expand Down
1 change: 1 addition & 0 deletions src/main/config/config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,3 +75,4 @@ void changeControlRateProfile(uint8_t profileIndex);
bool canSoftwareSerialBeUsed(void);

uint16_t getCurrentMinthrottle(void);
uint16_t getCurrentMaxthrottle(void);
137 changes: 69 additions & 68 deletions src/main/flight/mixer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -691,80 +691,81 @@ STATIC_UNIT_TESTED void servoMixer(void)
static int16_t currentOutput[MAX_SERVO_RULES];
uint8_t i;

if (FLIGHT_MODE(PASSTHRU_MODE)) {
// Direct passthru from RX
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
input[INPUT_STABILIZED_YAW] = axisPID[YAW];

// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
if ((currentMixerMode == MIXER_TRI) || (currentMixerMode == MIXER_CUSTOM_TRI))
{
triServoMixer(axisPID[YAW]);
}

input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);

input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]

// center the RC input value around the RC middle value
// by subtracting the RC middle value from the RC input value, we get:
// data - middle = input
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;

for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;

// mix servos according to rules
for (i = 0; i < servoRuleCount; i++) {
// consider rule if no box assigned or box is active
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;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;

if (currentServoMixer[i].speed == 0)
currentOutput[i] = input[from];
else {
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}

servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
else
{
if (FLIGHT_MODE(PASSTHRU_MODE)) {
// Direct passthru from RX
input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL];
input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH];
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
currentOutput[i] = 0;
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = axisPID[ROLL];
input[INPUT_STABILIZED_PITCH] = axisPID[PITCH];
input[INPUT_STABILIZED_YAW] = axisPID[YAW];

// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) {
input[INPUT_STABILIZED_YAW] *= -1;
}
}
}

for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(i);
input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500);
input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500);

input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500]

// center the RC input value around the RC middle value
// by subtracting the RC middle value from the RC input value, we get:
// data - middle = input
// 2000 - 1500 = +500
// 1500 - 1500 = 0
// 1000 - 1500 = -500
input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc;
input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc;
input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc;
input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc;
input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc;
input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc;
input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc;
input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc;

for (i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servo[i] = 0;

// mix servos according to rules
for (i = 0; i < servoRuleCount; i++) {
// consider rule if no box assigned or box is active
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;
int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;

if (currentServoMixer[i].speed == 0)
currentOutput[i] = input[from];
else {
if (currentOutput[i] < input[from])
currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
else if (currentOutput[i] > input[from])
currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
}

}
servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);
} else {
currentOutput[i] = 0;
}
}

if (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI)
{
triServoMixer();
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L;
servo[i] += determineServoMiddleOrForwardFromChannel(i);
}
}
}
#endif
Expand Down
2 changes: 2 additions & 0 deletions src/main/flight/mixer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,8 @@ typedef struct mixerConfig_s {
uint16_t tri_servo_min_adc;
uint16_t tri_servo_mid_adc;
uint16_t tri_servo_max_adc;
uint16_t tri_motor_acc_yaw_correction;
float tri_motor_acceleration;
#endif
} mixerConfig_t;

Expand Down
Loading

0 comments on commit efec142

Please sign in to comment.