From ba5a7d9b8aa93211366954735e27827f43c328e4 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar Do Carmo Lucas" Date: Fri, 9 Jun 2017 14:36:58 +0200 Subject: [PATCH] Always deliver horizontal and vertical accuracy if position is known - The current code only delivers this when in fix or float state, and that confuses the autopilot - reduces the number of sqrt() function calls from 4 to 2 - Document a bit more --- src/erb.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/erb.c b/src/erb.c index 7687843df..18690a441 100644 --- a/src/erb.c +++ b/src/erb.c @@ -127,22 +127,22 @@ static void buildver(char *payload, struct erb_ver version, const uint32_t time, static void buildpos(char *payload, struct erb_pos position, const uint32_t time, const sol_t *sol) { - float stdX,stdY,stdZ; + float stdX2,stdY2,stdZ2; double pos[3]; ecef2pos(sol->rr, pos); - /* if fix or float, then get std X/Y/Z */ - stdX = (sol->stat == SOLQ_FIX || sol->stat == SOLQ_FLOAT) ? SQRT(sol->qr[0]) : 0; - stdY = (sol->stat == SOLQ_FIX || sol->stat == SOLQ_FLOAT) ? SQRT(sol->qr[1]) : 0; - stdZ = (sol->stat == SOLQ_FIX || sol->stat == SOLQ_FLOAT) ? SQRT(sol->qr[2]) : 0; + /* if position available, then get std^2 X/Y/Z */ + stdX2 = (sol->stat != SOLQ_NONE && sol->stat != SOLQ_DR && sol->qr[0] > 0) ? sol->qr[0] : 0; + stdY2 = (sol->stat != SOLQ_NONE && sol->stat != SOLQ_DR && sol->qr[1] > 0) ? sol->qr[1] : 0; + stdZ2 = (sol->stat != SOLQ_NONE && sol->stat != SOLQ_DR && sol->qr[2] > 0) ? sol->qr[2] : 0; position.timeGPS = time; position.lng = pos[1] * R2D; position.lat = pos[0] * R2D; position.altEl = pos[2]; position.altMsl = pos[2] - geoidh(pos); - position.accHor = 1000 * SQRT(stdX * stdX + stdY * stdY); - position.accVer = 1000 * stdZ; + position.accHor = 1000 * SQRT(stdX2 + stdY2); /* convert float [m] to int32 [mm] */ + position.accVer = 1000 * SQRT(stdZ2); /* convert float [m] to int32 [mm] */ memcpy(payload, &position, LENGTH_POS); }