ekf2: Allow use of RTK GPS heading

src/modules/ekf2/ekf2_main.cpp

@@ -965,6 +965,8 @@ void Ekf2::run()
 				_gps_state[0].lat = gps.lat;
 				_gps_state[0].lon = gps.lon;
 				_gps_state[0].alt = gps.alt;
+				_gps_state[0].yaw = gps.heading;
+				_gps_state[0].yaw_offset = gps.heading_offset;
 				_gps_state[0].fix_type = gps.fix_type;
 				_gps_state[0].eph = gps.eph;
 				_gps_state[0].epv = gps.epv;
@@ -994,6 +996,8 @@ void Ekf2::run()
 				_gps_state[1].lat = gps.lat;
 				_gps_state[1].lon = gps.lon;
 				_gps_state[1].alt = gps.alt;
+				_gps_state[1].yaw = gps.heading;
+				_gps_state[1].yaw_offset = gps.heading_offset;
 				_gps_state[1].fix_type = gps.fix_type;
 				_gps_state[1].eph = gps.eph;
 				_gps_state[1].epv = gps.epv;
@@ -1074,6 +1078,8 @@ void Ekf2::run()
 				gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned[2];
 				gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid;
 				gps.satellites_used = _gps_output[_gps_select_index].nsats;
+				gps.heading = _gps_output[_gps_select_index].yaw;
+				gps.heading_offset = _gps_output[_gps_select_index].yaw_offset;
 				gps.selected = _gps_select_index;
 
 				// Publish to the EKF blended GPS topic
@@ -2165,6 +2171,19 @@ void Ekf2::update_gps_blend_states()
 	_gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
 	_gps_blended_state.alt += (int32_t)blended_alt_offset_mm;
 
+	// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
+	uint8_t gps_best_yaw_index = 0;
+	best_weight = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) {
+			best_weight = _blend_weights[i];
+			gps_best_yaw_index = i;
+		}
+	}
+
+	_gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw;
+	_gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset;
 }
 
 /*
@@ -2266,6 +2285,9 @@ void Ekf2::apply_gps_offsets()
 		_gps_output[i].gdop		= _gps_state[i].gdop;
 		_gps_output[i].nsats		= _gps_state[i].nsats;
 		_gps_output[i].vel_ned_valid	= _gps_state[i].vel_ned_valid;
+		_gps_output[i].yaw		= _gps_state[i].yaw;
+		_gps_output[i].yaw_offset	= _gps_state[i].yaw_offset;
+
 	}
 }
 
@@ -2325,6 +2347,8 @@ void Ekf2::calc_gps_blend_output()
 	_gps_output[GPS_BLENDED_INSTANCE].gdop		= _gps_blended_state.gdop;
 	_gps_output[GPS_BLENDED_INSTANCE].nsats		= _gps_blended_state.nsats;
 	_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid	= _gps_blended_state.vel_ned_valid;
+	_gps_output[GPS_BLENDED_INSTANCE].yaw		= _gps_blended_state.yaw;
+	_gps_output[GPS_BLENDED_INSTANCE].yaw_offset	= _gps_blended_state.yaw_offset;
 
 }
 

src/modules/ekf2/ekf2_params.c

@@ -490,13 +490,14 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
  * If set to '3-axis' 3-axis field fusion is used at all times.
  * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
  * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests controlled by the EKF2_MOVE_TEST parameter indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
- *
+ * If set to 'None' the magnetometer will not be used under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
  * @group EKF2
  * @value 0 Automatic
  * @value 1 Magnetic heading
  * @value 2 3-axis
  * @value 3 VTOL customn
  * @value 4 MC custom
+ * @value 5 None
  * @reboot_required true
  */
 PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
@@ -581,13 +582,14 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * 1 : Set to true to use optical flow data if available
  * 2 : Set to true to inhibit IMU bias estimation
  * 3 : Set to true to enable vision position fusion
- * 4 : Set to true to enable vision yaw fusion
+ * 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
  * 5 : Set to true to enable multi-rotor drag specific force fusion
  * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
+ * 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
  *
  * @group EKF2
  * @min 0
- * @max 127
+ * @max 255
  * @bit 0 use GPS
  * @bit 1 use optical flow
  * @bit 2 inhibit IMU bias estimation
@@ -595,6 +597,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
  * @bit 4 vision yaw fusion
  * @bit 5 multi-rotor drag fusion
  * @bit 6 rotate external vision
+ * @bit 7 GPS yaw fusion
  * @reboot_required true
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);