ackermann: only control relevant setpoints in manual modes

src/modules/rover_ackermann/AckermannActControl/AckermannActControl.cpp

@@ -77,12 +77,6 @@ void AckermannActControl::updateActControl()
 		actuator_motors.timestamp = _timestamp;
 		_actuator_motors_pub.publish(actuator_motors);
 
-	} else {
-		actuator_motors_s actuator_motors{};
-		actuator_motors.reversible_flags = _param_r_rev.get();
-		actuator_motors.control[0] = 0.f;
-		actuator_motors.timestamp = _timestamp;
-		_actuator_motors_pub.publish(actuator_motors);
 	}
 
 	// Servo control
@@ -114,15 +108,7 @@ void AckermannActControl::updateActControl()
 		actuator_servos.control[0] = _servo_setpoint.getState();
 		actuator_servos.timestamp = _timestamp;
 		_actuator_servos_pub.publish(actuator_servos);
-
-	} else {
-		actuator_servos_s actuator_servos{};
-		actuator_servos.control[0] = 0.f;
-		actuator_servos.timestamp = _timestamp;
-		_actuator_servos_pub.publish(actuator_servos);
 	}
-
-
 }
 
 void AckermannActControl::stopVehicle()

src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp

@@ -81,11 +81,6 @@ void AckermannAttControl::updateAttControl()
 		rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
 		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 
-	} else {
-		rover_rate_setpoint_s rover_rate_setpoint{};
-		rover_rate_setpoint.timestamp = _timestamp;
-		rover_rate_setpoint.yaw_rate_setpoint = 0.f;
-		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 	}
 
 	// Publish attitude controller status (logging only)

src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp

@@ -57,53 +57,49 @@ void AckermannPosControl::updatePosControl()
 	hrt_abstime timestamp = hrt_absolute_time();
 
 	const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
-	float distance_to_target = target_waypoint_ned.isAllFinite() ? (target_waypoint_ned - _curr_pos_ned).norm() : NAN;
 
-	if (PX4_ISFINITE(distance_to_target) && distance_to_target > _acceptance_radius) {
+	if (target_waypoint_ned.isAllFinite()) {
+		float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
 
-		float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
-				      0.f;
-		const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
-		float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
-				       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
-		speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
+		if (distance_to_target > _acceptance_radius) {
+			float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
+					      0.f;
+			const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
+			float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
+					       _param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
+			speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
 
-		if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
-			speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
-					 fabsf(_rover_position_setpoint.cruising_speed));
+			if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
+				speed_setpoint = sign(_rover_position_setpoint.cruising_speed) * math::min(speed_setpoint,
+						 fabsf(_rover_position_setpoint.cruising_speed));
+			}
+
+			pure_pursuit_status_s pure_pursuit_status{};
+			pure_pursuit_status.timestamp = timestamp;
+
+			const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
+						       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
+						       _curr_pos_ned, fabsf(speed_setpoint));
+			_pure_pursuit_status_pub.publish(pure_pursuit_status);
+			rover_velocity_setpoint_s rover_velocity_setpoint{};
+			rover_velocity_setpoint.timestamp = timestamp;
+			rover_velocity_setpoint.speed = speed_setpoint;
+			rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
+					bearing_setpoint + M_PI_F);
+			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+		} else {
+			rover_velocity_setpoint_s rover_velocity_setpoint{};
+			rover_velocity_setpoint.timestamp = timestamp;
+			rover_velocity_setpoint.speed = 0.f;
+			rover_velocity_setpoint.bearing = _vehicle_yaw;
+			_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
 		}
-
-		pure_pursuit_status_s pure_pursuit_status{};
-		pure_pursuit_status.timestamp = timestamp;
-
-		const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
-					       _param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), target_waypoint_ned, _start_ned,
-					       _curr_pos_ned, fabsf(speed_setpoint));
-		_pure_pursuit_status_pub.publish(pure_pursuit_status);
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = timestamp;
-		rover_velocity_setpoint.speed = speed_setpoint;
-		rover_velocity_setpoint.bearing = speed_setpoint > -FLT_EPSILON ? bearing_setpoint : matrix::wrap_pi(
-				bearing_setpoint + M_PI_F);
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
-
-	} else {
-		rover_velocity_setpoint_s rover_velocity_setpoint{};
-		rover_velocity_setpoint.timestamp = timestamp;
-		rover_velocity_setpoint.speed = 0.f;
-		rover_velocity_setpoint.bearing = _vehicle_yaw;
-		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
 	}
 }
 
 void AckermannPosControl::updateSubscriptions()
 {
-	if (_rover_position_setpoint_sub.updated()) {
-		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
-		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
-		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
-	}
-
 	if (_position_controller_status_sub.updated()) {
 		position_controller_status_s position_controller_status{};
 		_position_controller_status_sub.copy(&position_controller_status);
@@ -130,6 +126,12 @@ void AckermannPosControl::updateSubscriptions()
 		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
 	}
 
+	if (_rover_position_setpoint_sub.updated()) {
+		_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
+		_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
+		_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
+	}
+
 }
 
 bool AckermannPosControl::runSanityChecks()

