def run_control_none():
"""Function to forward the receiver signals to the control signals
Reads receiver data from redis database
sets command data to receiver data clipped to [-1, 1]
Writes command data to redis database
"""
print(os.path.basename(__file__))
db_redis = DBRedis()
db_sub = db_redis.subscribe([REDIS_RX_CHANNEL])
print("Running control_none...Ctrl-c to stop")
print_control_header()
try:
while True:
# check for new receiver or attitude data
db_notice = db_sub.get_message(timeout=10)
if db_notice is not None:
# get receiver command as ref
rx = redis_util.get_rx(db_redis)
# forward most of rx data to cmd except yaw
cmd = rx[:]
clip_rx(cmd)
redis_util.set_cmd(db_redis, cmd)
print_control_data(rx, cmd)
except KeyboardInterrupt:
print("\nInterrupt received: stopping control...")
def run_attitude_control():
"""Function to compute the control signals for attitude control
Reads IMU data from the redis database
Reads receiver data from the redis database
Writes command data to the redis database
"""
attitude_controller = AttitudePD(
roll_kp=0.04, roll_kd=0.000001, pitch_kp=0.04, pitch_kd=0.000001)
db_redis = DBRedis()
db_sub = db_redis.subscribe([REDIS_RX_CHANNEL, REDIS_ATTITUDE_CHANNEL, REDIS_IMU_CHANNEL])
try:
print("Running attitude control...Ctrl-c to stop")
print_control_header()
while True:
# check for new receiver or attitude data
db_notice = db_sub.get_message(timeout=10)
if db_notice is not None:
rx_data = redis_util.get_rx(db_redis)
attitude = redis_util.get_attitude(db_redis)
imu = redis_util.get_imu(db_redis)
droll_cmd, dpitch_cmd = attitude_controller.step(
attitude[:2], imu[:2], rx_data[1:3], [0., 0.])
throttle = rx_data[0]
dyaw_cmd = rx_data[3]
aux1, aux2 = rx_data[4:]
cmd = [throttle, droll_cmd, dpitch_cmd, dyaw_cmd, aux1, aux2]
redis_util.set_cmd(db_redis, cmd)
print_control_data(rx, cmd)
except KeyboardInterrupt:
print("\nInterrupt received: stopping attitude control...")
def run_drone_comm_forward_rx_rc():
"""Function to forward rc commands to the flight control board"""
print(os.path.basename(__file__))
db_redis = DBRedis()
mw_comm = MultiWii()
rx_sub = db_redis.subscribe(REDIS_RX_CHANNEL)
print_rx_rx_rc_header()
try:
while True:
# wait for data to be available
rx_notice = rx_sub.get_message(timeout=10)
if rx_notice is not None:
rx_data = redis_util.get_rx(db_redis)
rx_rc_data = redis_util.get_rx_rc(db_redis)
msp.set_rc(mw_comm, rx_rc_data)
print_rx_rx_rc_data(rx_data, rx_rc_data)
except KeyboardInterrupt:
print("Interrupt received: closing port and resetting settings...")
mw_comm.close_serial()
def run_estimate_kalman():
"""Function to compute the control signals for attitude control
Reads IMU data from the redis database
Reads sonar data from the redis database
Reads gps data from the redis database
Writes state estimate data to the redis database
"""
R = np.eye(12)
estimator = EKF(R)
estimator.initialize(Zgps=0, Zimu=0, Zagl=0)
db_redis = DBRedis()
db_sub = db_redis.subscribe(
[REDIS_IMU_CHANNEL, REDIS_SONAR_CHANNEL, REDIS_GPS_CHANNEL])
try:
print("Running state estimate...Ctrl-c to stop")
while True:
# check for new receiver or attitude data
db_notice = db_sub.get_message(timeout=10)
if db_notice is not None:
rx_data = redis_util.get_rx(db_redis)
imu = redis_util.get_imu(db_redis)
agl = redis_util.get_sonar(db_redis)
gps = redis_util.get_gps(db_redis)
estimator.step()
state_est = [
x, y, z, roll, pitch, yaw, dx, dy, dz, omega_x, omega_y,
omega_z
]
redis_util.set_cmd(db_redis, state_est)
except KeyboardInterrupt:
print("\nInterrupt received: stopping state estimation...")
def update(self, t):
"""update called with each step of the simulation"""
return np.array(redis_util.get_rx(self.dbredis))