Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: make body odomotry build depend on vehicle type

Browse Source 
saves about 11k of flash
This commit is contained in:
Andrew Tridgell 2021-01-19 12:53:20 +11:00
parent 3c825bdffc
commit 8da511f039
8 changed files with 42 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -459,6 +459,7 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
// return true if the filter is ready to start using body frame odometry measurements
bool NavEKF3_core::readyToUseBodyOdm(void) const
{
#if EK3_FEATURE_BODY_ODOM
if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV) &&
!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER)) {
// exit immediately if sources not configured to fuse external nav or wheel encoders
@ -476,6 +477,9 @@ bool NavEKF3_core::readyToUseBodyOdm(void) const
return (visoDataGood || wencDataGood)
&& tiltAlignComplete
&& delAngBiasLearned;
#else
return false;
#endif // EK3_FEATURE_BODY_ODOM
}
// return true if the filter to be ready to use gps

2
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -379,8 +379,10 @@ void NavEKF3_core::Log_Write(uint64_t time_us)
// write range beacon fusion debug packet if the range value is non-zero
Log_Write_Beacon(time_us);
#if EK3_FEATURE_BODY_ODOM
// write debug data for body frame odometry fusion
Log_Write_BodyOdom(time_us);
#endif
// log state variances every 0.49s
Log_Write_State_Variances(time_us);

5
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -113,6 +113,7 @@ void NavEKF3_core::readRangeFinder(void)
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
#if EK3_FEATURE_BODY_ODOM
// protect against NaN
if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
return;
@ -138,9 +139,10 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con
bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);
storedBodyOdm.push(bodyOdmDataNew);
#endif // EK3_FEATURE_BODY_ODOM
}
#if EK3_FEATURE_BODY_ODOM
void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
// This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
@ -166,6 +168,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam
storedWheelOdm.push(wheelOdmDataNew);
}
#endif // EK3_FEATURE_BODY_ODOM
// write the raw optical flow measurements
// this needs to be called externally.

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -59,6 +59,7 @@ float NavEKF3_core::errorScore() const
return score;
}
#if EK3_FEATURE_BODY_ODOM
// return data for debugging body frame odometry fusion
uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
{
@ -70,6 +71,7 @@ uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velIn
velInnovVar.z = varInnovBodyVel[2];
return MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
}
#endif // EK3_FEATURE_BODY_ODOM
// provides the height limit to be observed by the control loops
// returns false if no height limiting is required

5
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1146,6 +1146,7 @@ void NavEKF3_core::selectHeightForFusion()
}
}
#if EK3_FEATURE_BODY_ODOM
/*
* Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
@ -1781,7 +1782,9 @@ void NavEKF3_core::FuseBodyVel()
}
}
}
#endif // EK3_FEATURE_BODY_ODOM
#if EK3_FEATURE_BODY_ODOM
// select fusion of body odometry measurements
void NavEKF3_core::SelectBodyOdomFusion()
{
@ -1834,4 +1837,4 @@ void NavEKF3_core::SelectBodyOdomFusion()
}
}
}
#endif // EK3_FEATURE_BODY_ODOM

8
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -116,6 +116,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
if(dal.opticalflow_enabled() && !storedOF.init(flow_buffer_length)) {
return false;
}
#if EK3_FEATURE_BODY_ODOM
if(frontend->sources.ext_nav_enabled() && !storedBodyOdm.init(obs_buffer_length)) {
return false;
}
@ -123,6 +124,7 @@ bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
// initialise to same length of IMU to allow for multiple wheel sensors
return false;
}
#endif // EK3_FEATURE_BODY_ODOM
if(frontend->sources.gps_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
return false;
}
@ -372,9 +374,11 @@ void NavEKF3_core::InitialiseVariables()
bcnPosOffsetNED.zero();
bcnOriginEstInit = false;
#if EK3_FEATURE_BODY_ODOM
// body frame displacement fusion
memset((void *)&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
memset((void *)&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
#endif
lastbodyVelPassTime_ms = 0;
memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio));
memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel));
@ -408,8 +412,10 @@ void NavEKF3_core::InitialiseVariables()
storedRange.reset();
storedOutput.reset();
storedRangeBeacon.reset();
#if EK3_FEATURE_BODY_ODOM
storedBodyOdm.reset();
storedWheelOdm.reset();
#endif
storedExtNav.reset();
storedExtNavVel.reset();
@ -672,8 +678,10 @@ void NavEKF3_core::UpdateFilter(bool predict)
// Update states using optical flow data
SelectFlowFusion();
#if EK3_FEATURE_BODY_ODOM
// Update states using body frame odometry data
SelectBodyOdomFusion();
#endif
// Update states using airspeed data
SelectTasFusion();

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -35,6 +35,7 @@
#include <AP_DAL/AP_DAL.h>
#include "AP_NavEKF/EKFGSF_yaw.h"
#include "AP_NavEKF3_feature.h"
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
@ -1189,9 +1190,11 @@ private:
bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
// body frame odometry fusion
#if EK3_FEATURE_BODY_ODOM
EKF_obs_buffer_t<vel_odm_elements> storedBodyOdm; // body velocity data buffer
vel_odm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon
vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
#endif
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec)
Vector3 bodyVelTestRatio; // Innovation test ratios for body velocity XYZ measurements
Vector3 varInnovBodyVel; // Body velocity XYZ innovation variances (m/sec)^2
@ -1201,9 +1204,11 @@ private:
bool bodyVelFusionDelayed; // true when body frame velocity fusion has been delayed
bool bodyVelFusionActive; // true when body frame velocity fusion is active
#if EK3_FEATURE_BODY_ODOM
// wheel sensor fusion
EKF_obs_buffer_t<wheel_odm_elements> storedWheelOdm; // body velocity data buffer
wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon
#endif
// GPS yaw sensor fusion
uint32_t yawMeasTime_ms; // system time GPS yaw angle was last input to the data buffer

13
libraries/AP_NavEKF3/AP_NavEKF3_feature.h Normal file
View File

@ -0,0 +1,13 @@
/*
feature selection for EKF3
*/
#pragma once
#include <AP_Vehicle/AP_Vehicle_Type.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)
// body odomotry (which includes wheel encoding) on rover or 2M boards
#define EK3_FEATURE_BODY_ODOM EK3_FEATURE_ALL || APM_BUILD_TYPE(APM_BUILD_Rover) || BOARD_FLASH_SIZE > 1024