AP_HAL_SITL: record throttle value instead of motors on/off

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -640,7 +640,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
 
     float engine_mul = _sitl?_sitl->engine_mul.get():1;
     uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0;
-    bool motors_on = false;
+    float throttle = 0.0f;
     
     if (engine_fail >= ARRAY_SIZE(input.servos)) {
         engine_fail = 0;
@@ -651,28 +651,30 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
     } else {
         input.servos[engine_fail] = static_cast<uint16_t>(((input.servos[engine_fail] - 1500) * engine_mul) + 1500);
     }
-    
+
     if (_vehicle == ArduPlane) {
-        motors_on = ((input.servos[2] - 1000) / 1000.0f) > 0;
+        throttle = constrain_float((input.servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
     } else if (_vehicle == APMrover2) {
         input.servos[2] = static_cast<uint16_t>(constrain_int16(input.servos[2], 1000, 2000));
         input.servos[0] = static_cast<uint16_t>(constrain_int16(input.servos[0], 1000, 2000));
-        motors_on = !is_zero(((input.servos[2] - 1500) / 500.0f));
+        throttle = fabsf((input.servos[2] - 1500) / 500.0f);
     } else {
-        motors_on = false;
         // run checks on each motor
-        for (i=0; i<4; i++) {
+        uint8_t running_motors = 0;
-            // check motors do not exceed their limits
+        for (i=0; i<SITL_NUM_CHANNELS; i++) {
-            if (input.servos[i] > 2000) input.servos[i] = 2000;
+            float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f);
-            if (input.servos[i] < 1000) input.servos[i] = 1000;
             // update motor_on flag
-            if ((input.servos[i]-1000)/1000.0f > 0) {
+            if (!is_zero(motor_throttle)) {
-                motors_on = true;
+                throttle += motor_throttle;
+                running_motors++;
             }
         }
+        if (running_motors > 0) {
+            throttle /= running_motors;
+        }
     }
     if (_sitl) {
-        _sitl->motors_on = motors_on;
+        _sitl->throttle = throttle;
     }
 
     float voltage = 0;
@@ -694,17 +696,11 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
                 }
             } else {
                 // simulate simple battery setup
-                float throttle;
-                if (_vehicle == APMrover2) {
-                    throttle = motors_on ? (input.servos[2] - 1500) / 500.0f : 0;
-                } else {
-                    throttle = motors_on ? (input.servos[2] - 1000) / 1000.0f : 0;
-                }
                 // lose 0.7V at full throttle
-                voltage = _sitl->batt_voltage - 0.7f*fabsf(throttle);
+                voltage = _sitl->batt_voltage - 0.7f * throttle;
 
                 // assume 50A at full throttle
-                _current = 50.0f * fabsf(throttle);
+                _current = 50.0f * throttle;
             }
         } else {
             // FDM provides voltage and current