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 // parameter conversion of EKF3 parameters
void NavEKF3::convert_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); 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 * 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++; rngBcnFuseDataReportIndex++;
} }
#if EK3_FEATURE_BODY_ODOM
void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us) void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
{ {
if (core_index != frontend->primary) { if (core_index != frontend->primary) {
@ -270,25 +271,25 @@ void NavEKF3_core::Log_Write_BodyOdom(uint64_t time_us)
return; return;
} }
Vector3f velBodyInnov,velBodyInnovVar;
static uint32_t lastUpdateTime_ms = 0; 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) { if (updateTime_ms > lastUpdateTime_ms) {
const struct log_XKFD pkt11{ const struct log_XKFD pkt11{
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG), LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us, time_us : time_us,
core : DAL_CORE(core_index), core : DAL_CORE(core_index),
velInnovX : velBodyInnov.x, velInnovX : innovBodyVel[0],
velInnovY : velBodyInnov.y, velInnovY : innovBodyVel[1],
velInnovZ : velBodyInnov.z, velInnovZ : innovBodyVel[2],
velInnovVarX : velBodyInnovVar.x, velInnovVarX : varInnovBodyVel[0],
velInnovVarY : velBodyInnovVar.y, velInnovVarY : varInnovBodyVel[1],
velInnovVarZ : velBodyInnovVar.z velInnovVarZ : varInnovBodyVel[2]
}; };
AP::logger().WriteBlock(&pkt11, sizeof(pkt11)); AP::logger().WriteBlock(&pkt11, sizeof(pkt11));
lastUpdateTime_ms = updateTime_ms; lastUpdateTime_ms = updateTime_ms;
} }
} }
#endif
void NavEKF3_core::Log_Write_State_Variances(uint64_t time_us) const 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; 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 // provides the height limit to be observed by the control loops
// returns false if no height limiting is required // 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 // this is needed to ensure the vehicle does not fly too high when using optical flow navigation