Hey, Kimberly,
glad to hear your reply. According to your instruction, I setup an callback function to check the 'stabilizer.estimator'. The function is here,
Code: Select all
def param_stab_est_callback(name, value):
print('The crazyflie has parameter ' + name + ' set at number: ' + value)
Also, I write the test code in my main function as
Code: Select all
scf.cf.param.add_update_callback(group='stabilizer', name='estimator', cb=param_stab_est_callback)
time.sleep(1)
scf.cf.param.set_value('stabilizer.estimator', 2)
print('pass')
time.sleep(1)
scf.cf.param.set_value('stabilizer.estimator', 1)
The whole test script is also listed here in case you need it to analyze:
Code: Select all
import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.positioning.motion_commander import MotionCommander
#packages about sending external pose into estimator
from cflib.crazyflie.extpos import Extpos
URI = 'radio://0/80/2M/E7E7E7E7E7'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
DEFAULT_HEIGHT = 0.3
is_deck_attached = False
pose_est = [0, 0, 0]
def log_pose_est_callback(timestamp, data, logconf):
#print(data)
global pose_est
"""
pose_est[0] = data['stateEstimate.x']
pose_est[1] = data['stateEstimate.y']
pose_est[2] = data['stateEstimate.z']
print('stateEstimate')
print(pose_est)
"""
def param_deck_flow(name, value_str):
value = int(value_str)
print(value)
global is_deck_attached
if value:
is_deck_attached = True
print('Deck is attached!')
else:
is_deck_attached = False
print('Deck is NOT attached!')
def param_stab_est_callback(name, value):
print('The crazyflie has parameter ' + name + ' set at number: ' + value)
def reset_estimator(cf):
cf.param.set_value('kalman.resetEstimation', 1)
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', 0)
# time.sleep(1)
#wait_for_position_estimator(cf)
def activate_kalman_estimator(cf):
cf.param.set_value('stabilizer.estimator', 2)
# Set the std deviation for the quaternion data pushed into the
# kalman filter. The default value seems to be a bit too low.
cf.param.set_value('locSrv.extQuatStdDev', 0.06)
if __name__ == '__main__':
cflib.crtp.init_drivers()
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
#scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow)
scf.cf.param.add_update_callback(group='stabilizer', name='estimator', cb=param_stab_est_callback)
time.sleep(1)
scf.cf.param.set_value('stabilizer.estimator', 2)
print('pass')
time.sleep(1)
scf.cf.param.set_value('stabilizer.estimator', 1)
time.sleep(1)
logconf = LogConfig(name='stateEstimate', period_in_ms=10)
logconf.add_variable('stateEstimate.x', 'float')
logconf.add_variable('stateEstimate.y', 'float')
logconf.add_variable('stateEstimate.z', 'float')
scf.cf.log.add_config(logconf)
logconf.data_received_cb.add_callback(log_pose_est_callback)
#if is_deck_attached:
logconf.start()
activate_kalman_estimator(scf.cf)
reset_estimator(scf.cf)
#scf.cf.param.add_update_callback('stabilizer', 'estimator', param_stab_est_callback)
while True:
#scf.cf.extpos.send_extpose(1,1,1,0,0,0,1)
scf.cf.extpos.send_extpos(1,2,3)
logconf.stop()
The results(see attached picture 1) shows that I can only set the 'stabilizer.estimator'=1, once I set the 'stabilizer.estimator'=2, my callback function will not have any response. I think this is the reason why my estimator doesn't work, but I don't understand why this will happen?
Another interesting test that I have done is. First of all, I take off the multi-ranger deck but I didn't take off the optical flow v2 deck. Under this case, The results(see attached picture 2) shows that I can only set the 'stabilizer.estimator'=2, once I set the 'stabilizer.estimator'=1, my callback function will not have any response. Also, the stateEstimate.x and stateEstimate.y is the data that we send, but stateEstimate.z is not(seems like the data estimated from optical flow v2 deck).
I do flash a new firmware code by using the CFclient, the console info is attached picture3. Looking forward to your reply!