Skip to content
This repository has been archived by the owner on Feb 7, 2018. It is now read-only.

Commit

Permalink
Browse files Browse the repository at this point in the history
Basic attitude calc algorithm sketched out
  • Loading branch information
jctemkin committed Dec 4, 2012
1 parent c71f6f4 commit de5db94
Show file tree
Hide file tree
Showing 19 changed files with 1,343 additions and 798 deletions.
172 changes: 172 additions & 0 deletions attitude_calc.vhd
@@ -0,0 +1,172 @@
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;


entity attitude_calc is
port (
clk : in STD_LOGIC;
read_regs : in STD_LOGIC_VECTOR (143 downto 0);
write_en : out STD_LOGIC;
write_addr : out STD_LOGIC_VECTOR (7 downto 0);
write_data : out STD_LOGIC_VECTOR (15 downto 0)
);
end attitude_calc;

architecture Behavioral of attitude_calc is

signal ax_raw: std_logic_vector(15 downto 0);
signal ay_raw: std_logic_vector(15 downto 0);
signal az_raw: std_logic_vector(15 downto 0);

signal ax_offset: std_logic_vector(15 downto 0);
signal ay_offset: std_logic_vector(15 downto 0);
signal az_offset: std_logic_vector(15 downto 0);

signal axc: std_logic_vector(15 downto 0);
signal ayc: std_logic_vector(15 downto 0);
signal azc: std_logic_vector(15 downto 0);

signal acc_pitch: std_logic_vector(15 downto 0);
signal acc_roll: std_logic_vector(15 downto 0);



signal gx_raw: std_logic_vector(15 downto 0);
signal gy_raw: std_logic_vector(15 downto 0);
signal gz_raw: std_logic_vector(15 downto 0);

signal gx_offset: std_logic_vector(15 downto 0);
signal gy_offset: std_logic_vector(15 downto 0);
signal gz_offset: std_logic_vector(15 downto 0);

signal gxc: std_logic_vector(15 downto 0);
signal gyc: std_logic_vector(15 downto 0);
signal gzc: std_logic_vector(15 downto 0);

signal gx_scale: std_logic_vector(15 downto 0);
signal gy_scale: std_logic_vector(15 downto 0);
signal gz_scale: std_logic_vector(15 downto 0);

signal gx: std_logic_vector(15 downto 0);
signal gy: std_logic_vector(15 downto 0);
signal gz: std_logic_vector(15 downto 0);


signal Dt: std_logic_vector(15 downto 0);

signal gxt: std_logic_vector(15 downto 0);
signal gyt: std_logic_vector(15 downto 0);


signal last_pitch: std_logic_vector(15 downto 0);
signal last_roll: std_logic_vector(15 downto 0);

signal gx_int: std_logic_vector(15 downto 0);
signal gy_int: std_logic_vector(15 downto 0);


signal Kd: std_logic_vector(15 downto 0);
signal Kp: std_logic_vector(15 downto 0);

signal p_pitch: std_logic_vector(15 downto 0);
signal d_pitch: std_logic_vector(15 downto 0);

signal p_roll: std_logic_vector(15 downto 0);
signal d_roll: std_logic_vector(15 downto 0);

signal pitch: std_logic_vector(15 downto 0);
signal roll: std_logic_vector(15 downto 0);

signal atan_x: std_logic_vector(15 downto 0); --X and Y must have same binary point location.
signal atan_y: std_logic_vector(15 downto 0);
signal atan_out: std_logic_vector(15 downto 0); --Output in fixed-point 2's complement radians, with 3 integer places.
signal atan_rdy: std_logic;


component arctan
port (
x_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
y_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
phase_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0);
rdy : OUT STD_LOGIC;
clk : IN STD_LOGIC
);
end component;

begin


--
--Add stage: add constants to raw sensor values
gxc <= gx_raw + gx_offset;
gyc <= gy_raw + gy_offset;

--Multiply stage: scale gyro readings
gx <= gxc * gx_scale;
gy <= gyc * gy_scale;



--Multiply stage: multiply gyro readings by time
gxt <= gx_scale * Dt;
gyt <= gy_scale * Dt;

--Add stage: add gyro*time readings to last recorded attitude
gx_int <= gxt + last_pitch;
gy_int <= gxy + last_roll;

--Multiply stage: Multiply by complimentary filter constants
p_pitch <= gx_int * Kp;
p_roll <= gy_int * Kp;


--=========================--

--Add stage: add constants to raw sensor values
axc <= ax_raw + ax_offset;
ayc <= ay_raw + ay_offset;
azc <= az_raw + az_offset;

