AP_HAL_SITL: Add current and voltage monitoring implementation for Sub

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;

libraries/AP_HAL_SITL/SITL_State.h

@@ -36,7 +36,8 @@ public:
     enum vehicle_type {
         ArduCopter,
         APMrover2,
-        ArduPlane
+        ArduPlane,
+        ArduSub
     };
 
     int gps_pipe(void);

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) {