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