def test_apply_noise_filter():
""" Tests 4.3 Filter library
Tests 4.3.2 Noise filtering
"""
data = DataCapture()
low_noise_filter = LowPassFilter("Chebyshev", 100)
high_noise_filter = HighPassFilter("Chebyshev", 5000)
low_noise = [] # random <100HZ noise
high_noise = [] # random >5000HZ noise
old_data = data.dataCapture()
low_filtered_data = data.applyFilter(low_noise_filter, 2)
high_filtered_data = data.applyFilter(high_noise_filter, 2)
errors = []
#TODO
# replace == with a in comparison to check if the noise is in the data
if low_noise == low_filtered_data:
errors.append("Low noise detected in filtered data")
if high_noise == high_filtered_data:
errors.append("High noise detected in filtered data")
# NOTE: currently returns PASSED as the two data types are both "NONE" and python considers them equal
assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
def __init__(self, max, servo_to_hinge, rate):
self.max = max # max min limit of the FINAL control surface deflection in degrees (e.g. 30 degrees of aileron deflection is +-15 deg)
self.in_to_servo = 1000 / 175 # ratio of input channel to the corresponding servo deflection in degrees (in our case 1000 equals ~170-175 deg)
self.servo_to_hinge = servo_to_hinge # ratio of the servo deflection in degrees to the corresponding control surface deflection, depends on the mechanical arrangement
self.rate = rate # target deflection speed of the control surface in deg / s
self.output = 0 # set to zero to initialise
self.antiAliasing = LowPassFilter(
1,
0.05) # anti-wonkifying smoothing of the input signal to the servo
def test_apply_filter():
"""Tests 4.3.1 Filtering method
"""
data = DataCapture()
dfilter = LowPassFilter("Chebyshev", 1000)
old_data = data.dataCapture()
filtered_data = data.applyFilter(dfilter, 2)
assert old_data != filtered_data, "No changes to filtered data"
def __init__(self, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10):
"""
kp = proportional gain term
ki = integral gain term
kd = derivative gain term
kff= feed forward velocity term. Applies signal relative to dervative of the target
derivative_corner = corner frequency of derivative filter.
Our real world signals are too noisy to use without significant filtering
kfa= feed forward acceleration term. Applies signal relative to the difference in
rate of change of the target and desired.
"""
self.updateGainConstants(kp, ki, kd, kff, kfa)
self.max_movement_rate = 1e6
# NOTE: it is important that these three variables are floating point to avoid truncation
self.prev_error = 0.0
self.prev_desired_pos = 0.0
self.integral_error_accumulator = 0.0
self.peak_detector = HystereticPeakDetector(0.0, -1.0, 1.0, math.pi / 20)
self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)
def test_check_avail_filters():
""" Tests 4.3 Filter library
Tests 4.3.3 Data filtering
"""
data = DataCapture()
lpf = LowPassFilter("Chebyshev", 250)
hpf = HighPassFilter("Bessel", 500)
maf = MovingAverageFilter(10)
pkf = PeakFilter(10, 10)
errors = []
# replace == with a check if noise data is contained in filtered data
if not lpf.getFilterInfo():
errors.append("Low pass filter does not exist")
if not hpf.getFilterInfo():
errors.append("High pass filter does not exist")
if not maf.getFilterInfo():
errors.append("Moving average filter does not exist")
if not pkf.getFilterInfo():
errors.append("Peak filter does not exist")
assert not errors, "Assert errors occured:\n{}".format("\n".join(errors))
from AltIMU_v3 import AltIMUv3
from filters import LowPassFilter, HighPassFilter
import time
import RPi.GPIO as GPIO
import math
GPIO.setmode(GPIO.BOARD)
GPIO.setup(7, GPIO.OUT)
GPIO.setup(10, GPIO.OUT)
# Setup Altimu
altimu = AltIMUv3()
altimu.enable()
# Initialize a low pass filter with a default value and a bias of 80%
low_pass_filter = LowPassFilter([0.0, 0.0, 1.0], 0.8)
while True:
accel = altimu.get_accelerometer_cal()
gyro = altimu.get_gyro_cal()
time.sleep(0.1)
if accel[2] < -1:
if gyro[2] > 30 or gyro[2] < -30:
GPIO.output(7, True)
if accel[2] > 0.95:
GPIO.output(7, True)
if accel[1] > .7:
GPIO.output(7, True)
else:
GPIO.output(7, False)
if resultado > .98:
