diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 28be7e2baeebb..73a38bf5d69e5 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -50,8 +50,7 @@ Aircraft *Aircraft::instances[MAX_SIM_INSTANCES]; parent class for all simulator types */ -Aircraft::Aircraft(const char *frame_str) : - frame(frame_str) +Aircraft::Aircraft(const char *unused_frame_str) { // make the SIM_* variables available to simulator backends sitl = AP::sitl(); diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 13992dfb2b35b..fb3f355d1f88e 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -53,7 +53,7 @@ namespace SITL { */ class Aircraft { public: - Aircraft(const char *frame_str); + Aircraft(const char *unused_frame_str); // called directly after constructor: virtual void set_start_location(const Location &start_loc, const float start_yaw); @@ -174,7 +174,8 @@ class Aircraft { void set_dronecan_device(DroneCANDevice *_dronecan) { dronecan = _dronecan; } #endif float get_battery_voltage() const { return battery_voltage; } - float get_battery_temperature() const { return battery.get_temperature(); } + float get_battery_temperature() const { return battery_temperature; } + float get_battery_current() const { return battery_current; } float ambient_temperature_degC() const; @@ -222,12 +223,14 @@ class Aircraft { float airspeed_pitot; // m/s, EAS airspeed, as seen by fwd pitot tube float battery_voltage; float battery_current; + float battery_temperature; float local_ground_level; // ground level at local position bool lock_step_scheduled; bool flightaxis_sync_imus_to_frames; // causes the frame counter to be incremented on each timestep, IMUs will then update at the same rate uint32_t last_one_hz_ms; - // battery model + // OPTIONAL battery model + // ("OPTIONAL" because a child can ignore it and directly set/get battery_* members.) Battery battery; uint32_t motor_mask; @@ -274,7 +277,6 @@ class Aircraft { uint32_t last_frame_count; uint8_t instance; const char *autotest_dir; - const char *frame; bool use_time_sync = true; float last_speedup = -1.0f; const char *config_ = ""; diff --git a/libraries/SITL/SIM_Battery.cpp b/libraries/SITL/SIM_Battery.cpp index 7618745cf2796..7881aa2c97064 100644 --- a/libraries/SITL/SIM_Battery.cpp +++ b/libraries/SITL/SIM_Battery.cpp @@ -17,6 +17,8 @@ */ #include "SIM_Battery.h" +#include +#include using namespace SITL; @@ -71,9 +73,14 @@ static const struct { /* use table to get resting voltage from remaining capacity */ -float Battery::get_resting_voltage(float charge_pct) const +float Battery::get_resting_voltage(void) const { + if (capacity_is_unlimited()) { + return voltage_set; + } + float charge_pct = 100 * remaining_Ah / capacity_Ah; const float max_cell_voltage = soc_table[0].volt_per_cell; + const float min_cell_voltage = soc_table[ARRAY_SIZE(soc_table) - 1].volt_per_cell; for (uint8_t i=1; i= soc_table[i].soc_pct) { // linear interpolation between table rows @@ -85,15 +92,21 @@ float Battery::get_resting_voltage(float charge_pct) const 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; + // off the bottom of the table + return min_cell_voltage; } /* - use table to set initial state of charge from voltage + return remaining Amp-hours (aka "charge", "state of charge") corresponding to a voltage + + this is const for readability: the remaining_ah computation has no "side effects" */ -void Battery::set_initial_SoC(float voltage) +float Battery::compute_remaining_ah(float voltage) const { + if (capacity_is_unlimited()) { + return FLT_MAX; + } + const float max_cell_voltage = soc_table[0].volt_per_cell; float cell_volt = (voltage / max_voltage) * max_cell_voltage; @@ -105,36 +118,46 @@ void Battery::set_initial_SoC(float voltage) 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; + return capacity_Ah * soc * 0.01; } } - // off the bottom of the table - remaining_Ah = 0; + return 0.0f; } -void Battery::setup(float _capacity_Ah, float _resistance, float _max_voltage) +void Battery::set_remaining_ah(void) { - capacity_Ah = _capacity_Ah; - resistance = _resistance; - max_voltage = _max_voltage; + remaining_Ah = compute_remaining_ah(voltage_set); } -void Battery::init_voltage(float voltage) +// Reminder: capacity <= 0 means **unlimited** +void Battery::setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage) { - voltage_filter.reset(voltage); - voltage_set = voltage; - set_initial_SoC(voltage); + capacity_Ah = _capacity_Ah; + resistance_ohm = _resistance_ohm; + max_voltage = _max_voltage; + + voltage_set = max_voltage; + voltage_filter.reset(voltage_set); + set_remaining_ah(); } -void Battery::init_capacity(float capacity) +void Battery::maybe_reset(float desired_voltage, float desired_capacity_Ah) { - capacity_Ah = capacity; - set_initial_SoC(voltage_set); + const bool reset_not_needed = (is_equal(voltage_set, desired_voltage) + && is_equal(capacity_Ah, desired_capacity_Ah)); + if (reset_not_needed) { + return; + } + + capacity_Ah = desired_capacity_Ah; + // a negative desired voltage is unexpected, but not problematic + voltage_set = MIN(desired_voltage, max_voltage); + voltage_filter.reset(voltage_set); + set_remaining_ah(); } -void Battery::set_current(float current) +void Battery::consume_energy(float current_amps) { uint64_t now = AP_HAL::micros64(); float dt = (now - last_us) * 1.0e-6; @@ -143,31 +166,29 @@ void Battery::set_current(float current) dt = 0; } last_us = now; - float delta_Ah = current * dt / 3600; + float delta_Ah = current_amps * 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, dt); + float voltage_delta = current_amps * resistance_ohm; + float sagged_voltage = get_resting_voltage() - voltage_delta; + voltage_filter.apply(sagged_voltage, dt); - { - const uint64_t temperature_dt = now - temperature.last_update_micros; - temperature.last_update_micros = now; - // 1 amp*1 second == 0.1 degrees of energy. Did those units hurt? - temperature.kelvin += 0.1 * current * temperature_dt * 0.000001; - // decay temperature at some %second towards ambient - temperature.kelvin -= (temperature.kelvin - 273) * 0.10 * temperature_dt * 0.000001; - } + update_temperature(current_amps, now); } -float Battery::get_voltage(void) const +void Battery::update_temperature(float current_amps, uint64_t now) { - return voltage_filter.get(); + const float dt = (now - temperature.last_update_micros) * 1.0e-6; + temperature.last_update_micros = now; + + // temperature growth model + + // thermal_capacity value chosen to match previous steady-state behavior at 28amps + // (reminder: thermal_capacity = mass * specific_heat) + const float inverse_of_thermal_capacity = 0.36f; // use inverse so we can multiply, not divide + temperature.kelvin += (current_amps * current_amps) * resistance_ohm * dt * inverse_of_thermal_capacity; + + // temperature decay model: first-order + temperature.kelvin -= (temperature.kelvin - (ambient_temperature + 273.15f)) * 0.10f * dt; } diff --git a/libraries/SITL/SIM_Battery.h b/libraries/SITL/SIM_Battery.h index 46a686eaaf02a..bf0a46a9412d3 100644 --- a/libraries/SITL/SIM_Battery.h +++ b/libraries/SITL/SIM_Battery.h @@ -22,37 +22,45 @@ namespace SITL { +constexpr float ambient_temperature = 0.0f; // degC + class Battery { public: - void setup(float _capacity_Ah, float _resistance, float _max_voltage); + // Note: capacity<=0 means **unlimited** capacity. + void setup(float _capacity_Ah, float _resistance_ohm, float _max_voltage); + + // Implements the (re)setting behavior described by SIM_BATT_* parameters + // (reminder: if the desired values are different than previous choices, reset) + void maybe_reset(float desired_voltage, float desired_capacity_Ah); - void init_voltage(float voltage); - void init_capacity(float capacity); + // Call this periodically to "step" the battery forward in time + void consume_energy(float current_amps); - void set_current(float current_amps); - float get_voltage(void) const; - float get_capacity(void) const { return capacity_Ah; } + float get_voltage(void) const { return voltage_filter.get(); } // return battery temperature in Kelvin: float get_temperature(void) const { return temperature.kelvin; } private: - float capacity_Ah; - float resistance; + float capacity_Ah; // set <= 0 for unlmited capacity + float resistance_ohm; float max_voltage; float voltage_set; - float remaining_Ah; + float remaining_Ah; // if capacity is unlimited, set this to FLT_MAX uint64_t last_us; struct { - float kelvin = 273; + float kelvin = ambient_temperature + 273.15f; uint64_t last_update_micros; } temperature; + void update_temperature(float current_amps, uint64_t now); // 10Hz filter for battery voltage LowPassFilterFloat voltage_filter{10}; - float get_resting_voltage(float charge_pct) const; - void set_initial_SoC(float voltage); + float get_resting_voltage(void) const; + float compute_remaining_ah(float voltage) const; // const shows this has no side effects + void set_remaining_ah(void); + bool capacity_is_unlimited(void) const { return !(is_positive(capacity_Ah)); } // for readability }; } diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 28720caec05fb..f1c8c32ef7dad 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -656,6 +656,8 @@ void FlightAxis::update(const struct sitl_input &input) battery_voltage = MAX(state.m_batteryVoltage_VOLTS, 0); battery_current = MAX(state.m_batteryCurrentDraw_AMPS, 0); + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin rpm[0] = state.m_heliMainRotorRPM; rpm[1] = state.m_propRPM; motor_mask = 3; diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index b9db8a731086b..02db5af0835aa 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -577,10 +577,9 @@ void Frame::parse_vector3(AP_JSON::value val, const char* label, Vector3f ¶m /* initialise the frame */ -void Frame::init(const char *frame_str, Battery *_battery) +void Frame::init(const char *frame_str) { model = default_model; - battery = _battery; const char *colon = strchr(frame_str, ':'); size_t slen = strlen(frame_str); @@ -620,8 +619,6 @@ void Frame::init(const char *frame_str, Battery *_battery) // power_factor is ratio of power consumed per newton of thrust float power_factor = hover_power / hover_thrust; - battery->setup(model.battCapacityAh, model.refBatRes, model.maxVoltage); - if (uint8_t(model.num_motors) != num_motors) { ::printf("Warning model expected %u motors and got %u\n", uint8_t(model.num_motors), num_motors); } @@ -682,7 +679,8 @@ void Frame::calculate_forces(const Aircraft &aircraft, const auto *_sitl = AP::sitl(); for (uint8_t i=0; iget_voltage(), use_drag); + motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, + gyro, air_density, aircraft.get_battery_voltage(), use_drag); torque += mtorque; thrust += mthrust; // simulate motor rpm @@ -727,23 +725,13 @@ void Frame::calculate_forces(const Aircraft &aircraft, body_accel = thrust/aircraft.gross_mass(); } - -// calculate current and voltage -void Frame::current_and_voltage(float &voltage, float ¤t) +// computes (total) instantaneous current +float Frame::get_current_amp(void) { - 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; - } - const float param_capacity = AP::sitl()->batt_capacity_ah; - if (!is_equal(battery->get_capacity(), param_capacity)) { - battery->init_capacity(param_capacity); - } - voltage = battery->get_voltage(); - current = 0; + float current = 0; for (uint8_t i=0; iinit(frame_str, &battery); + frame->init(frame_str); + battery.setup(frame->get_model_batt_capacity_ah(), + frame->get_model_batt_resistance_ohm(), + frame->get_model_batt_max_voltage()); mass = frame->get_mass(); frame_height = 0.1; @@ -70,10 +73,12 @@ void MultiCopter::update(const struct sitl_input &input) accel_body.zero(); } - // estimate voltage and current - frame->current_and_voltage(battery_voltage, battery_current); + battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah); + battery_voltage = battery.get_voltage(); + battery_current = frame->get_current_amp(); - battery.set_current(battery_current); + battery.consume_energy(battery_current); + battery_temperature = battery.get_temperature(); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 6a0322c84a999..ad9fffc61313e 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -401,7 +401,6 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel float elevator = filtered_servo_angle(input, 1); float rudder = filtered_servo_angle(input, 3); bool launch_triggered = input.servos[6] > 1700; - float throttle; if (reverse_elevator_rudder) { elevator = -elevator; rudder = -rudder; @@ -444,16 +443,7 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel } //printf("Aileron: %.1f elevator: %.1f rudder: %.1f\n", aileron, elevator, rudder); - if (reverse_thrust) { - throttle = filtered_servo_angle(input, 2); - } else { - throttle = filtered_servo_range(input, 2); - } - - float thrust = throttle; - - battery_voltage = sitl->batt_voltage - 0.7*throttle; - battery_current = (battery_voltage/sitl->batt_voltage)*50.0f*sq(throttle); + float thrust = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2); if (ice_engine) { thrust = icengine.update(input); @@ -526,7 +516,13 @@ void Plane::update(const struct sitl_input &input) update_wind(input); calculate_forces(input, rot_accel); - + + float throttle = reverse_thrust ? filtered_servo_angle(input, 2) : filtered_servo_range(input, 2); + battery_voltage = sitl->batt_voltage - 0.7*throttle; + battery_current = (battery_voltage/sitl->batt_voltage)*50.0f*sq(throttle); + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin + update_dynamics(rot_accel); /* diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 924e1af4be6b4..8ebbd71024016 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/libraries/SITL/SIM_QuadPlane.cpp @@ -101,7 +101,10 @@ 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, &battery); + frame->init(frame_str); + battery.setup(frame->get_model_batt_capacity_ah(), + frame->get_model_batt_resistance_ohm(), + frame->get_model_batt_max_voltage()); // increase mass for plane components mass = frame->get_mass() * 1.5; @@ -135,10 +138,12 @@ void QuadPlane::update(const struct sitl_input &input) quad_accel_body.rotate(ROTATION_PITCH_270); } - // estimate voltage and current - frame->current_and_voltage(battery_voltage, battery_current); + battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah); + battery_voltage = battery.get_voltage(); + battery_current = frame->get_current_amp(); - battery.set_current(battery_current); + battery.consume_energy(battery_current); + battery_temperature = battery.get_temperature(); float throttle; if (reverse_thrust) { diff --git a/libraries/SITL/SIM_Scrimmage.cpp b/libraries/SITL/SIM_Scrimmage.cpp index 709e3e4afaac9..0aa3ed9d3da8a 100644 --- a/libraries/SITL/SIM_Scrimmage.cpp +++ b/libraries/SITL/SIM_Scrimmage.cpp @@ -117,6 +117,7 @@ void Scrimmage::recv_fdm(const struct sitl_input &input) battery_voltage = 0; battery_current = 0; + battery_temperature = 273.0f; rpm[0] = 0; rpm[1] = 0; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index b12b1c872784c..71d4618de9098 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -117,13 +117,13 @@ const AP_Param::GroupInfo SIM::var_info[] = { AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270), // @Param: BATT_VOLTAGE // @DisplayName: Simulated battery voltage - // @Description: Simulated battery voltage. Constant voltage when SIM_BATT_CAP_AH is 0, otherwise changing this parameter will re-initialize the state of charge of the battery based on this voltage versus the battery's maximum voltage (default is max voltage). + // @Description: Simulated battery resting voltage. This value (re)sets the battery's resting voltage (before load sag). Simultaneously, it (re)sets the remaining charge: a value less than the max voltage (re)sets to a partially-charged battery. The maximum voltage of the internal battery model is the default value of this parameter. (Do not confuse the default value with the actual value. A few uncommon ways exist to set or modify the default.) A value greater than the max voltage is treated as equal to the max voltage. (This makes it easy to reset to fully charged.) For the special case of unlimited capacity, see `SIM_BATT_CAP_AH`. For the special case where an external source provides battery state, this parameter has no effect. // @Units: V // @User: Advanced AP_GROUPINFO("BATT_VOLTAGE", 19, SIM, batt_voltage, 12.6f), // @Param: BATT_CAP_AH // @DisplayName: Simulated battery capacity - // @Description: Simulated battery capacity. Set to 0 for unlimited capacity. Changing this parameter will re-initialize the state of charge of the battery. + // @Description: Simulated battery capacity. Changing this value (re)sets the battery's charge percentage, like `SIM_BATT_VOLTAGE` does. Set to 0 for unlimited capacity. An unlimited-capacity battery's instantaneous voltage may sag under load, but charge percentage (and thus resting voltage) remain constant. For the special case where an external source provides battery state, this parameter has no effect. // @Units: Ah // @User: Advanced AP_GROUPINFO("BATT_CAP_AH", 20, SIM, batt_capacity_ah, 0), diff --git a/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp b/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp index 1e131f758a794..bcbe8e2fac396 100644 --- a/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp +++ b/libraries/SITL/examples/EvaluateBatteryModel/EvaluateBatteryModel.cpp @@ -62,7 +62,6 @@ void setup(void) // setup battery model battery.setup(amp_hour_capacity, resistance, max_voltage); - battery.init_voltage(max_voltage); uint64_t time = 0; hal.scheduler->stop_clock(time); @@ -70,7 +69,7 @@ void setup(void) ::printf("time, voltage\n"); while (battery.get_voltage() >= min_voltage) { - battery.set_current(current); + battery.consume_energy(current); ::printf("%0.2f, %0.2f\n", time * 1.0e-6, battery.get_voltage());