diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 08e78f3739013e..50ff1e03d9a5c6 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -213,6 +213,7 @@ void SITL_State::_fdm_input_local(void) // construct servos structure for FDM _simulator_servos(input); + _set_voltage_and_current_pins(input); #if AP_SIM_JSON_MASTER_ENABLED // read servo inputs from ride along flight controllers @@ -368,8 +369,29 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } } _sitl->throttle = throttle; +} + +void SITL_State::_set_voltage_and_current_pins(struct sitl_input &input) +{ + float voltage = 0; + float current = 0; + + if (_sitl->state.battery_voltage > 0) { + // FDM provides voltage and current + voltage = _sitl->state.battery_voltage; + current = _sitl->state.battery_current; + } else { + // internal sitl model provides voltage and current + voltage = sitl_model->get_battery_voltage(); + current = sitl_model->get_battery_current(); + } - update_voltage_current(input, throttle); + // assume 3DR power brick + voltage_pin_voltage = voltage / 10.1f; + current_pin_voltage = current / 17.0f; + // fake battery2 as just a 25% gain on the first one + voltage2_pin_voltage = voltage_pin_voltage * 0.25f; + current2_pin_voltage = current_pin_voltage * 0.25f; } void SITL_State::init(int argc, char * const argv[]) diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 97ef75236d8d60..97ae2acfc916d2 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -70,6 +70,7 @@ class HALSITL::SITL_State : public SITL_State_Common { void _fdm_input_local(void); void _output_to_flightgear(void); void _simulator_servos(struct sitl_input &input); + void _set_voltage_and_current_pins(struct sitl_input &input); void _fdm_input_step(void); void wait_clock(uint64_t wait_time_usec); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index acb74cbe3e74c3..4c17d10d7bde7a 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -432,47 +432,4 @@ void SITL_State_Common::sim_update(void) /* update voltage and current pins */ -void SITL_State_Common::update_voltage_current(struct sitl_input &input, float throttle) -{ - float voltage = 0; - float current = 0; - - if (_sitl != nullptr) { - if (_sitl->state.battery_voltage <= 0) { - if (_vehicle == ArduSub) { - voltage = _sitl->batt_voltage; - for (uint8_t i=0; i<6; i++) { - float pwm = input.servos[i]; - //printf("i: %d, pwm: %.2f\n", i, pwm); - float fraction = fabsf((pwm - 1500) / 500.0f); - - voltage -= fraction * 0.5f; - - float draw = fraction * 15; - current += draw; - } - } else { - // simulate simple battery setup - // lose 0.7V at full throttle - voltage = _sitl->batt_voltage - 0.7f * throttle; - - // assume 50A at full throttle - current = 50.0f * throttle; - } - } else { - // FDM provides voltage and current - voltage = _sitl->state.battery_voltage; - current = _sitl->state.battery_current; - } - } - - // assume 3DR power brick - voltage_pin_voltage = (voltage / 10.1f); - current_pin_voltage = current/17.0f; - // fake battery2 as just a 25% gain on the first one - voltage2_pin_voltage = voltage_pin_voltage * 0.25f; - current2_pin_voltage = current_pin_voltage * 0.25f; -} - #endif // HAL_BOARD_SITL - diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 82f9488f95e624..91e407b9bf996c 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -218,8 +218,6 @@ class HALSITL::SITL_State_Common { SITL::Aircraft *sitl_model; SITL::SIM *_sitl; - - void update_voltage_current(struct sitl_input &input, float throttle); }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 28be7e2baeebb0..73a38bf5d69e59 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 13992dfb2b35b9..fb3f355d1f88ef 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 7618745cf2796c..f592b50847f72f 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 max_voltage; + } + 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: it 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 46a686eaaf02a5..bf0a46a9412d34 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 28720caec05fbd..f1c8c32ef7dad1 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 b9db8a731086ba..02db5af0835aa6 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; ibatt_voltage, sitl->batt_capacity_ah); + battery.consume_energy(battery_current); + battery_voltage = battery.get_voltage(); + battery_temperature = battery.get_temperature(); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index dfa9223e0e01f5..9e6fee92657d31 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -490,11 +490,13 @@ void JSON::recv_fdm(const struct sitl_input &input) // update battery state if ((received_bitmask & BAT_VOLT) != 0) { - battery_voltage = state.bat_volt; + battery_voltage = state.bat_volt; } if ((received_bitmask & BAT_AMP) != 0) { battery_current = state.bat_amp; } + // this aircraft has a constant battery temperature + battery_temperature = 273.0f; // kelvin double deltat; if (state.timestamp_s < last_timestamp_s) { @@ -615,4 +617,4 @@ void JSON::update(const struct sitl_input &input) #endif } -#endif // AP_SIM_JSON_ENABLED \ No newline at end of file +#endif // AP_SIM_JSON_ENABLED diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 30d0cfef7ed62d..ce50f8b9d0b794 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -32,7 +32,10 @@ MultiCopter::MultiCopter(const char *frame_str) : exit(1); } - 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()); 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 6a0322c84a9993..ad9fffc61313ea 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 924e1af4be6b42..8ebbd710240169 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 709e3e4afaac95..0aa3ed9d3da8ae 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/SIM_Submarine.cpp b/libraries/SITL/SIM_Submarine.cpp index dcfca1dcd3d329..fa9691fcfcd996 100644 --- a/libraries/SITL/SIM_Submarine.cpp +++ b/libraries/SITL/SIM_Submarine.cpp @@ -121,6 +121,20 @@ void Submarine::calculate_forces(const struct sitl_input &input, Vector3f &rot_a } +void Submarine::update_battery(const struct sitl_input &input) +{ + battery_voltage = sitl->batt_voltage; + battery_current = 0; + for (uint8_t i=0; i<6; i++) { + float pwm = input.servos[i]; + //printf("i: %d, pwm: %.2f\n", i, pwm); + float fraction = fabsf((pwm - 1500) / 500.0f); + battery_voltage -= fraction * 0.5f; + float draw = fraction * 15; + battery_current += draw; + } +} + /** * @brief Calculate the torque induced by buoyancy foam * @@ -239,6 +253,7 @@ void Submarine::update(const struct sitl_input &input) Vector3f rot_accel; calculate_forces(input, rot_accel, accel_body); + update_battery(input); update_dynamics(rot_accel); update_external_payload(input); diff --git a/libraries/SITL/SIM_Submarine.h b/libraries/SITL/SIM_Submarine.h index ea0e311760b216..fd38af5ec9be64 100644 --- a/libraries/SITL/SIM_Submarine.h +++ b/libraries/SITL/SIM_Submarine.h @@ -105,6 +105,8 @@ class Submarine : public Aircraft { void calculate_angular_drag_torque(const Vector3f &angular_velocity, const Vector3f &drag_coefficient, Vector3f &torque) const; // calculate torque induced by buoyancy foams void calculate_buoyancy_torque(Vector3f &torque); + // compute and set battery usage + void update_battery(const struct sitl_input &input); Frame *frame; Thruster* thrusters; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index b12b1c872784cf..84f640cff87f96 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 voltage. This parameter's default value is the battery's maximum voltage. A value higher than the max will be treated identically as exactly that max value. When capacity is unlimited (see SIM_BATT_CAP_AH), this is the (constant) resting voltage. Otherwise, changing this value correspondingly (re)sets the battery's charge percentage (by comparing the new value against the max voltage). // @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. Set to 0 for unlimited capacity. Changing this value (re)sets the battery's charge percentage (based on SIM_BATT_VOLTAGE). // @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 1e131f758a7943..bcbe8e2fac3968 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());