FC2カウンター FPGAの部屋 GPS と 3軸加速度センサーを使ったシステム6(Vitis でアプリケーション・ソフトウェアを作成)
fc2ブログ

FPGAやCPLDの話題やFPGA用のツールの話題などです。 マニアックです。 日記も書きます。

FPGAの部屋

FPGAの部屋の有用と思われるコンテンツのまとめサイトを作りました。Xilinx ISEの初心者の方には、FPGAリテラシーおよびチュートリアルのページをお勧めいたします。

GPS と 3軸加速度センサーを使ったシステム6(Vitis でアプリケーション・ソフトウェアを作成)

GPS と 3軸加速度センサーを使ったシステム5(Vivado 2020.1 のプロジェクトを作成中)”の続き。

前回は、GPS と 3 軸加速度センサーを 4 個使ったシステムの Vivado 2020.1 プロジェクトを紹介した。 GPS と 4 個の 3 軸加速度センサーそれぞれに MicroBlaze プロセッサを実装して、合計 5 個の MicroBlaze プロセッサと Zynq PS プロセッサが実装される回路になった。今回は、このシステムのアプリケーション・ソフトウェアを Vitis 2020.1 で作成したので、紹介する。

Vitis 2020.1 の画面を示す。
合計 5 個の MicroBlaze プロセッサと Zynq PS プロセッサについて、プラットフォームとアプリケーション・ソフトウェアを作成してある。
acc_gps_sensor_21_201118.png

4 個の MicroBlaze 用のアプリケーション・ソフトウェアは同じコードとなっている。
その mb_acc.c ソースコードを示す。
なお、ハードウェアはまだ作成していないので、すべてのコードは検証されていない。

// mb_app.c
// 2020/11/13 by marsee
//

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>

#define TX_FIFO_EMPTY   0x80
#define RX_FIFO_EMPTY   0x40
#define BB              0x04

#define AXI_GPIO_0  0x40000000
#define AXI_GPIO_1  0x40010000
#define AXI_IIC_0   0x40800000

void acc_sensor_init(volatile uint32_t *axi_iic_ad){
    axi_iic_ad[72] = 0x0F; // RX_FIFI_PIRQ
    axi_iic_ad[64] = 0x2; // Control Register (100h) reset tx fifo
    axi_iic_ad[64] = 0x1; // Control Register (100h) enable i2c
}

void idle_check(volatile uint32_t *axi_iic_ad){
    int32_t status_reg;
    int32_t check_bit;

    do{
        status_reg = axi_iic_ad[65]; // Status Register (104h))
        check_bit = status_reg & (TX_FIFO_EMPTY | RX_FIFO_EMPTY | BB);
    }while(check_bit != (TX_FIFO_EMPTY | RX_FIFO_EMPTY)) ;
}

void acc_sensor_write(volatile uint32_t *axi_iic_ad, uint32_t device_addr, uint32_t write_addr, uint32_t write_data){
    idle_check(axi_iic_ad);
    axi_iic_ad[66] = 0x100 | (device_addr & 0xfe); // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
    axi_iic_ad[66] = write_addr & 0xff;           // address
    axi_iic_ad[66] = 0x200 | (write_data & 0xff);      // data
}

uint32_t acc_sensor_read(volatile uint32_t *axi_iic_ad, uint32_t device_addr, uint32_t read_addr){
    int32_t status_reg, rx_fifo_empty;

    idle_check(axi_iic_ad);
    axi_iic_ad[66] = 0x100 | (device_addr & 0xfe); // Slave IIC Write Address, address is 0x108, i2c_tx_fifo
    axi_iic_ad[66] = read_addr & 0xff;  // address byte
    axi_iic_ad[66] = 0x100 | (device_addr & 0xff); // Slave IIC Read Address, address is 0x108, i2c_tx_fifo, with repeat start
    axi_iic_ad[66] = 0x201;      // 1 byte data, NACK condition

    do{
        status_reg = axi_iic_ad[65];
        rx_fifo_empty = status_reg & RX_FIFO_EMPTY;
    }while(rx_fifo_empty); // Wait untill not RX_FIFO_EMPTY(Status Register (104h))

    int32_t read_data = axi_iic_ad[67] & 0xff; // Read Receive FIFO (10Ch)
    return(read_data);
}

