i have made an off- board controll, which means the position controller in PC and send the control commanders(roll, pitch, yaw and thrust) into crazyflie throught crazyradio. I can accomplish the goal now, but there is still a problem. When I shorten the log sampling time, the flight becomes very unstable. This problem occurs when the sampling time is less than 15ms, and the shorter the time, the greater the problem, until the flight is completely out of control. I tried to monitor the code running time and found that when the log sampling time is less than 15ms, the running time of function send_setpoint(roll, pitch, yaw, thrust) will be shortened as the sampling time decreases.
somebody seeing the same problem?
Thank you in advance.
The following is just a sample code
Code: Select all
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from pid import *
state_estimate = state_s()
SUM = 0
thrust_actual = 0
calculate = 0
# URI to the Crazyflie to connect to
uri = 'radio://0/80/2M'
# Change the sequence according to your setup
# x y z YAW
sequence = [
(0.0, 0.0, 0.5, 0)
]
def wait_for_position_estimator(scf):
print('Waiting for estimator to find position...')
log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
log_config.add_variable('kalman.varPX', 'float')
log_config.add_variable('kalman.varPY', 'float')
log_config.add_variable('kalman.varPZ', 'float')
var_y_history = [1000] * 10
var_x_history = [1000] * 10
var_z_history = [1000] * 10
threshold = 0.001
with SyncLogger(scf, log_config) as logger:
for log_entry in logger:
data = log_entry[1]
var_x_history.append(data['kalman.varPX'])
var_x_history.pop(0)
var_y_history.append(data['kalman.varPY'])
var_y_history.pop(0)
var_z_history.append(data['kalman.varPZ'])
var_z_history.pop(0)
min_x = min(var_x_history)
max_x = max(var_x_history)
min_y = min(var_y_history)
max_y = max(var_y_history)
min_z = min(var_z_history)
max_z = max(var_z_history)
# print("{} {} {}".
# format(max_x - min_x, max_y - min_y, max_z - min_z))
if (max_x - min_x) < threshold and (
max_y - min_y) < threshold and (
max_z - min_z) < threshold:
break
def reset_estimator(scf):
cf = scf.cf
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
wait_for_position_estimator(cf)
def position_callback(timestamp, data, logconf):
global state_estimate
state_estimate.position.x = data['kalman.stateX']
def start_position_printing(scf):
log_conf = LogConfig(name='Position', period_in_ms= 150) # actual 15ms
log_conf.add_variable('kalman.stateY', 'float')
scf.cf.log.add_config(log_conf)
log_conf.data_received_cb.add_callback(position_callback)
log_conf.start()
def run_test(scf, sequence, SUM ):
cf = scf.cf
for position in sequence:
print('Setting position {}'.format(position))
for i in range(350): # 3.5s
time_start2 = time.time()
cf.commander.send_setpoint(0, -0, 0, 0)
time_end2 = time.time()
SUM += time_end2 - time_start2
time.sleep(0.01)
print('SUM: {} '.format((SUM) * 1000))
cf.commander.send_stop_setpoint()
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
reset_estimator(scf)
# Unlock startup thrust protection
scf.cf.commander.send_setpoint(0, 0, 0, 0 )
start_position_printing(scf)
time_start = time.time()
run_test(scf, sequence, SUM )
time_end = time.time()
print('total time: {} '.format((time_end - time_start)))