ackermann: implement suggestions

removed acro support and included vehicle_status in guidance

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -99,8 +99,7 @@ void RoverAckermann::Run()
 	if (_armed) {
 		// Navigation modes
 		switch (_nav_state) {
-		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
-		case vehicle_status_s::NAVIGATION_STATE_ACRO: {
+		case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
 				manual_control_setpoint_s manual_control_setpoint{};
 
 				if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
@@ -123,7 +122,6 @@ void RoverAckermann::Run()
 
 		// Acceleration slew rate
 		if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON
-		    && _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO
 		    && fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) {
 			_throttle_with_accel_limit.update(_motor_setpoint.throttle, dt);
 
@@ -132,41 +130,40 @@ void RoverAckermann::Run()
 		}
 
 		// Steering slew rate
-		if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON
-		    && _nav_state != vehicle_status_s::NAVIGATION_STATE_ACRO) {
+		if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) {
 			_steering_with_rate_limit.update(_motor_setpoint.steering, dt);
 
 		} else {
 			_steering_with_rate_limit.setForcedValue(_motor_setpoint.steering);
 		}
 
+		// Publish rover ackermann status (logging)
+		rover_ackermann_status_s rover_ackermann_status{};
+		rover_ackermann_status.timestamp = _timestamp;
+		rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
+		rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
+		rover_ackermann_status.actual_speed = _actual_speed;
+		_rover_ackermann_status_pub.publish(rover_ackermann_status);
+
+		// Publish to motor
+		actuator_motors_s actuator_motors{};
+		actuator_motors.reversible_flags = _param_r_rev.get();
+		actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f);
+		actuator_motors.timestamp = _timestamp;
+		_actuator_motors_pub.publish(actuator_motors);
+
+		// Publish to servo
+		actuator_servos_s actuator_servos{};
+		actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f);
+		actuator_servos.timestamp = _timestamp;
+		_actuator_servos_pub.publish(actuator_servos);
+
 	} else { // Reset on disarm
 		_motor_setpoint.steering = 0.f;
 		_motor_setpoint.throttle =  0.f;
 		_throttle_with_accel_limit.setForcedValue(0.f);
 		_steering_with_rate_limit.setForcedValue(0.f);
 	}
-
-	// Publish rover ackermann status (logging)
-	rover_ackermann_status_s rover_ackermann_status{};
-	rover_ackermann_status.timestamp = _timestamp;
-	rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle;
-	rover_ackermann_status.steering_setpoint = _motor_setpoint.steering;
-	rover_ackermann_status.actual_speed = _actual_speed;
-	_rover_ackermann_status_pub.publish(rover_ackermann_status);
-
-	// Publish to motor
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	actuator_motors.control[0] = _throttle_with_accel_limit.getState();
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	// Publish to servo
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = _steering_with_rate_limit.getState();
-	actuator_servos.timestamp = _timestamp;
-	_actuator_servos_pub.publish(actuator_servos);
 }
 
 int RoverAckermann::task_spawn(int argc, char *argv[])

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -109,7 +109,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
 		mission_finished = mission_result.finished;
 	}
 
-	if (nav_state == NAVIGATION_STATE_AUTO_RTL
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
 	    && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0),
 					     _next_wp(1)) < _acceptance_radius) { // Return to launch
 		mission_finished = true;

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -44,6 +44,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/home_position.h>
@@ -57,7 +58,6 @@
 #include <lib/pid/pid.h>
 
 using namespace matrix;
-static constexpr uint8_t NAVIGATION_STATE_AUTO_RTL = 5;
 
 /**
  * @brief Class for ackermann drive guidance.