AC_Sprayer: disentangle ENABLED from permission-to-run

libraries/AC_Sprayer/AC_Sprayer.cpp

@@ -66,34 +66,41 @@ AC_Sprayer::AC_Sprayer(const AP_InertialNav* inav) :
     // To-Do: ensure that the pump and spinner servo channels are enabled
 }
 
-void AC_Sprayer::enable(bool true_false)
+void AC_Sprayer::run(const bool true_false)
 {
     // return immediately if no change
-    if (true_false == _enabled) {
+    if (true_false == _flags.running) {
         return;
     }
 
-    // set enabled/disabled parameter (in memory only)
-    _enabled = true_false;
+    // set flag indicate whether spraying is permitted:
+    // do not allow running to be set to true if we are currently not enabled
+    _flags.running = true_false && _enabled;
 
     // turn off the pump and spinner servos if necessary
-    if (!_enabled) {
-        // send output to pump channel
-        // To-Do: change 0 below to radio_min of pump servo
-        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_pump);
-
-        // send output to spinner channel
-        // To-Do: change 0 below to radio_min of spinner servo
-        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_spinner);
+    if (!_flags.running) {
+        stop_spraying();
     }
 }
 
+void AC_Sprayer::stop_spraying()
+{
+    // send output to pump channel
+    RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_pump);
+
+    // send output to spinner channel
+    RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_spinner);
+
+    _flags.spraying = false;
+}
+
 /// update - adjust pwm of servo controlling pump speed according to the desired quantity and our horizontal speed
 void
 AC_Sprayer::update()
 {
-    // exit immediately if we are disabled (perhaps set pwm values back to defaults)
-    if (!_enabled) {
+    // exit immediately if we are disabled or shouldn't be running
+    if (!_enabled || !running()) {
+        run(false);
         return;
     }
 
@@ -109,6 +116,7 @@ AC_Sprayer::update()
     // get the current time
     const uint32_t now = AP_HAL::millis();
 
+    bool should_be_spraying = _flags.spraying;
     // check our speed vs the minimum
     if (ground_speed >= _speed_min) {
         // if we are not already spraying
@@ -119,7 +127,7 @@ AC_Sprayer::update()
             }else{
                 // check if we've been over the speed long enough to engage the sprayer
                 if((now - _speed_over_min_time) > AC_SPRAYER_DEFAULT_TURN_ON_DELAY) {
-                    _flags.spraying = true;
+                    should_be_spraying = true;
                     _speed_over_min_time = 0;
                 }
             }
@@ -135,7 +143,7 @@ AC_Sprayer::update()
             }else{
                 // check if we've been over the speed long enough to engage the sprayer
                 if((now - _speed_under_min_time) > AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY) {
-                    _flags.spraying = false;
+                    should_be_spraying = false;
                     _speed_under_min_time = 0;
                 }
             }
@@ -147,15 +155,18 @@ AC_Sprayer::update()
     // if testing pump output speed as if travelling at 1m/s
     if (_flags.testing) {
         ground_speed = 100.0f;
+        should_be_spraying = true;
     }
 
     // if spraying or testing update the pump servo position
-    if (_flags.spraying || _flags.testing) {        
-        RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
+    if (should_be_spraying) {
+        float pos = ground_speed * _pump_pct_1ms;
+        pos = MAX(pos, 100 *_pump_min_pct); // ensure min pump speed
+        pos = MIN(pos,10000); // clamp to range
+        RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, pos, 0, 10000);
         RC_Channel_aux::set_radio(RC_Channel_aux::k_sprayer_spinner, _spinner_pwm);
+        _flags.spraying = true;
     }else{
-        // ensure sprayer and spinner are off
-        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_pump);
-        RC_Channel_aux::set_radio_to_min(RC_Channel_aux::k_sprayer_spinner);
+        stop_spraying();
     }
 }

libraries/AC_Sprayer/AC_Sprayer.h

@@ -37,11 +37,14 @@ public:
     /// Constructor
     AC_Sprayer(const AP_InertialNav* inav);
 
-    /// enable - allows sprayer to be enabled/disabled.  Note: this does not update the eeprom saved value
-    void enable(bool true_false);
+    /// run - allow or disallow spraying to occur
+    void run(bool true_false);
 
-    /// enabled - returns true if sprayer is enabled
-    bool enabled() const { return _enabled; }
+    /// running - returns true if spraying is currently permitted
+    bool running() const { return _flags.running; }
+
+    /// spraying - returns true if spraying is actually happening
+    bool spraying() const { return _flags.spraying; }
 
     /// test_pump - set to true to turn on pump as if travelling at 1m/s as a test
     void test_pump(bool true_false) { _flags.testing = true_false; }
@@ -70,9 +73,12 @@ private:
     struct sprayer_flags_type {
         uint8_t spraying    : 1;            ///< 1 if we are currently spraying
         uint8_t testing     : 1;            ///< 1 if we are testing the sprayer and should output a minimum value
+        uint8_t running     : 1;            ///< 1 if we are permitted to run sprayer
     } _flags;
 
     // internal variables
     uint32_t        _speed_over_min_time;   ///< time at which we reached speed minimum
     uint32_t        _speed_under_min_time;  ///< time at which we fell below speed minimum
+
+    void stop_spraying();
 };