Page 1 of 1

Receive rate pitch

Posted: Thu Jan 07, 2021 7:36 pm
by GIduarte

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.

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).

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

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

    if len(available) == 0:
        print('No Crazyflies found, cannot run example')
        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
        cf = Crazyflie(rw_cache='./cache')
        #print("antes 2")
        with SyncCrazyflie(available[0][0], cf=cf) as scf:
            cf.param.set_value('flightmode.stabModePitch', '0')
            cf.param.set_value('flightmode.stabModeRoll', '0')
            cf.param.set_value('kalman.resetEstimation', '1')
            cf.param.set_value('kalman.resetEstimation', '0')
            cf.commander.send_setpoint(0, 0, 0, 0)
            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)

                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
                        pitch = 0
                        cf.commander.send_setpoint(0, 0, 0, 0)

                    cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
                    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.

Re: Receive rate pitch

Posted: Fri Jan 08, 2021 10:44 am
by kimberly

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?

Re: Receive rate pitch

Posted: Wed Jan 20, 2021 1:57 pm
by GIduarte


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. :(

Re: Receive rate pitch

Posted: Fri Jan 22, 2021 10:32 am
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.