src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp

@@ -64,47 +64,49 @@ void AckermannRateControl::updateRateControl()
 	_timestamp = hrt_absolute_time();
 	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
 
-	if (fabsf(_estimated_speed) > FLT_EPSILON && PX4_ISFINITE(_yaw_rate_setpoint)) {
-		// Set up feasible yaw rate setpoint
-		float steering_setpoint{0.f};
-		float max_possible_yaw_rate = fabsf(_estimated_speed) * tanf(_param_ra_max_str_ang.get()) /
-					      _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
-		float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
-		float constrained_yaw_rate = math::constrain(_yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);
+	if (PX4_ISFINITE(_yaw_rate_setpoint)) {
+		if (fabsf(_estimated_speed) > FLT_EPSILON) {
+			// Set up feasible yaw rate setpoint
+			float steering_setpoint{0.f};
+			float max_possible_yaw_rate = fabsf(_estimated_speed) * tanf(_param_ra_max_str_ang.get()) /
+						      _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity
+			float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate);
+			float constrained_yaw_rate = math::constrain(_yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit);
 
-		if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured
-			if (fabsf(_adjusted_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate -
-					_vehicle_yaw_rate)) {
-				_adjusted_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate);
+			if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured
+				if (fabsf(_adjusted_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate -
+						_vehicle_yaw_rate)) {
+					_adjusted_yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate);
+				}
+
+				_adjusted_yaw_rate_setpoint.update(constrained_yaw_rate, dt);
+
+			} else {
+				_adjusted_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate);
 			}
 
-			_adjusted_yaw_rate_setpoint.update(constrained_yaw_rate, dt);
+			// Feed forward
+			steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed);
+
+			// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
+			if (_estimated_speed > FLT_EPSILON) {
+				_pid_yaw_rate.setSetpoint(_adjusted_yaw_rate_setpoint.getState());
+				steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, dt);
+			}
+
+			rover_steering_setpoint_s rover_steering_setpoint{};
+			rover_steering_setpoint.timestamp = _timestamp;
+			rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
+					-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
+			_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
 
 		} else {
-			_adjusted_yaw_rate_setpoint.setForcedValue(constrained_yaw_rate);
+			_pid_yaw_rate.resetIntegral();
+			rover_steering_setpoint_s rover_steering_setpoint{};
+			rover_steering_setpoint.timestamp = _timestamp;
+			rover_steering_setpoint.normalized_steering_angle = 0.f;
+			_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
 		}
-
-		// Feed forward
-		steering_setpoint = atanf(_adjusted_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed);
-
-		// Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability)
-		if (_estimated_speed > FLT_EPSILON) {
-			_pid_yaw_rate.setSetpoint(_adjusted_yaw_rate_setpoint.getState());
-			steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, dt);
-		}
-
-		rover_steering_setpoint_s rover_steering_setpoint{};
-		rover_steering_setpoint.timestamp = _timestamp;
-		rover_steering_setpoint.normalized_steering_angle = math::interpolate<float>(steering_setpoint,
-				-_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint
-		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-
-	} else {
-		_pid_yaw_rate.resetIntegral();
-		rover_steering_setpoint_s rover_steering_setpoint{};
-		rover_steering_setpoint.timestamp = _timestamp;
-		rover_steering_setpoint.normalized_steering_angle = 0.f;
-		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
 	}
 
 

src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp

@@ -87,12 +87,6 @@ void AckermannVelControl::updateVelControl()
 		rover_throttle_setpoint.throttle_body_y = NAN;
 		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 
-	} else {
-		rover_throttle_setpoint_s rover_throttle_setpoint{};
-		rover_throttle_setpoint.timestamp = _timestamp;
-		rover_throttle_setpoint.throttle_body_x = 0.f;
-		rover_throttle_setpoint.throttle_body_y = NAN;
-		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 	}
 
 	// Publish position controller status (logging only)

src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.cpp

@@ -52,27 +52,6 @@ void ManualMode::updateParams()
 	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
 }
 
