Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Improve Accuracy of roll_yaw_cam_mix feature

Browse source
This commit is contained in:
borisbstyle 2016-07-17 01:41:17 +02:00
parent 4f19458347
commit 34fd8f3c88
1 changed files with 23 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

46
src/main/mw.c
View file

@ -188,6 +188,24 @@ float calculateRate(int axis, int16_t rc) {
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
} }
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = angleRate[ROLL];
int16_t yaw = angleRate[YAW];
angleRate[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
angleRate[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
void processRcCommand(void) void processRcCommand(void)
{ {
static int16_t lastCommand[4] = { 0, 0, 0, 0 }; static int16_t lastCommand[4] = { 0, 0, 0, 0 };
@ -208,6 +226,11 @@ void processRcCommand(void)
if (isRXDataNew) { if (isRXDataNew) {
for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]); for (axis = 0; axis < 3; axis++) angleRate[axis] = calculateRate(axis, rcCommand[axis]);
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
for (int channel=0; channel < 4; channel++) { for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel]; lastCommand[channel] = rcCommand[channel];
@ -230,24 +253,6 @@ void processRcCommand(void)
} }
} }
void scaleRcCommandToFpvCamAngle(void) {
//recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
static uint8_t lastFpvCamAngleDegrees = 0;
static float cosFactor = 1.0;
static float sinFactor = 0.0;
if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
}
int16_t roll = rcCommand[ROLL];
int16_t yaw = rcCommand[YAW];
rcCommand[ROLL] = constrain(roll * cosFactor - yaw * sinFactor, -500, 500);
rcCommand[YAW] = constrain(yaw * cosFactor + roll * sinFactor, -500, 500);
}
static void updateRcCommands(void) static void updateRcCommands(void)
{ {
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value // PITCH & ROLL only dynamic PID adjustment, depending on throttle value
@ -310,11 +315,6 @@ static void updateRcCommands(void)
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH; rcCommand[PITCH] = rcCommand_PITCH;
} }
// experimental scaling of RC command to FPV cam angle
if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
scaleRcCommandToFpvCamAngle();
}
} }
static void updateLEDs(void) static void updateLEDs(void)