Browse Source

DataFlash: protect against large testRatio from EKF

This should be resolved in the EKF but for now this stops the floating point exception
master
Randy Mackay 8 years ago
parent
commit
af7f4fef0a
  1. 4
      libraries/DataFlash/LogFile.cpp

4
libraries/DataFlash/LogFile.cpp

@ -1411,7 +1411,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) @@ -1411,7 +1411,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
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),
beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z),
@ -1739,7 +1739,7 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) @@ -1739,7 +1739,7 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
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),
beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z),

Loading…
Cancel
Save