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

no need to duplicate wrapping done in sin_approx

Browse source
This commit is contained in:
ctzsnooze 2024-10-26 19:45:23 +11:00
parent 7e25949465
commit 42597c8339
1 changed files with 2 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/flight/autopilot.c
View file

@ -203,7 +203,6 @@ bool positionControl(void) {
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS; posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
const float twoPi = 2 * M_PIf;
// get distance and bearing from current location (gpsSol.llh) to target location // get distance and bearing from current location (gpsSol.llh) to target location
uint32_t distanceCm = 0; uint32_t distanceCm = 0;
@ -262,12 +261,7 @@ bool positionControl(void) {
// separate PID controllers for latitude (NorthSouth or ns) and longitude (EastWest or ew) // separate PID controllers for latitude (NorthSouth or ns) and longitude (EastWest or ew)
// divide the distance vector into ns and ew parts depending on the bearing to the target point // divide the distance vector into ns and ew parts depending on the bearing to the target point
float bearingRadians = bearingDeg * RAD; // 0-360, so constrain to +/- π float bearingRadians = bearingDeg * RAD; // 0-360 degrees, constrained within sin_approx
if (bearingRadians > M_PIf) {
bearingRadians -= twoPi;
} else if (bearingRadians < -M_PIf) {
bearingRadians += twoPi;
}
const float nsDistance = -cos_approx(bearingRadians) * posHold.distanceCm; const float nsDistance = -cos_approx(bearingRadians) * posHold.distanceCm;
// Positive when North of the target, negative when when South of target // Positive when North of the target, negative when when South of target
@ -345,12 +339,7 @@ bool positionControl(void) {
float ewPidSum = ewP + ewI + ewDA; float ewPidSum = ewP + ewI + ewDA;
// ** Rotate pid Sum to quad frame of reference, into pitch and roll ** // ** Rotate pid Sum to quad frame of reference, into pitch and roll **
float headingRads = headingDeg * RAD; float headingRads = headingDeg * RAD; // headingDeg is 0-360 but will be constrained in sin_approx()
if (headingRads > M_PIf) {
headingRads -= twoPi;
} else if (headingRads < -M_PIf) {
headingRads += twoPi;
}
const float sinHeading = sin_approx(headingRads); const float sinHeading = sin_approx(headingRads);
const float cosHeading = cos_approx(headingRads); const float cosHeading = cos_approx(headingRads);
float pidSumRoll = -sinHeading * nsPidSum + cosHeading * ewPidSum; float pidSumRoll = -sinHeading * nsPidSum + cosHeading * ewPidSum;