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
*-----------------------------------------------------------*/
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance)
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity)
{
posControl.actualState.surface = surfaceDistance;
posControl.actualState.surfaceVel = surfaceVelocity;
posControl.flags.hasValidSurfaceSensor = 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_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_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
extern float magneticDeclination;
@ -99,7 +102,7 @@ typedef struct {
typedef struct {
uint32_t lastUpdateTime; // Last update time (us)
float alt; // Raw altitude measurement (cm)
float epv;
float vel;
} navPositionEstimatorSONAR_t;
typedef struct {
@ -107,6 +110,7 @@ typedef struct {
t_fp_vector pos;
t_fp_vector vel;
float surface;
float surfaceVel;
float eph;
float epv;
} navPositionEstimatorESTIMATE_t;
@ -388,18 +392,26 @@ static void updateSonarTopic(uint32_t currentTime)
if (updateTimer(&sonarUpdateTimer, HZ2US(INAV_SONAR_UPDATE_RATE), currentTime)) {
if (sensors(SENSOR_SONAR)) {
/* Read sonar */
float newSonarAltRaw = sonarRead();
float newSonarAlt = sonarRead();
newSonarAlt = sonarCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
posEstimator.sonar.alt = sonarCalculateAltitude(newSonarAltRaw, calculateCosTiltAngle());
/* If we have two valid sonar readings in a row - update topic */
/* Apply predictive filter to sonar readings (inspired by PX4Flow) */
if (posEstimator.sonar.alt > 0) {
float sonarPredVel, sonarPredAlt;
float sonarDt = (currentTime - posEstimator.sonar.lastUpdateTime) * 1e-6;
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 {
/* No sonar */
posEstimator.sonar.alt = 0;
posEstimator.sonar.vel = 0;
posEstimator.sonar.lastUpdateTime = 0;
}
}
@ -592,14 +604,17 @@ static void updateEstimatedTopic(uint32_t currentTime)
/* Surface offset */
#if defined(SONAR)
if (isSonarValid) {
posEstimator.est.surface = posEstimator.sonar.alt;
}
else {
posEstimator.est.surface = -1;
}
if (isSonarValid) {
posEstimator.est.surface = posEstimator.sonar.alt;
posEstimator.est.surfaceVel = posEstimator.sonar.vel;
}
else {
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
}
#else
posEstimator.est.surface = -1;
posEstimator.est.surfaceVel = 0;
#endif
}
@ -635,10 +650,10 @@ static void publishEstimatedTopic(uint32_t currentTime)
/* Publish surface distance */
if (posEstimator.est.surface > 0) {
updateActualSurfaceDistance(true, posEstimator.est.surface);
updateActualSurfaceDistance(true, posEstimator.est.surface, posEstimator.est.surfaceVel);
}
else {
updateActualSurfaceDistance(false, -1);
updateActualSurfaceDistance(false, -1, 0);
}
/* Store history data */

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

@ -107,6 +107,7 @@ typedef struct {
float sinYaw;
float cosYaw;
float surface;
float surfaceVel;
} navigationEstimatedState_t;
typedef struct {
@ -275,7 +276,7 @@ int16_t leanAngleToRcCommand(int16_t leanAngle);
void updateActualHeading(int32_t newHeading);
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY);
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 */
void setupAutonomousControllerRTH(void);