Merge pull request #9387 from shota3527/sh_ahrs

Yaw/Altitude estimation sensor fusion

docs/Settings.md

@@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements
 
 ---
 
+### ahrs_gps_yaw_weight
+
+Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 0 | 500 |
+
+---
+
 ### ahrs_gps_yaw_windcomp
 
 Wind compensation in heading estimation from gps groundcourse(fixed wing only)
@@ -1474,11 +1484,11 @@ Enable automatic configuration of UBlox GPS receivers.
 
 ### gps_dyn_model
 
-GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying.
+GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| AIR_1G |  |  |
+| AIR_2G |  |  |
 
 ---
 
@@ -1912,33 +1922,23 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
 
 ---
 
-### inav_w_xyz_acc_p
-
-_// TODO_
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 1.0 | 0 | 1 |
-
----
-
 ### inav_w_z_baro_p
 
-Weight of barometer measurements in estimated altitude and climb rate
+Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.35 | 0 | 10 |
+| 0.4 | 0 | 10 |
 
 ---
 
 ### inav_w_z_gps_p
 
-Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes
+Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.2 | 0 | 10 |
+| 0.4 | 0 | 10 |
 
 ---
 
@@ -1948,7 +1948,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0.1 | 0 | 10 |
+| 0.8 | 0 | 10 |
 
 ---
 

src/main/blackbox/blackbox.c

