fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Enable Offboard support for Rover position control

Browse Source
This commit is contained in:
JaeyoungLim 2019-08-21 18:04:19 +02:00 committed by Daniel Agar
parent ccdc2dffa9
commit 950dbc1d2e
2 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/rover_pos_control/RoverPositionControl.cpp
View File

@ -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;
@ -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);

3
src/modules/rover_pos_control/RoverPositionControl.hpp
View File

@ -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>
@ -102,6 +103,7 @@ private:
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 @@ private:
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{};