- if (_pnode != 0) {
- setParentPos();
- hdg = _p_hdg_node->getDoubleValue();
- pch = _p_pch_node->getDoubleValue();
- rll = _p_rll_node->getDoubleValue();
- setOffsetPos(_parentpos, hdg, pch, rll);
- setSpeed(_p_spd_node->getDoubleValue());
- }else {
- hdg = manager->get_user_heading();
- pch = manager->get_user_pitch();
- rll = manager->get_user_roll();
- setOffsetPos(userpos, hdg, pch, rll);
- setSpeed(manager->get_user_speed());
- }
-
- // elapsed time has a random initialisation so that each
- // wingman moves differently
- _elapsed_time += dt;
-
- // we derive a sine based factor to give us smoothly
- // varying error between -1 and 1
- double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
- double r_angle = 5 * factor;
- double p_angle = 2.5 * factor;
- double h_angle = 5 * factor;
- double h_feet = 3 * factor;
-
- pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
- pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
-
- if (getHtAGL(10000)){
-
- if(_ht_agl_ft <= 10) {
- _height = userpos.getElevationFt();
- } else if (_ht_agl_ft > 10 && _ht_agl_ft <= 150 ) {
- setHt(userpos.getElevationFt(), dt, 1.0);
- } else if (_ht_agl_ft > 150 && _ht_agl_ft <= 250) {
- setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
- } else
- setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
-
- pos.setElevationFt(_height);
- }
-
- // these calculations are unreliable at slow speeds
- if(speed >= 10) {
- setHdg(_azimuth + h_angle, dt, 0.9);
- setPch(_elevation + p_angle + _pitch_offset, dt, 0.9);
-
- if (roll <= 115 && roll >= -115)
- setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5);
- else
- roll = manager->get_user_roll() + r_angle + _roll_offset;
-
- } else {
- setHdg(manager->get_user_heading(), dt, 0.9);
- setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9);
- setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9);
- }
-
- setOffsetVelocity(dt, pos);
-}