SITL: SIM_Sprayer: a simulated sprayer

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

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

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple sprayer simulator class
+*/
+
+#include "SIM_Sprayer.h"
+#include <stdio.h>
+
+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;
+}
+

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  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;
+};
+
+}