MPU6050 sensor RTL

From Hamsterworks Wiki!

Jump to: navigation, search

This FPGA Project was completed in Jan 2016.

After connection my MPU6050 sensor to a Raspberry Pi Zero I decided to do a quick hack to connect it to an FPGA.

MPU6050 basys3.jpg

This project can display the readings of the accelerometer and gyroscope on the board's LEDs. Note that the values displayed are signed binary, with full scale being +/- 16 G for acceleration and +/- 2000 degrees per second for rotation. There is a short video at https://youtu.be/IKylOPzRqeY

Contents

Source

top_level.vhd

----------------------------------------------------------------------------------
-- Engineer: Mike Field <hamster@snap.net.nz> 
-- 
-- Module Name: top_level - Behavioral
-- 
-- Description: Top level design for the MPU6050 module test design 
-- 
----------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;

entity top_level is
    Port ( clk100   : in  STD_LOGIC;
           mpu6050  : in  STD_LOGIC;
           leds     : out STD_LOGIC_VECTOR (15 downto 0);
           switches : in  STD_LOGIC_VECTOR (2 downto 0));
end top_level;

architecture Behavioral of top_level is
    component rs232_115200_rx is
    Port ( clk100      : in STD_LOGIC;
           serial_in    : in STD_LOGIC;
           data_byte    : out STD_LOGIC_VECTOR (7 downto 0);
           data_byte_de : out STD_LOGIC);
    end component;

    component mpu6050_decode is
    Port ( clk          : in  STD_LOGIC;
           data_byte    : in  STD_LOGIC_VECTOR (7 downto 0);
           data_byte_de : in  STD_LOGIC;
           acc_x        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_y        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_z        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_de       : out STD_LOGIC;
           rot_x        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_y        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_z        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_de       : out STD_LOGIC;
           att_roll     : out STD_LOGIC_VECTOR (15 downto 0);
           att_pitch    : out STD_LOGIC_VECTOR (15 downto 0);
           att_yaw      : out STD_LOGIC_VECTOR (15 downto 0);
           att_de       : out STD_LOGIC);
    end component;
    
    signal data_byte    : STD_LOGIC_VECTOR (7 downto 0);
    signal data_byte_de : STD_LOGIC;

    signal acc_x        : STD_LOGIC_VECTOR (15 downto 0);
    signal acc_y        : STD_LOGIC_VECTOR (15 downto 0);
    signal acc_z        : STD_LOGIC_VECTOR (15 downto 0);
    signal acc_de       : STD_LOGIC;

    signal rot_x        : STD_LOGIC_VECTOR (15 downto 0);
    signal rot_y        : STD_LOGIC_VECTOR (15 downto 0);
    signal rot_z        : STD_LOGIC_VECTOR (15 downto 0);
    signal rot_de       : STD_LOGIC;

    signal att_roll     : STD_LOGIC_VECTOR (15 downto 0);
    signal att_pitch    : STD_LOGIC_VECTOR (15 downto 0);
    signal att_yaw      : STD_LOGIC_VECTOR (15 downto 0);
    signal att_de       : STD_LOGIC;

begin

i_rs232_115200_rx: rs232_115200_rx port map (
        clk100       => clk100,
        serial_in    => mpu6050,
        data_byte    => data_byte,
        data_byte_de => data_byte_de);

i_mpu6050_decode: mpu6050_decode port map (
        clk          => clk100,
        data_byte    => data_byte,
        data_byte_de => data_byte_de,
        acc_x        => acc_x,
        acc_y        => acc_y,
        acc_z        => acc_z,
        acc_de       => acc_de,
        rot_x        => rot_x,
        rot_y        => rot_y,
        rot_z        => rot_z,
        rot_de       => rot_de,
        att_roll     => att_roll,
        att_pitch    => att_pitch,
        att_yaw      => att_yaw,
        att_de       => att_de);

process(clk100) 
    begin
        if rising_edge(clk100) then
            case switches is
                when "000"  => leds <= acc_x;
                when "001"  => leds <= acc_y;
                when "010"  => leds <= acc_z;
                when "011"  => leds <= rot_x;
                when "100"  => leds <= rot_y;
                when "101"  => leds <= rot_z;
                when others => leds <= (others => '0');
            end case;         
        end if;
    end process;
