dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Improved Hold behaviour 

Added local position as a valid source of position
This commit is contained in:
ealdaz 2019-12-20 13:41:15 +00:00 committed by Julian Oes
parent 3897030c6f
commit c96b5246ff
2 changed files with 71 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -220,51 +220,56 @@ 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;
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 if (dist <= pos_sp_triplet.current.loiter_radius / 2) {
_waypoint_reached = true;
} else {
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
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;
should_idle = _waypoint_reached;
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));
} 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;
if (dist_between_waypoints > 0) {
_pos_ctrl_state = GOTO_WAYPOINT; // A new waypoint has arrived go to it
}
//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;
}
if (should_idle) {
_act_controls.control[actuator_controls_s::INDEX_YAW] = 0.0f;
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
} else {
/* 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);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;
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;
}
_prev_wp = curr_wp;
} else {
_control_mode_current = UGV_POSCTRL_MODE_OTHER;
@ -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);
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
if (control_throttle >= 0.0f) {
_act_controls.control[actuator_controls_s::INDEX_YAW] = control_effort;
const float control_throttle = att_sp.thrust_body[0];
} else {
// reverse steering, if driving backwards
_act_controls.control[actuator_controls_s::INDEX_YAW] = -control_effort;
}
_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);
}
@ -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;
@ -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()) {
@ -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 */
@ -462,6 +463,7 @@ RoverPositionControl::run()
_pos_ctrl_status_pub.publish(pos_ctrl_status);
}
} else if (!manual_mode && _control_mode.flag_control_velocity_enabled) {
@ -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);

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

@ -99,10 +99,11 @@ public:
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 */
@ -113,15 +114,15 @@ private:
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)};
@ -138,13 +139,20 @@ private:
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,
@ -165,7 +173,8 @@ private:
(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 */
)
/**