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."
- 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