Possible stabilize Crazyflie 2.0 drone autonomous flight without additional sensors?

Question

I just bought the Crazyflie 2.0 drone. This is my first drone, and it is my first time programming a drone.

My first goal is simple: Make the drone hover in place stably for 10 seconds.

I found a simple example script that turns up the thrust and then lands the drone. I modified this to 1) extend the time to 10 seconds, 2) reverse thrust if the drone starts tipping, and 3) constantly display the roll, pitch, and yaw in the console.

When I run this, the drone often flies randomly around the room and runs into things; it does not just lift up and hover stably. Why is this? How can I improve things so that it's much more stable? Do I need more sensors, or can I pull this off with just programming?

"""
Simple example that connects to the first Crazyflie found, hovers, and disconnects.
"""

import time
import sys
from threading import Thread
import logging

import cflib  # noqa
from cfclient.utils.logconfigreader import LogConfig  # noqa
from cflib.crazyflie import Crazyflie  # noqa

logging.basicConfig(level=logging.ERROR)


class HoverTest:
    """Example that connects to the first Crazyflie found, hovers, and disconnects."""

    def __init__(self, link_uri):
        """ Initialize and run the example with the specified link_uri """

        self._cf = Crazyflie()

        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)

        self._status = {}
        self._status['gyro.x'] = 0
        self._status['gyro.y'] = 0
        self._status['gyro.z'] = 0

        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."""
        print("Connected to %s" % link_uri)

        # The definition of the logconfig can be made before connecting
        self._lg_gryo = LogConfig(name="gyro", period_in_ms=10)
        self._lg_gryo.add_variable("gyro.x", "float")
        self._lg_gryo.add_variable("gyro.y", "float")
        self._lg_gryo.add_variable("gyro.z", "float")

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_gryo)
            # This callback will receive the data
            self._lg_gryo.data_received_cb.add_callback(self._gryo_log_data)
            # This callback will be called on errors
            self._lg_gryo.error_cb.add_callback(self._gryo_log_error)
            # Start the logging
            self._lg_gryo.start()
        except KeyError as e:
            print("Could not start log configuration,"
                  "{} not found in TOC".format(str(e)))
        except AttributeError:
            print("Could not add gyro log config, bad configuration.")

        # Start a separate thread to do the motor test.
        # Do not hijack the calling thread!
        Thread(target=self._hover).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 _gryo_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print("Error when logging %s: %s" % (logconf.name, msg))

    def _gryo_log_data(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        # print("[%d][%s]: %s" % (timestamp, logconf.name, data))

        self._status['gyro.x'] = data['gyro.x']
        self._status['gyro.y'] = data['gyro.y']
        self._status['gyro.z'] = data['gyro.z']

    def _hover(self):
        start_time = time.time()
        run_time = 7

        thrust_multiplier = 1
        thrust_step = 500
        thrust = 20000
        max_thrust = 39000
        roll = -1.00
        pitch = -2.00
        yaw = 0

        # Unlock startup thrust protection.
        self._cf.commander.send_setpoint(0, 0, 0, 0)

        # Turn on altitude hold.
        self._cf.param.set_value("flightmode.althold","True")

        while thrust >= 20000:
            # Update the position.
            self._cf.commander.send_setpoint(roll, pitch, yaw, thrust)

            time.sleep(0.01)

            if thrust >= max_thrust and time.time() - start_time >= run_time:
                # Reverse thrust
                thrust_multiplier = -1

            if thrust <= max_thrust or thrust_multiplier == -1:
                thrust += thrust_step * thrust_multiplier

            # Reverse thrust if the drone tips over.
            if abs(self._status['gyro.x']) >= 75 or abs(self._status['gyro.y']) >= 75:
                print('Aborting')
                thrust_multiplier = -2

            print(self._status['gyro.x'], self._status['gyro.y'], self._status['gyro.z'])

        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 (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)
    # Scan for Crazyflies and use the first one found
    print("Scanning interfaces for Crazyflies...")
    available = cflib.crtp.scan_interfaces()
    print("Crazyflies found:")
    for i in available:
        print(i[0])

    if len(available) > 0:
        le = HoverTest(available[0][0])
    else:
        print("No Crazyflies found, cannot run example")

As you can see, I tried simply adjusting the roll and pitch (to -1.00 and -2.00), but that did not help much. When I use the GUI and a joystick to control the drone and I adjust the roll and pitch trim values to -1.00 and -2.00, this definitely helps stabilize the drone.

Any ideas are welcome. Thank you!

Answer 1

tl;dr - You can't control the aircraft's side-to-side motion because you are only modifying "thrust", which controls the up-and-down motion. If you want to control roll and pitch (side-to-side motion), then you need to write to each motor individually. There are other problems, too - read on for more.

---

In looking at your code, I found a couple points where you might be having issues:

1. You initialize thrust to 20000, which means you enter the loop in the code. This locks you in the while loop for an indeterminate amount of time. This is because you stay locked in the loop until you can get thrust below 20000. The only way that you can decrement thrust is if you both are at or above max_thrust and you have exceeded your time limit run_time. If you just exceed run_time but you are not at or above max_thrust, then you do not decrement thrust. This is problematic because...
2. You don't have any bounds checking on thrust. The way your code is currently set up, you check if thrust <= max_thrust, which means that if thrust == max_thrust, you enter the case and proceed to tack on another thrust_step to your current thrust value, putting thrust = max_thrust + thrust_step. From that point on, your thrust is strictly greater than max_thrust and thus you cannot enter that if statement ever again unless your thrust_multiplier is exactly -1. This is an issue where...
3. Say you detect the the aircraft is tipping and now your thrust_multiplier is negative two. Well, you only modify thrust if your current thrust is <= max_thrust OR your thrust_multiplier == -1. As I state above, your code will wind up with a thrust value of max_thrust + thrust_step and, with thrust_multiplier = -2, you will not enter the statement that modifies thrust again. You will stay "locked out" of that statement until your elapsed time exceeds run_time, at which point thrust_multiplier gets set to -1, which then "unlocks" that if statement and (presumably) allows you to land.

So, these are code issues right now. To summarize, you can exceed max_thrust because you don't have bounds checking on thrust, which then locks you out of being able to modify thrust again until you get thrust_multiplier set to exactly -1, which then only happens when you exceed the time limit.

Now, even if you fix this (add bounds checking to thrust after you modify it!), you are still not likely to get the "hover in place" performance that you're looking for.

To be explicitly clear, I'm assuming you are actually writing thrust correctly. I don't see where thrust is used beyond your modifying the value. I do see that you wrote self._cf.commander.send_setpoint(0, 0, 0, 0); I'm not sure if that's the correct command or if you need to include thrust somewhere in there or not.

Anyways, back to flight - you're not going to get the hover in place functionality you want for several reasons:

1. Biggest reason is that you appear to only be modifying the total thrust. You check if the x gyro OR the y gyro are reading over 75 and take the same action regardless of which axis is rotating. If you wanted to "correct" the tip, then you need to modify the thrust about the particular axis that is "tipping". SIDE NOTE - You just check if the readings are "over 75" but there aren't any units there. Sub-problem (A) here is that a gyro outputs an angular velocity, not an angle, so you might not even be measuring the angle about that axis, and sub-problem (B) is those units might be degrees or radians. 75 radians is almost 4,300 degrees, or almost 12 revolutions.
2. The next problem is that you just appear to "land quickly" if you detect a tipping condition. Once you set the thrust_multiplier to -2, there's no way in your loop to ever set it positive again. If you did modify your code to counter-act roll or pitch, you have no way of resetting it once you stabilized (or got close to stabilized).

My advice:

1. Print the value of thrust to your monitor and verify that it does exceed max_thrust then enforce limit checking that caps thrust at max_thrust.
2. Verify that thrust is actually being used correctly. Can you hard-code a different value for thrust? Does that actually appear to make the change you expected?
3. If you want to control roll and pitch, you need to write to the individual motors, not overall thrust.
4. For roll and pitch control, you'll want something more sophisticated than just incrementing/decrementing the thrust by fixed amounts. Look up some flight controller PID control examples. If this is an open-source quadcopter then there should be a tutorial/manual published for the unit that would show you how to access those control gains.
5. Even if you can get everything setup to control roll and pitch, your quality of control is really only ever as good as your feedback. If you are using a low-quality IMU then you'll get some pretty significant drift and there really isn't much you can do about it.

Answer 2

You will not be able to achieve stable horizontal flight without external sensors. The reason is that the Crazyflie 2.0 quadcopter does not have sensors to know where it is in the room, it only has sensor to know its orientation and its altitude (using a pressure sensor).

Chuck is correct in saying that a quadcopter only pushed thrust downwards. The crazyflie runs a stabilisation loop that uses Gyroscope and Accelerometer to measure and correct its orientation. There is also an altitude-hold loop that uses the pressure sensor and accelerometer to keep a stable altitude. This also means that you do not need to care about the current orientation: just send the wanted orientation setpoint and the onboard control loop will do its best to keep the platform at the given setpoint.

To have a stable horizontal flight you should be able to ask for roll and pitch of 0, and alt-hold with a rising rate of 0m/s (this is encoded with thrust=32568 for the moment in the Crazyflie when alt-hold is enabled). In theory this should result in zero horizontal acceleration since the propeller would only push downwards. Unfortunately, practically this is not working because this is open-loop control and any imperfection in the sensors, motors, motor orientation, or anything else physical, will result in the quadcopter drifting away.

The only way against this drift is to have another sensor, aware of the environment, that can correct this physical imperfection thus closing the position loop. This could be anything from onboard motion sensor to external positioning or camera systems.