HAL_SITL: handle periph voltage/current and improve timing

libraries/AP_HAL_SITL/CANSocketIface.cpp

@@ -614,14 +614,15 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
         return true;
     }
 
-    while (duration_us > 0) {
+    const uint32_t start_us = AP_HAL::micros();
+    do {
         uint16_t wait_us = MIN(100, duration_us);
         // Timeout conversion
         auto ts = timespec();
         ts.tv_sec = 0;
         ts.tv_nsec = wait_us * 1000UL;
 
-        // Blocking here
+        // check FD for input
         const int res = ppoll(pollfds, num_pollfds, &ts, nullptr);
 
         if (res < 0) {
@@ -633,8 +634,7 @@ bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHan
         
         // ensure simulator runs
         hal.scheduler->delay_microseconds(wait_us);
-        duration_us -= wait_us;
+    } while (AP_HAL::micros() - start_us < duration_us);
-    }
 
 
     // Handling poll output

libraries/AP_HAL_SITL/SITL_Periph_State.cpp

@@ -119,6 +119,7 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
         struct sitl_input input {};
         sitl_model->update(input);
         sim_update();
+        update_voltage_current(input, 0);
         usleep(100);
     }
 }

libraries/AP_HAL_SITL/SITL_State.cpp

@@ -548,44 +548,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
         _sitl->throttle = throttle;
     }
 
-    float voltage = 0;
+    update_voltage_current(input, throttle);
-    _current = 0;
-    
-    if (_sitl != nullptr) {
-        if (_sitl->state.battery_voltage <= 0) {
-            if (_vehicle == ArduSub) {
-                voltage = _sitl->batt_voltage;
-                for (uint8_t 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
-                // lose 0.7V at full throttle
-                voltage = _sitl->batt_voltage - 0.7f * throttle;
-
-                // assume 50A at full throttle
-                _current = 50.0f * throttle;
-            }
-        } else {
-            // FDM provides voltage and current
-            voltage = _sitl->state.battery_voltage;
-            _current = _sitl->state.battery_current;
-        }
-    }
-
-    // assume 3DR power brick
-    voltage_pin_voltage = (voltage / 10.1f);
-    current_pin_voltage = _current/17.0f;
-    // fake battery2 as just a 25% gain on the first one
-    voltage2_pin_voltage = voltage_pin_voltage * .25f;
-    current2_pin_voltage = current_pin_voltage * .25f;
 }
 
 void SITL_State::init(int argc, char * const argv[])

libraries/AP_HAL_SITL/SITL_State.h

@@ -19,14 +19,6 @@ class HALSITL::SITL_State : public SITL_State_Common {
 public:
     void init(int argc, char * const argv[]);
 
-    enum vehicle_type {
-        ArduCopter,
-        Rover,
-        ArduPlane,
-        ArduSub,
-        Blimp
-    };
-
     uint16_t pwm_output[SITL_NUM_CHANNELS];
     uint16_t pwm_input[SITL_RC_INPUT_CHANNELS];
     bool output_ready = false;
@@ -90,7 +82,6 @@ private:
     void wait_clock(uint64_t wait_time_usec);
 
     // internal state
-    enum vehicle_type _vehicle;
     uint8_t _instance;
     uint16_t _base_port;
     pid_t _parent_pid;
@@ -102,7 +93,6 @@ private:
     uint16_t _rcin_port;
     uint16_t _fg_view_port;
     uint16_t _irlock_port;
-    float _current;
 
     bool _synthetic_clock_mode;
 

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -410,5 +410,50 @@ void SITL_State_Common::sim_update(void)
     }
 }
 
+/*
+  update voltage and current pins
+ */
+void SITL_State_Common::update_voltage_current(struct sitl_input &input, float throttle)
+{
+    float voltage = 0;
+    float current = 0;
+    
+    if (_sitl != nullptr) {
+        if (_sitl->state.battery_voltage <= 0) {
+            if (_vehicle == ArduSub) {
+                voltage = _sitl->batt_voltage;
+                for (uint8_t 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
+                // lose 0.7V at full throttle
+                voltage = _sitl->batt_voltage - 0.7f * throttle;
+
+                // assume 50A at full throttle
+                current = 50.0f * throttle;
+            }
+        } else {
+            // FDM provides voltage and current
+            voltage = _sitl->state.battery_voltage;
+            current = _sitl->state.battery_current;
+        }
+    }
+
+    // assume 3DR power brick
+    voltage_pin_voltage = (voltage / 10.1f);
+    current_pin_voltage = current/17.0f;
+    // fake battery2 as just a 25% gain on the first one
+    voltage2_pin_voltage = voltage_pin_voltage * .25f;
+    current2_pin_voltage = current_pin_voltage * .25f;
+}
+
 #endif // HAL_BOARD_SITL
 

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -75,6 +75,7 @@ public:
     void init(int argc, char * const argv[]);
 
     enum vehicle_type {
+        NONE,
         ArduCopter,
         Rover,
         ArduPlane,
@@ -217,12 +218,16 @@ public:
     void multicast_state_send(void);
 
 protected:
+    enum vehicle_type _vehicle;
+
     void sim_update(void);
 
     // internal SITL model
     SITL::Aircraft *sitl_model;
 
     SITL::SIM *_sitl;
+
+    void update_voltage_current(struct sitl_input &input, float throttle);
 };
 
 #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL