aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: add and use pr/EK3_FEATURE_OPTFLOW_FUSION

Browse Source
This commit is contained in:
Peter Barker 2023-02-10 18:46:40 +11:00 committed by Peter Barker
parent c5b9d1dcbb
commit 02c6784c11
9 changed files with 52 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1551,6 +1551,7 @@ bool NavEKF3::configuredToUseGPSForPosXY(void) const
// msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor.
// posOffset is the XYZ flow sensor position in the body frame in m
// heightOverride is the fixed height of the sensor above ground in m, when on rover vehicles. 0 if not used
#if EK3_FEATURE_OPTFLOW_FUSION
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
@ -1571,6 +1572,7 @@ bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vecto
}
return false;
}
#endif // EK3_FEATURE_OPTFLOW_FUSION
// write yaw angle sensor measurements
void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)

13
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -289,7 +289,11 @@ void NavEKF3_core::setAidingMode()
// GPS aiding is the preferred option unless excluded by the user
if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
PV_AidingMode = AID_ABSOLUTE;
} else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
} else if (
#if EK3_FEATURE_OPTFLOW_FUSION
readyToUseOptFlow() ||
#endif
readyToUseBodyOdm()) {
PV_AidingMode = AID_RELATIVE;
}
break;
@ -432,11 +436,14 @@ void NavEKF3_core::setAidingMode()
case AID_RELATIVE:
// We are doing relative position navigation where velocity errors are constrained, but position drift will occur
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
#if EK3_FEATURE_OPTFLOW_FUSION
if (readyToUseOptFlow()) {
// Reset time stamps
flowValidMeaTime_ms = imuSampleTime_ms;
prevFlowFuseTime_ms = imuSampleTime_ms;
} else if (readyToUseBodyOdm()) {
} else
#endif
if (readyToUseBodyOdm()) {
// Reset time stamps
lastbodyVelPassTime_ms = imuSampleTime_ms;
prevBodyVelFuseTime_ms = imuSampleTime_ms;
@ -530,6 +537,7 @@ bool NavEKF3_core::useRngFinder(void) const
return true;
}
#if EK3_FEATURE_OPTFLOW_FUSION
// return true if the filter is ready to start using optical flow measurements
bool NavEKF3_core::readyToUseOptFlow(void) const
{
@ -545,6 +553,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
}
#endif // EK3_FEATURE_OPTFLOW_FUSION
// return true if the filter is ready to start using body frame odometry measurements
bool NavEKF3_core::readyToUseBodyOdm(void) const

4
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -202,7 +202,11 @@ void NavEKF3_core::Log_Write_XKF5(uint64_t time_us) const
offset : (int16_t)(100*terrainState), // filter ground offset state error
RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range
#if EK3_FEATURE_OPTFLOW_FUSION
errHAGL : (uint16_t)(100*sqrtF(Popt)), // note Popt is constrained to be non-negative in EstimateTerrainOffset()
#else
errHAGL : 0, // note Popt is constrained to be non-negative in EstimateTerrainOffset()
#endif
angErr : (float)outputTrackError.x, // output predictor angle error
velErr : (float)outputTrackError.y, // output predictor velocity error
posErr : (float)outputTrackError.z // output predictor position tracking error

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -167,6 +167,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
#endif // EK3_FEATURE_BODY_ODOM
}
#if EK3_FEATURE_OPTFLOW_FUSION
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
@ -235,6 +236,7 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
storedOF.push(ofDataNew);
}
}
#endif // EK3_FEATURE_OPTFLOW_FUSION
/********************************************************

5
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -1,7 +1,11 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_NavEKF3.h"
#include "AP_NavEKF3_core.h"
#if EK3_FEATURE_OPTFLOW_FUSION
#include <GCS_MAVLink/GCS.h>
/********************************************************
@ -779,3 +783,4 @@ bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate,
* MISC FUNCTIONS *
********************************************************/
#endif // EK3_FEATURE_OPTFLOW_FUSION

3
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -460,6 +460,7 @@ void NavEKF3_core::setTerrainHgtStable(bool val)
terrainHgtStable = val;
}
#if EK3_FEATURE_OPTFLOW_FUSION
// Detect takeoff for optical flow navigation
void NavEKF3_core::detectOptFlowTakeoff(void)
{
@ -477,4 +478,4 @@ void NavEKF3_core::detectOptFlowTakeoff(void)
takeOffDetected = false;
}
}
#endif // EK3_FEATURE_OPTFLOW_FUSION

