Skip to content

Commit

Permalink
Improved Hold behaviour
Browse files Browse the repository at this point in the history 
Added local position as a valid source of position
  • Loading branch information
@ealdaz-seesai
ealdaz-seesai committed May 8, 2020
1 parent 678c3fe commit 9008f4c
Show file tree
Hide file tree
Showing 2 changed files with 69 additions and 57 deletions.
97 changes: 50 additions & 47 deletions src/modules/rover_pos_control/RoverPositionControl.cpp
View file
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) {
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
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 */
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{};

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

0 comments on commit 9008f4c

Please sign in to comment.