High-Level Modular Autopilot Solution for Fast Prototyping of Unmanned Aerial Systems

A redundant fast prototyping autopilot solution for unmanned aerial systems has been developed and successfully tested outdoors. While its low-level backbone is executed in a Raspberry PiⓇ 3 + NAVIO2Ⓡ with a backup autopilot, the computational power of an IntelⓇ NUC mini-computer is employed to implement complex functionalities directly in SimulinkⓇ, thus including in-flight debugging, tuning and monitoring. Altogether, the presented tool provides a flexible and user-friendly high-level environment with enhanced computational capabilities, which drastically reduces the prototyping timespans of complex algorithms –between 50% and 75%, according to our long and proven experience in aerial robotics–, while preventing incidents thanks to its redundant design with a human-in-the-loop pilot on the reliable PX4. Three typical outdoor cases are carried out for validation in real-life scenarios, all mounted in a DJI© F550 platform. Full integration results and telemetry for more than 50 hours of outdoor flight tests are provided.


I. INTRODUCTION
The increasing complexity of new applications for small Unmanned Aerial Systems (UAS) is demanding autopilots with higher computational power and, more importantly, faster prototyping timespans. The novel branch of Aerial Robotics is pushing forward in this direction, mainly due to its broad range of applications, such as contact inspection, maintenance, and assembling at inaccessible and/or dangerous places. For a detailed literature review on this emerging area, we refer interested readers to cutting-edge projects [1]- [3] and its ongoing follow-up in [4] (see survey papers [5], [6]), the overview of aerial manipulation in [7], the applications with multi-joint manipulators in [8], [9], [10] and [11], the self-coordinated approach in [12] and [13], and to the ad-hoc autopilot in [14]. In particular, in [2] we challenged ourselves to complete complex missions outdoors, being the inter-consortium integration and code The associate editor coordinating the review of this manuscript and approving it for publication was Cong Pu . customisation difficulties found while implementing new complex algorithms the main motivation of this work.
To cope with these obstacles, most of the aerial platforms depend whether on autopilots based on open-source software compiled in target computer boards (e.g. PX4 in [15], [16]), or on ad-hoc solutions made by research groups to fulfil their needs, e.g. in [17]. Thus, advances in these complex functionalities are implemented either modifying the open-source code to specifically implement new challenging guidance and/or control strategies, as in [18] for Model Predictive Control (MPC) or in [19] for backstepping sliding mode control (SMC), or for general purpose applications, as in [20], [21] and [22]; or linking it with previously built software, as in the multi-layer approach in [23], the model-based design and validation in [24] or the architecture for a nonlinear controller in [25]. Although these open autopilot options provide a wide range of well-polished features that are sufficient in most outdoor 1 cases, their thorough internal integration distorts the project schedules by expanding the time devoted to the customisation of source code. To illustrate this point, a comparison between widely used autopilots for UAVs 2 and the proposed Modular Autopilot Solution MAS PX4 is shown in Table 1. It is worth noting that this study is based on the deep knowledge acquired by our research group after more than 50 aerial projects along the last 10 years, allowing us to identify common drawbacks in the early stages of prototyping of these autopilot solutions: • The implementation of customised functionalities generally demands offline ad-hoc modifications of the native source code, resulting in longer debugging times during the integration stages.
• The implementation of complex algorithms is not possible in the existing computer boards, mainly due to the lack of computational power and/or to their closed nature, which, in turn, might compromise their feasibility. Even though the prolongation of the timespans by the former could be tolerated on paper, the solution commonly given to adhere to the project deadlines when it occurs is to limit the extension of the advances in complex functionalities. In contrast, we found the latter a hard constraint while executing dexterous manipulation tasks in the onboard computer in [2], mainly due to the coverage of localisation, vision, flight control and manipulation subtasks. Altogether it ends up forcing a trade-off that downgrades the implementation, and hence, the overall performance. To overcome these limitations, the main contributions of the MAS PX4 autopilot solution are: C1. Drastic reduction of programming and debugging times. The full use of the user-friendly and highly modular Simulink R [35] environment expedites the software development and implementation of complex algorithms. C2. High-level in-flight code debugging, tuning and monitoring, while preserving the autopilot redundancy for unexpected incidents and/or learning capabilities. C3. Enough computational power to implement and execute heavy and complex algorithms onboard, with no need for downgrading the implementation. C4. Nonetheless, selected functionalities of PX4 can be kept running, thus providing a step-by-step way to implement fast-prototyping functionalities as desired.
For instance, here we keep the EKF estimator and the motor mixer and modify the planning and control core algorithms. Remark 1: We underscore that the fast prototyping potential relies on two novel specifications. First, the mainframe Simulink R environment whose modularity and fast integration capabilities are widely known, facilitating the external validation and/or cooperation between research groups. Secondly, to the best of the authors' knowledge, MAS PX4 is the first autopilot with in-flight human-in-the-loop redundancy and debug capabilities, drastically reducing the implementation and tuning times.
Remark 2: It is also worth highlighting the flexibility of the MAS PX4 framework in terms of computational capabilities. In here, we have proposed a particular hardware solution with an Intel R NUC computer board, but it could be replaced by others. For example, sooner than later the All Up Weight (AUW) is expected to be reduced, as it evinces the arrival of lighter computers, such as [37] or the recent release [38] with an NPU of 5 TOPS. 3 The paper is structured as follows: Section II describes the hardware and software architectures as well as a thorough description of the autopilot low-level implementation, and Section III is devoted to the experimental validation in three separate cases of study. Finally, the paper is wrapped up with a conclusion section.

II. AUTOPILOT ARCHITECTURE
As aforementioned, the main goal of the proposed architecture is to obtain a safe autopilot that facilitates the fastprototyping implementation of complex algorithms onboard, thus overcoming the previously presented limitations and allowing their full experimental demonstration. This is achieved by making a reliable environment that integrates: i) the customised algorithms in Simulink R -more complex and optimised over a wider range of conditions-; and ii) the flight stack PX4 (see [15], [16]) -simpler, more reliable, and well-known for having been used for more than 10 years in the majority of our prototypes of different projects-. A detailed discussion of similar types of software  architectures in commercial flight control systems is provided in [39], of course despite the apparent differences. The actual hardware implementation of this solution is shown in Fig. 1 and a sketch of its global software functionality -highlighting the backup and bypass lines-is depicted in Fig. 2.
In particular, the proposed solution is implemented with custom daemon modules, located after the state estimator and before the motor mixer 4 : i) the PX4 ecl/EKF state estimation is sent to the customisation environment, making use of the current reliable estimator, which is not the focus of this work; and ii) both redundant controllers, i.e. the cascade-like of PX4 and the customised one, join at the command switch (RC Switch in Figs. 2 and 3).
In this second point, the roles of the two agents involved in the experimentation phase come into action. On the one hand, the ground operator, an expert engineer involved in the development of the algorithm being validated, confirms the correct behaviour of the new functionalities. On the other hand, the backup pilot (or pilot in command) is in charge of guaranteeing the safety conditions of aircraft during the flight. If during their task, this pilot detects any risk in the experiment, they would trigger the backup switch, bypassing the prototyped algorithm to the backup PX4 controller. It is worth noting that the latter has been previously tested and tuned to be used as a safe reliable controller. 4 This PX4 version is available at https://github.com/grvcTeam/MASp.

A. HARDWARE ARCHITECTURE
The hardware implementation -schematically depicted in Fig. 3-consists of a Raspberry Pi R 3 [40] equipped with a NAVIO2 R as carrier [41], used for both the backup controller and the state estimation; and an Intel R NUC [42] as the main computer. In this approach, the Raspberry Pi R 3 is adopted as the interface board and delegated the backup autopilot, and the Intel R NUC is dedicated to implementing customised algorithms directly on Simulink R modules (e.g. control, planning or navigation), maximising the computational capacity. All of these make the autopilot modular, customisable and safe, due to the redundancy master backup switch.

Remark 3: It is important to highlight that those modules do not need to be compatible with the Simulink R external mode, thus guaranteeing broader compatibility in normal mode and full functionality. Additionally, accelerator mode can be used if needed, with its implicit limitations (see Appendix A).
For the sake of completeness, a comparison with other environments is in order. On the one hand, the proposed autopilot has in common with the Dronecode ecosystem [43] -which includes PX4-the possibility of using the hardware Raspberry Pi R 3 + NAVIO2 R . However, none of the projects fostered by Dronecode employs an external computer board for the implementation of complex algorithms. Additionally, the ROS capabilities could be enabled through MAVLink/MAVROS (see [36]) -as in Dronecode-or via the ROS toolbox in Simulink R , and hence, both providing full connectivity. On the other hand, the proposed autopilot significantly improves the technical specifications of another VOLUME 8, 2020 common solution, Pixhawk [44], which is known for being only valid in simple and slow tasks -due to its PID implementation and sample frequency-, and for not supporting Ethernet communications, unlike the proposed approach.
Additionally, it is also worth mentioning ROSflight [45], a recently appeared board with which the MAS PX4 environment concurs on some characteristics. Both share, for instance, the implementation of new algorithms on a companion computer board, and their sensor and actuator interfaces are similar to our Raspberry Pi R 3 + NAVIO2 R approach. Nonetheless, although ROSflight also focuses on easing the development stages -in particular the low-level coding-, it is based on ROS; while in MAS PX4 , this feature is optional and the prototyping interface is the user-friendly graphical interface Simulink R . Additionally, MAS PX4 includes redundancy with the mature PX4 backup, thus offering the possibility of progressively implementing custom functionalities safely.
Moving back to MAS PX4 , it is worth noting that a trade-off between the computational power and the endurance has to be a priori decided, as usual in electrically-powered UAS applications. As commented in Remark 2, for lower computational missions, the external board Intel R NUC can be replaced by any other that reduces the AUW of the benchmark platform (3.2 Kg), and improves its endurance (about 8 min).

B. SOFTWARE INTEGRATION
The core of this reliable implementation is the integration of PX4 with the prototyping environment. A thorough analysis is therefore dedicated to the internal UDP communications, 5 which over a robust Ethernet communication form the sinequa-non condition for the reliability of the whole solution.
On the one hand, the quality of the Ethernet UDP connection is studied in terms of bandwidth, jitter and lost datagrams in a 20 seconds experiment with overestimated 6 packages (25 MB) using iPerf [46]. This tool calculates jitter as the difference between the receiving and sending times -via a timestamp added to the package sent by the client-, correcting any discrepancy due to the lack of clock synchronisation. The results are then collected in Table 2, confirming that no datagrams were lost during the test and that a bandwidth of 1 Mb/s is more than enough for the application. In what respects to the jitter, it should be highlighted that, although the package congestion in UDP protocols is lower than in TCP, this fluctuation could also have an impact on UDP applications. Nonetheless, the provided results evince that the jitter in the proposed solution is negligible.
On the other hand, a preliminary study on the Round Trip Time (RTT) -that includes the communication and the customised code execution-is also conducted. For that purpose, the time between a simple RC command is given and its impact on the controller output is measured. 7 As a result, we obtain a mean RTT estimation of 1235 µs using the controller in Subsection III-A. In comparison, the PX4 output runs at 250Hz (4000 µs), being the RTT of this application below this sample time. Nonetheless, the influence of the complexity of the customised algorithm on the execution times should also be studied to complete this analysis. This is done in part in Subsection II-C but more specifically in the load test for a computational-demanding implementation in Fig. 8, showing that the computational capabilities of the solution are well above the current demands. Accordingly, we can conclude that an increase in the complexity of the algorithms implemented can be compensated by the computational power margins of the proposed environment. Considering this, the RTT is not being significantly affected.

C. SIMULATION VERSUS REAL-TIME OVERSAMPLING
The proposed solution is designed to use the full potential of the Simulink R environment (i.e. avoiding incompatibilities), and to reduce the simulation-to-experimentation times at the expense of real-time. This results in two phenomena: timevariant random disturbances and high-frequency spurious signals. To study both of them, let us define the time ratio TR as the rate between the clock time according to the simulation records and its real value according to the PX4 board clock. 8 A thorough batch of experimental tests was carried out and, 7 Without passing by the EKF estimator, being considered the contributions of this step to the RTT negligible for this analysis. 8 Notice that the implementation of explicit time-variant terms, such as integrals, will need the corresponding correction with this time ratio. as shown in Fig. 4, the TR is only altered during short initial transients of about 2 seconds, thus being assimilable to the time convergence of the ecl/EKF estimator in PX4. Consequently, this time ratio can be considered constant in the design of custom algorithms (but being recommended to recalculate its value after significant changes in the complexity of the algorithms to re-adjust time-varying terms).
Moreover, the potential problems that may arise for highfrequency residual signals were also analysed in depth. The experimental tests evince that in all cases those spurious signals were completely filtered by the propulsion system dynamics and other common UAS filters, as well as for the 200-250Hz sampling rate limitation of the PX4 motor mixer uORB. In summary, no incidents were detected in more than 50 hours of flight, and therefore the influence of signal oversampling is considered negligible in UAS applications.

III. EXPERIMENTAL VALIDATION (EV)
The validation of the autopilot is demonstrated through three typical implementation cases, thus providing a general overview of its possibilities in real-life scenarios, and showing the friendly fast-prototyping implementation of simple and complex algorithms. These experiments, in which a standard DJI c F550 frame [47] (see Fig. 1) is selected as the platform to mount the autopilot, are: EV1. The progressive development of a cascade PID controller mimetic to the controller in PX4, to illustrate the redundancy and reliability of MAS PX4 and to serve as an example of the implementation of a complete strategy. EV2. A complex nonlinear control algorithm without any simplification, to reveal the computational capabilities of the autopilot as well as to demonstrate its feasibility for more complex implementations. Additionally, since the algorithm runs in faster-than-real-time, its implementation stability is analysed.
EV3. A navigation module, to illustrate the wide range of possibilities MAS PX4 offers for fast prototyping apart from control (not be confounded with a final evaluation). Accordingly, the controller chosen to run underneath this module is the cascade PID in EV1, thus assuring that those researchers not mainly interested in control can implement their modules without getting discouraged by the complexity and lack of intuitiveness of the controller. The procedure followed to implement these cases -thought to avoid incidents while changing the core algorithms-is also worth mentioning. Firstly, the basic functionalities of an initial solution in Simulink R progressively replace their homologous in PX4, testing these changes in a confined scenario with the platform suspending from a structure. When all these basic changes are checked, the limitations of the scenario are relaxed by removing the suspension of the platform, thus allowing a realistic evaluation of the novel functionalities. Finally, the obtained control strategy is demonstrated in a non-confined scenario equivalent to a fully autonomous flight outdoors, testing the higher-level capabilities. As a result, a reliable complete solution directly evolving from a simulation-based development is obtained.
To ease understanding of the implementation of the algorithms for EV1-EV3, in Fig. 5 we show the general modular structure of the Simulink R environment. This includes the inputs and outputs and the position (A) and attitude (B) subsystems, whose contents will be detailed in each experiment.

A. EV1: SIMPLE PID CONTROLLER MODULE
A standard PID cascade controller is implemented by progressively replacing parts of its PX4 counterpart and tuning them from the inner to the outer loop, as follows: i) Attitude rates; ii) Attitude; iii) Altitude; iv) Cartesian speed; and, finally, v) Cartesian position, resulting in a semi-autonomous controller. As previously mentioned, this cascade controller VOLUME 8, 2020 is intended to be mimetic to the native controller in PX4, but implemented in Simulink R using the Intel R NUC. This is used both to validate the customisation environment and to illustrate the redundancy capabilities of the solution.
In Fig. 6 we depict the Simulink R implementation for subsystems (top), together with the results for both attitude and position controllers (bottom), respectively. Moreover, all references, including position, attitude and their associated rates, are tracked with minimal steady-state error, similarly to the error in PX4. Altogether, this demonstrates that both controllers can be considered mimetic and corroborates the whole reliability of the autopilot.

B. EV2: COMPLEX NONLINEAR CONTROLLER MODULE
To exemplify the computational capabilities, we implement a cascade nonlinear controller based on feedback linearisation and command-filtered backstepping. This type of controllers -together with the cascade PIDs-are the most commonly used in this kind of multi-rotor platform. Although standard in aerial robotics, the one implemented here is an adapted version of [48] -and references therein-, being briefly summarised next. Thus, let U =col(U 1 , U 2 , U 3 ) ∈ R 3 be the virtual command in the Cartesian position outer loop of the cascade, hence becoming the feedback-linearisation controller with p ref ∈ R 3 the cartesian position reference and p e ∈ R 3 its associated error; K P , K I , K D ∈ R 3×3 the matrix control gains; and p I ,0 e =col(0, 0, z I ,0 e ) a feedforward integral action counteracting the initial vertical forces, z I ,0 e . This outer loop provides the attitude reference commands for the inner and nonlinear control loop reading where m ∈ R is the UAV mass, g ∈ R 3 the gravity acceleration vector, T ∈ R the total thrust and ψ ∈ R the yaw angle.
On the other hand, the attitude controller of the inner loop is formulated using the command-filtered backstepping technique (see references therein [48]), yielding with τ a ∈ R 3 the control torque produced by the differential thrust between the rotors (see [49] and references therein); =col(φ, θ, ψ) ∈ R 3 and ∈ R 3 the attitude and attitude rate, with W ( ) ∈ R 3×3 their associated matrix; I ∈ R 3×3 the inertia matrix of the UAV in the body frame; − the filtered attitude error; ∈ R 3 a filter to counteract the mismatch of the tracking error; the attitude rate error, and 1 , 2 and ∈ R 3×3 are control and filter matrix gains. Notice that the gyroscopic terms have been neglected because of their irrelevance for this platform, and that the highorder implementation of this solution includes 9 nonlinear integrators. In Fig. 7 we depict again the Simulink R implementation of this much more involved nonlinear controller for the subsystems (top), together with the results at the bottom when commanded to track a variable reference. It is worth noting in this case that the attitude controller steady-state error does not affect the position controller.
Additionally, a CPU load test for this algorithm -in which aggressive RC commands and ad-hoc parameter tuning were induced in its first half to estimate their maximum impactis shown in Fig. 8, becoming clear that the Intel R NUC is capable of running significantly more demanding algorithms even without turning to the accelerator mode of Simulink R ,  as expected. It is also worth noting that the variations due to the aforementioned additional elements in the first half of the experiment are perceptible. Nonetheless, considering the time ratio analysis in Fig. 4, in which significant computational power margins were detected, we can conclude that their impact on the reliability of the solution is negligible.

C. EV3: AUTONOMOUS NAVIGATION MODULE
Finally, a separate autonomous flight has been carried out in an outdoor scenario with wind disturbances and GPS inaccuracies including an additional navigation module depicted (see Fig. 9). The flight path, as depicted on the left side of Fig. 10, consists of an XY square followed by a vertical rectangle sharing one of its sides, being this manoeuvre chosen to show the response of the controller to changes in all directions. To highlight the changing speed during the flight, a set of time-equispaced points have been added to the position plot. It can be concluded that the solution is perfectly capable of tracking the proposed trajectory with a good overall performance. As shown by both path tracking and position subplots, the most important error peaks correspond to the regions around the waypoints, as expected, being the displayed overshoots at their typical range in aerial robotics. VOLUME 8, 2020

IV. CONCLUSION
In the present work, a high-level modular autopilot solution for fast prototyping of custom algorithms in UAVs is presented and demonstrated in an experimental setting. The approach is proven to be safe and reliable in terms of communication, integration and execution of new algorithms due to its inherent redundancy. Moreover, the proposed solution provides a user-friendly modular environment in the well-known Simulink R software, making the theory-toimplementation step easier and faster, and facilitating the interchange and external validation of custom algorithms. Finally, its feasibility for computation-intensive methods is demonstrated by the implementation of a high-order nonlinear controller, thus overcoming the prototyping issues generally arisen when applying complex algorithms on aerial robotics.
Future work is underway to include additional modules like estimator and mixer for other multi-rotor setups. All these implementations and modules will be available in a shared open repository, 9 thus facilitating the aforementioned interchange and validation of custom functionalities.

APPENDIX A Simulink R RUNNING MODES
Since the need for compilation in Simulink R is not evident, in here we include a disambiguation of the difference between compilation and interpretation in this graphical programming environment. According to the available technical documentation, the model is interpreted in Normal mode during each simulation run (see 10-11 in [50] and [51]), thus being this mode preferable when frequent changes of the model are needed. Moreover, including only Interpreted Function blocks -without using Math-Function ones-forces the execution engine to be called at each simulation time step, mimicking interpreters. Therefore, 9 https://github.com/grvcTeam/MASp some kind of compilation -in the sense of Simulink R , i.e. the translation of the block diagram to an internal representation that interacts with the Simulink R engine (see 11-26 in [50])-is made in most applications, but it can never be comparable to a full standalone solution with an external compiler.
In what respects to the Accelerator mode [52], Simulink R uses by default the Just-In-Time (JIT) compilation. This generates an execution engine in memory for the top-level model only, and not for referenced models. As a result, a C compiler is not required during simulations. This is related to another helpful prototyping tool for the interpreter/compiler, the Model Reference. Wherever this Model-Reference block is used, the simulation mode of a sub-system can be controlled -i.e. by setting it to Accelerator mode, it is simulated through code generation; and by setting it to Normal mode this is done in interpreted mode-. Additionally, Accelerator mode does not support most runtime diagnostics of Normal mode, which is also very important during prototyping.
Finally, in Rapid Accelerator mode, Simulink R creates a standalone executable including both the solver and the models interacting with Simulink R via the external mode, with the subsequent lack of debugging options (see [53]).

APPENDIX B PX4-Simulink R UDP COMMUNICATIONS
As mentioned in the Software Integration subsection, the proposed solution is based on the UDP communications between PX4 and a Simulink R environment. This is implemented via two sockets, one after the ecl/EKF estimation and another one just before the input to the motor mixer. The core of their respective pseudocodes is shown in Algorithms 1 and 2.
Additionally, for those interested in the integration between the basic PX4 functionalities and the customisable MAS PX4 environment, we refer readers to our GitHub repository at https://github.com/grvcTeam/MASp, which is currently under construction. Apart from the code support, a wiki of the Simulink R environment and the space to share custom modules with the MAS PX4 standard I/O connections will be included.