vehicle_global_position: remove velocity fields (duplicates of local vx, vy, vz)

* attitude_estimator_q: get velocity from local position (if available)

msg/vehicle_global_position.msg

@@ -16,10 +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 vel_n			# North velocity in NED earth-fixed frame, (metres/sec)
-float32 vel_e			# East velocity in NED earth-fixed frame, (metres/sec)
-float32 vel_d			# Down velocity in NED earth-fixed frame, (metres/sec)
-
 float32 yaw 			# Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians)
 
 float32 eph			# Standard deviation of horizontal position error, (metres)

src/drivers/telemetry/frsky_telemetry/frsky_data.cpp

@@ -55,6 +55,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_status.h>
@@ -68,6 +69,7 @@ struct frsky_subscription_data_s {
 	uORB::SubscriptionData<battery_status_s> battery_status_sub{ORB_ID(battery_status)};
 	uORB::SubscriptionData<sensor_combined_s> sensor_combined_sub{ORB_ID(sensor_combined)};
 	uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -149,6 +151,7 @@ void frsky_update_topics()
 	subscription_data->battery_status_sub.update();
 	subscription_data->sensor_combined_sub.update();
 	subscription_data->vehicle_air_data_sub.update();
+	subscription_data->vehicle_local_position_sub.update();
 	subscription_data->vehicle_global_position_sub.update();
 	subscription_data->vehicle_gps_position_sub.update();
 	subscription_data->vehicle_status_sub.update();
@@ -199,6 +202,7 @@ static float frsky_format_gps(float dec)
 void frsky_send_frame2(int uart)
 {
 	const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get();
+	const vehicle_local_position_s &lpos = subscription_data->vehicle_local_position_sub.get();
 	const battery_status_s &battery_status = subscription_data->battery_status_sub.get();
 	const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get();
 
@@ -218,8 +222,7 @@ void frsky_send_frame2(int uart)
 		lat_ns = (gpos.lat < 0) ? 'S' : 'N';
 		lon    = frsky_format_gps(fabsf(gpos.lon));
 		lon_ew = (gpos.lon < 0) ? 'W' : 'E';
-		speed  = sqrtf(gpos.vel_n * gpos.vel_n + gpos.vel_e * gpos.vel_e)
-			 * 25.0f / 46.0f;
+		speed  = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy) * 25.0f / 46.0f;
 		alt    = gpos.alt;
 	}
 

src/drivers/telemetry/frsky_telemetry/sPort_data.cpp

@@ -204,10 +204,10 @@ void sPort_send_ALT(int uart)
 // verified scaling for "calculated" option
 void sPort_send_SPD(int uart)
 {
-	const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
+	const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get();
 
 	/* send data for A2 */
-	float speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
+	float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy);
 	uint32_t ispeed = (int)(10 * speed);
 	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
 }
@@ -297,10 +297,10 @@ void sPort_send_GPS_TIME(int uart)
 
 void sPort_send_GPS_SPD(int uart)
 {
-	const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
+	const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get();
 
 	/* send 100 * knots */
-	float speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
+	float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy);
 	uint32_t ispeed = (int)(1944 * speed);
 	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
 }

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -58,7 +58,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
 
@@ -109,7 +110,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
 
 	uORB::Subscription		_parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription		_global_pos_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription		_gps_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription		_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription		_vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Subscription		_mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)};
 	uORB::Subscription		_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
@@ -307,29 +309,36 @@ AttitudeEstimatorQ::Run()
 			}
 		}
 
-		if (_global_pos_sub.updated()) {
-			vehicle_global_position_s gpos;
+		if (_gps_sub.updated()) {
+			vehicle_gps_position_s gps;
 
-			if (_global_pos_sub.copy(&gpos)) {
-				if (_param_att_mag_decl_a.get() && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1_s) {
+			if (_gps_sub.copy(&gps)) {
+				if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
 					/* set magnetic declination automatically */
-					update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
+					update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon)));
 				}
+			}
+		}
+
+		if (_local_position_sub.updated()) {
+			vehicle_local_position_s lpos;
+
+			if (_local_position_sub.copy(&lpos)) {
 
-				if (_param_att_acc_comp.get() && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f
-				    && _inited) {
+				if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
+				    && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
 
 					/* position data is actual */
-					Vector3f vel(gpos.vel_n, gpos.vel_e, gpos.vel_d);
+					const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
 
 					/* velocity updated */
-					if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) {
-						float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f;
+					if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
+						float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
 						/* calculate acceleration in body frame */
 						_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
 					}
 
-					_vel_prev_t = gpos.timestamp;
+					_vel_prev_t = lpos.timestamp;
 					_vel_prev = vel;
 
 				} else {

src/modules/ekf2/ekf2_main.cpp

@@ -1432,10 +1432,6 @@ void Ekf2::Run()
 					// global altitude has opposite sign of local down position
 					global_pos.delta_alt = -lpos.delta_z;
 
-					global_pos.vel_n = lpos.vx; // Ground north velocity, m/s
-					global_pos.vel_e = lpos.vy; // Ground east velocity, m/s
-					global_pos.vel_d = lpos.vz; // Ground downside velocity, m/s
-
 					global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI.
 
 					_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -361,13 +361,19 @@ void FixedwingAttitudeControl::Run()
 
 		vehicle_control_mode_poll();
 		vehicle_manual_poll();
-		_global_pos_sub.update(&_global_pos);
 		vehicle_land_detected_poll();
 
 		// the position controller will not emit attitude setpoints in some modes
 		// we need to make sure that this flag is reset
 		_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
 
+		bool wheel_control = false;
+
+		// TODO: manual wheel_control on ground?
+		if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
+			wheel_control = true;
+		}
+
 		/* lock integrator until control is started */
 		bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled
 				       || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode);
@@ -393,14 +399,6 @@ void FixedwingAttitudeControl::Run()
 
 			const float airspeed = get_airspeed_and_update_scaling();
 
-			/* Use min airspeed to calculate ground speed scaling region.
-			 * Don't scale below gspd_scaling_trim
-			 */
-			float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
-						  _global_pos.vel_e * _global_pos.vel_e);
-			float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
-			float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
-
 			/* reset integrals where needed */
 			if (_att_sp.roll_reset_integral) {
 				_roll_ctrl.reset_integrator();
@@ -444,8 +442,20 @@ void FixedwingAttitudeControl::Run()
 			control_input.airspeed = airspeed;
 			control_input.scaler = _airspeed_scaling;
 			control_input.lock_integrator = lock_integrator;
-			control_input.groundspeed = groundspeed;
-			control_input.groundspeed_scaler = groundspeed_scaler;
+
+			if (wheel_control) {
+				_local_pos_sub.update(&_local_pos);
+
+				/* Use min airspeed to calculate ground speed scaling region.
+				* Don't scale below gspd_scaling_trim
+				*/
+				float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
+				float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
+				float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
+
+				control_input.groundspeed = groundspeed;
+				control_input.groundspeed_scaler = groundspeed_scaler;
+			}
 
 			/* reset body angular rate limits on mode change */
 			if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
@@ -497,8 +507,16 @@ void FixedwingAttitudeControl::Run()
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
 					_roll_ctrl.control_attitude(control_input);
 					_pitch_ctrl.control_attitude(control_input);
-					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
-					_wheel_ctrl.control_attitude(control_input);
+
+					if (wheel_control) {
+						_wheel_ctrl.control_attitude(control_input);
+						_yaw_ctrl.reset_integrator();
+
+					} else {
+						// runs last, because is depending on output of roll and pitch attitude
+						_yaw_ctrl.control_attitude(control_input);
+						_wheel_ctrl.reset_integrator();
+					}
 
 					/* Update input data for rate controllers */
 					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -522,7 +540,7 @@ void FixedwingAttitudeControl::Run()
 
 					float yaw_u = 0.0f;
 
-					if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
+					if (wheel_control) {
 						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
 
 					} else {
@@ -595,12 +613,17 @@ void FixedwingAttitudeControl::Run()
 						_rates_sp.thrust_body[0] : 0.0f;
 			}
 
-			rate_ctrl_status_s rate_ctrl_status;
+			rate_ctrl_status_s rate_ctrl_status{};
 			rate_ctrl_status.timestamp = hrt_absolute_time();
 			rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator();
 			rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator();
-			rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
-			rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
+
+			if (wheel_control) {
+				rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator();
+
+			} else {
+				rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator();
+			}
 
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);
 		}

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -62,7 +62,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
@@ -97,7 +97,7 @@ private:
 
 	uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};		/**< vehicle attitude setpoint */
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};			/**< battery status subscription */
-	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};		/**< global position subscription */
+	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};		/**< local position subscription */
 	uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)};		/**< notification of manual control updates */
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< notification of parameter updates */
 	uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint */
