vehicle_local_position: rename yaw -> heading and add reset logic

- vehicle_global_position yaw removed (redundant)

msg/vehicle_global_position.msg

@@ -16,8 +16,6 @@ float32 delta_alt 	# Reset delta for altitude
 uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates
 uint8 alt_reset_counter 	# Counter for reset events on altitude
 
-float32 yaw 			# Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians)
-
 float32 eph			# Standard deviation of horizontal position error, (metres)
 float32 epv			# Standard deviation of vertical position error, (metres)
 

msg/vehicle_local_position.msg

@@ -36,8 +36,9 @@ float32 ax        # North velocity derivative in NED earth-fixed frame, (metres/
 float32 ay        # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
 float32 az        # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
 
-# Heading
+float32 heading				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians)
-float32 yaw				# Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI,  (radians)
+float32 delta_heading
+uint8 heading_reset_counter
 
 # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
 bool xy_global				# true if position (x, y) has a valid global reference (ref_lat, ref_lon)

src/drivers/telemetry/frsky_telemetry/frsky_data.cpp

@@ -212,7 +212,7 @@ void frsky_send_frame2(int uart)
 	int sec = 0;
 
 	if (gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000) {
-		course = gpos.yaw / M_PI_F * 180.0f;
+		course = lpos.heading / M_PI_F * 180.0f;
 
 		if (course < 0.f) { // course is in range [0, 360], 0=north, CW
 			course += 360.f;

src/drivers/telemetry/frsky_telemetry/sPort_data.cpp

@@ -265,7 +265,7 @@ void sPort_send_GPS_CRS(int uart)
 	/* send course */
 
 	/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
-	int32_t iYaw = s_port_subscription_data->vehicle_local_position_sub.get().yaw * 18000.0f / M_PI_F;
+	int32_t iYaw = s_port_subscription_data->vehicle_local_position_sub.get().heading * 18000.0f / M_PI_F;
 
 	if (iYaw < 0) { iYaw += 36000; }
 

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -32,7 +32,6 @@ bool FlightTask::updateInitialize()
 	_time_stamp_last = _time_stamp_current;
 
 	_sub_vehicle_local_position.update();
-	_sub_attitude.update();
 	_sub_home_position.update();
 
 	_evaluateVehicleLocalPosition();
@@ -69,10 +68,9 @@ void FlightTask::_checkEkfResetCounters()
 		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
 	}
 
-	if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) {
+	if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
-		float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi();
+		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
-		_ekfResetHandlerHeading(delta_psi);
+		_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
-		_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
 	}
 }
 
@@ -117,14 +115,12 @@ void FlightTask::_evaluateVehicleLocalPosition()
 	_yaw = NAN;
 	_dist_to_bottom = NAN;
 
-	if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
-		// yaw
-		_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi();
-	}
-
 	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
 	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
 
+		// yaw
+		_yaw = _sub_vehicle_local_position.get().heading;
+
 		// position
 		if (_sub_vehicle_local_position.get().xy_valid) {
 			_position(0) = _sub_vehicle_local_position.get().x;

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -60,7 +60,7 @@ struct ekf_reset_counters_s {
 	uint8_t vxy;
 	uint8_t z;
 	uint8_t vz;
-	uint8_t quat;
+	uint8_t heading;
 };
 
 class FlightTask : public ModuleParams
@@ -189,7 +189,6 @@ public:
 
 protected:
 	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<vehicle_attitude_s> _sub_attitude{ORB_ID(vehicle_attitude)};
 	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
 
 	/** Reset all setpoints to NAN */

src/modules/commander/Commander.cpp

@@ -1199,7 +1199,8 @@ Commander::set_home_position()
 			home.y = lpos.y;
 			home.z = lpos.z;
 
-			home.yaw = lpos.yaw;
+			home.yaw = lpos.heading;
+			_heading_reset_counter = lpos.heading_reset_counter;
 
 			home.manual_home = false;
 
@@ -1248,20 +1249,6 @@ Commander::updateHomePositionYaw(float yaw)
 	_home_pub.update(home);
 }
 
-void
-Commander::checkEkfResetCounters()
-{
-	if (_attitude_sub.get().quat_reset_counter != _quat_reset_counter) {
-		const float delta_psi = matrix::Eulerf(matrix::Quatf(_attitude_sub.get().delta_q_reset)).psi();
-
-		if (!_home_pub.get().manual_home) {
-			updateHomePositionYaw(matrix::wrap_pi(_home_pub.get().yaw + delta_psi));
-		}
-
-		_quat_reset_counter = _attitude_sub.get().quat_reset_counter;
-	}
-}
-
 void
 Commander::run()
 {
@@ -1629,7 +1616,7 @@ Commander::run()
 			}
 		}
 
-		estimator_check();
+		estimator_check(status_flags);
 
 		/* Update land detector */
 		if (_land_detector_sub.updated()) {
@@ -2279,8 +2266,6 @@ Commander::run()
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
-		checkEkfResetCounters();
-
 		// automatically set or update home position
 		if (!_home_pub.get().manual_home) {
 			const vehicle_local_position_s &local_position = _local_position_sub.get();
@@ -4048,18 +4033,25 @@ void Commander::battery_status_check()
 	}
 }
 
-void Commander::estimator_check()
+void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
 {
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
 
 	_local_position_sub.update();
 	_global_position_sub.update();
-	_attitude_sub.update();
 
 	const vehicle_local_position_s &lpos = _local_position_sub.get();
 	const vehicle_global_position_s &gpos = _global_position_sub.get();
 
+	if (lpos.heading_reset_counter != _heading_reset_counter) {
+		if (vstatus_flags.condition_home_position_valid) {
+			updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading);
+		}
+
+		_heading_reset_counter = lpos.heading_reset_counter;
+	}
+
 	const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
 
 	if (_estimator_status_sub.update()) {

src/modules/commander/Commander.hpp

@@ -137,7 +137,7 @@ private:
 
 	void esc_status_check(const esc_status_s &esc_status);
 
-	void estimator_check();
+	void estimator_check(const vehicle_status_flags_s &status_flags);
 
 	bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
 			    uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
@@ -156,7 +156,6 @@ private:
 	bool set_home_position();
 	bool set_home_position_alt_only();
 	void updateHomePositionYaw(float yaw);
-	void checkEkfResetCounters();
 
 	void update_control_mode();
 
@@ -362,7 +361,7 @@ private:
 	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
 
 	uint32_t	_counter{0};
-	uint8_t		_quat_reset_counter{0};
+	uint8_t		_heading_reset_counter{0};
 
 	bool		_status_changed{true};
 	bool		_arm_tune_played{false};
@@ -423,7 +422,6 @@ private:
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::SubscriptionData<vehicle_attitude_s>		_attitude_sub{ORB_ID(vehicle_attitude)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};

src/modules/ekf2/ekf2_main.cpp

@@ -1305,7 +1305,11 @@ void Ekf2::Run()
 				// The rotation of the tangent plane vs. geographical north
 				const matrix::Quatf q = _ekf.getQuaternion();
 
-				lpos.yaw = matrix::Eulerf(q).psi();
+				matrix::Quatf delta_q_reset;
+				_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
+
+				lpos.heading = matrix::Eulerf(q).psi();
+				lpos.delta_heading = matrix::Eulerf(delta_q_reset).psi();
 
 				// Vehicle odometry quaternion
 				q.copyTo(odom.q);
@@ -1479,8 +1483,6 @@ void Ekf2::Run()
 					// global altitude has opposite sign of local down position
 					global_pos.delta_alt = -lpos.delta_z;
 
-					global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI.
-
 					_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
 
 					global_pos.terrain_alt_valid = lpos.dist_bottom_valid;

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -595,7 +595,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
 			_pub_lpos.get().z = xLP(X_z);	// down
 		}
 
-		_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
+		_pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
 
 		_pub_lpos.get().vx = xLP(X_vx);		// north
 		_pub_lpos.get().vy = xLP(X_vy);		// east
@@ -793,7 +793,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
-		_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;
 		_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -294,8 +294,9 @@ bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *m
 bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg)
 {
 	vehicle_global_position_s global_pos;
+	vehicle_local_position_s local_pos;
 
-	if (_global_pos_sub.update(&global_pos)) {
+	if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) {
 		msg->latitude = global_pos.lat * 1e7;
 		msg->longitude = global_pos.lon * 1e7;
 
@@ -310,7 +311,7 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m
 
 		msg->altitude = altitude;
 
-		msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
+		msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(local_pos.heading)) * 0.5f);
 
 		return true;
 	}

src/modules/mavlink/mavlink_messages.cpp

@@ -1534,7 +1534,7 @@ protected:
 			mavlink_vfr_hud_t msg{};
 			msg.airspeed = airspeed_validated.equivalent_airspeed_m_s;
 			msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy);
-			msg.heading = math::degrees(wrap_2pi(lpos.yaw));
+			msg.heading = math::degrees(wrap_2pi(lpos.heading));
 
 			if (armed.armed) {
 				actuator_controls_s act0{};
@@ -2469,7 +2469,7 @@ protected:
 			msg.vy = lpos.vy * 100.0f;
 			msg.vz = lpos.vz * 100.0f;
 
-			msg.hdg = math::degrees(wrap_2pi(lpos.yaw)) * 100.0f;
+			msg.hdg = math::degrees(wrap_2pi(lpos.heading)) * 100.0f;
 
 			mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
 
@@ -4870,7 +4870,7 @@ protected:
 
 			vehicle_local_position_s lpos{};
 			_lpos_sub.copy(&lpos);
-			msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.yaw + mount_orientation.attitude_euler_angle[2]));
+			msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.heading + mount_orientation.attitude_euler_angle[2]));
 
 			mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -2600,7 +2600,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vz = hil_state.vz / 100.0f;
 
 		matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
