#endif
#include <simgear/math/sg_geodesy.hxx>
-#include <simgear/math/point3d.hxx>
-#include <simgear/math/polar3d.hxx>
#include <Aircraft/controls.hxx>
#include <Main/globals.hxx>
#include "UFO.hxx"
+double FGUFO::lowpass::_dt;
+
FGUFO::FGUFO( double dt ) :
- Throttle(new lowpass(fgGetDouble("/controls/damping/throttle", 0.3))),
+ 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))),
// Run an iteration of the EOM (equations of motion)
void FGUFO::update( double dt ) {
- // cout << "FGLaRCsim::update()" << endl;
if (is_suspended())
return;
+ lowpass::set_delta(dt);
double time_step = dt;
+ FGControls *ctrl = globals->get_controls();
// read the throttle
- double throttle = globals->get_controls()->get_throttle( 0 );
- double brake_left = globals->get_controls()->get_brake_left();
- double brake_right = globals->get_controls()->get_brake_right();
+ 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;
- throttle = Throttle->filter(dt, 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(dt, globals->get_controls()->get_aileron());
- double elevator = Elevator->filter(dt, globals->get_controls()->get_elevator());
- double rudder = Rudder->filter(dt, globals->get_controls()->get_rudder());
- aileron += Aileron_Trim->filter(dt, globals->get_controls()->get_aileron_trim());
- elevator += Elevator_Trim->filter(dt, globals->get_controls()->get_elevator_trim());
- rudder += Rudder_Trim->filter(dt, globals->get_controls()->get_rudder_trim());
+ // 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());
- double velocity = throttle * Speed_Max->getDoubleValue(); // meters/sec
+ 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 kts = velocity * SG_METER_TO_NM * 3600.0;
_set_V_equiv_kts( kts );
_set_V_calibrated_kts( kts );
- _set_V_ground_speed( kts );
+ set_V_ground_speed_kt( kts );
// 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_Psi() * SGD_RADIANS_TO_DEGREES,
dist, &lat2, &lon2, &az2 );
- _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS );
- _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS );
+ _set_Geodetic_Position( lat2 * SGD_DEGREES_TO_RADIANS, lon2 * SGD_DEGREES_TO_RADIANS );
}
// cout << "lon error = " << fabs(end.x()*SGD_RADIANS_TO_DEGREES - lon2)
sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc );
// update euler angles
- double heading = fmod(get_Psi() + turn + yaw, SGD_2PI);
+ double heading = SGMiscd::normalizePeriodic(0, SGD_2PI, get_Psi() + turn + yaw );
_set_Euler_Angles(roll, pitch, heading);
_set_Euler_Rates(0,0,0);