Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/SITL/SIM_Glider.cpp

413 lines
16 KiB
C++
Raw Blame History

/*
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/>.
*/
/*
glider model for high altitude balloon drop
*/
#include "SIM_Glider.h"
#if AP_SIM_GLIDER_ENABLED
#include <stdio.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
using namespace SITL;
// SITL glider parameters
const AP_Param::GroupInfo Glider::var_info[] = {
// @Param: BLN_BRST
// @DisplayName: balloon burst height
// @Description: balloon burst height
// @Units: m
AP_GROUPINFO("BLN_BRST", 1, Glider, balloon_burst_amsl, 30000),
// @Param: BLN_RATE
// @DisplayName: balloon climb rate
// @Description: balloon climb rate
// @Units: m/s
AP_GROUPINFO("BLN_RATE", 2, Glider, balloon_rate, 5.5),
AP_GROUPEND
};
Glider::Glider(const char *frame_str) :
Aircraft(frame_str)
{
ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
carriage_state = carriageState::WAITING_FOR_PICKUP;
AP::sitl()->models.glider_ptr = this;
AP_Param::setup_object_defaults(this, var_info);
}
// Torque calculation function
Vector3f Glider::getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const
{
// Calculate dynamic pressure
const auto &m = model;
double qPa = 0.5*air_density*sq(velocity_air_bf.length());
const float aileron_rad = inputAileron * radians(m.aileronDeflectionLimitDeg);
const float elevator_rad = inputElevator * radians(m.elevatorDeflectionLimitDeg);
const float rudder_rad = inputRudder * radians(m.rudderDeflectionLimitDeg);
const float tas = MAX(airspeed * AP::ahrs().get_EAS2TAS(), 1);
float Cl = (m.Cl2 * sq(alpharad) + m.Cl1 * alpharad + m.Cl0) * betarad;
float Cm = m.Cm2 * sq(alpharad) + m.Cm1 * alpharad + m.Cm0;
float Cn = (m.Cn2 * sq(alpharad) + m.Cn1 * alpharad + m.Cn0) * betarad;
Cl += m.deltaClperRadianElev * elevator_rad;
Cm += m.deltaCmperRadianElev * elevator_rad;
Cn += m.deltaCnperRadianElev * elevator_rad;
Cl += m.deltaClperRadianRud * rudder_rad;
Cm += m.deltaCmperRadianRud * rudder_rad;
Cn += m.deltaCnperRadianRud * rudder_rad;
Cl += (m.deltaClperRadianAil2 * sq(alpharad) + m.deltaClperRadianAil1 * alpharad + m.deltaClperRadianAil0) * aileron_rad;
Cm += m.deltaCmperRadianAil * aileron_rad;
Cn += (m.deltaCnperRadianAil2 * sq(alpharad) + m.deltaCnperRadianAil1 * alpharad + m.deltaCnperRadianAil0) * aileron_rad;
// derivatives
float Clp = m.Clp2 * sq(alpharad) + m.Clp1 * alpharad + m.Clp0;
float Clr = m.Clr2 * sq(alpharad) + m.Clr1 * alpharad + m.Clr0;
float Cnp = m.Cnp2 * sq(alpharad) + m.Cnp1 * alpharad + m.Cnp0;
float Cnr = m.Cnr2 * sq(alpharad) + m.Cnr1 * alpharad + m.Cnr0;
// normalise gyro rates
Vector3f pqr_norm = gyro;
pqr_norm.x *= 0.5 * m.refSpan / tas;
pqr_norm.y *= 0.5 * m.refChord / tas;
pqr_norm.z *= 0.5 * m.refSpan / tas;
Cl += pqr_norm.x * Clp;
Cl += pqr_norm.z * Clr;
Cn += pqr_norm.x * Cnp;
Cn += pqr_norm.z * Cnr;
Cm += pqr_norm.y * m.Cmq;
float Mx = Cl * qPa * m.Sref * m.refSpan;
float My = Cm * qPa * m.Sref * m.refChord;
float Mz = Cn * qPa * m.Sref * m.refSpan;
#if 0
AP::logger().Write("GLT", "TimeUS,Alpha,Beta,Cl,Cm,Cn", "Qfffff",
AP_HAL::micros64(),
degrees(alpharad),
degrees(betarad),
Cl, Cm, Cn);
#endif
return Vector3f(Mx/m.IXX, My/m.IYY, Mz/m.IZZ);
}
// Force calculation, return vector in Newtons
Vector3f Glider::getForce(float inputAileron, float inputElevator, float inputRudder)
{
const auto &m = model;
const float aileron_rad = inputAileron * radians(m.aileronDeflectionLimitDeg);
const float elevator_rad = inputElevator * radians(m.elevatorDeflectionLimitDeg);
const float rudder_rad = inputRudder * radians(m.rudderDeflectionLimitDeg);
// dynamic pressure
double qPa = 0.5*air_density*sq(velocity_air_bf.length());
float CA = m.CA2 * sq(alpharad) + m.CA1 * alpharad + m.CA0;
float CY = (m.CY2 * sq(alpharad) + m.CY1 * alpharad + m.CY0) * betarad;
float CN = m.CN2 * sq(alpharad) + m.CN1 * alpharad + m.CN0;
CN += m.deltaCNperRadianElev * elevator_rad;
CA += m.deltaCAperRadianElev * elevator_rad;
CY += m.deltaCYperRadianElev * elevator_rad;
CN += m.deltaCNperRadianRud * rudder_rad;
CA += m.deltaCAperRadianRud * rudder_rad;
CY += m.deltaCYperRadianRud * rudder_rad;
CN += m.deltaCNperRadianAil * aileron_rad;
CA += m.deltaCAperRadianAil * aileron_rad;
CY += m.deltaCYperRadianAil * aileron_rad;
float Fx = -CA * qPa * m.Sref;
float Fy = CY * qPa * m.Sref;
float Fz = -CN * qPa * m.Sref;
Vector3f ret = Vector3f(Fx, Fy, Fz);
float Flift = Fx * sin(alpharad) - Fz * cos(alpharad);
float Fdrag = -Fx * cos(alpharad) - Fz * sin(alpharad);
if (carriage_state == carriageState::RELEASED) {
uint32_t now = AP_HAL::millis();
sim_LD = 0.1 * constrain_float(Flift/MAX(1.0e-6,Fdrag),0,20) + 0.9 * sim_LD;
if (now - last_drag_ms > 10 &&
airspeed > 1) {
last_drag_ms = now;
#if HAL_LOGGING_ENABLED
AP::logger().Write("SLD", "TimeUS,AltFt,AltM,EAS,TAS,AD,Fl,Fd,LD,Elev,AoA,Fx,Fy,Fz,q", "Qffffffffffffff",
AP_HAL::micros64(),
(location.alt*0.01)/FEET_TO_METERS,
location.alt*0.01,
velocity_air_bf.length()/eas2tas,
velocity_air_bf.length(),
air_density,
Flift, Fdrag, sim_LD,
degrees(elevator_rad),
degrees(alpharad),
Fx, Fy, Fz,
qPa);
AP::logger().Write("SL2", "TimeUS,AltFt,KEAS,KTAS,AD,Fl,Fd,LD,Elev,Ail,Rud,AoA,SSA,q,Az", "Qffffffffffffff",
AP_HAL::micros64(),
(location.alt*0.01)/FEET_TO_METERS,
M_PER_SEC_TO_KNOTS*velocity_air_bf.length()/eas2tas,
M_PER_SEC_TO_KNOTS*velocity_air_bf.length(),
air_density,
Flift, Fdrag, sim_LD,
degrees(elevator_rad),
degrees(aileron_rad),
degrees(rudder_rad),
degrees(alpharad),
degrees(betarad),
qPa,
accel_body.z);
AP::logger().Write("SCTL", "TimeUS,Ail,Elev,Rudd", "Qfff",
AP_HAL::micros64(),
degrees(aileron_rad),
degrees(elevator_rad),
degrees(rudder_rad));
#endif // HAL_LOGGING_ENABLED
}
}
return ret;
}
void Glider::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel)
{
filtered_servo_setup(1, 1100, 1900, model.aileronDeflectionLimitDeg);
filtered_servo_setup(4, 1100, 1900, model.aileronDeflectionLimitDeg);
filtered_servo_setup(2, 1100, 1900, model.elevatorDeflectionLimitDeg);
filtered_servo_setup(3, 1100, 1900, model.rudderDeflectionLimitDeg);
float aileron = 0.5*(filtered_servo_angle(input, 1) + filtered_servo_angle(input, 4));
float elevator = filtered_servo_angle(input, 2);
float rudder = filtered_servo_angle(input, 3);
float balloon = filtered_servo_range(input, 5);
float balloon_cut = filtered_servo_range(input, 9);
// Move balloon upwards using balloon velocity from channel 6
// Aircraft is released from ground constraint when channel 6 PWM > 1010
// Once released, plane will be dropped when balloon_burst_amsl is reached or channel 6 is set to PWM 1000
if (carriage_state == carriageState::WAITING_FOR_RELEASE) {
balloon_velocity = Vector3f(-wind_ef.x, -wind_ef.y, -wind_ef.z -balloon_rate * balloon);
balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
const float height_AMSL = 0.01f * (float)home.alt - position.z;
// release at burst height or when channel 9 goes high
if (hal.scheduler->is_system_initialized() &&
(height_AMSL > balloon_burst_amsl || balloon_cut > 0.8)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "pre-release at %i m AMSL\n", (int)height_AMSL);
carriage_state = carriageState::PRE_RELEASE;
}
} else if (carriage_state == carriageState::PRE_RELEASE) {
// slow down for release
balloon_velocity *= 0.999;
balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
if (balloon_velocity.length() < 0.5) {
carriage_state = carriageState::RELEASED;
use_smoothing = false;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "released at %.0f m AMSL\n", (0.01f * home.alt) - position.z);
}
} else if (carriage_state == carriageState::WAITING_FOR_PICKUP) {
// Don't allow the balloon to drag sideways until the pickup
balloon_velocity = Vector3f(0.0f, 0.0f, -balloon_rate * balloon);
balloon_position += balloon_velocity * (1.0e-6 * (float)frame_time_us);
}
// calculate angle of attack
alpharad = atan2f(velocity_air_bf.z, velocity_air_bf.x);
betarad = atan2f(velocity_air_bf.y,velocity_air_bf.x);
alpharad = constrain_float(alpharad, -model.alphaRadMax, model.alphaRadMax);
betarad = constrain_float(betarad, -model.betaRadMax, model.betaRadMax);
Vector3f force;
if (!update_balloon(balloon, force, rot_accel)) {
force = getForce(aileron, elevator, rudder);
rot_accel = getTorque(aileron, elevator, rudder, force);
}
accel_body = force / model.mass;
if (on_ground()) {
// add some ground friction
Vector3f vel_body = dcm.transposed() * velocity_ef;
accel_body.x -= vel_body.x * 0.3f;
}
// constrain accelerations
accel_body.x = constrain_float(accel_body.x, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
accel_body.y = constrain_float(accel_body.y, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
accel_body.z = constrain_float(accel_body.z, -16*GRAVITY_MSS, 16*GRAVITY_MSS);
}
/*
update the plane simulation by one time step
*/
void Glider::update(const struct sitl_input &input)
{
Vector3f rot_accel;
update_wind(input);
calculate_forces(input, rot_accel, accel_body);
if (carriage_state == carriageState::WAITING_FOR_PICKUP) {
// Handle special case where plane is being held nose down waiting to be lifted
accel_body = dcm.transposed() * Vector3f(0.0f, 0.0f, -GRAVITY_MSS);
velocity_ef.zero();
gyro.zero();
dcm.from_euler(0.0f, radians(-80.0f), radians(home_yaw));
use_smoothing = true;
adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
} else {
update_dynamics(rot_accel);
}
update_external_payload(input);
// update lat/lon/altitude
update_position();
time_advance();
// update magnetic field
update_mag_field_bf();
}
/*
return true if we are on the ground
*/
bool Glider::on_ground() const
{
switch (carriage_state) {
case carriageState::NONE:
case carriageState::RELEASED:
return hagl() <= 0.001;
case carriageState::WAITING_FOR_PICKUP:
case carriageState::WAITING_FOR_RELEASE:
case carriageState::PRE_RELEASE:
// prevent bouncing around ground
// don't do ground interaction if being carried
break;
}
return false;
}
/*
implement balloon lift
controlled by SIM_BLN_BRST and SIM_BLN_RATE
*/
bool Glider::update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel)
{
if (!hal.util->get_soft_armed()) {
return false;
}
switch (carriage_state) {
case carriageState::NONE:
case carriageState::RELEASED:
// balloon not active
disable_origin_movement = false;
return false;
case carriageState::WAITING_FOR_PICKUP:
case carriageState::WAITING_FOR_RELEASE:
case carriageState::PRE_RELEASE:
// while under balloon disable origin movement to allow for balloon position calculations
disable_origin_movement = true;
break;
}
// assume a 50m tether with a 1Hz pogo frequency and damping ratio of 0.2
Vector3f tether_pos_bf{-1.0f,0.0f,0.0f}; // tether attaches to vehicle tail approx 1m behind c.g.
const float omega = model.tetherPogoFreq * M_2PI; // rad/sec
const float zeta = 0.7f;
float tether_stiffness = model.mass * sq(omega); // N/m
float tether_damping = 2.0f * zeta * omega / model.mass; // N/(m/s)
// NED relative position vector from tether attachment on plane to balloon attachment
Vector3f relative_position = balloon_position - (position.tofloat() + (dcm * tether_pos_bf));
const float separation_distance = relative_position.length();
// NED unit vector pointing from tether attachment on plane to attachment on balloon
Vector3f tether_unit_vec_ef = relative_position.normalized();
// NED velocity of attahment point on plane
Vector3f attachment_velocity_ef = velocity_ef + dcm * (gyro % tether_pos_bf);
// NED velocity of attachment point on balloon as seen by observer on attachemnt point on plane
Vector3f relative_velocity = balloon_velocity - attachment_velocity_ef;
float separation_speed = relative_velocity * tether_unit_vec_ef;
// rate increase in separation between attachment point on plane and balloon
// tension force in tether due to stiffness and damping
float tension_force = MAX(0.0f, (separation_distance - model.tetherLength) * tether_stiffness);
if (tension_force > 0.0f) {
tension_force += constrain_float(separation_speed * tether_damping, 0.0f, 0.05f * tension_force);
}
if (carriage_state == carriageState::WAITING_FOR_PICKUP && tension_force > 1.2f * model.mass * GRAVITY_MSS && balloon > 0.01f) {
carriage_state = carriageState::WAITING_FOR_RELEASE;
}
if (carriage_state == carriageState::WAITING_FOR_RELEASE ||
carriage_state == carriageState::PRE_RELEASE) {
Vector3f tension_force_vector_ef = tether_unit_vec_ef * tension_force;
Vector3f tension_force_vector_bf = dcm.transposed() * tension_force_vector_ef;
force = tension_force_vector_bf;
// drag force due to lateral motion assuming projected area from Y is 20% of projected area seen from Z and
// assuming bluff body drag characteristic. In reality we would need an aero model that worked flying backwards,
// but this will have to do for now.
Vector3f aero_force_bf = Vector3f(0.0f, 0.2f * velocity_air_bf.y * fabsf(velocity_air_bf.y), velocity_air_bf.z * fabsf(velocity_air_bf.z));
aero_force_bf *= air_density * model.Sref;
force -= aero_force_bf;
Vector3f tension_moment_vector_bf = tether_pos_bf % tension_force_vector_bf;
Vector3f tension_rot_accel = Vector3f(tension_moment_vector_bf.x/model.IXX, tension_moment_vector_bf.y/model.IYY, tension_moment_vector_bf.z/model.IZZ);
rot_accel = tension_rot_accel;
// add some rotation damping due to air resistance assuming a 2 sec damping time constant at SL density
// TODO model roll damping with more accuracy using Clp data for zero alpha as a first approximation
rot_accel -= gyro * 0.5 * air_density;
} else {
// tether is either slack awaiting pickup or released
rot_accel.zero();
force = dcm.transposed() * Vector3f(0.0f, 0.0f, -GRAVITY_MSS * model.mass);
}
// balloon is active
return true;
}
#endif // AP_SIM_GLIDER_ENABLED
Reference in New Issue View Git Blame Copy Permalink