diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 05f8c6952a..670f8ba4eb 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -31,6 +31,7 @@ #include "SIM_RichenPower.h" #include "SIM_I2C.h" #include "SIM_Buzzer.h" +#include "SIM_Battery.h" #include namespace SITL { @@ -170,6 +171,10 @@ protected: float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube float battery_voltage = -1.0f; float battery_current; + + // battery model + Battery battery; + uint8_t num_motors = 1; uint8_t vtol_motor_start; float rpm[12]; diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp new file mode 100644 index 0000000000..34ec483513 --- /dev/null +++ b/libraries/SITL/SIM_Battery.cpp @@ -0,0 +1,158 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + battery model for electric aircraft +*/ + +#include "SIM_Battery.h" + +using namespace SITL; + +/* + state of charge table for a single cell battery. + */ +static const struct { + float volt_per_cell; + float soc_pct; +} soc_table[] = { + { 4.173, 100 }, + { 4.112, 96.15 }, + { 4.085, 92.31 }, + { 4.071, 88.46 }, + { 4.039, 84.62 }, + { 3.987, 80.77 }, + { 3.943, 76.92 }, + { 3.908, 73.08 }, + { 3.887, 69.23 }, + { 3.854, 65.38 }, + { 3.833, 61.54 }, + { 3.801, 57.69 }, + { 3.783, 53.85 }, + { 3.742, 50 }, + { 3.715, 46.15 }, + { 3.679, 42.31 }, + { 3.636, 38.46 }, + { 3.588, 34.62 }, + { 3.543, 30.77 }, + { 3.503, 26.92 }, + { 3.462, 23.08 }, + { 3.379, 19.23 }, + { 3.296, 15.38 }, + { 3.218, 11.54 }, + { 3.165, 7.69 }, + { 3.091, 3.85 }, + { 2.977, 2.0 }, + { 2.8, 1.5 }, + { 2.7, 1.3 }, + { 2.5, 1.2 }, + { 2.3, 1.1 }, + { 2.1, 1.0 }, + { 1.9, 0.9 }, + { 1.6, 0.8 }, + { 1.3, 0.7 }, + { 1.0, 0.6 }, + { 0.6, 0.4 }, + { 0.3, 0.2 }, + { 0.01, 0.01}, + { 0.001, 0.001 }}; + +/* + use table to get resting voltage from remaining capacity + */ +float Battery::get_resting_voltage(float charge_pct) +{ + const float max_cell_voltage = soc_table[0].volt_per_cell; + for (uint8_t i=1; i= soc_table[i].soc_pct) { + // linear interpolation between table rows + float dv1 = charge_pct - soc_table[i].soc_pct; + float dv2 = soc_table[i-1].soc_pct - soc_table[i].soc_pct; + float vpc1 = soc_table[i].volt_per_cell; + float vpc2 = soc_table[i-1].volt_per_cell; + float cell_volt = vpc1 + (dv1 / dv2) * (vpc2 - vpc1); + return (cell_volt / max_cell_voltage) * max_voltage; + } + } + // off the bottom of the table, return a small non-zero to prevent math errors + return 0.001; +} + +/* + use table to set initial state of charge from voltage + */ +void Battery::set_initial_SoC(float voltage) +{ + const float max_cell_voltage = soc_table[0].volt_per_cell; + float cell_volt = (voltage / max_voltage) * max_cell_voltage; + + for (uint8_t i=1; i= soc_table[i].volt_per_cell) { + // linear interpolation between table rows + float dv1 = cell_volt - soc_table[i].volt_per_cell; + float dv2 = soc_table[i-1].volt_per_cell - soc_table[i].volt_per_cell; + float soc1 = soc_table[i].soc_pct; + float soc2 = soc_table[i-1].soc_pct; + float soc = soc1 + (dv1 / dv2) * (soc2 - soc1); + remaining_Ah = capacity_Ah * soc * 0.01; + return; + } + } + + // off the bottom of the table + remaining_Ah = 0; +} + +void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage) +{ + capacity_Ah = _capacity_Ah; + resistance = _resistance; + max_voltage = _max_voltage; +} + +void Battery::init_voltage(float voltage) +{ + voltage_filter.reset(voltage); + voltage_set = voltage; + set_initial_SoC(voltage); +} + +void Battery::set_current(float current) +{ + uint64_t now = AP_HAL::micros64(); + float dt = (now - last_us) * 1.0e-6; + if (dt > 0.1) { + // we stopped updating + dt = 0; + } + last_us = now; + float delta_Ah = current * dt / 3600; + remaining_Ah -= delta_Ah; + remaining_Ah = MAX(0, remaining_Ah); + + float voltage_delta = current * resistance; + float voltage; + if (!is_positive(capacity_Ah)) { + voltage = voltage_set; + } else { + voltage = get_resting_voltage(100 * remaining_Ah / capacity_Ah) - voltage_delta; + } + + voltage_filter.apply(voltage); +} + +float Battery::get_voltage(void) const +{ + return voltage_filter.get(); +} diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h new file mode 100644 index 0000000000..0abf6ffc33 --- /dev/null +++ b/libraries/SITL/SIM_Battery.h @@ -0,0 +1,51 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + battery model for electric aircraft +*/ + +#pragma once + +#include + +namespace SITL { + +/* + class to describe a motor position + */ +class Battery { +public: + void setup(float _capacity_Ah, float _resistance, float _max_voltage); + + void init_voltage(float voltage); + + void set_current(float current_amps); + float get_voltage(void) const; + +private: + float capacity_Ah; + float resistance; + float max_voltage; + float voltage_set; + float remaining_Ah; + uint64_t last_us; + + // 10Hz filter for battery voltage + LowPassFilterFloat voltage_filter{10}; + + float get_resting_voltage(float charge_pct); + void set_initial_SoC(float voltage); +}; +} diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index 8764eb8133..3f612b2ecb 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -362,6 +362,7 @@ void Frame::load_frame_params(const char *model_json) FRAME_VAR(refAlt), FRAME_VAR(refTempC), FRAME_VAR(maxVoltage), + FRAME_VAR(battCapacityAh), FRAME_VAR(refBatRes), FRAME_VAR(propExpo), FRAME_VAR(refRotRate), @@ -392,9 +393,10 @@ void Frame::load_frame_params(const char *model_json) /* initialise the frame */ -void Frame::init(const char *frame_str) +void Frame::init(const char *frame_str, Battery *_battery) { model = default_model; + battery = _battery; const char *colon = strchr(frame_str, ':'); size_t slen = strlen(frame_str); @@ -422,9 +424,8 @@ void Frame::init(const char *frame_str) // power_factor is ratio of power consumed per newton of thrust float power_factor = hover_power / hover_thrust; - // init voltage - voltage_filter.reset(AP::sitl()->batt_voltage); - + battery->setup(model.battCapacityAh, model.refBatRes, model.maxVoltage); + for (uint8_t i=0; iget_voltage()); ::printf("pwm[%u] cmd=%.3f thrust=%.3f hovthst=%.3f\n", pwm, motors[0].pwm_to_command(pwm), -thrust.z*num_motors, hover_thrust); } @@ -483,7 +484,7 @@ void Frame::calculate_forces(const Aircraft &aircraft, for (uint8_t i=0; iget_voltage()); current += motors[i].get_current(); rot_accel += mraccel; thrust += mthrust; @@ -515,9 +516,6 @@ void Frame::calculate_forces(const Aircraft &aircraft, body_accel += aircraft.get_dcm().transposed() * drag_ef / mass; } - float voltage_drop = current * model.refBatRes; - voltage_filter.apply(AP::sitl()->batt_voltage - voltage_drop); - // add some noise const float gyro_noise = radians(0.1); const float accel_noise = 0.3; @@ -534,7 +532,12 @@ void Frame::calculate_forces(const Aircraft &aircraft, // calculate current and voltage void Frame::current_and_voltage(float &voltage, float ¤t) { - voltage = voltage_filter.get(); + float param_voltage = AP::sitl()->batt_voltage; + if (!is_equal(last_param_voltage,param_voltage)) { + battery->init_voltage(param_voltage); + last_param_voltage = param_voltage; + } + voltage = battery->get_voltage(); current = 0; for (uint8_t i=0; i namespace SITL { @@ -36,16 +35,16 @@ public: Frame(const char *_name, uint8_t _num_motors, Motor *_motors) : - name(_name), - num_motors(_num_motors), - motors(_motors) {} + name(_name), + num_motors(_num_motors), + motors(_motors) {} // find a frame by name static Frame *find_frame(const char *name); // initialise frame - void init(const char *frame_str); + void init(const char *frame_str, Battery *_battery); // calculate rotational and linear accelerations void calculate_forces(const Aircraft &aircraft, @@ -96,6 +95,9 @@ private: // full pack voltage float maxVoltage = 4.2*3; + // battery capacity in Ah. Use zero for unlimited + float battCapacityAh = 0.0; + // CTUN.ThO at bover at refAlt float hoverThrOut = 0.39; @@ -127,9 +129,8 @@ private: float velocity_max; float thrust_max; float effective_prop_area; - - // 10Hz filter for battery voltage - LowPassFilterFloat voltage_filter{10}; + Battery *battery; + float last_param_voltage; // get air density in kg/m^3 float get_air_density(float alt_amsl) const; diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index 83a2fbe431..4c977fbf74 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -39,6 +39,14 @@ void Motor::calculate_forces(const struct sitl_input &input, float command = pwm_to_command(pwm); float voltage_scale = voltage / voltage_max; + if (voltage_scale < 0.1) { + // battery is dead + rot_accel.zero(); + thrust.zero(); + current = 0; + return; + } + // apply slew limiter to command uint64_t now_us = AP_HAL::micros64(); if (last_calc_us != 0 && slew_max > 0) { @@ -56,7 +64,7 @@ void Motor::calculate_forces(const struct sitl_input &input, float velocity_in = MAX(0, -velocity_air_bf.z); // get thrust for untilted motor - float motor_thrust = calc_thrust(command * voltage_scale, air_density, effective_prop_area, velocity_in, velocity_max); + float motor_thrust = calc_thrust(command, air_density, effective_prop_area, velocity_in, velocity_max * voltage_scale); // thrust in NED thrust = {0, 0, -motor_thrust}; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 25369398b0..ae187bac55 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -32,7 +32,7 @@ MultiCopter::MultiCopter(const char *frame_str) : exit(1); } - frame->init(frame_str); + frame->init(frame_str, &battery); mass = frame->get_mass(); frame_height = 0.1; @@ -64,6 +64,8 @@ void MultiCopter::update(const struct sitl_input &input) // estimate voltage and current frame->current_and_voltage(battery_voltage, battery_current); + battery.set_current(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 4dac0ef89a..0b99c52995 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -88,7 +88,7 @@ QuadPlane::QuadPlane(const char *frame_str) : frame->motor_offset = motor_offset; // we use zero terminal velocity to let the plane model handle the drag - frame->init(frame_str); + frame->init(frame_str, &battery); // increase mass for plane components mass = frame->get_mass() * 1.5; @@ -118,6 +118,8 @@ void QuadPlane::update(const struct sitl_input &input) // estimate voltage and current frame->current_and_voltage(battery_voltage, battery_current); + battery.set_current(battery_current); + float throttle; if (reverse_thrust) { throttle = filtered_servo_angle(input, 2); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index de974236df..d5e0fb9c7f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -46,6 +46,7 @@ const AP_Param::GroupInfo SITL::var_info[] = { AP_GROUPINFO("WIND_TURB", 11, SITL, wind_turbulance, 0), AP_GROUPINFO("SERVO_SPEED", 16, SITL, servo_speed, 0.14), AP_GROUPINFO("BATT_VOLTAGE", 19, SITL, batt_voltage, 12.6f), + AP_GROUPINFO("BATT_CAP_AH", 20, SITL, batt_capacity_ah, 0), AP_GROUPINFO("ACCEL_FAIL", 21, SITL, accel_fail, 0), AP_GROUPINFO("SONAR_GLITCH", 23, SITL, sonar_glitch, 0), AP_GROUPINFO("SONAR_RND", 24, SITL, sonar_noise, 0), diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 1fd2d763f6..3992bad51d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -55,6 +55,7 @@ struct sitl_fdm { double airspeed; // m/s double battery_voltage; // Volts double battery_current; // Amps + double battery_remaining; // Ah, if non-zero capacity uint8_t num_motors; uint8_t vtol_motor_start; float rpm[12]; // RPM of all motors @@ -200,6 +201,7 @@ public: AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D) AP_Float batt_voltage; // battery voltage base + AP_Float batt_capacity_ah; // battery capacity in Ah AP_Float accel_fail; // accelerometer failure value AP_Int8 rc_fail; // fail RC input AP_Int8 rc_chancount; // channel count