This repository has been archived by the owner on Feb 7, 2018. It is now read-only.
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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
Showing
19 changed files
with
1,343 additions
and
798 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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; |
Oops, something went wrong.