X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FUFO.cxx;h=edb56c0bbac1d5b96606c53815bb4a33e902fc6e;hb=e10b4934212bd25c7f6a17fb9078a44cedfbff8d;hp=c741505de07102851d4d4ef73279fddb0ca89bd5;hpb=0b84cddee737e9326168647b3ec51678895e300c;p=flightgear.git diff --git a/src/FDM/UFO.cxx b/src/FDM/UFO.cxx index c741505de..edb56c0bb 100644 --- a/src/FDM/UFO.cxx +++ b/src/FDM/UFO.cxx @@ -26,8 +26,6 @@ #endif #include -#include -#include #include #include
@@ -35,25 +33,30 @@ #include "UFO.hxx" -const double throttle_damp = 0.2; -const double aileron_damp = 0.05; -const double elevator_damp = 0.05; -const double elevator_trim_damp = 0.05; -const double rudder_damp = 0.4; - -FGUFO::FGUFO( double dt ) - : Throttle(0.0), - Aileron(0.0), - Elevator(0.0), - Elevator_Trim(0.0), - Rudder(0.0), +double FGUFO::lowpass::_dt; + + +FGUFO::FGUFO( double dt ) : + Throttle(new lowpass(fgGetDouble("/controls/damping/throttle", 0.1))), + Aileron(new lowpass(fgGetDouble("/controls/damping/aileron", 0.65))), + Elevator(new lowpass(fgGetDouble("/controls/damping/elevator", 0.65))), + Rudder(new lowpass(fgGetDouble("/controls/damping/rudder", 0.05))), + Aileron_Trim(new lowpass(fgGetDouble("/controls/damping/aileron-trim", 0.65))), + Elevator_Trim(new lowpass(fgGetDouble("/controls/damping/elevator-trim", 0.65))), + Rudder_Trim(new lowpass(fgGetDouble("/controls/damping/rudder-trim", 0.05))), Speed_Max(fgGetNode("/engines/engine/speed-max-mps", true)) { -// set_delta_t( dt ); } FGUFO::~FGUFO() { + delete Throttle; + delete Aileron; + delete Elevator; + delete Rudder; + delete Aileron_Trim; + delete Elevator_Trim; + delete Rudder_Trim; } @@ -68,97 +71,73 @@ void FGUFO::init() { // Run an iteration of the EOM (equations of motion) void FGUFO::update( double dt ) { - // cout << "FGLaRCsim::update()" << endl; if (is_suspended()) - return; + return; + lowpass::set_delta(dt); double time_step = dt; + FGControls *ctrl = globals->get_controls(); // read the throttle - double th = globals->get_controls()->get_throttle( 0 ); - if ( globals->get_controls()->get_brake_left() > 0.5 - || globals->get_controls()->get_brake_right() > 0.5 ) - { - th = -th; - } - Throttle = th * throttle_damp + Throttle * (1 - throttle_damp); - - // read the state of the control surfaces - Aileron = globals->get_controls()->get_aileron() * aileron_damp - + Aileron * (1 - aileron_damp); - Elevator = globals->get_controls()->get_elevator() * elevator_damp - + Elevator * (1 - elevator_damp); - Elevator_Trim = globals->get_controls()->get_elevator_trim() - * elevator_trim_damp - + Elevator_Trim * (1 - elevator_trim_damp); - Rudder = globals->get_controls()->get_rudder() * rudder_damp - + Rudder * (1 - rudder_damp); - - // the velocity of the aircraft - double velocity = Throttle * Speed_Max->getDoubleValue(); // meters/sec + double throttle = ctrl->get_throttle( 0 ); + double brake_left = ctrl->get_brake_left(); + double brake_right = ctrl->get_brake_right(); + + if (brake_left > 0.5 || brake_right > 0.5) + throttle = -throttle; + + double velocity = Throttle->filter(throttle) * Speed_Max->getDoubleValue(); // meters/sec + + + // read and lowpass-filter the state of the control surfaces + double aileron = Aileron->filter(ctrl->get_aileron()); + double elevator = Elevator->filter(ctrl->get_elevator()); + double rudder = Rudder->filter(ctrl->get_rudder()); + + aileron += Aileron_Trim->filter(ctrl->get_aileron_trim()); + elevator += Elevator_Trim->filter(ctrl->get_elevator_trim()); + rudder += Rudder_Trim->filter(ctrl->get_rudder_trim()); double old_pitch = get_Theta(); - double pitch_rate = SGD_PI_4; // assume I will be pitching up - double target_pitch = (-Elevator - Elevator_Trim) * SGD_PI_2; + double pitch_rate = SGD_PI_4; // assume I will be pitching up + double target_pitch = -elevator * SGD_PI_2; - // if I am pitching down - if (old_pitch > target_pitch) - // set the pitch rate to negative (down) + if (old_pitch > target_pitch) // pitching down pitch_rate *= -1; double pitch = old_pitch + (pitch_rate * time_step); - // if I am pitching up - if (pitch_rate > 0.0) - { - // clip the pitch at the limit - if ( pitch > target_pitch) - { + if (pitch_rate > 0.0) { // pitching up + if (pitch > target_pitch) pitch = target_pitch; - } - } - // if I am pitching down - else if (pitch_rate < 0.0) - { - // clip the pitch at the limit - if ( pitch < target_pitch) - { + + } else if (pitch_rate < 0.0) { // pitching down + if (pitch < target_pitch) pitch = target_pitch; - } } - double old_roll = get_Phi(); - double roll_rate = SGD_PI_4; - double target_roll = Aileron * SGD_PI_2; + double old_roll = get_Phi(); + double roll_rate = SGD_PI_4; + double target_roll = aileron * SGD_PI_2; if (old_roll > target_roll) roll_rate *= -1; - + double roll = old_roll + (roll_rate * time_step); - // if I am rolling CW - if (roll_rate > 0.0) - { - // clip the roll at the limit - if ( roll > target_roll) - { + if (roll_rate > 0.0) { // rolling CW + if (roll > target_roll) roll = target_roll; - } - } - // if I am rolling CCW - else if (roll_rate < 0.0) - { - // clip the roll at the limit - if ( roll < target_roll) - { + + } else if (roll_rate < 0.0) { // rolling CCW + if (roll < target_roll) roll = target_roll; - } } // the vertical speed of the aircraft double real_climb_rate = sin (pitch) * SG_METER_TO_FEET * velocity; // feet/sec - _set_Climb_Rate( -Elevator * 10.0 ); + _set_Climb_Rate( -elevator * 10.0 ); double climb = real_climb_rate * time_step; // the lateral speed of the aircraft @@ -172,19 +151,18 @@ void FGUFO::update( double dt ) { // angle of turn double turn_rate = sin(roll) * SGD_PI_4; // radians/sec double turn = turn_rate * time_step; - double yaw = fabs(Rudder) < .2 ? 0.0 : Rudder / (25 + fabs(speed) * .1); + double yaw = fabs(rudder) < .05 ? 0.0 : (rudder * (fabs(rudder) - 0.05) / 10); // update (lon/lat) position - double lat2, lon2, az2; + double lat2 = 0.0, lon2 = 0.0, az2 = 0.0; if ( fabs(speed) > SG_EPSILON ) { - geo_direct_wgs_84 ( get_Altitude(), - get_Latitude() * SGD_RADIANS_TO_DEGREES, - get_Longitude() * SGD_RADIANS_TO_DEGREES, - get_Psi() * SGD_RADIANS_TO_DEGREES, - dist, &lat2, &lon2, &az2 ); - - _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS ); - _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS ); + geo_direct_wgs_84 ( get_Altitude(), + get_Latitude() * SGD_RADIANS_TO_DEGREES, + get_Longitude() * SGD_RADIANS_TO_DEGREES, + get_Psi() * SGD_RADIANS_TO_DEGREES, + dist, &lat2, &lon2, &az2 ); + + _set_Geodetic_Position( lat2 * SGD_DEGREES_TO_RADIANS, lon2 * SGD_DEGREES_TO_RADIANS ); } // cout << "lon error = " << fabs(end.x()*SGD_RADIANS_TO_DEGREES - lon2) @@ -199,8 +177,8 @@ void FGUFO::update( double dt ) { _set_Euler_Angles(roll, pitch, heading); _set_Euler_Rates(0,0,0); - _set_Geocentric_Position( lat_geoc, get_Longitude(), - sl_radius + get_Altitude() + climb ); + _set_Geocentric_Position( lat_geoc, get_Longitude(), + sl_radius + get_Altitude() + climb ); // cout << "sea level radius (ft) = " << sl_radius << endl; // cout << "(setto) sea level radius (ft) = " << get_Sea_level_radius() << endl; _update_ground_elev_at_pos(); @@ -212,3 +190,5 @@ void FGUFO::update( double dt ) { set_V_east(sin(heading) * velocity * SG_METER_TO_FEET); set_V_down(-real_climb_rate); } + +