Browse Source

AP_NavEKF2: fixed undefined behaviour in logging

apm_2208
Andrew Tridgell 3 years ago
parent
commit
a24ed6a7f1
  1. 2
      libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp

2
libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp

@ -170,7 +170,7 @@ void NavEKF2_core::Log_Write_NKF5(uint64_t time_us) const
FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter FIX : (int16_t)(1000*innovOptFlow[0]), // optical flow LOS rate vector innovations from the main nav filter
FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter FIY : (int16_t)(1000*innovOptFlow[1]), // optical flow LOS rate vector innovations from the main nav filter
AFI : (int16_t)(1000 * auxFlowObsInnov.length()), // optical flow LOS rate innovation from terrain offset estimator AFI : (int16_t)(1000 * auxFlowObsInnov.length()), // optical flow LOS rate innovation from terrain offset estimator
HAGL : (int16_t)(100*(terrainState - stateStruct.position.z)), // height above ground level HAGL : float_to_int16(100*(terrainState - stateStruct.position.z)), // height above ground level
offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum offset : (int16_t)(100*terrainState), // // estimated vertical position of the terrain relative to the nav filter zero datum
RI : (int16_t)(100*innovRng), // range finder innovations RI : (int16_t)(100*innovRng), // range finder innovations
meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range meaRng : (uint16_t)(100*rangeDataDelayed.rng), // measured range

Loading…
Cancel
Save