Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable Offboard support for Rover position control #12765

Merged
merged 2 commits into from
Aug 21, 2019
Merged

Enable Offboard support for Rover position control #12765

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
46d6973
WIP offboard mode
Jaeyoung-Lim Aug 18, 2019
56662ae
Add local position subscription
Jaeyoung-Lim Aug 18, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 19 additions & 1 deletion src/modules/rover_pos_control/RoverPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,8 @@ RoverPositionControl::control_position(const matrix::Vector2f &current_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;
Expand Down Expand Up @@ -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));
Expand All @@ -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);

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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);
Expand Down
3 changes: 3 additions & 0 deletions src/modules/rover_pos_control/RoverPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/ekf2_timestamps.h>
#include <uORB/uORB.h>
Expand Down Expand Up @@ -102,6 +103,7 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, 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};
Expand All @@ -112,6 +114,7 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, 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{};
Expand Down