SITL: added simulated battery for quadplane

needed for motor interference for mag

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;
+}

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);
 };
 }

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;
+}

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);
 };
+
 }

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

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;