diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 81e26513d2..7f4eb8674a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -516,11 +516,12 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 
 		} else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
 			_mavlink->request_stop_ulog_streaming();
-		}
 
-		if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) {
-			// Not differentiating between airspeed and groundspeed yet
-			set_offb_cruising_speed(cmd_mavlink.param2);
+		} else if (cmd_mavlink.command == MAV_CMD_DO_CHANGE_SPEED) {
+			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
+				// Not differentiating between airspeed and groundspeed yet
+				set_offb_cruising_speed(cmd_mavlink.param2);
+			}
 		}
 
 		if (!send_ack) {
@@ -1539,7 +1540,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 						att_sp.yaw_sp_move_rate = 0.0f;
 					}
 
-					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+					if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
 						fill_thrust(att_sp.thrust_body, _vehicle_status.vehicle_type, set_attitude_target.thrust);
 					}
 
@@ -1577,7 +1578,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 						rates_sp.yaw = set_attitude_target.body_yaw_rate;
 					}
 
-					if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid
+					if (!offboard_control_mode.ignore_thrust) { // don't overwrite thrust if it's invalid
 						fill_thrust(rates_sp.thrust_body, _vehicle_status.vehicle_type, set_attitude_target.thrust);
 					}
 
@@ -2972,14 +2973,7 @@ MavlinkReceiver::Run()
 			updateParams();
 		}
 
-		// reset cruising speed on mode changes
-		if (_vehicle_status_sub.update(&_vehicle_status)) {
-			if (_last_nav_state != _vehicle_status.nav_state) {
-				reset_offb_cruising_speed();
-				_last_nav_state = _vehicle_status.nav_state;
-			}
-		}
-
+		_vehicle_status_sub.update(&_vehicle_status);
 
 		int ret = poll(&fds[0], 1, timeout);
 
@@ -3161,10 +3155,3 @@ MavlinkReceiver::set_offb_cruising_speed(float speed)
 		_offb_cruising_speed_fw = speed;
 	}
 }
-
-void
-MavlinkReceiver::reset_offb_cruising_speed()
-{
-	_offb_cruising_speed_mc = -1.0f;
-	_offb_cruising_speed_fw = -1.0f;
-}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 6b40f822e8..d19b9b3b1b 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -140,11 +140,6 @@ public:
 	 */
 	void set_offb_cruising_speed(float speed = -1.0f);
 
-	/**
-	 * Reset all offboard cruising speeds to default values
-	 */
-	void reset_offb_cruising_speed();
-
 private:
 
 	void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result);
@@ -340,7 +335,6 @@ private:
 	hrt_abstime			_last_utm_global_pos_com{0};
 
 	vehicle_status_s		_vehicle_status{};
-	uint8_t				_last_nav_state{0};
 
 	float 				_offb_cruising_speed_mc{-1.0f};
 	float 				_offb_cruising_speed_fw{-1.0f};