Well, we did a few more investigations.
First, we found one more strange thing. When we are not sending pitch commands the variable stabilizer.pitch seems to work as it should, but as soon as the engines turn on stabilizer.pitch it looks more like rate pitch.
This is the exact code we used.
Code: Select all
# -*- coding: utf-8 -*-
#
import logging
import time
import math
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
from cflib.utils import uri_helper
T = 5
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
# 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:
period_in_ms=10
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('controller.cmd_roll', 'float')
lg_stab.add_variable('controller.cmd_pitch', 'float')
lg_stab.add_variable('controller.pitch', 'float')
lg_stab.add_variable('controller.pitchRate', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')
lg_stab.add_variable('gyro.y', 'float')
i = 0
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(available[0][0], cf=cf) as scf:
#cf.param.set_value('pid_attitude.pitch_kp', '1.7344')
#cf.param.set_value('pid_attitude.pitch_ki', '0.0445')
#cf.param.set_value('pid_attitude.pitch_kd', '0')
cf.param.set_value('pid_attitude.pitch_kp', '6')
cf.param.set_value('pid_attitude.pitch_ki', '3')
cf.param.set_value('pid_attitude.pitch_kd', '0')
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
cf.commander.send_setpoint(0, 0, 0, 0)
with SyncLogger(scf, lg_stab) as logger:
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
#print("depois")
startTime = time.time()
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
nowTime = time.time() - startTime
if nowTime < 9:
time.sleep(period_in_ms/1000)
print('\'time\': %d,%s,\'ref_gx\':%f,\'ref_gy\': %f, \'ref_gz\': %f #' % (timestamp, data, roll, pitch, yawrate))
pitch = 0
continue
elif nowTime < 10:
# Unlock startup thrust protection
cf.commander.send_setpoint(0, 0, 0, 0)
elif nowTime < 12:
pitch = 0
elif nowTime < 17:
pitch = 10
elif nowTime < 22:
pitch = 0
else:
cf.commander.send_setpoint(0, 0, 0, 0)
cf.close_link()
break
cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(period_in_ms/1000)
print('\'time\': %d,%s,\'ref_gx\':%f,\'ref_gy\': %f, \'ref_gz\': %f #' % (timestamp, data, roll, pitch, yawrate))
During the initial 9s we manually moved the cf to an angle of around 45 and -45 degrees to prove that stabilizer.pitch looks reasonable. Then we send pitch = 10 and thrust=20000 with the send_setpoint command for 4 seconds and then pitch = 0 for 5 seconds, again showing incorrect behavior (see video).
https://youtu.be/hmB6R-fNXB0
But then if you analyze the response obtained with logging, table below, it is noticed that stabilizer.pitch is providing erroneous feedback to the PID controller when we send pitch! = 0 and thrust!=0.
And in another test to show that there is a problem with the variable stabilizer.pitch, we change the order, i. e, first we send the command pitch = 10 and thrust = 20000 for 4 seconds and then we change pitch to 0 for 5 seconds and we clearly see that the pitch (angle) is nowhere near 10. Then we change pitch and thrust to zero and keep the drone in position manually as can be seen in the video below.
https://youtu.be/Sm1gB5TJExM
This the code used in second test.
Code: Select all
# -*- coding: utf-8 -*-
#
import logging
import time
import math
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
from cflib.utils import uri_helper
T = 5
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
# 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:
period_in_ms=10
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('controller.cmd_roll', 'float')
lg_stab.add_variable('controller.cmd_pitch', 'float')
lg_stab.add_variable('controller.pitch', 'float')
lg_stab.add_variable('controller.pitchRate', 'float')
lg_stab.add_variable('stabilizer.pitch', 'float')
lg_stab.add_variable('gyro.y', 'float')
i = 0
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(available[0][0], cf=cf) as scf:
#cf.param.set_value('pid_attitude.pitch_kp', '1.7344')
#cf.param.set_value('pid_attitude.pitch_ki', '0.0445')
#cf.param.set_value('pid_attitude.pitch_kd', '0')
cf.param.set_value('pid_attitude.pitch_kp', '6')
cf.param.set_value('pid_attitude.pitch_ki', '3')
cf.param.set_value('pid_attitude.pitch_kd', '0')
cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
cf.commander.send_setpoint(0, 0, 0, 0)
with SyncLogger(scf, lg_stab) as logger:
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
startTime = time.time()
cf.commander.send_setpoint(0, 0, 0, 0)
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
nowTime = time.time() - startTime
if nowTime < 2: #2 seconds
pitch = 0
elif nowTime < 6: #4 seconds
pitch = 10
elif nowTime < 11: #5 seconds
pitch = 0
elif nowTime < 21: #10 seconds
pitch = 0
thrust = 0
else:
cf.commander.send_setpoint(0, 0, 0, 0)
cf.close_link()
break
cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(period_in_ms/1000)
print('\'time\': %d,%s,\'ref_gx\':%f,\'ref_gy\': %f, \'ref_gz\': %f #' % (timestamp, data, roll, pitch, yawrate))
As we can see in the table below, stabilizer.pitch is behaving erroneously since the cf is not at the 10 degree angle. By sending pitch = 0 and thrust = 0 and keeping the drone in the final position manually, we can see that stabilizer.pitch is going to the correct value.