From 93617f577df9aaf2932429a26480ff9e8eb5d394 Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Mon, 11 Jun 2007 19:13:08 +0000
Subject: [PATCH] Add an option to estimate control surfaces deflections based
 on rotational rates (not perfect, but lacking any data at all this is better
 than showing them all hard over full stop.)

---
 utils/GPSsmooth/UGear_main.cxx | 26 ++++++++++++++++++++++----
 1 file changed, 22 insertions(+), 4 deletions(-)

diff --git a/utils/GPSsmooth/UGear_main.cxx b/utils/GPSsmooth/UGear_main.cxx
index f8e40d97c..28b806907 100644
--- a/utils/GPSsmooth/UGear_main.cxx
+++ b/utils/GPSsmooth/UGear_main.cxx
@@ -73,6 +73,8 @@ bool ignore_checksum = false;
 
 bool sg_swap = false;
 
+bool est_controls = false;
+
 // The function htond is defined this way due to the way some
 // processors and OSes treat floating point values.  Some will raise
 // an exception whenever a "bad" floating point value is loaded into a
@@ -205,13 +207,26 @@ static void ugear2fg( gps *gpspacket, imu *imupacket, nav *navpacket,
     fdm->left_flap = 0.0;
     fdm->right_flap = 0.0;
 
-    fdm->elevator = ((double)servopacket->chn[1] / 32768.0) - 1.0;
+    if ( est_controls ) {
+        static float est_elev = 0.0;
+        static float est_aileron = 0.0;
+        static float est_rudder = 0.0;
+        est_elev = 0.95 * est_elev + 0.05 * (imupacket->q * 8);
+        est_aileron = 0.95 * est_aileron + 0.05 * (imupacket->p * 8);
+        est_rudder = 0.95 * est_rudder + 0.05 * (imupacket->r * 8);
+        fdm->elevator = -est_elev;
+        fdm->left_aileron = est_aileron;
+        fdm->right_aileron = -est_aileron;
+        fdm->rudder = est_rudder;
+    } else {
+        fdm->elevator = ((double)servopacket->chn[1] / 32768.0) - 1.0;
+        fdm->left_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
+        fdm->right_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
+        fdm->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
+    }
     fdm->elevator_trim_tab = 0.0;
     fdm->left_flap = 0.0;
     fdm->right_flap = 0.0;
-    fdm->left_aileron = ((double)servopacket->chn[0] / 32768.0) - 1.0;
-    fdm->right_aileron = 1.0 - ((double)servopacket->chn[0] / 32768.0);
-    fdm->rudder = 1.0 - ((double)servopacket->chn[3] / 32768.0);
     fdm->nose_wheel = 0.0;
     fdm->speedbrake = 0.0;
     fdm->spoilers = 0.0;
@@ -321,6 +336,7 @@ void usage( const string &argv0 ) {
     cout << "\t[ --broadcast ]" << endl;
     cout << "\t[ --fdm-port <fdm output port #> ]" << endl;
     cout << "\t[ --ctrls-port <ctrls output port #> ]" << endl;
+    cout << "\t[ --estimate-control-deflections ]" << endl;
     cout << "\t[ --altitude-offset <meters> ]" << endl;
     cout << "\t[ --skip-seconds <seconds> ]" << endl;
     cout << "\t[ --no-real-time ]" << endl;
@@ -405,6 +421,8 @@ int main( int argc, char **argv ) {
                 usage( argv[0] );
                 exit( -1 );
             }
+        } else if ( strcmp (argv[i], "--estimate-control-deflections" ) == 0 ) {
+            est_controls = true;
         } else if ( strcmp( argv[i], "--altitude-offset" ) == 0 ) {
             ++i;
             if ( i < argc ) {
-- 
2.39.5