Drone controller is in charge of sensor data fusion and drone stability.
Sensor fusion algorithms combine information from the different sensors (GNSS, IMU, barometer, magnetometer, ultrasonic sensor, TOF, vertical camera) in order to produce an estimation of the drone position, velocity and attitude.
Control algorithms compute the commands needed for the drone to reach the desired trajectory.
Drone Controller is triggered at 200Hz.
The inputs are Commands such as motors start and stop, estimator mode changes and on the other side the control references given on horizontal, vertical and yaw directions.
Drone controller produces an estimation of the drone states (position, velocity, attitude), see Drone Telemetry. Drone controller computes and sends commands to the motors. And sends Events to Flight supervisor.
This process is clocked by the reception of the IMU samples at 200 Hz. First, it acquires sensors data. Secondly, it receives trajectories on horizontal, vertical and yaw directions from Guidance. Then, sensors data are fused to estimate drone states and, subsequently, the motor commands to reach the references are computed and sent to the ESC. Finally, event messages are sent and telemetry data are updated.
If no control reference from Guidance is received, a
watchdog resets these references
and forces the drone into an emergency state. In this case, the specific
behaviour is determined by the latest configuration
message sent by Flight supervisor
set_emergency_config messages). For example, stopping motors
in early take-off steps, or entering in hovering.