-		hil_local_pos.yaw = euler.psi();
+		hil_local_pos.heading = euler.psi();
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 		hil_local_pos.vxy_max = INFINITY;

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -77,7 +77,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	body_z.normalize();
 
 	// vector of desired yaw direction in XY plane, rotated by PI/2
-	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
+	const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
 
 	// desired body_x axis, orthogonal to body_z
 	Vector3f body_x = y_C % body_z;
@@ -97,7 +97,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	body_x.normalize();
 
 	// desired body_y axis
-	Vector3f body_y = body_z % body_x;
+	const Vector3f body_y = body_z % body_x;
 
 	Dcmf R_sp;
 
@@ -109,14 +109,14 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	}
 
 	// copy quaternion setpoint to attitude setpoint topic
-	Quatf q_sp = R_sp;
+	const Quatf q_sp{R_sp};
 	q_sp.copyTo(att_sp.q_d);
 
 	// calculate euler angles, for logging only, must not be used for control
-	Eulerf euler = R_sp;
+	const Eulerf euler{R_sp};
-	att_sp.roll_body = euler(0);
+	att_sp.roll_body = euler.phi();
-	att_sp.pitch_body = euler(1);
+	att_sp.pitch_body = euler.theta();
-	att_sp.yaw_body = euler(2);
+	att_sp.yaw_body = euler.psi();
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -121,7 +121,6 @@ private:
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
 	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< notification of parameter updates */
-	uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)};				/**< vehicle attitude */
 	uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; 			/**< home position */
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 
@@ -423,14 +422,6 @@ MulticopterPositionControl::poll_subscriptions()
 	_control_mode_sub.update(&_control_mode);
 	_home_pos_sub.update(&_home_pos);
 
-	if (_att_sub.updated()) {
-		vehicle_attitude_s att;
-
-		if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
-			_states.yaw = Eulerf(Quatf(att.q)).psi();
-		}
-	}
-
 	if (_param_mpc_use_hte.get()) {
 		hover_thrust_estimate_s hte;
 
@@ -514,6 +505,10 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
 		// reset derivative to prevent acceleration spikes when regaining velocity
 		_vel_z_deriv.reset();
 	}
+
+	if (PX4_ISFINITE(_local_pos.heading)) {
+		_states.yaw = _local_pos.heading;
+	}
 }
 
 int

src/modules/navigator/follow_target.cpp

@@ -196,7 +196,7 @@ void FollowTarget::on_active()
 						_current_target_motion.lat,
 						_current_target_motion.lon);
 
-				_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0f));
+				_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
 
 			} else {
 				_yaw_angle = _yaw_rate = NAN;
@@ -229,7 +229,7 @@ void FollowTarget::on_active()
 	// 3 degrees of facing target
 
 	if (PX4_ISFINITE(_yaw_rate)) {
-		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->yaw)) < math::radians(3.0F)) {
+		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
 			_yaw_rate = NAN;
 		}
 	}

src/modules/navigator/loiter.cpp

@@ -131,7 +131,7 @@ Loiter::reposition()
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.velocity_valid = false;
-		pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
+		pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading;
 		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
 		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
 		pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;

src/modules/navigator/mission.cpp

@@ -711,7 +711,7 @@ Mission::set_mission_items()
 					_mission_item.lat = _navigator->get_global_position()->lat;
 					_mission_item.lon = _navigator->get_global_position()->lon;
 					/* hold heading for takeoff items */
-					_mission_item.yaw = _navigator->get_global_position()->yaw;
+					_mission_item.yaw = _navigator->get_local_position()->heading;
 					_mission_item.altitude = takeoff_alt;
 					_mission_item.altitude_is_relative = false;
 					_mission_item.autocontinue = true;
