Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Improved cosine approximation used when calculating altitude when tilted. Now second order Taylor expansion with only 2 extra float multiplies.

Browse source
This commit is contained in:
Martin Budden 2015-11-27 01:12:29 +00:00
parent a097e4912f
commit cda717dbe4
2 changed files with 12 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/sensors/sonar.c
View file

@ -142,14 +142,14 @@ int16_t sonarCalculateTiltAngle(int16_t rollDeciDegrees, int16_t pitchDeciDegree
*/ */
int32_t sonarCalculateAltitude(int32_t sonarDistance, int16_t rollDeciDegrees, int16_t pitchDeciDegrees) int32_t sonarCalculateAltitude(int32_t sonarDistance, int16_t rollDeciDegrees, int16_t pitchDeciDegrees)
{ {
int16_t tiltAngle = sonarCalculateTiltAngle(rollDeciDegrees, pitchDeciDegrees); #define coefX2 1.52309E-06f // (PI/1800)^2/2, coefficient of x^2 in Taylor expansion of cos(x)
int16_t tiltAngle = sonarCalculateTiltAngle(rollDeciDegrees, pitchDeciDegrees);
// calculate sonar altitude only if the ground is in the sonar cone // calculate sonar altitude only if the ground is in the sonar cone
if (tiltAngle > HCSR04_MAX_TILT_ANGLE_DECIDEGREES) if (tiltAngle > HCSR04_MAX_TILT_ANGLE_DECIDEGREES)
calculatedAltitude = SONAR_OUT_OF_RANGE; calculatedAltitude = SONAR_OUT_OF_RANGE;
else else
// altitude = distance * cos(tiltAngle), use approximation // altitude = distance * cos(tiltAngle), use approximation
calculatedAltitude = sonarDistance * (900.0f - tiltAngle) / 900.0f; calculatedAltitude = sonarDistance * (1.0f - tiltAngle*tiltAngle*coefX2);
return calculatedAltitude; return calculatedAltitude;
} }

17
src/test/unit/sonar_unittest.cc
View file

@ -70,18 +70,19 @@ TEST(SonarUnittest, TestAltitude)
// Check distance at various roll angles // Check distance at various roll angles
// distance 400, 5 degrees of roll // distance 400, 5 degrees of roll
EXPECT_EQ(sonarCalculateAltitude(400, 50, 0), 377); EXPECT_EQ(sonarCalculateAltitude(400, 50, 0), 398);
EXPECT_EQ(sonarGetLatestAltitude(), 377); EXPECT_EQ(sonarGetLatestAltitude(), 398);
// distance 400, 10 degrees of roll // distance 400, 10 degrees of roll
EXPECT_EQ(sonarCalculateAltitude(400, 100, 0), 355); EXPECT_EQ(sonarCalculateAltitude(400, 100, 0), 393);
EXPECT_EQ(sonarGetLatestAltitude(), 355); EXPECT_EQ(sonarGetLatestAltitude(), 393);
// distance 400, 20 degrees of roll // distance 400, 20 degrees of roll
EXPECT_EQ(sonarCalculateAltitude(400, 200, 0), 311); EXPECT_EQ(sonarCalculateAltitude(400, 200, 0), 375);
EXPECT_EQ(sonarGetLatestAltitude(), 311); EXPECT_EQ(sonarGetLatestAltitude(), 375);
// distance 400, maximum roll // distance 400, maximum roll
EXPECT_EQ(sonarCalculateAltitude(400, HCSR04_MAX_TILT_ANGLE_DECIDEGREES, 0), 300); EXPECT_EQ(sonarCalculateAltitude(400, HCSR04_MAX_TILT_ANGLE_DECIDEGREES, 0), 369);
EXPECT_EQ(sonarGetLatestAltitude(), 300); EXPECT_EQ(sonarGetLatestAltitude(), 369);
} }
typedef struct rollAndPitch_s { typedef struct rollAndPitch_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t rollDeciDegrees; int16_t rollDeciDegrees;