-void ManualMode::manualControl(const int nav_state)
-{
-	switch (nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-		manual();
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ACRO:
-		acro();
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_STAB:
-		stab();
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-		position();
-		break;
-	}
-}
-
 void ManualMode::manual()
 {
 	manual_control_setpoint_s manual_control_setpoint{};
@@ -121,20 +100,24 @@ void ManualMode::stab()
 	rover_throttle_setpoint.throttle_body_y = 0.f;
 	_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
 
-	const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-				_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-				_max_yaw_rate / _param_ro_yaw_p.get());
-
-	if (fabsf(yaw_delta) > FLT_EPSILON
-	    || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control
+	if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON
+	    || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) {
 		_stab_yaw_setpoint = NAN;
-		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + matrix::sign(manual_control_setpoint.throttle) * yaw_delta);
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoint invalid
 		rover_attitude_setpoint_s rover_attitude_setpoint{};
 		rover_attitude_setpoint.timestamp = hrt_absolute_time();
-		rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
+		rover_attitude_setpoint.yaw_setpoint = NAN;
 		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
 
-	} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
+	} else { // Heading control
 		if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
 			_stab_yaw_setpoint = _vehicle_yaw;
 		}
@@ -173,30 +156,44 @@ void ManualMode::position()
 
 	const float speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
 				     -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-	const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
-				_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
-				_max_yaw_rate / _param_ro_yaw_p.get());
 
