From b751f836bb27b46a8866a52e43d1a730fb47655f Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 23 Nov 2016 11:50:01 +1100
Subject: [PATCH] SITL: attach a string with a mass on the end to servo gripper

---
 libraries/SITL/SIM_Aircraft.h        |  2 ++
 libraries/SITL/SIM_Gripper_Servo.cpp |  8 ++++++++
 libraries/SITL/SIM_Gripper_Servo.h   | 10 ++++++++++
 libraries/SITL/SIM_Multicopter.cpp   |  4 +++-
 4 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index 96a9060233..a34fc83172 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -29,6 +29,8 @@ namespace SITL {
   parent class for all simulator types
  */
 class Aircraft {
+    friend class Gripper_Servo;
+
 public:
     Aircraft(const char *home_str, const char *frame_str);
 
diff --git a/libraries/SITL/SIM_Gripper_Servo.cpp b/libraries/SITL/SIM_Gripper_Servo.cpp
index 1227f9b2fa..b2e4d27ef3 100644
--- a/libraries/SITL/SIM_Gripper_Servo.cpp
+++ b/libraries/SITL/SIM_Gripper_Servo.cpp
@@ -63,3 +63,11 @@ bool Gripper_Servo::should_report()
     return false;
 }
 
+
+float Gripper_Servo::payload_mass() const
+{
+    if (aircraft->hagl() < string_length) {
+        return 0.0f;
+    }
+    return load_mass;
+}
diff --git a/libraries/SITL/SIM_Gripper_Servo.h b/libraries/SITL/SIM_Gripper_Servo.h
index 713af8cc7e..dd640be2af 100644
--- a/libraries/SITL/SIM_Gripper_Servo.h
+++ b/libraries/SITL/SIM_Gripper_Servo.h
@@ -33,8 +33,14 @@ public:
     // update Gripper state
     void update(const struct Aircraft::sitl_input &input);
 
+    float payload_mass() const; // kg
+
+    void set_aircraft(Aircraft *_aircraft) { aircraft = _aircraft; }
+
 private:
 
+    Aircraft *aircraft;
+
     const uint32_t report_interval = 1000000; // microseconds
     uint64_t last_report_us;
 
@@ -48,6 +54,10 @@ private:
 
     bool should_report();
     bool zero_report_done = false;
+
+    // dangle load from a string:
+    const float string_length = 2.0f; // metres
+    const float load_mass = 0.0f; // kilograms
 };
 
 }
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index f1ab2687d3..0b00f2e2bd 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -29,6 +29,8 @@ 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);
@@ -81,5 +83,5 @@ void MultiCopter::update(const struct sitl_input &input)
 
 float MultiCopter::gross_mass() const
 {
-    return Aircraft::gross_mass() + sprayer.payload_mass();
+    return Aircraft::gross_mass() + sprayer.payload_mass() + gripper.payload_mass();
 }