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

Rover hold/loiter #13767

Merged
merged 1 commit into from
May 13, 2020
+69 −57
Merged

Rover hold/loiter #13767

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
97 changes: 50 additions & 47 deletions src/modules/rover_pos_control/RoverPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -220,52 +220,57 @@ RoverPositionControl::control_position(const matrix::Vector2f &current_position,
}
}

float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);
float dist_target = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
(double)curr_wp(0), (double)curr_wp(1)); // pos_sp_triplet.current.lat, pos_sp_triplet.current.lon);

bool should_idle = true;
//PX4_INFO("Setpoint type %d", (int) pos_sp_triplet.current.type );
//PX4_INFO(" State machine state %d", (int) _pos_ctrl_state);
//PX4_INFO(" Setpoint Lat %f, Lon %f", (double) curr_wp(0), (double)curr_wp(1));
//PX4_INFO(" Distance to target %f", (double) dist_target);

if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
// Because of noise in measurements, if the rover was always trying to reach an exact point, it would
// move around when it should be parked. So, I try to get the rover within loiter_radius/2, but then
// once I reach that point, I don't move until I'm outside of loiter_radius.
// TODO: Find out if there's a better measurement to use than loiter_radius.
if (dist > pos_sp_triplet.current.loiter_radius) {
_waypoint_reached = false;

} else if (dist <= pos_sp_triplet.current.loiter_radius / 2) {
_waypoint_reached = true;
}

should_idle = _waypoint_reached;

} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
should_idle = false;
}
switch (_pos_ctrl_state) {
case GOTO_WAYPOINT: {
if (dist_target < _param_nav_loiter_rad.get()) {
_pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop.

} else {
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);

if (should_idle) {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;

} else {
float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
}
}
break;

/* waypoint is a plain navigation waypoint or the takeoff waypoint, does not matter */
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
case STOPPING: {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint.
float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0), (double)_prev_wp(1),
(double)curr_wp(0), (double)curr_wp(1));

_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
if (dist_between_waypoints > 0) {
_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
}

float desired_r = ground_speed_2d.norm_squared() / math::abs_t(_gnd_control.nav_lateral_acceleration_demand());
float desired_theta = (0.5f * M_PI_F) - atan2f(desired_r, _param_wheel_base.get());
float control_effort = (desired_theta / _param_max_turn_angle.get()) * sign(
_gnd_control.nav_lateral_acceleration_demand());
control_effort = math::constrain(control_effort, -1.0f, 1.0f);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
//PX4_INFO(" Distance between prev and curr waypoints %f", (double)dist_between_waypoints);
}
break;

default:
PX4_ERR("Unknown Rover State");
_pos_ctrl_state = STOPPING;
break;
}

_prev_wp = curr_wp;

} else {
_control_mode_current = UGV_POSCTRL_MODE_OTHER;
setpoint = false;
Expand Down Expand Up @@ -333,17 +338,11 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi
float control_effort = euler_sp(2) / _param_max_turn_angle.get();
control_effort = math::constrain(control_effort, -1.0f, 1.0f);

const float control_throttle = math::constrain(att_sp.thrust_body[0], -1.0f, 1.0f);

if (control_throttle >= 0.0f) {
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;

} else {
// reverse steering, if driving backwards
_act_controls.control[actuator_controls_s::INDEX_YAW] = -control_effort;
}
const float control_throttle = att_sp.thrust_body[0];

_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = control_throttle;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(control_throttle, 0.0f, 1.0f);

}

Expand All @@ -370,7 +369,7 @@ RoverPositionControl::run()
parameters_update(true);

/* wakeup source(s) */
px4_pollfd_struct_t fds[4];
px4_pollfd_struct_t fds[5];

/* Setup of loop */
fds[0].fd = _global_pos_sub;
Expand All @@ -379,8 +378,10 @@ RoverPositionControl::run()
fds[1].events = POLLIN;
fds[2].fd = _sensor_combined_sub;
fds[2].events = POLLIN;
fds[3].fd = _vehicle_attitude_sub;
fds[3].fd = _vehicle_attitude_sub; // Poll attitude
fds[3].events = POLLIN;
fds[4].fd = _local_pos_sub; // Added local position as source of position
fds[4].events = POLLIN;

while (!should_exit()) {

Expand All @@ -406,7 +407,7 @@ RoverPositionControl::run()
bool manual_mode = _control_mode.flag_control_manual_enabled;

/* only run controller if position changed */
if (fds[0].revents & POLLIN) {
if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we can use just local_position (fds[4]) and remove fds[0]

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is local position also available if only using GPS?

perf_begin(_loop_perf);

/* load local copies */
Expand Down Expand Up @@ -462,6 +463,7 @@ RoverPositionControl::run()

_pos_ctrl_status_pub.publish(pos_ctrl_status);


}

} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) {
Expand Down Expand Up @@ -514,6 +516,7 @@ RoverPositionControl::run()
/* Only publish if any of the proper modes are enabled */
if (_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_attitude_enabled ||
_control_mode.flag_control_position_enabled ||
manual_mode) {
/* publish the actuator controls */
_actuator_controls_pub.publish(_act_controls);
Expand Down
29 changes: 19 additions & 10 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 @@ -99,10 +99,11 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
void run() override;

private:
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)};

int _control_mode_sub{-1}; /**< control mode subscription */
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */
uORB::Publication<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */

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 */
Expand All @@ -113,15 +114,15 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod

uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

manual_control_setpoint_s _manual{}; /**< r/c channel data */
manual_control_setpoint_s _manual{}; /**< r/c channel data */
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

unnecessary indentation changes

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

White spaces are still not addressed

position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */
vehicle_attitude_setpoint_s _att_sp{}; /**< attitude setpoint > */
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{};
actuator_controls_s _act_controls{}; /**< direct control of actuators */
vehicle_attitude_s _vehicle_att{};
sensor_combined_s _sensor_combined{};
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same here

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

White spaces are still not addressed


SubscriptionData<vehicle_acceleration_s> _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};

Expand All @@ -138,13 +139,20 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod

ECL_L1_Pos_Controller _gnd_control;

bool _waypoint_reached{false};

enum UGV_POSCTRL_MODE {
UGV_POSCTRL_MODE_AUTO,
UGV_POSCTRL_MODE_OTHER
} _control_mode_current{UGV_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.


enum POS_CTRLSTATES {
GOTO_WAYPOINT,
STOPPING
} _pos_ctrl_state {STOPPING}; /// Position control state machine

/* previous waypoint */
matrix::Vector2f _prev_wp{0.0f, 0.0f};

DEFINE_PARAMETERS(
(ParamFloat<px4::params::GND_L1_PERIOD>) _param_l1_period,
(ParamFloat<px4::params::GND_L1_DAMPING>) _param_l1_damping,
Expand All @@ -165,7 +173,8 @@ class RoverPositionControl : public ModuleBase<RoverPositionControl>, public Mod
(ParamFloat<px4::params::GND_THR_CRUISE>) _param_throttle_cruise,

(ParamFloat<px4::params::GND_WHEEL_BASE>) _param_wheel_base,
(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle
(ParamFloat<px4::params::GND_MAX_ANG>) _param_max_turn_angle,
(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad /**< loiter radius for Rover */
)

/**
Expand Down