@@ -808,7 +808,7 @@ Mission::set_mission_items()
 						mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
 
 					} else {
-						_mission_item.yaw = _navigator->get_global_position()->yaw;
+						_mission_item.yaw = _navigator->get_local_position()->heading;
 
 						/* set position setpoint to target during the transition */
 						generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
@@ -1343,7 +1343,7 @@ Mission::heading_sp_update()
 
 		} else {
 			if (!pos_sp_triplet->current.yaw_valid) {
-				_mission_item.yaw = _navigator->get_local_position()->yaw;
+				_mission_item.yaw = _navigator->get_local_position()->heading;
 				pos_sp_triplet->current.yaw = _mission_item.yaw;
 				pos_sp_triplet->current.yaw_valid = true;
 			}

src/modules/navigator/mission_block.cpp

@@ -375,7 +375,7 @@ MissionBlock::is_mission_item_reached()
 
 			/* check course if defined only for rotary wing except takeoff */
 			float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ?
-				    _navigator->get_local_position()->yaw :
+				    _navigator->get_local_position()->heading :
 				    atan2f(
 					    _navigator->get_local_position()->vy,
 					    _navigator->get_local_position()->vx
@@ -690,7 +690,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
 	/* use current position */
 	item->lat = _navigator->get_global_position()->lat;
 	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->yaw;
+	item->yaw = _navigator->get_local_position()->heading;
 
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
@@ -720,7 +720,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	if (at_current_location) {
 		item->lat = (double)NAN; //descend at current position
 		item->lon = (double)NAN; //descend at current position
-		item->yaw = _navigator->get_local_position()->yaw;
+		item->yaw = _navigator->get_local_position()->heading;
 
 	} else {
 		/* use home position */
@@ -759,7 +759,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 {
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
-	item->yaw = _navigator->get_local_position()->yaw;
+	item->yaw = _navigator->get_local_position()->heading;
 	item->autocontinue = true;
 }
 

src/modules/navigator/navigator_main.cpp

@@ -262,7 +262,7 @@ Navigator::run()
 					position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
 
 					// store current position as previous position and goal as next
-					rep->previous.yaw = get_global_position()->yaw;
+					rep->previous.yaw = get_local_position()->heading;
 					rep->previous.lat = get_global_position()->lat;
 					rep->previous.lon = get_global_position()->lon;
 					rep->previous.alt = get_global_position()->alt;
@@ -345,7 +345,7 @@ Navigator::run()
 				position_setpoint_triplet_s *rep = get_takeoff_triplet();
 
 				// store current position as previous position and goal as next
-				rep->previous.yaw = get_local_position()->yaw;
+				rep->previous.yaw = get_local_position()->heading;
 				rep->previous.lat = get_global_position()->lat;
 				rep->previous.lon = get_global_position()->lon;
 				rep->previous.alt = get_global_position()->alt;
@@ -361,7 +361,7 @@ Navigator::run()
 					rep->previous.timestamp = hrt_absolute_time();
 
 				} else {
-					rep->current.yaw = get_local_position()->yaw;
+					rep->current.yaw = get_local_position()->heading;
 					rep->previous.valid = false;
 				}
 
@@ -505,7 +505,7 @@ Navigator::run()
 					    && get_vstatus()->nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
 						position_setpoint_triplet_s *rep = get_reposition_triplet();
 
-						rep->current.yaw = get_global_position()->yaw;
+						rep->current.yaw = get_local_position()->heading;
 						rep->current.lat = get_global_position()->lat;
 						rep->current.lon = get_global_position()->lon;
 						rep->current.alt = get_global_position()->alt;

src/modules/navigator/rtl.cpp

@@ -283,7 +283,7 @@ void RTL::set_rtl_item()
 			_mission_item.lon = gpos.lon;
 			_mission_item.altitude = _rtl_alt;
 			_mission_item.altitude_is_relative = false;
-			_mission_item.yaw = _navigator->get_local_position()->yaw;
+			_mission_item.yaw = _navigator->get_local_position()->heading;
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;

src/modules/simulator/simulator_mavlink.cpp

@@ -460,7 +460,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 		hil_lpos.vy = hil_state.vy / 100.0f;
 		hil_lpos.vz = hil_state.vz / 100.0f;
 		matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
-		hil_lpos.yaw = euler.psi();
+		hil_lpos.heading = euler.psi();
 		hil_lpos.xy_global = true;
 		hil_lpos.z_global = true;
 		hil_lpos.ref_lat = _hil_ref_lat;

src/modules/vmount/output.cpp

@@ -132,14 +132,20 @@ void OutputBase::_handle_position_update(bool force_update)
 	}
 
 	vehicle_global_position_s vehicle_global_position{};
+	vehicle_local_position_s vehicle_local_position{};
 
 	if (force_update) {
 		_vehicle_global_position_sub.copy(&vehicle_global_position);
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
 
 	} else {
 		if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
 			return;
 		}
+
+		if (!_vehicle_local_position_sub.update(&vehicle_local_position)) {
+			return;
+		}
 	}
 
 	const double &vlat = vehicle_global_position.lat;
@@ -159,7 +165,7 @@ void OutputBase::_handle_position_update(bool force_update)
 		_angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
 	}
 
-	_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_global_position.yaw;
+	_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading;
 
 	// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
 	_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;

src/modules/vmount/output.h

@@ -47,6 +47,7 @@
 #include <uORB/topics/mount_orientation.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 
 namespace vmount
 {
@@ -124,6 +125,7 @@ protected:
 private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
 };