Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Blimp uses internal SITL::Battery

Browse Source
This commit is contained in:
Hunter McClelland
2026-07-08 17:50:52 +10:00
committed by Peter Barker
parent c6791bf125
commit f5d725a2a8
2 changed files with 47 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/SITL/SIM_Blimp.cpp
+44 -11
View File
@@ -44,6 +44,13 @@ Blimp::Blimp(const char *frame_str) :
drag_gyr_lin_constant = 0.035;
lock_step_scheduled = true;
constexpr float default_battery_resistance_ohm = 0.01f;
battery.setup(sitl->batt_capacity_ah,
default_battery_resistance_ohm,
sitl->batt_voltage,
ambient_outside_temperature_degC());
::printf("Starting Blimp model\n");
if (strstr(frame_str, "motor")) {
@@ -65,16 +72,18 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc,
//all fin setup
for (uint8_t i=0; i<4; i++) {
fin[i].last_angle = fin[i].angle;
if (input.servos[i] == 0) {
fin[i].angle = 0;
fin[i].servo_angle = 0;
} else {
// filtered_servo_angle() normalises PWM against a fixed 1500 +/- 500
// range, but blimp.parm uses SERVOn_MIN 500 / TRIM 1350 / MAX 2200;
// the +13.5 degree offset recentres that asymmetric range to a
// symmetric -76.5..+76.5 degrees with 0 degrees at servo trim
fin[i].angle = filtered_servo_angle(input, i)*radians(45.0f)+radians(13.5);
fin[i].servo_angle = filtered_servo_angle(input, i);
if (!battery_is_empty()) {
if (input.servos[i] == 0) {
fin[i].angle = 0;
fin[i].servo_angle = 0;
} else {
// filtered_servo_angle() normalises PWM against a fixed 1500 +/- 500
// range, but blimp.parm uses SERVOn_MIN 500 / TRIM 1350 / MAX 2200;
// the +13.5 degree offset recentres that asymmetric range to a
// symmetric -76.5..+76.5 degrees with 0 degrees at servo trim
fin[i].angle = filtered_servo_angle(input, i)*radians(45.0f)+radians(13.5);
fin[i].servo_angle = filtered_servo_angle(input, i);
}
}
if (fin[i].angle < fin[i].last_angle) fin[i].dir = 0; //thus 0 = "angle is reducing"
@@ -291,7 +300,7 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc,
} else { //MotorBlimp
for (uint8_t i=0; i<4; i++) {
if (input.servos[i] == 0) {
if (battery_is_empty() || input.servos[i] == 0) {
mot[i].throttle = 0;
} else {
mot[i].throttle = filtered_servo_angle(input, i);
@@ -396,3 +405,27 @@ void Blimp::update(const struct sitl_input &input)
update_battery();
}
void Blimp::update_battery()
{
battery.maybe_reset(sitl->batt_voltage, sitl->batt_capacity_ah);
battery_current = 0.0f;
if (!battery_is_empty()) {
if (motorblimp) {
constexpr float motor_current_scaler_amps = 2.0f;
for (uint8_t i=0; i<4; i++) {
battery_current += fabsf(mot[i].throttle) * motor_current_scaler_amps;
}
} else {
constexpr float fin_current_scaler_amps = 0.01f;
for (uint8_t i=0; i<4; i++) {
battery_current += fabsf(fin[i].vel) * fin_current_scaler_amps;
}
}
}
battery.consume_energy(battery_current, AP_HAL::micros64());
battery_voltage = battery.get_voltage();
battery_temperature_degC = battery.get_temperature_degC();
}
libraries/SITL/SIM_Blimp.h
+3 -1
View File
@@ -30,7 +30,7 @@ struct Fins
float last_angle;
float servo_angle;
bool dir;
float vel; // velocity, in m/s
float vel; // ang velocity, in deg/s
float T; //Tangential (thrust) force, in Neutons
float N; //Normal force, in Newtons
float Fx; //Fx,y,z = Force in bodyframe orientation at servo position, in Newtons
@@ -84,6 +84,8 @@ protected:
bool motorblimp = false;
void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel);
void update_battery() override;
bool battery_is_empty() { return battery_voltage < 0.5f; };
float sq(float a) {return powf(a,2);}
};