bug fixes on mag gps mix

src/main/flight/imu.c

@@ -77,7 +77,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.2 // 20% or magnetic field error soft-accepted (0.8-1.2)
+#define MAX_MAG_NEARNESS 0.25 // 25% or magnetic field error soft-accepted (0.75-1.25)
+#define COS5DEG 0.996f
+#define COS20DEG 0.940f
 #define IMU_ROTATION_LPF 3 // Hz
 FASTRAM fpVector3_t imuMeasuredAccelBF;
 FASTRAM fpVector3_t imuMeasuredRotationBF;
@@ -330,16 +332,15 @@ bool isGPSTrustworthy(void)
  return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
 }
 
-static float imuCalculateMcMagCogWeight(void)
+static float imuCalculateMcCogWeight(void)
 {
  float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
  float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
  const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate + imuConfig()->acc_ignore_slope)) * 4.0f;
  wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
  return wCoG;
 }
-#define COS5DEG 0.996f
-#define COS25DEG 0.906f
+
 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 };
@@ -394,18 +395,11 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
  if (useCOG) {
  fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
  //vForward as trust vector
- if STATE(MULTIROTOR){
+ if (STATE(MULTIROTOR)){
  vForward.z = 1.0f;
  }else{
  vForward.x = 1.0f;
  }
- if (STATE(MULTIROTOR)){
- //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
- wCoG *= imuCalculateMcMagCogWeight();
- //scale accroading to multirotor`s tilt angle
- wCoG *= scaleRangef(constrainf(vForward.z, COS5DEG, COS25DEG), COS5DEG, COS25DEG, 0.0f, 1.0f);
- //for inverted flying, wCoG is lowered by imuCalculateMcMagCogWeight
- }
  fpVector3_t vHeadingEF;
 
  // Use raw heading error (from GPS or whatever else)
@@ -430,9 +424,17 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
  vectorNormalize(&vCoG, &vCoG);
  }
 #endif
- wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1500), 400, 1500, 0.0f, 1.0f);
+ wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1200), 400, 1200, 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, COS5DEG), COS20DEG, COS5DEG, 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

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/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());