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

DataFlash: protect against large testRatio from EKF

Browse Source 
This should be resolved in the EKF but for now this stops the floating point exception
This commit is contained in:
Randy Mackay 2017-06-08 20:49:20 +09:00
parent 5e3c30b0fe
commit f8b5714ced
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/DataFlash/LogFile.cpp
View File

@ -1411,7 +1411,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
rng : (int16_t)(100*rng), rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov), innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*safe_sqrt(innovVar)), sqrtInnovVar : (uint16_t)(100*safe_sqrt(innovVar)),
testRatio : (uint16_t)(100*testRatio), testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*beaconPosNED.x), beaconPosN : (int16_t)(100*beaconPosNED.x),
beaconPosE : (int16_t)(100*beaconPosNED.y), beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z), beaconPosD : (int16_t)(100*beaconPosNED.z),
@ -1739,7 +1739,7 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
rng : (int16_t)(100*rng), rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov), innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)), sqrtInnovVar : (uint16_t)(100*sqrtf(innovVar)),
testRatio : (uint16_t)(100*testRatio), testRatio : (uint16_t)(100*constrain_float(testRatio,0.0f,650.0f)),
beaconPosN : (int16_t)(100*beaconPosNED.x), beaconPosN : (int16_t)(100*beaconPosNED.x),
beaconPosE : (int16_t)(100*beaconPosNED.y), beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z), beaconPosD : (int16_t)(100*beaconPosNED.z),