rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Renamed annexCode to updateRcCommands and tidied.

This commit is contained in:
Martin Budden 2016-05-09 11:00:11 +01:00
parent db4da776c5
commit 52b40b1028
1 changed files with 28 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/main/mw.c
View File

@ -220,12 +220,10 @@ void scaleRcCommandToFpvCamAngle(void) {
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500); rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
} }
void annexCode(void) static void updateRcCommands(void)
{ {
int32_t tmp;
int32_t axis, prop;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value // PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop = 100; prop = 100;
} else { } else {
@ -236,35 +234,32 @@ void annexCode(void)
} }
} }
for (axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
PIDweight[axis] = prop;
int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) { if (axis == ROLL || axis == PITCH) {
if (masterConfig.rcControlsConfig.deadband) {
if (tmp > masterConfig.rcControlsConfig.deadband) { if (tmp > masterConfig.rcControlsConfig.deadband) {
tmp -= masterConfig.rcControlsConfig.deadband; tmp -= masterConfig.rcControlsConfig.deadband;
} else { } else {
tmp = 0; tmp = 0;
} }
}
rcCommand[axis] = rcLookupPitchRoll(tmp); rcCommand[axis] = rcLookupPitchRoll(tmp);
} else if (axis == YAW) { } else if (axis == YAW) {
if (masterConfig.rcControlsConfig.yaw_deadband) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) { if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband; tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else { } else {
tmp = 0; tmp = 0;
} }
}
rcCommand[axis] = rcLookupYaw(tmp) * -masterConfig.yaw_control_direction; rcCommand[axis] = rcLookupYaw(tmp) * -masterConfig.yaw_control_direction;
} }
if (rcData[axis] < masterConfig.rxConfig.midrc) {
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
PIDweight[axis] = prop;
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis]; rcCommand[axis] = -rcCommand[axis];
} }
}
int32_t tmp;
if (feature(FEATURE_3D)) { if (feature(FEATURE_3D)) {
tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - PWM_RANGE_MIN); tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
@ -280,10 +275,10 @@ void annexCode(void)
} }
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff); const float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff); const float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH; rcCommand[PITCH] = rcCommand_PITCH;
} }
@ -859,8 +854,8 @@ void taskUpdateRxMain(void)
processRx(); processRx();
isRXDataNew = true; isRXDataNew = true;
// the 'annexCode' initialses rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
annexCode(); updateRcCommands();
#ifdef BARO #ifdef BARO
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
updateAltHoldState(); updateAltHoldState();