Line data    Source code

       1             : #include "AC_Sprayer_config.h"
       2             : 
       3             : #if HAL_SPRAYER_ENABLED
       4             : 
       5             : #include "AC_Sprayer.h"
       6             : #include <AP_AHRS/AP_AHRS.h>
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_Math/AP_Math.h>
       9             : #include <SRV_Channel/SRV_Channel.h>
      10             : 
      11             : extern const AP_HAL::HAL& hal;
      12             : 
      13             : // ------------------------------
      14             : 
      15             : const AP_Param::GroupInfo AC_Sprayer::var_info[] = {
      16             :     // @Param: ENABLE
      17             :     // @DisplayName: Sprayer enable/disable
      18             :     // @Description: Allows you to enable (1) or disable (0) the sprayer
      19             :     // @Values: 0:Disabled,1:Enabled
      20             :     // @User: Standard
      21             :     AP_GROUPINFO_FLAGS("ENABLE", 0, AC_Sprayer, _enabled, 0, AP_PARAM_FLAG_ENABLE),
      22             : 
      23             :     // @Param: PUMP_RATE
      24             :     // @DisplayName: Pump speed
      25             :     // @Description: Desired pump speed when travelling 1m/s expressed as a percentage
      26             :     // @Units: %
      27             :     // @Range: 0 100
      28             :     // @User: Standard
      29             :     AP_GROUPINFO("PUMP_RATE",   1, AC_Sprayer, _pump_pct_1ms, AC_SPRAYER_DEFAULT_PUMP_RATE),
      30             : 
      31             :     // @Param: SPINNER
      32             :     // @DisplayName: Spinner rotation speed
      33             :     // @Description: Spinner's rotation speed in PWM (a higher rate will disperse the spray over a wider area horizontally)
      34             :     // @Units: ms
      35             :     // @Range: 1000 2000
      36             :     // @User: Standard
      37             :     AP_GROUPINFO("SPINNER",     2, AC_Sprayer, _spinner_pwm, AC_SPRAYER_DEFAULT_SPINNER_PWM),
      38             : 
      39             :     // @Param: SPEED_MIN
      40             :     // @DisplayName: Speed minimum
      41             :     // @Description: Speed minimum at which we will begin spraying
      42             :     // @Units: cm/s
      43             :     // @Range: 0 1000
      44             :     // @User: Standard
      45             :     AP_GROUPINFO("SPEED_MIN",   3, AC_Sprayer, _speed_min, AC_SPRAYER_DEFAULT_SPEED_MIN),
      46             : 
      47             :     // @Param: PUMP_MIN
      48             :     // @DisplayName: Pump speed minimum
      49             :     // @Description: Minimum pump speed expressed as a percentage
      50             :     // @Units: %
      51             :     // @Range: 0 100
      52             :     // @User: Standard
      53             :     AP_GROUPINFO("PUMP_MIN",   4, AC_Sprayer, _pump_min_pct, AC_SPRAYER_DEFAULT_PUMP_MIN),
      54             : 
      55             :     AP_GROUPEND
      56             : };
      57             : 
      58        1286 : AC_Sprayer::AC_Sprayer()
      59             : {
      60        1286 :     if (_singleton) {
      61             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      62           0 :         AP_HAL::panic("Too many sprayers");
      63             : #endif
      64           0 :         return;
      65             :     }
      66        1286 :     _singleton = this;
      67             : 
      68        1286 :     AP_Param::setup_object_defaults(this, var_info);
      69             : 
      70             :     // check for silly parameter values
      71        1286 :     if (_pump_pct_1ms < 0.0f || _pump_pct_1ms > 100.0f) {
      72           0 :         _pump_pct_1ms.set_and_save(AC_SPRAYER_DEFAULT_PUMP_RATE);
      73             :     }
      74        1286 :     if (_spinner_pwm < 0) {
      75           0 :         _spinner_pwm.set_and_save(AC_SPRAYER_DEFAULT_SPINNER_PWM);
      76             :     }
      77             : 
      78             :     // To-Do: ensure that the pump and spinner servo channels are enabled
      79             : }
      80             : 
      81             : /*
      82             :  * Get the AP_Sprayer singleton
      83             :  */
      84             : AC_Sprayer *AC_Sprayer::_singleton;
      85          19 : AC_Sprayer *AC_Sprayer::get_singleton()
      86             : {
      87          19 :     return _singleton;
      88             : }
      89             : 
      90      267048 : void AC_Sprayer::run(const bool activate)
      91             : {
      92             :     // return immediately if no change
      93      267048 :     if (_flags.running == activate) {
      94      267036 :         return;
      95             :     }
      96             : 
      97             :     // set flag indicate whether spraying is permitted:
      98             :     // do not allow running to be set to true if we are currently not enabled
      99          12 :     _flags.running = _enabled && activate;
     100             : 
     101             :     // turn off the pump and spinner servos if necessary
     102          12 :     if (!_flags.running) {
     103           6 :         stop_spraying();
     104             :     }
     105             : }
     106             : 
     107         173 : void AC_Sprayer::stop_spraying()
     108             : {
     109         173 :     SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_pump, SRV_Channel::Limit::MIN);
     110         173 :     SRV_Channels::set_output_limit(SRV_Channel::k_sprayer_spinner, SRV_Channel::Limit::MIN);
     111             : 
     112         173 :     _flags.spraying = false;
     113         173 : }
     114             : 
     115             : /// update - adjust pwm of servo controlling pump speed according to the desired quantity and our horizontal speed
     116      267333 : void AC_Sprayer::update()
     117             : {
     118             :     // exit immediately if we are disabled or shouldn't be running
     119      267333 :     if (!_enabled || !running()) {
     120      267029 :         run(false);
     121      267029 :         return;
     122             :     }
     123             : 
     124             :     // exit immediately if the pump function has not been set-up for any servo
     125         304 :     if (!SRV_Channels::function_assigned(SRV_Channel::k_sprayer_pump)) {
     126           0 :         return;
     127             :     }
     128             : 
     129             :     // get horizontal velocity
     130         304 :     Vector3f velocity;
     131         304 :     if (!AP::ahrs().get_velocity_NED(velocity)) {
     132             :         // treat unknown velocity as zero which should lead to pump stopping
     133             :         // velocity will already be zero but this avoids a coverity warning
     134           0 :         velocity.zero();
     135             :     }
     136             : 
     137         304 :     float ground_speed = velocity.xy().length() * 100.0;
     138             : 
     139             :     // get the current time
     140         304 :     const uint32_t now = AP_HAL::millis();
     141             : 
     142         304 :     bool should_be_spraying = _flags.spraying;
     143             :     // check our speed vs the minimum
     144         304 :     if (ground_speed >= _speed_min) {
     145             :         // if we are not already spraying
     146         138 :         if (!_flags.spraying) {
     147             :             // set the timer if this is the first time we've surpassed the min speed
     148          10 :             if (_speed_over_min_time == 0) {
     149           5 :                 _speed_over_min_time = now;
     150             :             }else{
     151             :                 // check if we've been over the speed long enough to engage the sprayer
     152           5 :                 if((now - _speed_over_min_time) > AC_SPRAYER_DEFAULT_TURN_ON_DELAY) {
     153           5 :                     should_be_spraying = true;
     154           5 :                     _speed_over_min_time = 0;
     155             :                 }
     156             :             }
     157             :         }
     158             :         // reset the speed under timer
     159         138 :         _speed_under_min_time = 0;
     160             :     } else {
     161             :         // we are under the min speed.
     162         166 :         if (_flags.spraying) {
     163             :             // set the timer if this is the first time we've dropped below the min speed
     164           5 :             if (_speed_under_min_time == 0) {
     165           1 :                 _speed_under_min_time = now;
     166             :             }else{
     167             :                 // check if we've been over the speed long enough to engage the sprayer
     168           4 :                 if((now - _speed_under_min_time) > AC_SPRAYER_DEFAULT_SHUT_OFF_DELAY) {
     169           1 :                     should_be_spraying = false;
     170           1 :                     _speed_under_min_time = 0;
     171             :                 }
     172             :             }
     173             :         }
     174             :         // reset the speed over timer
     175         166 :         _speed_over_min_time = 0;
     176             :     }
     177             : 
     178             :     // if testing pump output speed as if travelling at 1m/s
     179         304 :     if (_flags.testing) {
     180           0 :         ground_speed = 100.0f;
     181           0 :         should_be_spraying = true;
     182             :     }
     183             : 
     184             :     // if spraying or testing update the pump servo position
     185         304 :     if (should_be_spraying) {
     186         137 :         float pos = ground_speed * _pump_pct_1ms;
     187         137 :         pos = MAX(pos, 100 *_pump_min_pct); // ensure min pump speed
     188         137 :         pos = MIN(pos,10000); // clamp to range
     189         137 :         SRV_Channels::move_servo(SRV_Channel::k_sprayer_pump, pos, 0, 10000);
     190         137 :         SRV_Channels::set_output_pwm(SRV_Channel::k_sprayer_spinner, _spinner_pwm);
     191         137 :         _flags.spraying = true;
     192             :     } else {
     193         167 :         stop_spraying();
     194             :     }
     195             : }
     196             : 
     197             : namespace AP {
     198             : 
     199          19 : AC_Sprayer *sprayer()
     200             : {
     201          19 :     return AC_Sprayer::get_singleton();
     202             : }
     203             : 
     204             : };
     205             : #endif // HAL_SPRAYER_ENABLED