STM32 hydrofoil controler

The project consists in programming a function for an STM32 microcontroller. The program uses three input variables, variables A, B1 and B2. These values ​​represent the vertical distance between the boat and the water at the front (A) and at the rear (B1 => right, B2 => left). The thieves are rough, they give the profile of the waves. The frequency of the measurements is 50 Hz. Control loops must use these variables to stabilize the vehicle in a horizontal position at the requested elevation using at least 2 PIDs. One PID for the roll and one for the yaw. The output variables are INT16 which dictate the position of the three servomotors, the front hydrofoil and the two rear hydrofoils. The position instructions must integrate the result of the two PIDs. Much of the programming is to achieve a filter to obtain the average elevation of the water surface by the elevation sensors. The measurements should be filtered to give the average elevation of the water surface. So, detection of the top and the bottom of the waves, extraction of the average value. Input variables: Measure A, measure B1, measure B2 Elevation setpoint Constant for IDPs. Output variable: Position of the servomotors Average values ​​of measures A, B1 and B2