<< t.getFormattedMessage() << t.getOrigin());
return;
}
-
+
Instance * instance = new Instance;
SGModelPlacement *model = new SGModelPlacement;
instance->model = model;
{
SGModelPlacement* model = instance->model;
double lon, lat, elev, roll, pitch, heading;
+ lon = lat = elev = roll = pitch = heading = 0.0;
try {
// Optionally set position from properties
// array. This allows the Nasal handlers to do things like:
// Node.getChild = func { _getChild(me.ghost, arg) }
//
-#define NODEARG() \
+#define NODENOARG() \
if(argc < 2 || !naIsGhost(args[0]) || \
naGhost_type(args[0]) != &PropNodeGhostType) \
naRuntimeError(c, "bad argument to props function"); \
- SGPropertyNode_ptr* node = (SGPropertyNode_ptr*)naGhost_ptr(args[0]); \
+ SGPropertyNode_ptr* node = (SGPropertyNode_ptr*)naGhost_ptr(args[0]);
+
+#define NODEARG() \
+ NODENOARG(); \
naRef argv = args[1]
static naRef f_getType(naContext c, naRef me, int argc, naRef* args)
{
using namespace simgear;
- NODEARG();
+ NODENOARG();
const char* t = "unknown";
switch((*node)->getType()) {
case props::NONE: t = "NONE"; break;
static naRef f_getName(naContext c, naRef me, int argc, naRef* args)
{
- NODEARG();
+ NODENOARG();
return NASTR((*node)->getName());
}
static naRef f_getIndex(naContext c, naRef me, int argc, naRef* args)
{
- NODEARG();
+ NODENOARG();
return naNum((*node)->getIndex());
}
static naRef f_getValue(naContext c, naRef me, int argc, naRef* args)
{
using namespace simgear;
- NODEARG();
+ NODENOARG();
switch((*node)->getType()) {
case props::BOOL: case props::INT:
case props::LONG: case props::FLOAT:
SG_LOG(SG_NASAL, SG_ALERT, "Nasal getValue: property " << (*node)->getPath() << " is NaN");
return naNil();
}
-
+
return naNum(dv);
}
-
+
case props::STRING:
case props::UNSPECIFIED:
return NASTR((*node)->getStringValue());
naRef n = naNumValue(val);
if(naIsNil(n))
naRuntimeError(c, "props.setValue() with non-number");
-
+
double d = naNumValue(val).num;
if (osg::isNaN(d)) {
naRuntimeError(c, "props.setValue() passed a NaN");
}
-
+
result = (*node)->setDoubleValue(d);
}
return naNum(result);
naRef r = naNumValue(naVec_get(argv, 0));
if (naIsNil(r))
naRuntimeError(c, "props.setDoubleValue() with non-number");
-
+
if (osg::isNaN(r.num)) {
naRuntimeError(c, "props.setDoubleValue() passed a NaN");
}
-
+
return naNum((*node)->setDoubleValue(r.num));
}
static naRef f_getParent(naContext c, naRef me, int argc, naRef* args)
{
- NODEARG();
+ NODENOARG();
SGPropertyNode* n = (*node)->getParent();
if(!n) return naNil();
return propNodeGhostCreate(c, n);
static naRef f_unalias(naContext c, naRef me, int argc, naRef* args)
{
- NODEARG();
+ NODENOARG();
return naNum((*node)->unalias());
}
static naRef f_getAliasTarget(naContext c, naRef me, int argc, naRef* args)
{
- NODEARG();
+ NODENOARG();
return propNodeGhostCreate(c, (*node)->getAliasTarget());
}
#ifdef HAVE_CONFIG_H
# include <config.h>
-#endif
+#endif
#include <simgear/compiler.h>
MIDGTrack::~MIDGTrack() {};
-
-
+/*
+ * Unused function
+ */
+#if(0)
static uint32_t read_swab( char *buf, size_t offset, size_t size ) {
uint32_t result = 0;
return result;
}
+#endif
static bool validate_cksum( uint8_t id, uint8_t size, char *buf,
void MIDGTrack::parse_msg( const int id, char *buf, MIDGpos *pos, MIDGatt *att )
{
+/*
+ * Completely unused parser results. Removed from compiling to remove the warnings
+ */
+#if(0)
if ( id == 1 ) {
uint32_t ts;
uint16_t status;
// timestamp
ts = (uint32_t)read_swab( buf, 0, 4 );
// cout << " time stamp = " << ts << endl;
-
+
// status
status = (uint16_t)read_swab( buf, 4, 2 );
// cout << " status = " << status << endl;
// position dop
pdop = (uint16_t)read_swab( buf, 32, 2 );
// cout << " pdop = " << pdop << endl;
-
+
// position accuracy
pacc = (uint16_t)read_swab( buf, 34, 2 );
// cout << " pacc = " << pacc << endl;
-
+
// speed accuracy
sacc = (uint16_t)read_swab( buf, 36, 2 );
// cout << " sacc = " << sacc << endl;
-
+
} else {
cout << "unknown id = " << id << endl;
}
+#endif
}
// read checksum
myread( ch, log, tmpbuf, 1 ); uint8_t cksum0 = (unsigned char)tmpbuf[0];
myread( ch, log, tmpbuf, 1 ); uint8_t cksum1 = (unsigned char)tmpbuf[0];
-
+
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
parse_msg( id, savebuf, pos, att );
return id;
{
char tmpbuf[256];
char savebuf[256];
- int result = 0;
cout << "in next_message()" << endl;
// scan for sync characters
uint8_t sync0, sync1;
- result = serial_read( serial, tmpbuf, 2 );
+ serial_read( serial, tmpbuf, 2 );
sync0 = (unsigned char)tmpbuf[0];
sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != 129 || sync1 != 161) && !myeof ) {
serial_read( serial, tmpbuf, 2 );
uint8_t cksum0 = (unsigned char)tmpbuf[0];
uint8_t cksum1 = (unsigned char)tmpbuf[1];
-
+
if ( validate_cksum( id, size, savebuf, cksum0, cksum1 ) ) {
parse_msg( id, savebuf, pos, att );
cout << "Check sum failure!" << endl;
return -1;
-
+
}
// point value. By doing the BIG_ENDIAN test, I can optimize the
// routine for big-endian processors so it can be as efficient as
// possible
-static void htond (double &x)
+static void htond (double &x)
{
if ( sgIsLittleEndian() ) {
int *Double_Overlay;
int Holding_Buffer;
-
+
Double_Overlay = (int *) &x;
Holding_Buffer = Double_Overlay [0];
-
+
Double_Overlay [0] = htonl (Double_Overlay [1]);
Double_Overlay [1] = htonl (Holding_Buffer);
} else {
}
// Float version
-static void htonf (float &x)
+static void htonf (float &x)
{
if ( sgIsLittleEndian() ) {
int *Float_Overlay;
int Holding_Buffer;
-
+
Float_Overlay = (int *) &x;
Holding_Buffer = Float_Overlay [0];
-
+
Float_Overlay [0] = htonl (Holding_Buffer);
} else {
return;
static void send_data( const MIDGpos pos, const MIDGatt att ) {
- int len;
int fdmsize = sizeof( FGNetFDM );
- // cout << "Running main loop" << endl;
-
FGNetFDM fgfdm;
FGNetCtrls fgctrls;
midg2fg( pos, att, &fgfdm, &fgctrls );
- len = fdm_sock.send(&fgfdm, fdmsize, 0);
+ fdm_sock.send(&fgfdm, fdmsize, 0);
}
MIDGpos pos0, pos1;
pos0 = pos1 = track.get_pospt( 0 );
-
+
MIDGatt att0, att1;
att0 = att1 = track.get_attpt( 0 );
-
+
while ( current_time < end_time ) {
// cout << "current_time = " << current_time << " end_time = "
// << end_time << endl;
// process incoming data from the serial port
int count = 0;
- double current_time = 0.0;
MIDGpos pos;
MIDGatt att;
if ( id == 10 ) {
if ( att.get_msec() > att_time ) {
att_time = att.get_msec();
- current_time = att_time;
+ //current_time = att_time;
} else {
cout << "oops att back in time" << endl;
}
} else if ( id == 12 ) {
if ( pos.get_msec() > pos_time ) {
pos_time = pos.get_msec();
- current_time = pos_time;
+ //current_time = pos_time;
} else {
cout << "oops pos back in time" << endl;
}
#ifdef HAVE_CONFIG_H
# include <config.h>
-#endif
+#endif
#include <iostream>
#include <cstdio>
myread( ch, log, tmpbuf, 2 );
uint8_t cksum0 = (unsigned char)tmpbuf[0];
uint8_t cksum1 = (unsigned char)tmpbuf[1];
-
+
if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
{
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
{
char tmpbuf[256];
char savebuf[256];
- int result = 0;
// cout << "in next_message()" << endl;
// scan for sync characters
int scan_count = 0;
uint8_t sync0, sync1;
- result = serial_read( serial, log, tmpbuf, 2 );
+ serial_read( serial, log, tmpbuf, 2 );
sync0 = (unsigned char)tmpbuf[0];
sync1 = (unsigned char)tmpbuf[1];
while ( (sync0 != START_OF_MSG0 || sync1 != START_OF_MSG1) && !myeof ) {
uint8_t cksum1 = (unsigned char)tmpbuf[1];
// cout << "cksum0 = " << (int)cksum0 << " cksum1 = " << (int)cksum1
// << endl;
-
+
if ( validate_cksum( id, size, savebuf, cksum0, cksum1, ignore_checksum ) )
{
parse_msg( id, savebuf, gpspacket, imupacket, navpacket, servopacket,
cout << "Check sum failure!" << endl;
return -1;
-
+
}
// point value. By doing the BIG_ENDIAN test, I can optimize the
// routine for big-endian processors so it can be as efficient as
// possible
-static void htond (double &x)
+static void htond (double &x)
{
if ( sgIsLittleEndian() ) {
int *Double_Overlay;
int Holding_Buffer;
-
+
Double_Overlay = (int *) &x;
Holding_Buffer = Double_Overlay [0];
-
+
Double_Overlay [0] = htonl (Double_Overlay [1]);
Double_Overlay [1] = htonl (Holding_Buffer);
} else {
}
// Float version
-static void htonf (float &x)
+static void htonf (float &x)
{
if ( sgIsLittleEndian() ) {
int *Float_Overlay;
int Holding_Buffer;
-
+
Float_Overlay = (int *) &x;
Holding_Buffer = Float_Overlay [0];
-
+
Float_Overlay [0] = htonl (Holding_Buffer);
} else {
return;
htonf(fdm->speedbrake);
htonf(fdm->spoilers);
-#if 0
+#if 0
ctrls->version = FG_NET_CTRLS_VERSION;
ctrls->elevator_trim = 0.0;
ctrls->flaps = 0.0;
static void send_data_udp( gps *gpspacket, imu *imupacket, nav *navpacket,
servo *servopacket, health *healthpacket )
{
- int len;
int ogcsize = sizeof( ogcFGData );
int fdmsize = sizeof( FGNetFDM );
// int ctrlsize = sizeof( FGNetCtrls );
&fgfdm, &fgctrls );
ugear2opengc( gpspacket, imupacket, navpacket, servopacket, healthpacket,
&fgogc );
- len = opengc_sock.send(&fgogc, ogcsize, 0);
- len = fdm_sock.send(&fgfdm, fdmsize, 0);
+ opengc_sock.send(&fgogc, ogcsize, 0);
+ fdm_sock.send(&fgfdm, fdmsize, 0);
// len = ctrls_sock.send(&fgctrls, ctrlsize, 0);
}
gps gps0, gps1;
gps0 = gps1 = track.get_gpspt( 0 );
-
+
imu imu0, imu1;
imu0 = imu1 = track.get_imupt( 0 );
-
+
nav nav0, nav1;
nav0 = nav1 = track.get_navpt( 0 );
-
+
servo servo0, servo1;
servo0 = servo1 = track.get_servopt( 0 );
-
+
health health0, health1;
health0 = health1 = track.get_healthpt( 0 );
} else {
cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl;
}
-
+
}
if ( (current_time > gps_time + 2) ||
command_mgr.add("hb");
command_heartbeat = current_time;
}
-
+
// Command update @ 1hz
if ( current_time >= command_time + 1 ) {
command_mgr.update(&uavcom);
// double lonmin = fabs(navpacket.lon - londeg);
int latdeg = (int)navpacket.lat;
// double latmin = fabs(navpacket.lat - latdeg);
- char londir = 'E'; if ( londeg < 0 ) londir = 'W';
- char latdir = 'N'; if ( latdeg < 0 ) latdir = 'S';
londeg = abs(londeg);
latdeg = abs(latdeg);
/*printf( "%.2f %c%02d:%.4f %c%03d:%.4f %.1f %.2f %.2f %.2f\n",
// point value. By doing the BIG_ENDIAN test, I can optimize the
// routine for big-endian processors so it can be as efficient as
// possible
-static void htond (double &x)
+static void htond (double &x)
{
if ( sgIsLittleEndian() ) {
int *Double_Overlay;
int Holding_Buffer;
-
+
Double_Overlay = (int *) &x;
Holding_Buffer = Double_Overlay [0];
-
+
Double_Overlay [0] = htonl (Double_Overlay [1]);
Double_Overlay [1] = htonl (Holding_Buffer);
} else {
}
// Float version
-static void htonf (float &x)
+static void htonf (float &x)
{
if ( sgIsLittleEndian() ) {
int *Float_Overlay;
int Holding_Buffer;
-
+
Float_Overlay = (int *) &x;
Holding_Buffer = Float_Overlay [0];
-
+
Float_Overlay [0] = htonl (Holding_Buffer);
} else {
return;
static void send_data( const GPSPoint p ) {
- int len;
// int ctrlsize = sizeof( FGNetCtrls );
int fdmsize = sizeof( FGNetFDM );
FGNetCtrls fgctrls;
gps2fg( p, &fgfdm, &fgctrls );
- len = fdm_sock.send(&fgfdm, fdmsize, 0);
+ fdm_sock.send(&fgfdm, fdmsize, 0);
}
GPSPoint p, p0, p1;
p0 = p1 = track.get_point( 0 );
-
+
while ( current_time < end_time ) {
// cout << "current_time = " << current_time << " end_time = "
// << end_time << endl;