While thinking about ways to implement multiple sensor support – for several GPS or gyroscopes – I realized that an UAV can be modeled as a signal processing device.
It can be split like this:
- sources – like the MPU9250 which generates 4 signal streams: acceleration, angular velocity, magnetic field and temperature, the SRF02 sonar which generates a stream of distances
- streams – like the acceleration from the imu, or the location stream from the GPS. Each stream has a sample rate
- sink – like the pigpio PWM. Sinks accept streams of a certain sample rate only
- and processors – like low pass filters (LPF) or the PIDs, or the AHRS. Processors don’t have an inherent sample rate. They adopt the rate of the input usually.
All of these are implemented as nodes with several inputs and outputs. All nodes are added to the HAL layer and both their properties and connections are configured from a json file.
The important aspect is that – like all signal processing – signal rate is very important and has to be properly matched between nodes to satisfy the nyquist frequency. For example the PWM has a max sample rate of 490Hz so any signal higher than ~240Hz will be seen as noise. Higher frequency signals have to be resampled and low-pass-filtered before fed in the PWM node.
I started implementation last week and – after many rewrites – I have a model that ‘feels right’.
Here are some drawings:
Here the MPU9250 node exposes 4 sub-nodes (not shown) – an accelerometer, a gyroscope, a magnetometer and a thermometer. Each of these outputs a stream: acceleration stream, angular velocity, magnetic field and temperature.
The frequency of these streams is determined by the sensor. Acceleration and Angular Velocity are at 1Khz while magnetic field is at 100Hz. Temperature (not shown in the pic) is at 10Hz.
The Magnetic Field stream is fed into a Resampler node that takes it from 100Hz to 1000Hz, ready to be fed into the AHRS procesor node. The reason for this is that upsampling the stream needs a low-pass flter to avoid parasitic frequencies. A sample rate of 100Hz can carry at most a 50Hz signal so when going to 1000Hz sample rate this max frequency has to be maintained. Simply cloning each sample 10 times is not enough as this adds a lot of high frequency harmonics.
So now all signals at 1Khz sample rate are fed into the AHRS which outputs a Reference Frame stream, also at 1Khz. This reference frame represents the rotation of the UAV relative to earths frame of reference.
Right now I have a complimentary filter for the AHRS node but in the ear future I will write an extended kalman filter (EKF) for it – as soon as this tutorial is complete.
This diagram uses the AHRS output – the reference frame stream – plus the acceleration stream again to output 2 new streams – gravity vector and linear acceleration. These 2 streams will be further used for dead reckoning and stability pids.
Here I have the basic rate PIDs diagram. The angular velocity stream is first resampled from 1KHz to 50Hz (the pic says LPF but it’s actually a resampler node). The reason for this is that the usable bandwidth of the quadcopter is ~20-30Hz. Anything higher than this is noise.
The stream is further passed to the Rate PID processor node which outputs a torque stream (also at 50Hz). Torque is converted to thrust using a model of the motors+propellers (configurable of course), which is fed to the motor mixer and then in the PWM sink.
The advantage of this system is that new processors can be easily added to accommodate new hardware. For example if I want to use 2 GPS sensors I will add a redundancy processor that takes 2 location streams and outputs the healthiest one. Or I can merge them together to get better accuracy – while the rest of the nodes don’t even know nor care about this.
Basically I have minimal coupling between the elements of the system which talk through very well defined (and type safe) interfaces.
The ground station is also simplified. It can enumerate ll the nodes, read their configuration as a json value, change it and push it back to the nodes. Changing a filter cutoff will become trivial.
So far I still working to match the old functionality but in 1-2 weeks I believe this system will be ready for a test.