void acc_sensor_recv(volatile uint32_t *axi_iic_ad, int32_t *dataX, int32_t *dataY, int32_t *dataZ){
    int32_t read_data, data_ready;

    do{
        read_data = acc_sensor_read(axi_iic_ad, 0x3b, 0x04);
        data_ready = read_data & 0x01;
    }while(data_ready != 0x01);

    *dataX = acc_sensor_read(axi_iic_ad, 0x3b, 0x08) << 12; // XDATA3
    *dataX |= (acc_sensor_read(axi_iic_ad, 0x3b, 0x09) << 4); // XDATA2
    *dataX |= ((acc_sensor_read(axi_iic_ad, 0x3b, 0x0a) & 0xf0) >> 4); // XDATA1

    *dataY = acc_sensor_read(axi_iic_ad, 0x3b, 0x0b) << 12; // YDATA3
    *dataY |= (acc_sensor_read(axi_iic_ad, 0x3b, 0x0c) << 4); // YDATA2
    *dataY |= ((acc_sensor_read(axi_iic_ad, 0x3b, 0x0d) & 0xf0) >> 4); // YDATA1

    *dataZ = acc_sensor_read(axi_iic_ad, 0x3b, 0x0e) << 12; // ZDATA3
    *dataZ |= (acc_sensor_read(axi_iic_ad, 0x3b, 0x0f) << 4); // ZDATA2
    *dataZ |= ((acc_sensor_read(axi_iic_ad, 0x3b, 0x10) & 0xf0) >> 4); // ZDATA1
}

int main(){
    volatile uint32_t *axi_iic_ad, *axi_gpio_0_ad, *axi_gpio_1_ad;
    int32_t dataX, dataY, dataZ;
    uint8_t count, count_b;

    axi_iic_ad = (volatile uint32_t *)AXI_IIC_0;
    axi_gpio_0_ad = (volatile uint32_t *)AXI_GPIO_0;
    axi_gpio_1_ad = (volatile uint32_t *)AXI_GPIO_1;

    acc_sensor_init(axi_iic_ad);

    acc_sensor_write(axi_iic_ad, 0x3a, 0x2c, 0x82); // I2C speed is Hi speed, +-4g

    acc_sensor_write(axi_iic_ad, 0x3a, 0x1e, 0x00); // OFFSET_X_H
    acc_sensor_write(axi_iic_ad, 0x3a, 0x1f, 0x00); // OFFSET_X_L
    acc_sensor_write(axi_iic_ad, 0x3a, 0x20, 0x00); // OFFSET_Y_H
    acc_sensor_write(axi_iic_ad, 0x3a, 0x21, 0x00); // OFFSET_Y_L
    acc_sensor_write(axi_iic_ad, 0x3a, 0x22, 0x00); // OFFSET_Z_H
    acc_sensor_write(axi_iic_ad, 0x3a, 0x23, 0x00); // OFFSET_Z_L

    acc_sensor_write(axi_iic_ad, 0x3a, 0x2d, 0x00); // stanby clear

    int start = 1;
    while(1){
        while(1){ // wait new count
            count = (uint8_t)(axi_gpio_0_ad[0] & 0xff);
            if(start == 1){
                count_b = count - 1;
                start = 0;
            }
            if(count /= count_b){ // sensor receive start
                count_b = count;
                break;
            } else {
                usleep(500); // 500 us wait
            }
        }

        dataX = 0; dataY = 0; dataZ = 0;
        acc_sensor_recv(axi_iic_ad, &dataX, &dataY, &dataZ);

        dataX |= ((uint32_t)count << 24);
        axi_gpio_0_ad[0] = dataX; // GPIO_0, 0x0, GPIO_DATA
        axi_gpio_0_ad[2] = dataY; // GPIO_0, 0x8, GPIO2_DATA
        axi_gpio_1_ad[0] = dataZ; // GPIO_1, 0x0, GPIO_DATA
    }

    return(0);
}


GPS を担当する MicroBlaze 用のアプリケーション・ソフトウェアの mb_gps.c を示す。

// mb_gps.c
// 2020/11/17 by marsee
// reference : GPS NMEA format
// https://www.hiramine.com/physicalcomputing/general/gps_nmeaformat.html
//

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <unistd.h>
#include "xparameters.h"
#include "xuartlite_l.h"

// XPAR_GPS_UARTLITE_AXI_GPIO_MG0_BASEADDR 0x40000000 : [0]-latitude, [2]-longitude
// XPAR_GPS_UARTLITE_AXI_GPIO_MG1_BASEADDR 0x40010000 : [0]-gps_time

