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

Kalman filter for sonar readings. Provides surface rate velocity (currently unused)

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-18 14:13:13 +10:00
parent fb5c13fefa
commit 0f417d5246
3 changed files with 34 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/flight/navigation_rewrite.c
View file

@ -1005,9 +1005,11 @@ void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, fl
/*----------------------------------------------------------- /*-----------------------------------------------------------
* Processes an update to surface distance * Processes an update to surface distance
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance) void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity)
{ {
posControl.actualState.surface = surfaceDistance; posControl.actualState.surface = surfaceDistance;
posControl.actualState.surfaceVel = surfaceVelocity;
posControl.flags.hasValidSurfaceSensor = hasValidSensor; posControl.flags.hasValidSurfaceSensor = hasValidSensor;
if (hasValidSensor) { if (hasValidSensor) {

43
src/main/flight/navigation_rewrite_pos_estimator.c
View file

@ -66,10 +66,13 @@
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout #define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout #define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
#define INAV_SONAR_TIMEOUT_MS 300 // Sonar timeout (missed two readings in a row #define INAV_SONAR_TIMEOUT_MS 250 // Sonar timeout (missed two readings in a row
#define INAV_BARO_CLIMB_RATE_FILTER_SIZE 7 #define INAV_BARO_CLIMB_RATE_FILTER_SIZE 7
#define INAV_SONAR_W1 0.8461f // Sonar predictive filter gain for altitude
#define INAV_SONAR_W2 6.2034f // Sonar predictive filter gain for velocity
#define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data #define INAV_HISTORY_BUF_SIZE (INAV_POSITION_PUBLISH_RATE_HZ / 2) // Enough to hold 0.5 sec historical data
extern float magneticDeclination; extern float magneticDeclination;
@ -99,7 +102,7 @@ typedef struct {
typedef struct { typedef struct {
uint32_t lastUpdateTime; // Last update time (us) uint32_t lastUpdateTime; // Last update time (us)
float alt; // Raw altitude measurement (cm) float alt; // Raw altitude measurement (cm)
float epv; float vel;
} navPositionEstimatorSONAR_t; } navPositionEstimatorSONAR_t;
typedef struct { typedef struct {
@ -107,6 +110,7 @@ typedef struct {
t_fp_vector pos; t_fp_vector pos;
t_fp_vector vel; t_fp_vector vel;
float surface; float surface;
float surfaceVel;
float eph; float eph;
float epv; float epv;
} navPositionEstimatorESTIMATE_t; } navPositionEstimatorESTIMATE_t;
@ -388,18 +392,26 @@ static void updateSonarTopic(uint32_t currentTime)
if (updateTimer(&sonarUpdateTimer, HZ2US(INAV_SONAR_UPDATE_RATE), currentTime)) { if (updateTimer(&sonarUpdateTimer, HZ2US(INAV_SONAR_UPDATE_RATE), currentTime)) {
if (sensors(SENSOR_SONAR)) { if (sensors(SENSOR_SONAR)) {
/* Read sonar */ /* Read sonar */
float newSonarAltRaw = sonarRead(); float newSonarAlt = sonarRead();
newSonarAlt = sonarCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
posEstimator.sonar.alt = sonarCalculateAltitude(newSonarAltRaw, calculateCosTiltAngle()); /* Apply predictive filter to sonar readings (inspired by PX4Flow) */
/* If we have two valid sonar readings in a row - update topic */
if (posEstimator.sonar.alt > 0) { if (posEstimator.sonar.alt > 0) {
float sonarPredVel, sonarPredAlt;
float sonarDt = (currentTime - posEstimator.sonar.lastUpdateTime) * 1e-6;
posEstimator.sonar.lastUpdateTime = currentTime; posEstimator.sonar.lastUpdateTime = currentTime;
sonarPredVel = (sonarDt < 0.25f) ? posEstimator.sonar.vel : 0.0f;
sonarPredAlt = posEstimator.sonar.alt + sonarPredVel * sonarDt;
posEstimator.sonar.alt = sonarPredAlt + INAV_SONAR_W1 * (newSonarAlt - sonarPredAlt);
posEstimator.sonar.vel = sonarPredVel + INAV_SONAR_W2 * (newSonarAlt - sonarPredAlt);
} }
} }
else { else {
/* No sonar */ /* No sonar */
posEstimator.sonar.alt = 0; posEstimator.sonar.alt = 0;
posEstimator.sonar.vel = 0;
posEstimator.sonar.lastUpdateTime = 0; posEstimator.sonar.lastUpdateTime = 0;
} }
} }
@ -592,14 +604,17 @@ static void updateEstimatedTopic(uint32_t currentTime)
/* Surface offset */ /* Surface offset */
#if defined(SONAR) #if defined(SONAR)
if (isSonarValid) { if (isSonarValid) {
posEstimator.est.surface = posEstimator.sonar.alt; posEstimator.est.surface = posEstimator.sonar.alt;
} posEstimator.est.surfaceVel = posEstimator.sonar.vel;
else { }
posEstimator.est.surface = -1; else {
} posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
}
#else #else
posEstimator.est.surface = -1; posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
#endif #endif
} }
@ -635,10 +650,10 @@ static void publishEstimatedTopic(uint32_t currentTime)
/* Publish surface distance */ /* Publish surface distance */
if (posEstimator.est.surface > 0) { if (posEstimator.est.surface > 0) {
updateActualSurfaceDistance(true, posEstimator.est.surface); updateActualSurfaceDistance(true, posEstimator.est.surface, posEstimator.est.surfaceVel);
} }
else { else {
updateActualSurfaceDistance(false, -1); updateActualSurfaceDistance(false, -1, 0);
} }
/* Store history data */ /* Store history data */

3
src/main/flight/navigation_rewrite_private.h
View file

@ -107,6 +107,7 @@ typedef struct {
float sinYaw; float sinYaw;
float cosYaw; float cosYaw;
float surface; float surface;
float surfaceVel;
} navigationEstimatedState_t; } navigationEstimatedState_t;
typedef struct { typedef struct {
@ -275,7 +276,7 @@ int16_t leanAngleToRcCommand(int16_t leanAngle);
void updateActualHeading(int32_t newHeading); void updateActualHeading(int32_t newHeading);
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY); void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY);
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity); void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity);
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance); void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity);
/* Autonomous navigation functions */ /* Autonomous navigation functions */
void setupAutonomousControllerRTH(void); void setupAutonomousControllerRTH(void);