The core of our flight control system is a CPU/DSP board containing the following main components:
The DSP’s two on-board UARTs are connected to LVDS transceivers to interface with the I/O boards. An additional two UARTs are provided by a chip connected to the DSP’s EMIF; these are routed to the CPLD, which will normally link one with the CPU and one to the RFD900a interface.
The DSP is also connected to the CPU via a 480Mbps USB 2.0 link, also provided by a chip attached to the EMIF. This will be the primary interface, used for logging all data received from the I/O boards as well as transferring state information and commands.
The CPU’s built-in USB 2.0 interfaces are connected to the 3G modem module and the DSP; a further four USB ports are provided by a USB hub connected to the HSIC port. These are exposed via two Hirose DF13 pin headers and two micro-AB connectors.
The CPU has three UARTs exposed, one of which will normally be connected to the DSP via the CPLD, and one of which is used for console access (there is no networking capability on board). The last one is spare, potentially to be connected to the RFD900a if we don’t drive it from the DSP.
Both CPU and DSP have access to an SPI flash chip switched via the CPLD; this chip is used to store the DSP firmware so resets can occur without CPU intervention. After the firmware is loaded into RAM, the DSP releases control of the chip so firmware can be updated without disrupting system operation.
The CPLD is also able to switch the DSP’s SPI port to an external connector; we included this facility to enable the system to interface with higher-end Analog Devices IMU products if we find we need better sensors.
An external optically isolated camera trigger is provided, connected to the CPLD which can drive it from GPIOs on the DSP and CPU. At this stage we’re planning to trigger the camera periodically from the DSP, and flag the state data associated with the trigger timestamp; if we can measure camera latency accurately enough, this should give us georeferencing data accurate to 1ms.
Given the flexibility of this hardware, there’s quite a bit of scope for moving parts of the workload between the DSP and the main CPU. For example, there’s enough USB throughput, enough RAM and probably enough cycles for the DSP to handle some of the image processing workload, and the split between flight control and mission planning (essentially, flight control takes a planned path and tries to follow it; mission planning takes a set of waypoints and generates a path capable of being followed) can be shifted towards the CPU or DSP depending on power and accuracy requirements.
Thus, the DSP firmware needs a somewhat more flexible architecture than the simple event loop model used by the I/O boards. TI provide a lightweight operating system, SYS/BIOS, so initially we’ll be developing firmware components as deterministically-scheduled tasks to be managed by that OS.
The two main tasks are position/velocity/attitude estimation (UKF), and flight control (NMPC, not yet written). Secondary tasks include managing the I/O board serial links and timeout logic, mission abort checks, logging input and state to the CPU, and receiving paths and boundaries from the CPU.
The estimation and control tasks each run on a separate core, with the I/O board interface and timeout logic running on the same core as the estimation task and the mission abort and logging code running on the same core as the control task. The estimation, I/O board and timeout tasks run at 1000Hz, while the control tasks run at 50Hz (although this may be increased if we switch to 400Hz servos). Communication between cores will be via message queue, so there’s no need for shared memory or significant concurrency work.
As a first step, the existing UKF library had to be compiled in TI’s Code Composer Studio, for the TMS320C6657 EVM. Although TI’s compiler is quite full-featured for a proprietary system, its C++ template support is only barely adequate for the task—the UKF had to be split into two files in order to stay under the compiler’s memory limit. Aside from that, Eigen compiles with only a few minor modifications, and the UKF needed no changes aside from the file restructure.
Next up we’ll flesh out the estimation task loop, initially reading from a fixed buffer of sensor data in order to benchmark UKF performance. Although the DSP should on paper have at least the same floating-point performance as a current Intel Core processor at the same clock speed, Eigen is very dependent on compiler optimisation effectiveness, and there are currently no optimised primitives available for the TI DSPs. If performance proves insufficient, we’ll need to merge TI’s optimised math library into the Eigen backend.
The in-progress FCS source code is available at sfwa/fcs.