FlyPi/crazyflie-firmware-2021.06/docs/functional-areas/sensor-to-control/controllers.md

+---
+title: Controllers in the Crazyflie
+page_id: controllers
+---
+
+
+Once the [state estimator](/docs/functional-areas/sensor-to-control/state_estimators.md) have outputed the current (estimated) situation of the crazyflie in position velocity and attitude, it is time for the controllers to keep it that way or to move the crazyflie into a new position based on a setpoint. This is an important part of the stabilization system in the crazyflie.
+
+ * [Overview of Control](#overview-of-control)
+ * [Cascaded PID controller](#cascaded-pid-controller)
+ * Mellinger Controller (TO DO)
+ * INDI Controller (TO DO)
+
+## Overview of control
+There are three levels to control in the crazyflie:
+* Attitude rate
+* Attitude absoluut
+* Position or velocity
+
+
+Here is an overview of the types of controllers there are per level:
+
+![controller overview](/docs/images/controller_overview.png){:width="500"}
+
+We will now explain per controller how exactly they are being implemented in the [crazyflie-firmware](https://github.com/bitcraze/crazyflie-firmware/).
+
+[go back to top](#)
+
+
+## Cascaded PID controller
+
+
+So the default settings in the Crazyflie firmware is the [proportional integral derivative (PID)](https://en.wikipedia.org/wiki/PID_controller) control for all desired state aspects. So the High Level Commander (HLC) or position will send desired position set-points to the PID position controller. These result in desired pitch and roll angles, which are sent directly to the attitude PID controller. These determine the desired angle rates which is send to the angle rate controller. This is also called Cascaded PID controller. That results in the desired thrusts for the roll pitch yaw and height that will be handled by the power distribution by the motors.
+
+
+Here is a block schematics of how the PID controllers are implemented.
+
+![cascaded pid controller](/docs/images/cascaded_pid_controller.png){:width="700"}
+
+Here are the different loops of the cascaded PID explained in more detail.
+
+### Attitude Rate PID controller
+
+The attitude rate PID controller is the one that directly controls the attitude rate. It resieves almost directly the gyroscope rates (through a bit of filtering first) takes the error between the desired attitude rate as input. This output the commands that is send directly to the power distribution ([power_distribution_stock.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/power_distribution_stock.c)). The control loop runs at 500 Hz.
+
+Check the implementation details in [attitude_pid_controller.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/attitude_pid_controller.c) in `attitudeControllerCorrectRatePID()`.
+
+### Attitude PID controller
+
+The absolute attitude PID controller is the outerloop of the attitude controller. This takes in the estimated attitude of the [state estimator](/docs/functional-areas/sensor-to-control/state_estimators.md), and takes the error of the desired attitude setpoint to control the attitude of the Crazyflie. The output is desired attitude rate which is send to the attitude rate controller. The control loop runs at 500 Hz.
+
+Check the implementation details in [attitude_pid_controller.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/attitude_pid_controller.c) in `attitudeControllerCorrectAttitudePID()`.
+
+### Position and Velocity Controller
+
+The most outerloop of the cascaded PID controller is the position and velocity controller. It receives position or velcoityinput from a commander which are handled, since it is possible to set in the variable `setpoint_t` which  stabilization mode to use `stab_mode_t` (either position:  `modeAbs` or `modeVelocity`). These can be found in [stabilizer_types.h](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/stabilizer_types.h). The control loop runs at 100 Hz.
+
+Check the implementation details in Check the implementation details in [position_controller_pid.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/position_controller_pid.c) in `positionController()` and  `velocityController()`.
+
+[go back to top](#)

FlyPi/crazyflie-firmware-2021.06/docs/functional-areas/sensor-to-control/index.md

+---
+title: Stabilizer Module
+page_id: stabilizer_index
+---
+
+This page is meant as an introduction and overview of the path from
+sensor acquisition to motor control,also called the stabilizer module. It will not go into detail but it mostly give a general outline of how the sensor measurements go to the
+state estimators to the controllers and finally distributed to the motors
+by power distribution. Ofcourse, the motors have an affect on how the
+crazyflie flies and that inderectly has an effect on what the sensors
+detect in the next time step.
+
+ * [Sensors](#sensors)
+ * [State Estimation](#state-estimation)
+ * [State Controller](#state-controller)
+ * [Configure estimators and control](#configuring-controllers-and-estimators)
+ * [Commander Framework](#commander-framework)
+ * [Power Distribution](#power-distribution)
+
+
+
+### Overview
+
+![sensor](/docs/images/sensors_to_motors.png){:width="700"}
+
+## Modules
+
+
+### Sensors
+
+Sensors are essential for the flight of a crazyflie. Here is selection of the sensors
+ listed that the crazyflie eventually uses for state estimation:
+
+
+* [On-board Sensors](https://store.bitcraze.io/products/crazyflie-2-1)
+  * Accelerometer: acceleration in body fixed coordinates in m/s2
+  * Gyroscope: angle rate in roll pitch and yaw (rad/s)
+  * Pressure Sensor: Airpressure in mBar
+* [Flowdeck v2](https://store.bitcraze.io/products/flow-deck-v2)
+  * ToF sensor*:  Distance to a surface in milimeters
+  * Optical flow sensor:  The detection movement of pixels in px per timesample
+* [Loco positioning deck](https://store.bitcraze.io//products/loco-positioning-deck):
+  * Ultra Wide band module: The distance between two UWB modules or TDOA*** in meters.
+* [Lighthouse deck](https://store.bitcraze.io/products/lighthouse-positioning-deck):
+  * IR receivers: Sweep angle of htc vive basestations in radians.
+
+<sub><sup>_*Time-of-Flight_</sup></sub>
+
+<sub><sup>_**[Zranger v2](https://store.bitcraze.io/collections/decks/products/z-ranger-deck-v2) also contains a laser-ranger_</sup></sub>
+
+<sub><sup>_***Time-difference of Arrival_</sup></sub>
+
+[go back to top](#)
+
+### State Estimation
+
+There are 2 state estimators in the crazyflie:
+* Complementary Filter
+* Extended Kalman Filter
+
+ Go to the [state estimation page](state_estimators.md) for more indepth information about how the state estimation is implemented in the crazyflie firmware.
+
+[go back to top](#)
+
+
+### State Controller
+There are 3 controllers in the crazyflie
+* PID controller
+* INDI controller
+* Mellinger controller
+
+Go to the [controllers page](controllers.md), for more indepth information about how the controllers are implemented in the crazyflie firmware.
+
+[go back to top](#)
+
+
+### Configuring Controllers and Estimators
+Go to this [configuration page](configure_estimator_controller.md), if you would like to configure different controllers and estmators,
+
+[go back to top](#)
+
+
+### Commander Framework
+An desired state can be handled by the setpoint structure in position or atitude, which can be set by the cflib or the highlevel commander.
+
+Go to the [commander page](commanders_setpoints.md), for more indepth information about how the commander framework are implemented in the crazyflie firmware, please go
+
+[go back to top](#)
+
+### Power Distribution
+
+After the state controller has send out its commands, this is not the end of the line yet.
+The controllers send out their commands relating to their yaw, roll and pitch angles.
+How the motors should respond in order to adhere these attitude based commands depends on a few factors:
+  * Quadrotor configuration (found in: [power_distribution_stock.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/power_distribution_stock.c)):
+    * x-configuration: The body fixed coordinate system's x-axis is pointed in between two propellors (Default)
+    * +-configuration: The body fixed coordinate system's x-axis is pointed in one propellor
+  * Motors:
+    * **Explaination about this will come soon**
+
+
+[go back to top](#)

FlyPi/crazyflie-firmware-2021.06/docs/functional-areas/sensor-to-control/state_estimators.md

+---
+title: State estimation
+page_id: state_estimators
+---
+
+
+
+A state estimator turns sensor signals into an estimate of the state that the crazyflie is in. This is an essential part of crazyflie's stabilizing system, as explained in the [overview page](/docs/functional-areas/sensor-to-control/index.md). State estimation is really important in quadrotors (and robotics in general). The Crazyflie needs to first of all know in which angles it is at (roll, pitch, yaw). If it would be flying at a few degrees slanted in roll, the crazyflie would accelerate into that direction. Therefore the controller need to know an good estimate of current angles’ state and compensate for it. For a step higher in autonomy, a good position estimate becomes important too, since you would like it to move reliably from A to B.
+
+* [Complementary filter](#complementary-filter)
+* [Extended Kalman filter](#extended-kalman-filter)
+* [References](#references)
+
+## Complementary filter
+
+![complementary filter](/docs/images/complementary_filter.png){:width="500"}
+
+The complementary filter is consider a very lightweight and efficient filter which in general only uses the IMU input of the gyroscope (angle rate) and the accelerator. The estimator has been extended to also include input of the ToF distance measurement of the [Zranger deck](https://store.bitcraze.io/collections/decks/products/z-ranger-deck-v2). The estimated output is the Crazyflie’s attitude (roll, pitch, yaw) and its altitude (in the z direction). These values can be used by the controller and are meant to be used for manual control.
+
+To checkout the implementation details, please checkout the firmware in [estimator_complementary.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/estimator_complementary.c) and [sensfusion6.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/sensfusion6.c). The complementary filter is set as the default state estimator on the Crazyflie firmware, unless a deck is mounted that requires the kalman filter.
+
+[go back to top](#)
+
+## Extended Kalman filter
+
+![extended kalman filter](/docs/images/extended_kalman_filter.png){:width="500"}
+
+The (extended) Kalman filter (EKF) is an step up in complexity compared to the complementary filter, as it accepts more sensor inputs of both internal and external sensors. It is an recursive filter that estimates the current state of the Crazyflie based on incoming measurements (in combination with a predicted standard deviation of the noise), the measurement model and the model of the system itself.
+
+We will not go into detail on this but we encourage people to learn more about EKFs by reading up some material like [this](https://idsc.ethz.ch/education/lectures/recursive-estimation.html).
+
+Because of more state estimation possibilities, we prefer the EKF for certain decks that can provide information for **full pose estimation** (position/velocity + attitude). These are the:
+
+ * [Flowdeck v2](https://store.bitcraze.io/collections/decks/products/flow-deck-v2)
+ * [Loco positioning deck](https://store.bitcraze.io/collections/positioning/products/loco-positioning-deck)
+ * [Lighthouse deck](https://store.bitcraze.io/products/lighthouse-positioning-deck).
+ * Mocap deck [passive](https://store.bitcraze.io/products/motion-capture-marker-deck) or [active](https://store.bitcraze.io/products/active-marker-deck)
+
+ When the `DeckDriver` is initialized in the [crazyflie-firmware](https://github.com/bitcraze/crazyflie-firmware/), for these decks the variable `.requiredEstimator` is set to `kalmanEstimator`. This says that the firmware should not use the default complementary filter but the EKF instead.
+
+Here we will explain a couple of important elements that are essential to the implementation, however we do encourage people to also look into the code [estimator_kalman.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/estimator_kalman.c) and [kalman_core.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/kalman_core.c). Also read the papers of [1] and [2] for implementation details.
+
+### Kalman supervisor
+
+The Kalman filter has an supervisor that resets the state estimation if the values get out of bounds. This can be found here in [kalman_supervisor.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/kalman_supervisor.c).
+
+### Measurement Models
+
+This section will explain how the signals of the sensors are transformed to state estimates. These equations are the base of the measurement models of the EKF.
+
+* [Flowdeck Measurement Model](#flowdeck-measurement-model)
+* Locodeck Measurement Model (TODO)
+* [Lighthouse Measurement Model](/docs/functional-areas/lighthouse/kalman_measurement_model.md)
+
+
+#### Flowdeck Measurement Model
+
+This illustration explains how the height from the VL53L1x sensor and flow from the PMW3901 sensor are combined to calculate velocity. This has been implemented by the work of [3] and can be found in [kalman_core.c](https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/kalman_core.c) in the function `kalmanCoreUpdateWithFlow()`.
+
+![flowdeck velocity](/docs/images/flowdeck_velocity.png){:width="500"}
+
+
+[go back to top](#)
+
+
+## References
+[1] Mueller, Mark W., Michael Hamer, and Raffaello D'Andrea. "Fusing ultra-wideband range measurements with accelerometers and rate gyroscopes for quadrocopter state estimation." 2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015.
+
+[2] Mueller, Mark W., Markus Hehn, and Raffaello D’Andrea. "Covariance correction step for kalman filtering with an attitude." Journal of Guidance, Control, and Dynamics 40.9 (2017): 2301-2306.
+
+[3] M. Greiff, Modelling and Control of the Crazyflie Quadrotor for Aggressive and Autonomous Flight by Optical Flow Driven State Estimation, Master’s thesis, Lund University, 2017

FlyPi/crazyflie-firmware-2021.06/docs/functional-areas/trajectory_formats.md

+---
+title: Trajectory formats
+page_id: trajectory_formats
+---
+
+This document describes how the Crazyflie high-level commander stores
+trajectories in the internal trajectory memory.
+
+Trajectories for the high-level commander may be stored in one of two formats.
+either as a sequence of raw 7th degree polynomials, or as a more space-efficient
+representation using the same polynomials in Bernstein form. In both cases, a
+single segment stores a duration and one polynomial for each of the X, Y, Z and
+yaw coordinates of the trajectory.
+
+Raw representation
+------------------
+
+The raw representation stores the coefficients of the polynomial describing
+the X coordinate of the trajectory first, starting with the constant term and
+ending with the 7th degree term, followed by the terms of the polynomials
+describing the Y, Z and yaw coordinates, and the duration of the segment itself
+in seconds. Each term and the duration is encoded as standard IEEE
+single-precision floats, meaning that a single segment requires 132 bytes:
+8x4 floats for the X, Y, Z and yaw components per trajectory segment, plus one
+additional float per segment to store its duration. Given that the default
+size of the trajectory memory is 4 Kbytes, you can only store 31 segments.
+
+Compressed representation
+-------------------------
+
+The compressed representation was designed to be more space-efficient than the
+raw representation while still trying to maintain almost the same degree of
+accuracy as the raw representation.
+
+The general guidelines of this representation are as follows:
+
+* Durations are represented as milliseconds and are stored as signed 2-byte
+  integers.
+
+* Spatial coordinates (X, Y and Z) are represented as millimeters and are stored
+  as signed 2-byte integers, meaning that the maximum spatial volume that this
+  representation can cover is roughly 64m x 64m x 64m, assuming that the origin
+  is at the center.
+
+* Angles (for the yaw coordinate) are represented as 1/10th of degrees and are
+  stored as signed 2-byte integers.
+
+* Trajectory segments are described with varying degrees of Bézier curves,
+  ranging from degree 0 (constant) to degree 7 (full 7D polynomial, as in the
+  raw representation).
+
+The compressed representation starts with the description of the starting point
+of the trajectory:
+
+```
++--------------+--------------+--------------+--------------+
+| X coordinate | Y coordinate | Z coordinate | Initial yaw  |
++--------------+--------------+--------------+--------------+
+```
+
+This is then followed by data blocks describing the individual segments - one
+data block per segment:
+
+```
++------------------+----------------+------------------+----------------+-----+
+| Segment 1 header | Segment 1 body | Segment 2 header | Segment 2 body | ... |
++------------------+----------------+------------------+----------------+-----+
+```
+
+The header of a segment is three bytes long. The first byte describes how the
+X, Y, Z and yaw coordinates will be encoded in the body of the data block. The
+remaining two bytes contain the duration of the segment:
+
+```
+  Bits 6-7     Bits 4-5   Bits 2-3   Bits 0-1        Byte 2         Byte 3
++------------+----------+----------+----------+  +--------------+--------------+
+| Yaw format | Z format | Y format | X format |  | Duration LSB | Duration MSB |
++------------+----------+----------+----------+  +--------------+--------------+
+```
+
+For each of the X, Y, Z and yaw coordiates, there are two bits in the first byte
+of the header. `00` means that the coordinate is constant throughout the
+section (i.e. it is a zero-order Bézier curve). `01` means that the coordinate
+changes linearly (it is a Bézier curve of order 1). `10` means that the
+coordinate changes according to a cubic Bézier curve. `11` means that the
+coordinate changes according to a full 7th degree polynomial, expressed again
+as a 7th degree Bézier curve.
+
+For instance, a header containing `0x0a 0xd0 0x07` means that the segment is
+2000 milliseconds long (because `0x07d0` = 2000), and it will have a constant
+yaw and Z coordinate. Cubic Bézier curves will be used to describe the X and
+Y coordinates of the segment. (This is because `0x0a` = `00 00 10 10` in
+binary).
+
+Note that the choice of Bézier curves means that we need to store not the raw
+coefficients of the corresponding polynomials (which may be anywhere in the full
+4-byte float range), but the coordinates of the _control points_ of the Bézier
+curves, whose range is much more predictable and easier to represent with a
+fixed millimeter-scale quantization.
+
+The body of the segment is then simply a concatenation of the control points
+for the X, Y, Z and yaw coordinates, _omitting the first control point_
+because it is always the same as the last control point of the previous
+segment. (That's why we needed to store the starting point of the trajectory
+separately).
+
+```
++----------------------+----------------------+----------------------+------------------------+
+| Control points for X | Control points for Y | Control points for Z | Control points for yaw |
++----------------------+----------------------+----------------------+------------------------+
+```
+
+Obviously, when the header specifies that a coordinate is constant, it means
+that we do not need to store the control points for that coordinate at all
+(there would be only a single control point, but we already know that it is
+the same as the last control point of the previous segment). For linear
+segments, we only need to store the end of the segment. For cubic Bézier
+segments, we need to store the first and second control point and the end
+of the segment. This can result in significant space savings if the trajectory
+consists mostly of linear segments and cubic Bézier curves, especially if the
+yaw does not change, or if the movement mostly occurs in the X-Y, X-Z or Y-Z
+plane. Note that cubic Bézier curves are enough to ensure C0, C1 and C2
+continuity (inposition, velocity and acceleration) for the trajectories.
+
+Internally, whenever the high-level commander starts processing a trajectory
+segment that is encoded using the compressed representation, it converts the
+Bézier curve back into its raw polynomial representation. It means that most of
+the codebase only needs to work with raw 7th degree polynomials.
+
+A downside of the compressed representation is that it is hard to play the
+trajectory backwards. The current implementation does not support reverse
+traversal at all.