Send pitch angle

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

Send pitch angle

Post by GIduarte »

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.

limitador.jpeg
limitador.jpeg (36.47 KiB) Viewed 3558 times

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?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Send pitch angle

Post by arnaud »

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:

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)
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.
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Re: Send pitch angle

Post by GIduarte »

Hi,

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

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)
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 :cry:
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Re: Send pitch angle

Post by GIduarte »

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.
NoInicio.jpeg
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.
NoFIm1.jpeg
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Send pitch angle

Post by arnaud »

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:

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)
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?
GIduarte
Beginner
Posts: 13
Joined: Mon Aug 17, 2020 6:18 pm

Re: Send pitch angle

Post by GIduarte »

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?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Send pitch angle

Post by arnaud »

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 :-).
Post Reply