@@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"accSmooth",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"accSmooth",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
+    {"accVib",     -1, UNSIGNED, .Ipredict = PREDICT(0),       .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
     {"attitude",    0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
     {"attitude",    2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
@@ -487,6 +488,7 @@ typedef struct blackboxMainState_s {
     int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
 
     int16_t accADC[XYZ_AXIS_COUNT];
+    int16_t accVib;
     int16_t attitude[XYZ_AXIS_COUNT];
     int32_t debug[DEBUG32_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -917,6 +919,7 @@ static void writeIntraframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
+        blackboxWriteUnsignedVB(blackboxCurrent->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1182,6 +1185,7 @@ static void writeInterframe(void)
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
         blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
+        blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
     }
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
@@ -1601,6 +1605,7 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
     }
+    blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
 
     if (STATE(FIXED_WING_LEGACY)) {
 

src/main/drivers/compass/compass_fake.c

@@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev)
 {
     UNUSED(magDev);
     // initially point north
-    fakeMagData[X] = 4096;
+    fakeMagData[X] = 1024;
     fakeMagData[Y] = 0;
     fakeMagData[Z] = 0;
     return true;

src/main/fc/fc_msp.c

@@ -478,7 +478,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
             }
             for (int i = 0; i < 3; i++) {
 #ifdef USE_MAG
-                sbufWriteU16(dst, mag.magADC[i]);
+                sbufWriteU16(dst, lrintf(mag.magADC[i]));
 #else
                 sbufWriteU16(dst, 0);
 #endif

src/main/fc/settings.yaml

@@ -50,7 +50,7 @@ tables:
     values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
     enum: sbasMode_e
   - name: gps_dyn_model
-    values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
+    values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
     enum: gpsDynModel_e
   - name: reset_type
     values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
@@ -1463,6 +1463,12 @@ groups:
         default_value: ADAPTIVE
         field: inertia_comp_method
         table: imu_inertia_comp_method
+      - name: ahrs_gps_yaw_weight
+        description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass"
+        default_value: 100
+        field: gps_yaw_weight
+        min: 0
+        max: 500
 
   - name: PG_ARMING_CONFIG
     type: armingConfig_t
@@ -1608,8 +1614,8 @@ groups:
         table: gps_sbas_mode
         type: uint8_t
       - name: gps_dyn_model
-        description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying."
+        description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying."
-        default_value: "AIR_1G"
+        default_value: "AIR_2G"
         field: dynModel
         table: gps_dyn_model
         type: uint8_t
@@ -2322,23 +2328,23 @@ groups:
         max: 100
         default_value: 2.0
       - name: inav_w_z_baro_p
-        description: "Weight of barometer measurements in estimated altitude and climb rate"
+        description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_baro_p
         min: 0
         max: 10
-        default_value: 0.35
+        default_value: 0.4
       - name: inav_w_z_gps_p
-        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes"
+        description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
         field: w_z_gps_p
         min: 0
         max: 10
-        default_value: 0.2
+        default_value: 0.4
       - name: inav_w_z_gps_v
         description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
         field: w_z_gps_v
         min: 0
         max: 10
-        default_value: 0.1
+        default_value: 0.8
       - name: inav_w_xy_gps_p
         description: "Weight of GPS coordinates in estimated UAV position and speed."
         default_value: 1.0
@@ -2363,11 +2369,6 @@ groups:
         field: w_xy_res_v
         min: 0
         max: 10
-      - name: inav_w_xyz_acc_p
-        field: w_xyz_acc_p
-        min: 0
-        max: 1
-        default_value: 1.0
       - name: inav_w_acc_bias
         description: "Weight for accelerometer drift estimation"
         default_value: 0.01

src/main/flight/imu.c

@@ -49,6 +49,7 @@
 
 #include "flight/imu.h"
 #include "flight/mixer.h"
+#include "flight/mixer_profile.h"
 #include "flight/pid.h"
 #if defined(USE_WIND_ESTIMATOR)
 #include "flight/wind_estimator.h"
@@ -77,6 +78,9 @@
 
 #define SPIN_RATE_LIMIT             20
 #define MAX_ACC_NEARNESS            0.2    // 20% or G error soft-accepted (0.8-1.2G)
+#define MAX_MAG_NEARNESS            0.25    // 25% or magnetic field error soft-accepted (0.75-1.25)
+#define COS10DEG 0.985f
+#define COS20DEG 0.940f
 #define IMU_ROTATION_LPF         3       // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -111,6 +115,8 @@ FASTRAM bool gpsHeadingInitialized;
 
 FASTRAM bool imuUpdated = false;
 
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF);
+
 PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
@@ -122,7 +128,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
     .acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
     .acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
     .gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
-    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
+    .inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
+    .gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT
 );
 
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@@ -326,6 +333,15 @@ bool isGPSTrustworthy(void)
     return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
 }
 
+static float imuCalculateMcCogWeight(void)
+{
+    float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
+    float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
+    const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f;
+    wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
+    return wCoG;
+}
+
 static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
 {
     STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
@@ -339,11 +355,15 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
     /* Step 1: Yaw correction */
     // Use measured magnetic field vector
     if (magBF || useCOG) {
-        static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
+        float wMag = 1.0f;
+        float wCoG = 1.0f;
+        if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;}
 
-        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
 
         if (magBF && vectorNormSquared(magBF) > 0.01f) {
+            wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS);
             fpVector3_t vMag;
 
             // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
@@ -369,13 +389,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
 
                 // Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
                 // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
-                vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
+                vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vMagErr, &vMagErr, &orientation);
             }
         }
-        else if (useCOG) {
+        if (useCOG) {
+            fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
+            //vForward as trust vector
+            if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
+                vForward.z = 1.0f;
+            }else{
+                vForward.x = 1.0f;
+            }
             fpVector3_t vHeadingEF;
 
             // Use raw heading error (from GPS or whatever else)
@@ -386,6 +413,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             // (Rxx; Ryx) - measured (estimated) heading vector (EF)
             // (-cos(COG), sin(COG)) - reference heading vector (EF)
 
+            float airSpeed = gpsSol.groundSpeed;
             // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards.
             fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
 #if defined(USE_WIND_ESTIMATOR)
@@ -395,12 +423,21 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed);
                 vCoG.x += getEstimatedWindSpeed(X);
                 vCoG.y -= getEstimatedWindSpeed(Y);
+                airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG));
                 vectorNormalize(&vCoG, &vCoG);
             }
 #endif
-
+            wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
             // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
             quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
