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;
+};
+
+}