--Trig stage: take arctangent of offset sensor values
atan_x1 <= axc;
atan_y1 <= azc;
atan_x2 <= ayc;
atan_y2 <= azc;

--Multiply stage: Multiply by complimentary filter constants
d_pitch <= atan_out1 * Kd;
d_roll <= atan_out2 * Kd;


--=========================--

--Add stage: Add P and D terms to obtain final pitch and roll
pitch <= p_pitch + d_pitch;
roll <= p_roll + d_roll;



arctan_i1: arctan
port map(
x_in => atan_x1,
y_in => atan_y1,
phase_out => atan_out1,
rdy => atan_rdy1,
clk => clk
);

arctan_i2: arctan
port map(
x_in => atan_x2,
y_in => atan_y2,
phase_out => atan_out2,
rdy => atan_rdy2,
clk => clk
);




end Behavioral;

25 changes: 25 additions & 0 deletions ipcore_dir/arctan.asy
@@ -0,0 +1,25 @@
Version 4
SymbolType BLOCK
TEXT 32 32 LEFT 4 arctan
RECTANGLE Normal 32 32 544 352
LINE Wide 0 80 32 80
PIN 0 80 LEFT 36
PINATTR PinName x_in[15:0]
PINATTR Polarity IN
LINE Wide 0 112 32 112
PIN 0 112 LEFT 36
PINATTR PinName y_in[15:0]
PINATTR Polarity IN
LINE Normal 0 304 32 304
PIN 0 304 LEFT 36
PINATTR PinName clk
PINATTR Polarity IN
LINE Wide 576 144 544 144
PIN 576 144 RIGHT 36
PINATTR PinName phase_out[15:0]
PINATTR Polarity OUT
LINE Normal 576 240 544 240
PIN 576 240 RIGHT 36
PINATTR PinName rdy
PINATTR Polarity OUT

32 changes: 32 additions & 0 deletions ipcore_dir/arctan.gise
@@ -0,0 +1,32 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<generated_project xmlns="http://www.xilinx.com/XMLSchema" xmlns:xil_pn="http://www.xilinx.com/XMLSchema">

<!-- -->

<!-- For tool use only. Do not edit. -->

<!-- -->

<!-- ProjectNavigator created generated project file. -->

<!-- For use in tracking generated file and other information -->

<!-- allowing preservation of process status. -->

<!-- -->

<!-- Copyright (c) 1995-2011 Xilinx, Inc. All rights reserved. -->

<version xmlns="http://www.xilinx.com/XMLSchema">11.1</version>

<sourceproject xmlns="http://www.xilinx.com/XMLSchema" xil_pn:fileType="FILE_XISE" xil_pn:name="arctan.xise"/>

<files xmlns="http://www.xilinx.com/XMLSchema">
<file xil_pn:fileType="FILE_ASY" xil_pn:name="arctan.asy" xil_pn:origination="imported"/>
<file xil_pn:fileType="FILE_SYMBOL" xil_pn:name="arctan.sym" xil_pn:origination="imported"/>
<file xil_pn:fileType="FILE_VHO" xil_pn:name="arctan.vho" xil_pn:origination="imported"/>
</files>

<transforms xmlns="http://www.xilinx.com/XMLSchema"/>

</generated_project>
3 changes: 3 additions & 0 deletions ipcore_dir/arctan.ngc

Large diffs are not rendered by default.

24 changes: 24 additions & 0 deletions ipcore_dir/arctan.sym
@@ -0,0 +1,24 @@
<?xml version="1.0" encoding="UTF-8"?>
<symbol version="7" name="arctan">
<symboltype>BLOCK</symboltype>
<timestamp>2012-12-4T3:39:26</timestamp>
<pin polarity="Input" x="0" y="80" name="x_in[15:0]" />
<pin polarity="Input" x="0" y="112" name="y_in[15:0]" />
<pin polarity="Input" x="0" y="304" name="clk" />
<pin polarity="Output" x="576" y="144" name="phase_out[15:0]" />
<pin polarity="Output" x="576" y="240" name="rdy" />
<graph>
<text style="fontsize:40;fontname:Arial" x="32" y="32">arctan</text>
<rect width="512" x="32" y="32" height="320" />
<line x2="32" y1="80" y2="80" style="linewidth:W" x1="0" />
<attrtext style="fontsize:24;fontname:Arial" attrname="PinName" x="36" y="80" type="pin x_in[15:0]" />
<line x2="32" y1="112" y2="112" style="linewidth:W" x1="0" />
<attrtext style="fontsize:24;fontname:Arial" attrname="PinName" x="36" y="112" type="pin y_in[15:0]" />
<line x2="32" y1="304" y2="304" x1="0" />
<attrtext style="fontsize:24;fontname:Arial" attrname="PinName" x="36" y="304" type="pin clk" />
<line x2="544" y1="144" y2="144" style="linewidth:W" x1="576" />
<attrtext style="alignment:RIGHT;fontsize:24;fontname:Arial" attrname="PinName" x="540" y="144" type="pin phase_out[15:0]" />
<line x2="544" y1="240" y2="240" x1="576" />
<attrtext style="alignment:RIGHT;fontsize:24;fontname:Arial" attrname="PinName" x="540" y="240" type="pin rdy" />
</graph>
</symbol>
110 changes: 110 additions & 0 deletions ipcore_dir/arctan.vhd
@@ -0,0 +1,110 @@
--------------------------------------------------------------------------------
-- This file is owned and controlled by Xilinx and must be used solely --
-- for design, simulation, implementation and creation of design files --
-- limited to Xilinx devices or technologies. Use with non-Xilinx --
-- devices or technologies is expressly prohibited and immediately --
-- terminates your license. --
-- --
-- XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" SOLELY --
-- FOR USE IN DEVELOPING PROGRAMS AND SOLUTIONS FOR XILINX DEVICES. BY --
-- PROVIDING THIS DESIGN, CODE, OR INFORMATION AS ONE POSSIBLE --
-- IMPLEMENTATION OF THIS FEATURE, APPLICATION OR STANDARD, XILINX IS --
-- MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION IS FREE FROM ANY --
-- CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE FOR OBTAINING ANY --
-- RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION. XILINX EXPRESSLY --
-- DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO THE ADEQUACY OF THE --
-- IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OR --
-- REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE FROM CLAIMS OF --
-- INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A --
-- PARTICULAR PURPOSE. --
-- --
-- Xilinx products are not intended for use in life support appliances, --
-- devices, or systems. Use in such applications are expressly --
-- prohibited. --
-- --
-- (c) Copyright 1995-2012 Xilinx, Inc. --
-- All rights reserved. --
--------------------------------------------------------------------------------
--------------------------------------------------------------------------------
-- You must compile the wrapper file arctan.vhd when simulating
-- the core, arctan. When compiling the wrapper file, be sure to
-- reference the XilinxCoreLib VHDL simulation library. For detailed
-- instructions, please refer to the "CORE Generator Help".

-- The synthesis directives "translate_off/translate_on" specified
-- below are supported by Xilinx, Mentor Graphics and Synplicity
-- synthesis tools. Ensure they are correct for your synthesis tool(s).

LIBRARY ieee;
USE ieee.std_logic_1164.ALL;
-- synthesis translate_off
LIBRARY XilinxCoreLib;
-- synthesis translate_on
ENTITY arctan IS
PORT (
x_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
y_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
phase_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0);
rdy : OUT STD_LOGIC;
clk : IN STD_LOGIC
);
END arctan;

ARCHITECTURE arctan_a OF arctan IS
-- synthesis translate_off
COMPONENT wrapped_arctan
PORT (
x_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
y_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
phase_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0);
rdy : OUT STD_LOGIC;
clk : IN STD_LOGIC
);
END COMPONENT;

-- Configuration specification
FOR ALL : wrapped_arctan USE ENTITY XilinxCoreLib.cordic_v4_0(behavioral)
GENERIC MAP (
c_architecture => 2,
c_coarse_rotate => 1,
c_cordic_function => 3,
c_data_format => 0,
c_family => "spartan6",
c_has_ce => 0,
c_has_clk => 1,
c_has_nd => 0,
c_has_phase_in => 0,
c_has_phase_out => 1,
c_has_rdy => 1,
c_has_rfd => 0,
c_has_sclr => 0,
c_has_x_in => 1,
c_has_x_out => 0,
c_has_y_in => 1,
c_has_y_out => 0,
c_input_width => 16,
c_iterations => 0,
c_output_width => 16,
c_phase_format => 0,
c_pipeline_mode => -2,
c_precision => 0,
c_reg_inputs => 1,
c_reg_outputs => 1,
c_round_mode => 0,
c_scale_comp => 0,
c_xdevicefamily => "spartan6"
);
-- synthesis translate_on
BEGIN
-- synthesis translate_off
U0 : wrapped_arctan
PORT MAP (
x_in => x_in,
y_in => y_in,
phase_out => phase_out,
rdy => rdy,
clk => clk
);
-- synthesis translate_on

END arctan_a;

0 comments on commit de5db94

Please sign in to comment.