+
+            if (STATE(MULTIROTOR)){
+                //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
+                wCoG *= imuCalculateMcCogWeight();
+                //scale accroading to multirotor`s tilt angle
+                wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f);
+                //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
+            }
             vHeadingEF.z = 0.0f;
 
             // We zeroed out vHeadingEF.z -  make sure the whole vector didn't go to zero
@@ -409,13 +446,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
                 vectorNormalize(&vHeadingEF, &vHeadingEF);
 
                 // error is cross product between reference heading and estimated heading (calculated in EF)
-                vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
+                vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF);
 
                 // Rotate error back into body frame
-                quaternionRotateVector(&vErr, &vErr, &orientation);
+                quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation);
             }
         }
-
+        fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
+        vectorScale(&vMagErr, &vMagErr, wMag);
+        vectorScale(&vCoGErr, &vCoGErr, wCoG);
+        vectorAdd(&vErr, &vMagErr, &vCoGErr);
         // Compute and apply integral feedback if enabled
         if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
             // Stop integrating if spinning beyond the certain limit
@@ -535,10 +575,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     }
 }
 
-static float imuCalculateAccelerometerWeightNearness(void)
+static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF)
 {
     fpVector3_t accBFNorm;
-    vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS);
+    vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS);
     const float accMagnitudeSq = vectorNormSquared(&accBFNorm);
     const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
     return accWeight_Nearness;
@@ -672,36 +712,29 @@ static void imuCalculateEstimatedAttitude(float dT)
     bool useMag = false;
     bool useCOG = false;
 #if defined(USE_GPS)
-    if (STATE(FIXED_WING_LEGACY)) {
+    bool canUseCOG = isGPSHeadingValid();
-        bool canUseCOG = isGPSHeadingValid();
 
-        // Prefer compass (if available)
+    // Use compass (if available)
-        if (canUseMAG) {
+    if (canUseMAG) {
-            useMag = true;
+        useMag = true;
-            gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
+        gpsHeadingInitialized = true;   // GPS heading initialised from MAG, continue on GPS if compass fails
-        }
-        else if (canUseCOG) {
-            if (gpsHeadingInitialized) {
-                courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
-                useCOG = true;
-            }
-            else {
-                // Re-initialize quaternion from known Roll, Pitch and GPS heading
-                imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
-                gpsHeadingInitialized = true;
-
-                // Force reset of heading hold target
-                resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
-            }
-        } else if (!ARMING_FLAG(ARMED)) {
-            gpsHeadingInitialized = false;
-        }
     }
-    else {
+    // Use GPS (if available)
-        // Multicopters don't use GPS heading
+    if (canUseCOG) {
-        if (canUseMAG) {
+        if (gpsHeadingInitialized) {
-            useMag = true;
+            courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
+            useCOG = true;
         }
+        else if (!canUseMAG) {
+            // Re-initialize quaternion from known Roll, Pitch and GPS heading
+            imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
+            gpsHeadingInitialized = true;
+
+            // Force reset of heading hold target
+            resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+        }
+    } else if (!ARMING_FLAG(ARMED)) {
+        gpsHeadingInitialized = false;
     }
 
     imuCalculateFilters(dT);
@@ -751,7 +784,7 @@ static void imuCalculateEstimatedAttitude(float dT)
     }
     compansatedGravityBF = imuMeasuredAccelBF
 #endif
-    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness();
+    float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
     accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
     const bool useAcc = (accWeight > 0.001f);
 
@@ -807,6 +840,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
     }
 }
  
