diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index e397695889f1..5f100e970393 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -156,7 +156,8 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, bool setpoint = true; - if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + if ((_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ _control_mode_current = UGV_POSCTRL_MODE_AUTO; @@ -272,6 +273,7 @@ RoverPositionControl::run() { _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); @@ -283,6 +285,7 @@ RoverPositionControl::run() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_local_pos_sub, 20); parameters_update(_params_sub, true); @@ -325,10 +328,24 @@ RoverPositionControl::run() /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); position_setpoint_triplet_poll(); vehicle_attitude_poll(); + //Convert Local setpoints to global setpoints + if (_control_mode.flag_control_offboard_enabled) { + if (!globallocalconverter_initialized()) { + globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_alt, _local_pos.ref_timestamp); + + } else { + globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); + + } + } + // update the reset counters in any case _pos_reset_counter = _global_pos.lat_lon_reset_counter; @@ -411,6 +428,7 @@ RoverPositionControl::run() orb_unsubscribe(_control_mode_sub); orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_manual_control_sub); orb_unsubscribe(_params_sub); orb_unsubscribe(_pos_sp_triplet_sub); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 5d1231c2b96e..c10a3cc68150 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -102,6 +103,7 @@ class RoverPositionControl : public ModuleBase, public Mod int _control_mode_sub{-1}; /**< control mode subscription */ int _global_pos_sub{-1}; + int _local_pos_sub{-1}; int _manual_control_sub{-1}; /**< notification of manual control updates */ int _params_sub{-1}; /**< notification of parameter updates */ int _pos_sp_triplet_sub{-1}; @@ -112,6 +114,7 @@ class RoverPositionControl : public ModuleBase, public Mod position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; sensor_combined_s _sensor_combined{};