diff --git a/APMrover2/mode.h b/APMrover2/mode.h index c4a5d731b3..38b34bda3d 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -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 }; diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index dec02f7251..fd210e280c 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -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)); }