We added a hardware accelerated compass synthesized from C in Vitis HLS to our environmental HUD monitor on the AMD Xilinx SP701 development board. We have tested two different magnetometers: the ST's LSM9DS1 of the Digilent PmodNAV module and the Memsic's MMC34160PJ of the Digilent PmodCMPS2 module.
This compass will be used in addition to providing information on the orientation of the device to indicate the orientation in the panoramas that will be added later.
Table of Contents
- Hardware needed
- Adding a Compass Heads-up Display (HUD) overlay for orientation and referencing panoramas.
- Painting the dynamic dial of the magnetic compass
- Hardware design in VIVADO
- Magnetometer Sensors
- Digilent Pmod CMPS2
- Using the magnetometer as a Compass
- Programming a Bare Metal Compass App with VITIS IDE
- Moving Average Filter
- Averaging Angles or Not
- Bare Metal Compass App C code with Moving Average Filter
- Building the test system.
- Vivado Block Design
- Conclusions and Next Steps
- The Complete "Sensor Fusion for Firefighters" Blog Series
The Complete "Sensor Fusion for Firefighters" Blog Series
- Sensor Fusion for Firefighters. Introductory blog
- Sensor Fusion for Firefighters. Getting Started with the AMD Xilinx SP701
- Sensor Fusion for Firefighters. AMD Xilinx SP701 - MIPI Video Pipe Camera to HDMI Display
- Sensor Fusion for Firefighters. Displaying heads-up video on the live feed
- Sensor Fusion for Firefighters. Environmental monitor heads-up display on Xilinx Spartan-7 SP701 development board
- Sensor Fusion for Firefighters. Compass and Environmental HUD monitor with the Spartan-7
- Sensor Fusion for Firefighters. Thermal Vision, Compass and Environmental HUD monitor with the Spartan-7
- Sensor Fusion for Firefighters. Summary Blog
Hardware needed
To build the system we used:
- AMD Xilinx SP701 evaluation board and power supply
- JTAG USB Platform cable or USB cable Type A to micro - B
- HDMI cable
- HDMI Monitor: ELECROW 7" HDMI IPS Monitor - 1024x600
- Digilent Pcam 5C sensor module
- HDMI Monitor: HP 25x 24.5-inch Display
- Digilent PmodHYGRO
- SparkFun Environmental Combo Breakout - CCS811/BME280 (Qwiic)
- or Digilent Pmod AQS
- Digilent Pmod CMPS2
- Digilent Pmod NAV
Adding a Compass Heads-up Display (HUD) overlay for orientation and referencing panoramas.
Starting from the design of the frame generator to show the environmental information of temperature, relative humidity and CO2 level in the air that we designed in the previous blog, we will add the overlay of a compass dial that will show the orientation in real time. We continue using the AMD Xilinx Vitis HLS tool to generate an IP block that implements the generation of the AXIS stream with which we will feed the video mixer again. See previous blog: Environmental monitor heads-up display
Compass Heads-up Display (HUD) overlay demo without filtering:
Painting the dynamic dial of the magnetic compass
First we are going to design the compass dial. In the design of the compass we will take into account that we are designing hardware and that the resources we have are scarce, which is why we will use several tricks that make use of the ease of doing calculations with powers of two through logic gates. Avoiding in doing multiplications, divisions or modules that are not with powers of 2.
The magnetic compass dial consists of three parts:
- Labels for the four cardinal points.
- Graduated ruler with small and large marks
- Indicator needle or cursor
The dial is painted centered horizontally using 1440 pixels of the 1920 that has horizontally the resolution used by the display.
The dial is drawn on a semi-transparent dark background for better visibility over the background video image.
1440 pixels have been chosen for being the quadruple of 360 degrees. This allows us to transform the information into degrees to pixels using a 2-bit right shift, or multiplying by 4. Or as is the case of the actual module to use units of 1/4 degree.
At the top of the dial are the labels of the 4 cardinal points, north, south, east and west. The four new characters are formed in the same way that the digits from zero to nine have been created and are scalable in both width and height.
The labels are placed equidistant in barrel at 360 pixels, we use powers of 2 to divide the 1440 points into four equal parts of 360 points. The angular separation between two contiguous barrel pixels of the display is 1/4 degree.
The labels are moved horizontally so that in the center of the dial, where the indicator needle or cursor is painted, is always the front orientation of the device.
The graduated ruler is marked by small and large marks, painted as smaller or larger lines. Large marks represent angular distances of 16 degrees and small angular distances of 2 degrees. These values have been chosen because they are powers of 2 and allow painting using a function modulo for powers of two that consists of making a nand.
For small marks the module is made with 8. 1440 divided by 8 is 180. There are 180 marks and each of them represents an angular distance of 2 degrees with the previous and with the posterior, such as:
! (compassX & 0x00000007) = compassX % 8
Module with 64, 1440 divided by 64 is 22. There are 22 large marks separated by an angular distance of 16 degrees between them. Such as
! (compassX & 0x0000003F) = compassX % 64
The graduated marks also move horizontally making a barrel displacement.
The indicator needle or cursor is painted in red in the middle of the graduated ruler and occupies three pixels wide.
if ((x >= 959) & (x<=961)) {
hud_int.data = 0xFFFF0000;
}
This is the end result we want. The image has lost quality when converting to png format.
Vitis HLS Code
Below you have the complete code of the Vitis HLS module in C language.
In the same loop, both the pixels for the environmental information display and the complete compass dial are generated.
It is an extension of the module created in the previous blog and still meets the processing time requirements for the video stream to be generated correctly.
#include "ff_monitor.h"
#include "char7segment2.h"
#define COUNT_PIXEL 8
#define COUNT_LINE 6
#define COUNT_PIXELCMPS 3
#define COUNT_LINECMPS 1
#define YCHAR1 50
#define CHAR_WIDTH 5
// compass labels
#define YCMPSLABEL 730
#define CMPSLABEL_MARGINY 10
// compass scale
#define YCMPS1 770
#define CMPS_MARGINY 8
#define BLOCK_SEPARATION 200
#define DIGIT_SEPARATION 15
#define DISPLAY_MARGINY 10
#define XCHAR0 + 800
#define XCHAR1 XCHAR0 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION
#define XCHAR2 XCHAR1 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION
#define XCHAR3 XCHAR2 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION
#define XCHAR4 XCHAR3 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION + 500
#define XCHAR5 XCHAR4 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION
#define XCHAR6 XCHAR5 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION + 100
#define XCHAR7 XCHAR6 + COUNT_PIXEL * CHAR_WIDTH + DIGIT_SEPARATION
extern const unsigned int display_0[11][5];
extern const unsigned int display_1[11][5];
extern const unsigned int display_2[11][5];
extern const unsigned int display_3[11][5];
extern const unsigned int display_4[11][5];
extern const unsigned int display_5[11][5];
extern const unsigned int display_6[11][5];
extern const unsigned int display_7[11][5];
extern const unsigned int display_8[11][5];
extern const unsigned int display_9[11][5];
extern const unsigned int display_n[11][5];
extern const unsigned int display_e[11][5];
extern const unsigned int display_s[11][5];
extern const unsigned int display_w[11][5];
#define MAX_DIGITS 8
void drawDigitPixel(video_stream &hud_int, int line, int pixel, int digit);
void updateCounters(int &pixel, int &count_pixel);
void updateCountersCmps(int &pixel, int &count_pixel);
#define NORTH 10
#define EAST 11
#define SOUTH 12
#define WEST 13
#define COMPASSXMIN 240
#define COMPASSXMAX COMPASSXMIN + 4 * 360
// 90 * 4
#define DEGREES_90_1_4 360
// 180 * 4
#define DEGREES_180_1_4 720
// 270 * 4
#define DEGREES_270_1_4 1080
/**
* envData BCD representation of the 8 digits to display on the top
* angle compass orientation angle in 1/4 degrees units
*/
void ff_monitor_gen(axis& op, int row, int column, int envData, int angle) {
#pragma HLS INTERFACE s_axilite port=return
#pragma HLS INTERFACE s_axilite port=envData
#pragma HLS INTERFACE s_axilite port=angle
#pragma HLS INTERFACE s_axilite port=column
#pragma HLS INTERFACE s_axilite port=row
#pragma HLS INTERFACE axis register both port=op
int y = 0;
int x = 0;
int displacement = angle ;
int compassX = 0;
if (angle >= DEGREES_270_1_4) {
displacement = (angle - DEGREES_270_1_4) ;
} else if (angle >= DEGREES_180_1_4 ){
displacement = (angle - DEGREES_180_1_4) ;
} else if (angle >= DEGREES_90_1_4){
displacement = (angle - DEGREES_90_1_4) ;
} else {
displacement = angle ;
}
int xcardinal0 = COMPASSXMIN + 359 - displacement - (COUNT_PIXELCMPS + 1 )* CHAR_WIDTH/2;
int xcardinal1 = COMPASSXMIN + 719 - displacement - (COUNT_PIXELCMPS + 1 ) * CHAR_WIDTH/2;
int xcardinal2 = COMPASSXMIN + 1079 - displacement - (COUNT_PIXELCMPS + 1 ) * CHAR_WIDTH/2;
int xcardinal3 = COMPASSXMIN + 1439 - displacement - (COUNT_PIXELCMPS + 1 ) * CHAR_WIDTH/2;
int compassdisplacement = angle;
unsigned char digits[MAX_DIGITS] = {0};
digits[0] = (unsigned char)((envData >> 28) & 0x0F);
digits[1] = (unsigned char)((envData >> 24) & 0x0F);
digits[2] = (unsigned char)((envData >> 20) & 0x0F);
digits[3] = (unsigned char)((envData >> 16) & 0x0F);
digits[4] = (unsigned char)((envData >> 12) & 0x0F);
digits[5] = (unsigned char)((envData >> 8) & 0x0F);
digits[6] = (unsigned char)((envData >> 4) & 0x0F);
digits[7] = (unsigned char)((envData >> 0) & 0x0F);
unsigned char cardinals[4] = {SOUTH, WEST, NORTH, EAST};
if (angle >= 0 & angle <DEGREES_90_1_4) {
cardinals[0] = WEST;
cardinals[1] = NORTH;
cardinals[2] = EAST;
cardinals[3] = SOUTH;
} else if (angle >= DEGREES_90_1_4 & angle <DEGREES_180_1_4) {
cardinals[0] = NORTH;
cardinals[1] = EAST;
cardinals[2] = SOUTH;
cardinals[3] = WEST;
} else if (angle >= DEGREES_180_1_4 & angle <DEGREES_270_1_4) {
cardinals[0] = EAST;
cardinals[1] = SOUTH;
cardinals[2] = WEST;
cardinals[3] = NORTH;
}
int line = 0;
int pixel = 0;
int count_line = 0;
int count_pixel = 0;
int lineCmps = 0;
int pixelCmps = 0;
int count_lineCmps = 0;
int count_pixelCmps = 0;
video_stream hud_int;
row_loop:for (y =0; y<row; y++){
column_loop:for (x =0; x < column; x++) {
if (y == 0 && x == 0 ){
hud_int.user = 1;
}
else{
if (x == (column-1) ){
hud_int.last = 1;
}
else{
hud_int.last = 0;
hud_int.user = 0;
compassX = x + compassdisplacement;
if((y>YCHAR1-DISPLAY_MARGINY ) & (y<(YCHAR1+ 11 * (COUNT_LINE + 1) +DISPLAY_MARGINY )) ){
if((y>YCHAR1) & (y<(YCHAR1+ 11 * (COUNT_LINE + 1)))){
if(x>XCHAR0 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR0 + 1){
drawDigitPixel(hud_int, line, pixel, digits[0]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR1 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR1 + 1){
drawDigitPixel(hud_int, line, pixel, digits[1]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR2 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR2 + 1){
drawDigitPixel(hud_int, line, pixel, digits[2]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR3 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR3 + 1){
drawDigitPixel(hud_int, line, pixel, digits[3]);
updateCounters(pixel, count_pixel);
}else if(x>XCHAR4 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR4 + 1){
drawDigitPixel(hud_int, line, pixel, digits[4]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR5 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR5 + 1){
drawDigitPixel(hud_int, line, pixel, digits[5]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR6 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR6 + 1){
drawDigitPixel(hud_int, line, pixel, digits[6]);
updateCounters(pixel, count_pixel);
} else if(x>XCHAR7 & x < (COUNT_PIXEL + 1) * CHAR_WIDTH + XCHAR7 + 1){
drawDigitPixel(hud_int, line, pixel, digits[7]);
if (count_pixel == COUNT_PIXEL) {
if ((pixel == (CHAR_WIDTH - 1)) & (count_line == COUNT_LINE)) {
count_line = 0;
pixel = 0;
count_pixel = 0;
line ++;
} else if ((pixel == (CHAR_WIDTH - 1)) & (count_line < COUNT_LINE)) {
count_line ++;
pixel = 0;
count_pixel = 0;
} else {
count_pixel = 0;
pixel ++;
}
} else {
count_pixel ++;
}
}
else { //////
hud_int.data = BGN;
}
} else {
hud_int.data = hud_int.data = BGN;
}
} //// COMPASS LABELS
else if((y>YCMPSLABEL-CMPSLABEL_MARGINY )
& (y<(YCMPSLABEL + CMPSLABEL_MARGINY + 11 * (COUNT_LINECMPS + 1) )) ){
if((y>YCMPSLABEL ) & (y<(YCMPSLABEL + 11 * (COUNT_LINECMPS + 1) )) ){
if((x>xcardinal0) & (x < xcardinal0 + (COUNT_PIXELCMPS + 1) * CHAR_WIDTH + 1)){
drawDigitPixel(hud_int, lineCmps, pixelCmps, cardinals[0]);
updateCountersCmps(pixelCmps, count_pixelCmps);
} else if((x>xcardinal1) & (x < xcardinal1 + (COUNT_PIXELCMPS + 1) * CHAR_WIDTH + 1)){
drawDigitPixel(hud_int, lineCmps, pixelCmps, cardinals[1]);
updateCountersCmps(pixelCmps, count_pixelCmps);
} else if((x>xcardinal2) & (x < xcardinal2 + (COUNT_PIXELCMPS + 1) * CHAR_WIDTH + 1)){
drawDigitPixel(hud_int, lineCmps, pixelCmps, cardinals[2]);
updateCountersCmps(pixelCmps, count_pixelCmps);
} else if((x>xcardinal3) & (x < xcardinal3 + (COUNT_PIXELCMPS + 1) * CHAR_WIDTH + 1)){
drawDigitPixel(hud_int, lineCmps, pixelCmps, cardinals[3]);
if (count_pixelCmps == COUNT_PIXELCMPS) {
if ((pixelCmps == (CHAR_WIDTH - 1)) & (count_lineCmps == COUNT_LINECMPS)) {
count_lineCmps = 0;
lineCmps ++;
} else if ((pixelCmps == (CHAR_WIDTH - 1)) & (count_lineCmps < COUNT_LINECMPS)) {
count_lineCmps ++;
} else {
count_pixelCmps = 0;
pixelCmps ++;
}
} else {
count_pixelCmps ++;
}
} else if ((x >= 959) & (x<=961)) {
hud_int.data = 0xFFFF0000;
} else {
hud_int.data = BGN;
}
} else if ((x >= 959) & (x<=961)) {
hud_int.data = 0xFFFF0000;
} else {
hud_int.data = BGN;
}
} //// COMPASS AXIS
else if((y>YCMPS1-CMPS_MARGINY ) & (y<(YCMPS1 + 10 )) & (x > COMPASSXMIN) & (x<COMPASSXMAX) ){
if ((x >= 959) & (x<=961)) {
hud_int.data = 0xFFFF0000;
} else if((y>YCMPS1 ) & (y<(YCMPS1 + 10 )) ){
if(!(compassX & 0x00000007)) {
hud_int.data = CPX;
} else {
hud_int.data = BGN;
}
} else if(!(compassX & 0x0000003F)) {
hud_int.data = CPX;
} else {
hud_int.data = BGN;
}
}else {
hud_int.data = 0;
}
}
}
op.write(hud_int);
}
// row
pixelCmps = 0;
count_pixelCmps = 0;
}
}
void drawDigitPixel(video_stream &hud_int, int line, int pixel, int digit) {
#pragma HLS inline
if (digit == 0) {
hud_int.data = display_0[line][pixel];
} else if (digit == 1) {
hud_int.data = display_1[line][pixel];
} else if (digit == 2) {
hud_int.data = display_2[line][pixel];
} else if (digit == 3) {
hud_int.data = display_3[line][pixel];
} else if (digit == 4) {
hud_int.data = display_4[line][pixel];
} else if (digit == 5) {
hud_int.data = display_5[line][pixel];
} else if (digit == 6) {
hud_int.data = display_6[line][pixel];
} else if (digit == 7) {
hud_int.data = display_7[line][pixel];
} else if (digit == 8) {
hud_int.data = display_8[line][pixel];
} else if (digit == 9) {
hud_int.data = display_9[line][pixel];
} else if (digit == 10) {
hud_int.data = display_n[line][pixel];
} else if (digit == 11) {
hud_int.data = display_e[line][pixel];
} else if (digit == 12) {
hud_int.data = display_s[line][pixel];
} else if (digit == 13) {
hud_int.data = display_w[line][pixel];
} else {
hud_int.data = BGN;
}
}
void updateCounters(int &pixel, int &count_pixel) {
#pragma HLS inline off
if (count_pixel == COUNT_PIXEL) {
count_pixel = 0;
if (pixel == (CHAR_WIDTH - 1)) {
pixel = 0;
} else {
pixel++;
}
} else {
count_pixel++;
}
}
void updateCountersCmps(int &pixel, int &count_pixel) {
#pragma HLS inline off
if (count_pixel == COUNT_PIXELCMPS) {
count_pixel = 0;
if (pixel == (CHAR_WIDTH - 1)) {
pixel = 0;
} else {
pixel++;
}
} else {
count_pixel++;
}
}
Font definition
To the definition of fonts from the previous blog we have added 4 more characters that represent the lowercase characters of the four cardinal points, n, s, e and w
In addition, the main block has added the possibility of indicating that a display digit is not shown at all and that the new letters can also be shown on the numeric displays. In this way, e14 can also be shown on the upper displays. Cool!
#define CPX 0xffff00ff
#define BGN 0x7f101010
const unsigned int display_0[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_1[11][5] = {
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,}
};
const unsigned int display_2[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_3[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_4[11][5] = {
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,}
};
const unsigned int display_5[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_6[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_7[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,}
};
const unsigned int display_8[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_9[11][5] = {
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,}
};
const unsigned int display_n[11][5] = {
{ CPX,BGN,CPX,CPX,BGN,},
{ CPX,CPX,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,}
};
const unsigned int display_s[11][5] = {
{ BGN,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ BGN,CPX,CPX,CPX,BGN,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ BGN,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,BGN,}
};
const unsigned int display_w[11][5] = {
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ CPX,BGN,CPX,BGN,CPX,},
{ BGN,CPX,BGN,CPX,BGN,}
};
const unsigned int display_e[11][5] = {
{ BGN,CPX,CPX,CPX,BGN,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,BGN,BGN,BGN,CPX,},
{ CPX,CPX,CPX,CPX,CPX,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ CPX,BGN,BGN,BGN,BGN,},
{ BGN,CPX,CPX,CPX,CPX,}
};
Vitis HLS Pragma Directives for C-Synthesis Optimization
This is a list of the Pragma Directives used in the HLS module to help the C-Synthesis
The INTERFACE pragma specifies how RTL ports are created from the function definition during interface synthesis. In C/C++ code, all input and output operations are performed, in zero time, through formal function arguments. In a RTL design, these same input and output operations must be performed through a port in the design interface. Five ports are using the AXI Lite protocol and one is using the AXI Stream protocol.
The pragma HLS inline removes a function as a separate entity in the hierarchy. After inlining, the function is dissolved into the calling function and no longer appears as a separate level of hierarchy in the RTL. The pragma INLINE without arguments means that the function it is specified in should be inlined upward into any calling functions. The pragma INLINE OFF specifies that the function it is specified in should not be inlined upward into
any calling functions. This disables the inline of a specific function that can be automatically inlined or inlined as part of recursion.
The updatecounters and updatecounterscmps functions increment counters that allow fonts to scale in width and height. Since the labels on the compass dial and the digits on the numeric displays use different scales, different counters are used.
Resource estimates after HLS block synthesis
Hardware design in VIVADO
Once the module is created and verified, we export it as a Vivado IP from VITIS HLS and import it into the VIVADO project.
We made several modifications to our hardware:
- We added a PmodNAV IP block to support the Digilent Pmod NAV inertial sensor module connected to the external Pmod 2 port of the SP701 board.
- Added a PmodCMPS2 IP block to support the Digilent Pmod CMPS2 magnetometer sensor connected to the external Pmod 4 port of the SP701 board
- We changed the HUD overlay generator module for the new module with support for the compass display.
- Added a timer to be able to communicate with sensors at regular intervals.
The Pmod NAV module also has a magnetometer, but during our experiments we found that there were problems reading the values and we added the two to be able to compare results.
Utilization report
The following table shows the report on the use of resources for each of the blocks of the design. In particular, the use of our block for the Heads Up Display overlay block.
| Name | Slice LUTs | Slice Registers | F7 Muxes | F8 Muxes | Slice | LUT as Logic | LUT as Memory | Block RAM Tile | DSPs |
| axi4s_vid_0 (design_1_axi4s_vid_0_0) | 0.28% | 0.34% | 0.00% | 0.00% | 0.72% | 0.28% | 0.00% | 1.25% | 0.00% |
| axi_gpio_0 (design_1_axi_gpio_0_0) | 0.05% | 0.03% | 0.00% | 0.00% | 0.06% | 0.05% | 0.00% | 0.00% | 0.00% |
| axi_gpio_1 (design_1_axi_gpio_1_0) | 0.06% | 0.03% | 0.00% | 0.00% | 0.07% | 0.06% | 0.00% | 0.00% | 0.00% |
| axi_gpio_2 (design_1_axi_gpio_2_0) | 0.05% | 0.03% | 0.00% | 0.00% | 0.08% | 0.05% | 0.00% | 0.00% | 0.00% |
| axi_gpio_4 (design_1_axi_gpio_4_0) | 0.05% | 0.03% | 0.00% | 0.00% | 0.08% | 0.05% | 0.00% | 0.00% | 0.00% |
| axi_iic_0 (design_1_axi_iic_0_0) | 0.57% | 0.28% | 0.03% | 0.03% | 0.79% | 0.56% | 0.06% | 0.00% | 0.00% |
| axi_iic_1 (design_1_axi_iic_1_0) | 0.57% | 0.28% | 0.03% | 0.03% | 0.79% | 0.56% | 0.06% | 0.00% | 0.00% |
| axi_iic_2 (design_1_axi_iic_2_0) | 0.65% | 0.31% | 0.03% | 0.03% | 0.85% | 0.63% | 0.06% | 0.00% | 0.00% |
| axi_intc_0 (design_1_axi_intc_0_0) | 0.34% | 0.18% | <0.01% | 0.00% | 0.41% | 0.29% | 0.18% | 0.00% | 0.00% |
| axi_interc_0 (design_1_axi_interc_0_0) | 1.84% | 1.13% | 0.00% | 0.00% | 3.60% | 1.84% | 0.00% | 0.00% | 0.00% |
| axi_smc (design_1_axi_smc_0) | 9.65% | 6.71% | <0.01% | 0.00% | 15.00% | 7.83% | 6.62% | 0.00% | 0.00% |
| axi_timer_0 (design_1_axi_timer_0_0) | 0.31% | 0.14% | 0.00% | 0.00% | 0.49% | 0.31% | 0.00% | 0.00% | 0.00% |
| axi_tmr_0 (design_1_axi_tmr_0_0) | 0.46% | 0.19% | 0.00% | 0.00% | 0.68% | 0.46% | 0.00% | 0.00% | 0.00% |
| axi_uartl_0 (design_1_axi_uartl_0_0) | 0.15% | 0.09% | <0.01% | 0.00% | 0.23% | 0.13% | 0.06% | 0.00% | 0.00% |
| axi_vdma_0 (design_1_axi_vdma_0_0) | 4.83% | 3.86% | 0.11% | 0.00% | 8.78% | 4.52% | 1.11% | 10.83% | 0.00% |
| axis_slice_1 (design_1_axis_slice_1_0) | 0.03% | 0.04% | 0.00% | 0.00% | 0.11% | 0.03% | 0.00% | 0.00% | 0.00% |
| bram_ctrl_0 (design_1_bram_ctrl_0_0) | 0.35% | 0.18% | <0.01% | 0.00% | 0.56% | 0.35% | 0.00% | 0.00% | 0.00% |
| bram_ctrl_1 (design_1_bram_ctrl_1_0) | 0.01% | <0.01% | 0.00% | 0.00% | 0.03% | <0.01% | 0.01% | 1.67% | 0.00% |
| clkwiz_0 (design_1_clkwiz_0_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
| csirx_0 (design_1_csirx_0_0) | 4.65% | 3.84% | 0.15% | 0.02% | 8.03% | 4.54% | 0.41% | 1.67% | 0.00% |
| ff_monitor_gen_1 (design_1_ff_monitor_gen_1_0) | 6.04% | 1.99% | 0.02% | 0.00% | 7.82% | 6.04% | <0.01% | 0.00% | 2.50% |
| mb_concat (design_1_mb_concat_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
| mdm_1 (design_1_mdm_1_0) | 0.15% | 0.09% | 0.00% | 0.00% | 0.24% | 0.13% | 0.04% | 0.00% | 0.00% |
| microblaze_0 (design_1_microblaze_0_0) | 2.41% | 1.09% | 0.34% | 0.00% | 3.56% | 2.10% | 1.14% | 5.00% | 0.00% |
| microblaze_0_local_memory (microblaze_0_local_memory_imp_1K0VQXK) | 0.03% | 0.01% | 0.00% | 0.00% | 0.08% | 0.03% | 0.01% | 3.33% | 0.00% |
| mig_7series_1 (design_1_mig_7series_1_0) | 9.07% | 4.29% | 0.10% | 0.00% | 11.75% | 8.14% | 3.36% | 0.00% | 0.00% |
| oddr_0 (design_1_oddr_0_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
| p_sys_rst_0 (design_1_p_sys_rst_0_0) | 0.02% | 0.02% | 0.00% | 0.00% | 0.05% | 0.02% | <0.01% | 0.00% | 0.00% |
| PmodAQS_0 (design_1_PmodAQS_0_0) | 0.60% | 0.29% | 0.03% | 0.03% | 0.79% | 0.58% | 0.06% | 0.00% | 0.00% |
| PmodCMPS2_0 (design_1_PmodCMPS2_0_0) | 0.60% | 0.29% | 0.03% | 0.03% | 0.81% | 0.58% | 0.06% | 0.00% | 0.00% |
| PmodHYGRO_0 (design_1_PmodHYGRO_0_0) | 0.90% | 0.43% | 0.03% | 0.03% | 1.26% | 0.89% | 0.06% | 0.00% | 0.00% |
| PmodNAV_0 (design_1_PmodNAV_0_0) | 0.67% | 0.52% | 0.00% | 0.00% | 1.06% | 0.65% | 0.07% | 0.00% | 0.00% |
| rst_100M_0 (design_1_rst_100M_0_0) | 0.03% | 0.02% | 0.00% | 0.00% | 0.06% | 0.03% | <0.01% | 0.00% | 0.00% |
| rst_100M_1 (design_1_rst_100M_1_0) | 0.02% | 0.02% | 0.00% | 0.00% | 0.04% | 0.02% | <0.01% | 0.00% | 0.00% |
| rst_200M_0 (design_1_rst_200M_0_0) | 0.02% | 0.02% | 0.00% | 0.00% | 0.04% | 0.02% | <0.01% | 0.00% | 0.00% |
| v_mix_0 (design_1_v_mix_0_0) | 4.19% | 2.94% | 0.03% | <0.01% | 6.53% | 4.09% | 0.36% | 0.00% | 2.50% |
| v_tc_0 (design_1_v_tc_0_0) | 1.48% | 2.05% | 0.46% | 0.00% | 4.29% | 1.48% | 0.03% | 0.00% | 0.00% |
| v_tpg_0 (design_1_v_tpg_0_0) | 1.83% | 0.89% | 0.00% | 0.00% | 2.56% | 1.78% | 0.17% | 0.00% | 0.00% |
| vdm0 (design_1_vdm0_0) | 8.18% | 5.11% | <0.01% | 0.00% | 12.73% | 7.93% | 0.91% | 16.67% | 10.00% |
| vpss_1 (design_1_vpss_1_0) | 4.32% | 1.73% | 0.10% | 0.06% | 5.21% | 3.88% | 1.57% | 0.00% | 5.63% |
| xlconcat_0 (design_1_xlconcat_0_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
| xlslice_0 (design_1_xlslice_0_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
| xlslice_1 (design_1_xlslice_1_0) | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% | 0.00% |
Power Summary
Magnetometer Sensors
Digilent Pmod NAV with the LSM9DS1
The Digilent Pmod NAV (Revision C) uses the LSM9DS1 3-axis accelerometer, 3-axis gyroscope, 3-axis magnetometer, plus the LPS25HB digital barometer to provide 10-DOF functionality.
We use only the magnetometer for this project. In a future blog we will use the rest of the sensors to determine the position and orientation of the device. At the moment we work with a compass in a plane parallel to the ground.
The LSM9DS1 magnetometer features ±4/±8/±12/±16 gauss magnetic full scale. The magnetometer detects the magnetic field present around the module, including both the earth's magnetic field as well as the local magnetic environment around it. Each of the three magnetic axis registers provide signed 16-bit full-scale data with up to ±16 gauss sensitivity.
Before using the magnetometer, we should run a calibration routine to correct for any hard iron biases that may be present around them.
Test program
To better understand the operation of the magnetometer, we prepared a simple bare metal program that sends the data received from the magnetometer through the serial port and we capture it from a Windows program that graphs the data received through the serial port, SerialPlot.
/***************************** Include Files *******************************/
#include <stdio.h>
#include "math.h"
#include "PmodNAV.h"
#include "sleep.h"
#include "xil_cache.h"
#include "xparameters.h"
/*************************** Global Variables ******************************/
PmodNAV nav;
/********************* Function Forward Declarations ***********************/
void NavDemo_Initialize(void);
void NavDemo_Run(void);
void NavDemo_Cleanup(void);
float NavDemo_AngleInXY(NAV_RectCoord r);
void NavDemo_EnableCaches(void);
void NavDemo_DisableCaches(void);
/***************************** Function Definitions ************************/
int main(void) {
NavDemo_Initialize();
NavDemo_Run();
NavDemo_Cleanup();
return 0;
}
void NavDemo_Initialize(void) {
NavDemo_EnableCaches();
xil_printf("Pmod Nav Demo Initializing...\n\r");
NAV_begin ( // intialize the PmodNAV driver device
&nav,
XPAR_PMODNAV_0_AXI_LITE_GPIO_BASEADDR,
XPAR_PMODNAV_0_AXI_LITE_SPI_BASEADDR
);
xil_printf("Pmod Nav Demo Initialized\n\r");
NAV_Init(&nav); // initialize the connection with each spi slave
}
void NavDemo_Run(void) {
printf("Pmod Nav Demo Started\n\r");
NAV_SetRangeMAG(&nav, NAV_MAG_PAR_MAG_4GAUSS);
usleep(100000);
while(1){
NAV_GetData(&nav);
printf("%.4f, ", NavDemo_AngleInXY(nav.magData));
printf("%.4f,", nav.magData.X);
printf("%.4f,", nav.magData.Y);
printf("%.4f", nav.magData.Z);
printf("\n\r");
usleep(500000);
}
}
void NavDemo_Cleanup(void) {
NAV_end(&nav);
NavDemo_DisableCaches();
}
float NavDemo_AngleInXY(NAV_RectCoord r) {
float d;
if (r.X == 0)
d = (r.Y < 0) ? 90 : 0;
else
d = atan2f(r.Y, r.X) * 180 / 3.1415;
if (d > 360)
d -= 360;
else if (d < 0)
d += 360;
return d;
}
void NavDemo_EnableCaches(void) {
#ifdef __MICROBLAZE__
#ifdef XPAR_MICROBLAZE_USE_ICACHE
Xil_ICacheEnable();
#endif
#ifdef XPAR_MICROBLAZE_USE_DCACHE
Xil_DCacheEnable();
#endif
#endif
}
void NavDemo_DisableCaches(void) {
#ifdef __MICROBLAZE__
#ifdef XPAR_MICROBLAZE_USE_ICACHE
Xil_ICacheDisable();
#endif
#ifdef XPAR_MICROBLAZE_USE_DCACHE
Xil_DCacheDisable();
#endif
#endif
}
Testing the Digilent Pmod NAV
Graphs made with SerialPlot - Realtime Plotting Software | Hackaday.io
Digilent Pmod CMPS2
The Digilent Pmod CMPS2 (Revision A) is a 3-axis anisotropic magneto-resistive sensor. With Memsic's MMC34160PJ, the local magnetic field strength in a ±16 Gauss range with a heading accuracy of 1° and up to 0.5 mG of resolution.
Testing the Digilent Pmod CMPS2
As with the LSM9DS1 we created a simple program to test the sensor and graph its outputs:
Using the magnetometer as a Compass
The direction and strength of the earth's magnetic field (He) can be represented by the values of the three axes Hx, Hy, and Hz. The information from Hx and Hy can be used to determine compass headings with reference to the magnetic poles. To determine compass bearings using a magnetometer, the device must be level with the Earth's surface, with no ferrous material interfering with it. The earth's field and the angle of declination must be known.
Compass heading can be determined using only the Hx and Hy component of the earth's magnetic field, that is, the directions flat with the earth's surface.
The maximum value of Hx and Hy depends on the strength of the earth's field at that point. The compass magnetic heading can be determined (in degrees) from the x and y readings of the magnetometer using the following set of equations:
- Direction (y>0) = 90 - [arcTAN(x/y)]*180/PI
- Direction (y<0) = 270 - [arcTAN(x/y)]*180/PI
- Direction (y=0, x<0) = 180.0
- Direction (y=0, x>0) = 0.0
To determine the true north heading we must add or subtract the appropriate declination angle.
Programming a Bare Metal Compass App with VITIS IDE
With the Vitis IDE we created the bare metal application for the Microblaze softprocessor to configure and orchestrate the communication with the sensors with I2C and SPI interfaces.
This first version passes the data received from the magnetometer unfiltered to the compass. Later in this blog we will build a version with simple moving average lowpass filtering.
#define DEBUG 1
#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"
#include "xparameters.h"
#include "xiic.h"
#include "xil_exception.h"
#include "function_prototype.h"
#include "pcam_5C_cfgs.h"
#include "xstatus.h"
#include "sleep.h"
#include "xiic_l.h"
#include "xil_io.h"
#include "xil_types.h"
#include "xv_tpg.h"
#include "xil_cache.h"
#include "stdio.h"
#include "xv_mix.h"
#include "xff_monitor_gen.h"
#include "PmodHYGRO.h"
#include "PmodAQS.h"
#include <stdio.h>
#include "math.h"
//#include "PmodNAV.h"
#include "sleep.h"
#include "xil_cache.h"
#include "xparameters.h"
#include "PmodCMPS2.h"
// Calibration data struct, track minimum, maximum, and average sample seen for
// each x/y/z channel
typedef struct {
CMPS2_DataPacket max, min, mid;
} CMPS2_CalibrationData;
PmodCMPS2 cmps2;
CMPS2_CalibrationData myCalibrationData;
void CMPS2Initialize();
void CMPS2ClearCalibration(CMPS2_CalibrationData *calib);
void CMPS2Calibrate(PmodCMPS2 *InstancePtr, CMPS2_CalibrationData *calib,
CMPS2_DataPacket data);
float CMPS2ConvertDegree(PmodCMPS2 *InstancePtr, CMPS2_CalibrationData calib,
CMPS2_DataPacket data, int declination);
const int myDeclination = 1; // Magnetic declination for Zaragoza, Spain
const u8 chip_address = 0x30; // Pmod CMPS2 I2C chip address
/*************************** Global Variables ******************************/
#define U32TOBCD(u) ((((u/10000000)%10)<<28)| (((u/1000000)%10)<<24)|\
(((u/100000)%10)<<20) | (((u/10000)%10)<<16)|\
(((u/1000)%10)<<12) | (((u/100)%10)<<8)|\
(((u/10)%10)<<4)|(u%10))
#define U16TOBCD(u) ((((u/1000)%10)<<12) | (((u/100)%10)<<8)|\
(((u/10)%10)<<4)|(u%10))
#define U8TOBCD(u) ((((u/10)%10)<<4)|(u%10))
#ifdef __MICROBLAZE__
#define TIMER_FREQ_HZ XPAR_CPU_M_AXI_DP_FREQ_HZ
#else
#define TIMER_FREQ_HZ 100000000
#endif
/************************** Constant Definitions *****************************/
#define PAGE_SIZE 16
#define IIC_BASE_ADDRESS XPAR_IIC_2_BASEADDR
#define EEPROM_TEST_START_ADDRESS 0x80
#define IIC_SWITCH_ADDRESS 0x74
#define IIC_ADV7511_ADDRESS 0x39
typedef u8 AddressType;
typedef struct {
u8 addr;
u8 data;
u8 init;
} HDMI_REG;
#define NUMBER_OF_HDMI_REGS 16
HDMI_REG hdmi_iic[NUMBER_OF_HDMI_REGS] = {
{0x41, 0x00, 0x10},
{0x98, 0x00, 0x03},
{0x9A, 0x00, 0xE0},
{0x9C, 0x00, 0x30},
{0x9D, 0x00, 0x61},
{0xA2, 0x00, 0xA4},
{0xA3, 0x00, 0xA4},
{0xE0, 0x00, 0xD0},
{0xF9, 0x00, 0x00},
{0x18, 0x00, 0xE7},
{0x55, 0x00, 0x00},
{0x56, 0x00, 0x28},
{0xD6, 0x00, 0xC0},
{0xAF, 0x00, 0x4},
{0xF9, 0x00, 0x00}
};
u8 EepromIicAddr; /* Variable for storing Eeprom IIC address */
int IicLowLevelDynEeprom();
u8 EepromReadByte(AddressType Address, u8 *BufferPtr, u8 ByteCount);
u8 EepromWriteByte(AddressType Address, u8 *BufferPtr, u8 ByteCount);
/****************i************ Type Definitions *******************************/
typedef u8 AddressType;
/************************** Variable Definitions *****************************/
extern XIic IicFmc, IicAdapter ; /* IIC device. */
//HDMI IIC
int IicLowLevelDynEeprom()
{
u8 BytesRead;
u32 StatusReg;
u8 Index;
int Status;
u32 i;
EepromIicAddr = IIC_SWITCH_ADDRESS;
Status = XIic_DynInit(IIC_BASE_ADDRESS);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
xil_printf("\r\nAfter XIic_DynInit\r\n");
while (((StatusReg = XIic_ReadReg(IIC_BASE_ADDRESS,
XIIC_SR_REG_OFFSET)) &
(XIIC_SR_RX_FIFO_EMPTY_MASK |
XIIC_SR_TX_FIFO_EMPTY_MASK |
XIIC_SR_BUS_BUSY_MASK)) !=
(XIIC_SR_RX_FIFO_EMPTY_MASK |
XIIC_SR_TX_FIFO_EMPTY_MASK)) {
}
EepromIicAddr = IIC_ADV7511_ADDRESS;
for ( Index = 0; Index < NUMBER_OF_HDMI_REGS; Index++)
{
EepromWriteByte(hdmi_iic[Index].addr, &hdmi_iic[Index].init, 1);
}
for ( Index = 0; Index < NUMBER_OF_HDMI_REGS; Index++)
{
BytesRead = EepromReadByte(hdmi_iic[Index].addr, &hdmi_iic[Index].data, 1);
for(i=0;i<1000;i++) {}; // IIC delay
if (BytesRead != 1) {
return XST_FAILURE;
}
}
return XST_SUCCESS;
}
/*****************************************************************************/
/**
* This function writes a buffer of bytes to the IIC serial EEPROM.
*
* @param BufferPtr contains the address of the data to write.
* @param ByteCount contains the number of bytes in the buffer to be
* written. Note that this should not exceed the page size of the
* EEPROM as noted by the constant PAGE_SIZE.
*
* @return The number of bytes written, a value less than that which was
* specified as an input indicates an error.
*
* @note one.
*
******************************************************************************/
u8 EepromWriteByte(AddressType Address, u8 *BufferPtr, u8 ByteCount)
{
u8 SentByteCount;
u8 WriteBuffer[sizeof(Address) + PAGE_SIZE];
u8 Index;
/*
* A temporary write buffer must be used which contains both the address
* and the data to be written, put the address in first based upon the
* size of the address for the EEPROM
*/
if (sizeof(AddressType) == 2) {
WriteBuffer[0] = (u8) (Address >> 8);
WriteBuffer[1] = (u8) (Address);
} else if (sizeof(AddressType) == 1) {
WriteBuffer[0] = (u8) (Address);
EepromIicAddr |= (EEPROM_TEST_START_ADDRESS >> 8) & 0x7;
}
/*
* Put the data in the write buffer following the address.
*/
for (Index = 0; Index < ByteCount; Index++) {
WriteBuffer[sizeof(Address) + Index] = BufferPtr[Index];
}
/*
* Write a page of data at the specified address to the EEPROM.
*/
SentByteCount = XIic_DynSend(IIC_BASE_ADDRESS, EepromIicAddr,
WriteBuffer, sizeof(Address) + ByteCount,
XIIC_STOP);
/*
* Return the number of bytes written to the EEPROM.
*/
return SentByteCount - sizeof(Address);
}
/******************************************************************************
*
* This function reads a number of bytes from the IIC serial EEPROM into a
* specified buffer.
*
* @param BufferPtr contains the address of the data buffer to be filled.
* @param ByteCount contains the number of bytes in the buffer to be read.
* This value is constrained by the page size of the device such
* that up to 64K may be read in one call.
*
* @return The number of bytes read. A value less than the specified input
* value indicates an error.
*
* @note None.
*
******************************************************************************/
u8 EepromReadByte(AddressType Address, u8 *BufferPtr, u8 ByteCount)
{
u8 ReceivedByteCount;
u8 SentByteCount;
u16 StatusReg;
/*
* Position the Read pointer to specific location in the EEPROM.
*/
do {
StatusReg = XIic_ReadReg(IIC_BASE_ADDRESS, XIIC_SR_REG_OFFSET);
if (!(StatusReg & XIIC_SR_BUS_BUSY_MASK)) {
SentByteCount = XIic_DynSend(IIC_BASE_ADDRESS, EepromIicAddr,
(u8 *) &Address, sizeof(Address), XIIC_REPEATED_START);
}
} while (SentByteCount != sizeof(Address));
/*
* Receive the data.
*/
ReceivedByteCount = XIic_DynRecv(IIC_BASE_ADDRESS, EepromIicAddr,
BufferPtr, ByteCount);
/*
* Return the number of bytes received from the EEPROM.
*/
return ReceivedByteCount;
}
/*****************************************************************************/
/**
*
* Main function to initialize interop system and read data from AR0330 sensor
* @param None.
*
* @return
* - XST_SUCCESS if MIPI Interop was successful.
* - XST_FAILURE if MIPI Interop failed.
*
* @note None.
*
******************************************************************************/
int main() {
int Status;
int pcam5c_mode = 1;
XV_mix xv_mix;
XV_mix_Config *xv_config;
XFf_monitor_gen_Config *XV_Ff_monitor_cfg;
XFf_monitor_gen xv_ff_monitor;
xil_printf("\n\r******************************************************\n\r");
Xil_ICacheDisable();
Xil_DCacheDisable();
xil_printf("\n\r** Environmental HUD **");
PmodHYGRO hygro;
PmodAQS aqs;
float temp_degc, hum_perrh;
Status = IicLowLevelDynEeprom();
if (Status != XST_SUCCESS) {
xil_printf("ADV7511 IIC programming FAILED\r\n");
return XST_FAILURE;
}
xil_printf("ADV7511 IIC programming PASSED\r\n");
//Initialize FMC, Adapter and Sensor IIC
Status = InitIIC();
if (Status != XST_SUCCESS) {
xil_printf("\n\r IIC initialization Failed \n\r");
return XST_FAILURE;
}
xil_printf("IIC Initializtion Done \n\r");
//Initialize FMC Interrupt System
Status = SetupFmcInterruptSystem(&IicFmc);
if (Status != XST_SUCCESS) {
xil_printf("\n\rInterrupt System Initialization Failed \n\r");
return XST_FAILURE;
}
xil_printf("FMC Interrupt System Initialization Done \n\r");
//Set up IIC Interrupt Handlers
SetupIICIntrHandlers();
xil_printf("IIC Interrupt Handlers Setup Done \n\r");
Status = SetFmcIICAddress();
if (Status != XST_SUCCESS) {
xil_printf("\n\rFMC IIC Address Setup Failed \n\r");
return XST_FAILURE;
}
xil_printf("Fmc IIC Address Set\n\r");
//Initialize Adapter Interrupt System
Status = SetupAdapterInterruptSystem(&IicAdapter);
if (Status != XST_SUCCESS) {
xil_printf("\n\rInterrupt System Initialization Failed \n\r");
return XST_FAILURE;
}
xil_printf("Adapter Interrupt System Initialization Done \n\r");
//Set Address of Adapter IIC
Status = SetAdapterIICAddress();
if (Status != XST_SUCCESS) {
xil_printf("\n\rAdapter IIC Address Setup Failed \n\r");
return XST_FAILURE;
}
xil_printf("Adapter IIC Address Set\n\r");
Status = InitializeCsiRxSs();
if (Status != XST_SUCCESS) {
xil_printf("CSI Rx Ss Init failed status = %x.\r\n", Status);
return XST_FAILURE;
}
xv_config = XV_mix_LookupConfig(XPAR_XV_MIX_0_DEVICE_ID);
XV_mix_CfgInitialize(&xv_mix,xv_config,xv_config->BaseAddress);
XV_mix_Set_HwReg_width(&xv_mix, (u32)1920);
XV_mix_Set_HwReg_height(&xv_mix, (u32) 1080);
XV_mix_Set_HwReg_layerEnable(&xv_mix,(u32)3);
XV_mix_Set_HwReg_layerStartX_0(&xv_mix,(u32)0);
XV_mix_Set_HwReg_layerStartY_0(&xv_mix,0);
XV_mix_Set_HwReg_layerWidth_0(&xv_mix,(u32)1920);
XV_mix_Set_HwReg_layerHeight_0(&xv_mix,(u32)1080);
XV_mix_Set_HwReg_layerAlpha_0(&xv_mix, 255);
XV_mix_Set_HwReg_layerStartX_1(&xv_mix,(u32)0);
XV_mix_Set_HwReg_layerStartY_1(&xv_mix,0);
XV_mix_Set_HwReg_layerWidth_1(&xv_mix,(u32)1920);
XV_mix_Set_HwReg_layerHeight_1(&xv_mix,(u32)1080);
XV_mix_Set_HwReg_layerAlpha_1(&xv_mix, 128);
XV_mix_EnableAutoRestart(&xv_mix);
XV_mix_Start(&xv_mix);
// Ff monitor
XV_Ff_monitor_cfg = XFf_monitor_gen_LookupConfig(XPAR_FF_MONITOR_GEN_1_DEVICE_ID);
XFf_monitor_gen_CfgInitialize(&xv_ff_monitor,XV_Ff_monitor_cfg);
XFf_monitor_gen_Set_row(&xv_ff_monitor, (u32) 1080);
XFf_monitor_gen_Set_column(&xv_ff_monitor, (u32) 1920);
XFf_monitor_gen_EnableAutoRestart(&xv_ff_monitor);
XFf_monitor_gen_Start(&xv_ff_monitor);
XFf_monitor_gen_Set_envData(&xv_ff_monitor, 0x12345678);
XFf_monitor_gen_Set_angle(&xv_ff_monitor, 10);
resetIp();
EnableCSI();
Status = demosaic();
if (Status != XST_SUCCESS) {
xil_printf("\n\rDemosaic Failed \n\r");
return XST_FAILURE;
}
CamReset();
//Configure Sensor
Status = SensorPreConfig(pcam5c_mode);
if (Status != XST_SUCCESS) {
xil_printf("\n\rSensor PreConfiguration Failed \n\r");
return XST_FAILURE;
}
xil_printf("\n\rSensor is PreConfigured\n\r");
Status = vdma_hdmi();
if (Status != XST_SUCCESS) {
xil_printf("\n\rVdma_hdmi Failed \n\r");
return XST_FAILURE;
}
Status = vtpg_hdmi();
if (Status != XST_SUCCESS) {
xil_printf("\n\rVtpg Failed \n\r");
return XST_FAILURE;
}
WritetoReg(0x30, 0x08, 0x02);
Sensor_Delay();
xil_printf("\n\rPipeline Configuration Completed \n\r");
xil_printf("\n\rCMPS2 Init Started");
CMPS2Initialize();
CMPS2_DataPacket data;
CMPS2ClearCalibration(&myCalibrationData);
xil_printf("\n\rCMPS2 Init done");
xil_printf("\n\rHYGRO Init Started");
HYGRO_begin(
&hygro,
XPAR_PMODHYGRO_0_AXI_LITE_IIC_BASEADDR,
0x40, // Chip address of PmodHYGRO IIC
XPAR_PMODHYGRO_0_AXI_LITE_TMR_BASEADDR,
XPAR_PMODHYGRO_0_DEVICE_ID,
TIMER_FREQ_HZ // Clock frequency of AXI bus, used to convert timer data
);
xil_printf("\n\rHYGRO Init Done");
xil_printf("\n\rAQS Init Started");
AQS_begin(&aqs, XPAR_PMODAQS_0_AXI_LITE_IIC_BASEADDR, 0x5B); // Chip address of CS811 0x5A module IIC 0x5B PmocAQS
xil_printf("\n\rAQS Init Done");
cleanup_platform();
int hum = 0;
int temp = 0;
u8 buf[5];
int eCO2 = 0;
// u16 TVOC;
int counter = 0;
XFf_monitor_gen_Set_envData(&xv_ff_monitor, 0);
XFf_monitor_gen_Set_angle(&xv_ff_monitor,0) ;
float magXYd = 0;
XFf_monitor_gen_Set_envData(&xv_ff_monitor, 0xFB14FB14);
while(1) {
if(counter% 10 == 0){
data = CMPS2_GetData(&cmps2);
CMPS2Calibrate(&cmps2, &myCalibrationData, data);
magXYd = CMPS2ConvertDegree(&cmps2, myCalibrationData, data,
myDeclination);
XFf_monitor_gen_Set_angle(&xv_ff_monitor, (int)(magXYd * 4)) ;
}
if(counter% 100 == 0){
temp_degc = HYGRO_getTemperature(&hygro);
temp = (int) temp_degc;
hum_perrh = HYGRO_getHumidity(&hygro);
hum = (int) hum_perrh;
AQS_GetData(&aqs, buf);
eCO2 = (((int)buf[0]) << 8) | ((int)buf[1]);
XFf_monitor_gen_Set_envData(&xv_ff_monitor, (U16TOBCD(eCO2)<<16) | (U8TOBCD(temp)<<8) | U8TOBCD(hum));
}
++counter;
}
return XST_SUCCESS;
}
void CMPS2Initialize() {
CMPS2_begin(&cmps2, XPAR_PMODCMPS2_0_AXI_LITE_IIC_BASEADDR, chip_address);
usleep(10000);
CMPS2_SetSensor(&cmps2);
usleep(10000);
CMPS2_SetOutputResolution(&cmps2, 0b00);
}
void CMPS2ClearCalibration(CMPS2_CalibrationData *calib) {
calib->max.x = 0x8000; // Center point of 0x0000 -> 0xFFFF
calib->max.y = 0x8000;
calib->max.z = 0x8000;
calib->min.x = 0x8000;
calib->min.y = 0x8000;
calib->min.z = 0x8000;
calib->mid.x = 0x8000;
calib->mid.y = 0x8000;
calib->mid.z = 0x8000;
}
void CMPS2Calibrate(PmodCMPS2 *InstancePtr, CMPS2_CalibrationData *calib,
CMPS2_DataPacket data) {
if (data.x > calib->max.x) calib->max.x = data.x; // Track maximum / minimum
if (data.y > calib->max.y) calib->max.y = data.y; // value seen per axis
if (data.z > calib->max.z) calib->max.z = data.z;
if (data.x < calib->min.x) calib->min.x = data.x;
if (data.y < calib->min.y) calib->min.y = data.y;
if (data.z < calib->min.z) calib->min.z = data.z;
calib->mid.x = (calib->max.x >> 1) + (calib->min.x >> 1); // Find average
calib->mid.y = (calib->max.y >> 1) + (calib->min.y >> 1);
calib->mid.z = (calib->max.z >> 1) + (calib->min.z >> 1);
}
float CMPS2ConvertDegree(PmodCMPS2 *InstancePtr, CMPS2_CalibrationData calib,
CMPS2_DataPacket data, int declination) {
int tx, ty;
float deg;
if (data.x < calib.mid.x)
tx = (calib.mid.x - data.x);
else
tx = (data.x - calib.mid.x);
if (data.y < calib.mid.y)
ty = (calib.mid.y - data.y);
else
ty = (data.y - calib.mid.y);
if (data.x < calib.mid.x) {
if (data.y > calib.mid.y)
deg = 90.0f - atan2f(ty, tx) * 180.0f / 3.14159f;
else
deg = 90.0f + atan2f(ty, tx) * 180.0f / 3.14159f;
} else {
if (data.y < calib.mid.y)
deg = 270.0f - atan2f(ty, tx) * 180.0f / 3.14159f;
else
deg = 270.0f + atan2f(ty, tx) * 180.0f / 3.14159f;
}
deg += declination;
while (deg >= 360)
deg -= 360;
while (deg < 0)
deg += 360;
#ifdef DEBUG
printf("%d, ", (int)deg);
printf("%d,", data.x);
printf("%d,", data.y);
printf("%d", data.z);
printf("\n\r");
#endif
return deg;
}
In this video we can see the result of the compass receiving the data without filtering.
Moving Average Filter
To smooth out the movement of our compass we will use a moving average filter. The mean averaging filter is a simple FIR (finite impulse response) low pass filter that is commonly used to smooth a matrix of sampled data/signals. It takes N input samples at a time and takes the average of those N-samples and produces a single output point. It is a very simple LPF (Low Pass Filter) structure that is useful for filtering out unwanted noisy components from the predicted data.
Averaging Angles or Not
When we calculate the average or mean of an angle, we must take into account how the angles are wrapped so that any angle in degrees plus any integer multiple of 360 degrees is a measure of the same angle.
If we take the compass bearing in two readings where the first reading was 350 degrees and the second was 10 degrees, then the average of the numbers is 180 degrees, whereas we can note that 350 degrees is equivalent to -10 degrees, so we have two readings at 10 degrees to either side of zero degrees leading to a more suitable mean angle of zero degrees.
To avoid this problem we can put all the angles in the unit circle and convert them into complex numbers expressed in real and imaginary form. Once this is done, we calculate the mean of the complex numbers. So we convert the complex mean to polar coordinates and the phase of the complex mean is the angular mean we are looking for.
Instead of converting the orientation angle to its representation as a complex number to obtain the mean, we will make the moving average of the components of the magnetic magnetic field, x and y, that the sensor returns to us, separately.
Bare Metal Compass App C code with Moving Average Filter
The initialization and configuration code is the same, below you can see the main loop of the compass and environment HUD monitor applying the moving average low pass filter on the two components, X and Y reported by the sensor.
data = CMPS2_GetData(&cmps2);
float n = 4.0; // average last 4 measurements
// start average with actual orientation
float avgx = data.x;
float avgy = data.y;
while(1) {
data = CMPS2_GetData(&cmps2);
// moving varage for x magnetic component
avgx = avgx - avgx/n;
avgx = avgx + data.x/n;
avgy = avgy - avgy/n;
avgy = avgy + data.y/n;
data.x = (u16)avgx;
data.y = (u16)avgy;
CMPS2Calibrate(&cmps2, &myCalibrationData, data);
magXYd = CMPS2ConvertDegree(&cmps2, myCalibrationData, data,
myDeclination);
XFf_monitor_gen_Set_angle(&xv_ff_monitor, (int)(magXYd * 4)) ;
if(counter% 100 == 0){
temp_degc = HYGRO_getTemperature(&hygro);
temp = (int) temp_degc;
hum_perrh = HYGRO_getHumidity(&hygro);
hum = (int) hum_perrh;
AQS_GetData(&aqs, buf);
eCO2 = (((int)buf[0]) << 8) | ((int)buf[1]);
XFf_monitor_gen_Set_envData(&xv_ff_monitor, (U16TOBCD(eCO2)<<16) | (U8TOBCD(temp)<<8) | U8TOBCD(hum));
}
++counter;
}
In this video we can see the result of the compass with the Moving Average Filter activated, with n = 5
Building the test system.
In order to be able to rotate the board for the tests, the board was mounted on a round metal cookie box with the upper lid greased and the base fixed to the table.
An attempt has been made to place the magnetometer as close as possible to the nodal point of the PCAM 5C camera lens and in a plane parallel to the earth's surface.
In the image, tests with the Pmod NAV module.
In the video you can see the rotation movement of the board on the lid of the round tin box. The HDMI video output is connected to an HP 25x monitor. The version of the software loaded on the MicroBlaze has several bugs but it serves as a sample to see how the test environment works.
Vivado Block Design
Conclusions and Next Steps
This is the sixth blog in the Sensor Fusion for Firefighters series. We already have a HUD that presents information overlaid on the video feed in real time reporting temperature, relative humidity, CO2 level and now also a compass that shows the orientation of the device.
We have tested two different magnetometers. The main reason was the poor results obtained with the Pmod NAV module. Later, after comparing the response of both sensors, we realized that the problem with PmodNAV was in the software, on the one hand some integer divisions in the driver demos that reduced precision and then a badly designed filter. Once these problems were solved, we have been able to use the compass with both sensors.
We already started to add a thermal array sensor for the next blog.
The Complete "Sensor Fusion for Firefighters" Blog Series
- Sensor Fusion for Firefighters. Introductory blog
- Sensor Fusion for Firefighters. Getting Started with the AMD Xilinx SP701
- Sensor Fusion for Firefighters. AMD Xilinx SP701 - MIPI Video Pipe Camera to HDMI Display
- Sensor Fusion for Firefighters. Displaying heads-up video on the live feed
- Sensor Fusion for Firefighters. Environmental monitor heads-up display on Xilinx Spartan-7 SP701 development board
- Sensor Fusion for Firefighters. Compass and Environmental HUD monitor with the Spartan-7
- Sensor Fusion for Firefighters. Thermal Vision, Compass and Environmental HUD monitor with the Spartan-7
- Sensor Fusion for Firefighters. Summary Blog
