- double pt = _total_pressure_node->getDoubleValue();
- double p = _static_pressure_node->getDoubleValue();
- double q = pt - p; // dynamic pressure
-
- // Now, reverse the equation
- double v_fps = sqrt((2 * q) / SEA_LEVEL_DENSITY_SLUGFT3);
-
- _speed_node->setDoubleValue(v_fps * FPSTOKTS);
+ double pt = _total_pressure_node->getDoubleValue() * INHGTOPSF;
+ double p = _static_pressure_node->getDoubleValue() * INHGTOPSF;
+ double r = _density_node->getDoubleValue();
+ double q = ( pt - p ); // dynamic pressure
+
+ // Now, reverse the equation (normalize dynamic pressure to
+ // avoid "nan" results from sqrt)
+ if ( q < 0 ) { q = 0.0; }
+ double v_fps = sqrt((2 * q) / r);
+
+ // Publish the indicated airspeed
+ double last_speed_kt = _speed_node->getDoubleValue();
+ double current_speed_kt = v_fps * FPSTOKTS;
+ _speed_node->setDoubleValue(fgGetLowPass(last_speed_kt,
+ current_speed_kt,
+ dt * RESPONSIVENESS));