Receive rate pitch

Discussions and questions about the Crazyradio
Post Reply
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Receive rate pitch

Post by GIduarte »

Hello,

I want to apply a tuning technique for pid parameters. For this, I need to send rate roll and rate pitch and receive the data used to calculate the input error of the PID controller.

I'm using stabilizationModeRoll = RATE and stabilizationModePitch = RATE together with send_setpoint () to send the references.

And for the feedback for calculating the PID error I'm trying to get with the logging:
  • gyro.yRaw
  • stateEstimateZ.ratePitch
  • controller.r_pitch
  • stabilizer.pitch

We put a structure to limit Crazyflie's rotation, as seen here.
limitador.jpeg

However, the behavior observed on the drone is very close to the reference sent (blue) and has nothing to do with the measured signal (red).
referencia.jpeg


Here my code

Code: Select all

import logging
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

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)

if __name__ == '__main__':
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    available = cflib.crtp.scan_interfaces()

    if len(available) == 0:
        print('No Crazyflies found, cannot run example')
    else:
        lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
        lg_stab.add_variable('pm.batteryLevel', 'uint8_t')

        
        #Realimentação PID rate 
        lg_stab.add_variable('gyro.yRaw', 'float')
        lg_stab.add_variable('stateEstimateZ.ratePitch', 'int16_t')
        lg_stab.add_variable('stateEstimateZ.rateRoll', 'int16_t')
        lg_stab.add_variable('stateEstimateZ.rateYaw', 'int16_t')
        lg_stab.add_variable('controller.r_roll', 'float')
        lg_stab.add_variable('controller.r_pitch', 'float')
        lg_stab.add_variable('stabilizer.pitch', 'float')
        i = 0
        
        #print("antes")
        cf = Crazyflie(rw_cache='./cache')
        #print("antes 2")
        with SyncCrazyflie(available[0][0], cf=cf) as scf:
            cf.param.set_value('flightmode.stabModePitch', '0')
            time.sleep(0.1)
            cf.param.set_value('flightmode.stabModeRoll', '0')
            time.sleep(0.1)
            cf.param.set_value('kalman.resetEstimation', '1')
            time.sleep(0.1)
            cf.param.set_value('kalman.resetEstimation', '0')
            time.sleep(0.1)
            cf.commander.send_setpoint(0, 0, 0, 0)
            time.sleep(2)
            
            
            with SyncLogger(scf, lg_stab) as logger:
                thrust = 20000
                pitch = 0
                roll = 0
                yawrate = 0
                # Unlock startup thrust protection
                cf.commander.send_setpoint(0, 0, 0, 0)

                #print("depois")
                startTime = time.time()
                direction_time = 3

                for log_entry in logger:
                    timestamp = log_entry[0]
                    data = log_entry[1]
                    #logconf_name = log_entry[2]
                    nowTime = time.time()
                    if nowTime < startTime + direction_time:
                        pitch = 10
                    elif nowTime > startTime + direction_time and nowTime < startTime + 2*direction_time:
                        pitch = 0
                    elif nowTime > startTime + 2*direction_time and nowTime < startTime + 4*direction_time:
                        pitch = -10
                    elif nowTime > startTime + 4*direction_time and nowTime < startTime + 5*direction_time:
                        pitch = 10
                    else:
                        pitch = 0
                        cf.commander.send_setpoint(0, 0, 0, 0)
                        break

                    cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
                    time.sleep(0.1)
                    print('\'time\': %d,%s,\'ref_gx\':%f,\'ref_gy\': %f, \'ref_gz\': %f #' % (timestamp, data, roll, pitch, yawrate))

I've run out of ideas, can someone help me?

Thanks for listening.
kimberly
Bitcraze
Posts: 796
Joined: Fri Jul 06, 2018 11:13 am

Re: Receive rate pitch

Post by kimberly »

hi!

Your setup looks amazing and super useful! This is something we have wanted to do for a while now already, since we do expect that the tuning can be improved.

Could you specify which logs are blue and red in your plot, since that is not shown in the script? I guess that blue is 'controller.r_pitch' but what is red?
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Re: Receive rate pitch

Post by GIduarte »

Hi!

Thanks!!!

So, blue is the rate pitch we are sending, pitch in script.
The red is stateEstimateZ.ratePitch log.

The answer we get from crazyflie is completely different from the log. Every other log has a strange answer as well. :(
kimberly
Bitcraze
Posts: 796
Joined: Fri Jul 06, 2018 11:13 am

Re: Receive rate pitch

Post by kimberly »

On the first view I would say that blue is the pitch and not the pitch rate, that would at least explain a lot.

The desired pitch rate you can monitor with the log variable controller.pitchRate, since this monitors the variable rateDesired.pitch in the controller code.
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Re: Receive rate pitch

Post by GIduarte »

I found the error.

It's time.sleep (0.1) on the penultimate line. It should be time.sleep (0.01).
Post Reply