Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simple sprayer simulator class
      17             : */
      18             : 
      19             : #include "SIM_Sprayer.h"
      20             : #include "AP_HAL/AP_HAL.h"
      21             : #include "AP_Math/AP_Math.h"
      22             : 
      23             : #include <stdio.h>
      24             : 
      25             : using namespace SITL;
      26             : 
      27             : // table of user settable parameters
      28             : const AP_Param::GroupInfo Sprayer::var_info[] = {
      29             : 
      30             :     // @Param: ENABLE
      31             :     // @DisplayName: Sprayer Sim enable/disable
      32             :     // @Description: Allows you to enable (1) or disable (0) the Sprayer simulation
      33             :     // @Values: 0:Disabled,1:Enabled
      34             :     // @User: Advanced
      35             :     AP_GROUPINFO("ENABLE", 0, Sprayer, sprayer_enable, 0),
      36             : 
      37             :     // @Param: PUMP
      38             :     // @DisplayName: Sprayer pump pin
      39             :     // @Description: The pin number that the Sprayer pump is connected to. (start at 1)
      40             :     // @Range: 0 15
      41             :     // @User: Advanced
      42             :     AP_GROUPINFO("PUMP", 1, Sprayer, sprayer_pump_pin, -1),
      43             : 
      44             :     // @Param: SPIN
      45             :     // @DisplayName: Sprayer spinner servo pin
      46             :     // @Description: The pin number that the Sprayer spinner servo is connected to. (start at 1)
      47             :     // @Range: 0 15
      48             :     // @User: Advanced
      49             :     AP_GROUPINFO("SPIN", 2, Sprayer, sprayer_spin_pin, -1),
      50             : 
      51             :     AP_GROUPEND
      52             : };
      53             : 
      54             : /*
      55             :   update sprayer state
      56             :  */
      57      271619 : void Sprayer::update(const struct sitl_input &input)
      58             : {
      59      271619 :     const int16_t pump_pwm = sprayer_pump_pin >= 1 ? input.servos[sprayer_pump_pin-1] : -1;
      60      271619 :     const int16_t spinner_pwm = sprayer_spin_pin >= 1 ? input.servos[sprayer_spin_pin-1] : -1;
      61      271619 :     const uint64_t now = AP_HAL::micros64();
      62      271619 :     const float dt = (now - last_update_us) * 1.0e-6f;
      63      271619 :     if (pump_pwm >= 0) {
      64             :         // update remaining payload
      65      271616 :         if (capacity > 0) {
      66      271616 :             const double delta = last_pump_output * pump_max_rate * dt;
      67      271616 :             capacity -= delta;
      68      271616 :             if (capacity < 0) {
      69           0 :                 capacity = 0.0f;
      70             :             }
      71             :         }
      72             : 
      73             :         // update pump
      74      271616 :         float pump_demand = (pump_pwm - 1000) * 0.001f;
      75             :         // ::fprintf(stderr, "pump_demand=%f\n", pump_demand);
      76      271616 :         if (pump_demand < 0) { // never updated
      77      110664 :             pump_demand = 0;
      78             :         }
      79      271616 :         const float pump_max_change = pump_slew_rate * 0.01f * dt;
      80      271616 :         last_pump_output =
      81      271616 :             constrain_float(pump_demand, last_pump_output - pump_max_change, last_pump_output + pump_max_change);
      82      271616 :         last_pump_output = constrain_float(last_pump_output, 0, 1);
      83             :     } else {
      84           3 :         last_pump_output = 0.0f;
      85             :     }
      86             :     // update spinner (if any)
      87      271619 :     if (spinner_pwm >= 0) {
      88      271385 :         const float spinner_demand = (spinner_pwm - 1000) * 0.001f;
      89      271385 :         const float spinner_max_change = spinner_slew_rate * 0.01f * dt;
      90      271385 :         last_spinner_output = constrain_float(spinner_demand,
      91             :                                               last_spinner_output - spinner_max_change,
      92             :                                               last_spinner_output + spinner_max_change);
      93      271385 :         last_spinner_output = constrain_float(last_spinner_output, 0, 1);
      94             :     }
      95             : 
      96      271619 :     if (should_report()) {
      97         139 :         printf("Remaining: %f litres\n", capacity);
      98         139 :         printf("Pump: %f l/s\n", last_pump_output * pump_max_rate);
      99         139 :         if (spinner_pwm >= 0) {
     100         138 :             printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate) / 360.0f);
     101             :         }
     102         139 :         last_report_us = now;
     103             :     }
     104      271619 :     last_update_us = now;
     105      271619 : }
     106             : 
     107      271619 : bool Sprayer::should_report()
     108             : {
     109      271619 :     if (AP_HAL::micros64() - last_report_us < report_interval) {
     110      168019 :         return false;
     111             :     }
     112             : 
     113      103600 :     if (!is_zero(last_pump_output) || !is_zero(last_spinner_output)) {
     114         135 :         zero_report_done = false;
     115         135 :         return true;
     116             :     }
     117             : 
     118      103465 :     if (!zero_report_done) {
     119           4 :         zero_report_done = true;
     120           4 :         return true;
     121             :     }
     122             : 
     123      103461 :     return false;
     124             : }