def __init__(self,
pipe_write,
pipe_read,
time_budget=40,
time_period=50,
dt=None,
beta=0.0):
self.pipe_read = pipe_read # it's useful for checking if the last value was consumed
self.pipe_write = pipe_write
self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
self.tof.open(reset=True)
self.time_budget = time_budget
self.time_period = time_period # TIME_PERIOD MUST BE > THAN TIME_BUDGET
freq = 1 / (self.time_period / 1000)
self.tof.set_timing(self.time_budget * 1000, self.time_period)
self.tof.start_ranging(0)
self.prev_d = 0
dt = dt if dt else freq
self.filter_d = FadingMemoryFilter([self.prev_d, 0.],
dt,
order=1,
beta=beta)
print(NODE_STR + "ToF Node Initialized @ {}Hz!".format(freq))
class ToF():
def __init__(self,
pipe_write,
pipe_read,
time_budget=40,
time_period=50,
dt=None,
beta=0.0):
self.pipe_read = pipe_read # it's useful for checking if the last value was consumed
self.pipe_write = pipe_write
self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
self.tof.open(reset=True)
self.time_budget = time_budget
self.time_period = time_period # TIME_PERIOD MUST BE > THAN TIME_BUDGET
freq = 1 / (self.time_period / 1000)
self.tof.set_timing(self.time_budget * 1000, self.time_period)
self.tof.start_ranging(0)
self.prev_d = 0
dt = dt if dt else freq
self.filter_d = FadingMemoryFilter([self.prev_d, 0.],
dt,
order=1,
beta=beta)
print(NODE_STR + "ToF Node Initialized @ {}Hz!".format(freq))
def close(self):
self.tof.stop_ranging()
print(NODE_STR + "Shutting down ToF...")
def read(self):
RangingMeasurementData = self.tof.get_RangingMeasurementData()
RangingMeasurementData['time_stamp'] = time.time()
if not self.pipe_read.poll(): #timeout=None => blocking
# print(NODE_STR + "New reading!")
if not RangingMeasurementData['RangeStatus']:
d = RangingMeasurementData['RangeMilliMeter']
self.filter_d.update(d)
d = self.filter_d.x[0]
self.prev_d = d
RangingMeasurementData['RangeMilliMeter'] = d
self.pipe_write.send(RangingMeasurementData)
# Important Keys:
# - RangeMilliMeter
# - SigmaMilliMeter
# - RangeStatus
else:
print(NODE_STR + "RangeStatus problem")
def dotest_1d(order, beta):
fm = FadingMemoryFilter(x0=0, dt=1, order=order, beta=beta)
xs = [x for x in range(0,50)]
fxs = []
for x in xs:
data = x+randn()*3
fm.update(data)
fxs.append(fm.x[0])
def dotest_2d_data():
""" tests having multidimensional data for x"""
fm = FadingMemoryFilter(x0=np.array([[0.,2.],[0.,0.]]), dt=1, order=1, beta=.6)
xs = [x for x in range(0,50)]
for x in xs:
data = [x+randn()*3, x+2+randn()*3]
fm.update(data)
def dotest_1d(order, beta):
fm = FadingMemoryFilter(x0=0, dt=1, order=order, beta=beta)
xs = [x for x in range(0,50)]
fxs = []
for x in xs:
data = x+randn()*3
fm.update(data)
plt.scatter(x, fm.x[0], c = 'r')
fxs.append(fm.x[0])
plt.scatter(x,data,c='b')
plt.plot(fxs, c='r')
def dotest_1d(order, beta):
fm = FadingMemoryFilter(x0=0, dt=1, order=order, beta=beta)
xs = [x for x in range(0, 50)]
fxs = []
for x in xs:
data = x + randn() * 3
fm.update(data)
plt.scatter(x, fm.x[0], c='r')
fxs.append(fm.x[0])
plt.scatter(x, data, c='b')
plt.plot(fxs, c='r')
def dotest_2d_data():
""" tests having multidimensional data for x"""
fm = FadingMemoryFilter(x0=np.array([[0., 2.], [0., 0.]]),
dt=1,
order=1,
beta=.6)
xs = [x for x in range(0, 50)]
for x in xs:
data = [x + randn() * 3, x + 2 + randn() * 3]
fm.update(data)
plt.scatter(fm.x[0, 0], fm.x[0, 1], c='r')
plt.scatter(data[0], data[1], c='b')
def update_tof(self, dt=1 / 20, beta=0.8):
# Update the ToF values
if self.control_tof_pipe_read.poll(2):
RangingMeasurementData = self.control_tof_pipe_read.recv()
if self.imu_ready: # the corrections depend on the imu
altitude_sensor = RangingMeasurementData[
'RangeMilliMeter'] / 1000 #[m]
# self.tof_time_stamp = RangingMeasurementData['time_stamp']
self.tof_std = RangingMeasurementData['SigmaMilliMeter']
if 3.0 > altitude_sensor > 0.0: # outlier rejection...
#
# TODO: altitude compensation according to ROLL and PITCH angles.
#
# The sensor is not at the center axis of rotation (offset on pitch, but very small)
# ...ignore the offset to make calculations faster
self.tof_altitude = np.cos(
self.imu[2][0] * PI_OVER_180) * np.cos(
self.imu[2][1] * PI_OVER_180) * altitude_sensor
if not self.tof_ready:
self.tof_ready = True
self.tof_vel = 0.0
self._filter_tof = FadingMemoryFilter(
[self.tof_altitude, 0.0, 0.0], dt, order=2, beta=beta)
self._filter_tof_v = FadingMemoryFilter([0.0, 0.0, 0.0],
dt,
order=2,
beta=beta)
self.tof_time_stamp = time.time()
self.tof_prev_altitude = self.tof_altitude
self.tof_prev_time_stamp = self.tof_time_stamp
return
if self.tof_ready:
self._filter_tof.update(self.tof_altitude)
self.tof_altitude = self._filter_tof.x[0]
self.tof_time_stamp = time.time()
self.tof_diff = (self.tof_altitude - self.tof_prev_altitude)
self.tof_dt = (self.tof_time_stamp - self.tof_prev_time_stamp)
self._filter_tof_v.update(self.tof_diff / self.tof_dt)
self.tof_vel = self._filter_tof_v.x[0]
self.tof_prev_altitude = self.tof_altitude
self.tof_prev_time_stamp = self.tof_time_stamp
def __init__(self, kp, ki, kd, integral_threshold = 10):
# Proportional gain Kp
self.kp = kp
# Integral gain Ki
self.ki = ki
# Derivative gain Kd
self.kd = kd
# Sum of error*dt
self.integral = 0
# THRESHOLD for integrator
self.integral_threshold = integral_threshold
# Previous error
self.pre_error = None
self.integrate = True
self._filter_d = FadingMemoryFilter([0., 0., 0.], 1/20, order=2, beta=0.80)
def __init__(self, pipe_write, pipe_read, dt=None, beta=0.0):
self.pipe_read = pipe_read # it's useful for checking if the last value was consumed
self.pipe_write = pipe_write
# spi_port=1, spi_cs=2, spi_cs_gpio=16 (pin 36) => compatible with the hat
# Using /boot/config.txt and dtoverlay=spi1-3cs
keep_trying = True
while keep_trying:
try:
self.flo = PMW3901(spi_port=1,
spi_cs=2,
spi_cs_gpio=16,
secret_sauce=3)
print(NODE_STR + "Sensor initialized!")
keep_trying = False
except RuntimeError:
print(NODE_STR + "Sensor not initialized...")
time.sleep(0.5)
self.flo.set_rotation(
180) # X points to the same direction as the drone (Z up)
self.OptFlowMeasurementData = {
'dx': 0,
'dy': 0,
'Quality': 0,
'time_stamp': 0,
'flow_cte': 30.0 / (4.2 * 3.14 / 180.0)
}
freq = 1 / self.flo.SAMPLE_INTERVAL
self.prev_dx = 0
self.prev_dy = 0
dt = dt if dt else freq
self.filter_x = FadingMemoryFilter([self.prev_dx, 0.],
dt,
order=1,
beta=beta)
self.filter_y = FadingMemoryFilter([self.prev_dy, 0.],
dt,
order=1,
beta=beta)
print(NODE_STR + "OptFlow Node Initialized @ {}Hz!".format(freq))
def test_ghk_formulation():
beta = .6
g = 1-beta**3
h = 1.5*(1+beta)*(1-beta)**2
k = 0.5*(1-beta)**3
f1 = GHKFilter(0,0,0,1, g, h, k)
f2 = FadingMemoryFilter(x0=0, dt=1, order=2, beta=beta)
def fx(x):
return .02*x**2 + 2*x - 3
for i in range(1,100):
z = fx(i)
f1.update(z)
f2.update(z)
assert abs(f1.x-f2.x[0]) < 1.e-80
def test_ghk_formulation():
beta = .6
g = 1 - beta**3
h = 1.5 * (1 + beta) * (1 - beta)**2
k = 0.5 * (1 - beta)**3
f1 = GHKFilter(0, 0, 0, 1, g, h, k)
f2 = FadingMemoryFilter(x0=0, dt=1, order=2, beta=beta)
def fx(x):
return .02 * x**2 + 2 * x - 3
for i in range(1, 100):
z = fx(i)
f1.update(z)
f2.update(z)
assert abs(f1.x - f2.x[0]) < 1.e-80
class Control():
def __init__(self,
control_optflow_pipe_read,
control_tof_pipe_read,
control_imu_pipe_read,
ext_control_pipe_write,
ext_control_pipe_read,
altitude_setpoint=0.0,
pid_dict=None,
debug=False):
self.control_optflow_pipe_read = control_optflow_pipe_read
self.control_tof_pipe_read = control_tof_pipe_read
self.control_imu_pipe_read = control_imu_pipe_read
self.ext_control_pipe_write = ext_control_pipe_write
self.ext_control_pipe_read = ext_control_pipe_read
self.altitude_setpoint = altitude_setpoint
self.takeoff_threshold = 0.035 # in meters
self.takeoff_gain = 1500.0
self.x_setpoint = 0.0
self.y_setpoint = 0.0
self.DEBUG = debug
#
# Define all the parameters
#
# The variables below MUST be initialized with False
self.tof_ready = False
self.imu_ready = False
self.optflow_ready = False
# The value used to cancel gravity when the
# battery is fully charged (12.60V)
self.BASE_THROTTLE = 400 #580
# Gravity cancelation
# Betaflight has a vbat_pid_compensation!
self.gravity_cancelation = self.BASE_THROTTLE
# Basic parameters
self.ABS_MAX_VALUE_ROLL = 10 # PID Roll limit (saturation)
self.ABS_MAX_VALUE_PITCH = 10 # PID Pitch limit (saturation)
self.ABS_MAX_VALUE_THROTTLE = 300 # PID Throttle limit (saturation)
# ToF sensor offset
self.TOFOFFSET_Z = 0.030 # [m]
self.TOFOFFSET_X = 0.023 # [m]
self.TOFOFFSET_R = np.sqrt(self.TOFOFFSET_Z**2 + self.TOFOFFSET_Z**2)
self.TOFOFFSET_ANGLE = np.tan(self.TOFOFFSET_Z / self.TOFOFFSET_X)
#
# PID Gains
#
# PID Init
# self.throttle_pid = PID(pid_dict['kpz_p'], pid_dict['kiz_p'], pid_dict['kdz_p'], integral_threshold=1000) #throttle PID
self.throttle_pid_vel = PID(pid_dict['kpz_v'],
pid_dict['kiz_v'],
pid_dict['kdz_v'],
integral_threshold=100) #throttle PID
self.throttle_pid_takeoff = PID(
0.0, 1.0, 0.0,
integral_threshold=300) #throttle PID for take off only
self.roll_pid_vel = PID(pid_dict['kpy_v'], pid_dict['kiy_v'],
pid_dict['kdy_v']) #roll PID
self.pitch_pid_vel = PID(pid_dict['kpx_v'], pid_dict['kix_v'],
pid_dict['kdx_v']) #pitch PID
self.roll_pid_pos = PID(pid_dict['kpy_p'], pid_dict['kiy_p'],
pid_dict['kdy_p']) #roll PID
self.pitch_pid_pos = PID(pid_dict['kpx_p'], pid_dict['kix_p'],
pid_dict['kdx_p']) #pitch PID
self.CMDS_init = {}
self.CMDS_init['throttle'] = 0.0
self.CMDS_init['roll'] = 0.0
self.CMDS_init['pitch'] = 0.0
self.CMDS_init['p_ctrl'] = [0.0, 0.0]
self.CMDS_init['error_p'] = [0.0, 0.0, 0.0,
0.0] # error_x,error_y,error_z,error_h
self.CMDS_init['error_v'] = [0.0, 0.0, 0.0, 0.0
] # error_vx,error_vy,error_vz,error_vh
self.CMDS_init['velocity'] = [0.0, 0.0, 0.0, 0.0] # vx,vy,vz,vh
self.CMDS_init['time_stamp'] = 0.0
self.next_throttle = 0.0
self.tof_altitude = 0.0
self.autonomous_mode = False
self.prev_step_time = time.time() # only for the derivative
print(NODE_STR + "Control Node Initialized!")
def value_limit(self, output, limit):
'''Set the value not exceed the limited value'''
if abs(output) > limit:
return limit * int(output / abs(output))
else:
return output
def truncate(self, data, dp=2):
''' Truncate the value down to 2 dp as default'''
return (int(data * (10**dp))) / (10**dp)
def update_imu(self):
# Update the IMU values (independently if autonomous mode is on or off)
if self.control_imu_pipe_read.poll(
2): # timeout=2 is for closing the thread at the end
# ideally it should call recv directly, but it
# will block the thread at the end
self.imu, self.mean_batt_voltage, self.imu_time_stamp = self.control_imu_pipe_read.recv(
)
# imu => [[accX,accY,accZ], [gyroX,gyroY,gyroZ], [roll,pitch,yaw]]
self.imu_ready = True
else:
print(NODE_STR + "Imu timeout?!?!?")
def update_tof(self, dt=1 / 20, beta=0.8):
# Update the ToF values
if self.control_tof_pipe_read.poll(2):
RangingMeasurementData = self.control_tof_pipe_read.recv()
if self.imu_ready: # the corrections depend on the imu
altitude_sensor = RangingMeasurementData[
'RangeMilliMeter'] / 1000 #[m]
# self.tof_time_stamp = RangingMeasurementData['time_stamp']
self.tof_std = RangingMeasurementData['SigmaMilliMeter']
if 3.0 > altitude_sensor > 0.0: # outlier rejection...
#
# TODO: altitude compensation according to ROLL and PITCH angles.
#
# The sensor is not at the center axis of rotation (offset on pitch, but very small)
# ...ignore the offset to make calculations faster
self.tof_altitude = np.cos(
self.imu[2][0] * PI_OVER_180) * np.cos(
self.imu[2][1] * PI_OVER_180) * altitude_sensor
if not self.tof_ready:
self.tof_ready = True
self.tof_vel = 0.0
self._filter_tof = FadingMemoryFilter(
[self.tof_altitude, 0.0, 0.0], dt, order=2, beta=beta)
self._filter_tof_v = FadingMemoryFilter([0.0, 0.0, 0.0],
dt,
order=2,
beta=beta)
self.tof_time_stamp = time.time()
self.tof_prev_altitude = self.tof_altitude
self.tof_prev_time_stamp = self.tof_time_stamp
return
if self.tof_ready:
self._filter_tof.update(self.tof_altitude)
self.tof_altitude = self._filter_tof.x[0]
self.tof_time_stamp = time.time()
self.tof_diff = (self.tof_altitude - self.tof_prev_altitude)
self.tof_dt = (self.tof_time_stamp - self.tof_prev_time_stamp)
self._filter_tof_v.update(self.tof_diff / self.tof_dt)
self.tof_vel = self._filter_tof_v.x[0]
self.tof_prev_altitude = self.tof_altitude
self.tof_prev_time_stamp = self.tof_time_stamp
def update_optflow(self,
optflow_min_quality=30,
dt=1 / 20,
beta=0.8,
gyro_threshold=100):
# Update the Optical Flow values
if self.control_optflow_pipe_read.poll(2):
OptFlowMeasurementData = self.control_optflow_pipe_read.recv(
) # it will block until a brand new value comes.
if self.tof_ready and self.imu_ready: # the corrections depend on the tof and imu
dx = OptFlowMeasurementData['dx']
dy = OptFlowMeasurementData['dy']
self.optflow_quality = OptFlowMeasurementData['Quality']
# self.optflow_time_stamp = OptFlowMeasurementData['time_stamp']
# See equations 6.55 and 6.56 for the compensations on the optical flow:
# Modelling and Control of the Crazyflie Quadrotor for Aggressive and
# Autonomous Flight by Optical Flow Driven State Estimation
# http://lup.lub.lu.se/luur/download?func=downloadFile&recordOId=8905295&fileOId=8905299
# and also see Crazyflie firmware optical flow: https://git.io/Je59e
# the FC gives values in degrees/sec => so, convert to rad/s
gyro_y = self.imu[1][0] * PI_OVER_180 # roll
gyro_x = self.imu[1][1] * PI_OVER_180 # pitch
altitude = self.tof_altitude
#
# DX and DY NEED TO BE ROTATED ACCORDING TO YAW ANGLE...
#
#
if not self.optflow_ready:
self.optflow_ready = True
# Optical Flow sensor (PMW3901) details
NofPixels = 30.0
Aperture = (4.2 * PI_OVER_180) # to rad
self.ap_npix = Aperture / NofPixels
self.optflow_prev_vel_y = 0.0
self.optflow_prev_vel_x = 0.0
self.optflow_time_stamp = time.time()
self.optflow_prev_time_stamp = self.optflow_time_stamp
self._optflow_time_stamp = self.optflow_time_stamp
self._optflow_prev_time_stamp = self._optflow_time_stamp
self.optflow_dt = 0.0
self.optflow_vel_y = 0.0
self.optflow_vel_x = 0.0
self.optflow_x = 0.0
self.optflow_y = 0.0
self._filter_optflow_vx = FadingMemoryFilter(
[0.0, 0.0, 0.0], dt, order=2, beta=beta)
self._filter_optflow_vy = FadingMemoryFilter(
[0.0, 0.0, 0.0], dt, order=2, beta=beta)
return
else:
self._optflow_time_stamp = time.time()
self._optflow_dt = (self._optflow_time_stamp -
self._optflow_prev_time_stamp)
if self.optflow_quality > optflow_min_quality:
if abs(
gyro_x
) < gyro_threshold: # simple outlier rejection...
self.optflow_vel_x = altitude * (
dx * self.ap_npix / self._optflow_dt + gyro_x)
if abs(
gyro_y
) < gyro_threshold: # simple outlier rejection...
self.optflow_vel_y = altitude * (
dy * self.ap_npix / self._optflow_dt + gyro_y)
self._optflow_prev_time_stamp = self._optflow_time_stamp
if self.optflow_ready:
self.optflow_time_stamp = time.time()
self.optflow_dt = (self.optflow_time_stamp -
self.optflow_prev_time_stamp)
self._filter_optflow_vx.update(self.optflow_vel_x)
self._filter_optflow_vy.update(self.optflow_vel_y)
self.optflow_vel_x = self._filter_optflow_vx.x[0]
self.optflow_vel_y = self._filter_optflow_vy.x[0]
self.optflow_x += self.optflow_dt * self.optflow_vel_x
self.optflow_y += self.optflow_dt * self.optflow_vel_y
self.optflow_prev_vel_x = self.optflow_vel_x
self.optflow_prev_vel_y = self.optflow_vel_y
self.optflow_prev_time_stamp = self.optflow_time_stamp
def step(self, min_dt=0.1):
# Resets the last commands
value_available = False
# This order is important because it follows the dependencies.
# There's no guarantee they will update the values, but the PID
# controller will generate new control outputs after the first
# time the values are available.
self.update_imu()
self.update_tof(beta=0.8)
self.update_optflow(
beta=0.8) # this filter will actuate on the ground velocity
#
# Here I need to process setpoints received from a pipe
# to allow an external process (running something like a CNN) to control the drone.
# It will always update the last setpoints and if nothing is received the controller
# will just keep seeking the last setpoints received or set by default.
# Read the joystick trigger: auto mode or not
if self.ext_control_pipe_write.poll():
# cognifly_main will send True or False
# When cognifly_main sends False, it means all future
# commands sent from the control_node will be ignored.
self.autonomous_mode = self.ext_control_pipe_write.recv()
print(
NODE_STR +
f"Received self.autonomous_mode value: {self.autonomous_mode} - [imu:{self.imu_ready}, tof:{self.tof_ready}, optflow:{self.optflow_ready}]"
)
if self.autonomous_mode:
self.CMDS = self.CMDS_init.copy()
if self.tof_ready:
if (self.tof_altitude < 0.05):
self.throttle_pid_takeoff.integrate = True
self.last_takeoff_altitude = 0.0
else:
print(NODE_STR + "Tof was not available / ready!")
return 1 # this will force the system to shutdown
# Everything depends on tof, so no tof no pid...
if self.autonomous_mode and self.tof_ready:
# Calculate next Throttle
dt = time.time() - self.prev_step_time
self.prev_step_time = time.time()
error_z = self.altitude_setpoint - self.tof_altitude
# trying to keep the velocity below 0.5x the max velocity that gravity alone could stop
# before reaching the error_z = 0 (that's the "/2.0")
error_vz = np.sign(error_z) * np.sqrt(
2 * 9.81 * abs(error_z)) / 2.0 - self.tof_vel
self.CMDS['error_p'][2] = error_z
self.CMDS['error_v'][2] = error_vz
self.CMDS['velocity'][2] = self.tof_vel
if (self.tof_altitude > self.takeoff_threshold
) and self.throttle_pid_takeoff.integrate:
self.throttle_pid_takeoff.integrate = False
self.gravity_cancelation = self.gravity_cancelation + self.value_limit(
self.next_throttle, self.ABS_MAX_VALUE_THROTTLE)
print(
f"{NODE_STR} - TAKE OFF FINISHED >> {self.gravity_cancelation:.4f}, {self.tof_altitude:.4f}, {self.imu[0][2]:.4f}"
)
if self.throttle_pid_takeoff.integrate:
if self.tof_altitude >= self.last_takeoff_altitude:
# a high value of self.takeoff_gain will not be a problem IFF a controller takes over just after it
# otherwise it will probably generate a value that will accelerate upwards instead of only cancelling gravity...
self.next_throttle = self.throttle_pid_takeoff.calc(
self.takeoff_gain *
(self.takeoff_threshold - self.tof_altitude),
dt=0.1)
self.last_takeoff_altitude = self.tof_altitude
else:
self.next_throttle = self.throttle_pid_vel.calc(error_vz, dt)
print(
f'{NODE_STR} - HOVERING >> P:{self.throttle_pid_vel.p_value:.4f}, I:{self.throttle_pid_vel.i_value:.4f}, D:{self.throttle_pid_vel.d_value:.4f}, dt:{dt:.4f}, t:{(self.prev_step_time):.4f}'
)
print(
f'{NODE_STR} - batt:{self.mean_batt_voltage:.4f}, alt:{self.tof_altitude:.4f},{self.tof_vel:.4f}, thr:{self.next_throttle:.4f}, imu:{self.imu[0][2]:.4f}'
)
# Gravity cancelation
# self.gravity_cancelation = self.gravity_cancelation / ((np.cos(self.imu[2][0]*INV_PI_OVER_180)) * (np.cos(self.imu[2][1]*INV_PI_OVER_180)))
self.CMDS['throttle'] = self.value_limit(
self.next_throttle,
self.ABS_MAX_VALUE_THROTTLE) + self.gravity_cancelation
# Calculate next Roll and Pitch
if self.optflow_ready:
# Position control (outer loop)
error_x = (self.x_setpoint - self.optflow_x)
self.CMDS['error_p'][0] = error_x
# next_pitch = self.pitch_pid_pos.calc(error_x, dt)
# self.CMDS['p_ctrl'][0] = self.value_limit(next_pitch, self.ABS_MAX_VALUE_PITCH)
# self.CMDS['pitch'] = self.value_limit(next_pitch, self.ABS_MAX_VALUE_ROLL)
error_y = (self.y_setpoint - self.optflow_y)
self.CMDS['error_p'][1] = error_y
# next_roll = self.roll_pid_pos.calc(-error_y, dt)
# self.CMDS['p_ctrl'][1] = self.value_limit(next_roll, self.ABS_MAX_VALUE_ROLL)
# self.CMDS['roll'] = self.value_limit(next_roll, self.ABS_MAX_VALUE_ROLL)
# # Velocity control (inner loop)
error_vx = (0.0 - self.optflow_vel_x
) #(self.CMDS['p_ctrl'][0] - self.optflow_vel_x)
self.CMDS['error_v'][0] = error_vx
self.CMDS['velocity'][0] = self.optflow_vel_x
next_pitch = self.pitch_pid_vel.calc(error_vx, dt)
self.CMDS['pitch'] = self.value_limit(next_pitch,
self.ABS_MAX_VALUE_PITCH)
print(
f'{NODE_STR} - POS.X >> P:{self.pitch_pid_vel.p_value:.4f}, I:{self.pitch_pid_vel.i_value:.4f}, D:{self.pitch_pid_vel.d_value:.4f}, dt:{dt:.4f}, t:{(self.prev_step_time):.4f}'
)
error_vy = (0.0 - self.optflow_vel_y
) #(self.CMDS['p_ctrl'][1] - self.optflow_vel_y)
self.CMDS['error_v'][1] = error_vy
self.CMDS['velocity'][1] = self.optflow_vel_y
next_roll = self.roll_pid_vel.calc(-error_vy, dt)
self.CMDS['roll'] = self.value_limit(next_roll,
self.ABS_MAX_VALUE_ROLL)
print(
f'{NODE_STR} - POS.Y >> P:{self.roll_pid_vel.p_value:.4f}, I:{self.roll_pid_vel.i_value:.4f}, D:{self.roll_pid_vel.d_value:.4f}, dt:{dt:.4f}, t:{(self.prev_step_time):.4f}'
)
print(f'{NODE_STR} - x:{self.optflow_x}, y:{self.optflow_y}')
self.CMDS['time_stamp'] = self.prev_step_time
# TODO: Decide if it's better to reset tof_ready and optflow_ready to avoid PID calc using old values...
value_available = True
# Send out the CMDS values back to the joystick loop
if value_available:
if not self.ext_control_pipe_read.poll(
): # verify if the last value was consumed
self.ext_control_pipe_write.send(self.CMDS)
return 0
def update_optflow(self,
optflow_min_quality=30,
dt=1 / 20,
beta=0.8,
gyro_threshold=100):
# Update the Optical Flow values
if self.control_optflow_pipe_read.poll(2):
OptFlowMeasurementData = self.control_optflow_pipe_read.recv(
) # it will block until a brand new value comes.
if self.tof_ready and self.imu_ready: # the corrections depend on the tof and imu
dx = OptFlowMeasurementData['dx']
dy = OptFlowMeasurementData['dy']
self.optflow_quality = OptFlowMeasurementData['Quality']
# self.optflow_time_stamp = OptFlowMeasurementData['time_stamp']
# See equations 6.55 and 6.56 for the compensations on the optical flow:
# Modelling and Control of the Crazyflie Quadrotor for Aggressive and
# Autonomous Flight by Optical Flow Driven State Estimation
# http://lup.lub.lu.se/luur/download?func=downloadFile&recordOId=8905295&fileOId=8905299
# and also see Crazyflie firmware optical flow: https://git.io/Je59e
# the FC gives values in degrees/sec => so, convert to rad/s
gyro_y = self.imu[1][0] * PI_OVER_180 # roll
gyro_x = self.imu[1][1] * PI_OVER_180 # pitch
altitude = self.tof_altitude
#
# DX and DY NEED TO BE ROTATED ACCORDING TO YAW ANGLE...
#
#
if not self.optflow_ready:
self.optflow_ready = True
# Optical Flow sensor (PMW3901) details
NofPixels = 30.0
Aperture = (4.2 * PI_OVER_180) # to rad
self.ap_npix = Aperture / NofPixels
self.optflow_prev_vel_y = 0.0
self.optflow_prev_vel_x = 0.0
self.optflow_time_stamp = time.time()
self.optflow_prev_time_stamp = self.optflow_time_stamp
self._optflow_time_stamp = self.optflow_time_stamp
self._optflow_prev_time_stamp = self._optflow_time_stamp
self.optflow_dt = 0.0
self.optflow_vel_y = 0.0
self.optflow_vel_x = 0.0
self.optflow_x = 0.0
self.optflow_y = 0.0
self._filter_optflow_vx = FadingMemoryFilter(
[0.0, 0.0, 0.0], dt, order=2, beta=beta)
self._filter_optflow_vy = FadingMemoryFilter(
[0.0, 0.0, 0.0], dt, order=2, beta=beta)
return
else:
self._optflow_time_stamp = time.time()
self._optflow_dt = (self._optflow_time_stamp -
self._optflow_prev_time_stamp)
if self.optflow_quality > optflow_min_quality:
if abs(
gyro_x
) < gyro_threshold: # simple outlier rejection...
self.optflow_vel_x = altitude * (
dx * self.ap_npix / self._optflow_dt + gyro_x)
if abs(
gyro_y
) < gyro_threshold: # simple outlier rejection...
self.optflow_vel_y = altitude * (
dy * self.ap_npix / self._optflow_dt + gyro_y)
self._optflow_prev_time_stamp = self._optflow_time_stamp
if self.optflow_ready:
self.optflow_time_stamp = time.time()
self.optflow_dt = (self.optflow_time_stamp -
self.optflow_prev_time_stamp)
self._filter_optflow_vx.update(self.optflow_vel_x)
self._filter_optflow_vy.update(self.optflow_vel_y)
self.optflow_vel_x = self._filter_optflow_vx.x[0]
self.optflow_vel_y = self._filter_optflow_vy.x[0]
self.optflow_x += self.optflow_dt * self.optflow_vel_x
self.optflow_y += self.optflow_dt * self.optflow_vel_y
self.optflow_prev_vel_x = self.optflow_vel_x
self.optflow_prev_vel_y = self.optflow_vel_y
self.optflow_prev_time_stamp = self.optflow_time_stamp
class PID():
'''
This class is a PID control for cognifly
@ MistLab
'''
def __init__(self, kp, ki, kd, integral_threshold = 10):
# Proportional gain Kp
self.kp = kp
# Integral gain Ki
self.ki = ki
# Derivative gain Kd
self.kd = kd
# Sum of error*dt
self.integral = 0
# THRESHOLD for integrator
self.integral_threshold = integral_threshold
# Previous error
self.pre_error = None
self.integrate = True
self._filter_d = FadingMemoryFilter([0., 0., 0.], 1/20, order=2, beta=0.80)
def p(self, error):
self.p_value = self.kp*error # slows down, but helps debugging
return self.p_value
def i(self, dt, error):
if self.integrate:
self.integral += error * dt
if abs(self.integral) > self.integral_threshold:
if self.integral < 0:
self.integral = -self.integral_threshold
else:
self.integral = self.integral_threshold
self.i_value = self.ki*self.integral # slows down, but helps debugging
return self.i_value
def d(self, dt, error):
if self.pre_error == None:
self.pre_error = error
d = (error-self.pre_error)/dt
self._filter_d.update(d)
self.pre_error = error
self.d_value = self.kd*(self._filter_d.x[0]) # slows down, but helps debugging
return self.d_value
def reset(self):
'''Reset the Integration Part'''
self.integral = 0
def calc(self, error, dt = 1):
'''
return = Kp*error + I + Ki*Sum(error*dt) + Kd*(error-pre_error)/dt
'''
return (self.p(error) + self.i(dt, error) + self.d(dt, error))
class OptFlow():
def __init__(self, pipe_write, pipe_read, dt=None, beta=0.0):
self.pipe_read = pipe_read # it's useful for checking if the last value was consumed
self.pipe_write = pipe_write
# spi_port=1, spi_cs=2, spi_cs_gpio=16 (pin 36) => compatible with the hat
# Using /boot/config.txt and dtoverlay=spi1-3cs
keep_trying = True
while keep_trying:
try:
self.flo = PMW3901(spi_port=1,
spi_cs=2,
spi_cs_gpio=16,
secret_sauce=3)
print(NODE_STR + "Sensor initialized!")
keep_trying = False
except RuntimeError:
print(NODE_STR + "Sensor not initialized...")
time.sleep(0.5)
self.flo.set_rotation(
180) # X points to the same direction as the drone (Z up)
self.OptFlowMeasurementData = {
'dx': 0,
'dy': 0,
'Quality': 0,
'time_stamp': 0,
'flow_cte': 30.0 / (4.2 * 3.14 / 180.0)
}
freq = 1 / self.flo.SAMPLE_INTERVAL
self.prev_dx = 0
self.prev_dy = 0
dt = dt if dt else freq
self.filter_x = FadingMemoryFilter([self.prev_dx, 0.],
dt,
order=1,
beta=beta)
self.filter_y = FadingMemoryFilter([self.prev_dy, 0.],
dt,
order=1,
beta=beta)
print(NODE_STR + "OptFlow Node Initialized @ {}Hz!".format(freq))
def close(self):
self.flo.reset()
print(NODE_STR + "Shutting down OptFlow Node...")
def read(self):
dx, dy, q = self.flo.get_motion_with_quality()
self.OptFlowMeasurementData['time_stamp'] = time.time()
if not self.pipe_read.poll(): #timeout=None => blocking
self.filter_x.update(dx)
self.filter_y.update(dy)
dx = self.filter_x.x[0]
self.prev_dx = dx
dy = self.filter_y.x[0]
self.prev_dy = dy
# print(NODE_STR + "New reading!")
self.OptFlowMeasurementData['dx'] = dx
self.OptFlowMeasurementData['dy'] = dy
self.OptFlowMeasurementData['Quality'] = q
self.pipe_write.send(self.OptFlowMeasurementData)
# else:
# print(NODE_STR + "No readings...")
return
