Hi,
I'm trying to send the pitch angle setpoint to crazyflie 2 using python lib.
It may seem like a naive question, but does the command send_setpoint (roll, pitch, yawrate, thrust) send the desired setpoint of the pitch angle?
I modified example/ramp.py to send with send_setpoint(0, 10, 0, 20000) for two seconds and then change send_setpoint (0, 0, 0, 20000) for two more seconds.
Using the structure below to limit the crazyflie's movements, I expected it move to an angle of 10 degrees for two seconds and then return to the 0 degree (horizontal) position.
But crazyflie spinning constantly for two seconds and then stopping and staying in that position for another two seconds.
Looking like I'm sending rate pitch instead of pitch.
I tested it several times, tried set 'flightmode.stabModePitch' parameter to 1 and building and upload again the firmware, but this effect persists.
So how do I send a pitch angle reference?
Send pitch angle
Re: Send pitch angle
Without the exact code, it is hard to know what is happening. I tried your experiment by holding the Crazyflie between my fingers keeping the pitch axis free-ish and the following code worked for me: it kept pitch at ~10degree for 2 seconds and at ~0 degrees for 2 seconds before stopping:
One of the key is to send setpoints at regular intervals: here I over-do it a bit with 100Hz in that script, it has to be at least at 2Hz otherwise a setpoint watchdog is kicking-in.
Something-else that will happen is that the yaw control will eventually reach its max which might limit the pich control ability, but this is not a problem for short tests. Eventually, it might be preferable to disable roll and yaw control if you want to measure the pitch control in isolation.
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2014 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, ramps up/down
the motors and disconnects.
"""
import logging
import time
from threading import Thread
import cflib
from cflib.crazyflie import Crazyflie
from cflib.utils import uri_helper
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
logging.basicConfig(level=logging.ERROR)
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
for i in range(200):
self._cf.commander.send_setpoint(0, 10, 0, 20000)
time.sleep(0.01)
for i in range(200):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
if __name__ == '__main__':
# Initialize the low-level drivers
cflib.crtp.init_drivers()
le = MotorRampExample(uri)
Something-else that will happen is that the yaw control will eventually reach its max which might limit the pich control ability, but this is not a problem for short tests. Eventually, it might be preferable to disable roll and yaw control if you want to measure the pitch control in isolation.
Re: Send pitch angle
Hi,
Thanks for answering.
So, I tested agains, and this is the exact code I used:
And that’s what crazyflie did:
https://www.youtube.com/watch?v=OBmHvr3i0WQ
Then I changed lines 98 and 99, increasing the pitch on line 99 from 5 to 10 and reducing the period from 5 seconds to 2.5s. If I'm really sending rate pitch, expect the drone to stay at the same pitch angle, but faster.
This is the exact code:
And this is what happens:
https://youtu.be/v1cnReOm_mY
It happens exactly as it should if I sent rate pitch, the final angle of the crazyflie is the same, but it arrived faster.
Please, help me
Thanks for answering.
So, I tested agains, and this is the exact code I used:
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2014 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, ramps up/down
the motors and disconnects.
"""
import logging
import time
from threading import Thread
import cflib
from cflib.crazyflie import Crazyflie
from cflib.utils import uri_helper
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
logging.basicConfig(level=logging.ERROR)
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
for i in range(200):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
for i in range(500):
self._cf.commander.send_setpoint(0, 5, 0, 20000)
time.sleep(0.01)
for i in range(500):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
if __name__ == '__main__':
# Initialize the low-level drivers
cflib.crtp.init_drivers()
le = MotorRampExample(uri)
https://www.youtube.com/watch?v=OBmHvr3i0WQ
Then I changed lines 98 and 99, increasing the pitch on line 99 from 5 to 10 and reducing the period from 5 seconds to 2.5s. If I'm really sending rate pitch, expect the drone to stay at the same pitch angle, but faster.
This is the exact code:
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2014 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, ramps up/down
the motors and disconnects.
"""
import logging
import time
from threading import Thread
import cflib
from cflib.crazyflie import Crazyflie
from cflib.utils import uri_helper
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
logging.basicConfig(level=logging.ERROR)
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
for i in range(200):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
for i in range(250):
self._cf.commander.send_setpoint(0, 10, 0, 20000)
time.sleep(0.01)
for i in range(500):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
if __name__ == '__main__':
# Initialize the low-level drivers
cflib.crtp.init_drivers()
le = MotorRampExample(uri)
https://youtu.be/v1cnReOm_mY
It happens exactly as it should if I sent rate pitch, the final angle of the crazyflie is the same, but it arrived faster.
Please, help me
Re: Send pitch angle
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.
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.
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.
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))
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))
Re: Send pitch angle
Hi,
I am not sure what is happening on your rig, I cannot reproduce your problems as for me the attitude control looks good (a bit "nervous" but my rotation axis does not pass through the center of mass and has some friction so it might explain some oscillations).
I tried to build a similar experiment and one problem I have almost directly is that there is a buid-up happening which likely comes from the roll and yaw control loop trying to correct a static platform. The effect of the roll and yaw loop can be disabled by setting to 0 the parameters "pid_rate.roll_k*" and "pid_rate.yaw_k*".
I tested both your scripts and they behave quite well for me, this is the second one: https://drive.google.com/file/d/1f2acdm ... sp=sharing. The PID build-up can be seen with one of the motor stopping, this is the yaw PID trying to compensate.
To get rid of the buildup, I made a new script that disables roll/yaw and goes to [0, 10, -10, 0] degree in steps of 4 seconds: https://drive.google.com/file/d/1ez_Nox ... sp=sharing. This is the script:
I am using a recent unmodified firmware (22:a70c357a7c03 (2021.03 +22)) and my Crazyflie has no deck installed. Is your setup different? Do you have a modified firmware or any deck installed on the Crazyflie?
I am not sure what is happening on your rig, I cannot reproduce your problems as for me the attitude control looks good (a bit "nervous" but my rotation axis does not pass through the center of mass and has some friction so it might explain some oscillations).
I tried to build a similar experiment and one problem I have almost directly is that there is a buid-up happening which likely comes from the roll and yaw control loop trying to correct a static platform. The effect of the roll and yaw loop can be disabled by setting to 0 the parameters "pid_rate.roll_k*" and "pid_rate.yaw_k*".
I tested both your scripts and they behave quite well for me, this is the second one: https://drive.google.com/file/d/1f2acdm ... sp=sharing. The PID build-up can be seen with one of the motor stopping, this is the yaw PID trying to compensate.
To get rid of the buildup, I made a new script that disables roll/yaw and goes to [0, 10, -10, 0] degree in steps of 4 seconds: https://drive.google.com/file/d/1ez_Nox ... sp=sharing. This is the script:
Code: Select all
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2014 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
# MA 02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, ramps up/down
the motors and disconnects.
"""
import logging
import time
from threading import Thread
import cflib
from cflib.crazyflie import Crazyflie
from cflib.utils import uri_helper
uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
logging.basicConfig(level=logging.ERROR)
class MotorRampExample:
"""Example that connects to a Crazyflie and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._cf = Crazyflie(rw_cache='./cache')
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
self._cf.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._ramp_motors).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
time.sleep(10)
self._cf.param.set_value('pid_rate.roll_kp', '0')
self._cf.param.set_value('pid_rate.roll_ki', '0')
self._cf.param.set_value('pid_rate.roll_kd', '0')
self._cf.param.set_value('pid_rate.yaw_kp', '0')
self._cf.param.set_value('pid_rate.yaw_ki', '0')
self._cf.param.set_value('pid_rate.yaw_kd', '0')
time.sleep(1)
# Unlock startup thrust protection
self._cf.commander.send_setpoint(0, 0, 0, 0)
for i in range(400):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
for i in range(400):
self._cf.commander.send_setpoint(0, 10, 0, 20000)
time.sleep(0.01)
for i in range(400):
self._cf.commander.send_setpoint(0, -10, 0, 20000)
time.sleep(0.01)
for i in range(400):
self._cf.commander.send_setpoint(0, 0, 0, 20000)
time.sleep(0.01)
# while thrust >= 20000:
# self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
# time.sleep(0.1)
# if thrust >= 25000:
# thrust_mult = -1
# thrust += thrust_step * thrust_mult
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
self._cf.close_link()
if __name__ == '__main__':
# Initialize the low-level drivers
cflib.crtp.init_drivers()
le = MotorRampExample(uri)
Re: Send pitch angle
Helo!
Thanks for the answer!
I was using the flow deck. I did the tests again, without any decks, and it really worked correctly.
I haven't studied the kalman filter yet, but could this behavior (with the flow deck) be related to the fact that there is no linear movement?
Thanks for the answer!
I was using the flow deck. I did the tests again, without any decks, and it really worked correctly.
I haven't studied the kalman filter yet, but could this behavior (with the flow deck) be related to the fact that there is no linear movement?
Re: Send pitch angle
Yes it is very probable the flow deck is causing problems there: the Kalman filter (EKF) is using the flow deck measurements to correct the attitude estimate: if you ask for a positive pitch and no forward acceleration is measured, the EKF will assume that the platform is not actually pitching and so it will make it pitch more and more until some acceleration is measured. The behavior you observed seems to make sense actually .