From e33d42e659b29c607b3c52e92adf6bb5cfe0872f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 13 Jan 2019 14:07:36 +1100
Subject: [PATCH] SITL: added simulated battery for quadplane

needed for motor interference for mag
---
 libraries/SITL/SIM_Frame.cpp     | 15 +++++++++++++++
 libraries/SITL/SIM_Frame.h       |  3 +++
 libraries/SITL/SIM_Motor.cpp     | 15 +++++++++++++++
 libraries/SITL/SIM_Motor.h       |  4 ++++
 libraries/SITL/SIM_QuadPlane.cpp | 13 +++++++++++++
 5 files changed, 50 insertions(+)

diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index 4034f3c7a5..607cb89420 100644
--- a/libraries/SITL/SIM_Frame.cpp
+++ b/libraries/SITL/SIM_Frame.cpp
@@ -243,3 +243,18 @@ void Frame::calculate_forces(const Aircraft &aircraft,
                            aircraft.rand_normal(0, 1)) * accel_noise * noise_scale;
 }
 
+
+// calculate current and voltage
+void Frame::current_and_voltage(const Aircraft::sitl_input &input, float &voltage, float &current)
+{
+    voltage = 0;
+    current = 0;
+    for (uint8_t i=0; i<num_motors; i++) {
+        float c, v;
+        motors[i].current_and_voltage(input, v, c, motor_offset);
+        current += c;
+        voltage += v;
+    }
+    // use average for voltage, total for current
+    voltage /= num_motors;
+}
diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h
index 5695633075..11c9f8efdc 100644
--- a/libraries/SITL/SIM_Frame.h
+++ b/libraries/SITL/SIM_Frame.h
@@ -55,5 +55,8 @@ public:
     float terminal_rotation_rate;
     float thrust_scale;
     uint8_t motor_offset;
+
+    // calculate current and voltage
+    void current_and_voltage(const Aircraft::sitl_input &input, float &voltage, float &current);
 };
 }
diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp
index c9326ba2f3..71cbb2dd36 100644
--- a/libraries/SITL/SIM_Motor.cpp
+++ b/libraries/SITL/SIM_Motor.cpp
@@ -108,3 +108,18 @@ uint16_t Motor::update_servo(uint16_t demand, uint64_t time_usec, float &last_va
     last_value = constrain_float(demand, last_value-max_change, last_value+max_change);
     return uint16_t(last_value+0.5);
 }
+
+
+// calculate current and voltage
+void Motor::current_and_voltage(const Aircraft::sitl_input &input, float &voltage, float &current,
+                                uint8_t motor_offset)
+{
+    // get motor speed from 0 to 1
+    float motor_speed = constrain_float((input.servos[motor_offset+servo]-1100)/900.0, 0, 1);
+
+    // assume 10A per motor at full speed
+    current = 10 * motor_speed;
+
+    // assume 3S, and full throttle drops voltage by 0.7V
+    voltage = 12.4 - motor_speed * 0.7;
+}
diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h
index b263123f48..3b5bc8b990 100644
--- a/libraries/SITL/SIM_Motor.h
+++ b/libraries/SITL/SIM_Motor.h
@@ -76,5 +76,9 @@ public:
                           Vector3f &body_thrust); // Z is down
 
     uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value);
+
+    // calculate current and voltage
+    void current_and_voltage(const Aircraft::sitl_input &input, float &voltage, float &current, uint8_t motor_offset);
 };
+
 }
diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp
index e6ae2d9a82..c3b4fd1998 100644
--- a/libraries/SITL/SIM_QuadPlane.cpp
+++ b/libraries/SITL/SIM_QuadPlane.cpp
@@ -109,6 +109,19 @@ void QuadPlane::update(const struct sitl_input &input)
 
     frame->calculate_forces(*this, input, quad_rot_accel, quad_accel_body);
 
+    // estimate voltage and current
+    frame->current_and_voltage(input, battery_voltage, battery_current);
+
+    float throttle;
+    if (reverse_thrust) {
+        throttle = filtered_servo_angle(input, 2);
+    } else {
+        throttle = filtered_servo_range(input, 2);
+    }
+    // assume 20A at full fwd throttle
+    throttle = fabsf(throttle);
+    battery_current += 20 * throttle;
+    
     rot_accel += quad_rot_accel;
     accel_body += quad_accel_body;