poor z control?

[jrjbertram]

Hi there,

Just a quick note that if you give the controller a fixed goal and let it run for a while you get pretty good x-y control, but very poor z control. (See attached image.) I'm not sure why this doesn't show up in the GUI... maybe because the setpoint is updated periodically? I know the controller isn't the focus of the project, but just a heads up to anyone poking at it.

In case there's something about my usage of it that is generating this behavior, sample code I'm using (I removed the threads for this test and am playing with it in a notebook). Maybe my dt of 0.1 seconds is bad, for example.

Cell that sets up simulation:

# Set goals to go to
GOALS = [(1,1,2),(1,-1,4),(-1,-1,2),(-1,1,4)]
YAWS = [0,3.14,-1.54,1.54]
# Define the quadcopters
QUADCOPTER={'q1':{'position':[1,0,4],'orientation':[0,0,0],'L':0.3,'r':0.1,'prop_size':[10,4.5],'weight':1.2}}
# Controller parameters
CONTROLLER_PARAMETERS = {'Motor_limits':[4000,9000],
                    'Tilt_limits':[-10,10],
                    'Yaw_Control_Limits':[-900,900],
                    'Z_XY_offset':500,
                    'Linear_PID':{'P':[300,300,7000],'I':[0.04,0.04,4.5],'D':[450,450,5000]},
                    'Linear_To_Angular_Scaler':[1,1,0],
                    'Yaw_Rate_Scaler':0.18,
                    'Angular_PID':{'P':[22000,22000,1500],'I':[0,0,1.2],'D':[12000,12000,0]},
                    }

# Make objects for quadcopter, gui and controller
quad = Quadcopter(QUADCOPTER)
ctrl = Controller_PID_Point2Point(quad.get_state,quad.get_time,quad.set_motor_speeds,params=CONTROLLER_PARAMETERS,quad_identifier='q1')


goal,y = list( zip(GOALS,YAWS) )[0]

hist = [ quad.get_position('q1' ) ];

if True:
    ctrl.update_target(goal)
    ctrl.update_yaw_target(y)


    for i in range(10):
        ctrl.update()
        quad.update( dt=.1 )

        hist.append( quad.get_position('q1') )

Cell I run repeatedly that advances the sim 10 seconds at a time and generates the attached plot:

for tick in range(10):
    for i in range(10):
        ctrl.update()
        quad.update( dt=.1 )

        hist.append( quad.get_position('q1') )
        
import matplotlib.pyplot as plt

plt.figure( figsize=[10,10] )
ax = plt.axes(projection='3d')


ax.plot( *np.array( hist ).T )   

Cheers!
J.