int main(){
    int i;
    char buf[100];
    char gps_time_s[20], latitude_s[20], longitude_s[20];
    int32_t gps_time, latitude, longitude;
    volatile int32_t *gpio_mg0, *gpio_mg1;
    double temp;

    gpio_mg0 = (volatile int32_t *)XPAR_GPS_UARTLITE_AXI_GPIO_MG0_BASEADDR;
    gpio_mg1 = (volatile int32_t *)XPAR_GPS_UARTLITE_AXI_GPIO_MG1_BASEADDR;

    XUartLite_SetControlReg(XPAR_GPS_UARTLITE_AXI_UARTLITE_0_BASEADDR, XUL_CR_FIFO_RX_RESET|XUL_CR_FIFO_TX_RESET); // Rst Rx FIFO, Rst TX FIFO
    XUartLite_SetControlReg(XPAR_GPS_UARTLITE_AXI_UARTLITE_0_BASEADDR, 0); // normal

    while(1){
        for(i=0; i<200; i++){
            char status = XUartLite_GetStatusReg(XPAR_GPS_UARTLITE_AXI_UARTLITE_0_BASEADDR);
            while(!(status & XUL_SR_RX_FIFO_VALID_DATA)); // Wait received data

            u8 read_data = XUartLite_RecvByte(XPAR_GPS_UARTLITE_AXI_UARTLITE_0_BASEADDR);
            buf[i] = read_data;
            if(read_data == '\n')
                break;
        }
        if(strstr(buf, "$GPRMC") == NULL)
            continue;
        strncpy(gps_time_s, &buf[7], 6); // UTC Time
        gps_time_s[6] = '\0';
        strncpy(latitude_s, &buf[20], 8);
        latitude_s[8] = '\0';
        strncpy(longitude_s, &buf[32], 9);
        longitude_s[9] = '\0';

        temp = atof(latitude_s); // convert double
        latitude = (int32_t)(temp * 1000.0);
        temp = atof(longitude_s); // convert double
        longitude = (int32_t)(temp * 1000.0);
        gps_time = (int32_t)(atoi(gps_time_s));

        gpio_mg0[0] = latitude;
        gpio_mg0[2] = longitude;
        gpio_mg1[0] = gps_time;
    }
}


最後に全体を統括する Zynq PS のアプリケーション・ソフトウェアを示す。

// arm_app.c
// 2020/11/16 by marsee
//

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "xtime_l.h"
#include "xparameters.h"

#define WAIT_US 3333 // 3333 us = 300 Hz

// XPAR_AXI_UARTLITE_0_BASEADDR 0x42C00000U         : for GPS UART
// XPAR_AXI_GPIO_4_BASEADDR 0x41200000              : for GPS gpio
// XPAR_MB_IIC_0_AXI_GPIO_P00_BASEADDR 0x41210000   : for MB0 X and Y axis
// XPAR_MB_IIC_0_AXI_GPIO_P01_BASEADDR 0x41250000   : for MB0 Z axis
// XPAR_MB_IIC_1_AXI_GPIO_P10_BASEADDR 0x41220000   : for MB1 X and Y axis
// XPAR_MB_IIC_1_AXI_GPIO_P11_BASEADDR 0x41260000   : for MB1 Z axis
// XPAR_MB_IIC_2_AXI_GPIO_P20_BASEADDR 0x41230000   : for MB2 X and Y axis
// XPAR_MB_IIC_2_AXI_GPIO_P21_BASEADDR 0x41270000   : for MB2 Z axis
// XPAR_MB_IIC_3_AXI_GPIO_P30_BASEADDR 0x41240000   : for MB3 X and Y axis
// XPAR_MB_IIC_3_AXI_GPIO_P31_BASEADDR 0x41280000   : for MB3 Z axis
// XPAR_GPS_UARTLITE_AXI_GPIO_PG0_BASEADDR 0x41290000   : for GPS axi_uartlite
// XPAR_GPS_UARTLITE_AXI_GPIO_PG1_BASEADDR 0x412A0000   : for GPS axi_uartlite
// XPAR_AXI_GPIO_4_BASEADDR 0x41200000  : for GPS pps

void get_axis_data(volatile int32_t *gpio0, volatile int32_t *gpio1, uint8_t count,
        int32_t *dataX, int32_t *dataY, int32_t *dataZ){
    int32_t gpiod;

    do {
        gpiod = gpio0[0]; // dataX
    } while(((gpiod>>24) & 0xff) != count) ;

    if(gpiod & 0x80000) // sign extended
        gpiod |= 0xfff00000;
    else
        gpiod &= 0x000fffff;
    *dataX = gpiod;

    gpiod = gpio0[2]; // dataY
    if(gpiod & 0x80000) // sign extended
        gpiod |= 0xfff00000;
    else
        gpiod &= 0x000fffff;
    *dataY = gpiod;

    gpiod = gpio1[0]; // dataZ
    if(gpiod & 0x80000) // sign extended
        gpiod |= 0xfff00000;
    else
        gpiod &= 0x000fffff;
    *dataZ = gpiod;
}

