Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: remove getBodyFrameOdomDebug 

Not used after we moved logging to be within AP_NavEKF3
This commit is contained in:
Peter Barker 2021-05-30 12:07:39 +10:00 committed by Andrew Tridgell
parent dd3ab29b2f
commit d6348a0786
4 changed files with 9 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1588,21 +1588,6 @@ void NavEKF3::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms,
}
}
// return data for debugging body frame odometry fusion
uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const
{
uint32_t ret = 0;
if (instance < 0 || instance >= num_cores) {
instance = primary;
}
if (core) {
ret = core[instance].getBodyFrameOdomDebug(velInnov, velInnovVar);
}
return ret;
}
// parameter conversion of EKF3 parameters
void NavEKF3::convert_parameters()
{

10
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -222,16 +222,6 @@ public:
*/
void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
/*
* Return data for debugging body frame odometry fusion:
*
* velInnov are the XYZ body frame velocity innovations (m/s)
* velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
*
* Return the system time stamp of the last update (msec)
*/
uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar) const;
/*
* Writes the measurement from a yaw angle sensor
*

17
libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
View File

@ -263,6 +263,7 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
rngBcnFuseDataReportIndex++;
}
#if EK3_FEATURE_BODY_ODOM
void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
{
if (core_index != frontend->primary) {
@ -270,25 +271,25 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
return;
}
Vector3f velBodyInnov,velBodyInnovVar;
static uint32_t lastUpdateTime_ms = 0;
uint32_t updateTime_ms = getBodyFrameOdomDebug( velBodyInnov, velBodyInnovVar);
const uint32_t updateTime_ms = MAX(bodyOdmDataDelayed.time_ms,wheelOdmDataDelayed.time_ms);
if (updateTime_ms > lastUpdateTime_ms) {
const struct log_XKFD pkt11{
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us,
core : DAL_CORE(core_index),
velInnovX : velBodyInnov.x,
velInnovY : velBodyInnov.y,
velInnovZ : velBodyInnov.z,
velInnovVarX : velBodyInnovVar.x,
velInnovVarY : velBodyInnovVar.y,
velInnovVarZ : velBodyInnovVar.z
velInnovX : innovBodyVel[0],
velInnovY : innovBodyVel[1],
velInnovZ : innovBodyVel[2],
velInnovVarX : varInnovBodyVel[0],
velInnovVarY : varInnovBodyVel[1],
velInnovVarZ : varInnovBodyVel[2]
};
AP::logger().WriteBlock(&pkt11, sizeof(pkt11));
lastUpdateTime_ms = updateTime_ms;
}
}
#endif
void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const
{

14
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -59,20 +59,6 @@ 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)
{
velInnov.x = innovBodyVel[0];
velInnov.y = innovBodyVel[1];
velInnov.z = innovBodyVel[2];
velInnovVar.x = varInnovBodyVel[0];
velInnovVar.y = varInnovBodyVel[1];
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
// this is needed to ensure the vehicle does not fly too high when using optical flow navigation