diff --git a/data/aircrafts/as350.3d b/data/aircrafts/as350.3d index 03aca39..7e8b5d5 100644 --- a/data/aircrafts/as350.3d +++ b/data/aircrafts/as350.3d @@ -66,6 +66,9 @@ belly_height 1.7 # Length in meters length 10.92 +# Rotor diameter in meters +rotor_diameter 10.8966 + # Gear (down position) to belly height in meters gear_height 0.35 diff --git a/data/aircrafts/b47.3d b/data/aircrafts/b47.3d index b0dd1cc..7cd9936 100644 --- a/data/aircrafts/b47.3d +++ b/data/aircrafts/b47.3d @@ -60,6 +60,9 @@ belly_height 0.5 # Length in meters length 9.63 +# Rotor diameter in meters +rotor_diameter 9.4742 + # Gear (down position) to belly height in meters gear_height 0.0 diff --git a/data/aircrafts/ec145.3d b/data/aircrafts/ec145.3d index dac6766..711a3b6 100644 --- a/data/aircrafts/ec145.3d +++ b/data/aircrafts/ec145.3d @@ -55,6 +55,9 @@ belly_height 1.55 # Length, in meters length 13.03 +# Rotor diameter in meters +rotor_diameter 11 + # Gear (down position) to belly height, in meters gear_height 0.44 diff --git a/data/aircrafts/hh60.3d b/data/aircrafts/hh60.3d index 6c75eb7..71bc94f 100644 --- a/data/aircrafts/hh60.3d +++ b/data/aircrafts/hh60.3d @@ -71,6 +71,9 @@ belly_height 2.2 # Length in meters length 19.81 +# Rotor diameter in meters +rotor_diameter 16.4592 + # Gear (down position) to belly height in meters gear_height 0.5 diff --git a/data/aircrafts/hh65.3d b/data/aircrafts/hh65.3d index 545a0bb..4f69cc6 100644 --- a/data/aircrafts/hh65.3d +++ b/data/aircrafts/hh65.3d @@ -69,6 +69,9 @@ belly_height 2.38 # Length in meters length 13.54 +# Rotor diameter in meters +rotor_diameter 11.938 + # Gear (down position) to belly height in meters gear_height 0.35 diff --git a/data/aircrafts/ka27.3d b/data/aircrafts/ka27.3d index dda95e8..94490cd 100644 --- a/data/aircrafts/ka27.3d +++ b/data/aircrafts/ka27.3d @@ -67,6 +67,9 @@ belly_height 2.35 # Length in meters length 10.49 +# Rotor diameter in meters +rotor_diameter 16.637 + # Gear (down position) to belly height in meters gear_height 0.3 diff --git a/data/aircrafts/s64.3d b/data/aircrafts/s64.3d index d5e855a..64e3226 100644 --- a/data/aircrafts/s64.3d +++ b/data/aircrafts/s64.3d @@ -61,6 +61,9 @@ belly_height 3.55 # Length in meters length 21.41 +# Rotor diameter in meters +rotor_diameter 21.9456 + # Gear (down position) to belly height in meters gear_height 0.45 diff --git a/data/aircrafts/uh1d.3d b/data/aircrafts/uh1d.3d index 3493dd0..74b3ea9 100644 --- a/data/aircrafts/uh1d.3d +++ b/data/aircrafts/uh1d.3d @@ -60,6 +60,9 @@ belly_height 1.7 # Length in meters length 12.75 +# Rotor diameter in meters +rotor_diameter 14.7828 + # Gear (down position) to belly height in meters gear_height 0.3 diff --git a/data/aircrafts/v22.3d b/data/aircrafts/v22.3d index d1dda1c..827dd0c 100644 --- a/data/aircrafts/v22.3d +++ b/data/aircrafts/v22.3d @@ -70,6 +70,9 @@ belly_height 2.8 # Length in meters length 17.47 +# Rotor diameter in meters +rotor_diameter 11.5824 + # Wingspan in meters wingspan 15.24 diff --git a/src/cmdset.c b/src/cmdset.c index c09f7e6..3ea688e 100644 --- a/src/cmdset.c +++ b/src/cmdset.c @@ -579,6 +579,17 @@ void SARCmdSet(SAR_CMD_PROTOTYPE) got_match = True; } } + /* Rotor diameter */ + else if(!strcasecmp(parm, "rotor_diameter")) + { + sar_object_aircraft_struct *obj_aircraft_ptr = + SAR_OBJ_GET_AIRCRAFT(obj_ptr); + if(obj_aircraft_ptr != NULL) + { + obj_aircraft_ptr->rotor_diameter = (float)ATOF(val); + got_match = True; + } + } /* Gear height */ else if(!strcasecmp(parm, "gear_height")) { diff --git a/src/obj.h b/src/obj.h index edb466e..c8c5ea2 100644 --- a/src/obj.h +++ b/src/obj.h @@ -861,6 +861,9 @@ typedef struct { /* Wingspan of aircraft in meters */ float wingspan; + /* Rotor diameter in meters */ + float rotor_diameter; + /* Height of landing gear in meters */ float gear_height; @@ -961,6 +964,9 @@ typedef struct { */ float center_to_ground_height; + /* Rate of heading change due to torque (radians/cycle) */ + float torque_velocity; + /* Moveable parts */ sar_obj_part_struct **part; diff --git a/src/objio.c b/src/objio.c index a3cb161..163f6ca 100644 --- a/src/objio.c +++ b/src/objio.c @@ -3245,6 +3245,19 @@ static void SARObjLoadLine( if(aircraft != NULL) aircraft->wingspan = wingspan; } + else if(!strcasecmp(parm, "rotor_diameter")) + { + /* Arguments: + * + * + */ + float rotor_diameter = 0.0f; + + arg = GET_ARG_F(arg, &rotor_diameter); + + if(aircraft != NULL) + aircraft->rotor_diameter = rotor_diameter; + } /* Landing Gear Height */ else if(!strcasecmp(parm, "gear_height")) { diff --git a/src/sfm.h b/src/sfm.h index 9d699f7..c640f60 100644 --- a/src/sfm.h +++ b/src/sfm.h @@ -42,6 +42,18 @@ */ #define SFMDefaultGravity 9.80665 + +/* + * Effective transactional lift speed value in m/s. + */ +#define SFMETLSpeed 12.35 // 24 knots + +/* + * Transverse Flow Effect (TF) values in m/s. + */ +#define SFMTFStart 2.57 // 5 knots +#define SFMTFEnd 12.86 // 25 knots + /* * Core structure: */ diff --git a/src/sfmmodel.c b/src/sfmmodel.c index 73c8a6e..060bed0 100644 --- a/src/sfmmodel.c +++ b/src/sfmmodel.c @@ -297,6 +297,10 @@ SFMBoolean SFMModelChangeValues( { model->wingspan = value->wingspan; } + if(flags & SFMFlagRotorDiameter) + { + model->rotor_diameter = value->rotor_diameter; + } if(flags & SFMFlagGearState) { model->gear_state = value->gear_state; diff --git a/src/sfmmodel.h b/src/sfmmodel.h index a35661b..e1b4d15 100644 --- a/src/sfmmodel.h +++ b/src/sfmmodel.h @@ -78,6 +78,7 @@ #define SFMFlagStopped ((SFMFlags)1 << 41) #define SFMFlagLength ((SFMFlags)1 << 42) #define SFMFlagWingspan ((SFMFlags)1 << 43) +#define SFMFlagRotorDiameter ((SFMFlags)1 << 44) /* * Flight model types: @@ -128,7 +129,7 @@ typedef struct { */ SFMFlags flags; - int type; /* One of SFMFlightModel*. */ + int type; /* One of SFMFlightModel*. */ SFMPositionStruct position; /* Meters. */ SFMDirectionStruct direction; /* Radians. */ SFMPositionStruct velocity_vector; /* Meters/cycle. */ @@ -146,8 +147,9 @@ typedef struct { SFMPositionStruct accel_responsiveness; double ground_elevation_msl; /* Meters. */ double service_ceiling; /* Meters. */ - double length; /* Meters */ + double length; /* Meters */ double wingspan; /* Meters */ + double rotor_diameter; /* Meters */ double belly_height; /* Undercarrage to center, meters. */ SFMBoolean gear_state; /* True when down. */ int gear_type; /* One of SFMGearType*. */ @@ -165,11 +167,12 @@ typedef struct { * ground height, * in meters. */ + double torque_velocity; /* Radians/cycle internal */ double heading_control_coeff; /* -1.0 to 1.0. */ double pitch_control_coeff; /* -1.0 to 1.0. */ double bank_control_coeff; /* -1.0 to 1.0. */ double elevator_trim_coeff; /* -1.0 to 1.0. */ - double throttle_coeff; /* 0.0 to 1.0. */ + double throttle_coeff; /* 0.0 to 1.0. */ SFMBoolean after_burner_state; double after_burner_power_coeff; /* Times engine power. */ double engine_power; /* In kg * m / cycle^2. */ diff --git a/src/sfmsimforce.c b/src/sfmsimforce.c index 12e3961..bf0f15f 100644 --- a/src/sfmsimforce.c +++ b/src/sfmsimforce.c @@ -183,7 +183,7 @@ static SFMBoolean SFMForceCollisionCheck( model2 = realm->model[i]; if(model2 == NULL) continue; - + if(!(model2->flags & (SFMFlagPosition | SFMFlagCanCauseCrash | SFMFlagCrashContactShape | SFMFlagCrashableSizeRadius) @@ -195,7 +195,7 @@ static SFMBoolean SFMForceCollisionCheck( if(model2 == model) /* Skip itself */ continue; - /* Calculate distance in meters */ + /* Calculate distance in meters */ r = SFMHypot2( model2->position.x - pos->x, model2->position.y - pos->y @@ -230,7 +230,7 @@ static SFMBoolean SFMForceCollisionCheck( /* Callback deleted model? */ if(SFMModelInRealm(realm, model) < 0) - return(status); + return(status); } } else if((model2->crash_contact_shape == SFMCrashContactShapeCylendrical) && @@ -320,7 +320,7 @@ static SFMBoolean SFMForceCollisionCheck( } } } - + return(status); } #endif @@ -378,8 +378,8 @@ static int SFMForceApplySlew( time_compensation * time_compression; a[2] = 0.0; - /* Rotate position change so that it is relative to world - * cooridnates. + /* Rotate position change so that it is relative to world + * coordinates. */ MatrixRotateHeading3(a, dir->heading, r); @@ -716,7 +716,7 @@ int SFMForceApplyNatural( else { /* Pitched down so need to pitch up */ - dir->pitch -= pitch_leveling_rate * + dir->pitch -= pitch_leveling_rate * time_compensation * time_compression; if(dir->pitch < pitch_down_max) dir->pitch = pitch_down_max; @@ -738,43 +738,92 @@ int SFMForceApplyNatural( /* Pitched down so need to pitch up */ dir->pitch -= pitch_leveling_rate * time_compensation * time_compression; - if(dir->pitch < (0.0 * PI)) + if(dir->pitch < (0.0 * PI)) dir->pitch = (0.0 * PI); } } - /* Helicopter flight model in flight pitch attitude - * leveling (bank leveling will be applied later). - */ - else if(flags & SFMFlagAttitudeLevelingRate) + /* Helicopter flight model in flight */ + else { - double leveling_coeff; - - if(dir->pitch > (1.5 * PI)) + /* pitch attitude leveling (bank leveling will be applied later). */ + if(flags & SFMFlagAttitudeLevelingRate) { - leveling_coeff = MIN( - (dir->pitch - (1.5 * PI)) / (0.5 * PI), - 1.0 - ); - dir->pitch = MIN( - dir->pitch + (leveling_coeff * - model->attitude_leveling_rate.pitch * - time_compensation * time_compression), - 2.0 * PI - ); + double leveling_coeff; + + if(dir->pitch > (1.5 * PI)) + { + leveling_coeff = MIN( + (dir->pitch - (1.5 * PI)) / (0.5 * PI), + 1.0 + ); + dir->pitch = MIN( + dir->pitch + (leveling_coeff * + model->attitude_leveling_rate.pitch * + time_compensation * time_compression), + 2.0 * PI + ); + } + else if(dir->pitch < (0.5 * PI)) + { + leveling_coeff = MAX( + ((0.5 * PI) - dir->pitch) / (0.5 * PI), + 0.0 + ); + dir->pitch = MAX( + dir->pitch - (leveling_coeff * + model->attitude_leveling_rate.pitch * + time_compensation * time_compression), + 0.0 * PI + ); + } } - else if(dir->pitch < (0.5 * PI)) - { - leveling_coeff = MAX( - ((0.5 * PI) - dir->pitch) / (0.5 * PI), - 0.0 - ); - dir->pitch = MAX( - dir->pitch - (leveling_coeff * - model->attitude_leveling_rate.pitch * - time_compensation * time_compression), - 0.0 * PI + + /* The source of the next chunk of code is a sar2 fork by + * @metiscus that was manually ported and adapted: + * https://github.com/metiscus/sar2/commit/7e94bf4649791d0baa8b9cb5eb5593169be2fbee + * + * It improves upon the original code doing this, which + * has been removed: + + if(!model->landed_state) + dir->heading = SFMSanitizeRadians( + dir->heading + (sin_pitch * sin_bank * + (0.2 * PI) * + time_compensation * time_compression) + + * + * When the helicopter pitches forward and banks, the + * heading will then change. + * + * atan2() becomes smaller if significant Z speed (up or + * down) compared to the total airspeed (essentially + * hovering, since Z speeds are never too high). In such + * case, the helicopter yaws a little less when + * rolling. Other than that atan is usually around 0.8. + * + * The aircraft changes heading depending on the pitch + + * compensation from the atan2 above and the bank angle value. + * + * Original comment by @metiscus in this calculation was that: + * + * "relative_pitch is the actual helicopter pitch plus the + * inflow component of the airspeed. This ensures the + * helicopter will turn at a standard rate (15 degree bank 100 + * knots 3 degrees a second)" + * + * We have however reduced the PI multiplier to 0.15 + * instead of 0.2. + */ + + // Originally this was part of ArtificialForces, but + // now it is here (i don't remember the reason). It is a + // thrust-independent effect after all. + double airspeed_3d = SFMHypot3(airspeed->x, airspeed->y, airspeed->z); + double relative_pitch = sin(dir->pitch) + 0.5f * atan2(ABS(vel->x)+ABS(vel->y), airspeed_3d); + dir->heading = SFMSanitizeRadians( + dir->heading + (relative_pitch * sin(dir->bank) * + (0.15 * PI) * time_compensation * time_compression) ); - } } } break; @@ -824,7 +873,7 @@ int SFMForceApplyNatural( else { vel->x -= x_vel_ground_drag; - if(vel->x < 0.0) + if(vel->x < 0.0) vel->x = 0.0; } @@ -836,7 +885,7 @@ int SFMForceApplyNatural( vel->y = 0.0; } else - { + { vel->y -= x_vel_ground_drag; if(vel->y < 0.0) vel->y = 0.0; @@ -932,7 +981,7 @@ int SFMForceApplyNatural( 1.0 - (((2.0 * PI) - dir->bank) / (0.5 * PI)), 0.0 ); - dir->bank = dir->bank + (leveling_coeff * + dir->bank = dir->bank + (leveling_coeff * model->attitude_leveling_rate.bank * time_compensation * time_compression ); @@ -1185,11 +1234,10 @@ int SFMForceApplyArtificial( double net_weight = 0.0; /* net_mass * SAR_GRAVITY */ double ground_elevation_msl = model->ground_elevation_msl, center_to_gear_height = 0.0; - double airspeed_3d; + double airspeed_3d, airspeed_2d; double tc_min = MIN(realm->time_compensation, 1.0); - double time_compensation = realm->time_compensation; - double time_compression = realm->time_compression; + double time_compx = realm->time_compensation * realm->time_compression; SFMFlags flags = model->flags; SFMDirectionStruct *dir = &model->direction; @@ -1269,13 +1317,17 @@ int SFMForceApplyArtificial( } } + if(flags & SFMFlagAirspeedVector) + { + airspeed_3d = SFMHypot3(airspeed->x, airspeed->y, airspeed->z); + airspeed_2d = SFMHypot2(airspeed->x, airspeed->y); + } + /* Check if current speed is exceeding its maximum expected * speed (overspeed). */ if(flags & (SFMFlagSpeedMax | SFMFlagAirspeedVector)) { - /* Use all airspeeds. */ - airspeed_3d = SFMHypot3(airspeed->x, airspeed->y, airspeed->z); /* Current speed greater than expected overspeed? */ if(airspeed_3d > model->overspeed_expected) { @@ -1385,8 +1437,7 @@ int SFMForceApplyArtificial( if(!model->landed_state) dir->heading = SFMSanitizeRadians( dir->heading + (sin_bank * speed_coeff * - (0.03 * PI) * time_compensation * - time_compression) + (0.03 * PI) * time_compx) ); /* Calculate new thrust vector in meters per cycle, include @@ -1444,8 +1495,8 @@ int SFMForceApplyArtificial( /* Calculate new x y position: rotate speed to match scene * heading. */ - dic = vel->x * time_compensation * time_compression; - djc = vel->y * time_compensation * time_compression; + dic = vel->x * time_compx; + djc = vel->y * time_compx; SFMOrthoRotate2D(dir->heading, &dic, &djc); pos->x += dic; pos->y += djc; @@ -1453,7 +1504,7 @@ int SFMForceApplyArtificial( /* Calculate new z position (but it will actually be * set farther below). */ - dkc = vel->z * time_compensation * time_compression; + dkc = vel->z * time_compx; } break; @@ -1463,7 +1514,9 @@ int SFMForceApplyArtificial( if(flags & (SFMFlagPosition | SFMFlagDirection | SFMFlagVelocityVector | SFMFlagAirspeedVector | SFMFlagSpeedStall | SFMFlagSpeedMax | - SFMFlagAccelResponsiveness | SFMFlagLandedState) + SFMFlagAccelResponsiveness | SFMFlagLandedState | + SFMFlagBellyHeight + ) ) { double sin_pitch = sin(dir->pitch), @@ -1471,6 +1524,145 @@ int SFMForceApplyArtificial( sin_bank = sin(dir->bank), cos_bank = cos(dir->bank); + /* This is the speed vector relative to the rotor blades. + Used to calculate pitch and bank changes. It should match + airspeed vector when aircraft horizontal */ + SFMPositionStruct airspeed_rotor; + // Set rotor speed vector by rotating airspeeds according to + // pitch/bank/heading. Airspeed is already relative to the + // heading of the aircraft so we don't need to rotate + // heading. A rotor moving forward will just see y_speed and + // z_speed (because pitched). + double a[3 *1], r[3 * 1]; + a[0] = airspeed->x; + a[1] = airspeed->y; + a[2] = airspeed->z; + MatrixRotateBank3(a, -dir->bank, r); + MatrixRotatePitch3(r, dir->pitch, a); // re-use variable a + airspeed_rotor.x = a[0]; + airspeed_rotor.y = a[1]; + airspeed_rotor.z = a[2]; + + double airspeed_rotor_2d = SFMHypot2(airspeed_rotor.x, airspeed_rotor.y); + // fprintf(stderr, "airspeed_rotor. b: %.2f, p: %.2f, h: %.2f x: %.2f, y: %.2f, z: %.2f, 2d: %.2f\n", dir->bank, dir->pitch, dir->heading, a[0], a[1], a[2], airspeed_rotor_2d); + + /* IGE effect (In-Ground-Effect) adds more lift force when close to the ground. + * We effectively give thrust_output a maximum of 28% bonus in that region. + * + * Effect starts at a rotor height of 1.25 rotor diameters. + * http://www.copters.com/aero/ground_effect.html + */ + if(flags & SFMFlagRotorDiameter && model->rotor_diameter > 0) + { + // horizontallity_coeff dampens IGE based on how horizontal to the + // ground the aircraft is. It can be played with. + double horizontallity_coeff = POW(ABS(cos_pitch) * ABS(cos_bank),2); + double ige_height = 1.25 * model->rotor_diameter; + // The rotor is as high as the center of the aircraft plus the + // belly_height (assume distance from center to belly is the + // same as from center to rotor. + double rotor_height = ABS(pos->z - model->ground_elevation_msl + model->belly_height); + // Increases towards 1 when ground_elevation is lower. Non + // linear, approximate by the square. + double ige_coeff = (1 - POW(CLIP(rotor_height, 0, ige_height) / ige_height, 2)); + // Increase thrust output 28% at most. + // It is always assumed that the ground is horizontal and effect happens + // when we are parallel to ground. + thrust_output = (1 + 0.28 * ige_coeff * horizontallity_coeff) * thrust_output; + // fprintf(stderr, "IGE. hor_coeff: %.2f, coeff: %.2f, thrust_bonus: %.2f\n", horizontallity_coeff, ige_coeff, 1 + 0.28 * ige_coeff); + } + + /* Transverse Flow Effect (TF) happens from around 5 knots, + * reaches max magnitude at 15 knots and dissapears by 25 + * knots. When moving forward, the blades at the front work on + * clean air while the back work on downwards-accelerated air. + * It causes a roll to the right when moving forward due to + * ~90 degree phase shift. + * https://en.wikipedia.org/wiki/Transverse_flow_effect + */ + + // The effect starts at SFMTFStart and follows a sin wave + // incidence until it is 0 again at SFMTFEnd. Otherwise 0. + // sin((speed-effect_start)*PI / effect_speed_range) + if (!model->landed_state && airspeed_rotor_2d > 0) { + double tf_coeff = sin( + (CLIP(airspeed_rotor_2d, SFMTFStart, SFMTFEnd) - SFMTFStart) * PI / (SFMTFEnd - SFMTFStart)); + + double tf_coeff_bank = (airspeed_rotor.y / airspeed_rotor_2d) * tf_coeff; + double tf_coeff_pitch = -(airspeed_rotor.x / airspeed_rotor_2d) * tf_coeff; + dir->pitch = SFMSanitizeRadians(dir->pitch + tf_coeff_pitch * 0.08 * PI * time_compx); + dir->bank = SFMSanitizeRadians(dir->bank + tf_coeff_bank * 0.08 * PI * time_compx); + // fprintf(stderr, "TF. coeff: %.2f, coeff_bank: %.2f, coeff_pitch: %.2f\n", tf_coeff, tf_coeff_bank, tf_coeff_pitch); + } + + /* Effective Transactional Lift (ETL): as airspeed increases, air + * vortexes at the tip of the rotor blades dissappear, + * providing a bonus lift effect. Fully effective at + * SFMETLEnd (24 knots). Without ETL, we suffer a thrust + * penalty of up to 25%. + * + * ETL effect on the advancing side vs retreating side + * of the rotor, plus phase shift results in an additional + * pitch increase which needs to be compensated by the pilot, + * when flying forward. This would need calculating pitch/bank + * adjustments based on speed direction. + * + * https://en.wikipedia.org/wiki/Translational_lift + */ + // Goes from 1 (full penalty) to 0 when it reaches SFMETLSpeed + // Square progression so that it goes a bit slower close to 0. + double etl_thrust_coeff = 1 - POW(CLIP(airspeed_rotor_2d / SFMETLSpeed, 0, 1),2); + thrust_output = (1 - 0.25 * etl_thrust_coeff) * thrust_output; + + if (!model->landed_state && airspeed_rotor_2d > 0) { + // Similar to TF, we add some pitch/bank changes while + // entering ETL. The difference here is that nose rises + // when going forward, rather than causing a roll. By end + // of ETL we assume tail has compensated effect and + // dissappears. + double etl_pitch_bank_coeff = sin(MIN(airspeed_rotor_2d, SFMETLSpeed) * PI / SFMETLSpeed); + double etl_bank_coeff = -(airspeed_rotor.x / airspeed_rotor_2d) * etl_pitch_bank_coeff; + double etl_pitch_coeff = -(airspeed_rotor.y / airspeed_rotor_2d) * etl_pitch_bank_coeff; + dir->pitch = SFMSanitizeRadians( + dir->pitch + etl_pitch_coeff * 0.03 * PI * time_compx); + dir->bank = SFMSanitizeRadians( + dir->bank + etl_bank_coeff * 0.03 * PI * time_compx); + // fprintf(stderr, "ETL: speed: %.2f, thrust_coeff: %.2f, pb_coeff: %.2f, coeff_bank: %.2f, coeff_pitch: %.2f\n", airspeed_rotor_2d, etl_thrust_coeff, etl_pitch_bank_coeff, etl_bank_coeff, etl_pitch_coeff); + } + + /* Torque: as the motor pushes the rotor Counter-Clock-Wise, + * the aircraft is pushed Clock-Wise. At higher speeds the + * tail counteracts this but otherwise the pilot should use + * the tail rotor. We introduce a heading change to the + * right. The effect depends onn thottle and is maximum at 0 + * speed, reduces up to SFMETLEnd and is counteracted from + * there. + */ + + if(!model->landed_state) + { + // torque_coeff: 1 at 0-speed, 0 at SFMETLEnd and negative + // above that so that torque acceleration dissappears. + double torque_coeff = 1 - airspeed_2d / SFMETLSpeed; + double torque_accel; + // Remove effect faster than it appears. + if (torque_coeff > 0) { + torque_accel = torque_coeff * 0.2; + } else { + torque_accel = torque_coeff * 0.5; + } + + model->torque_velocity += torque_accel * time_compx; + if (model->torque_velocity<0) { + model->torque_velocity = 0.0; + } + double dir_change = atan(model->torque_velocity) * 0.1 * model->throttle_coeff * time_compx; + dir->heading += dir_change; + // fprintf(stderr, "torque accel: %f, torque_vel: %f, atan: %f, throttle_coeff: %f, dir_change: %f\n", torque_accel, model->torque_velocity, atan(model->torque_velocity), model->throttle_coeff, dir_change); + } else { + model->torque_velocity = 0.0; + } + /* To calculate movement offset, use the equation: * * (last_vel / compilation) + ((thrust_in_direction) / response) @@ -1482,32 +1674,20 @@ int SFMForceApplyArtificial( * applied, lower values increase the responsiveness. */ - /* Heading by pitch and bank caused by thrust direction. - * When the helicopter pitches forward and banks, the - * heading will then change. - */ - if(!model->landed_state) - dir->heading = SFMSanitizeRadians( - dir->heading + (sin_pitch * sin_bank * - (0.2 * PI) * - time_compensation * time_compression) - ); - - /* XY Plane: Pitch and bank applied force */ /* Calculate i force compoent (obj relative) */ di = (vel->x / 1.00) + (thrust_output * sin_bank * cos_pitch * tc_min / MAX(ar->x, 1.0)); - dic = di * time_compensation * time_compression; + dic = di * time_compx; vel->x = di; /* Calculate j force compoent (obj relative) */ dj = (vel->y / 1.00) + (thrust_output * sin_pitch * cos_bank * tc_min / MAX(ar->y, 1.0)); - djc = dj * time_compensation * time_compression; + djc = dj * time_compx; vel->y = dj; /* Set new positions after rotating the speeds to match @@ -1517,7 +1697,6 @@ int SFMForceApplyArtificial( pos->x += dic; pos->y += djc; - /* Vertical velocity */ /* dk = (vel->z / 1.60) + @@ -1547,7 +1726,7 @@ int SFMForceApplyArtificial( dk_dv = dk_new - dk_prev; /* Add the previous velocit to the new delta change - * in velocity multiplied by the time compensation + * in velocity multiplied by the time compensation * min. Make sure tc_min stays about 0.01 to ensure * that as this function cycles that it will converge * to dk. @@ -1558,7 +1737,7 @@ int SFMForceApplyArtificial( if((dk < 0.0) && model->landed_state) dk = 0.0; - dkc = dk * time_compensation * time_compression; + dkc = dk * time_compx; vel->z = dk; } break; @@ -1677,7 +1856,7 @@ int SFMForceApplyArtificial( if(vel->y < 0) { vel->y += wheel_brakes_power; - if(vel->y > 0) + if(vel->y > 0) vel->y = 0; } else @@ -1830,7 +2009,7 @@ int SFMForceApplyControl( dir->bank = SFMSanitizeRadians(dir->bank + PI); if(dir->pitch > (1.0 * PI)) dir->pitch = SFMSanitizeRadians( - (2.0 * PI) - dir->pitch + PI + (2.0 * PI) - dir->pitch + PI ); else dir->pitch = SFMSanitizeRadians( @@ -1939,7 +2118,7 @@ int SFMForceApplyControl( dir->bank = SFMSanitizeRadians(dir->bank + PI); if(dir->pitch > (1.0 * PI)) dir->pitch = SFMSanitizeRadians( - (2.0 * PI) - dir->pitch + PI + (2.0 * PI) - dir->pitch + PI ); else dir->pitch = SFMSanitizeRadians( @@ -1952,7 +2131,7 @@ int SFMForceApplyControl( dir->heading + (sin(dir->bank) * p_con_coeff * acr->pitch * -1 * time_compensation * time_compression) ); - + //printf("h_con_coeff: %f\n", h_con_coeff); /* Check if engines are on */ if(1) diff --git a/src/simop.c b/src/simop.c index e43ba5d..4d9c73e 100644 --- a/src/simop.c +++ b/src/simop.c @@ -1054,7 +1054,7 @@ void SARSimSetSFMValues( SFMFlagCrashContactShape | SFMFlagCrashableSizeRadius | SFMFlagCrashableSizeZMin | SFMFlagCrashableSizeZMax | SFMFlagTouchDownCrashResistance | SFMFlagCollisionCrashResistance | - SFMFlagStopped | SFMFlagLength | SFMFlagWingspan + SFMFlagStopped | SFMFlagLength | SFMFlagWingspan | SFMFlagRotorDiameter ); /* Update flight model type only if SFM not in slew mode */ @@ -1110,6 +1110,7 @@ void SARSimSetSFMValues( TAR_PTR->belly_height = SRC_PTR->belly_height; TAR_PTR->length = SRC_PTR->length; TAR_PTR->wingspan = SRC_PTR->wingspan; + TAR_PTR->rotor_diameter = SRC_PTR->rotor_diameter; TAR_PTR->ground_elevation_msl = obj_ptr->ground_elevation_msl; TAR_PTR->gear_state = (SFMBoolean)((lgear_ptr != NULL) ? (lgear_ptr->flags & SAR_OBJ_PART_FLAG_STATE) : False @@ -1289,6 +1290,8 @@ void SARSimGetSFMValues(sar_scene_struct *scene, sar_object_struct *obj_ptr) TAR_PTR->center_to_ground_height = (float)SRC_PTR->center_to_ground_height; + TAR_PTR->torque_velocity = (float)SRC_PTR->torque_velocity; + /* Get z acceleration before updating velocity */ TAR_PTR->z_accel = (float)SRC_PTR->velocity_vector.z - TAR_PTR->vel.z;