nokov_estimator_high_level_commander

[Felix-kkk]

Hi！
These days, I'm trying to plug in an external mocap system to locate the crazyfile.

1. I used the example successfully send the position x, y, z, and quaternions to crazyfile via the send_extpose_quat, and successfully printed the position estimate via logconf.add_variable('stateEstimate.x', 'float'), just like the mocap.
2. I started to control the crazyfile.The crazyflie flipped immediately aftertakeoff, like
3. I've tried different commanders, but none worked.I looked at almost all of the disscussion and I saw that there was a saying that the coordinate system was not uniform, but I can confirm that it was unified.
   I've been stuck in this part for a long time, and I'd love to hear from you.
   Thank you!

Here's my code

from nokov.nokovsdk import *

import time
import sys
from threading import Thread
import logging
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.utils import uri_helper
from cflib.positioning.position_hl_commander import PositionHlCommander
from cflib.positioning.motion_commander import MotionCommander

uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E6')

logging.basicConfig(level=logging.ERROR)

> I tried changing it to False and it didn't work

send_full_pose = True
orientation_std_dev = 1.0e-3
pos_std_dev = 1e-3


preFrmNo = 0
curFrmNo = 0
global client
global nokov_data
nokov_data = None  

def initialize_nokov(server_ip='10.1.1.198'):
    global client
    client = PySDKClient()

    ver = client.PyNokovVersion()
    print('NokovSDK Sample Client 2.4.0.5428(NokovSDK ver. %d.%d.%d.%d)' % (ver[0], ver[1], ver[2], ver[3]))
    
    print("Initializing Nokov SDK Client...")
    ret = client.Initialize(bytes(server_ip, encoding="utf8"))
    if ret == 0:
        print("Connected to Nokov successfully.")
    else:
        print("Failed to connect to Nokov. Exiting.")
        sys.exit(1)

def py_data_func(pFrameOfMocapData, pUserData):
    if pFrameOfMocapData is None:  
        print("Not get the data frame.\n")
    else:
        frameData = pFrameOfMocapData.contents
        global preFrmNo, curFrmNo 
        curFrmNo = frameData.iFrame
        if curFrmNo == preFrmNo:
            return
        global client
        preFrmNo = curFrmNo

        for iBody in range(frameData.nRigidBodies):
            body = frameData.RigidBodies[iBody]
            # print(f"\tid:{body.ID}")
            # print(f"\t    (mm)\tx:{body.x:6.2f}\ty:{body.y:6.2f}\tz:{body.z:6.2f}")
            # print(f"\t\t\tqx:{body.qx:6.2f}\tqy:{body.qy:6.2f}\tqz:{body.qz:6.2f}\tqw:{body.qw:6.2f}")	
            global nokov_data
            nokov_data = body

class MocapWrapper(Thread):
    def __init__(self, server_ip='10.1.1.198'):
        Thread.__init__(self)
        self._stay_open = True
        self.on_pose = None
        self.server_ip = server_ip
        self.start()

    def close(self):
        self._stay_open = False

    def run(self):
        # Initialize and connect to the Nokov system
        initialize_nokov(self.server_ip)

        # Set data callback
        client.PySetDataCallback(py_data_func, None)

        # Loop to receive data
        while self._stay_open:
            global nokov_data
            if nokov_data is not None:
                x = nokov_data.x*0.001
                y = nokov_data.y*0.001
                z = nokov_data.z*0.001
                q = [nokov_data.qx, nokov_data.qy, nokov_data.qz, nokov_data.qw]
                if self.on_pose:
                    pos = [x, y, z, q]
                    print(x,y,z)
                    self.on_pose([pos[0], pos[1], pos[2], pos[3]])
            time.sleep(0.01)  
def send_extpose_quat(cf, x, y, z, quat):
    """
    Send the current Crazyflie X, Y, Z position and attitude as a quaternion.
    This is going to be forwarded to the Crazyflie's position estimator.
    """
    if send_full_pose:
        #print(x)
        cf.extpos.send_extpose(x, y, z, quat[0], quat[1], quat[2], quat[3])
    else:
        cf.extpos.send_extpos(x, y, z)

