Fixed an IP address problem with no network situations, fixed an equa…

…tion that was wrong (thank you Antonio) and started to fix up the wind estimator

ekf.py

@@ -96,7 +96,7 @@ def gain_calculation_and_variance_update(self, p, q, r, Vair):
         """ Calculate linearized output equations...this is the magic of the Kalman filter; here we are figuring out how much we trust the sensors [ISEFMAV 2.27] """
         self.Cx = matrix([[0, ((q * Vair / GRAVITY) * cos(self.thetahat)) + cos(self.thetahat)]])
         self.Cy = matrix([[-cos(self.thetahat) * cos(self.phihat), ((-r * Vair / GRAVITY) * sin(self.thetahat)) - ((p * Vair / GRAVITY) * cos(self.thetahat)) + (sin(self.thetahat) * sin(self.phihat))]])
-        self.Cz = matrix([[cos(self.thetahat) * cos(self.phihat), (((q * Vair / GRAVITY) * sin(self.thetahat)) + cos(self.phihat)) * sin(self.thetahat)]])
+        self.Cz = matrix([[cos(self.thetahat) * sin(self.phihat), (((q * Vair / GRAVITY) * sin(self.thetahat)) + cos(self.phihat)) * sin(self.thetahat)]])
 
     def get_sensor_noise_covariance_matrix(self, q):
         """ Sensor noise penalty, needs a better explanation of how it works. Tau is a tuning parameter which is increased to reduce the Kalman gain on x-axis accelerometer during high pitch rate maneuvers (that flight dynamic is unmodeled). I'm trying values between 10-100. [ISEFMAV 2.28] """

fgo.py

@@ -1,8 +1,10 @@
 from math import radians, sin, sqrt, atan2, cos, degrees
 from utils import safe_tangent, wrap
 from display import Display
+from truthdata import TruthData
 
 display = Display()
+TD = TruthData()
 
 ''' FGO = Fixed-Gain Observers
 '''
@@ -27,16 +29,27 @@ class WindObserver:
     ''' Ryan Beall is awesome.
     '''
     def __init__(self):
-        self.k = 0.01
+        self.k = 0.025
         self.Wn_fgo = 0.0
         self.We_fgo = 0.0
 
     def estimate(self, theta, psi, Vair, ground_speed, course_over_ground, dt):
-        wind_north_error = Vair * cos(psi) * cos(theta) - ground_speed * cos(course_over_ground) - self.Wn_fgo
-        wind_east_error = Vair * sin(psi) * cos(theta) - ground_speed * sin(course_over_ground) - self.We_fgo
+        ''' Units:
+                radians: theta, psi, course_over_ground
+                meters per second: Vair, ground_speed
+                seconds: dt
+            Issues to fix:
+                Speeds need to come in as the right units for this estimator to work properly
+        '''
+        # if they come in as knots
+        #Vair = Vair / 0.5144444444
+        #ground_speed = ground_speed / 0.5144444444
+        display.register_scalars({"Vair":Vair, "GSPD":ground_speed, "TD.Wdir":TD.WIND_DIRECTION, "TD.Wvel":TD.WIND_VELOCITY}, "Wind FGO")
+        wind_north_error = (Vair * cos(psi) * cos(theta)) - (ground_speed * cos(course_over_ground)) - self.Wn_fgo
+        wind_east_error = (Vair * sin(psi) * cos(theta)) - (ground_speed * sin(course_over_ground)) - self.We_fgo
         self.Wn_fgo += self.k * wind_north_error
         self.We_fgo += self.k * wind_east_error
