diff --git a/utils/GPSsmooth/Makefile.am b/utils/GPSsmooth/Makefile.am index 326e6317c..f68325b8d 100644 --- a/utils/GPSsmooth/Makefile.am +++ b/utils/GPSsmooth/Makefile.am @@ -19,7 +19,9 @@ MIDGsmooth_LDADD = \ UGsmooth_SOURCES = \ UGear.cxx UGear.hxx \ - UGear_main.cxx + UGear_command.cxx UGear_command.hxx \ + UGear_main.cxx \ + UGear_opengc.hxx UGsmooth_LDADD = \ -lsgio -lsgserial -lsgtiming -lsgmath -lsgbucket -lsgmisc -lsgdebug \ diff --git a/utils/GPSsmooth/UGear.cxx b/utils/GPSsmooth/UGear.cxx index d46e992a6..ca5312050 100644 --- a/utils/GPSsmooth/UGear.cxx +++ b/utils/GPSsmooth/UGear.cxx @@ -22,12 +22,12 @@ SG_USING_STD(endl); #define START_OF_MSG1 224 -UGEARTrack::UGEARTrack(): +UGTrack::UGTrack(): sg_swap(false) { }; -UGEARTrack::~UGEARTrack() {}; +UGTrack::~UGTrack() {}; // swap the 1st 4 bytes with the last 4 bytes of a stargate double so @@ -91,7 +91,7 @@ static bool validate_cksum( uint8_t id, uint8_t size, char *buf, } -void UGEARTrack::parse_msg( const int id, char *buf, +void UGTrack::parse_msg( const int id, char *buf, struct gps *gpspacket, imu *imupacket, nav *navpacket, servo *servopacket, health *healthpacket ) @@ -157,7 +157,7 @@ void UGEARTrack::parse_msg( const int id, char *buf, // load the named stream log file into internal buffers -bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) { +bool UGTrack::load_stream( const string &file, bool ignore_checksum ) { int count = 0; gps gpspacket; @@ -236,7 +236,7 @@ bool UGEARTrack::load_stream( const string &file, bool ignore_checksum ) { // load the named stream log file into internal buffers -bool UGEARTrack::load_flight( const string &path ) { +bool UGTrack::load_flight( const string &path ) { gps gpspacket; imu imupacket; nav navpacket; @@ -261,7 +261,7 @@ bool UGEARTrack::load_flight( const string &path ) { // open the gps file file = path; file.append( "gps.dat.gz" ); if ( (fgps = gzopen( file.c_str(), "r" )) == NULL ) { - printf("Cannont open %s\n", file.c_str()); + printf("Cannot open %s\n", file.c_str()); return false; } @@ -373,7 +373,7 @@ int serial_read( SGSerialPort *serial, SGIOChannel *log, // load the next message of a real time data stream -int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log, +int UGTrack::next_message( SGIOChannel *ch, SGIOChannel *log, gps *gpspacket, imu *imupacket, nav *navpacket, servo *servopacket, health *healthpacket, bool ignore_checksum ) @@ -443,10 +443,10 @@ int UGEARTrack::next_message( SGIOChannel *ch, SGIOChannel *log, // load the next message of a real time data stream -int UGEARTrack::next_message( SGSerialPort *serial, SGIOChannel *log, - gps *gpspacket, imu *imupacket, nav *navpacket, - servo *servopacket, health *healthpacket, - bool ignore_checksum ) +int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log, + gps *gpspacket, imu *imupacket, nav *navpacket, + servo *servopacket, health *healthpacket, + bool ignore_checksum ) { char tmpbuf[256]; char savebuf[256]; @@ -592,9 +592,7 @@ servo UGEARInterpSERVO( const servo A, const servo B, const double percent ) health UGEARInterpHEALTH( const health A, const health B, const double percent ) { health p; - p.volts_raw = interp(A.volts_raw, B.volts_raw, percent); - p.volts = interp(A.volts, B.volts, percent); - p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent); + p.command_sequence = B.command_sequence; p.time = interp(A.time, B.time, percent); return p; diff --git a/utils/GPSsmooth/UGear.hxx b/utils/GPSsmooth/UGear.hxx index 2540cfe04..8e69bf0f8 100644 --- a/utils/GPSsmooth/UGear.hxx +++ b/utils/GPSsmooth/UGear.hxx @@ -65,16 +65,14 @@ struct servo { struct health { double time; - float volts_raw; /* raw volt reading */ - float volts; /* filtered volts */ - uint32_t est_seconds; /* estimated useful seconds remaining */ - uint32_t loadavg; /* system "1 minute" load average */ - uint32_t ahrs_hz; /* actual ahrs loop hz */ - uint32_t nav_hz; /* actual nav loop hz */ + uint64_t command_sequence; /* highest received command sequence num */ + uint64_t loadavg; /* system "1 minute" load average */ + uint64_t ahrs_hz; /* actual ahrs loop hz */ + uint64_t nav_hz; /* actual nav loop hz */ }; // Manage a saved ugear log (track file) -class UGEARTrack { +class UGTrack { private: @@ -96,8 +94,8 @@ private: public: - UGEARTrack(); - ~UGEARTrack(); + UGTrack(); + ~UGTrack(); // read/parse the next message from the specified data stream, // returns id # if a valid message found. diff --git a/utils/GPSsmooth/UGear_command.cxx b/utils/GPSsmooth/UGear_command.cxx new file mode 100644 index 000000000..0cf9a40aa --- /dev/null +++ b/utils/GPSsmooth/UGear_command.cxx @@ -0,0 +1,98 @@ +#include "UGear_command.hxx" + + +UGCommand::UGCommand(): + cmd_send_index(0), + cmd_recv_index(0), + prime_state(true) +{} + + +UGCommand::~UGCommand() {} + + +// calculate the nmea check sum +static char calc_nmea_cksum(const char *sentence) { + unsigned char sum = 0; + int i, len; + + // cout << sentence << endl; + + len = strlen(sentence); + sum = sentence[0]; + for ( i = 1; i < len; i++ ) { + // cout << sentence[i]; + sum ^= sentence[i]; + } + // cout << endl; + + // printf("sum = %02x\n", sum); + return sum; +} + + +// package and send the serial command +static int serial_send( SGSerialPort *serial, int sequence, + const string command ) +{ + char sequence_str[10]; + snprintf( sequence_str, 9, "%d", sequence ); + + string package = sequence_str; + package += ","; + package += command; + + char pkg_sum[10]; + snprintf( pkg_sum, 3, "%02X", calc_nmea_cksum(package.c_str()) ); + + package += "*"; + package += pkg_sum; + package += "\n"; + + unsigned int result = serial->write_port( package.c_str(), + package.length() ); + if ( result != package.length() ) { + printf("ERROR: wrote %d of %d bytes to serial port!\n", + result, package.length() ); + return 0; + } + + return 1; +} + + +// send current command until acknowledged +int UGCommand::update( SGSerialPort *serial ) +{ + // if current command has been received, advance to next command + printf("sent = %d recv = %d\n", cmd_send_index, cmd_recv_index); + if ( cmd_recv_index >= cmd_send_index ) { + if ( ! cmd_queue.empty() ) { + if ( ! prime_state ) { + cmd_queue.pop(); + cmd_send_index++; + } else { + prime_state = false; + } + } + } + + // nothing to do if command queue empty + if ( cmd_queue.empty() ) { + prime_state = true; + return 0; + } + + // send the command + string command = cmd_queue.front(); + int result = serial_send( serial, cmd_send_index, command ); + + return cmd_send_index; +} + + +void UGCommand::add( const string command ) +{ + printf("command queue: %s\n", command.c_str()); + cmd_queue.push( command ); +} diff --git a/utils/GPSsmooth/UGear_command.hxx b/utils/GPSsmooth/UGear_command.hxx new file mode 100644 index 000000000..4d3938f61 --- /dev/null +++ b/utils/GPSsmooth/UGear_command.hxx @@ -0,0 +1,53 @@ +#ifndef _FG_UGEAR_COMMAND_HXX +#define _FG_UGEAR_COMMAND_HXX + + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include +#include +#include + +#include +#include +#include + +SG_USING_STD(cout); +SG_USING_STD(endl); +SG_USING_STD(string); +SG_USING_STD(queue); + + +// Manage UGear Command Channel +class UGCommand { + +private: + + int cmd_send_index; + int cmd_recv_index; + bool prime_state; + queue cmd_queue; + +public: + + UGCommand(); + ~UGCommand(); + + // send current command until acknowledged + int update( SGSerialPort *serial ); + + void add( const string command ); + inline int cmd_queue_size() { + return cmd_queue.size(); + } + inline void update_cmd_sequence( int sequence ) { + cmd_recv_index = sequence; + } +}; + + +#endif // _FG_UGEAR_COMMAND_HXX diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx index 0b4b5acf0..9065c69cc 100644 --- a/utils/GPSsmooth/UGear_main.cxx +++ b/utils/GPSsmooth/UGear_main.cxx @@ -24,6 +24,7 @@ #include #include "UGear.hxx" +#include "UGear_command.hxx" #include "UGear_opengc.hxx" @@ -36,7 +37,7 @@ SG_USING_STD(string); static netSocket fdm_sock, ctrls_sock, opengc_sock; // ugear data -UGEARTrack track; +UGTrack track; // Default ports static int fdm_port = 5505; @@ -193,10 +194,10 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket, Ps_error = (Ps_count/span) * Ps_error + ((span-Ps_count)/span) * error; fdm->altitude = Ps + Ps_error; - printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n", + /* printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n", imupacket->time, imupacket->the, -navpacket->vd, climbf, navpacket->lat, navpacket->lon, gpspacket->alt, navpacket->alt, - imupacket->Ps, Ps, Ps + Ps_error); + imupacket->Ps, Ps, Ps + Ps_error); */ // cout << "climb rate = " << aero->hdota << endl; fdm->v_north = 0.0; @@ -364,15 +365,12 @@ static void ugear2opengc( gps *gpspacket, imu *imupacket, nav *navpacket, servo *servopacket, health *healthpacket, ogcFGData *ogc ) { - unsigned int i; - // Version sanity checking ogc->version_id = OGC_VERSION; // Aero parameters - ogc->longitude = navpacket->lon * SG_DEGREES_TO_RADIANS; - ogc->latitude = navpacket->lat * SG_DEGREES_TO_RADIANS; - ogc->altitude = ogc->elevation = navpacket->alt + alt_offset; + ogc->longitude = navpacket->lon; + ogc->latitude = navpacket->lat; ogc->heading = imupacket->psi * SG_RADIANS_TO_DEGREES; // heading ogc->bank = imupacket->phi * SG_RADIANS_TO_DEGREES; // roll ogc->pitch = imupacket->the * SG_RADIANS_TO_DEGREES; // pitch; @@ -437,18 +435,26 @@ static void ugear2opengc( gps *gpspacket, imu *imupacket, nav *navpacket, climbf = 0.994 * climbf + 0.006 * climb; ogc->vvi = climbf; // fps + // uncomment one of the following schemes for setting elevation: + + // use the navigation (inertially augmented gps estimate) + // ogc->altitude = ogc->elevation + // = (navpacket->alt + alt_offset * SG_METER_TO_FEET); + + // use estimate error between pressure sensor and gps altitude over time + // use pressure sensor + error average for altitude estimate. static double Ps_error = 0.0; static double Ps_count = 0; const double span = 10000.0; Ps_count += 1.0; if (Ps_count > (span-1.0)) { Ps_count = (span-1.0); } double error = navpacket->alt - Ps; Ps_error = (Ps_count/span) * Ps_error + ((span-Ps_count)/span) * error; - ogc->elevation = Ps + Ps_error; + ogc->elevation = (Ps + Ps_error) * SG_METER_TO_FEET; - printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n", + /* printf("%.3f, %.3f, %.3f, %.3f, %.8f, %.8f, %.3f, %.3f, %.3f, %.3f, %.3f\n", imupacket->time, imupacket->the, -navpacket->vd, climbf, navpacket->lat, navpacket->lon, gpspacket->alt, navpacket->alt, - imupacket->Ps, Ps, Ps + Ps_error); + imupacket->Ps, Ps, Ps + Ps_error); */ if ( est_controls ) { static float est_elev = 0.0; @@ -472,21 +478,22 @@ static void ugear2opengc( gps *gpspacket, imu *imupacket, nav *navpacket, double fd_bank = 0.0; double fd_pitch = 0.0; ogc->egt[0] = ogc->bank + fd_bank; // flight director target roll - ogc->egt[1] = -ogc->pitch * 2 + fd_pitch; // flight director target pitch + ogc->egt[1] = -ogc->pitch + fd_pitch; // flight director target pitch ogc->egt[2] = ogc->heading + 15; // target heading bug ogc->egt[3] = -ogc->vvi; // target VVI bug - ogc->epr[0] = ogc->altitude + 100; // target altitude bug + ogc->epr[0] = 1400 /*ogc->elevation - 50*/ ; // target altitude bug ogc->epr[1] = ogc->v_kcas + 5; // target speed bug ogc->epr[2] = gps_status; // gps status box } -static void send_data( gps *gpspacket, imu *imupacket, nav *navpacket, - servo *servopacket, health *healthpacket ) { +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 ); + // int ctrlsize = sizeof( FGNetCtrls ); // cout << "Running main loop" << endl; @@ -727,11 +734,11 @@ int main( int argc, char **argv ) { if ( track.gps_size() > 0 ) { double tmp = track.get_gpspt(track.gps_size()-1).ITOW; - int days = tmp / (24 * 60 * 60); + int days = (int)(tmp / (24 * 60 * 60)); tmp -= days * 24 * 60 * 60; - int hours = tmp / (60 * 60); + int hours = (int)(tmp / (60 * 60)); tmp -= hours * 60 * 60; - int min = tmp / 60; + int min = (int)(tmp / 60); tmp -= min * 60; double sec = tmp; printf("[GPS ]:ITOW= %.3f[sec] %dd %02d:%02d:%06.3f\n", @@ -930,8 +937,8 @@ int main( int argc, char **argv ) { gps_status = 1.0; } - send_data( &gpspacket, &imupacket, &navpacket, &servopacket, - &healthpacket ); + send_data_udp( &gpspacket, &imupacket, &navpacket, &servopacket, + &healthpacket ); if ( run_real_time ) { // Update the elapsed time. @@ -987,15 +994,17 @@ int main( int argc, char **argv ) { servo servopacket; bzero( &servopacket, sizeof(servopacket) ); health healthpacket; bzero( &healthpacket, sizeof(healthpacket) ); - double gps_time = 0; - double imu_time = 0; - double nav_time = 0; - double servo_time = 0; - double health_time = 0; + double gps_time = 0.0; + double imu_time = 0.0; + double nav_time = 0.0; + double servo_time = 0.0; + double health_time = 0.0; + double command_time = 0.0; + double command_heartbeat = 0.0; // open the serial port device - SGSerialPort input( serialdev, 115200 ); - if ( !input.is_enabled() ) { + SGSerialPort uavcom( serialdev, 115200 ); + if ( !uavcom.is_enabled() ) { cout << "Cannot open: " << serialdev << endl; return false; } @@ -1006,15 +1015,23 @@ int main( int argc, char **argv ) { << endl; return false; } - SGFile output( outfile ); - if ( !output.open( SG_IO_OUT ) ) { + SGFile log( outfile ); + if ( !log.open( SG_IO_OUT ) ) { cout << "Cannot open: " << outfile << endl; return false; } - while ( input.is_enabled() ) { + // create the command channel manager + UGCommand command; + + // add some test commands + command.add("apalt,1000"); + command.add("home,158.0,32.5"); + command.add("gohome"); + + while ( uavcom.is_enabled() ) { // cout << "looking for next message ..." << endl; - int id = track.next_message( &input, &output, &gpspacket, + int id = track.next_message( &uavcom, &log, &gpspacket, &imupacket, &navpacket, &servopacket, &healthpacket, ignore_checksum ); // cout << "message id = " << id << endl; @@ -1052,9 +1069,13 @@ int main( int argc, char **argv ) { if ( healthpacket.time > health_time ) { health_time = healthpacket.time; current_time = health_time; + printf("Received a health packet, sequence: %d\n", + healthpacket.command_sequence); + command.update_cmd_sequence(healthpacket.command_sequence); } else { cout << "oops health back in time: " << healthpacket.time << " " << health_time << endl; } + } if ( (current_time > gps_time + 2) || @@ -1068,6 +1089,19 @@ int main( int argc, char **argv ) { gps_status = -1.0; } + // Generate a ground station heart beat every 5 seconds + if ( current_time >= command_heartbeat + 5 ) { + command.add("hb"); + command_heartbeat = current_time; + } + + // Command update @ 1hz + if ( current_time >= command_time + 1 ) { + command.update(&uavcom); + command_time = current_time; + } + + // Relay data on to FlightGear and LFSTech Glass if ( current_time >= last_time + (1/hertz) ) { // if ( gpspacket.lat > -500 ) { int londeg = (int)navpacket.lon; @@ -1078,20 +1112,18 @@ int main( int argc, char **argv ) { 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 %.2f %d\n", + /*printf( "%.2f %c%02d:%.4f %c%03d:%.4f %.1f %.2f %.2f %.2f\n", current_time, latdir, latdeg, latmin, londir, londeg, lonmin, navpacket.alt, imupacket.phi*SGD_RADIANS_TO_DEGREES, imupacket.the*SGD_RADIANS_TO_DEGREES, - imupacket.psi*SGD_RADIANS_TO_DEGREES, - healthpacket.volts, - healthpacket.est_seconds); + imupacket.psi*SGD_RADIANS_TO_DEGREES ); */ // } last_time = current_time; - send_data( &gpspacket, &imupacket, &navpacket, &servopacket, - &healthpacket ); + send_data_udp( &gpspacket, &imupacket, &navpacket, + &servopacket, &healthpacket ); } } }