@@ -120,7 +120,7 @@ private:
 	vehicle_attitude_s			_att {};		/**< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s		_att_sp {};		/**< vehicle attitude setpoint */
 	vehicle_control_mode_s			_vcontrol_mode {};	/**< vehicle control mode */
-	vehicle_global_position_s		_global_pos {};		/**< global position */
+	vehicle_local_position_s		_local_pos {};		/**< local position */
 	vehicle_rates_setpoint_s		_rates_sp {};		/* attitude rates setpoint */
 	vehicle_status_s			_vehicle_status {};	/**< vehicle status */
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1369,7 +1369,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
 			_land_noreturn_vertical = true;
 
 		} else {
-			if (_global_pos.vel_d > 0.1f) {
+			if (_local_pos.vz > 0.1f) {
 				_flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) *
 						  constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
 			}
@@ -1525,7 +1525,7 @@ FixedwingPositionControl::Run()
 		_vehicle_rates_sub.update();
 
 		Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
-		Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);
+		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
 		//Convert Local setpoints to global setpoints
 		if (_control_mode.flag_control_offboard_enabled) {

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -803,9 +803,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
-		_pub_gpos.get().vel_n = xLP(X_vx);
-		_pub_gpos.get().vel_e = xLP(X_vy);
-		_pub_gpos.get().vel_d = xLP(X_vz);
 		_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
 		_pub_gpos.get().eph = eph;
 		_pub_gpos.get().epv = epv;

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -51,6 +51,7 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_status.h>
@@ -76,6 +77,8 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
 	_pos_ctrl_status_time(0),
 	_geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
 	_geofence_time(0),
+	_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
+	_local_pos_time(0),
 	_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
 	_global_pos_time(0),
 	_gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
@@ -503,7 +506,7 @@ void MavlinkStreamHighLatency2::update_data()
 
 	update_battery_status();
 
-	update_global_position();
+	update_local_position();
 
 	update_gps();
 
@@ -543,14 +546,13 @@ void MavlinkStreamHighLatency2::update_battery_status()
 	}
 }
 
-void MavlinkStreamHighLatency2::update_global_position()
+void MavlinkStreamHighLatency2::update_local_position()
 {
-	vehicle_global_position_s global_pos;
+	vehicle_local_position_s local_pos;
 
-	if (_global_pos_sub->update(&global_pos)) {
-		_climb_rate.add_value(fabsf(global_pos.vel_d), _update_rate_filtered);
-		_groundspeed.add_value(sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e),
-				       _update_rate_filtered);
+	if (_local_pos_sub->update(&local_pos)) {
+		_climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered);
+		_groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered);
 	}
 }
 

src/modules/mavlink/mavlink_high_latency2.h

@@ -113,6 +113,9 @@ private:
 	MavlinkOrbSubscription *_geofence_sub;
 	uint64_t _geofence_time;
 
+	MavlinkOrbSubscription *_local_pos_sub;
+	uint64_t _local_pos_time;
+
 	MavlinkOrbSubscription *_global_pos_sub;
 	uint64_t _global_pos_time;
 