+
 bool isImuReady(void)
 {
     return sensors(SENSOR_ACC) && STATE(ACCELEROMETER_CALIBRATED) && gyroIsCalibrationComplete();
@@ -814,7 +848,7 @@ bool isImuReady(void)
 
 bool isImuHeadingValid(void)
 {
-    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
+    return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized;
 }
 
 float calculateCosTiltAngle(void)

src/main/flight/imu.h

@@ -54,6 +54,7 @@ typedef struct imuConfig_s {
     uint8_t acc_ignore_slope;
     uint8_t gps_yaw_windcomp;
     uint8_t inertia_comp_method;
+    uint16_t gps_yaw_weight;
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);

src/main/io/gps.c

@@ -453,7 +453,7 @@ bool isGPSHealthy(void)
 
 bool isGPSHeadingValid(void)
 {
-    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
+    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 400;
 }
 
 #endif

src/main/io/gps.h

@@ -76,7 +76,9 @@ typedef enum {
 
 typedef enum {
     GPS_DYNMODEL_PEDESTRIAN = 0,
+    GPS_DYNMODEL_AUTOMOTIVE,
     GPS_DYNMODEL_AIR_1G,
+    GPS_DYNMODEL_AIR_2G,
     GPS_DYNMODEL_AIR_4G,
 } gpsDynModel_e;
 

src/main/io/gps_fake.c

@@ -31,6 +31,7 @@
 
 #include "platform.h"
 #include "build/build_config.h"
+#include "common/log.h"
 
 #if defined(USE_GPS_FAKE)
 

src/main/io/gps_ublox.c

@@ -800,10 +800,16 @@ STATIC_PROTOTHREAD(gpsConfigure)
         case GPS_DYNMODEL_PEDESTRIAN:
             configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
             break;
-        case GPS_DYNMODEL_AIR_1G:   // Default to this
+        case GPS_DYNMODEL_AUTOMOTIVE:
-        default:
+            configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO);
+            break;
+        case GPS_DYNMODEL_AIR_1G:
             configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
             break;
+        case GPS_DYNMODEL_AIR_2G:   // Default to this
+        default:
+            configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO);
+            break;
         case GPS_DYNMODEL_AIR_4G:
             configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
             break;

src/main/io/gps_ublox.h

