From 8464e82812e5f3d6dd0931856ecd44cd1d24bcac Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 9 Jun 2018 10:19:41 +0900 Subject: [PATCH] Rover: set_desired_speed range checks speed --- APMrover2/mode.cpp | 10 ++++++++++ APMrover2/mode.h | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 0bdade9f2d..2c4549e90f 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -216,6 +216,16 @@ void Mode::set_desired_speed_to_default(bool rtl) _desired_speed = get_speed_default(rtl); } +// set desired speed in m/s +bool Mode::set_desired_speed(float speed) +{ + if (!is_negative(speed)) { + _desired_speed = speed; + return true; + } + return false; +} + void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled) { // add in speed nudging diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 5183dc9821..78e5a4bde7 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -104,8 +104,8 @@ public: // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication) float get_speed_default(bool rtl = false) const; - // set desired speed - void set_desired_speed(float speed) { _desired_speed = speed; } + // set desired speed in m/s + bool set_desired_speed(float speed); // restore desired speed to default from parameter values (CRUISE_SPEED or WP_SPEED) // rtl argument should be true if called from RTL or SmartRTL modes (handled here to avoid duplication)