@@ -193,7 +196,7 @@ protected:
 
 	void update_battery_status();
 
-	void update_global_position();
+	void update_local_position();
 
 	void update_gps();
 

src/modules/mavlink/mavlink_messages.cpp

@@ -2067,7 +2067,7 @@ public:
 private:
 	MavlinkOrbSubscription *_local_pos_sub;
 	uint64_t _local_pos_time = 0;
-	vehicle_local_position_s _local_position   = {};
+	vehicle_local_position_s _local_position = {};
 
 	MavlinkOrbSubscription *_global_pos_sub;
 	uint64_t _global_pos_time = 0;

src/modules/mavlink/mavlink_receiver.cpp

@@ -2492,9 +2492,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.lat = hil_state.lat / ((double)1e7);
 		hil_global_pos.lon = hil_state.lon / ((double)1e7);
 		hil_global_pos.alt = hil_state.alt / 1000.0f;
-		hil_global_pos.vel_n = hil_state.vx / 100.0f;
-		hil_global_pos.vel_e = hil_state.vy / 100.0f;
-		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 		hil_global_pos.eph = 2.0f;
 		hil_global_pos.epv = 4.0f;
 

src/modules/navigator/follow_target.cpp

@@ -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_global_position()->yaw)) < math::radians(3.0F)) {
+		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->yaw)) < math::radians(3.0F)) {
 			_yaw_rate = NAN;
 		}
 	}

src/modules/navigator/mission_block.cpp

@@ -336,10 +336,10 @@ 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_global_position()->yaw :
+				    _navigator->get_local_position()->yaw :
 				    atan2f(
-					    _navigator->get_global_position()->vel_e,
-					    _navigator->get_global_position()->vel_n
+					    _navigator->get_local_position()->vy,
+					    _navigator->get_local_position()->vx
 				    );
 
 			float yaw_err = wrap_pi(_mission_item.yaw - cog);
@@ -642,7 +642,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_global_position()->yaw;
+	item->yaw = _navigator->get_local_position()->yaw;
 
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
@@ -711,7 +711,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_global_position()->yaw;
+	item->yaw = _navigator->get_local_position()->yaw;
 	item->autocontinue = true;
 }
 

src/modules/navigator/navigator_main.cpp

@@ -305,7 +305,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_global_position()->yaw;
+				rep->previous.yaw = get_local_position()->yaw;
 				rep->previous.lat = get_global_position()->lat;
 				rep->previous.lon = get_global_position()->lon;
 				rep->previous.alt = get_global_position()->alt;

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -433,7 +433,7 @@ RoverPositionControl::run()
 			// update the reset counters in any case
 			_pos_reset_counter = _global_pos.lat_lon_reset_counter;
 
-			matrix::Vector3f ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
+			matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy,  _local_pos.vz);
 			matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon);
 			matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
 

src/modules/sih/sih.cpp

@@ -391,9 +391,6 @@ void Sih::publish_sih()
 	_gpos_gt.lat = _gps_lat_noiseless;
 	_gpos_gt.lon = _gps_lon_noiseless;
 	_gpos_gt.alt = _gps_alt_noiseless;
-	_gpos_gt.vel_n = _v_I(0);
-	_gpos_gt.vel_e = _v_I(1);
-	_gpos_gt.vel_d = _v_I(2);
 
 	_gpos_gt_pub.publish(_gpos_gt);
 }

src/modules/simulator/simulator_mavlink.cpp

@@ -421,10 +421,6 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 		hil_gpos.lon = hil_state.lon / 1E7;//1E7
 		hil_gpos.alt = hil_state.alt / 1E3;//1E3
 
-		hil_gpos.vel_n = hil_state.vx / 100.0f;
-		hil_gpos.vel_e = hil_state.vy / 100.0f;
-		hil_gpos.vel_d = hil_state.vz / 100.0f;
-
 		// always publish ground truth attitude message
 		_gpos_ground_truth_pub.publish(hil_gpos);
 	}