SITL: rearrange Grippers and Sprayers in SITL

libraries/SITL/SIM_Aircraft.cpp

@@ -84,7 +84,6 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
     // allow for orientation settings, such as with tailsitters
     enum ap_var_type ptype;
     ahrs_orientation = (AP_Int8 *)AP_Param::find("AHRS_ORIENTATION", &ptype);
-
     terrain = reinterpret_cast<AP_Terrain *>(AP_Param::find_object("TERRAIN_"));
 }
 
@@ -737,4 +736,24 @@ void Aircraft::extrapolate_sensors(float delta_time)
     velocity_air_bf = dcm.transposed() * velocity_air_ef;
 }
 
+void Aircraft::update_external_payload(const struct sitl_input &input)
+{
+    external_payload_mass = 0;
 
+    // update sprayer
+    if (sprayer && sprayer->is_enabled()) {
+        sprayer->update(input);
+        external_payload_mass += sprayer->payload_mass();
+    }
+
+    // update grippers
+    if (gripper && gripper->is_enabled()) {
+        gripper->set_alt(hagl());
+        gripper->update(input);
+        external_payload_mass += gripper->payload_mass();
+    }
+    if (gripper_epm && gripper_epm->is_enabled()) {
+        gripper_epm->update(input);
+        external_payload_mass += gripper_epm->payload_mass();
+    }
+}

libraries/SITL/SIM_Aircraft.h

@@ -21,8 +21,11 @@
 #include <AP_Math/AP_Math.h>
 
 #include "SITL.h"
+#include "SITL_Input.h"
 #include <AP_Terrain/AP_Terrain.h>
-
+#include "SIM_Sprayer.h"
+#include "SIM_Gripper_Servo.h"
+#include "SIM_Gripper_EPM.h"
 
 namespace SITL {
 
@@ -30,25 +33,9 @@ namespace SITL {
   parent class for all simulator types
  */
 class Aircraft {
-    friend class Gripper_Servo;
-
 public:
     Aircraft(const char *home_str, const char *frame_str);
 
-    /*
-      structure passed in giving servo positions as PWM values in
-      microseconds
-     */
-    struct sitl_input {
-        uint16_t servos[16];
-        struct {
-            float speed;      // m/s
-            float direction;  // degrees 0..360
-            float turbulence;
-            float dir_z;	  //degrees -90..90 
-        } wind;
-    };
-
     /*
       set simulation speedup
      */
@@ -111,7 +98,7 @@ public:
         return mag_bf;
     }
 
-    virtual float gross_mass() const { return mass; }
+    float gross_mass() const { return mass + external_payload_mass; }
 
     const Location &get_location() const { return location; }
 
@@ -121,6 +108,10 @@ public:
         attitude.from_rotation_matrix(dcm);
     }
 
+    void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
+    void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
+    void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
+
 protected:
     SITL *sitl;
     Location home;
@@ -129,26 +120,27 @@ protected:
     float ground_level;
     float home_yaw;
     float frame_height;
-    Matrix3f dcm;                   // rotation matrix, APM conventions, from body to earth
+    Matrix3f dcm;                        // rotation matrix, APM conventions, from body to earth
-    Vector3f gyro;                  // rad/s
+    Vector3f gyro;                       // rad/s
-    Vector3f gyro_prev;             // rad/s
+    Vector3f gyro_prev;                  // rad/s
-    Vector3f ang_accel;             // rad/s/s
+    Vector3f ang_accel;                  // rad/s/s
-    Vector3f velocity_ef;           // m/s, earth frame
+    Vector3f velocity_ef;                // m/s, earth frame
-    Vector3f wind_ef;               // m/s, earth frame
+    Vector3f wind_ef;                    // m/s, earth frame
-    Vector3f velocity_air_ef;       // velocity relative to airmass, earth frame
+    Vector3f velocity_air_ef;            // velocity relative to airmass, earth frame
-    Vector3f velocity_air_bf;       // velocity relative to airmass, body frame
+    Vector3f velocity_air_bf;            // velocity relative to airmass, body frame
-    Vector3f position;              // meters, NED from origin
+    Vector3f position;                   // meters, NED from origin
-    float mass;                     // kg
+    float mass;                          // kg
-    Vector3f accel_body;            // m/s/s NED, body frame
+    float external_payload_mass = 0.0f;  // kg
-    float airspeed;                 // m/s, apparent airspeed
+    Vector3f accel_body;                 // m/s/s NED, body frame
-    float airspeed_pitot;           // m/s, apparent airspeed, as seen by fwd pitot tube
+    float airspeed;                      // m/s, apparent airspeed
+    float airspeed_pitot;                // m/s, apparent airspeed, as seen by fwd pitot tube
     float battery_voltage = -1.0f;
     float battery_current = 0.0f;
     float rpm1 = 0;
     float rpm2 = 0;
     uint8_t rcin_chan_count = 0;
     float rcin[8];
-    float range = -1.0f;            // rangefinder detection in m
+    float range = -1.0f;                 // rangefinder detection in m
 
     // Wind Turbulence simulated Data
     float turbulence_azimuth = 0.0f;
@@ -175,7 +167,7 @@ protected:
 
     // allow for AHRS_ORIENTATION
     AP_Int8 *ahrs_orientation;
-    
+
     enum {
         GROUND_BEHAVIOR_NONE = 0,
         GROUND_BEHAVIOR_NO_MOVEMENT,
@@ -234,7 +226,10 @@ protected:
 
     // extrapolate sensors by a given delta time in seconds
     void extrapolate_sensors(float delta_time);
-    
+
+    // update external payload/sensor dynamic
+    void update_external_payload(const struct sitl_input &input);
+
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;
@@ -253,6 +248,10 @@ private:
     } smoothing;
 
     LowPassFilterFloat servo_filter[4];
+
+    Sprayer *sprayer;
+    Gripper_Servo *gripper;
+    Gripper_EPM *gripper_epm;
 };
 
 } // namespace SITL

libraries/SITL/SIM_Calibration.cpp

@@ -30,7 +30,7 @@ SITL::Calibration::Calibration(const char *home_str, const char *frame_str)
     mass = 1.5f;
 }
 
-void SITL::Calibration::update(const struct sitl_input& input)
+void SITL::Calibration::update(const struct sitl_input &input)
 {
     Vector3f rot_accel{0, 0, 0};
 
@@ -54,7 +54,7 @@ void SITL::Calibration::update(const struct sitl_input& input)
     update_mag_field_bf();
 }
 
-void SITL::Calibration::_stop_control(const struct sitl_input& input,
+void SITL::Calibration::_stop_control(const struct sitl_input &input,
                                       Vector3f& rot_accel)
 {
     Vector3f desired_angvel{0, 0, 0};
@@ -65,7 +65,7 @@ void SITL::Calibration::_stop_control(const struct sitl_input& input,
     rot_accel *= 0.002f;
 }
 
-void SITL::Calibration::_attitude_control(const struct sitl_input& input,
+void SITL::Calibration::_attitude_control(const struct sitl_input &input,
                                           Vector3f& rot_accel)
 {
     float desired_roll = -M_PI + 2 * M_PI * (input.servos[5] - 1000) / 1000.f;
@@ -103,7 +103,7 @@ void SITL::Calibration::_attitude_set(float desired_roll, float desired_pitch, f
     rot_accel = error * (1.0f / dt);
 }
 
-void SITL::Calibration::_angular_velocity_control(const struct sitl_input& in,
+void SITL::Calibration::_angular_velocity_control(const struct sitl_input &in,
                                                   Vector3f& rot_accel)
 {
     Vector3f axis{(float)(in.servos[5] - 1500),

libraries/SITL/SIM_Calibration.h

@@ -66,22 +66,22 @@ class Calibration : public Aircraft {
 public:
     Calibration(const char *home_str, const char *frame_str);
 
-    void update(const struct sitl_input& input);
+    void update(const struct sitl_input &input);
 
     static Aircraft *create(const char *home_str, const char *frame_str) {
         return new Calibration(home_str, frame_str);
     }
 
 private:
-    void _stop_control(const struct sitl_input& input, Vector3f& rot_accel);
+    void _stop_control(const struct sitl_input &input, Vector3f& rot_accel);
 
     void _attitude_set(float desired_roll, float desired_pitch, float desired_yaw,
                        Vector3f& rot_accel);
 
-    void _attitude_control(const struct sitl_input& input,
+    void _attitude_control(const struct sitl_input &input,
                            Vector3f& rot_accel);
 
-    void _angular_velocity_control(const struct sitl_input& input,
+    void _angular_velocity_control(const struct sitl_input &input,
                                    Vector3f& rot_accel);
 
     void _calibration_poses(Vector3f& rot_accel);

libraries/SITL/SIM_Frame.cpp

@@ -202,7 +202,7 @@ Frame *Frame::find_frame(const char *name)
 
 // calculate rotational and linear accelerations
 void Frame::calculate_forces(const Aircraft &aircraft,
-                             const Aircraft::sitl_input &input,
+                             const struct sitl_input &input,
                              Vector3f &rot_accel,
                              Vector3f &body_accel)
 {

libraries/SITL/SIM_Frame.h

@@ -48,7 +48,7 @@ public:
 
     // calculate rotational and linear accelerations
     void calculate_forces(const Aircraft &aircraft,
-                          const Aircraft::sitl_input &input,
+                          const struct sitl_input &input,
                           Vector3f &rot_accel, Vector3f &body_accel);
     
     float terminal_velocity;

libraries/SITL/SIM_Gripper_EPM.cpp

@@ -17,42 +17,65 @@
 */
 
 #include "SIM_Gripper_EPM.h"
+#include "AP_HAL/AP_HAL.h"
 #include <stdio.h>
-#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
 
 using namespace SITL;
 
+// table of user settable parameters
+const AP_Param::GroupInfo Gripper_EPM::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Gripper servo Sim enable/disable
+    // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 0, Gripper_EPM, gripper_emp_enable, 0),
+
+    // @Param: PIN
+    // @DisplayName: Gripper emp pin
+    // @Description: The pin number that the gripper emp is connected to. (start at 1)
+    // @Range: 0 15
+    // @User: Advanced
+    AP_GROUPINFO("PIN", 1, Gripper_EPM, gripper_emp_servo_pin, -1),
+
+    AP_GROUPEND
+};
+
 /*
   update gripper state
  */
-void Gripper_EPM::update_servobased(const Aircraft::sitl_input &input)
+void Gripper_EPM::update_servobased(int16_t gripper_pwm)
 {
-    if (! servo_based) {
+    if (!servo_based) {
         return;
     }
-    demand = (input.servos[gripper_servo]-1000) * 0.001f; // 0.0 - 1.0
+    if (gripper_pwm >= 0) {
-    if (demand < 0) { // never updated
+        demand = (gripper_pwm - 1000) * 0.001f; // 0.0 - 1.0
-        demand = 0;
+        if (is_negative(demand)) { // never updated
+            demand = 0.0f;
+        }
     }
 }
 
 
-void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input)
+void Gripper_EPM::update_from_demand()
 {
     const uint64_t now = AP_HAL::micros64();
     const float dt = (now - last_update_us) * 1.0e-6f;
 
     // decay the field
-    field_strength = field_strength * (100-field_decay_rate * dt)/100.0f;
+    field_strength = field_strength * (100.0f - field_decay_rate * dt) / 100.0f;
 
     // note that "demand" here is just an on/off switch; we only care
     // about which range it falls into
-    if (demand > 0.6) {
+    if (demand > 0.6f) {
         // we are instructed to grip harder
-        field_strength = field_strength + (100.0f-field_strength) * field_strength_slew_rate/100.0f * dt;
+        field_strength = field_strength + (100.0f - field_strength) * field_strength_slew_rate / 100.0f * dt;
-    } else if (demand < 0.4) {
+    } else if (demand < 0.4f) {
         // we are instructed to loosen grip
-        field_strength = field_strength * (100-field_degauss_rate * dt)/100.0f;
+        field_strength = field_strength * (100.0f - field_degauss_rate * dt) / 100.0f;
     } else {
         // neutral; no demanded change
     }
@@ -66,14 +89,15 @@ void Gripper_EPM::update_from_demand(const Aircraft::sitl_input &input)
     }
 
     last_update_us = now;
-    return;
 }
 
-void Gripper_EPM::update(const Aircraft::sitl_input &input)
+void Gripper_EPM::update(const struct sitl_input &input)
 {
-    update_servobased(input);
+    const int16_t gripper_pwm = gripper_emp_servo_pin >= 1 ? input.servos[gripper_emp_servo_pin-1] : -1;
 
-    update_from_demand(input);
+    update_servobased(gripper_pwm);
+
+    update_from_demand();
 }
 
 
@@ -83,7 +107,7 @@ bool Gripper_EPM::should_report()
         return false;
     }
 
-    if (fabs(reported_field_strength - field_strength) > 10.0f) {
+    if (abs(reported_field_strength - field_strength) > 10.0) {
         return true;
     }
 
@@ -94,6 +118,6 @@ float Gripper_EPM::tesla()
 {
     // https://en.wikipedia.org/wiki/Orders_of_magnitude_(magnetic_field)
     // 200N lifting capacity ~= 2.5T
-    const float percentage_to_tesla = 0.25;
+    const float percentage_to_tesla = 0.25f;
-    return percentage_to_tesla * field_strength/100.0f;
+    return static_cast<float>(percentage_to_tesla * field_strength / 100.0f);
 }

libraries/SITL/SIM_Gripper_EPM.h

@@ -18,23 +18,31 @@
 
 #pragma once
 
-#include "SIM_Aircraft.h"
+#include "stdint.h"
+#include <AP_Param/AP_Param.h>
+#include "SITL_Input.h"
 
 namespace SITL {
 
 class Gripper_EPM {
 public:
-    const uint8_t gripper_servo;
+    Gripper_EPM() {
+        AP_Param::setup_object_defaults(this, var_info);
+    };
 
-    Gripper_EPM(const uint8_t _gripper_servo) :
+    float payload_mass() const { return 0.0f; } // kg
-        gripper_servo(_gripper_servo)
-    {}
 
     // update field stength
-    void update(const struct Aircraft::sitl_input &input);
+    void update(const struct sitl_input &input);
+
+    static const struct AP_Param::GroupInfo var_info[];
+    bool is_enabled() const {return static_cast<bool>(gripper_emp_enable);}
 
 private:
 
+    AP_Int8  gripper_emp_enable;  // enable gripper sim
+    AP_Int8  gripper_emp_servo_pin;
+
     const uint32_t report_interval = 100000; // microseconds
     uint64_t last_report_us;
 
@@ -52,8 +60,8 @@ private:
 
     bool should_report();
 
-    void update_from_demand(const Aircraft::sitl_input &input);
+    void update_from_demand();
-    void update_servobased(const struct Aircraft::sitl_input &input);
+    void update_servobased(int16_t gripper_pwm);
 
     float tesla();
 

libraries/SITL/SIM_Gripper_Servo.cpp

@@ -17,29 +17,57 @@
 */
 
 #include "SIM_Gripper_Servo.h"
+#include "AP_HAL/AP_HAL.h"
+#include "AP_Math/AP_Math.h"
 #include <stdio.h>
 
 using namespace SITL;
 
+// table of user settable parameters
+const AP_Param::GroupInfo Gripper_Servo::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Gripper servo Sim enable/disable
+    // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 0, Gripper_Servo, gripper_enable, 0),
+
+    // @Param: PIN
+    // @DisplayName: Gripper servo pin
+    // @Description: The pin number that the gripper servo is connected to. (start at 1)
+    // @Range: 0 15
+    // @User: Advanced
+    AP_GROUPINFO("PIN", 1, Gripper_Servo, gripper_servo_pin, -1),
+
+    AP_GROUPEND
+};
+
 /*
   update gripper state
  */
-void Gripper_Servo::update(const Aircraft::sitl_input &input)
+void Gripper_Servo::update(const struct sitl_input &input)
 {
+    const int16_t gripper_pwm = gripper_servo_pin >= 1 ? input.servos[gripper_servo_pin-1] : -1;
+
     const uint64_t now = AP_HAL::micros64();
     const float dt = (now - last_update_us) * 1.0e-6f;
 
     // update gripper position
-
+    if (gripper_pwm < 0) {
-    float position_demand = (input.servos[gripper_servo]-1000) * 0.001f;
+        last_update_us = now;
-    if (position_demand < 0) { // never updated
+        return;
-        position_demand = 0;
     }
 
-    const float position_max_change = position_slew_rate/100.0f * dt;
+    float position_demand = (gripper_pwm - 1000) * 0.001f;
-    position = constrain_float(position_demand, position-position_max_change, position+position_max_change);
+    if (is_negative(position_demand)) { // never updated
+        position_demand = 0.0f;
+    }
 
-    const float jaw_gap = gap*(1.0f-position);
+    const float position_max_change = position_slew_rate / 100.0f * dt;
+    position = constrain_float(position_demand, position - position_max_change, position + position_max_change);
+
+    const float jaw_gap = gap * (1.0f - position);
     if (should_report()) {
         ::fprintf(stderr, "position_demand=%f jaw_gap=%f load=%f\n", position_demand, jaw_gap, load_mass);
         last_report_us = now;
@@ -47,7 +75,7 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
     }
 
     if (jaw_gap < 5) {
-        if (aircraft->on_ground()) {
+        if (altitude <= 0.0f) {
             load_mass = 1.0f; // attach the load
         }
     } else if (jaw_gap > 10) {
@@ -55,7 +83,6 @@ void Gripper_Servo::update(const Aircraft::sitl_input &input)
     }
 
     last_update_us = now;
-    return;
 }
 
 bool Gripper_Servo::should_report()
@@ -74,7 +101,7 @@ bool Gripper_Servo::should_report()
 
 float Gripper_Servo::payload_mass() const
 {
-    if (aircraft->hagl() < string_length) {
+    if (altitude < string_length) {
         return 0.0f;
     }
     return load_mass;

libraries/SITL/SIM_Gripper_Servo.h

@@ -17,35 +17,35 @@
 */
 
 #pragma once
-
+#include "stdint.h"
-#include "SIM_Aircraft.h"
+#include <AP_Param/AP_Param.h>
+#include "SITL_Input.h"
 
 namespace SITL {
 
 class Gripper_Servo {
 public:
-    const uint8_t gripper_servo;
+    Gripper_Servo() {
-
+        AP_Param::setup_object_defaults(this, var_info);
-    Gripper_Servo(const uint8_t _gripper_servo) :
+    };
-        gripper_servo(_gripper_servo)
-    {}
 
     // update Gripper state
-    void update(const struct Aircraft::sitl_input &input);
+    void update(const struct sitl_input &input);
 
     float payload_mass() const; // kg
 
-    void set_aircraft(Aircraft *_aircraft) { aircraft = _aircraft; }
+    void set_alt(float alt) {altitude = alt;};
+    static const struct AP_Param::GroupInfo var_info[];
+    bool is_enabled() const {return static_cast<bool>(gripper_enable);}
 
 private:
-
+    AP_Int8  gripper_enable;  // enable gripper sim
-    Aircraft *aircraft;
+    AP_Int8  gripper_servo_pin;
-
     const uint32_t report_interval = 1000000; // microseconds
     uint64_t last_report_us;
 
     const float gap = 30; // mm
-
+    float altitude;
     float position; // percentage
     float position_slew_rate = 35; // percentage
     float reported_position = -1; // unlikely

libraries/SITL/SIM_ICEngine.cpp

@@ -24,7 +24,7 @@ using namespace SITL;
 /*
   update engine state, returning power output from 0 to 1
  */
-float ICEngine::update(const Aircraft::sitl_input &input)
+float ICEngine::update(const struct sitl_input &input)
 {
     bool have_ignition = ignition_servo>=0;
     bool have_choke = choke_servo>=0;

libraries/SITL/SIM_ICEngine.h

@@ -39,7 +39,7 @@ public:
     {}
 
     // update motor state
-    float update(const struct Aircraft::sitl_input &input);
+    float update(const struct sitl_input &input);
 
 private:
     float last_output;

libraries/SITL/SIM_Motor.cpp

@@ -22,7 +22,7 @@
 using namespace SITL;
 
 // calculate rotational accel and thrust for a motor
-void Motor::calculate_forces(const Aircraft::sitl_input &input,
+void Motor::calculate_forces(const struct sitl_input &input,
                              const float thrust_scale,
                              uint8_t motor_offset,
                              Vector3f &rot_accel,

libraries/SITL/SIM_Motor.h

@@ -69,7 +69,7 @@ public:
         pitch_max(_pitch_max)
     {}
 
-    void calculate_forces(const Aircraft::sitl_input &input,
+    void calculate_forces(const struct sitl_input &input,
                           float thrust_scale,
                           uint8_t motor_offset,
                           Vector3f &rot_accel, // rad/sec

libraries/SITL/SIM_Multicopter.cpp

@@ -29,8 +29,6 @@ MultiCopter::MultiCopter(const char *home_str, const char *frame_str) :
 {
     mass = 1.5f;
 
-    gripper.set_aircraft(this);
-
     frame = Frame::find_frame(frame_str);
     if (frame == nullptr) {
         printf("Frame '%s' not found", frame_str);
@@ -66,6 +64,7 @@ void MultiCopter::update(const struct sitl_input &input)
     calculate_forces(input, rot_accel, accel_body);
 
     update_dynamics(rot_accel);
+    update_external_payload(input);
 
     // update lat/lon/altitude
     update_position();
@@ -73,16 +72,4 @@ void MultiCopter::update(const struct sitl_input &input)
 
     // update magnetic field
     update_mag_field_bf();
-
-    // update sprayer
-    sprayer.update(input);
-
-    // update gripper
-    gripper.update(input);
-    gripper_epm.update(input);
-}
-
-float MultiCopter::gross_mass() const
-{
-    return Aircraft::gross_mass() + sprayer.payload_mass() + gripper.payload_mass();
 }

libraries/SITL/SIM_Multicopter.h

@@ -22,10 +22,6 @@
 #include "SIM_Motor.h"
 #include "SIM_Frame.h"
 
-#include "SIM_Sprayer.h"
-#include "SIM_Gripper_Servo.h"
-#include "SIM_Gripper_EPM.h"
-
 namespace SITL {
 
 /*
@@ -47,14 +43,6 @@ protected:
     // calculate rotational and linear accelerations
     void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
     Frame *frame;
-
-    // The numbers below are the pwm output channels with "0" meaning the first output (aka RC1)
-    Sprayer sprayer{6, 7};
-    Gripper_Servo gripper{8};
-    Gripper_EPM gripper_epm{9};
-
-    float gross_mass() const override;
-
 };
 
 }

libraries/SITL/SIM_Sprayer.cpp

@@ -17,56 +17,89 @@
 */
 
 #include "SIM_Sprayer.h"
-#include <stdio.h>
+#include "AP_HAL/AP_HAL.h"
+#include "AP_Math/AP_Math.h"
 
 using namespace SITL;
 
+// table of user settable parameters
+const AP_Param::GroupInfo Sprayer::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Gripper servo Sim enable/disable
+    // @Description: Allows you to enable (1) or disable (0) the gripper servo simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 0, Sprayer, sprayer_enable, 0),
+
+    // @Param: PUMP
+    // @DisplayName: Sprayer pump pin
+    // @Description: The pin number that the Sprayer pump is connected to. (start at 1)
+    // @Range: 0 15
+    // @User: Advanced
+    AP_GROUPINFO("PUMP", 1, Sprayer, sprayer_pump_pin, -1),
+
+    // @Param: SPIN
+    // @DisplayName: Sprayer spinner servo pin
+    // @Description: The pin number that the Sprayer spinner servo is connected to. (start at 1)
+    // @Range: 0 15
+    // @User: Advanced
+    AP_GROUPINFO("SPIN", 2, Sprayer, sprayer_spin_pin, -1),
+
+    AP_GROUPEND
+};
+
 /*
   update sprayer state
  */
-void Sprayer::update(const Aircraft::sitl_input &input)
+void Sprayer::update(const struct sitl_input &input)
 {
+    const int16_t pump_pwm = sprayer_pump_pin >= 1 ? input.servos[sprayer_pump_pin-1] : -1;
+    const int16_t spinner_pwm = sprayer_spin_pin >= 1 ? input.servos[sprayer_spin_pin-1] : -1;
     const uint64_t now = AP_HAL::micros64();
     const float dt = (now - last_update_us) * 1.0e-6f;
-
+    if (pump_pwm >= 0) {
-    // update remaining payload
+        // update remaining payload
-    if (capacity > 0) {
+        if (capacity > 0) {
-        const double delta = last_pump_output * pump_max_rate * dt;
+            const double delta = last_pump_output * pump_max_rate * dt;
-        capacity -= delta;
+            capacity -= delta;
-        if (capacity < 0) {
+            if (capacity < 0) {
-            capacity = 0.0f;
+                capacity = 0.0f;
+            }
         }
-    }
 
-    // update pump
+        // update pump
-    float pump_demand = (input.servos[pump_servo]-1000) * 0.001f;
+        float pump_demand = (pump_pwm - 1000) * 0.001f;
-    // ::fprintf(stderr, "pump_demand=%f\n", pump_demand);
+        // ::fprintf(stderr, "pump_demand=%f\n", pump_demand);
-    if (pump_demand < 0) { // never updated
+        if (pump_demand < 0) { // never updated
-        pump_demand = 0;
+            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);
+    } else {
+        last_pump_output = 0.0f;
     }
-    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) {
+    if (spinner_pwm >= 0) {
-        const float spinner_demand = (input.servos[spinner_servo]-1000) * 0.001f;
+        const float spinner_demand = (spinner_pwm - 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(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) {
+        if (spinner_pwm >= 0) {
-            printf("Spinner: %f rev/s\n", (last_spinner_output * spinner_max_rate)/360.0f);
+            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()

libraries/SITL/SIM_Sprayer.h

@@ -18,36 +18,41 @@
 
 #pragma once
 
-#include "SIM_Aircraft.h"
+#include "stdint.h"
+#include <AP_Param/AP_Param.h>
+#include "SITL_Input.h"
 
 namespace SITL {
 
 class Sprayer {
 public:
-    const uint8_t pump_servo;
+    Sprayer() {
-    const int8_t spinner_servo;
+        AP_Param::setup_object_defaults(this, var_info);
-
+    };
-    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);
+    void update(const struct sitl_input &input);
 
-    float payload_mass() const { return capacity; }; // kg; water, so kg=l
+    float payload_mass() const { return static_cast<float>(capacity); }; // kg; water, so kg=l
 
-private:
+    static const struct AP_Param::GroupInfo var_info[];
+    bool is_enabled() const {return static_cast<bool>(sprayer_enable);}
+
+ private:
+
+    AP_Int8  sprayer_enable;  // enable sprayer sim
+    AP_Int8  sprayer_pump_pin;
+    AP_Int8  sprayer_spin_pin;
 
     const uint32_t report_interval = 1000000; // microseconds
     uint64_t last_report_us;
 
-    const float pump_max_rate = 0.01; // litres/second
+    const float pump_max_rate = 0.01f; // litres/second
-    const float pump_slew_rate = 20; // percent/scond
+    const float pump_slew_rate = 20.0f; // percent/second
     float last_pump_output; // percentage
 
-    const float spinner_max_rate = 3600; // degrees/second
+    const float spinner_max_rate = 3600.0f; // degrees/second
-    const float spinner_slew_rate = 20; // percent/second
+    const float spinner_slew_rate = 20.0f; // percent/second
     float last_spinner_output; // percentage
 
     double capacity = 0.25; // litres

libraries/SITL/SITL.cpp

@@ -122,6 +122,16 @@ const AP_Param::GroupInfo SITL::var_info2[] = {
     AP_GROUPINFO("MAG_ODI",     19, SITL,  mag_offdiag, 0),
     AP_GROUPINFO("MAG_ORIENT",  20, SITL,  mag_orient, 0),
     AP_GROUPINFO("RC_CHANCOUNT",21, SITL,  rc_chancount, 16),
+    // @Group: SPR_
+    // @Path: ./SIM_Sprayer.cpp
+    AP_SUBGROUPINFO(sprayer_sim, "SPR_", 22, SITL, Sprayer),
+    // @Group: GRPS_
+    // @Path: ./SIM_Gripper_Servo.cpp
+    AP_SUBGROUPINFO(gripper_sim, "GRPS_", 23, SITL, Gripper_Servo),
+    // @Group: GRPE_
+    // @Path: ./SIM_Gripper_EPM.cpp
+    AP_SUBGROUPINFO(gripper_epm_sim, "GRPE_", 24, SITL, Gripper_EPM),
+
     AP_GROUPEND
 };
     

libraries/SITL/SITL.h

@@ -3,6 +3,10 @@
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 
+#include "SIM_Sprayer.h"
+#include "SIM_Gripper_Servo.h"
+#include "SIM_Gripper_EPM.h"
+
 class DataFlash_Class;
 
 namespace SITL {
@@ -144,7 +148,7 @@ public:
     AP_Int16 pin_mask; // for GPIO emulation
     AP_Float speedup; // simulation speedup
     AP_Int8  odom_enable; // enable visual odomotry data
-    
+
     // wind control
     enum WindType {
         WIND_TYPE_SQRT = 0,
@@ -206,6 +210,11 @@ public:
 
     // convert a set of roll rates from body frame to earth frame
     static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
+
+    Sprayer sprayer_sim;
+
+    Gripper_Servo gripper_sim;
+    Gripper_EPM gripper_epm_sim;
 };
 
 } // namespace SITL

libraries/SITL/SITL_Input.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <stdint.h>
+
+/*
+  structure passed in giving servo positions as PWM values in
+  microseconds
+*/
+struct sitl_input {
+    uint16_t servos[16];
+    struct {
+        float speed;      // m/s
+        float direction;  // degrees 0..360
+        float turbulence;
+        float dir_z;	  //degrees -90..90
+    } wind;
+};
+