SITL: added Glider SITL model
This commit is contained in:
Andrew Tridgell
parent
4370d2e348
commit
53c0619a9a
@ -37,6 +37,7 @@
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <AP_JSON/AP_JSON.h>
|
||||
#include <AP_Filesystem/AP_Filesystem.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
||||
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
|
||||
|
||||
@ -37,6 +37,7 @@
|
||||
#include "SIM_Battery.h"
|
||||
#include <Filter/Filter.h>
|
||||
#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;
|
||||
|
||||
412
libraries/SITL/SIM_Glider.cpp
Normal file
412
libraries/SITL/SIM_Glider.cpp
Normal file
@ -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 <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
|
||||
203
libraries/SITL/SIM_Glider.h
Normal file
203
libraries/SITL/SIM_Glider.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
Glider model for high altitude balloon drop
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "SIM_config.h"
|
||||
|
||||
#if AP_SIM_GLIDER_ENABLED
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
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
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
};
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
136
libraries/SITL/ServoModel.cpp
Normal file
136
libraries/SITL/ServoModel.cpp
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
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<float>();
|
||||
}
|
||||
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);
|
||||
}
|
||||
51
libraries/SITL/ServoModel.h
Normal file
51
libraries/SITL/ServoModel.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
simple model of a servo. Model is:
|
||||
|
||||
- time delay for transport protocol delay
|
||||
- slew limit
|
||||
- 2-pole butterworth
|
||||
*/
|
||||
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
|
||||
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<float> *delay;
|
||||
float last_v;
|
||||
float delta_max;
|
||||
uint16_t pwm_min = 1000;
|
||||
uint16_t pwm_max = 2000;
|
||||
float angle_deg = 45.0;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user