From c3d6ed1e3e8e88db7b7207395364e9562a6791f4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 1 May 2014 16:54:31 +0900 Subject: [PATCH] Replay: add constraint on EKF4 data packet values --- Tools/Replay/Replay.pde | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index 38ea893c0d..62b7e16ffe 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -49,6 +49,9 @@ #include #include +#define INT16_MIN -32768 +#define INT16_MAX 32767 + #include "LogReader.h" const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; @@ -509,15 +512,15 @@ void loop() innovVT); // define messages for EKF4 data packet - int16_t sqrtvarV = (int16_t)(100*velVar); - int16_t sqrtvarP = (int16_t)(100*posVar); - int16_t sqrtvarH = (int16_t)(100*hgtVar); - int16_t sqrtvarMX = (int16_t)(100*magVar.x); - int16_t sqrtvarMY = (int16_t)(100*magVar.y); - int16_t sqrtvarMZ = (int16_t)(100*magVar.z); - int16_t sqrtvarVT = (int16_t)(100*tasVar); - int16_t offsetNorth = (int8_t)(offset.x); - int16_t offsetEast = (int8_t)(offset.y); + int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); + int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); + int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); + int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); + int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); + int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); + int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); + int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); + int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); // print EKF4 data packet fprintf(ekf4f, "%.3f %d %d %d %d %d %d %d %d %d %d\n",