Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: guided mode refactoring 

add set-desired methods
add get_distance_to_destination
fix to one interation with no update to motors
slow down for turns
This commit is contained in:
Randy Mackay 2017-08-03 17:14:16 +09:00
parent 279491ed20
commit 946a0a8e54
2 changed files with 136 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
APMrover2/mode.h
View File

@ -176,24 +176,38 @@ public:
// methods that affect movement of the vehicle in this mode
void update() override;
void update_navigation() override;
// attributes of the mode
bool is_autopilot_mode() const override { return true; }
bool failsafe_throttle_suppress() const override { return false; }
enum GuidedMode {
Guided_WP,
Guided_Angle,
Guided_Velocity
};
// return distance (in meters) to destination
float get_distance_to_destination() const override;
// Guided
GuidedMode guided_mode; // stores which GUIDED mode the vehicle is in
// set desired location, heading and speed
void set_desired_location(const struct Location& destination) override;
void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
// set desired heading-delta, turn-rate and speed
void set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed);
void set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed);
protected:
enum GuidedMode {
Guided_WP,
Guided_HeadingAndSpeed,
Guided_TurnRateAndSpeed
};
bool _enter() override;
GuidedMode _guided_mode; // stores which GUIDED mode the vehicle is in
// attitude control
bool have_attitude_target; // true if we have a valid attitude target
uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout)
float _desired_yaw_rate_cds;// target turn rate centi-degrees per second
};

138
APMrover2/mode_guided.cpp
View File

@ -8,7 +8,7 @@ bool ModeGuided::_enter()
location. This matches the behaviour of the copter code.
*/
lateral_acceleration = 0.0f;
rover.set_guided_WP(rover.current_loc);
set_desired_location(rover.current_loc);
g2.motors.slew_limit_throttle(true);
return true;
}
@ -19,32 +19,66 @@ void ModeGuided::update()
rover.set_reverse(false);
}
switch (guided_mode) {
case Guided_Angle:
rover.nav_set_yaw_speed();
break;
switch (_guided_mode) {
case Guided_WP:
if (rover.rtl_complete || rover.verify_RTL()) {
// we have reached destination so stop where we are
if (fabsf(g2.motors.get_throttle()) > g.throttle_min.get()) {
{
if (!_reached_destination) {
// check if we've reached the destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
// trigger reached
_reached_destination = true;
rover.gcs().send_mission_item_reached_message(0);
}
// continue driving towards destination
calc_lateral_acceleration(_origin, _destination);
calc_nav_steer();
calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
} else {
// we've reached destination so stop
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
} else {
calc_lateral_acceleration();
calc_nav_steer();
calc_throttle(rover.guided_control.target_speed);
rover.Log_Write_GuidedTarget(guided_mode, Vector3f(rover.next_WP.lat, rover.next_WP.lng, rover.next_WP.alt),
Vector3f(rover.guided_control.target_speed, g2.motors.get_throttle(), 0.0f));
}
break;
}
case Guided_Velocity:
rover.nav_set_speed();
case Guided_HeadingAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
const float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
g2.motors.set_steering(rover.steerController.get_steering_out_angle_error(yaw_error_cd));
calc_throttle(_desired_speed);
} else {
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
}
break;
}
case Guided_TurnRateAndSpeed:
{
// stop vehicle if target not updated within 3 seconds
if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) {
gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping");
have_attitude_target = false;
}
if (have_attitude_target) {
// run steering and throttle controllers
g2.motors.set_steering(rover.steerController.get_steering_out_rate(_desired_yaw_rate_cds / 100.0f));
calc_throttle(_desired_speed);
} else {
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
}
break;
}
default:
gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
@ -52,13 +86,69 @@ void ModeGuided::update()
}
}
void ModeGuided::update_navigation()
// return distance (in meters) to destination
float ModeGuided::get_distance_to_destination() const
{
// no loitering around the wp with the rover, goes direct to the wp position
if (guided_mode == Guided_WP && (rover.rtl_complete || rover.verify_RTL())) {
// we have reached destination so stop where we are
g2.motors.set_throttle(g.throttle_min.get());
g2.motors.set_steering(0.0f);
lateral_acceleration = 0.0f;
if (_guided_mode != Guided_WP || _reached_destination) {
return 0.0f;
}
return _distance_to_destination;
}
// set desired location
void ModeGuided::set_desired_location(const struct Location& destination)
{
// call parent
Mode::set_desired_location(destination);
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_WP;
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_destination.lat, _destination.lng, 0), Vector3f(_desired_speed, 0.0f, 0.0f));
}
// set desired attitude
void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
{
// call parent
Mode::set_desired_heading_and_speed(yaw_angle_cd, target_speed);
// handle guided specific initialisation and logging
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_des_att_time_ms = AP_HAL::millis();
_reached_destination = false;
// record targets
_desired_yaw_cd = yaw_angle_cd;
_desired_speed = target_speed;
have_attitude_target = true;
// log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
}
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
{
// handle initialisation
if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) {
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_desired_yaw_cd = ahrs.yaw_sensor;
}
set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd+yaw_delta_cd), target_speed);
}
// set desired velocity
void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed)
{
// handle initialisation
_guided_mode = ModeGuided::Guided_TurnRateAndSpeed;
_des_att_time_ms = AP_HAL::millis();
_reached_destination = false;
// record targets
_desired_yaw_rate_cds = turn_rate_cds;
_desired_speed = target_speed;
have_attitude_target = true;
// log new target
rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
}