@@ -34,7 +34,9 @@ extern "C" {
 #define GPS_CAPA_INTERVAL                   5000
 
 #define UBX_DYNMODEL_PEDESTRIAN 3
+#define UBX_DYNMODEL_AUTOMOVITE 4
 #define UBX_DYNMODEL_AIR_1G     6
+#define UBX_DYNMODEL_AIR_2G     7
 #define UBX_DYNMODEL_AIR_4G     8
 
 #define UBX_FIXMODE_2D_ONLY 1

src/main/navigation/navigation.h

@@ -203,7 +203,6 @@ typedef struct positionEstimationConfig_s {
     float w_xy_res_v;
 
     float w_acc_bias;   // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
-    float w_xyz_acc_p;
 
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error

src/main/navigation/navigation_pos_estimator.c

@@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
 
-        .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
-
         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
 
         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
@@ -389,28 +387,30 @@ static bool gravityCalibrationComplete(void)
 
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
-
+#define ACC_VIB_FACTOR_S 1.0f
+#define ACC_VIB_FACTOR_E 3.0f
 static void updateIMUEstimationWeight(const float dt)
 {
-    bool isAccClipped = accIsClipped();
+    static float acc_clip_factor = 1.0f;
-
+    // If accelerometer measurement is clipped - drop the acc weight to 0.3
-    // If accelerometer measurement is clipped - drop the acc weight to zero
     // and gradually restore weight back to 1.0 over time
-    if (isAccClipped) {
+    if (accIsClipped()) {
-        posEstimator.imu.accWeightFactor = 0.0f;
+        acc_clip_factor = 0.5f;
     }
     else {
         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
-        posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
+        acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
     }
-
+    // Update accelerometer weight based on vibration levels and clipping
+    float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
+    posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
     // DEBUG_VIBE[0-3] are used in IMU
     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 }
 
 float navGetAccelerometerWeight(void)
 {
-    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    const float accWeightScaled = posEstimator.imu.accWeightFactor;
     DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
 
     return accWeightScaled;
@@ -553,13 +553,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 
 static void estimationPredict(estimationContext_t * ctx)
 {
-    const float accWeight = navGetAccelerometerWeight();
 
     /* Prediction step: Z-axis */
     if ((ctx->newFlags & EST_Z_VALID)) {
         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
-        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
-        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
     }
 
     /* Prediction step: XY-axis */
@@ -570,10 +569,10 @@ static void estimationPredict(estimationContext_t * ctx)
 
         // If heading is valid, accelNEU is valid as well. Account for acceleration
         if (navIsHeadingUsable() && navIsAccelerationUsable()) {
-            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
+            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
-            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
-            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
-            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
         }
     }
 }
@@ -589,7 +588,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
-    if (ctx->newFlags & EST_BARO_VALID) {
+    bool correctOK = false;
+    
+    //ignore baro if difference is too big, baro is probably wrong
+    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+    //fade out the baro to prevent sudden jump
+    const float start_epv = positionEstimationConfig()->max_eph_epv;
+    const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+    const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+    //use both baro and gps
+    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -599,44 +607,33 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
         else {
             if (posEstimator.est.vel.z > 15) {
-                if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
+                posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
-                    posEstimator.state.isBaroGroundValid = false;
-                }
             }
             else {
                 posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
             }
         }
 
-        // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
+        // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
         bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
                                             (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 
         // Altitude
         const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
-        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
+        ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
-
-        // If GPS is available - also use GPS climb rate
-        if (ctx->newFlags & EST_GPS_Z_VALID) {
-            // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
-            const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-            const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
-            ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
-        }
 
         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
         // Accelerometer bias
         if (!isAirCushionEffectDetected) {
-            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+            ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
         }
 
-        return true;
+        correctOK = true;
     }
-    else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+    if (ctx->newFlags & EST_GPS_Z_VALID) {
-        // If baro is not available - use GPS Z for correction on a plane
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -646,20 +643,21 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         else {
             // Altitude
             const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
+            const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
 
             ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
             ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
-            ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
             ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
 
             // Accelerometer bias
             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
 
-        return true;
+        correctOK = true;
     }
 
-    return false;
+    return correctOK;
 }
 
 static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
@@ -714,15 +712,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
 
 static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
 {
+    UNUSED(currentTimeUs);
     if (STATE(GPS_FIX) && navIsHeadingUsable()) {
-        static timeUs_t lastUpdateTimeUs = 0;
+        uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
-
+        posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
-        if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) {   // limit update rate
-            const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
-            uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
-            posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
-            lastUpdateTimeUs = currentTimeUs;
-        }
     }
 }
 
@@ -779,7 +772,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
     if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
     }
-
+    // Boost the corrections based on accWeight
+    const float accWeight = navGetAccelerometerWeight();
+    vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
+    vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
     // Apply corrections
     vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
@@ -817,13 +813,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 {
     static navigationTimer_t posPublishTimer;
 
-    /* IMU operates in decidegrees while INAV operates in deg*100
-     * Use course over ground when GPS heading valid */
-    int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
-    updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
-
     /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
+        /* Publish heading update */
+        /* IMU operates in decidegrees while INAV operates in deg*100
+        * Use course over ground when GPS heading valid */
+        int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
+        updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
+
         /* Publish position update */
         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
             // FIXME!!!!!

src/main/navigation/navigation_pos_estimator_private.h

@@ -39,7 +39,6 @@
 
 #define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
 #define INAV_PITOT_UPDATE_RATE              10
-#define INAV_COG_UPDATE_RATE_HZ             20      // ground course update rate
 
 #define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
 #define INAV_BARO_TIMEOUT_MS                200     // Baro timeout

src/main/sensors/acceleration.c

@@ -576,6 +576,7 @@ void accUpdate(void)
         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
         const float accDiff = acc.accADCf[axis] - accFloorFilt;
         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
+        acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
     }
 
     // Filter acceleration
@@ -612,7 +613,7 @@ void accGetVibrationLevels(fpVector3_t *accVibeLevels)
 
 float accGetVibrationLevel(void)
 {
-    return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
+    return acc.accVibe;
 }
 
 uint32_t accGetClipCount(void)

src/main/sensors/acceleration.h

@@ -27,7 +27,7 @@
 #define GRAVITY_CMSS    980.665f
 #define GRAVITY_MSS     9.80665f
 
-#define ACC_CLIPPING_THRESHOLD_G        7.9f
+#define ACC_CLIPPING_THRESHOLD_G        15.9f
 #define ACC_VIBE_FLOOR_FILT_HZ          5.0f
 #define ACC_VIBE_FILT_HZ                2.0f
 
@@ -58,6 +58,7 @@ typedef struct acc_s {
     uint32_t accTargetLooptime;
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
     float accVibeSq[XYZ_AXIS_COUNT];
+    float accVibe;
     uint32_t accClipCount;
     bool isClipped;
     acc_extremes_t extremes[XYZ_AXIS_COUNT];

src/main/target/SITL/sim/realFlight.c

@@ -296,9 +296,9 @@ static void exchangeData(void)
     //rfValues.m_orientationQuaternion_W = getDoubleFromResponse(response, "m-orientationQuaternion-W");
     rfValues.m_aircraftPositionX_MTR = getDoubleFromResponse(response, "m-aircraftPositionX-MTR");
     rfValues.m_aircraftPositionY_MTR = getDoubleFromResponse(response, "m-aircraftPositionY-MTR");
-    //rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
+    rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
-    //rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
+    rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
-    //rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
+    rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
     //rfValues.m_velocityBodyU_MPS = getDoubleFromResponse(response, "m-velocityBodyU-MPS");
     //rfValues.m_velocityBodyV_MPS = getDoubleFromResponse(response, "mm-velocityBodyV-MPS");
     //rfValues.m_velocityBodyW_MPS = getDoubleFromResponse(response, "m-velocityBodyW-MPS");
@@ -332,7 +332,7 @@ static void exchangeData(void)
     float lat, lon;
     fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
     
-    int16_t course = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
+    int16_t course = (int16_t)roundf(RADIANS_TO_DECIDEGREES(atan2_approx(-rfValues.m_velocityWorldU_MPS,rfValues.m_velocityWorldV_MPS)));
     int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100);
     gpsFakeSet(
         GPS_FIX_3D,
@@ -342,9 +342,9 @@ static void exchangeData(void)
         altitude,
         (int16_t)roundf(rfValues.m_groundspeed_MPS * 100),
         course,
-        0, 
+        0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction
-        0,
+        0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),
-        0,
+        0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),
         0
     );
 