-        display.register_scalars({"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "SoG": ground_speed, "Vair": Vair}, "Debugging")
-        wind_direction = degrees(atan2(self.We_fgo,self.Wn_fgo))
-        wind_velocity = sqrt(self.We_fgo**2 + self.Wn_fgo**2)
+        display.register_scalars({"Wn error": wind_north_error, "We error": wind_east_error, "Wn_fgo": self.Wn_fgo, "We_fgo": self.We_fgo, "cos(psi)": cos(psi), "cos(theta)": cos(theta), "sin(psi)": sin(psi), "cos(cog)": cos(course_over_ground), "sin(cog)": sin(course_over_ground), "wind_north_error": wind_north_error, "wind_east_error": wind_east_error}, "Wind FGO")
+        wind_direction = degrees(atan2(self.We_fgo,self.Wn_fgo)) + 180.0
+        wind_velocity = sqrt(self.We_fgo**2 + self.Wn_fgo**2) / 0.5144444444 # convert from m/s to knots
         return wind_direction, wind_velocity

shumai.py

@@ -21,6 +21,12 @@
 except:
     pass
 
+def safe_get_ip_address():
+    try:
+        return get_ip_address()
+    except:
+        return "127.0.0.1"
+
 logging.config.fileConfig("logging.conf")
 logger = logging.getLogger("shumai")
 
@@ -98,7 +104,7 @@ def loop(self):
         altitude = self.altitude_observer.estimate(theta, Vair, gps_data['altitude'], TD.DT)
         display.register_scalars({"alt_est": altitude}, "Estimates")
         display.register_scalars({"Altitude": altitude - TD.ALTITUDE}, "Performance")
-        wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'], gps_data['course_over_ground'], TD.DT)
+        wind_direction, wind_velocity = self.wind_observer.estimate(theta, psi, Vair, gps_data['speed_over_ground'] * .5144444444, radians(gps_data['course_over_ground']), TD.DT)
         display.register_scalars({"wind_direction": wind_direction, "wind_velocity": wind_velocity}, "Estimates")
         display.register_scalars({"Wdir error": wind_direction - TD.WIND_DIRECTION, "Wvel error": wind_velocity - TD.WIND_VELOCITY}, "Performance")
         # Shoot, I forget what this is, something from Ryan
@@ -123,7 +129,7 @@ class XplaneListener(DatagramProtocol):
 
     def __init__(self):
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        self.sock.bind((get_ip_address(),49998))
+        self.sock.bind((safe_get_ip_address(),49998))
         self.last_time = datetime.now()
         self.ekf = Shumai(self, self, self, self, self) # hack for X-Plane
         self.bxyz = matrix("0.0 0.0 0.0; 0.0 0.0 0.0; 0.0 0.0 0.0")
@@ -156,6 +162,7 @@ def datagramReceived(self, data, (host, port)):
         TD.LONGITUDE = unpack_from(fmt, data, 9+216+4)[0]
         TD.ALTITUDE = unpack_from(fmt, data, 9+216+8)[0]
         TD.SPEEDOVERGROUND = unpack_from(fmt, data, 9+12)[0]
+        #display.register_scalars({"SOG":TD.SPEEDOVERGROUND, "IAS":TD.AIRSPEED}, "Testing")
         display.register_scalars({"lat":TD.LATITUDE,"lon":TD.LONGITUDE,"alt":TD.ALTITUDE,"sog":TD.SPEEDOVERGROUND,"cog":degrees(TD.COURSEOVERGROUND)}, "Sensors")
         self.generate_virtual_magnetometer_readings(TD.ROLL,TD.PITCH,TD.HEADING)
         display.register_scalars({"bx":TD.BX,"by":TD.BY,"bz":TD.BZ,"true heading":degrees(TD.HEADING)}, "Sensors")
@@ -218,25 +225,25 @@ def sendThrottle(self, throttle):
         data_selection_packet = "DATA0\x19\x00\x00\x00" # throttle
         data = pack('ffffffff', throttle, throttle, throttle, throttle, throttle, throttle, throttle, throttle)
         data_selection_packet += data
-        self.sock.sendto(data_selection_packet,(get_ip_address(),49000))
+        self.sock.sendto(data_selection_packet,(safe_get_ip_address(),49000))
 
     def sendJoystick(self, joystick, rudder):
         data_selection_packet = "DATA0\x08\x00\x00\x00" # joystick
         data = pack('ffffffff', joystick[1], joystick[0], rudder, 0, 0, 0, 0, 0)
         data_selection_packet += data
-        self.sock.sendto(data_selection_packet,(get_ip_address(),49000))
+        self.sock.sendto(data_selection_packet,(safe_get_ip_address(),49000))
 
     def sendGearBrakes(self, gear, brakes):
         data_selection_packet = "DATA0\x0E\x00\x00\x00" # gear/brakes
         data = pack('ffffffff', gear, brakes, brakes, brakes, 0, 0, 0, 0)
         data_selection_packet += data
-        self.sock.sendto(data_selection_packet,(get_ip_address(),49000))
+        self.sock.sendto(data_selection_packet,(safe_get_ip_address(),49000))
 
     def sendFlaps(self, flaps):
         data_selection_packet = "DATA0\x0D\x00\x00\x00" # flaps
         data = pack('ffffffff', flaps, flaps, flaps, flaps, flaps, flaps, flaps, flaps)
         data_selection_packet += data
-        self.sock.sendto(data_selection_packet,(get_ip_address(),49000))
+        self.sock.sendto(data_selection_packet,(safe_get_ip_address(),49000))
 
 class XplaneIMU():
 
@@ -245,7 +252,7 @@ def __init__(self):
         We need to setup the conections, send a setup packet to X-Plane and then start listening.
         """
         self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-        self.sock.bind((get_ip_address(),49999))
+        self.sock.bind((safe_get_ip_address(),49999))
         self.send_data_selection_packet()
         self.listener = XplaneListener()
 
@@ -269,7 +276,7 @@ def send_data_selection_packet(self):
         data_selection_packet += "\x12\x00\x00\x00" # pitch and roll (for sanity check)
         data_selection_packet += "\x13\x00\x00\x00" # hpath (course over ground)
         data_selection_packet += "\x14\x00\x00\x00" # altimeter and GPS
-        self.sock.sendto(data_selection_packet,(get_ip_address(),UDP_SENDTO_PORT))
+        self.sock.sendto(data_selection_packet,(safe_get_ip_address(),UDP_SENDTO_PORT))
 
 if __name__ == "__main__":
     try: