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

useless code removed

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2016-03-23 19:07:01 +01:00
parent f4cdbb2d0a
commit 72b611ac5c
4 changed files with 2 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -34,13 +34,9 @@
#include "sensors/sonar.h" #include "sensors/sonar.h"
// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range. // Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
// Inclination is adjusted by imu
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
#ifdef SONAR #ifdef SONAR
int16_t sonarMaxRangeCm;
int16_t sonarMaxAltWithTiltCm;
STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees; STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
float sonarMaxTiltCos; float sonarMaxTiltCos;
@ -113,10 +109,8 @@ void sonarInit(const sonarHardware_t *sonarHardware)
hcsr04_init(sonarHardware, &sonarRange); hcsr04_init(sonarHardware, &sonarRange);
sensorsSet(SENSOR_SONAR); sensorsSet(SENSOR_SONAR);
sonarMaxRangeCm = sonarRange.maxRangeCm;
sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2; sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2;
sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD); sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD);
sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos;
calculatedAltitude = SONAR_OUT_OF_RANGE; calculatedAltitude = SONAR_OUT_OF_RANGE;
} }
@ -125,8 +119,6 @@ void sonarUpdate(void)
hcsr04_start_reading(); hcsr04_start_reading();
} }
#define SONAR_SAMPLES_MEDIAN 3
/** /**
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned. * Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned.
*/ */

3
src/main/sensors/sonar.h
View file

@ -19,9 +19,6 @@
#define SONAR_OUT_OF_RANGE (-1) #define SONAR_OUT_OF_RANGE (-1)
extern int16_t sonarMaxRangeCm;
extern int16_t sonarMaxAltWithTiltCm;
void sonarUpdate(void); void sonarUpdate(void);
int32_t sonarRead(void); int32_t sonarRead(void);
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);

1
src/test/unit/flight_imu_unittest.cc
View file

@ -143,7 +143,6 @@ uint8_t armingFlags;
int32_t sonarAlt; int32_t sonarAlt;
int16_t sonarCfAltCm; int16_t sonarCfAltCm;
int16_t sonarMaxAltWithTiltCm;
int32_t accADC[XYZ_AXIS_COUNT]; int32_t accADC[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT]; int32_t gyroADC[XYZ_AXIS_COUNT];

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

@ -44,13 +44,9 @@ TEST(SonarUnittest, TestConstants)
TEST(SonarUnittest, TestSonarInit) TEST(SonarUnittest, TestSonarInit)
{ {
sonarInit(0); sonarInit(0);
EXPECT_EQ(sonarMaxRangeCm, HCSR04_MAX_RANGE_CM);
// Check against gross errors in max range values // Check against gross errors in max range values
EXPECT_GE(sonarMaxAltWithTiltCm, 100);
EXPECT_LE(sonarMaxAltWithTiltCm, sonarMaxRangeCm);
EXPECT_GE(sonarCfAltCm, 100); EXPECT_GE(sonarCfAltCm, 100);
EXPECT_LE(sonarCfAltCm, sonarMaxRangeCm); EXPECT_LE(sonarCfAltCm, HCSR04_MAX_RANGE_CM);
EXPECT_LE(sonarCfAltCm, sonarMaxAltWithTiltCm);
// Check reasonable values for maximum tilt // Check reasonable values for maximum tilt
EXPECT_GE(sonarMaxTiltDeciDegrees, 0); EXPECT_GE(sonarMaxTiltDeciDegrees, 0);
EXPECT_LE(sonarMaxTiltDeciDegrees, 450); EXPECT_LE(sonarMaxTiltDeciDegrees, 450);
@ -102,9 +98,6 @@ TEST(SonarUnittest, TestAltitude)
// distance 400, 22.4 degrees of roll, this corresponds to HC-SR04 effective max detection angle // distance 400, 22.4 degrees of roll, this corresponds to HC-SR04 effective max detection angle
EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(224))), 369); EXPECT_EQ(sonarCalculateAltitude(400, cosf(DECIDEGREES_TO_RADIANS(224))), 369);
EXPECT_EQ(sonarGetLatestAltitude(), 369); EXPECT_EQ(sonarGetLatestAltitude(), 369);
// max range, max tilt
EXPECT_EQ(sonarCalculateAltitude(sonarMaxRangeCm, cosf(DECIDEGREES_TO_RADIANS(sonarMaxTiltDeciDegrees))), sonarMaxAltWithTiltCm);
EXPECT_EQ(sonarGetLatestAltitude(), sonarMaxAltWithTiltCm);
} }
// STUBS // STUBS