@@ -357,7 +357,7 @@ static void exchangeData(void)
 
     const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10);
     const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10);
-    const int16_t yaw_inav = course;
+    const int16_t yaw_inav = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
     if (!useImu) {
         imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
         imuUpdateAttitude(micros());
@@ -399,9 +399,9 @@ static void exchangeData(void)
     computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
     transformVectorEarthToBody(&north, &quat);
     fakeMagSet(
-        constrainToInt16(north.x * 16000.0f),
+        constrainToInt16(north.x * 1024.0f),
-        constrainToInt16(north.y * 16000.0f),
+        constrainToInt16(north.y * 1024.0f),
-        constrainToInt16(north.z * 16000.0f)
+        constrainToInt16(north.z * 1024.0f)
     );
 
     free(rfValues.m_currentAircraftStatus);

src/main/target/SITL/sim/xplane.c

@@ -421,9 +421,9 @@ static void* listenWorker(void* arg)
         computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
         transformVectorEarthToBody(&north, &quat);
         fakeMagSet(
-            constrainToInt16(north.x * 16000.0f),
+            constrainToInt16(north.x * 1024.0f),
-            constrainToInt16(north.y * 16000.0f),
+            constrainToInt16(north.y * 1024.0f),
-            constrainToInt16(north.z * 16000.0f)
+            constrainToInt16(north.z * 1024.0f)
         );
 
         if (!initalized) {