end Behavioral;

mpu6050_decode.vhd

Note - this does not have the error detection / counters that the Raspberry Pi 'C' version had. You might want to add it if you use this i a project. It does however verify the checksum, so the odds of bad data slipping through is pretty small.

----------------------------------------------------------------------------------
-- Engineer: 
-- Module Name: mpu6050_decode - Behavioral
-- Description: Decode the data stream from the MPU6050 + Kalman filter module
----------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;

entity mpu6050_decode is
    Port ( clk          : in  STD_LOGIC;
           data_byte    : in  STD_LOGIC_VECTOR (7 downto 0);
           data_byte_de : in  STD_LOGIC;
           acc_x        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_y        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_z        : out STD_LOGIC_VECTOR (15 downto 0);
           acc_de       : out STD_LOGIC;
           rot_x        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_y        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_z        : out STD_LOGIC_VECTOR (15 downto 0);
           rot_de       : out STD_LOGIC;
           att_roll     : out STD_LOGIC_VECTOR (15 downto 0);
           att_pitch    : out STD_LOGIC_VECTOR (15 downto 0);
           att_yaw      : out STD_LOGIC_VECTOR (15 downto 0);
           att_de       : out STD_LOGIC);
end mpu6050_decode;

architecture Behavioral of mpu6050_decode is
    -- Constants for hte protocol from the sensor
    constant HEADER_BYTE : std_logic_vector(7 downto 0) := x"55";
    constant HEADER_ACC  : std_logic_vector(7 downto 0) := x"51";
    constant HEADER_ROT  : std_logic_vector(7 downto 0) := x"52";
    constant HEADER_ATT  : std_logic_vector(7 downto 0) := x"53";
    
    type c_array is array(0 to 9) of STD_LOGIC_VECTOR (7 downto 0);
    signal c      : c_array := (others => (others => '0'));
    signal chksum : unsigned(7 downto 0) := (others => '0');
    
begin

process(clk)
    begin
        if rising_edge(clk) then
            acc_de <= '0';
            rot_de <= '0';
            att_de <= '0';
            if data_byte_de = '1' then
                -- Update the running checksum
                chksum <= chksum + unsigned(data_byte) - unsigned(c(0));
                
                -- Shift in the character
                c(0 to 8) <= c(1 to 9);
                c(9)      <= data_byte;
                if c(0) = HEADER_BYTE then
                    if chksum = unsigned(data_byte) then
                        case c(1) is
                            when HEADER_ACC => -- Acceleration
                                acc_x     <= c(3) & c(2);
                                acc_y     <= c(5) & c(4);
                                acc_z     <= c(7) & c(6);
                                acc_de    <= '1';
                            when HEADER_ROT => -- Rotation
                                rot_x     <= c(3) & c(2);
                                rot_y     <= c(5) & c(4);
                                rot_z     <= c(7) & c(6);
                                rot_de    <= '1';
                            when HEADER_ATT => -- Attitude
                                att_roll  <= c(3) & c(2);
                                att_pitch <= c(5) & c(4);
                                att_yaw   <= c(7) & c(6);
                                att_de    <= '1';
                            when others => NULL;
                        end case;
                    end if;
                end if;
            end if;
        end if;
    end process;
end Behavioral;

rs232_115200_rx.vhd

A very quick-and-dirty RS232 115200 baud RX module, coded to work first time and not be efficient

 
----------------------------------------------------------------------------------
-- Engineer: Mike Field <hamster@snap.net.nz>
-- 
-- Description: A hack to Receive bytes over a 115200 serial link 
-- 
----------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;

entity rs232_115200_rx is
    Port ( clk100       : in  STD_LOGIC;
           serial_in    : in  STD_LOGIC;
           data_byte    : out STD_LOGIC_VECTOR (7 downto 0);
           data_byte_de : out STD_LOGIC);
end rs232_115200_rx;

