Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: add constraint on EKF4 data packet values

This commit is contained in:
Randy Mackay 2014-05-01 16:54:31 +09:00
parent d857427444
commit c3d6ed1e3e
1 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Tools/Replay/Replay.pde
View File

@ -49,6 +49,9 @@
#include <errno.h>
#include <fenv.h>
#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",