The software implementation of the system is the most important part. It is responsible for the data collection and processing and also for all the decisions.
The following diagram describes the main software components of the system:
Basic Software Block Diagram
First of all we can recognize the software drivers for all hardware devices. This is the link layers between the software and the hardware. The basic device drivers are the FPGA driver that gives us the ability to communicate and exchange data with the FPGA of the system, the GPS drivers that collect data from the two GPSs receivers, the SERVO driver that gives us the ability to export the decisions of the system and finally the telemetry driver that transmits and receives data to and from the base station.
If we continue we can see two signal processing components, the first is process data from the sensors, which mainly removes the offsets of the sensors and performs a low pass filtering. The second controls the outputs to the SERVOs, this is a PID controller for the system outputs.
The INS and KALMAN software components are responsible for knowing the position of the system. The first takes all the sensor outputs and calculates the INS parameters (traveled distance, velocity, acceleration, heading information etc). The second takes the output from the previous component and the output from the GPS receivers and calculates the real position the vehicle.
The Navigation component is responsible for calculating the desired route and output to SERVO the appropriate commands in order to achieve it.
The two storage components are used in order to store the predefined route in the first and the traveling data in the second (used in order to improve the navigation algorithm).
A custom real-time monolithic non-preemptive kernel has been developed to support the proper functionality of the system. The main tasks that the kernel must handle are the following:
A. Process scheduling
B. Low level hardware support
C. Exception handling
D. System failure handling
E. Missing deadline handling
The major characteristics of the kernel are the following:
Priority scheduling: The system has many hard deadlines, for example the data from the acceleration sensors, if we lose any interrupt from the sensor then we will insert an error to the velocity (we will lose a sample in the integration needed in order to calculate the velocity) and this will cause an error for the estimation of the position. So, missing deadlines may be catastrophic for the performance and the functionality of the system.In order to protect system from missing hard deadlines we have created priorities for the various tasks. In the following table we can see some of the basic tasks of the system and each priority:
No Preemption: there is no preemption in the processes, this makes the system more simple, but extra care must be given in order to protect the system from deadlocks, in order to achieve this, processes must be as small as possible so that a high priority will not have to wait for a long time for execution while a low priority process is running. Also, extra care must be given to prevent the system from deadlock, so a sophisticated lock structure has been developed in order to achieve boundary wait, especially for the access to hardware, and also the structure of the task (especially the low priority tasks) guarantee that we will not have a low or medium priority task waiting for a resource while executing in the CPU (no spin locks).
Real Time: this means that the system has many deadlines (hard & soft). For example, a hard deadline case is the following: if an interrupt from the INS comes then the process that handles the new data from the sensors must finish the data collection and processing before the next interrupt from the INS comes. A soft deadline is the one from the GPR, if the GPS has a new position packet (usually this happens every one second) we can miss it if we have already enough packets so we are sure for the position and all the necessary corrections that have been made (of course this does not mean that we do not need the GPS data but under specific conditions if a soft deadline for the GPS is lost the only think that can happen is to have for one second an error of some centimeters in the position estimation).
Memory management: There are three main memory banks. One memory bank of 256KByte embedded in the processor that is used for saving system data, stack & heap and two external SRAM banks of 512KByte each, the first used for storing and running the software code and the other used as buffer basically for the hardware (storing data from and to GPS devices, telemetry data, and statistical information used for calibration system self test and history purposes).
Major Devices Drivers
The following component has been implemented in order to support the system hardware:
Flash Memory controller: provides functions such as Flash_Erase, that can erase a sector in the flash memory of the system Flash write, to write a sector, flash_lock & flash_unlock to control write access to the memory and more.
FPGA Controller: used for access to the various register of the FPGA logic.
V24 Driver: this driver provides the communication of the system with RS232 devices such as the GPS, Digital MODEM, Direct connection to Base Station.
I2C driver: a software implementation of the I2C 2 wire serial communication protocol that has been implemented in order to support communication with I2C compatible devices, such as the EEPROM device (24C64) used for storing general system parameter and variables.
The following low pass Butterworth (in order to have flat pass band) IIR filter has been implemented for each ADC channel. The order of the filter is four and we use two sets of coefficients, one for cut off frequency of 40Hz and one for 10Hz.
In the following graph we can see the magnitude response of the filter:
A software structure has been implemented in order to keep the global coefficients of the IIR filters. Each time new data arrive from the ADCs we call the function adc_filters() that calculate the output of the filter for the twelve channels of the ADCs
The main objective of the INS module is to collect the sampling data from the twelve analog to digital channels and then to process them in order to feed the Kalman filter with valid data. After the IIR filtering of the data the INS module can extract information about the vehicle position and state. Except the data form the pressure sensors and the compass, all the other outputs need to be integrated in order to give us useful information. So, extra care is needed in the data handling. Hard deadlines must be set in order to avoid loss of samples, also a Fast Interrupt used each time that new data arrive in order to collect them as fast as we can.
The data processed by the INS are the following:
3 Axis Acceleration: with the first integration we get the speed and with a second integration the traveled distance
3 Axis Gyroscope: we continuously integrate in order to have to real state of the vehicle:
3 Axis Compass: we just filter to get heading information of the vehicle (based on the earth magnetic field).
Altimeter Pressure Sensor: we filter and then we convert the output to Altimeter using the following formula:
Air Speed Pressure Sensor: we filtered the output and then we subtract the output of the previous sensor in order to convert to result to real Vehicle Air Speed using the following formula:
Vi = Velocity in i axis
ΔDi = Traveled Distance in i axis
Acci = Acceleration in i axis
Gyroi = Gyroscope output in i axis
pv = Airspeed pressure sensor
pa = Altimeter pressure sensor
ρ = air density at sea level (1.225 kg/)
The position and velocity of the vehicle can be determined, when navigating and guiding an autonomous vehicle, with the Global Positioning System (GPS). However, several errors are associated with the GPS measurement. It has superior long-term error performance, but poor short-term accuracy. For many vehicle navigation systems, GPS is insufficient as a stand-alone position system.
Basic Kalman Filter Configuration
Kalman filtering is a form of optimal estimation characterized by recursive evaluation, and an internal model of the dynamics of the system being estimated. The dynamic weighting of incoming evidence with ongoing expectation produces estimates of the state of the observed system. An extended Kalman filter (EKF) can be used to fuse measurements from GPS and INS. In this EKF, the INS data is used as a reference trajectory, and GPS data is applied to update and estimate the error states of this trajectory.
An important part of the whole implementation is the interface with the actuators. In our application we will use Servos used in radio-controlled modeling in order to link the system output with the real world. Servos is electromechanical devices that take as input a Pulse Modulation Waveform (PWM) add turn the motor to an angle that depends on the Duty Cycle of the PWM input. As we can see, perhaps it is the easier and the most accurate way to convert a digital signal to a mechanical action.
The PWM modulator (consisted of seven input and seven output channels) has been implemented in the FPGA and is memory mapped in to an address range of the ARM external memory map. So, no extra effort needed by the processor for the PWM conversion.
In order to make the navigation algorithm easier we have implemented a function in the device driver of the SERVO Ctrl that gives the ability to set any servo for specific time to a specific angle for specific time and then, after the time passes, without any action the SERVO returns to the initial position. The function takes as input the SERVO ID, the desired angle and the time. So, we can easily perform maneuvering using this function.
One of the most important parts of the application is the GPS and all the related software and hardware components. In order to achieve better performance, we have implemented two hardware universal asynchronous (UART) channels in the FPGA that are dedicated for the communication with the two GPS (Primary and Secondary). The UARTs collect all the messages from the GPS and put them in two dual port RAMs in the FPGA (one for each GPS of 512byte). At the same time an interrupt signal the processor that data from GPS is available. The ARM processor has direct access to the DPRAM so the data collection is very fast (only some ARM cycles).
Then, a GPS driver takes action to verify and extract the useful information from the packets.
The format used for the communication with the GPS is the Trimble Standard Interface Protocol (TSIP). The main advantages of TSIP comparing with the standard NMEA protocol are the following:
- Smaller packets (TSIP is binary protocol so the packet length is less than this of NMEA that is ASCII protocol)
- TSIP provides more command for GPS configure and data acquisition.
- Simpler packet striping with predefined packet length and fixed length data fields
- Full configurable automatic packet output
GPS Driver Test Data Output:
The navigation algorithm is responsible to manage the route of the vehicle and to give the command in order to keep the right route based on the desired predefined waypoints.
So, the main parameters of the navigation algorithm are the following:
First of all, the list of the waypoints, a user predefined list of the points that that vehicle must pass through. For its point the user must specify the desired coordinators of the point (longitude, latitude, altitude).
Some secondary parameters, that will help the navigation algorithm to be more efficient, are: the desired velocity, the approximation factor (how far from the point the vehicle can pass) and optionally some time parameters (in order to make a real time shortcuts if it is necessary).
The implementation of the algorithm is simple. First of all the points are shorted in the memory in the same order as we want them to be accessed by the vehicle. The algorithm reads the first point and set it as target. Then calculate the error from the current route (basically the angles between the target and vehicle route and also the difference in the velocity) then calculate the desired action and sends a massage to the SERVO control unit with the desired action (how many degrees we need to turn which SERVO for how long). In this way the navigation module constantly tries to keep the head of the vehicle “looking” the target.
In parallel the algorithm checks how far we are from the target compared with the approximation factor. So, if we are close enough to the target then a flag arises for the specific waypoint saying that we have successfully passed from the point. The algorithm gets the next waypoint only if the flag is set for the current point and only if the vehicle traveling away from the current waypoint. So, even if the flag is set the navigation tries to pass as close as it can from the waypoint.
The basic components cooperating with the navigation are the INS (in order to have a valid position of the vehicle and also information about the velocity and the traveling destination), the global system timer (a hardware timer located in the FPGA and used for synchronization of all the system) and finally the SERVO control module (in order to control the vehicle).
In the current implementation the software has the ability to keep up to 100 waypoints (no limitation on that by the hardware but no reason for more). The waypoint can be downloaded only before the starting of the system and for security reasons cannot be updated real time during the autonomous mode.
Base Station Communication Software
This part is very important especially in the development and debugging period.
The main functions that can be performed from a PC using the base station software are the following:
Sensor real time data acquisition, that give us the ability to have a real time visual estimation of the sensors output.
Application Code downloading. We can update the application of the board and also download new FPGA versions to the board.
Waypoint uploading. We can enter in to the system new waypoints for the navigation. Except the latitude and the longitude we have the ability to upload the altitude, the speed the approximation factor for each point.
Waypoint upload panel
Real time event log viewer. We can get in real time useful information from the board.
Perform real time tests for the hardware parts of the board (i.e. check SERVO hardware/software interface).
Upload and processing trace memory from the system. The board has the ability to log for five minutes all the sensors output pure data and all the data coming from the GPS in order to simulate situation and test the algorithms with real data. All the recorder data saved to the disk in two files (one binary file that includes the image of the system log memory and one ASCII file with a fixed size table that includes the data after the processing).
The communication with the board is based on the RS232 port of the PC. The baud rate is 115200bps for direct connection to the board and 19200bps for wireless connection (using the XStream modem), in both cases a ODD parity bit is used.
Base Station Main Panel
The following picture shows the output format (ASCII) of the logging data: