# Writing a Roll/Pitch Controller with cFS The main idea in this section would be to wrap up our adventure analyzing and modifying JSBSim and cFS by developing a bit more than just command interpreters and telemetry senders. The plan would be to try to control some aspects of the time evolution of our airliner by finding a way of holding certain parameters of flight under control, and all is done from our little sample application in cFS. Automatic flight control is a complex matter, and a rich body of knowledge exists on the topic. There are myriads of books written about the topic, notably "Automatic Control of Atmospheric and Space Flight Vehicle, Design and Analysis with MATLAB and Simulink" by Ashish Tewari (published by Birkhäuser), or "Flight Dynamics, Simulation and Control for Rigid and Flexible Aircraft" by Ranjan Vepa (CRC Press). There are many, many more, and I would recommend always using bibliography that comes with some examples in the code at hand, be it MATLAB, Simulink, Python, or similar, besides the math. It is very useful to see code executing equations which may appear a bit scary when seen written on a book page. What is more, different books use different notations in equations, which does not really help. In code, integrals and derivatives boil down to additions and subtractions. Additionally, if you want proper exposure to physics engine development from scratch (in this case, for video games) I strongly recommend "Physics for Game Developers" by David M. Bourg and Bryan Bywalec (O'Reilly). I happen to have a space background, and I've been involved in developing and testing software for 3-axis attitude control of spacecraft of various kinds. I must admit, this background has been giving me some mental friction when thinking of controlling an aircraft. The set of sensors and actuators used for orienting a satellite in space is rather different compared to the ones used for aircraft. What is more, although the underlying equations of motion are similar—at the end of the day, a satellite and an aircraft are bodies subject to forces and torques translating and rotating with respect to arbitrary frames of references—there are effects and physical couplings that happen in aircraft and do not happen in spacecraft. From by somewhat biased perspective, it feels like spacecraft control could be a tad more tractable than aircraft control. But I do not intend to spark any polemic here, so I will leave it to the reader to compare the two (Tewari's book covers both topics, which is a great way of comparing the particularities of both worlds). Now, because of my space background, I will employ quaternions for this experiment. Quaternions are widely adopted in space and video game development because of their versatility and numerical stability when using them on computers. For controlling actual aircraft, it is more frequent to see control methods using Euler angles and their rates. Why? In spacecraft, sensors tend to directly provide quaternions, for example, star trackers. In aircraft, the sensor suite typically used provides angles, like phi (roll), theta (pitch), and psi (yaw). What are these angles? The phi angle, also known as the roll angle, represents the angle between the aircraft's longitudinal axis and the lateral axis. Aircraft can sense their roll angle using several instruments and sensors: - Attitude indicator: The attitude indicator, also known as an artificial horizon, can display the aircraft's roll angle. As the aircraft rolls, the gyro in the attitude indicator precesses, and the instrument displays the roll angle. - Inertial measurement units (IMUs): An IMU is a sensor package that contains gyroscopes and accelerometers. The gyroscopes in the IMU can detect changes in the aircraft's roll angle, and the data from the IMU can be used to calculate the roll angle. - GPS: The Global Positioning System (GPS) can also be used to determine the aircraft's roll angle. The GPS can provide information about the aircraft's heading, and changes in the heading can be used to calculate the roll angle. - Magnetometers: Magnetometers can be used to detect changes in the aircraft's heading, which can be used to calculate the roll angle. Magnetometers may suffer from a variety of issues. For instance, magnetometers can be affected by magnetic interference from other sources, such as electrical wiring or metallic objects and surfaces. This interference can cause inaccurate readings and affect the performance of the aircraft's navigation system. Magnetometers can be sensitive to changes in temperature, which can cause them to drift and give inaccurate readings. Additionally, magnetometers need to be calibrated periodically to ensure accurate readings. This calibration process can be time-consuming and may require specialized equipment. Magnetometers have a limited range of operation and may not be able to detect magnetic fields at a distance and also have a limited accuracy and resolution compared to other sensors like gyroscopes or accelerometers, which can affect the accuracy of the aircraft's navigation and orientation. The autopilot system can adjust the aircraft's ailerons and other control surfaces to maintain the desired roll angle. The theta angle, or pitch angle, represents the angle between the aircraft's longitudinal axis and the horizon. Aircraft can sense their pitch angle using several instruments and sensors: - Attitude indicator: Like the roll angle, the attitude indicator can also display the aircraft's pitch angle. As the aircraft pitches up or down, the gyro in the attitude indicator precesses, and the instrument displays the pitch angle. - Air data sensors: Air data sensors, such as pitot tubes and static ports, can provide information about the aircraft's angle of attack, which is the angle between the aircraft's wing and the oncoming airflow. The angle of attack is related to the pitch angle, and the data from the air data sensors can be used to calculate the pitch angle.  >[!Figure] > _Angle of attack (AoA) sensor_ - Accelerometers: Accelerometers can also detect changes in the aircraft's pitch angle. An accelerometer measures changes in the aircraft's acceleration, and these measurements can be used to calculate the pitch angle. - Inertial measurement units (IMUs): An IMU is a sensor package that contains accelerometers and gyroscopes. The data from the IMU can be used to calculate the aircraft's pitch angle. As with the roll angle, the aircraft's autopilot system can use information from the above sensors to maintain a desired pitch angle during flight. The autopilot system can adjust the aircraft's elevator control surfaces to maintain the desired pitch angle. Finally, the psi angle, or yaw angle, represents the angle between the aircraft's longitudinal axis and the vertical axis. Aircraft can sense their yaw angle using several instruments and sensors: - Inertial measurement units (IMUs): The gyroscopes in the IMU can detect changes in the aircraft's yaw angle, and the data from the IMU can be used to calculate the yaw angle. - Magnetometers: Magnetometers can be used to determine the aircraft's heading, which is related to the yaw angle. Changes in the heading can be used to calculate the yaw angle. - GPS: The Global Positioning System can also provide information about the aircraft's heading, which can be used to calculate the yaw angle. - Air data sensors: Air data sensors can provide information about the aircraft's sideslip angle, which is related to the yaw angle. The data from the air data sensors can be used to calculate the yaw angle. The autopilot system can adjust the aircraft's rudder and other control surfaces to maintain the desired yaw angle. ## Seeking stabilized pitch Ok, now we embark on the task of trying to stabilize some of these angles. How do we do that? To achieve our goal, we will create some code to: 1. Apply full throttle so we have plenty of power while we experiment with automatic flight 2. Compute and apply the right amount of torque in the right flight surface to keep the flight under some control. We add the new code right after the housekeeping reporting function:  As the snippet above depicts, we have created a new function called `controlLoop()`, whose execution is conditional on the status of a variable called AP_Status in the already familiar `SAMPLE_APP_Data`. Now what does this controlLoop() function do? Here's the code:  And we compute the necessary torque to stabilize the aircraft in the following manner:  All code shown above transpires as quite straightforward: First, we compute the difference between the desired quaternion (`qdLocal`) and the actual instantaneous local quaternion[^34] of the aircraft, then we compute the derivative of this error quaternion (`qErrorDot`), and then, we compute the necessary torque using proportional and derivative (PD) control. But first, we need to obtain the local quaternion from JSBSim, and we do it the usual way, with properties:  And, of course, we need to create the new function codes to be able to switch our autopilot ON or OFF so we can play with it:  ## Flight Control Channels We must not forget to create the necessary FCS channels in JSBSim to be able to command flight surfaces. We do that in the aircraft file, this way;  And now we can start putting all the pieces together. ## A Poor Man's Fly-By-Wire With this basic code in place, we can roll out our experiment on stabilizing a simulated Boeing 737 by using the X and Y components of the torque computed in the section above and applying that torque for roll and pitch control, this means, controlling the elevator and the ailerons of the aircraft. How do we do what? Let's start with pitch, by just setting the normalized elevator position property directly with the number we got from the torque computation:  And we give it a go: we start our flight at 30000ft, throttle to zero, let our untrimmed aircraft plummet, and after a while, we command our rudimentary pitch control ON with these next commands to cFS: First, as usual, we must enable the telemetry sending from TO_LAB_APP with this simple Space Packet (command code 6): ```Console ./cmdUtil --endian=LE --host=localhost --port=1234 --pktid=0x1880 --pktfc=6 ``` Next, we flip the switch of our pitch stabilizing prototype with this Space Packet targeted to our sample application: ```Console ./cmdUtil --endian=LE --host=localhost --port=1234 --pktid=0x1882 --pktfc=5 ``` How does it look?  Not bad! In fact, after losing half of its altitude very rapidly and steeply with no control and now thrust, the aircraft stopped falling, recovered altitude climbed back, and stabilized, or kind of. Yes, there are overshoots and oscillations, and yes, there is a non-zero pitch angle which is making the aircraft continue climbing after stabilization. But these two effects are kind of expected: overshoots are a product of our very rudimentary trial-and-error method to choose proportional and derivative gains, and the offset is a typical artifact in proportional-derivative controllers. We must add an integral term to handle that pitch build-up, but we will leave it for now; after all, this is not a text about control systems design, but about software, and with this we have already proved our point. Adding more telemetry points, including the Euler angles, the plots look like this:  It is very illustrative in the plot above to observe how the climb direction changes as the theta angle crosses zero: aircraft climbs when theta positive and climbs down when theta negative. Expected, yes, still nice to see it in a plot. But wait, we are not only deflecting the elevator in our basic autopilot, but at the same time, we are applying a lot of thrust because we are throttling the engines to the maximum. Is it possible we are fooling ourselves into believing we are controlling the pitch where it is the push of the engines stabilizing things around? Let's run it with engines at full power but without any pitch control:  It looks different. Although the full thrust is indeed acting on the pitch (you can see theta angle stabilizing around zero), the other orientation angles of the aircraft are behaving quite differently. The aircraft is yawing considerably. Our humble elevator driving routing is doing some job keeping the vehicle stabilized. ### Wing Leveling Why not add some wing leveling then to get a nicer flight? We need to make sure we are deflecting the ailerons accordingly (a positive deflection of an aileron in a wing will decrease lift on said wing). Let's try to level the roll angle by only using the right aileron:  The reason why the yaw angle (which we are not controlling) is drifting is nothing else than adverse yaw. Adverse yaw is the natural (and undesirable) tendency for an aircraft to yaw in the opposite direction of a roll. It is caused by the difference in lift and drag of each wing. The effect can be minimized with ailerons deliberately designed to create drag when deflected upward and/or mechanisms that automatically apply some amount of coordinated rudder. Let's now add the second aileron and see if things improve a bit:  Things look different, for sure. Adding the second aileron (and remember they must work in opposite, coordinated manner such as one going positive while the other going negative, and vice versa) makes the roll axis now behave differently, although it is now showing some oscillations which take quite a long time to be damped. Let's increase a bit the initial roll angle (it was 10 degrees) to 40 degrees and stress a bit our humble wing leveler:  The fluctuations in pitch can be observed in the distance in our STK visualization:  (Bumpy road: oscillations can have peaks of 10000ft! Some tuning wouldn't hurt) We run one final test for our roll/pitch stabilizer, by adding a bit spicier initial conditions in the cruise_init.xml file of our aircraft:  Quite hectic: 80 degrees roll, -20 degrees pitch (nose down). Hell of a ride. Can our humble controller stabilize our airliner? Let's give it a try:  After a while, the aircraft levels up, and clears the aircraft from disaster:   Pitch oscillations are visible in the distance. It's a low-cost airline, did you expect any comfort?  Pitch oscillations are (slowly) being damped. ## Commanding Some Roll Angle Finally, we can add some capability to command a roll angle. We will do the usual: first, create a new command code:  Then, we define a new command:  And then, we add the new command code in the switch case:  Finally, we must write the code for `SAMPLE_APP_SetAngle()`:  As you can see, we are taking an angle as a parameter, but then we craft our own quaternions for control:  And now, we use cmdUtil to test this, but issuing a command to roll the aircraft by 5 degrees, and then back to 0 degrees. How does it look? It is wobbly, yes, but still follows our commands You can see in the graph, that despite the instabilities due to poor gain selection, both commands act on the phi angle. Neat. ## Summary In summary, what we did in this part was to rapidly prototype some code to interact with the dynamics of the system (the simulated aircraft) and apply torques in the body to keep certain variables of interest within boundaries. Some stabilization was achieved, not without a dose of overshoots due to the basic method of choosing gains. Digital control system is a complex matter, and out of the scope of this section. Proper resources on the matter are provided for you to dive deeper and understand how to design a proper, stable autopilot. Here, all we wanted was to show that we could interact with the dynamics of our aircraft from flight software, all controlled by Space Packets sent from a ground operator. # Conclusion A lot happened on this page. We fetched two projects that grew unaware of each other, and we made them work together. The process was not simple: it required going through tens if not hundreds of errors and warnings. We had to configure our projects from scratch, identifying dependencies and their building blocks in the process. We made both projects compile again (after scrapping their build systems), and then we modified them to start talking to each other. In the process, we understood that we cannot magically read physical variables from our simulated "universes", so we created somewhat more realistic, noisy, sensors. We learned what Space Packets are, and after that, we crafted our own commands formatted in that standard. We devised rudimentary ways of sending commands and visualizing the telemetry that we also managed to define and send to the ground observer. We included some more human-friendly visualization in the process, and, finally, we ventured ourselves into writing code to control a simulated vehicle as it flew out of control. If we continued this work further, we would be ready to start adding real embedded hardware in the loop running our application in cFS in real time, all connected to JSBSim by means of some data interface understood both by the computer running the FSM and the embedded system (Ethernet being a solid candidate), plus analog inputs and outputs. In conclusion, we only scratched the surface. Real life control of aircraft includes an insane amount of complexity we purposely ignored: hydraulics, servos, flexible airframes, gas turbines, fueling systems, noisy networks and imperfect sensor and actuators full of delays, artifacts, and dead bands, along with the strictest safety regulations you can find. Equivalently, the simulation environments used by aircraft and avionics manufacturers may take full hangars to fit in, and may include electrical systems, hydraulics, and actual flight controls. These massive test rigs incorporate major working components installed in the relative locations found on the actual airframe, brought together in a test installation comprising tons of scaffolding arranged in the same skeletal shape of the aircraft being verified. In the years leading up to a new aircraft's first flight, changes made during the development phase are tested and validated using this valuable although incredibly complex tool. But wait, aren't we in the era of digital twins? How come we have simulators taking full hangars? As much as so-called 'digital twins' are on the hype, adding real hardware in the loop brings invaluable insight. And here's a dilemma: eventually, our simulation and verification environments become as complex as the thing they are trying to replicate, and when that happens, they automatically lose their purpose. When we model a complex system like an aircraft, we want to create a simplified representation of it. Modeling allows us to gain understanding we surely lack, and in some cases to predict future behavior. While modeling, we go from coarse to fine, and we accept that we are minding only certain macro-level functionalities and purposely not minding the reasons why such macro function emerges: we just take it for granted, we just accept the behavior of a block magically appears. Why? Optimization: more agency requires more resources allocated to compute each agent's evolution. Mostly we're referring to computing resources here, but also time and complexity: the more details we add to our models, the longer it takes to create the model, and the more complex the model gets, which invalidates the whole modeling effort: the "usefulness" of a model decreases as its fidelity increases, since it becomes as difficult to understand and operate as the actual system, even though it cannot execute the global function of the real system in the real environment. As the saying goes: all models are wrong, but some are useful. [^1]: https://jsbsim.sourceforge.net/ [^2]: https://cfs.gsfc.nasa.gov/ [^3]: Mind that counting lines of code has never been an incredibly accurate method and it may only represent a rough approximation of code complexity. [^4]: UML stands for Unified Modeling Language, and it is a graphical way of describing software structure and behavior. [^5]: According to a 2011 report by Boeing, the 787 Dreamliner contains over 6.5 million lines of code. This code is spread across various systems, including the flight control system, avionics system, and passenger entertainment system. This number has likely increased. [^6]: Nedhal A. Al-Saiyd "Source Code Comprehension Analysis in Software Maintenance", Computer Science Department, Faculty of Information Technology Applied Science Private University, Amman-Jordan [^7]: M. A. Storey "Theories, Methods and Tools in Program Comprehension: Past, Present and Future", Software Quality Journal 14, pp. 187--208, DOI 10.1007/s11219-006-9216-4. [^8]: Adapted from "Peopleware, Third Edition" by Tom De Marco and Timothy Lister, Addison-Wesley [^9]: Summarized from https://www.gnu.org/software/automake/manual/html_node/GNU-Build-System.html [^10]: Adapted from: https://mesonbuild.com/Tutorial.html [^11]: Adapted from https://ninja-build.org/manual.html [^12]: This is just meant to be a brief introduction. For a deeper dive, see for example *Automotive Software Architectures*, by Miroslaw Staron (Springer, 2017) [^13]: https://en.wikipedia.org/wiki/Occam%27s_razor [^14]: https://en.wikipedia.org/wiki/Larry_Tesler [^15]: Saffer, D. (2009). Designing for interaction: Creating innovative applications and devices. New Riders. [^16]: JSBSim is licensed under the terms of the GNU Lesser GPL (LGPL) [^17]: https://en.wikipedia.org/wiki/RTFM [^18]: https://agilemanifesto.org/ [^19]: https://jsbsim.sourceforge.net/JSBSimReferenceManual.pdf [^20]: https://en.wikipedia.org/wiki/Euler_method [^21]: https://en.wikipedia.org/wiki/Trapezoidal_rule [^22]: https://en.wikipedia.org/wiki/Linear_multistep_method#Families_of_multistep_methods> [^23]: https://ntrs.nasa.gov/api/citations/19770009539/downloads/19770009539.pdf [^24]: https://ntrs.nasa.gov/api/citations/19980028448/downloads/19980028448.pdf [^25]: RTEMS is a Real-Time Operating System (RTOS) popular in mission-critical applications [^26]: https://public.ccsds.org/Pubs/133x0b2e1.pdf [^27]: https://ecss.nl/standard/ecss-e-st-70-41c-space-engineering-telemetry-and-telecommand-packet-utilization-15-april-2016/ [^28]: https://dash.plotly.com/ [^29]: Can you spot the error? Yes, there is a bug with the velocity vector being copied twice, this bug is fixed down below [^30]: https://cesium.com/ [^31]: https://www.flightgear.org/ [^32]: https://help.agi.com/stk/Subsystems/connect/connect.htm [^33]: https://public.ccsds.org/Pubs/133x0b2e1.pdf [^34]: Mind I use the JPL notation of quaternions (the scalar element at the end)