From 53c0619a9a91877222311a1d298ac3341dde5cf5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 28 Apr 2024 13:21:41 +1000 Subject: [PATCH] SITL: added Glider SITL model --- libraries/SITL/SIM_Aircraft.cpp | 62 ++--- libraries/SITL/SIM_Aircraft.h | 8 +- libraries/SITL/SIM_Glider.cpp | 412 +++++++++++++++++++++++++++++ libraries/SITL/SIM_Glider.h | 203 ++++++++++++++ libraries/SITL/SIM_StratoBlimp.cpp | 2 +- libraries/SITL/SIM_config.h | 4 + libraries/SITL/SITL.cpp | 15 +- libraries/SITL/SITL.h | 17 +- libraries/SITL/ServoModel.cpp | 136 ++++++++++ libraries/SITL/ServoModel.h | 51 ++++ 10 files changed, 870 insertions(+), 40 deletions(-) create mode 100644 libraries/SITL/SIM_Glider.cpp create mode 100644 libraries/SITL/SIM_Glider.h create mode 100644 libraries/SITL/ServoModel.cpp create mode 100644 libraries/SITL/ServoModel.h diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index d84631b853..77e7e3e0aa 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -37,6 +37,7 @@ #include #include #include +#include using namespace SITL; @@ -176,16 +177,18 @@ void Aircraft::update_position(void) pos_home.x, pos_home.y, pos_home.z); #endif - uint32_t now = AP_HAL::millis(); - if (now - last_one_hz_ms >= 1000) { - // shift origin of position at 1Hz to current location - // this prevents sperical errors building up in the GPS data - last_one_hz_ms = now; - Vector2d diffNE = origin.get_distance_NE_double(location); - position.xy() -= diffNE; - smoothing.position.xy() -= diffNE; - origin.lat = location.lat; - origin.lng = location.lng; + if (!disable_origin_movement) { + uint32_t now = AP_HAL::millis(); + if (now - last_one_hz_ms >= 1000) { + // shift origin of position at 1Hz to current location + // this prevents sperical errors building up in the GPS data + last_one_hz_ms = now; + Vector2d diffNE = origin.get_distance_NE_double(location); + position.xy() -= diffNE; + smoothing.position.xy() -= diffNE; + origin.lat = location.lat; + origin.lng = location.lng; + } } } @@ -619,6 +622,12 @@ void Aircraft::update_model(const struct sitl_input &input) */ void Aircraft::update_dynamics(const Vector3f &rot_accel) { + // update eas2tas and air density +#if AP_AHRS_ENABLED + eas2tas = AP::ahrs().get_EAS2TAS(); +#endif + air_density = SSL_AIR_DENSITY / sq(eas2tas); + const float delta_time = frame_time_us * 1.0e-6f; // update rotational rates in body frame @@ -902,34 +911,13 @@ void Aircraft::smooth_sensors(void) smoothing.last_update_us = now; } -/* - return a filtered servo input as a value from -1 to 1 - servo is assumed to be 1000 to 2000, trim at 1500 - */ -float Aircraft::filtered_idx(float v, uint8_t idx) -{ - if (sitl->servo_speed <= 0) { - return v; - } - const float cutoff = 1.0f / (2 * M_PI * sitl->servo_speed); - servo_filter[idx].set_cutoff_frequency(cutoff); - - if (idx >= ARRAY_SIZE(servo_filter)) { - AP_HAL::panic("Attempt to filter invalid servo at offset %u", (unsigned)idx); - } - - return servo_filter[idx].apply(v, frame_time_us * 1.0e-6f); -} - - /* return a filtered servo input as a value from -1 to 1 servo is assumed to be 1000 to 2000, trim at 1500 */ float Aircraft::filtered_servo_angle(const struct sitl_input &input, uint8_t idx) { - const float v = (input.servos[idx] - 1500)/500.0f; - return filtered_idx(v, idx); + return servo_filter[idx].filter_angle(input.servos[idx], frame_time_us * 1.0e-6); } /* @@ -938,8 +926,14 @@ float Aircraft::filtered_servo_angle(const struct sitl_input &input, uint8_t idx */ float Aircraft::filtered_servo_range(const struct sitl_input &input, uint8_t idx) { - const float v = (input.servos[idx] - 1000)/1000.0f; - return filtered_idx(v, idx); + return servo_filter[idx].filter_range(input.servos[idx], frame_time_us * 1.0e-6); +} + +// setup filtering for servo +void Aircraft::filtered_servo_setup(uint8_t idx, uint16_t pwm_min, uint16_t pwm_max, float deflection_deg) +{ + servo_filter[idx].set_pwm_range(pwm_min, pwm_max); + servo_filter[idx].set_deflection(deflection_deg); } // extrapolate sensors by a given delta time in seconds diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 7c6d536a7b..a96e486949 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -37,6 +37,7 @@ #include "SIM_Battery.h" #include #include "SIM_JSON_Master.h" +#include "ServoModel.h" namespace SITL { @@ -244,6 +245,8 @@ protected: bool use_time_sync = true; float last_speedup = -1.0f; const char *config_ = ""; + float eas2tas = 1.0; + float air_density = SSL_AIR_DENSITY; // allow for AHRS_ORIENTATION AP_Int8 *ahrs_orientation; @@ -260,6 +263,7 @@ protected: } ground_behavior; bool use_smoothing; + bool disable_origin_movement; float ground_height_difference() const; @@ -300,9 +304,9 @@ protected: void update_wind(const struct sitl_input &input); // return filtered servo input as -1 to 1 range - float filtered_idx(float v, uint8_t idx); float filtered_servo_angle(const struct sitl_input &input, uint8_t idx); float filtered_servo_range(const struct sitl_input &input, uint8_t idx); + void filtered_servo_setup(uint8_t idx, uint16_t pwm_min, uint16_t pwm_max, float deflection_deg); // extrapolate sensors by a given delta time in seconds void extrapolate_sensors(float delta_time); @@ -336,7 +340,7 @@ private: Location location; } smoothing; - LowPassFilterFloat servo_filter[5]; + ServoModel servo_filter[16]; Buzzer *buzzer; Sprayer *sprayer; diff --git a/libraries/SITL/SIM_Glider.cpp b/libraries/SITL/SIM_Glider.cpp new file mode 100644 index 0000000000..5ac36148d4 --- /dev/null +++ b/libraries/SITL/SIM_Glider.cpp @@ -0,0 +1,412 @@ +/* + 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 . + */ +/* + glider model for high altitude balloon drop + */ + +#include "SIM_Glider.h" + +#if AP_SIM_GLIDER_ENABLED + +#include +#include +#include +#include + +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 diff --git a/libraries/SITL/SIM_Glider.h b/libraries/SITL/SIM_Glider.h new file mode 100644 index 0000000000..0c0aa3fa5e --- /dev/null +++ b/libraries/SITL/SIM_Glider.h @@ -0,0 +1,203 @@ +/* + 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 . + */ +/* + Glider model for high altitude balloon drop +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GLIDER_ENABLED + +#include "SIM_Aircraft.h" +#include + +namespace SITL { + +/* + a very simple plane simulator + */ +class Glider : public Aircraft { +public: + Glider(const char *frame_str); + + /* update model by one time step */ + virtual void update(const struct sitl_input &input) override; + + /* static object creator */ + static Aircraft *create(const char *frame_str) { + return new Glider(frame_str); + } + + bool on_ground() const override; + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + float alpharad; + float betarad; + + AP_Float balloon_burst_amsl; + AP_Float balloon_rate; + + /* + parameters that define the glider model + */ + const struct Model { + // total vehicle mass + float mass = 9.07441; // kg + + // reference area + float Sref = 0.92762; // m^2 + + float refSpan = 1.827411; // m + float refChord = 0.507614; // m + float IXX = 0.234; // kg-m^2 + float IYY = 1.85; // kg-m^2 + float IZZ = 2.04; // kg-m^2 + + // CN is coefficients for forces on +Z axis + // quadratic in alpharad + float CN2 = -0.5771; + float CN1 = 3.9496; + float CN0 = 0; + + // CA is the coefficients for forces on +X axis + // quadratic in alpharad + float CA2 = -1.6809; + float CA1 = -0.0057; + float CA0 = 0.0150; + + // CY is the coefficients for forces on the +Y axis + // quadratic in alpharad, with betarad factor + float CY2 = -3.342; + float CY1 = 0.0227; + float CY0 = -0.4608; + + // Cl is the coefficients for moments on X axis + // quadratic in alpharad, with betarad factor + float Cl2 = 0.2888; + float Cl1 = -0.8518; + float Cl0 = -0.0491; + + // Cm is the coefficients for moments on Y axis + // quadratic in alpharad + float Cm2 = 0.099; + float Cm1 = -0.6506; + float Cm0 = -0.0005; + + // Cn is the coefficients for moments on Z axis + // quadratic in alpharad, with betarad factor + float Cn2 = 0.0057; + float Cn1 = -0.0101; + float Cn0 = 0.1744; + + // controls neutral dynamic derivatives + // p, q, r are gyro rates + float Cmq = -6.1866; + + float Clp2 = 0.156; + float Clp1 = 0.0129; + float Clp0 = -0.315; + + float Clr2 = -0.0284; + float Clr1 = 0.2641; + float Clr0 = 0.0343; + + float Cnp2 = 0.0199; + float Cnp1 = -0.315; + float Cnp0 = -0.013; + + float Cnr2 = 0.1297; + float Cnr1 = 0.0343; + float Cnr0 = -0.264; + + // elevator + float elevatorDeflectionLimitDeg = -12.5; + float deltaCNperRadianElev = -0.7; + float deltaCAperRadianElev = 0.12; + float deltaCmperRadianElev = 1.39; + float deltaCYperRadianElev = 0; + float deltaClperRadianElev = 0; + float deltaCnperRadianElev = 0; + + // rudder + float rudderDeflectionLimitDeg = 18.0; + float deltaCNperRadianRud = 0; + float deltaCAperRadianRud = 0.058; + float deltaCmperRadianRud = 0; + float deltaCYperRadianRud = 0.31; + float deltaClperRadianRud = 0.038; + float deltaCnperRadianRud = -0.174; + + // aileron + float aileronDeflectionLimitDeg = 15.5; + float deltaCNperRadianAil = 0; + float deltaCAperRadianAil = 0.016; + float deltaCmperRadianAil = 0; + float deltaCYperRadianAil = -0.015; + + // quadratic in alpharad + float deltaClperRadianAil0 = 0.09191; + float deltaClperRadianAil1 = 0.0001; + float deltaClperRadianAil2 = -0.08645; + + // quadratic in alpharad + float deltaCnperRadianAil0 = 0.00789; + float deltaCnperRadianAil1 = 0.00773; + float deltaCnperRadianAil2 = -0.01162; + + // Forces in the +X direction are –CA * q * Sref + // Forces in the +Y direction are +CY * q * Sref + // Forces in the +Z direction are –CN * q *Sref + // Moments about the X axis are +Cl * q * Sref * RefSpan + // Moments about the Y axis are +Cm * q * Sref * RefChord + // Moments about the Z axis are +Cn * q * Sref * RefSpan + + // low altitude + float alphaRadMax = 0.209; + float betaRadMax = 0.209; + + // balloon launch parameters + float tetherLength = 50.0f; // length of tether from balloon to aircraft (m) + float tetherPogoFreq = 2.0f; // measured vertical frequency of on tether (Hz) + + } model; + + Vector3f getForce(float inputAileron, float inputElevator, float inputRudder); + Vector3f getTorque(float inputAileron, float inputElevator, float inputRudder, const Vector3f &force) const; + bool update_balloon(float balloon, Vector3f &force, Vector3f &rot_accel); + void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); + + Vector3f balloon_velocity; // balloon velocity NED + Vector3f balloon_position{0.0f, 0.0f, -45.0f}; // balloon position NED from origin + + enum class carriageState { + NONE = 0, // no carriage option available + WAITING_FOR_PICKUP = 1, // in launch cradle waiting to be picked up by launch vehicle + WAITING_FOR_RELEASE = 2, // being carried by luanch vehicle waitng to be released + PRE_RELEASE = 3, // had been released by launch vehicle + RELEASED = 4 // had been released by launch vehicle + } carriage_state; + bool plane_air_release; // true when plane has separated from the airborne launching platform + + uint32_t last_drag_ms; + float sim_LD; +}; + +} // namespace SITL + +#endif // AP_SIM_GLIDER_ENABLED diff --git a/libraries/SITL/SIM_StratoBlimp.cpp b/libraries/SITL/SIM_StratoBlimp.cpp index 66bfcc837a..915fceb7a1 100644 --- a/libraries/SITL/SIM_StratoBlimp.cpp +++ b/libraries/SITL/SIM_StratoBlimp.cpp @@ -29,7 +29,7 @@ using namespace SITL; extern const AP_HAL::HAL& hal; -// SITL Ship parameters +// SITL StratoBlimp parameters const AP_Param::GroupInfo StratoBlimp::var_info[] = { // @Param: MASS // @DisplayName: mass diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 2764ccef67..d5f6a00192 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -97,3 +97,7 @@ #ifndef AP_SIM_STRATOBLIMP_ENABLED #define AP_SIM_STRATOBLIMP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif + +#ifndef AP_SIM_GLIDER_ENABLED +#define AP_SIM_GLIDER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 156745a1cc..7ca40de3d9 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -38,6 +38,7 @@ #endif // SFML_JOYSTICK #include "SIM_StratoBlimp.h" +#include "SIM_Glider.h" extern const AP_HAL::HAL& hal; @@ -86,7 +87,11 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Units: m/s // @User: Advanced AP_GROUPINFO("WIND_TURB", 11, SIM, wind_turbulance, 0), - AP_GROUPINFO("SERVO_SPEED", 16, SIM, servo_speed, 0.14), + + // @Group: SERVO_ + // @Path: ./ServoModel.cpp + AP_SUBGROUPINFO(servo, "SERVO_", 16, SIM, ServoParams), + AP_GROUPINFO("SONAR_ROT", 17, SIM, sonar_rot, Rotation::ROTATION_PITCH_270), // @Param: BATT_VOLTAGE // @DisplayName: Simulated battery voltage @@ -1203,7 +1208,13 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { // @Path: ./SIM_StratoBlimp.cpp AP_SUBGROUPPTR(stratoblimp_ptr, "SB_", 2, SIM::ModelParm, StratoBlimp), #endif - + +#if AP_SIM_GLIDER_ENABLED + // @Group: GLD_ + // @Path: ./SIM_Glider.cpp + AP_SUBGROUPPTR(glider_ptr, "GLD_", 3, SIM::ModelParm, Glider), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 8505391aaa..9939fb10c0 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -49,6 +49,7 @@ struct float_array { }; class StratoBlimp; +class Glider; struct sitl_fdm { // this is the structure passed between FDM models and the main SITL code @@ -183,7 +184,6 @@ public: AP_Int8 mag_orient[HAL_COMPASS_MAX_SENSORS]; // external compass orientation AP_Int8 mag_fail[HAL_COMPASS_MAX_SENSORS]; // fail magnetometer, 1 for no data, 2 for freeze AP_Int8 mag_save_ids; - AP_Float servo_speed; // servo speed in seconds AP_Float sonar_glitch;// probability between 0-1 that any given sonar sample will read as max distance AP_Float sonar_noise; // in metres @@ -297,6 +297,18 @@ public: }; AirspeedParm airspeed[AIRSPEED_MAX_SENSORS]; + class ServoParams { + public: + ServoParams(void) { + AP_Param::setup_object_defaults(this, var_info); + } + static const struct AP_Param::GroupInfo var_info[]; + AP_Float servo_speed; // servo speed in seconds per 60 degrees + AP_Float servo_delay; // servo delay in seconds + AP_Float servo_filter; // servo 2p filter in Hz + }; + ServoParams servo; + // physics model parameters class ModelParm { public: @@ -306,6 +318,9 @@ public: #endif #if AP_SIM_SHIP_ENABLED ShipSim shipsim; +#endif +#if AP_SIM_GLIDER_ENABLED + Glider *glider_ptr; #endif }; ModelParm models; diff --git a/libraries/SITL/ServoModel.cpp b/libraries/SITL/ServoModel.cpp new file mode 100644 index 0000000000..deaff68b70 --- /dev/null +++ b/libraries/SITL/ServoModel.cpp @@ -0,0 +1,136 @@ +/* + 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 . + */ +/* + simple model of a servo. Model is: + + - time delay for transport protocol delay + - slew limit + - 2-pole butterworth +*/ + +#include "ServoModel.h" +#include "SITL.h" + +// SITL servo model parameters +const AP_Param::GroupInfo SITL::SIM::ServoParams::var_info[] = { + // @Param: SPEED + // @DisplayName: servo speed + // @Description: servo speed (time for 60 degree deflection). If DELAY and FILTER are not set then this is converted to a 1p lowpass filter. If DELAY or FILTER are set then this is treated as a rate of change limit + // @Units: s + AP_GROUPINFO("SPEED", 1, ServoParams, servo_speed, 0.14), + + // @Param: DELAY + // @DisplayName: servo delay + // @Description: servo delay + // @Units: s + AP_GROUPINFO("DELAY", 2, ServoParams, servo_delay, 0.0), + + // @Param: FILTER + // @DisplayName: servo filter + // @Description: servo filter + // @Units: Hz + AP_GROUPINFO("FILTER", 3, ServoParams, servo_filter, 0), + + AP_GROUPEND +}; + +/* + simpler filter used when SIM_SERVO_FILTER and SIM_SERVO_DELAY are not set + this filter is a 1p low pass based on SIM_SERVO_SPEED + */ +float ServoModel::apply_simple_filter(float v, float dt) +{ + const auto *sitl = AP::sitl(); + if (!is_positive(sitl->servo.servo_speed)) { + return v; + } + const float cutoff = 1.0f / (2 * M_PI * sitl->servo.servo_speed); + filter1p.set_cutoff_frequency(cutoff); + return filter1p.apply(v, dt); +} + +/* + apply a filter for a servo model consistting of a delay, speed and 2p filter + */ +float ServoModel::apply_filter(float v, float dt) +{ + const auto *sitl = AP::sitl(); + if (!sitl) { + return v; + } + + if (!is_positive(sitl->servo.servo_delay) && + !is_positive(sitl->servo.servo_filter)) { + // fallback to a simpler 1p filter model + return apply_simple_filter(v, dt); + } + + // apply delay + if (sitl->servo.servo_delay > 0) { + uint32_t delay_len = MAX(1,sitl->servo.servo_delay * sitl->loop_rate_hz); + if (!delay) { + delay = new ObjectBuffer(); + } + if (delay->get_size() != delay_len) { + delay->set_size(delay_len); + } + while (delay->space() > 0) { + delay->push(v); + } + IGNORE_RETURN(delay->pop(v)); + } + + // apply slew limit + if (sitl->servo.servo_speed > 0) { + // assume SIM_SERVO_SPEED is time for 60 degrees + float time_per_degree = sitl->servo.servo_speed / 60.0; + float proportion_per_second = 1.0 / (angle_deg * time_per_degree); + delta_max = proportion_per_second * dt; + v = constrain_float(v, last_v-delta_max, last_v+delta_max); + v = constrain_float(v, -1, 1); + last_v = v; + } + + // apply filter + if (sitl->servo.servo_filter > 0) { + filter.set_cutoff_frequency(sitl->loop_rate_hz, sitl->servo.servo_filter); + v = filter.apply(v); + } + + return v; +} + +float ServoModel::filter_range(uint16_t pwm, float dt) +{ + const float v = (pwm - pwm_min)/float(pwm_max - pwm_min); + return apply_filter(v, dt); +} + +float ServoModel::filter_angle(uint16_t pwm, float dt) +{ + const float v = (pwm - 0.5*(pwm_max+pwm_min))/(0.5*float(pwm_max - pwm_min)); + return apply_filter(v, dt); +} + +void ServoModel::set_pwm_range(uint16_t _pwm_min, uint16_t _pwm_max) +{ + pwm_min = _pwm_min; + pwm_max = _pwm_max; +} + +void ServoModel::set_deflection(float _angle_deg) +{ + angle_deg = fabsf(_angle_deg); +} diff --git a/libraries/SITL/ServoModel.h b/libraries/SITL/ServoModel.h new file mode 100644 index 0000000000..941f8a0312 --- /dev/null +++ b/libraries/SITL/ServoModel.h @@ -0,0 +1,51 @@ +/* + 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 . + */ +/* + simple model of a servo. Model is: + + - time delay for transport protocol delay + - slew limit + - 2-pole butterworth +*/ + +#include +#include +#include +#include + + +class ServoModel { +public: + float filter_angle(uint16_t pwm, float dt); + float filter_range(uint16_t pwm, float dt); + void set_pwm_range(uint16_t _pwm_min, uint16_t _pwm_max); + void set_deflection(float _angle_deg); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + float apply_filter(float v, float dt); + float apply_simple_filter(float v, float dt); + + LowPassFilter2pFloat filter; + LowPassFilterFloat filter1p; + ObjectBuffer *delay; + float last_v; + float delta_max; + uint16_t pwm_min = 1000; + uint16_t pwm_max = 2000; + float angle_deg = 45.0; +}; +