Hover implementation

Firmware/software/electronics/mechanics
Post Reply
islamoc
Beginner
Posts: 23
Joined: Sat Nov 05, 2016 5:59 pm

Hover implementation

Post by islamoc »

Hello
I'm working on a School project on the Crazyflie 2.0
What I want to do basically is to implement a hover mode that is going to make the Fly get to a certain attitude and stay there for a certain period
I do not want to use any extra sensors or camera for now I just want to use the sensors on the Crazyflie and also just the capacities of the Crazyflie no extra frameworks

is there is any path to achieve this

Actually I started to work on an extra PID controller that I'm going to implement using the cflib and I'm having a hard time to access the sensors
I don't know if there is any other way to access the sensors other than using the Logger and callbacks I mean a direct access to the sensors in real time

Thank you
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: Hover implementation

Post by arnaud »

Hi,

What do you mean by "just the capacities of the Crazyflie no extra frameworks"? Do you want to implement in the Crazyflie or on the ground doing the calculation on a PC?

To implement in the Crazyflie you will want to look at stabilizer.c, this is the high level stabilizer loop.

To implement from the ground, you need to get the sensor readings using the log subsystem (there is an example in the client git repos to get you started) and send commander packets to steer the copter. Maybe that is something we can help with, what problems are you getting?

/Arnaud
islamoc
Beginner
Posts: 23
Joined: Sat Nov 05, 2016 5:59 pm

Re: Hover implementation

Post by islamoc »

My first problem:
I have started implementing a PID controller on ground to control just the Z axe and for simplification purposes I started just with The P component
Now What I need to to is to get the initial baro.asl when the Copter is on the ground to make it as a zero value reference and after that enter the loop to control the thrust value my problem is that the logger does not get just one value and I don't know if it is possible to use multiple Loggers one to get the initial value and one for the loop any recommandations about this ?
marcus
Bitcraze
Posts: 659
Joined: Mon Jan 28, 2013 7:02 pm
Location: Sweden
Contact:

Re: Hover implementation

Post by marcus »

Hi,

You can add multiple logging configurations and you can start and stop them. It's not possible to do "one-shot" logging, but you can set up a logging configuration and when you get the first value back stop it.
islamoc
Beginner
Posts: 23
Joined: Sat Nov 05, 2016 5:59 pm

Re: Hover implementation

Post by islamoc »

I tried creating two Loggers but one of them is not working when I activate the second

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, logs the Stabilizer
and prints it to the console. After 10s the application disconnects and exits.
"""
import logging
import sys
import time
from threading import Timer

sys.path.append("../src")
sys.path.append("../src/cflib")
import cflib.crtp  # noqa
from cfclient.utils.logconfigreader import LogConfig  # noqa
from cflib.crazyflie import Crazyflie  # noqa

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)


class LoggingExample:
    """
    Simple logging example class that logs the Stabilizer from a supplied
    link uri and disconnects after 5s.
    """

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

        # Create a Crazyflie object without specifying any cache dirs
        self._cf = Crazyflie()
        self.inithight = 0
        cflib.crtp.init_drivers()

        # Connect some callbacks from the Crazyflie API
        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)


        print("Connecting to %s" % link_uri)

        # Try to connect to the Crazyflie
        self._cf.open_link("radio://0/80/250K")

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

    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_stab = LogConfig(name="Stabilizer", period_in_ms=10)
        """self._lg_stab.add_variable("stabilizer.roll", "float")
        self._lg_stab.add_variable("stabilizer.pitch", "float")
        self._lg_stab.add_variable("stabilizer.yaw", "float")"""
        self._lg_stab.add_variable("baro.asl", "float")
        self._lg_stab.add_variable("baro.pressure", "float")
        self._lg_stab.add_variable("baro.temp", "float")
        self._lg_baro = LogConfig(name="Baro", period_in_ms=2000)
        """self._lg_stab.add_variable("stabilizer.roll", "float")
        self._lg_stab.add_variable("stabilizer.pitch", "float")
        self._lg_stab.add_variable("stabilizer.yaw", "float")"""
        self._lg_baro.add_variable("baro.asl", "float")
        self._lg_baro.add_variable("baro.pressure", "float")
        self._lg_baro.add_variable("baro.temp", "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_baro)
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_baro.data_received_cb.add_callback(self._init_asl)
            self._lg_stab.data_received_cb.add_callback(self._return_log_asl)

            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            self._lg_baro.start()
            self._lg_stab.start()
        except KeyError as e:
            print("Could not start log configuration,"
                  "{} not found in TOC".format(str(e)))
        except AttributeError:
            print("Could not add Stabilizer log config, bad configuration.")

        # Start a timer to disconnect in 10s
        t = Timer(2.2, self._cf.close_link)
        t.start()

    def _stab_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print("Error when logging %s: %s" % (logconf.name, msg))

    def _stab_log_data(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        print("[%d][%s]: %s" % (timestamp, logconf.name, data))
        time.sleep(0.1)
    def _stab_log_asl(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        print("[%d][%s]: %f" % (timestamp, logconf.name, data["baro.pressure"]))
        time.sleep(0.1)
    def _init_asl(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        #print("[%d][%s]: %f" % (timestamp, logconf.name, data["baro.asl"]))
        self.inithight = data["baro.asl"]
        print("Initial Hight: %f" % (self.inithight))
    def _return_log_asl(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        hnow = data["baro.asl"]
        hdiff = hnow - self.inithight
        print(self.inithight)
        """thrust_mult = 1
        thrust_step = 500
        thrust = 20000
        pitch = 0
        roll = 0
        yawrate = 0
        self._cf.commander.send_setpoint(0, 0, 0, 0)
        while thrust >= 20000:
            self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
            time.sleep(0.1)
            if thrust >= 35000:
                thrust_mult = -1
            thrust += thrust_step * thrust_mult"""
        time.sleep(0.02)

    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the speficied address)"""
        print("Connection to %s failed: %s" % (link_uri, msg))
        self.is_connected = False

    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)
        self.is_connected = False


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 = LoggingExample("radio://0/80/250K")
    #else:
    #    print("No Crazyflies found, cannot run example")

    # The Crazyflie lib doesn't contain anything to keep the application alive,
    # so this is where your application should do something. In our case we
    # are just waiting until we are disconnected.
    while le.is_connected:
        time.sleep(1)
Post Reply