From 07fa65a88c403e828dcb9cd8c6c220539ea5e686 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Fri, 27 Oct 2017 13:24:53 -0400
Subject: [PATCH] AP_HAL_SITL: Add current and voltage monitoring
 implementation for Sub

---
 libraries/AP_HAL_SITL/SITL_State.cpp   | 28 +++++++++++++++++++-------
 libraries/AP_HAL_SITL/SITL_State.h     |  3 ++-
 libraries/AP_HAL_SITL/SITL_cmdline.cpp |  2 ++
 3 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 01e9236a0d..18519b54d3 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -410,13 +410,27 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
     
     if (_sitl != nullptr) {
         if (_sitl->state.battery_voltage <= 0) {
-            // simulate simple battery setup
-            float throttle = motors_on?(input.servos[2]-1000) / 1000.0f:0;
-            // lose 0.7V at full throttle
-            voltage = _sitl->batt_voltage - 0.7f*fabsf(throttle);
-            
-            // assume 50A at full throttle
-            _current = 50.0f * fabsf(throttle);
+            if (_vehicle == ArduSub) {
+                voltage = _sitl->batt_voltage;
+                for (i = 0; i < 6; i++) {
+                    float pwm = input.servos[i];
+                    //printf("i: %d, pwm: %.2f\n", i, pwm);
+                    float fraction = fabsf((pwm - 1500) / 500.0f);
+
+                    voltage -= fraction * 0.5f;
+
+                    float draw = fraction * 15;
+                    _current += draw;
+                }
+            } else {
+                // simulate simple battery setup
+                float throttle = motors_on?(input.servos[2]-1000) / 1000.0f:0;
+                // lose 0.7V at full throttle
+                voltage = _sitl->batt_voltage - 0.7f*fabsf(throttle);
+
+                // assume 50A at full throttle
+                _current = 50.0f * fabsf(throttle);
+            }
         } else {
             // FDM provides voltage and current
             voltage = _sitl->state.battery_voltage;
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index d9e4bf0bd7..13871bb160 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -36,7 +36,8 @@ public:
     enum vehicle_type {
         ArduCopter,
         APMrover2,
-        ArduPlane
+        ArduPlane,
+        ArduSub
     };
 
     int gps_pipe(void);
diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
index a99a37dbe4..e37835ab9d 100644
--- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp
+++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
@@ -372,6 +372,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
         }
         // set right default throttle for rover (allowing for reverse)
         pwm_input[2] = 1500;
+    } else if (strcmp(SKETCH, "ArduSub") == 0) {
+        _vehicle = ArduSub;
     } else {
         _vehicle = ArduPlane;
         if (_framerate == 0) {