//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
//
// $Id$
+#ifdef HAVE_CONFIG_H
+# include <config.h>
+#endif
#include <simgear/constants.h>
#include <simgear/debug/logstream.hxx>
#include <simgear/io/iochannel.hxx>
-#include <FDM/flight.hxx>
-
+#include <FDM/flightProperties.hxx>
+#include <Environment/gravity.hxx>
#include "ray.hxx"
bool FGRAY::gen_message() {
// cout << "generating RayWoodworth message" << endl;
- FGInterface *f = cur_fdm_state;
+ FlightProperties f;
+
int axis, subaxis;
const double fullscale[6] = { -0.5, -0.5, -0.5, /* radians */
-0.3, -0.3, -0.15 /* meters */ };
double dt = 0.05; /* seconds */
/* get basic information about gravity */
- double grav_acc = -9.81;
- double vert_acc = f->get_A_Z_pilot() * 0.3;
+ double grav_acc = -Environment::Gravity::instance()->getGravity( f.getPosition() );
+ double vert_acc = f.get_A_Z_pilot() * 0.3;
if ( -3.0 < vert_acc )
vert_acc = -3.0;
/* Retrieve the desired components */
switch ( axis ) {
- case 0: ang_pos = f->get_Phi();
- lin_acc = f->get_A_Y_pilot() * 0.3;
+ case 0: ang_pos = f.get_Phi();
+ lin_acc = f.get_A_Y_pilot() * 0.3;
break;
- case 1: ang_pos = f->get_Theta();
- lin_acc = f->get_A_X_pilot() * 0.3;
+ case 1: ang_pos = f.get_Theta();
+ lin_acc = f.get_A_X_pilot() * 0.3;
break;
- case 2: ang_pos = f->get_Psi();
+ case 2: ang_pos = f.get_Psi();
lin_acc = grav_acc - vert_acc;
break;
default:
/* Make sure the angles are reasonable onscale */
/* We use an asymmetric mapping so that the chair behaves
reasonably when upside down. Otherwise it oscillates. */
- while ( ang_pos < -2*SGD_PI/3 ) {
- ang_pos += 2 * SGD_PI;
+ while ( ang_pos < -SGD_2PI/3 ) {
+ ang_pos += SGD_2PI;
}
- while ( ang_pos > 4*SGD_PI/3 ) {
- ang_pos -= 2 * SGD_PI;
+ while ( ang_pos > 2*SGD_2PI/3 ) {
+ ang_pos -= SGD_2PI;
}
/* Tell interested parties what the situation is */
if ( fabs ( ang_pos - chair_heading ) > SGD_PI )
{ /* Need to swing chair by 360 degrees */
if ( ang_pos < chair_heading )
- chair_heading -= 2 * SGD_PI;
- else chair_heading += 2 * SGD_PI;
+ chair_heading -= SGD_2PI;
+ else chair_heading += SGD_2PI;
}
/* Remove the chair heading from the true heading */
ang_pos -= chair_heading;
can just about represent 30 degrees full scale. */
chair_heading += ang_pos * dt * 0.2;
/* If they turn fast, at 90 deg error subtract 30 deg */
- if ( fabs(ang_pos) > SGD_PI / 2 )
+ if ( fabs(ang_pos) > SGD_PI_2 )
chair_heading += ang_pos / 3;
} else
/* Scale to the hardware's full scale range */
propose /= fullscale [ subaxis ];
/* Use a sine shaped washout on all axes */
- if ( propose < -SGD_PI / 2 ) *dac = 0x0000; else
- if ( propose > SGD_PI / 2 ) *dac = 0xFFFF; else
+ if ( propose < -SGD_PI_2 ) *dac = 0x0000; else
+ if ( propose > SGD_PI_2 ) *dac = 0xFFFF; else
*dac = (unsigned short) ( 32767 *
( 1.0 + sin ( propose ) ) );
}
// parse RUL message
bool FGRAY::parse_message() {
- FG_LOG( FG_IO, FG_ALERT, "RAY input not supported" );
+ SG_LOG( SG_IO, SG_ALERT, "RAY input not supported" );
return false;
}
if ( get_direction() == SG_IO_OUT ) {
gen_message();
if ( ! io->write( buf, length ) ) {
- FG_LOG( FG_IO, FG_ALERT, "Error writing data." );
+ SG_LOG( SG_IO, SG_ALERT, "Error writing data." );
return false;
}
} else if ( get_direction() == SG_IO_IN ) {
- FG_LOG( FG_IO, FG_ALERT, "in direction not supported for RAY." );
+ SG_LOG( SG_IO, SG_ALERT, "in direction not supported for RAY." );
return false;
}