diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 78afb6da6b..b51205268f 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -66,6 +66,9 @@ void MultiCopter::update(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); + + // update sprayer + sprayer.update(input); } diff --git a/libraries/SITL/SIM_Multicopter.h b/libraries/SITL/SIM_Multicopter.h index 2d9c88c6fb..90dbf35c53 100644 --- a/libraries/SITL/SIM_Multicopter.h +++ b/libraries/SITL/SIM_Multicopter.h @@ -21,6 +21,7 @@ #include "SIM_Aircraft.h" #include "SIM_Motor.h" #include "SIM_Frame.h" +#include "SIM_Sprayer.h" namespace SITL { @@ -43,6 +44,11 @@ protected: // calculate rotational and linear accelerations void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); Frame *frame; + + // The numbers here are offsets into the input servos array + // (generally output-servo-number-1 e.g. 2 for throttle) + Sprayer sprayer{6, 7}; + }; } diff --git a/libraries/SITL/SIM_Sprayer.cpp b/libraries/SITL/SIM_Sprayer.cpp new file mode 100644 index 0000000000..60093d5f24 --- /dev/null +++ b/libraries/SITL/SIM_Sprayer.cpp @@ -0,0 +1,90 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple sprayer simulator class +*/ + +#include "SIM_Sprayer.h" +#include + +using namespace SITL; + +/* + update sprayer state + */ +void Sprayer::update(const Aircraft::sitl_input &input) +{ + const uint64_t now = AP_HAL::micros64(); + const float dt = (now - last_update_us) * 1.0e-6f; + + // update remaining payload + if (capacity > 0) { + const double delta = last_pump_output * pump_max_rate * dt; + capacity -= delta; + if (capacity < 0) { + capacity = 0.0f; + } + } + + // update pump + float pump_demand = (input.servos[pump_servo]-1000) * 0.001f; + // ::fprintf(stderr, "pump_demand=%f\n", pump_demand); + if (pump_demand < 0) { // never updated + pump_demand = 0; + } + const float pump_max_change = pump_slew_rate/100.0f * dt; + last_pump_output = constrain_float(pump_demand, last_pump_output-pump_max_change, last_pump_output+pump_max_change); + last_pump_output = constrain_float(last_pump_output, 0, 1); + + // update spinner (if any) + if (spinner_servo >= 0) { + const float spinner_demand = (input.servos[spinner_servo]-1000) * 0.001f; + const float spinner_max_change = spinner_slew_rate * 0.01f * dt; + last_spinner_output = constrain_float(spinner_demand, last_spinner_output-spinner_max_change, last_spinner_output+spinner_max_change); + last_spinner_output = constrain_float(last_spinner_output, 0, 1); + } + + if (should_report()) { + printf("Remaining: %f litres\n", capacity); + printf("Pump: %f l/s\n", last_pump_output * pump_max_rate); + if (spinner_servo >= 0) { + printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate)/360.0f); + } + last_report_us = now; + } + + last_update_us = now; + return; +} + +bool Sprayer::should_report() +{ + if (AP_HAL::micros64() - last_report_us < report_interval) { + return false; + } + + if (!is_zero(last_pump_output) || !is_zero(last_spinner_output)) { + zero_report_done = false; + return true; + } + + if (!zero_report_done) { + zero_report_done = true; + return true; + } + + return false; +} + diff --git a/libraries/SITL/SIM_Sprayer.h b/libraries/SITL/SIM_Sprayer.h new file mode 100644 index 0000000000..e6f3043311 --- /dev/null +++ b/libraries/SITL/SIM_Sprayer.h @@ -0,0 +1,62 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple sprayer simulation class +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +namespace SITL { + +class Sprayer { +public: + const uint8_t pump_servo; + const int8_t spinner_servo; + + Sprayer(const uint8_t _pump_servo, int8_t _spinner_servo) : + pump_servo(_pump_servo), + spinner_servo(_spinner_servo) + {} + + // update sprayer state + void update(const struct Aircraft::sitl_input &input); + + float payload_mass() const { return capacity; }; // kg; water, so kg=l + +private: + + const uint32_t report_interval = 1000000; // microseconds + uint64_t last_report_us; + + const float pump_max_rate = 0.01; // litres/second + const float pump_slew_rate = 20; // percent/scond + float last_pump_output; // percentage + + const float spinner_max_rate = 3600; // degrees/second + const float spinner_slew_rate = 20; // percent/second + float last_spinner_output; // percentage + + double capacity = 0.25; // litres + + uint64_t start_time_us; + uint64_t last_update_us; + + bool should_report(); + bool zero_report_done = false; +}; + +}