Sensor Fusion Block Documentation
- Mostafa Hussein
Sensor Fusion Block
To create a block that runs using the sensor fusion code already in the ZP3.5 code base, a C function. More about them can be read here: https://www.mathworks.com/help/simulink/slref/cfunction.html?s_tid=mwa_osa_a, (there is also a link to youtube guide at the bottom of this page). This was also paired with some modification to the C++ code, such that the data that was previously retrieved through instances of the various sensors’ drivers, is now passed into the function.
Moreover, when talking about “the function” this refers to the ‘SF_GenerateNewResult’ function in the ZP3.5 sensor fusion code, as this is the block we care about.
Simulink Block Inputs & Outputs:
Note that all data types are automatically generated as Simulink.Bus types based on the struct definitions in the code. Within each struct however, each data type essentially gets translated to the equivalent data type in MATLAB.
Inputs:
imuData
Input for IMU data readings into sensor fusion block.
Of type IMU_Data_t.
GpsData
Input for GPS data readings into sensor fusion block.
Of type Gps_Data_t.
altimeterData
Input for altimeter data readings into sensor fusion block.
Of type Altimeter_Data_t.
iterData
Input for the values stored between iterations.
Of type SFIterationData_t.
SFoutput
Doesn’t need an input, just used so that an output can be created for this (I believe).
Of type SFOutput_t.
twoKi
Input for Ki constant for MahonyAHRS.
Of type single (equivalent to float).
twoKp
Input for Kp constant for MahonyAHRS.
Of type single (equivalent to float).
Outputs:
out
Can be ignored (created because of input)
imuData
Can be ignored (created because of input)
GpsData
Can be ignored (created because of input)
altimeterData
Can be ignored (created because of input)
iterData
Output of the changed iteration data.
Of type SFIterationData_t.
SFoutput
Output of the sensor fusion block (all the estimated values)
Of type SFOutput_t
Errors List (will keep expanding on it):
(From the errors I still remember while documenting this)
Compilation errors:
Had trouble with extern C functions being called from the sensor fusion c++ file. This was just fixed by moving these functions into the sensor fusion file and it seemed to work. (Still not fully sure what the issue was).
Simulink runtime errors:
Had issues running the Simulink model with the block imported using the Code importer wizard, but now works fine when importing the code using a c function block.
“’Sensor_Fusion_ZP.sldd’ was not found” error on running Simulink model. This can occur when opening the project for the first time or when reopening it, and can be fixed by relinking the Sensor Fusion data dictionary file (’Sensor_Fusion_ZP.sldd’).
This can be done through the model explorer, which can be accessed from the bottom left corner of the Simulink window (picture of the symbol attached below).
Testing
Initial Testing - Making sure it runs as expected
To check that the sensor fusion code was imported into Simulink correctly, constant values were used as inputs (representing inputs from the different sensors), then the output was compared to the inputs to be matched.
This was then paired with the sensor simulation blocks in SIMULINK, from the Sensor Fusion Blockset. This included the IMU and the GPS blocks. They take in inputs and output noisy measurements, based on their configuration. Images below show these blocks.
Notice, that data types had to be converted in some cases to ensure they fit into the Data Busses that represent the different structs in the SF code. There are also transpose blocks to correct the vector dimensions.
To verify the outputs (and can also be used to check the inputs) scope blocks were added to view the measurements. This was also paired with a visual representation of a drone to get a visual representation of some of the output data.
Notice that the latitude is varying a lot, while others to seem smoother. That actually isn’t the case. Taking a closer look at other data values such as the latitude or longitude, it is clear they too vary. Check the example below.
How to create a C Function Block