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)
{
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
if (tiltAngle > HCSR04_MAX_TILT_ANGLE_DECIDEGREES)
calculatedAltitude = SONAR_OUT_OF_RANGE;
else
// altitude = distance * cos(tiltAngle), use approximation
calculatedAltitude = sonarDistance * (900.0f - tiltAngle) / 900.0f;
calculatedAltitude = sonarDistance * (1.0f - tiltAngle*tiltAngle*coefX2);
return calculatedAltitude;
}

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

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