8
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -249,7 +249,9 @@ void NavEKF3_core::InitialiseVariables()
memset(&nextP[0][0], 0, sizeof(nextP));
flowDataValid = false;
rangeDataToFuse = false;
#if EK3_FEATURE_OPTFLOW_FUSION
Popt = 0.0f;
#endif
terrainState = 0.0f;
prevPosN = stateStruct.position.x;
prevPosE = stateStruct.position.y;
@ -318,7 +320,9 @@ void NavEKF3_core::InitialiseVariables()
ZERO_FARRAY(statesArray);
memset(&vertCompFiltState, 0, sizeof(vertCompFiltState));
posVelFusionDelayed = false;
#if EK3_FEATURE_OPTFLOW_FUSION
optFlowFusionDelayed = false;
#endif
flowFusionActive = false;
airSpdFusionDelayed = false;
sideSlipFusionDelayed = false;
@ -609,8 +613,10 @@ void NavEKF3_core::CovarianceInit()
P[23][23] = P[22][22];
#if EK3_FEATURE_OPTFLOW_FUSION
// optical flow ground height covariance
Popt = 0.25f;
#endif
}
@ -667,8 +673,10 @@ void NavEKF3_core::UpdateFilter(bool predict)
SelectRngBcnFusion();
#endif
#if EK3_FEATURE_OPTFLOW_FUSION
// Update states using optical flow data
SelectFlowFusion();
#endif
#if EK3_FEATURE_BODY_ODOM
// Update states using body frame odometry data

12
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -868,8 +868,10 @@ private:
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(ftype K1, ftype K2);
#if EK3_FEATURE_OPTFLOW_FUSION
// return true if the filter is ready to start using optical flow measurements for position and velocity estimation
bool readyToUseOptFlow(void) const;
#endif
// return true if the filter is ready to start using body frame odometry measurements
bool readyToUseBodyOdm(void) const;
@ -880,8 +882,10 @@ private:
// return true if we should use the range finder sensor
bool useRngFinder(void) const;
#if EK3_FEATURE_OPTFLOW_FUSION
// determine when to perform fusion of optical flow measurements
void SelectFlowFusion();
#endif
// determine when to perform fusion of body frame odometry measurements
void SelectBodyOdomFusion();
@ -889,9 +893,11 @@ private:
// Estimate terrain offset using a single state EKF
void EstimateTerrainOffset(const of_elements &ofDataDelayed);
#if EK3_FEATURE_OPTFLOW_FUSION
// fuse optical flow measurements into the main filter
// really_fuse should be true to actually fuse into the main filter, false to only calculate variances
void FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse);
#endif
// Control filter mode changes
void controlFilterModes();
@ -934,8 +940,10 @@ private:
// Apply a median filter to range finder data
void readRangeFinder();
#if EK3_FEATURE_OPTFLOW_FUSION
// check if the vehicle has taken off during optical flow navigation by looking at inertial and range finder data
void detectOptFlowTakeoff(void);
#endif
// align the NE earth magnetic field states with the published declination
void alignMagStateDeclination();
@ -1182,7 +1190,9 @@ private:
bool motorsArmed; // true when the motors have been armed
bool prevMotorsArmed; // value of motorsArmed from previous frame
bool posVelFusionDelayed; // true when the position and velocity fusion has been delayed
#if EK3_FEATURE_OPTFLOW_FUSION
bool optFlowFusionDelayed; // true when the optical flow fusion has been delayed
#endif
bool airSpdFusionDelayed; // true when the air speed fusion has been delayed
bool sideSlipFusionDelayed; // true when the sideslip fusion has been delayed
bool airDataFusionWindOnly; // true when sideslip and airspeed fusion is only allowed to modify the wind states
@ -1262,7 +1272,9 @@ private:
Vector2 flowVarInnov; // optical flow innovations variances (rad/sec)^2
Vector2 flowInnov; // optical flow LOS innovations (rad/sec)
uint32_t flowInnovTime_ms; // system time that optical flow innovations and variances were recorded (to detect timeouts)
#if EK3_FEATURE_OPTFLOW_FUSION
ftype Popt; // Optical flow terrain height state covariance (m^2)
#endif
ftype terrainState; // terrain position state (m)
ftype prevPosN; // north position at last measurement
ftype prevPosE; // east position at last measurement

6
libraries/AP_NavEKF3/AP_NavEKF3_feature.h
View File

@ -8,6 +8,7 @@
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_Beacon/AP_Beacon_config.h>
#include <AP_AHRS/AP_AHRS_config.h>
#include <AP_OpticalFlow/AP_OpticalFlow_config.h>
// define for when to include all features
#define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
@ -40,3 +41,8 @@
#ifndef EK3_FEATURE_RANGEFINDER_MEASUREMENTS
#define EK3_FEATURE_RANGEFINDER_MEASUREMENTS AP_RANGEFINDER_ENABLED
#endif
// Flow Fusion if Flow data available
#ifndef EK3_FEATURE_OPTFLOW_FUSION
#define EK3_FEATURE_OPTFLOW_FUSION HAL_NAVEKF3_AVAILABLE && AP_OPTICALFLOW_ENABLED
#endif