Offboard support for rover

src/modules/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "regex": "cpp"
+    }
+}

src/modules/rover_ackermann/RoverAckermann.cpp

@@ -92,11 +92,14 @@ void RoverAckermann::Run()
 				}
 
 			} break;
-
+		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+		     _motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
+		     break;  
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
 		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
 			_motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
 			break;
+
 
 		default: // Unimplemented nav states will stop the rover
 			_motor_setpoint.steering = 0.f;

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp

@@ -81,7 +81,8 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
 	} else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command
 		_desired_speed = 0.f;
 
-	} else { // Regular guidance algorithm
+    }
+    else { // Regular guidance algorithm
 
 		_desired_speed = calcDesiredSpeed(_wp_max_desired_vel, _param_ra_miss_vel_min.get(),
 						  _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius,
@@ -115,6 +116,10 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
 
 void RoverAckermannGuidance::updateSubscriptions()
 {
+	if (_control_mode_sub.updated()) {
+		_control_mode_sub.copy(&_control_mode);
+	}
+
 	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s vehicle_global_position{};
 		_vehicle_global_position_sub.copy(&vehicle_global_position);
@@ -141,9 +146,12 @@ void RoverAckermannGuidance::updateSubscriptions()
 		_home_position = Vector2d(home_position.lat, home_position.lon);
 	}
 
-	if (_position_setpoint_triplet_sub.updated()) {
+	if (_position_setpoint_triplet_sub.updated() && !_control_mode.flag_control_offboard_enabled) {
 		updateWaypointsAndAcceptanceRadius();
 	}
+    if (_control_mode.flag_control_offboard_enabled){
+		updateWaypointsAndAcceptanceRadius();
+    }
 
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude{};
@@ -186,6 +194,11 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
 		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
 
 	}
+    if (_control_mode.flag_control_offboard_enabled) {
+        _trajectory_setpoint_sub.update(&_trajectory_setpoint);
+	_curr_wp=Vector2d(_trajectory_setpoint.position[0], _trajectory_setpoint.position[1]);
+        _next_wp =_curr_wp;
+	}
 
 	// NED waypoint coordinates
 	_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));

src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp

@@ -49,6 +49,10 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/home_position.h>
+//changes
+#include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
 
 // Standard library includes
 #include <matrix/matrix/math.hpp>
@@ -180,6 +184,12 @@ class RoverAckermannGuidance : public ModuleParams
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
 	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+    // changes
+    uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+    uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */
+    vehicle_control_mode_s _control_mode{};		/**< control mode */
+	//
+    trajectory_setpoint_s _trajectory_setpoint{};
 
 	// uORB publications
 	uORB::Publication<rover_ackermann_guidance_status_s> _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)};