Working vortex speed sensor!

vortex/logs/easyplot.m

@@ -0,0 +1,5 @@
+load('lcmlog_2015_04_09_01.mat')
+figure
+hold on
+plot(vortex_sensor(:,3),vortex_sensor(:,1),'.')
+plot(vortex_sensor(:,3),vortex_sensor(:,2),'.')

vortex/logs/testing0409/40galzooka/lcmlog_2015_04_09_00.m

@@ -0,0 +1,9 @@
+function [d imFnames]=lcmlog_2015_04_09_00()
+full_fname = 'lcmlog_2015_04_09_00.mat';
+fname = '/Users/pflomacpro/crazyflie-tools/vortex/logs/testing0409/40galzooka/lcmlog_2015_04_09_00.mat';
+if (exist(full_fname,'file'))
+    filename = full_fname;
+else
+    filename = fname;
+end
+d = load(filename);

vortex/logs/testing0409/40galzooka/lcmlog_2015_04_09_01.m

@@ -0,0 +1,9 @@
+function [d imFnames]=lcmlog_2015_04_09_01()
+full_fname = 'lcmlog_2015_04_09_01.mat';
+fname = '/Users/pflomacpro/crazyflie-tools/vortex/logs/testing0409/40galzooka/lcmlog_2015_04_09_01.mat';
+if (exist(full_fname,'file'))
+    filename = full_fname;
+else
+    filename = fname;
+end
+d = load(filename);

vortex/logs/testing0409/toyzooka/lcmlog_2015_04_09_00.m

@@ -0,0 +1,9 @@
+function [d imFnames]=lcmlog_2015_04_09_00()
+full_fname = 'lcmlog_2015_04_09_00.mat';
+fname = '/Users/pflomacpro/crazyflie-tools/vortex/logs/testing0409/toyzooka/lcmlog_2015_04_09_00.mat';
+if (exist(full_fname,'file'))
+    filename = full_fname;
+else
+    filename = fname;
+end
+d = load(filename);

vortex/logs/testing0409/toyzooka/lcmlog_2015_04_09_01.m

@@ -0,0 +1,9 @@
+function [d imFnames]=lcmlog_2015_04_09_01()
+full_fname = 'lcmlog_2015_04_09_01.mat';
+fname = '/Users/pflomacpro/crazyflie-tools/vortex/logs/testing0409/toyzooka/lcmlog_2015_04_09_01.mat';
+if (exist(full_fname,'file'))
+    filename = full_fname;
+else
+    filename = fname;
+end
+d = load(filename);

vortex/vortextimer.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python2
+
+import u3
+import lcm
+import math
+from crazyflie_t import vortex_sensor_t
+import time
+
+class VortexSensor():
+
+  def __init__(self):
+    self.lookingforvortex = False
+    self.start = time.time()
+
+  def my_handler(self, channel, data):
+    msg = vortex_sensor_t.decode(data)
+
+    # Print statements for receiving messages
+
+    #print("Received message on channel \"%s\"" % channel)
+    #print("   sensor1   = %s" % str(msg.sensor1))
+    #print("   sensor2    = %s" % str(msg.sensor2))
+    #print("")
+
+    # Sensor 2 is up front
+    if msg.sensor2 > 5 and not self.lookingforvortex:
+      print("She's a beauty!  Vortex at the front sensor!")
+      self.start = time.time()
+      self.lookingforvortex = True
+
+    if msg.sensor1 > 5 and self.lookingforvortex:
+      print("She's at the rear now!")
+      delay = time.time() - self.start
+      print("That took " + str(delay) + " seconds!")
+      speed = 0.58 / delay
+      print("Estimated vortex speed is " + str(speed) + " meters / sec!")
+      self.lookingforvortex = False
+
+
+
+if __name__=="__main__":
+
+  print("Looking for vortexes!")
+
+  lc = lcm.LCM()
+  sensor = VortexSensor()
+  subscription = lc.subscribe("vortex_sensor", sensor.my_handler)
+
+  try:
+
+    while True:
+      lc.handle()
+
+  except KeyboardInterrupt:
+    exit(0)
+
+  lc.unsubscribe(subscription)

vortexsensor.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python2
+
+import u3
+import lcm
+import math
+from crazyflie_t import vortex_sensor_t
+
+
+
+if __name__=="__main__":
+
+    VOLTS_TO_PASCAL = 819 # Source: ardupilot AP_Airspeed_analog.cpp
+    _ratio = 1.9936 # Source: ardupilot AP_Airspeed_analog.cpp
+    _airspeed0 = 0
+    _airspeed2 = 0
+
+    # Calibrate by taking 2000 measurements and taking simple average
+    ain0offset = 0
+    ain2offset = 2
+
+
+    d = u3.U3()
+    lc = lcm.LCM()
+
+    CALIBRATION_TIME = 5 # desired calibration time in seconds
+    SENSOR_FREQUENCY = 500 # in seconds
+
+    for i in range(1,SENSOR_FREQUENCY*CALIBRATION_TIME):
+        ain0bits, = d.getFeedback(u3.AIN(0)) # Read from raw bits from AIN0
+        ain0Value = d.binaryToCalibratedAnalogVoltage(ain0bits, isLowVoltage=False, channelNumber=0)
+        ain0offset = (ain0offset*(i-1) + ain0Value) / i
+
+        ain2bits, = d.getFeedback(u3.AIN(2)) # Read from raw bits from AIN0
+        ain2Value = d.binaryToCalibratedAnalogVoltage(ain2bits, isLowVoltage=False, channelNumber=0)
+        ain2offset = (ain2offset*(i-1) + ain2Value) / i
+
+
+    try:
+
+        while True:
+
+
+
+            msg = vortex_sensor_t()
+
+            ain0bits, = d.getFeedback(u3.AIN(0)) # Read from raw bits from AIN0
+            ain0Value = d.binaryToCalibratedAnalogVoltage(ain0bits, isLowVoltage=False, channelNumber=0)
+
+            airspeed_pressure0 = (ain0Value - ain0offset)*VOLTS_TO_PASCAL
+            raw_pressure0      = airspeed_pressure0
+            if airspeed_pressure0 < 0:
+                _raw_airspeed0 = -math.sqrt( -airspeed_pressure0 * _ratio)
+            else:
+                _raw_airspeed0 = math.sqrt( airspeed_pressure0 * _ratio)
+            _airspeed0         = 0.7 * _airspeed0  +  0.3 * _raw_airspeed0;
+            msg.sensor1 = _raw_airspeed0 #smoothing turned off for now
+
+            ain2bits, = d.getFeedback(u3.AIN(2)) # Read from raw bits from AIN2
+            ain2Value = d.binaryToCalibratedAnalogVoltage(ain2bits, isLowVoltage=False, channelNumber=2)
+
+            airspeed_pressure2 = (ain2Value - ain2offset)*VOLTS_TO_PASCAL
+            raw_pressure2      = airspeed_pressure2
+            if airspeed_pressure2 < 0:
+                _raw_airspeed2 = -math.sqrt( -airspeed_pressure2 * _ratio)
+            else:
+                _raw_airspeed2 = math.sqrt( airspeed_pressure2 * _ratio)
+            _airspeed2         = 0.7 * _airspeed2  +  0.3 * _raw_airspeed2;
+            msg.sensor2 = _raw_airspeed2 #smoothing turned off for now
+
+
+            lc.publish('vortex_sensor',msg.encode())
+
+
+    except KeyboardInterrupt:
+        exit(0)