def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Kalman Variance', period_in_ms=12)
    log_config.add_variable('kalman.varPX', 'float')
    log_config.add_variable('kalman.varPY', 'float')
    log_config.add_variable('kalman.varPZ', 'float')
    log_config.add_variable('kalman.varD0', 'float')
    log_config.add_variable('kalman.varD1', 'float')
    log_config.add_variable('kalman.varD2', 'float')

    var_y_history = [1000] * 10
    var_x_history = [1000] * 10
    var_z_history = [1000] * 10
    var_Roll_history = [1000] * 10
    var_Pitch_history = [1000] * 10
    var_Yaw_history = [1000] * 10

    threshold = 0.0001

    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)
            var_Roll_history.append(data['kalman.varD0'])
            var_Roll_history.pop(0)
            var_Pitch_history.append(data['kalman.varD1'])
            var_Pitch_history.pop(0)
            var_Yaw_history.append(data['kalman.varD2'])
            var_Yaw_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)
            min_Roll = min(var_Roll_history)
            max_Roll = max(var_Roll_history)
            min_Pitch = min(var_Pitch_history)
            max_Pitch = max(var_Pitch_history)
            min_Yaw = min(var_Yaw_history)
            max_Yaw = max(var_Yaw_history)

            if (max_x - min_x) < threshold and (
                    max_y - min_y) < threshold and (
                    max_z - min_z) < threshold and (
                    max_Roll -  min_Roll) < threshold and (
                    max_Pitch -  min_Pitch) < threshold and(
                    max_Yaw -  min_Yaw) < threshold:
                print('Position found!')
                break

def reset_estimator(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 adjust_orientation_sensitivity(cf):
    cf.param.set_value('locSrv.extQuatStdDev', orientation_std_dev)
def adjust_pos_sensitivity(cf):
    cf.param.set_value('locSrv.extPosStdDev', pos_std_dev)

def activate_kalman_estimator(cf):
    cf.param.set_value('stabilizer.estimator', '2')
    #cf.param.set_value('locSrv.extQuatStdDev', 0.06)

def activate_mellinger_controller(cf):
    cf.param.set_value('stabilizer.controller', '2')
def log_stab_callback(timestamp, data, logconf):
    print('[%d][%s]: %s' % (timestamp, logconf.name, data))

def simple_log_async(scf, logconf):
    cf = scf.cf
    cf.log.add_config(logconf)
    logconf.data_received_cb.add_callback(log_stab_callback)
    logconf.start()
    #time.sleep(5)
    
if __name__ == '__main__':
    print("Initializing Crazyflie drivers...")
    cflib.crtp.init_drivers()
    print("Crazyflie drivers initialized.")
    
    # Set up the motion capture system handler
    mocap_wrapper = MocapWrapper(server_ip='10.1.1.198')  # Set the desired server IP
    
    logconf = LogConfig(name='Position', period_in_ms=100)
    logconf.add_variable('stateEstimate.x', 'float')
    logconf.add_variable('stateEstimate.y', 'float')
    logconf.add_variable('stateEstimate.z', 'float')

    # logconf.add_variable('stateEstimate.qx', 'float')
    # logconf.add_variable('stateEstimate.qy', 'float')
    # logconf.add_variable('stateEstimate.qz', 'float')
    # logconf.add_variable('stateEstimate.qw', 'float')
    # Crazyflie control loop

    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:  
        cf = scf.cf
        #cf.param.set_value('stabilizer.estimator', '2')
        #cf.param.set_value('locSrv.extPosStdDev', 0.001)
        adjust_orientation_sensitivity(cf)
        adjust_pos_sensitivity(cf)
        activate_kalman_estimator(cf)
        #activate_mellinger_controller(cf)
       
        simple_log_async(scf, logconf)

        mocap_wrapper.on_pose = lambda pose: send_extpose_quat(cf, pose[0], pose[1], pose[2], pose[3])
        #time.sleep(30)
        reset_estimator(cf)
        time.sleep(5)
        #1.PositionHlCommander
        #controller=PositionHlCommander.CONTROLLER_PID
        # with PositionHlCommander(scf, controller=2) as pc:
        #     # pc.go_to(-0.3, -0.3, 1)
        #     # time.sleep(2)
        #     # pc.go_to(0.3, -0.3, 1)
        #     # time.sleep(2)
        #     # pc.go_to(0.3, 0.3, 1)
        #     # time.sleep(2)
        #     # pc.go_to(-0.3, 0.3, 1)
     
        #2.high_level_commander
        commander = scf.cf.high_level_commander
        commander.takeoff(1.0,20)  # Despegar a 1 m de altura en 2 segundos
        time.sleep(2)
    logconf.stop()        
    # Close the motion capture data processing
    mocap_wrapper.close()
    ```

[Bananazooka]

Hi, could you check the position and orientation before flying? It seems that that's where it's going wrong.