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_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))
class Actuator:
# class for the control surface actuator
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 step(
self, stick_input, sas_input, dt
): #stick_input is in 0 to 1000 whereas sas_input is stability augmentation input in degrees corresponding to the surface deflection
max_1 = self.max * self.servo_to_hinge * self.in_to_servo
stick_input_1 = mapInput(stick_input, 0, 1000, 0, max_1)
rate_1 = self.rate * self.servo_to_hinge * self.in_to_servo
sas_to_in = sas_input * self.servo_to_hinge * self.in_to_servo
input_1 = self.antiAliasing.step(
limit(stick_input_1 + sas_to_in, max_1, 0), dt)
self.output = limitByRate(input_1, self.output, rate_1, dt)
return self.output
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:
class PIDController:
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 updateGainConstants(self, kp, ki, kd, kff=None, kfa=None):
self.kp = kp
self.ki = ki
self.kd = kd
if kff is not None:
self.kff = kff
else:
self.kff = 0.0
if kfa is not None:
self.kfa = kfa
else:
self.kfa = 0.0
def update(self, desired_pos, measured_pos):
delta_time = time_sources.global_time.getDelta()
# bound the desired position
#desired_pos = self.boundDesiredPosition(desired_pos)
desired_vel = (desired_pos - self.prev_desired_pos) / delta_time
error = desired_pos - measured_pos
self.peak_detector.update(error)
if 0:
if self.peak_detector.hasConverged():
if self.peak_detector.isUnstable():
warningstring = ("LimbController: Maximum error for the" +
" desired point has increased for %d seconds," +
" but is within converged range. Might be unstable." %
self.peak_detector.getResolveTime())
#logger.warning(warningstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
elif self.peak_detector.isLimitCycle():
warningstring = ("LimbController: Maximum error for the" +
" desired point has increased once or more for %d seconds," +
" but is within converged range. Might be unstable." %
self.peak_detector.getResolveTime())
#logger.warning(warningstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
else:
if self.peak_detector.isUnstable():
errorstring = ("LimbController: Maximum error for the desired point" +
"has increased for %d seconds. System potentially unstable." %
self.peak_detector.getResolveTime())
#logger.error(errorstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
raise ValueError(errorstring)
elif self.peak_detector.isLimitCycle():
errorstring = ("LimbController: Controller has not converged" +
"over %d seconds. System potentially in a limit cycle." %
self.peak_detector.getResolveTime())
#logger.error(errorstring,
# desired_pos=desired_pos,
# measured_pos=measured_pos,
# error=error,
# bad_value=error)
raise ValueError(errorstring)
self.integral_error_accumulator += self.ki * error * delta_time
derivative_error = self.d_lowpass.update((error - self.prev_error) / delta_time)
velocity_error = desired_vel - derivative_error
self.prev_error = error
self.prev_desired_pos = desired_pos
actuator_command = (self.kp * error +
self.integral_error_accumulator +
self.kd * derivative_error +
self.kff * desired_vel +
self.kfa * velocity_error)
#actuator_command = self.boundActuatorCommand(actuator_command, measured_pos)
return actuator_command
def isErrorInBounds(self, error, measured_pos):
"""tests whether or not the error signal is within reasonable range
not checking for NaN, since both desired and measured position
are tested for that
"""
#makes sure the error is bounded by a single leg rotation
error = error % (2 * math.pi)
error_min = -math.pi / 2
error_max = math.pi / 2
#is error within available soft range?
if error > (measured_pos - error_min) or error > (error_max - measured_pos):
#logger.error("LimbController.isErrorInBounds: error out of soft bounds.",
# error=error,
# measured_pos=measured_pos)
raise ValueError("Error signal points to a position out of soft bounds.")
return error
def boundDesiredPosition(self, desired_pos):
#caps desired position to soft movement range
if math.isnan(desired_pos):
#logger.error("LimbController.boundDesiredPosition: NaN where aN expected!",
# desired_pos=desired_pos,
# bad_value="desired_pos")
raise ValueError("LimbController: desired_pos cannot be NaN.")
command_min = -20
command_max = 20
if desired_pos < command_min or desired_pos > command_max:
#logger.error("LimbController.boundDesiredPosition:"+
# " desired position out of bounds!",
# desired_pos=desired_pos,
# command_min=command_min,
# command_max=command_max,
# bad_value="desired_pos")
raise ValueError("LimbController.boundDesiredPosition:" +
" desired position out of soft bounds")
bounded_pos = saturate(desired_pos, command_min, command_max)
return bounded_pos
def boundActuatorCommand(self, actuator_command, measured_pos):
#prevent the controller from commanding an unsafely fast actuator move
if (abs(actuator_command - measured_pos) / time_sources.global_time.getDelta() > self.max_movement_rate):
raise ValueError("LimbController: Actuator command would cause" +
"joint to move at an unsafe rate.")
return actuator_command