int main(){
    volatile int32_t *mb_p00, *mb_p01, *mb_p10, *mb_p11, *mb_p20, *mb_p21, *mb_p30, *mb_p31;
    volatile int32_t *gps_uart_pg0, *gps_uart_pg1, *gps_pps;
    int32_t dataX0, dataY0, dataZ0;
    int32_t dataX1, dataY1, dataZ1;
    int32_t dataX2, dataY2, dataZ2;
    int32_t dataX3, dataY3, dataZ3;
    uint8_t count;
    XTime time;
    long long int time_us, time_us_n, temp_time;
    int32_t latitude, longitude, gps_time, pps;
    uint32_t int_time;

    mb_p00 = (volatile int32_t *)XPAR_MB_IIC_0_AXI_GPIO_P00_BASEADDR;
    mb_p01 = (volatile int32_t *)XPAR_MB_IIC_0_AXI_GPIO_P01_BASEADDR;
    mb_p10 = (volatile int32_t *)XPAR_MB_IIC_1_AXI_GPIO_P10_BASEADDR;
    mb_p11 = (volatile int32_t *)XPAR_MB_IIC_1_AXI_GPIO_P11_BASEADDR;
    mb_p20 = (volatile int32_t *)XPAR_MB_IIC_2_AXI_GPIO_P20_BASEADDR;
    mb_p21 = (volatile int32_t *)XPAR_MB_IIC_2_AXI_GPIO_P21_BASEADDR;
    mb_p30 = (volatile int32_t *)XPAR_MB_IIC_3_AXI_GPIO_P30_BASEADDR;
    mb_p31 = (volatile int32_t *)XPAR_MB_IIC_3_AXI_GPIO_P31_BASEADDR;
    gps_uart_pg0 = (volatile int32_t *)XPAR_GPS_UARTLITE_AXI_GPIO_PG0_BASEADDR;
    gps_uart_pg1 = (volatile int32_t *)XPAR_GPS_UARTLITE_AXI_GPIO_PG1_BASEADDR;
    gps_pps = (volatile int32_t *)XPAR_AXI_GPIO_4_BASEADDR;

    count = 0;
    XTime_GetTime(&time);
    time_us = (long long int)((double)(time)/333.333);
    int_time = time_us;
    while(1){
        usleep(2500); // 2.5 ms Wait
        do {
            XTime_GetTime(&time);
            time_us_n = (long long int)((double)(time)/333.333);
            usleep(10); // 10us Wait
        } while((time_us_n - time_us) < WAIT_US);
        time_us = time_us_n;

        count++;
        mb_p00[0] = count;
        mb_p10[0] = count;
        mb_p20[0] = count;
        mb_p30[0] = count;

        get_axis_data(mb_p00, mb_p01, count, &dataX0, &dataY0, &dataZ0);
        get_axis_data(mb_p10, mb_p11, count, &dataX1, &dataY1, &dataZ1);
        get_axis_data(mb_p20, mb_p21, count, &dataX2, &dataY2, &dataZ2);
        get_axis_data(mb_p30, mb_p31, count, &dataX3, &dataY3, &dataZ3);

        latitude = gps_uart_pg0[0];
        longitude = gps_uart_pg0[2];
        gps_time = gps_uart_pg1[0];
        pps = gps_pps[0];

        temp_time = (time_us/1000000)*1000000;
        int_time = (uint32_t)(time_us - temp_time);

        printf("0, %d, %d, %d, %d, %d, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x, %x\n", latitude, longitude, gps_time, pps, int_time,
                dataX0, dataY0, dataZ0, dataX1, dataY1, dataZ1, dataX2, dataY2, dataZ2, dataX3, dataY3, dataZ3);
    }
}


当初、MicroBlaze には、BRAM のメモリを 8kバイトマップしたのだが、 mb_gps.c は 40 kバイト程度のメモリを使用しているので、ELF ファイルが生成されなかった。そこで、ブロックデザインで BRAM の容量を 64 kバイトに変更した。というか、変更できなかったので、MicroBlaze を一旦消去して、再度 Add IP した。今度は BRAM の容量を 64 kバイトに指定した。
acc_gps_sensor_28_201119.png

3 軸加速度センサー用の MicoBlaze の BRAM の容量は 8 kバイトに設定してある。
acc_gps_sensor_29_201119.png
  1. 2020年11月19日 04:41 |
  2. FPGAを使用したシステム
  3. | トラックバック:0
  4. | コメント:0

コメント

コメントの投稿


管理者にだけ表示を許可する

トラックバック URL
https://marsee101.blog.fc2.com/tb.php/5061-7c159e70
この記事にトラックバックする(FC2ブログユーザー)