Several updates to implement a sequenced/verified/check summed serial

command uplink on the same port we are receiving the telemetry data.

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 \

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,
+int UGTrack::next_message( SGSerialPort *serial, SGIOChannel *log,
-			      gps *gpspacket, imu *imupacket, nav *navpacket,
+                           gps *gpspacket, imu *imupacket, nav *navpacket,
-			      servo *servopacket, health *healthpacket,
+                           servo *servopacket, health *healthpacket,
-			      bool ignore_checksum )
+                           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.command_sequence = B.command_sequence;
-    p.volts = interp(A.volts, B.volts, percent);
-    p.est_seconds = (uint16_t)interp(A.est_seconds, B.est_seconds, percent);
     p.time = interp(A.time, B.time, percent);
 
     return p;

utils/GPSsmooth/UGear.hxx

@@ -65,16 +65,14 @@ struct servo {
 
 struct health {
     double time;
-    float volts_raw;            /* raw volt reading */
+    uint64_t command_sequence;  /* highest received command sequence num */
-    float volts;                /* filtered volts */
+    uint64_t loadavg;           /* system "1 minute" load average */
-    uint32_t est_seconds;       /* estimated useful seconds remaining */
+    uint64_t ahrs_hz;           /* actual ahrs loop hz */
-    uint32_t loadavg;            /* system "1 minute" load average */
+    uint64_t nav_hz;            /* actual nav loop hz */
-    uint32_t ahrs_hz;            /* actual ahrs loop hz */
-    uint32_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();
+    UGTrack();
-    ~UGEARTrack();
+    ~UGTrack();
 
     // read/parse the next message from the specified data stream,
     // returns id # if a valid message found.

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 );
+}

utils/GPSsmooth/UGear_command.hxx

@@ -0,0 +1,53 @@
+#ifndef _FG_UGEAR_COMMAND_HXX
+#define _FG_UGEAR_COMMAND_HXX
+
+
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
+#include <simgear/compiler.h>
+
+#include <iostream>
+#include <string>
+#include <queue>
+
+#include <simgear/misc/stdint.hxx>
+#include <simgear/io/iochannel.hxx>
+#include <simgear/serial/serial.hxx>
+
+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 <string> 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

utils/GPSsmooth/UGear_main.cxx

@@ -24,6 +24,7 @@
 #include <Network/net_fdm.hxx>
 
 #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->longitude = navpacket->lon;
-    ogc->latitude = navpacket->lat * SG_DEGREES_TO_RADIANS;
+    ogc->latitude = navpacket->lat;
-    ogc->altitude = ogc->elevation = navpacket->alt + alt_offset;
     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,
+static void send_data_udp( gps *gpspacket, imu *imupacket, nav *navpacket,
-		       servo *servopacket, health *healthpacket ) {
+                           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,
+            send_data_udp( &gpspacket, &imupacket, &navpacket, &servopacket,
-		       &healthpacket );
+                           &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 gps_time = 0.0;
-        double imu_time = 0;
+        double imu_time = 0.0;
-        double nav_time = 0;
+        double nav_time = 0.0;
-        double servo_time = 0;
+        double servo_time = 0.0;
-        double health_time = 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 );
+        SGSerialPort uavcom( serialdev, 115200 );
-        if ( !input.is_enabled() ) {
+        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 );
+        SGFile log( outfile );
-        if ( !output.open( SG_IO_OUT ) ) {
+        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,
+                        imupacket.psi*SGD_RADIANS_TO_DEGREES ); */
-                        healthpacket.volts,
-                        healthpacket.est_seconds);
                 // }
 
                 last_time = current_time;
-                send_data( &gpspacket, &imupacket, &navpacket, &servopacket,
+                send_data_udp( &gpspacket, &imupacket, &navpacket,
-                           &healthpacket );
+                               &servopacket, &healthpacket );
             }
         }
     }