-	if (fabsf(yaw_delta) > FLT_EPSILON
-	    || fabsf(speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+	if (fabsf(manual_control_setpoint.roll) > FLT_EPSILON
+	    || fabsf(speed_setpoint) < FLT_EPSILON) {
 		_pos_ctl_course_direction = Vector2f(NAN, NAN);
-		// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
-		const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + sign(speed_setpoint) * yaw_delta);
-		const Vector2f pos_ctl_course_direction = Vector2f(cos(yaw_setpoint), sin(yaw_setpoint));
-		const Vector2f target_waypoint_ned = _curr_pos_ned + sign(speed_setpoint) * _param_pp_lookahd_max.get() *
-						     pos_ctl_course_direction;
+
+		// Speed control
+		rover_velocity_setpoint_s rover_velocity_setpoint{};
+		rover_velocity_setpoint.timestamp = hrt_absolute_time();
+		rover_velocity_setpoint.speed = speed_setpoint;
+		rover_velocity_setpoint.bearing = NAN;
+		rover_velocity_setpoint.yaw = NAN;
+		_rover_velocity_setpoint_pub.publish(rover_velocity_setpoint);
+
+		// Rate control
+		rover_rate_setpoint_s rover_rate_setpoint{};
+		rover_rate_setpoint.timestamp = hrt_absolute_time();
+		rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);;
+		_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
+
+		// Set uncontrolled setpoints invalid
+		rover_attitude_setpoint_s rover_attitude_setpoint{};
+		rover_attitude_setpoint.timestamp = hrt_absolute_time();
+		rover_attitude_setpoint.yaw_setpoint = NAN;
+		_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
+
 		rover_position_setpoint_s rover_position_setpoint{};
 		rover_position_setpoint.timestamp = hrt_absolute_time();
-		rover_position_setpoint.position_ned[0] = target_waypoint_ned(0);
-		rover_position_setpoint.position_ned[1] = target_waypoint_ned(1);
+		rover_position_setpoint.position_ned[0] = NAN;
+		rover_position_setpoint.position_ned[1] = NAN;
 		rover_position_setpoint.start_ned[0] = NAN;
 		rover_position_setpoint.start_ned[1] = NAN;
 		rover_position_setpoint.arrival_speed = NAN;
-		rover_position_setpoint.cruising_speed = speed_setpoint;
+		rover_position_setpoint.cruising_speed = NAN;
 		rover_position_setpoint.yaw = NAN;
 		_rover_position_setpoint_pub.publish(rover_position_setpoint);
 
-	} else { // Course control if the steering input is zero (keep driving on a straight line)
+	} else { // Course control
 		if (!_pos_ctl_course_direction.isAllFinite()) {
 			_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
 			_pos_ctl_start_position_ned = _curr_pos_ned;

src/modules/rover_ackermann/DriveModes/ManualMode/ManualMode.hpp

@@ -67,23 +67,6 @@ public:
 	ManualMode(ModuleParams *parent);
 	~ManualMode() = default;
 
-	/**
-	 * @brief Generate and publish roverSetpoints from manualControlSetpoints.
-	 */
-	void manualControl(int nav_state);
-
-	/**
-	 * @brief Reset manual mode variables.
-	 */
-	void reset();
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
 	/**
 	 * @brief Publish roverThrottleSetpoint and roverSteeringSetpoint from manualControlSetpoint.
 	 */
@@ -104,6 +87,18 @@ private:
 	 */
 	void position();
 
+	/**
+	 * @brief Reset manual mode variables.
+	 */
+	void reset();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
 	// uORB subscriptions
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -66,13 +66,18 @@ void RoverAckermann::Run()
 		vehicle_control_mode_s vehicle_control_mode{};
 		_vehicle_control_mode_sub.copy(&vehicle_control_mode);
 
-		// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated and not on changes)
-		if (vehicle_control_mode.flag_control_position_enabled != _vehicle_control_mode.flag_control_position_enabled ||
-		    vehicle_control_mode.flag_control_velocity_enabled != _vehicle_control_mode.flag_control_velocity_enabled ||
-		    vehicle_control_mode.flag_control_attitude_enabled != _vehicle_control_mode.flag_control_attitude_enabled ||
-		    vehicle_control_mode.flag_control_rates_enabled != _vehicle_control_mode.flag_control_rates_enabled) {
+		// Run sanity checks if the control mode changes (Note: This has to be done this way, because the topic is periodically updated at 2 Hz)
+		if (_vehicle_control_mode.flag_control_manual_enabled != vehicle_control_mode.flag_control_manual_enabled ||
+		    _vehicle_control_mode.flag_control_auto_enabled != vehicle_control_mode.flag_control_auto_enabled ||
+		    _vehicle_control_mode.flag_control_offboard_enabled != vehicle_control_mode.flag_control_offboard_enabled ||
+		    _vehicle_control_mode.flag_control_position_enabled != vehicle_control_mode.flag_control_position_enabled ||
+		    _vehicle_control_mode.flag_control_velocity_enabled != vehicle_control_mode.flag_control_velocity_enabled ||
+		    _vehicle_control_mode.flag_control_attitude_enabled != vehicle_control_mode.flag_control_attitude_enabled ||
+		    _vehicle_control_mode.flag_control_rates_enabled != vehicle_control_mode.flag_control_rates_enabled ||
+		    _vehicle_control_mode.flag_control_allocation_enabled != vehicle_control_mode.flag_control_allocation_enabled) {
 			_vehicle_control_mode = vehicle_control_mode;
 			runSanityChecks();
+			reset();
 
 		} else {
 			_vehicle_control_mode = vehicle_control_mode;
@@ -80,20 +85,13 @@ void RoverAckermann::Run()
 
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status{};
-		_vehicle_status_sub.copy(&vehicle_status);
-
-		// Reset all controllers if the navigation state changes
-		if (vehicle_status.nav_state != _nav_state) { reset();}
-
-		_nav_state = vehicle_status.nav_state;
-	}
-
 	if (_vehicle_control_mode.flag_armed && _sanity_checks_passed) {
+
+		_was_armed = true;
+
 		// Generate setpoints
 		if (_vehicle_control_mode.flag_control_manual_enabled) {
-			_manual_mode.manualControl(_nav_state);
+			manualControl();
 
 		} else if (_vehicle_control_mode.flag_control_auto_enabled) {
 			_auto_mode.autoControl();
@@ -112,6 +110,22 @@ void RoverAckermann::Run()
 
 }
 
+void RoverAckermann::manualControl()
+{
+	if (_vehicle_control_mode.flag_control_position_enabled) {
+		_manual_mode.position();
+
+	} else if (_vehicle_control_mode.flag_control_attitude_enabled) {
+		_manual_mode.stab();
+
+	} else if (_vehicle_control_mode.flag_control_rates_enabled) {
+		_manual_mode.acro();
+
+	} else if (_vehicle_control_mode.flag_control_allocation_enabled) {
+		_manual_mode.manual();
+	}
+}
+
 void RoverAckermann::updateControllers()
 {
 	if (_vehicle_control_mode.flag_control_position_enabled) {

src/modules/rover_ackermann/RoverAckermann.hpp

@@ -48,7 +48,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
 
 // Local includes
 #include "AckermannActControl/AckermannActControl.hpp"
@@ -90,6 +89,11 @@ protected:
 private:
 	void Run() override;
 
+	/**
+	 * @brief Generate and publish roverSetpoints from manualControlSetpoints.
+	 */
+	void manualControl();
+
 	/**
 	 * @brief Update the active controllers.
 	 */
@@ -112,7 +116,6 @@ private:
 	// uORB subscriptions
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	vehicle_control_mode_s _vehicle_control_mode{};
 
 	// Class instances
@@ -126,7 +129,6 @@ private:
 	OffboardMode 	     _offboard_mode{this};
 
 	// Variables
-	int _nav_state{0}; // Navigation state of the vehicle
 	bool _sanity_checks_passed{true}; // True if checks for all active controllers pass
 	bool _was_armed{false}; // True if the vehicle was armed before the last reset
 };