From 35f0402b476e5553e4b4c6d61018a8ebe3d179ab Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Jun 2020 16:29:00 +0900 Subject: [PATCH] Rover: Guided mode supports direct steering and throttle control --- Rover/Rover.cpp | 13 +++++++++++++ Rover/Rover.h | 1 + Rover/mode.h | 12 +++++++++++- Rover/mode_guided.cpp | 43 +++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 68 insertions(+), 1 deletion(-) diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 4c89582d33..780677403c 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -167,6 +167,19 @@ bool Rover::set_target_velocity_NED(const Vector3f& vel_ned) return true; } +// set steering and throttle (-1 to +1) (for use by scripting) +bool Rover::set_steering_and_throttle(float steering, float throttle) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!control_mode->in_guided_mode()) { + return false; + } + + // set steering and throttle + mode_guided.set_steering_and_throttle(steering, throttle); + return true; +} + #if STATS_ENABLED == ENABLED /* update AP_Stats diff --git a/Rover/Rover.h b/Rover/Rover.h index 4f204ac880..aaec91148f 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -278,6 +278,7 @@ private: // Rover.cpp bool set_target_location(const Location& target_loc) override; bool set_target_velocity_NED(const Vector3f& vel_ned) override; + bool set_steering_and_throttle(float steering, float throttle) override; void stats_update(); void ahrs_update(); void gcs_failsafe_check(void); diff --git a/Rover/mode.h b/Rover/mode.h index 1c55602afb..acf86e6e03 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -387,6 +387,9 @@ public: 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); + // set steering and throttle (-1 to +1). Only called from scripts + void set_steering_and_throttle(float steering, float throttle); + // vehicle start loiter bool start_loiter(); @@ -402,7 +405,8 @@ protected: Guided_WP, Guided_HeadingAndSpeed, Guided_TurnRateAndSpeed, - Guided_Loiter + Guided_Loiter, + Guided_SteeringAndThrottle }; bool _enter() override; @@ -416,6 +420,12 @@ protected: bool sent_notification; // used to send one time notification to ground station float _desired_speed; // desired speed used only in HeadingAndSpeed submode + // direct steering and throttle control + bool _have_strthr; // true if we have a valid direct steering and throttle inputs + uint32_t _strthr_time_ms; // system time last call to set_steering_and_throttle was made (used for timeout) + float _strthr_steering; // direct steering input in the range -1 to +1 + float _strthr_throttle; // direct throttle input in the range -1 to +1 + // limits struct { uint32_t timeout_ms;// timeout from the time that guided is invoked diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp index eed844de0f..06af7184c0 100644 --- a/Rover/mode_guided.cpp +++ b/Rover/mode_guided.cpp @@ -105,6 +105,30 @@ void ModeGuided::update() break; } + case Guided_SteeringAndThrottle: + { + // handle timeout + if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > 3000) { + _have_strthr = false; + gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); + } + if (_have_strthr) { + // pass latest steering and throttle directly to motors library + g2.motors.set_steering(_strthr_steering * 4500.0f, false); + g2.motors.set_throttle(_strthr_throttle * 100.0f); + } else { + // loiter or stop vehicle + if (rover.is_boat()) { + if (!start_loiter()) { + stop_vehicle(); + } + } else { + stop_vehicle(); + } + } + break; + } + default: gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; @@ -122,6 +146,8 @@ float ModeGuided::get_distance_to_destination() const return 0.0f; case Guided_Loiter: return rover.mode_loiter.get_distance_to_destination(); + case Guided_SteeringAndThrottle: + return 0.0f; } // we should never reach here but just in case, return 0 @@ -137,6 +163,7 @@ bool ModeGuided::reached_destination() const case Guided_HeadingAndSpeed: case Guided_TurnRateAndSpeed: case Guided_Loiter: + case Guided_SteeringAndThrottle: return true; } @@ -160,6 +187,9 @@ bool ModeGuided::set_desired_speed(float speed) return false; case Guided_Loiter: return rover.mode_loiter.set_desired_speed(speed); + case Guided_SteeringAndThrottle: + // no speed control + return false; } return false; } @@ -181,6 +211,9 @@ bool ModeGuided::get_desired_location(Location& destination) const case Guided_Loiter: // get destination from loiter return rover.mode_loiter.get_desired_location(destination); + case Guided_SteeringAndThrottle: + // no desired location in this submode + break; } // should never get here but just in case @@ -246,6 +279,16 @@ void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float targ rover.Log_Write_GuidedTarget(_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f)); } +// set steering and throttle (both in the range -1 to +1) +void ModeGuided::set_steering_and_throttle(float steering, float throttle) +{ + _guided_mode = ModeGuided::Guided_SteeringAndThrottle; + _strthr_time_ms = AP_HAL::millis(); + _strthr_steering = constrain_float(steering, -1.0f, 1.0f); + _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f); + _have_strthr = true; +} + bool ModeGuided::start_loiter() { if (rover.mode_loiter.enter()) {