architecture Behavioral of rs232_115200_rx is
    -- to synchronise the serial input
    signal serial_in_meta : std_logic := '0';
    signal serial_in_safe : std_logic := '0';
    
    -- Must be big enough to hold 10 bit-times (approx 9,000)
    -- (actually 9.5 bit-times + 1, but who's counting...)
    signal counter      : unsigned(13 downto 0)        := (others => '0');
    
    -- Holding the incoming bits
    signal data_byte_sr : std_logic_vector(7 downto 0) := (others => '0');
    
    -- Constants for the bit timings
    constant half_bit   : integer := 100000000/115200/2; 
    constant full_bit   : integer := 100000000/115200; 
begin
    data_byte <= data_byte_sr; 
process(clk100)
    begin
        if rising_edge(clk100) then
            data_byte_de <= '0';
            if counter = 0 then
                -- this is the falling edge of the start bit
                if serial_in_safe = '0' then
                    counter <= counter + 1;
                end if;
            else
                -- Grab the bits at 115200 baud (intially waiting 1.5 bit times
                case counter is 
                    when to_unsigned(half_bit+1*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+2*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+3*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+4*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+5*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+6*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+7*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+8*full_bit, counter'length) => data_byte_sr <= serial_in_safe & data_byte_sr(7 downto 1);
                    when to_unsigned(half_bit+9*full_bit, counter'length) => data_byte_de <= serial_in_safe; -- Stop bit should be '1'
                    when others => NULL;                                                                             
                end case;
                
                if counter = to_unsigned(half_bit+9*full_bit+1, counter'length) then
                    if serial_in_safe = '0' then
                        counter <= (others => '0');
                    end if;
                else
                    counter <= counter + 1;
                end if;
            end if;
            
            -- Synchronise the serial input
            serial_in_safe <= serial_in_meta;
            serial_in_meta <= serial_in;
        end if;
    end process;

end Behavioral;

basys3.ucf

 
## Clock signal
set_property PACKAGE_PIN W5 [get_ports clk100]							
	set_property IOSTANDARD LVCMOS33 [get_ports clk100]
	create_clock -add -name sys_clk_pin -period 10.00 -waveform {0 5} [get_ports clk100]
 
## Switches
set_property PACKAGE_PIN V17 [get_ports {switches[0]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {switches[0]}]
set_property PACKAGE_PIN V16 [get_ports {switches[1]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {switches[1]}]
set_property PACKAGE_PIN W16 [get_ports {switches[2]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {switches[2]}]
 

## LEDs
set_property PACKAGE_PIN U16 [get_ports {leds[0]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[0]}]
set_property PACKAGE_PIN E19 [get_ports {leds[1]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[1]}]
set_property PACKAGE_PIN U19 [get_ports {leds[2]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[2]}]
set_property PACKAGE_PIN V19 [get_ports {leds[3]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[3]}]
set_property PACKAGE_PIN W18 [get_ports {leds[4]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[4]}]
set_property PACKAGE_PIN U15 [get_ports {leds[5]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[5]}]
set_property PACKAGE_PIN U14 [get_ports {leds[6]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[6]}]
set_property PACKAGE_PIN V14 [get_ports {leds[7]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[7]}]
set_property PACKAGE_PIN V13 [get_ports {leds[8]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[8]}]
set_property PACKAGE_PIN V3 [get_ports {leds[9]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[9]}]
set_property PACKAGE_PIN W3 [get_ports {leds[10]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[10]}]
set_property PACKAGE_PIN U3 [get_ports {leds[11]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[11]}]
set_property PACKAGE_PIN P3 [get_ports {leds[12]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[12]}]
set_property PACKAGE_PIN N3 [get_ports {leds[13]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[13]}]
set_property PACKAGE_PIN P1 [get_ports {leds[14]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[14]}]
set_property PACKAGE_PIN L1 [get_ports {leds[15]}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {leds[15]}]

##Pmod Header JA
set_property PACKAGE_PIN J1 [get_ports {mpu6050}]					
	set_property IOSTANDARD LVCMOS33 [get_ports {mpu6050}]

Personal tools