January 15, 2018 at 17:15

Autopilot on AT91SAM7

    1. Introduction


    I got carried away with radio-controlled aircraft back in 2002, living in Prague. The first plane was with a wingspan of 1.2 m, a 2-stroke engine ICE. Here is a fragment of the video of those times. Once in 2008, the ADXL202E accelerometer fell into my hands. Having experience developing devices on the MCS-51 family controller (it was a Rubidium frequency standard and a data acquisition system), I decided to make an autopilot for the Mentor radio-controlled model. First of all, I connected the accelerometer to the controller. For simplicity, I chose a debug board with an AT91SAM7 controller, for the convenience and simplicity of working with it. It should be noted that I decided not to learn anything about autopilots on the Internet, but to do everything on my own with "0" to make it more interesting.



    So, by chance in 2008, I happened to fly on an airplane and be able to use a laptop and my handkerchief with an accelerometer on board. Today, any owner of a
    tablet or smartphone can conduct such an experiment on their own, since
    accelerometers are available in almost all devices. It was then that I made the first “discovery” for myself that you can use the accelerometer to stabilize the pitch (because the plane cannot gain speed or slow down too quickly), but it is not suitable for stabilizing the roll. Since with a correct turn without slipping, lateral accelerations are compensated by the roll of the aircraft, and the accelerometer does not feel them. I remember the movie Yolki-2, where the roll of the plane was tracked by a glass of water, which will be perplexing for a real pilot. Why then invent a horizon?
    So, in these articles I will describe what I got from 2008 to 2017, when I was doing this project in my free time. In June 2012, the autopilot was tested 1 time really. Then still without GPS. In 2017, several tests were performed already with GPS. One of the successful, in slow motion, but with no time to find your position on GPS can be seen here . The start is to turn off the remote. Take-off is completely automatic, then an attempt to take control, when it became clear that the plane was flying the wrong way. It was found that GPS determined the coordinates of 1 km approximately from the present. Another successful flight on the route 10/17/2017. The shooting is unsuccessful, but takeoff and landing is visible. Landing in the field due to battery discharge. However, beautiful. The entire flight is fully automatic. Development began with the development of the autopilot software model using the Borland C ++ Builder and XPlane 6 flight simulator. As a result, the program code was adapted for the controller almost unchanged. The autopilot is focused on simple docking with standard radio control modules. As the most suitable mode was selected PPM (Pulse Position Modulation). Not even PPM can work with all modern remotes today: it is required that the PPM pulse sequence should run sequentially, and not start at the same time as in some modern remotes, such as FlySky FS-16. Although a control solution from such remotes is also present in the software, but for today it is not tested and needs debugging. The system has been tested on Robbie Futaba FC-16 and Art-Tech EFLY-100B remotes. One of the drawbacks is the need to directly output the PPM signal from the bowels of the receiver to the decoder, the pulse distributor for steering machines.


    2. Hardware-software model


    The functional diagram of the autopilot is shown in Fig. 1. The heart of the autopilot is the LEVEL1 unit. This module is a high-priority function and runs independently of other events every (15.625 ms tested version) 46.875 ms. Its task is to keep the aircraft in the correct flight position



    Fig. 1
    The input for the module is the FSSi structure, which contains the parameters Roll, Pitch, and Air Speed. The correct position is considered in the absence of signals Ref roll = 0 and pitch = 0. Also, this module controls the airplane engine controller and maintains the speed received from the LEVEL2 unit in accordance with the flight task. The Level2 module perturbs the LEVEL1 module and thus forces it to hold not the zero roll and pitch values, but calculated in accordance with the flight mission. The speed parameter is directly transmitted from the flight task. The output of the module is a data structure that contains the current position of the ailerons, elevator, rudder and rudder (Throttle). This structure in the program is called FSSo. The module is named FltStSys (flight stabilization system). It can be applied to the PWMC (Pulse Wide Modulation Controller) if autopilot mode is activated. Otherwise, a structure filled with a timer in accordance with the measured values ​​received from the HF receiver is fed to the PWMC. This control mode is traditional for radio-controlled models.
    The Level2 module works more slowly, once a second it generates control actions - displaces zero points - for the Level1 module. Data transferred to Level1 is marked with the word keep in the program. Those. this is what the Level1 module should hold. This module performs all other functions:
    1) implementation of the take-off mode.
    2) Retention of altitude
    3) Retention of course
    4) Calculation of the route and conducting the aircraft along the route specified on the SD card.
    5) Record flight parameters to the SD card once per second. (Black box function)
    6) When connected to one of the COM ports of the bluetooth module, the issuance of telemetry information.
    The main idea of ​​building a LEVEL1 autopilot is to build a completely inertial aircraft piloting system that does not use external signals, such as photosensors of the color temperature of heaven and earth, or for example NAVSTAR or GLONASS to keep the aircraft in normal flight position. When the GPS signal disappears, the flight is performed using the magnetic compass. The following sensors are used to meet these requirements:


    1. Roll. The roll value is calculated from the value of the angular velocity of the aircraft’s turn and its linear speed under the assumption that the turn takes place without slipping. The analog signal from the GYRO angular velocity sensor is digitized built-in to the ADC controller (ADC1) and then the Calc module calculates the roll.
    2. Pitch. The pitch is obtained by the controller by measuring the pulse duration from the accelerometer sensor. This is permissible to do since, as a rule, too quickly a change in speed (except for the take-off mode) in aircraft usually does not happen.
    3. Altitude An absolute pressure sensor is used to measure the height. The data is obtained in the form of voltage, which is digitized by one of the ADC2 channels of the controller.
    4. Course To obtain the heading value, either a GPS module is used, or, when it is unavailable or at zero speed (heading control is started before), a Honeywell digital compass with Hall sensors. Connection to the controller via I2C interface.
    5. Speed Air velocity sensor on a pitot tube. A relative pressure sensor is used. The voltage from the sensor is measured by one of the ADC3 channels of the controller. Also, for some things, the speed value from the GPS module is used.
    6. GPS GPS module is connected via com port. It is used to obtain route data and speed relative to the ground (GND speed).
    7. SD card. It is not a sensor, however, module settings data (coefficients for formulas) and route data are read from it. It records flight data once a second.

    3. Sensors and modules


    3.1 Accelerometer


    The sensor is designed to measure linear acceleration. As an accelerometer, the ADXL202E (Analog Devices) device was used. This is a two-axis accelerometer with an output in the form of pulses whose duration is proportional to acceleration. The pulse width is measured by a timer. Only one of the axes (X) is used to measure the tilt of the aircraft. The Y axis can be used to calculate lateral acceleration and its compensation by trimming the rudder position. Now it is not implemented. This sensor shows the position of the "ball" in the corresponding sensor of angular velocity and sliding of the aircraft (for example, Ya-18T, EUP-53) During a turn (with a turn for pilots we mean any change in the course of the aircraft), the pilot usually pedals with the pedals and keeps the ball in a central position to reduce slipping. The accelerometer drift when measuring the slope is shown in Fig. 2. As you can see, when placed in the refrigerator for 15 minutes, the drift was -5 degrees.



    Figure 2


    3.2 Gyroscope


    A key element of the panel of any aircraft is the horizon indicator. It is usually built on the basis of a mechanical gyroscope. It shows the roll and pitch of the airplane in instrument flight mode. This project uses the MEV-50A semiconductor gyroscope to measure aircraft roll. However, unlike the classical gyroscope, semiconductor gyroscopes measure not the absolute roll, but its derivative - the angular velocity. This is the main reason why you can not use a semiconductor gyroscope to measure the roll of the aircraft. The drift of such a measurement system is not defined at small angular velocities. If, for example, in a VR helmet (virtual reality), in which the gyroscope is not based on a magnetic compass with Hall sensors, but on a semiconductor device, you try to turn your head very slowly, you will find that he "does not notice" turning his head. So I applied another way to calculate the roll. The gyroscope is installed to measure the rotation speed ω of the aircraft around a vertical axis. Then the roll of the aircraft can be calculated by the formula Θ = arctan (V ^ 2 / Rg) = arctan (Vω / g). See fig. 3.



    Here R is the radius of the aircraft’s turn, g is the acceleration of gravity (9.8 m / s2), V is the velocity relative to the ground (true airspeed), ω is the angular velocity, rad / s. The true speed is given by GPS, or it needs to be calculated based on the course, speed and direction of the wind. This project uses GPS speed. To exclude multiplication by 0, the minimum speed for calculation is limited from below 10 km / h. The gyroscope installed in this way has very serious requirements for measurement accuracy. So, the first gyroscope that was tested led to the loss of the aircraft. You can watch the video of the flight here , the data of the "black box"also available. The plane would be raised from the remote control to a height of about 150m and the autopilot would be turned on. However, as can be seen from the graphs, from the very beginning the gyroscope was giving incorrect data - Gyr in the picture - and as soon as the autopilot was turned on at 97 seconds of the flight, the aircraft expectedly went into a tailspin. Unfortunately, it was not possible to safely get it out because of an ergonomic error: manual control was turned on from the remote control with the toggle switch in the middle position, which could not be found on time from the excitement :). It was a gyroscope from a simple coaxial helicopter. In general, it was clear to me from the very beginning that it would be so, since temperature tests in the refrigerator showed a huge drift of this device. The departure in 15 minutes was about 15g / s, although the normal operating values ​​of this parameter usually do not exceed several degrees per second in the entire range of banks (up to 30 grams).
    You can look at the test data and the measurement process here. A comparison of the characteristics of the gyroscopes is shown in Fig. 5. It can be seen that, in general, the normal flight could not be expected from a noname gyroscope if it drifted in 15 minutes. in the freezer was approximately 24 ° / s. Now the gyroscope is MEV-50A . It can be seen that its drift in 25 minutes is about 0.6 ° / s.




    Fig.5


    3.3 Altimeter


    As an altimeter, an absolute pressure sensor is usually used. In this project, the MPXA4115A6U sensor is used . The sensor provides a voltage that is measured by the controller. Before flying it must be warmed up. Warming up time reaches 10-15 minutes. As atmospheric pressure changes, the altimeter is calibrated automatically before each flight. The sensor has a significant temperature drift. Figure 6 shows the drift graph of the altimeter. Since the speed sensor is also a pressure meter, the drift is the same.



    Fig. 6
    The most interesting test of a pressure sensor is undoubtedly a trip on the subway. The trip black box data is shown in Figure 7. About this graph, we can say that the autopilot is turned on on the 12th floor, (about 36 m). Next, descend by elevator, then I took the bus, and then the subway. The end point of my journey, when I got off the subway, is really on a hill.



    Fig. 7


    3.4 Airspeed sensor.


    Airspeed is usually measured using a pitot tube. In principle, these things are usually calculated. But it is clear that if you blow into the tube from the outside (without touching it with your lips), then the pressure in it will increase. By measuring the pressure in the tube, you can estimate the speed of flight. The tube is usually placed either on the wing or on the fuselage (if there is no pulling screw). For proper measurement, it is necessary that the engine does not blow air directly into the tube. Being a pioneer in this business, I applied the MPXV5004GP relative pressure sensor. Like the absolute pressure sensor, the output of this sensor is voltage; the controller measures it. At startup, it is also calibrated. But a coarse adjustment with a resistor is not required if it is not possible to reset the height, unlike the absolute pressure sensor. The tube was taken from carbon fiber with a diameter of 3 mm. See fig. 8.



    Fig. 8
    To calibrate the sensor, the handset was installed on the rear-view mirror of the car, and a check-in was made, the current speed was recorded on the recorder. If you combine the audio and the log, you can calibrate the airspeed sensor. So I "discovered" for myself that speed is proportional to the square root of pressure. V = k * √p In the future, the correctness of the speed measurement was confirmed during test flights with GPS. The temperature drift of the airspeed sensor can be seen in Fig. 6. A scientific discovery has just been made. During acceleration, the air is pumped out of the passenger compartment, the pressure drops, which corresponds to an increase in height? Is it so? Correlation with height cannot be accidental ...


    3.5 Digital Compass


    The digital compass allows you to determine the direction of flight in the absence of a signal or GPS receiver. Initially, the system was built as an inertial one. The flight program was not recorded in the form of GPS coordinates, but the direction, speed and flight time to the next point were set. A 2-axis compass HMC6352 was used as a compass. If the developer were smarter then he would certainly use a 3-axis compass. But so far it has been done. The compass has an I2C interface. This device can give out directly a course, or levels of components of a magnetic field. To increase accuracy, it is sometimes necessary, or when changing the external magnetic environment, to calibrate it. For calibration, it is necessary to hold the calibration button when the autopilot is turned on, the field of which the controller enters calibration mode for 1 minute. The calibration mode is indicated by a special flashing of the operating mode indicator (J9, BLUE-LED, 4 times per second). At this time, it is necessary to make several complete turns of the aircraft in a strictly horizontal position of the aircraft. In order to be able to use the sensor, magnetic materials cannot be used for the autopilot housing. Unfortunately, the first experiments showed that the measurement accuracy is good only in the case of a strictly horizontal position of the device. On an airplane, this requirement cannot be fulfilled. Moreover, the accuracy does not just fall, but it does not exist at all. When tilted even at a small angle, a few degrees we get at the output all + -180 °. Therefore, I had to refuse to get the course directly from the compass, but to get the values ​​of the magnetic field components, and calculate the course using compensation as much as possible. In the absence of a third Z axis, we had to take the value of this component as a constant. The result of the compass with and without compensation is shown in Fig. 9. HDGn - uncompensated compass, HDGc - compensated. As can be seen from the graphs, compensation significantly improves the quality of course measurements, however, of course, we are not talking about accuracy in tenths of a degree.Flight data x without GPS 11.09.2012. Here the flight took place in Vohonovo . It should be noted that the flights in 2009 . there was no compass compensation yet. As you can see in the figure, the start really takes place with a course close to 120 degrees, and ends in the forest with a course of about 300 degrees. As shown by later flights, a strong rocking of the aircraft occurred (yes, yes! :)) due to poor attachment of the wing to the fuselage and a decrease in flight speed after turning off the engine.



    Fig. 9.


    3.6 GPS module


    A GPS module with a built-in antenna is used. This is a small shawl located outside the shielded autopilot box. (See Fig. 8). The documentation for the module can be found in the GPS folder. The S3126 module is applied . The GPS module is connected to the controller through one of the UART. Communication with the module is carried out by the standard protocol NMEA0183. In test 2017_09_02, before launching the aircraft, the GPS did not work correctly, and the plane flew very nicely to the wrong place. As you can see from the data of the "black box" TRACK005.LOG GPS "thought" that he was a kilometer from a given point. This can be seen in the flight data from 2017_09_02. It should be noted that in this test I still could not control the distance to the next flight point while the remote control was turned on (i.e. the autopilot was turned off). Therefore, a distance of 134 M in the log file did not mean that the place of the aircraft was determined correctly. The distance to the next point was supposed to be about 100 m, the offset was about 0, and the course was 323. However, as you can see, real data appeared only at the time of takeoff at 14:10:33. And the course turned out to be 150, distance 1012M, offset 121M (deviation from the axis along which the plane should fly in accordance with the assignment). After this test, a change was made to the program so that route data was displayed in telemetry regardless of mode. It became possible to see before starting whether GPS is working correctly. This control is also reflected in the launch map.. The autopilot receives the following parameters from the GPS module: time, date (used when recording a log file), availability, number of visible satellites, coordinates longitude, latitude and speed in knots. GPS readiness is indicated by a green LED blinking at the moment of recording information on the card, about 1 time per second. It is important to understand that the GPS module cannot determine the height as accurately as the coordinates, so the height is not used. When you turn on the autopilot, immediately after warming up and calibrating the sensors, the controller calculates the route according to the coordinates of the flight task. This route is not recorded on the map, but displayed via BT telemetry. Based on these data, you can evaluate the correctness of flight data. The program now does not wait for the GPS to initialize, but this is easy to change. This is what the telemetry log looks like:
    GPILOT V2.17.0 24 / Jul / 2017
    Waiting GPS ...
    GPS ready ...
    Init from MMC card..Hidden sectors = 57
    G0,60.674983,29.173461,0,0,0,0,0,1
    G1,60.675504,29.172667,30M, 50kM / h, 323D, 5s, 2.72M G
    2.60.676388.29.171305.30M, 50kM / h, 322D, 8s, 3.123M
    G 3.60.675641.29, 68697.30M, 50kM / h, 239D, 11s, 4.164M
    G 4.60.672877.29.972249 , 30M, 50kM / h, 147D, 26s, 5.362M
    G5.60.673546,29.174448,30M, 50kM / h, 58D, 10s, 6,140M
    G6,60.674767,29.173783,20M, 30kM / h, 345D, 16s, 7,140M
    G7 , 60.675737,29.172313,20M, 0kM / h, 323D, 0s, 0,134M
    Compass calibrate skip ...
    Sensors calibrate ...
    Sensors calibrate complete ...


    3.7 Bluetooth module


    For convenience, control of parameters before takeoff and debugging in general, the output via UART was equipped with a parani-ESD1000 bluetooth module . To link with other devices, a button is installed. The most convenient application for Android devices was the Bluetooth Graphics application . Available on Google Play.


    3.8 SD card


    Like any self-respecting aircraft, my project has a black box on board, the role of which is played by an SD card. The card is connected via a slow SPI interface, for its connection I took a ready-made code from Rolf Freitag, for which I am grateful to him. The code provides reading and writing blocks of 512 bytes (Sector). Autopilot settings, flight route are recorded on the map. When working on the card once a second, data is written that the programmer wants to record. When I started working on autopilot, I had a 16 MB SD card at my disposal, so FAT12 was written first, and only this summer I added FAT32. To use it, you need to replace the file fat.c and fat.h in the project with fat32.c and fat32.h. As the SD card wears out, as practice has shown, the number of hidden sectors can increase. This is due to the deterioration of the SD card and the fact that the initial sectors are overwritten more often than others, especially in the FAT12 system. In the FAT32 system, the table is placed in arbitrary places on the map as a fragmented file, so wear is slower. Flight data is written to a log file of the form TRACKXXX.LOG automatically. Data for recording is written to the card immediately, without buffering, once a second, and the file table is updated immediately so that it is possible to have flight data up to the last second of flight. Thanks to GPS, each log file has a current date and time of creation.

    Tags:

    Also popular now: