- double dist = last_x - x;
- last_x = x;
- if ( dt > 0.0 ) {
- // avoid nan
- double new_vel = ( dist / dt );
-
- horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
- // double horiz_vel = cur_fdm_state->get_V_ground_speed()
- // * SG_FEET_TO_METER * 60.0;
- // double horiz_vel = airspeed_node->getFloatValue()
- // * SG_FEET_TO_METER * 60.0;
-
- gs_rate_of_climb_node
- ->setDoubleValue( -sin( des_angle * SGD_DEGREES_TO_RADIANS )
- * horiz_vel * SG_METER_TO_FEET );
+ double elapsedDistance = last_x - dist;
+ last_x = dist;
+
+ double new_vel = ( elapsedDistance / dt );
+ horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
+
+ gs_rate_of_climb_node
+ ->setDoubleValue( -sin( des_angle * SGD_DEGREES_TO_RADIANS )
+ * horiz_vel * SG_METER_TO_FEET );
+}
+
+void FGNavRadio::updateGPSSlaved()
+{
+ has_gs_node->setBoolValue(gps_has_gs_node->getBoolValue());
+
+ _toFlag = gps_to_flag_node->getBoolValue();
+ _fromFlag = gps_from_flag_node->getBoolValue();
+
+ inrange_node->setBoolValue(_toFlag | _fromFlag);
+
+ _cdiDeflection = gps_cdi_deflection_node->getDoubleValue();
+ // clmap to some range (+/- 10 degrees) as the regular deflection
+ SG_CLAMP_RANGE(_cdiDeflection, -10.0, 10.0 );
+
+ _cdiCrossTrackErrorM = 0.0; // FIXME, supply this
+ _gsNeedleDeflection = 0.0; // FIXME, supply this
+}
+
+void FGNavRadio::updateCDI(double dt)
+{
+ bool cdi_serviceable = cdi_serviceable_node->getBoolValue();
+ bool inrange = inrange_node->getBoolValue();
+
+ if (tofrom_serviceable_node->getBoolValue()) {
+ to_flag_node->setBoolValue(_toFlag);
+ from_flag_node->setBoolValue(_fromFlag);
+ } else {
+ to_flag_node->setBoolValue(false);
+ from_flag_node->setBoolValue(false);
+ }
+
+ if (!cdi_serviceable) {
+ _cdiDeflection = 0.0;
+ _cdiCrossTrackErrorM = 0.0;
+ }
+
+ cdi_deflection_node->setDoubleValue(_cdiDeflection);
+ cdi_xtrack_error_node->setDoubleValue(_cdiCrossTrackErrorM);
+
+ //////////////////////////////////////////////////////////
+ // compute an approximate ground track heading error
+ //////////////////////////////////////////////////////////
+ double hdg_error = 0.0;
+ if ( inrange && cdi_serviceable ) {
+ double vn = fgGetDouble( "/velocities/speed-north-fps" );
+ double ve = fgGetDouble( "/velocities/speed-east-fps" );
+ double gnd_trk_true = atan2( ve, vn ) * SGD_RADIANS_TO_DEGREES;
+ if ( gnd_trk_true < 0.0 ) { gnd_trk_true += 360.0; }
+
+ SGPropertyNode *true_hdg
+ = fgGetNode("/orientation/heading-deg", true);
+ hdg_error = gnd_trk_true - true_hdg->getDoubleValue();
+
+ // cout << "ground track = " << gnd_trk_true
+ // << " orientation = " << true_hdg->getDoubleValue() << endl;