From 358c570d9dfa31a609cb279dea0f831f96735012 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_Multicopter.cpp |  3 +++
 libraries/SITL/SIM_QuadPlane.cpp   | 13 +++++++++++++
 6 files changed, 53 insertions(+)

diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
index 083544d14c..bf38da1302 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 struct 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 be1b620e8f..f231df48e3 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 struct sitl_input &input, float &voltage, float &current);
 };
 }
diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp
index 0f7ea8cf97..38a85a1fd8 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 struct 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 = AP::sitl()->batt_voltage - motor_speed * 0.7;
+}
diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h
index 8bbf451a3a..47e629ee2f 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 struct sitl_input &input, float &voltage, float &current, uint8_t motor_offset);
 };
+
 }
diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp
index 875c8e08f4..877aa83216 100644
--- a/libraries/SITL/SIM_Multicopter.cpp
+++ b/libraries/SITL/SIM_Multicopter.cpp
@@ -63,6 +63,9 @@ void MultiCopter::update(const struct sitl_input &input)
 
     calculate_forces(input, rot_accel, accel_body);
 
+    // estimate voltage and current
+    frame->current_and_voltage(input, battery_voltage, battery_current);
+
     update_dynamics(rot_accel);
     update_external_payload(input);
 
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;