ardupilot/libraries/SITL/SIM_StratoBlimp.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

301 lines
9.2 KiB
C++
Raw Normal View History

2024-04-26 05:47:28 -03:00
/*
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 <http://www.gnu.org/licenses/>.
*/
/*
StratoBlimp simulator class
*/
#include "SIM_StratoBlimp.h"
#if AP_SIM_STRATOBLIMP_ENABLED
#include <AP_Logger/AP_Logger.h>
#include <AP_Motors/AP_Motors.h>
#include <stdio.h>
using namespace SITL;
extern const AP_HAL::HAL& hal;
// SITL Ship parameters
const AP_Param::GroupInfo StratoBlimp::var_info[] = {
// @Param: MASS
// @DisplayName: mass
// @Description: mass of blimp not including lifting gas
// @Units: kg
AP_GROUPINFO("MASS", 1, StratoBlimp, mass, 80),
// @Param: HMASS
// @DisplayName: helium mass
// @Description: mass of lifting gas
// @Units: kg
AP_GROUPINFO("HMASS", 2, StratoBlimp, helium_mass, 13.54),
// @Param: ARM_LEN
// @DisplayName: arm length
// @Description: distance from center of mass to one motor
// @Units: m
AP_GROUPINFO("ARM_LEN", 3, StratoBlimp, arm_length, 3.6),
// @Param: MOT_THST
// @DisplayName: motor thrust
// @Description: thrust at max throttle for one motor
// @Units: N
AP_GROUPINFO("MOT_THST", 4, StratoBlimp, motor_thrust, 145),
// @Param: DRAG_FWD
// @DisplayName: drag in forward direction
// @Description: drag on X axis
AP_GROUPINFO("DRAG_FWD", 5, StratoBlimp, drag_fwd, 0.27),
// @Param: DRAG_FWD
// @DisplayName: drag in upward direction
// @Description: drag on Z axis
AP_GROUPINFO("DRAG_UP", 6, StratoBlimp, drag_up, 0.1),
// @Param: MOI_YAW
// @DisplayName: moment of inertia in yaw
// @Description: moment of inertia in yaw
AP_GROUPINFO("MOI_YAW", 7, StratoBlimp, moi_yaw, 2800),
// @Param: MOI_ROLL
// @DisplayName: moment of inertia in roll
// @Description: moment of inertia in roll
AP_GROUPINFO("MOI_ROLL", 8, StratoBlimp, moi_roll, 1400),
// @Param: MOI_PITCH
// @DisplayName: moment of inertia in pitch
// @Description: moment of inertia in pitch
AP_GROUPINFO("MOI_PITCH", 9, StratoBlimp, moi_pitch, 3050),
// @Param: ALT_TARG
// @DisplayName: altitude target
// @Description: altitude target
// @Units: m
AP_GROUPINFO("ALT_TARG", 10, StratoBlimp, altitude_target, 20000),
// @Param: CLMB_RT
// @DisplayName: climb rate
// @Description: climb rate
// @Units: m/s
AP_GROUPINFO("CLMB_RT", 11, StratoBlimp, climb_rate, 5),
// @Param: YAW_RT
// @DisplayName: yaw rate
// @Description: maximum yaw rate with full left throttle at target altitude
// @Units: deg/s
AP_GROUPINFO("YAW_RT", 12, StratoBlimp, yaw_rate_max, 60),
// @Param: MOT_ANG
// @DisplayName: motor angle
// @Description: maximum motor tilt angle
// @Units: deg
AP_GROUPINFO("MOT_ANG", 13, StratoBlimp, motor_angle, 20),
// @Param: COL
// @DisplayName: center of lift
// @Description: center of lift position above CoG
// @Units: m
AP_GROUPINFO("COL", 14, StratoBlimp, center_of_lift, 2.54),
// @Param: WVANE
// @DisplayName: weathervaning offset
// @Description: center of drag for weathervaning
// @Units: m
AP_GROUPINFO("WVANE", 15, StratoBlimp, center_of_drag, 2),
AP_GROUPEND
};
StratoBlimp::StratoBlimp(const char *frame_str) :
Aircraft(frame_str)
{
AP::sitl()->models.stratoblimp_ptr = this;
AP_Param::setup_object_defaults(this, var_info);
}
/*
calculate coefficients to match parameters
*/
void StratoBlimp::calculate_coefficients(void)
{
// calculate yaw drag based on turn rate at the given altitude
drag_yaw = 1.0;
// get full throttle rotational accel for one motor
Vector3f body_acc, rot_accel;
handle_motor(1, 0, body_acc, rot_accel, -arm_length);
// get rotational drag at target alt
Vector3f vel_bf, g, drag_linear, drag_rotaccel;
g.z = radians(yaw_rate_max);
get_drag(vel_bf, g,
altitude_target,
drag_linear, drag_rotaccel);
drag_yaw = rot_accel.z / -drag_rotaccel.z;
}
void StratoBlimp::handle_motor(float throttle, float tilt, Vector3f &body_acc, Vector3f &rot_accel, float lateral_position)
{
const float angle_rad = radians(motor_angle) * tilt;
const float thrust_x = motor_thrust * throttle;
const float total_mass = mass + helium_mass;
const Vector3f thrust{cosf(angle_rad)*thrust_x, 0, -sinf(angle_rad)*thrust_x}; // assume constant with pressure alt and linear
Vector3f accel = thrust / total_mass;
Vector3f pos{0, lateral_position, 0};
Vector3f torque = (pos % thrust);
rot_accel.z += torque.z / moi_yaw;
body_acc += accel;
}
/*
get body frame linear and rotational drag for a given velocity and altitude
*/
void StratoBlimp::get_drag(const Vector3f &velocity_linear,
const Vector3f &velocity_rot,
float altitude,
Vector3f &drag_linear, Vector3f &drag_rotaccel)
{
Vector3f vel_air_bf = velocity_linear;
const float drag_x_sign = vel_air_bf.x>0? -1 : 1;
const float drag_y_sign = vel_air_bf.y>0? -1 : 1;
const float drag_z_sign = vel_air_bf.z>0? -1 : 1;
drag_linear.x = 0.5 * drag_x_sign * air_density * sq(vel_air_bf.x) * drag_fwd;
drag_linear.y = 0.5 * drag_y_sign * air_density * sq(vel_air_bf.y) * drag_fwd;
drag_linear.z = 0.5 * drag_z_sign * air_density * sq(vel_air_bf.z) * drag_up;
drag_rotaccel = -velocity_rot * drag_yaw;
/*
apply torque from drag
*/
Vector3f drag_force = drag_linear * mass;
Vector3f drag_pos{-center_of_drag, 0, -center_of_lift};
Vector3f drag_torque = (drag_pos % drag_force);
drag_rotaccel += drag_torque / moi_pitch;
}
/*
get vertical thrust from lift in Newtons
*/
float StratoBlimp::get_lift(float altitude)
{
// start with neutral buoyancy
float lift_accel = GRAVITY_MSS;
// assume lift will equal drag at the given climb rate
float climb_accel = 0.5 * air_density * sq(climb_rate) * drag_up;
// ramp down lift as we approach target alt
lift_accel += linear_interpolate(climb_accel, 0,
altitude,
altitude_target-1000, altitude_target);
return mass * lift_accel;
}
// calculate rotational and linear accelerations in body frame
void StratoBlimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f &rot_accel)
{
//float delta_time = frame_time_us * 1.0e-6f;
if (!hal.scheduler->is_system_initialized()) {
return;
}
const float left_tilt = filtered_servo_angle(input, 0);
const float right_tilt = filtered_servo_angle(input, 1);
const float left_throttle = filtered_servo_range(input, 2);
const float right_throttle = filtered_servo_range(input, 3);
const float ground_release = filtered_servo_range(input, 4);
body_acc.zero();
rot_accel.zero();
handle_motor(left_throttle, left_tilt, body_acc, rot_accel, -arm_length);
handle_motor(right_throttle, right_tilt, body_acc, rot_accel, arm_length);
Vector3f drag_linear, drag_rotaccel;
get_drag(velocity_air_bf, gyro,
location.alt*0.01,
drag_linear, drag_rotaccel);
body_acc += drag_linear;
rot_accel += drag_rotaccel;
if (ground_release > 0.9) {
released = true;
}
if (released) {
Vector3f lift_thrust_ef{0, 0, -get_lift(location.alt*0.01)};
Vector3f lift_thrust_bf = dcm.transposed() * lift_thrust_ef;
body_acc += lift_thrust_bf / mass;
/*
apply righting moment
*/
Vector3f lift_pos{0, 0, -center_of_lift};
Vector3f lift_torque = (lift_pos % lift_thrust_bf);
rot_accel += lift_torque / moi_roll;
}
}
/*
update the airship simulation by one time step
*/
void StratoBlimp::update(const struct sitl_input &input)
{
air_density = get_air_density(location.alt*0.01);
EAS2TAS = sqrtf(SSL_AIR_DENSITY / air_density);
calculate_coefficients();
float delta_time = frame_time_us * 1.0e-6f;
Vector3f rot_accel = Vector3f(0,0,0);
calculate_forces(input, accel_body, rot_accel);
// update rotational rates in body frame
gyro += rot_accel * delta_time;
gyro.x = constrain_float(gyro.x, -radians(2000.0f), radians(2000.0f));
gyro.y = constrain_float(gyro.y, -radians(2000.0f), radians(2000.0f));
gyro.z = constrain_float(gyro.z, -radians(2000.0f), radians(2000.0f));
dcm.rotate(gyro * delta_time);
dcm.normalize();
update_dynamics(rot_accel);
update_external_payload(input);
// update lat/lon/altitude
update_position();
update_wind(input);
time_advance();
// update magnetic field
update_mag_field_bf();
}
#endif // AP_SIM_STRATOBLIMP_ENABLED