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 ¤t) +{ + 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 ¤t); }; } 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 ¤t, + 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 ¤t, 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;