diff --git a/docs/Settings.md b/docs/Settings.md index 29ac257fbd1..989da571cbd 100644 --- a/docs/Settings.md +++ b/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 | --- diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb12a7a8968..ae64a3e35a2 100644 --- a/src/main/blackbox/blackbox.c +++ b/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)) { diff --git a/src/main/drivers/compass/compass_fake.c b/src/main/drivers/compass/compass_fake.c index a95b3297a5f..2ef8692fd2d 100644 --- a/src/main/drivers/compass/compass_fake.c +++ b/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; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 79e7418ced9..952f13dddb0 100644 --- a/src/main/fc/fc_msp.c +++ b/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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index adcd21c6fcc..af93fb31988 100644 --- a/src/main/fc/settings.yaml +++ b/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." - default_value: "AIR_1G" + 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_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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ed5cbcbfaa4..afdc86e6bf9 100644 --- a/src/main/flight/imu.c +++ b/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) - if (canUseMAG) { - useMag = true; - gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails + // Use compass (if available) + if (canUseMAG) { + useMag = true; + gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails + } + // Use GPS (if available) + if (canUseCOG) { + if (gpsHeadingInitialized) { + courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + useCOG = true; } - 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; + 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; - } - } - else { - // Multicopters don't use GPS heading - if (canUseMAG) { - useMag = 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); @@ -806,6 +839,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) acc.accADCf[Z] = 0.0f; } } + bool isImuReady(void) { @@ -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) diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index a3b87e67360..8afcc50e579 100644 --- a/src/main/flight/imu.h +++ b/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); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0fae3a7f318..f71abe897d4 100755 --- a/src/main/io/gps.c +++ b/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 diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 199652a3a24..3e9d073c54c 100755 --- a/src/main/io/gps.h +++ b/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; diff --git a/src/main/io/gps_fake.c b/src/main/io/gps_fake.c index 6d574893436..5b0de8c50b5 100644 --- a/src/main/io/gps_fake.c +++ b/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) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index da0f54dd2fb..b2f9843aee1 100755 --- a/src/main/io/gps_ublox.c +++ b/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 - default: + case GPS_DYNMODEL_AUTOMOTIVE: + 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; diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 00b42eeb2b7..8ec9be16d09 100644 --- a/src/main/io/gps_ublox.h +++ b/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 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 3fb660fcc90..3c830f2c1c6 100644 --- a/src/main/navigation/navigation.h +++ b/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 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d7ef73c9382..f66c641928b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/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(); - - // If accelerometer measurement is clipped - drop the acc weight to zero + static float acc_clip_factor = 1.0f; + // If accelerometer measurement is clipped - drop the acc weight to 0.3 // and gradually restore weight back to 1.0 over time - if (isAccClipped) { - posEstimator.imu.accWeightFactor = 0.0f; + if (accIsClipped()) { + 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.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); + posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; + 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.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight; - posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight); - posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(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; + posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt; + 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 = false; - } + posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true; } 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->estVelCorr.z += 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->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * 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 baro is not available - use GPS Z for correction on a plane + if (ctx->newFlags & EST_GPS_Z_VALID) { // 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; - - 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; - } + 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); } } @@ -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!!!!! diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 40129f844b0..0b19dbb0023 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/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 diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index f897d7e580a..695f49b00b2 100644 --- a/src/main/sensors/acceleration.c +++ b/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) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index d81a83a9084..f3385edc6d5 100644 --- a/src/main/sensors/acceleration.h +++ b/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]; diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index 23e77bcde04..b4e5b9283ab 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/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_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS"); - //rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS"); + rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS"); + rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-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, - 0, + 0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction + 0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100), + 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.y * 16000.0f), - constrainToInt16(north.z * 16000.0f) + constrainToInt16(north.x * 1024.0f), + constrainToInt16(north.y * 1024.0f), + constrainToInt16(north.z * 1024.0f) ); free(rfValues.m_currentAircraftStatus); diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index 44089117d2e..d629c2130e3 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/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.y * 16000.0f), - constrainToInt16(north.z * 16000.0f) + constrainToInt16(north.x * 1024.0f), + constrainToInt16(north.y * 1024.0f), + constrainToInt16(north.z * 1024.0f) ); if (!initalized) {