data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v
1: b'%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:27: Cannot find include file: common_defines.v\n`include "common_defines.v" \n         ^~~~~~~~~~~~~~~~~~\n        ... Looked in:\n             data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source,data/full_repos/permissive/114714167/common_defines.v\n             data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source,data/full_repos/permissive/114714167/common_defines.v.v\n             data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source,data/full_repos/permissive/114714167/common_defines.v.sv\n             common_defines.v\n             common_defines.v.v\n             common_defines.v.sv\n             obj_dir/common_defines.v\n             obj_dir/common_defines.v.v\n             obj_dir/common_defines.v.sv\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:28: Cannot find include file: pid_parameters.v\n`include "pid_parameters.v" \n         ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:36: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0]body_yaw_angle_target,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:37: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_out,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:38: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output wire [`RATE_BIT_WIDTH-1:0] debug_out,\n                 ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:42: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_pwm_value_input,\n                 ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:43: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    input  wire [`REC_VAL_BIT_WIDTH-1:0] yaw_pwm_value_input,\n                 ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:44: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_imu,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:54: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking_temp;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:55: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:56: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    old_body_yaw_angle_target_tracking;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:57: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_throttle_pwm_value;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:58: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_pwm_value;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:59: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_stick_neutral_value;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:60: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_stick_normalized;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:61: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_stick_normalized;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:62: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_imu;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:63: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_imu;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:64: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error_temp;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:65: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:66: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_error;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:70: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target_tracking;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:71: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:72: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:73: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error_out;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:88: Define or directive not defined: \'`FALSE\'\n    reg start_flag = `FALSE;\n                     ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:88: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n    reg start_flag = `FALSE;\n                           ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:113: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    localparam signed [`RATE_BIT_WIDTH-1:0] YAW_ERROR_MAX_CHANGE = 200;  \n                       ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:119: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    localparam signed [`RATE_BIT_WIDTH-1:0]\n                       ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:146: Define or directive not defined: \'`FALSE\'\n            complete_signal <= `FALSE;\n                               ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:146: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            complete_signal <= `FALSE;\n                                     ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:147: Define or directive not defined: \'`FALSE\'\n            active_signal   <= `FALSE;\n                               ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:147: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            active_signal   <= `FALSE;\n                                     ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:160: Define or directive not defined: \'`FALSE\'\n                if (imu_ready == `FALSE)\n                                 ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:160: syntax error, unexpected \')\', expecting TYPE-IDENTIFIER\n                if (imu_ready == `FALSE)\n                                       ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:218: Define or directive not defined: \'`FALSE\'\n        next_yaw_stick_out_of_neutral_window        = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:218: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        next_yaw_stick_out_of_neutral_window        = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:219: Define or directive not defined: \'`FALSE\'\n        trigger_body_yaw_angle_target_tracking      = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:219: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_body_yaw_angle_target_tracking      = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:220: Define or directive not defined: \'`FALSE\'\n        trigger_body_yaw_angle_target_tracking_temp = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:220: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_body_yaw_angle_target_tracking_temp = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:221: Define or directive not defined: \'`FALSE\'\n        trigger_yaw_stick_normalized                = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:221: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_yaw_stick_normalized                = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:222: Define or directive not defined: \'`FALSE\'\n        trigger_body_yaw_angle_target               = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:222: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_body_yaw_angle_target               = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:223: Define or directive not defined: \'`FALSE\'\n        trigger_yaw_angle_error                     = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:223: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_yaw_angle_error                     = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:224: Define or directive not defined: \'`FALSE\'\n        trigger_yaw_angle_error_out                 = `FALSE;\n                                                      ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:224: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n        trigger_yaw_angle_error_out                 = `FALSE;\n                                                            ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/yaw_angle_accumulator.v:225: Define or directive not defined: \'`FALSE\'\n        trigger_yaw_stick_neutral_value             = `FALSE;\n                                                      ^~~~~~\n%Error: Exiting due to too many errors encountered; --error-limit=50\n'
module yaw_angle_accumulator (
    output reg  signed [`RATE_BIT_WIDTH-1:0]body_yaw_angle_target,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_out,
    output wire [`RATE_BIT_WIDTH-1:0] debug_out,
    output reg  active_signal,
    output reg  complete_signal,
    output reg yaw_stick_out_of_neutral_window,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_pwm_value_input,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] yaw_pwm_value_input,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_imu,
    input  wire yaac_enable_n,
    input  wire [2:0] switch_a,

    input  wire start_signal,
    input  wire imu_ready,
    input  wire resetn,
    input  wire us_clk
    );
    
    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking_temp;
    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_throttle_pwm_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_pwm_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_stick_neutral_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_stick_normalized;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_stick_normalized;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_imu;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_imu;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error_temp;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_error;
    reg old_yaw_stick_out_of_neutral_window;
    

    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error_out;

    reg next_yaw_stick_out_of_neutral_window;
    reg next_complete_signal;
    reg next_active_signal;
    reg trigger_body_yaw_angle_target_tracking;
    reg trigger_yaw_stick_normalized;
    reg trigger_body_yaw_angle_target;
    reg trigger_yaw_angle_error;
    reg trigger_yaw_angle_error_out;
    reg trigger_yaw_stick_neutral_value;
    reg trigger_latch_new_values;
    reg trigger_body_yaw_angle_target_tracking_temp;
    reg trigger_yaw_angle_error_temp;
    reg trigger_yaw_stick_out_of_neutral_window;
    reg start_flag = `FALSE;


    `define NUM_STATES 15


    localparam [`NUM_STATES-1:0]

        STATE_INIT                    = `NUM_STATES'b1<<0,
        STATE_WAITING                 = `NUM_STATES'b1<<1,
        STATE_LATCH_VALUES            = `NUM_STATES'b1<<2,
        STATE_GET_NEUTRAL_YAW         = `NUM_STATES'b1<<3,
        STATE_NORMALIZE_STICK         = `NUM_STATES'b1<<4,
        STATE_TRACK_STICK             = `NUM_STATES'b1<<5,
        STATE_CALC_TRACK_ANGLE_TEMP   = `NUM_STATES'b1<<6,
        STATE_CALC_TRACK_ANGLE        = `NUM_STATES'b1<<7,
        STATE_LIMIT_ERROR_ACCUM       = `NUM_STATES'b1<<8,
        STATE_SCALE_TRACK_ANGLE       = `NUM_STATES'b1<<9,
        STATE_CALC_ANGLE_ERROR_TEMP   = `NUM_STATES'b1<<10,
        STATE_CALC_ANGLE_ERROR        = `NUM_STATES'b1<<11,
        STATE_LIMIT_ERROR_CHANGE      = `NUM_STATES'b1<<12,
        STATE_LIMIT_ERROR             = `NUM_STATES'b1<<13,
        STATE_COMPLETE                = `NUM_STATES'b1<<14;
        
    localparam SCALE_BITS = 2;
    localparam signed [`RATE_BIT_WIDTH-1:0] YAW_ERROR_MAX_CHANGE = 200;





    localparam signed [`RATE_BIT_WIDTH-1:0]
        ANGLE_360_DEG = 5760, 
        ANGLE_270_DEG = 4320, 
        ANGLE_180_DEG = 2880, 
        ANGLE_90_DEG  = 1440,
        ANGLE_10_DEG  = 160,
        ANGLE_0_DEG   = 0,
        
        ANGLE_360_DEG_SCALED = (ANGLE_360_DEG <<<SCALE_BITS), 
        ANGLE_270_DEG_SCALED = (ANGLE_270_DEG <<<SCALE_BITS), 
        ANGLE_180_DEG_SCALED = (ANGLE_180_DEG <<<SCALE_BITS), 
        ANGLE_90_DEG_SCALED  = (ANGLE_90_DEG  <<<SCALE_BITS),
        ANGLE_10_DEG_SCALED  = (ANGLE_10_DEG  <<<SCALE_BITS),
        ANGLE_0_DEG_SCALED   = ANGLE_0_DEG;
    

    reg [`NUM_STATES-1:0] state, next_state;    
    



    assign debug_out = yaw_angle_error_temp;


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            state           <= STATE_INIT;
            complete_signal <= `FALSE;
            active_signal   <= `FALSE;
        end
        else begin
            state           <= next_state;
            complete_signal <= next_complete_signal;
            active_signal   <= next_active_signal;
        end
    end


    always @(state or start_flag or imu_ready) begin
        case(state)
            STATE_INIT: begin
                if (imu_ready == `FALSE)
                    next_state = STATE_INIT;
                else
                    next_state = STATE_WAITING;
            end
            STATE_WAITING: begin
                if(start_flag)
                    next_state = STATE_LATCH_VALUES;
                else
                    next_state = STATE_WAITING;
            end
            STATE_LATCH_VALUES: begin
                next_state = STATE_GET_NEUTRAL_YAW;
            end
            STATE_GET_NEUTRAL_YAW: begin
                next_state = STATE_NORMALIZE_STICK;
            end
            STATE_NORMALIZE_STICK: begin
                next_state = STATE_TRACK_STICK;
            end
            STATE_TRACK_STICK: begin
                next_state = STATE_CALC_TRACK_ANGLE_TEMP;
            end
            STATE_CALC_TRACK_ANGLE_TEMP: begin
                next_state = STATE_CALC_TRACK_ANGLE;
            end
            STATE_CALC_TRACK_ANGLE: begin
                next_state = STATE_LIMIT_ERROR_ACCUM;
            end
            STATE_LIMIT_ERROR_ACCUM: begin
                next_state = STATE_SCALE_TRACK_ANGLE;
            end
            STATE_SCALE_TRACK_ANGLE: begin
                next_state = STATE_CALC_ANGLE_ERROR_TEMP;
            end
            STATE_CALC_ANGLE_ERROR_TEMP: begin
                next_state = STATE_CALC_ANGLE_ERROR;
            end
            STATE_CALC_ANGLE_ERROR: begin
                next_state = STATE_LIMIT_ERROR_CHANGE;
            end
            STATE_LIMIT_ERROR_CHANGE: begin
                next_state = STATE_LIMIT_ERROR;
            end
            STATE_LIMIT_ERROR: begin
                next_state = STATE_COMPLETE;
            end
            STATE_COMPLETE: begin
                next_state = STATE_WAITING;
            end
            default: begin
                next_state = STATE_INIT;
            end
        endcase
    end


    always @(*) begin
        next_yaw_stick_out_of_neutral_window        = `FALSE;
        trigger_body_yaw_angle_target_tracking      = `FALSE;
        trigger_body_yaw_angle_target_tracking_temp = `FALSE;
        trigger_yaw_stick_normalized                = `FALSE;
        trigger_body_yaw_angle_target               = `FALSE;
        trigger_yaw_angle_error                     = `FALSE;
        trigger_yaw_angle_error_out                 = `FALSE;
        trigger_yaw_stick_neutral_value             = `FALSE;
        trigger_latch_new_values                    = `FALSE;
        trigger_yaw_angle_error_temp                = `FALSE;
        trigger_yaw_stick_out_of_neutral_window     = `FALSE;
        if(!resetn) begin

            next_complete_signal                     = `FALSE;
            next_active_signal                       = `FALSE;
            next_body_yaw_angle_target               = `ALL_ZERO_2BYTE;
            next_body_yaw_angle_target_tracking      = `ALL_ZERO_2BYTE;
            next_yaw_angle_error                     = `ALL_ZERO_2BYTE;
            next_yaw_angle_error_out                 = `ALL_ZERO_2BYTE;
        end
        else begin
            case(state)
                STATE_INIT: begin

                    next_complete_signal                        = `TRUE;
                    next_active_signal                          = `FALSE;
                    next_body_yaw_angle_target                  = `ALL_ZERO_2BYTE;
                    next_body_yaw_angle_target_tracking         = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error                        = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error_out                    = `ALL_ZERO_2BYTE;
                    next_yaw_stick_out_of_neutral_window        = `FALSE;
                    trigger_body_yaw_angle_target_tracking      = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                    trigger_yaw_stick_normalized                = `TRUE;
                    trigger_body_yaw_angle_target               = `TRUE;
                    trigger_yaw_angle_error                     = `TRUE;
                    trigger_yaw_angle_error_out                 = `TRUE;
                    trigger_yaw_stick_neutral_value             = `TRUE;
                    trigger_latch_new_values                    = `TRUE;
                    trigger_yaw_angle_error_temp                = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window     = `TRUE;
                end
                STATE_WAITING: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `FALSE;
                end
                STATE_LATCH_VALUES: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_latch_new_values = `TRUE;
                end
                STATE_GET_NEUTRAL_YAW: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    if (latched_throttle_pwm_value < 10)
                        trigger_yaw_stick_neutral_value = `TRUE;

                end
                STATE_NORMALIZE_STICK: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_stick_normalized = `TRUE;
                end
                STATE_TRACK_STICK: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window   = `TRUE;

                    if (latched_throttle_pwm_value < 10)
                        next_yaw_stick_out_of_neutral_window = `FALSE;


                    else if ((latched_yaw_stick_normalized  < 4 ) && (latched_yaw_stick_normalized > -4 ))
                        next_yaw_stick_out_of_neutral_window = `FALSE;

                    else
                        next_yaw_stick_out_of_neutral_window = `TRUE;
                end
                STATE_CALC_TRACK_ANGLE_TEMP: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                end
                STATE_CALC_TRACK_ANGLE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target_tracking   = `TRUE;
                    

                    if (latched_throttle_pwm_value < 10)
                        next_body_yaw_angle_target_tracking    =(latched_yaw_angle_imu<<<SCALE_BITS);

                    else if (yaw_stick_out_of_neutral_window == `FALSE) begin
                        next_body_yaw_angle_target_tracking = body_yaw_angle_target_tracking;
                    end

                    else begin

                        if (body_yaw_angle_target_tracking_temp > ANGLE_360_DEG_SCALED )
                            next_body_yaw_angle_target_tracking    = (body_yaw_angle_target_tracking_temp - ANGLE_360_DEG_SCALED);

                        else if (body_yaw_angle_target_tracking_temp < ANGLE_0_DEG_SCALED )
                            next_body_yaw_angle_target_tracking    = (body_yaw_angle_target_tracking_temp + ANGLE_360_DEG_SCALED);
                        else
                            next_body_yaw_angle_target_tracking    = body_yaw_angle_target_tracking_temp;
                    end
                end
                STATE_LIMIT_ERROR_ACCUM: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;

                    if( ( yaw_angle_error >= ANGLE_90_DEG) && (latched_yaw_stick_normalized > 0) ) begin
                        next_body_yaw_angle_target_tracking      = old_body_yaw_angle_target_tracking;
                        trigger_body_yaw_angle_target_tracking   = `TRUE;
                    end
                    else if (( yaw_angle_error <= -ANGLE_90_DEG) && (latched_yaw_stick_normalized < 0)) begin
                        next_body_yaw_angle_target_tracking      = old_body_yaw_angle_target_tracking;
                        trigger_body_yaw_angle_target_tracking   = `TRUE;
                    end
                end
                STATE_SCALE_TRACK_ANGLE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target = `TRUE;
                    

                    if (latched_throttle_pwm_value < 10)
                        next_body_yaw_angle_target = latched_yaw_angle_imu;
                    else
                        next_body_yaw_angle_target = (body_yaw_angle_target_tracking>>>SCALE_BITS);
                end
                STATE_CALC_ANGLE_ERROR_TEMP: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error_temp = `TRUE;
                end
                STATE_CALC_ANGLE_ERROR: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error  = `TRUE;

                    if (latched_throttle_pwm_value < 10) begin
                        next_yaw_angle_error = 0;
                    end
                    else if((body_yaw_angle_target > ANGLE_270_DEG) && (latched_yaw_angle_imu <  ANGLE_90_DEG)) begin
                        next_yaw_angle_error = -(ANGLE_360_DEG - body_yaw_angle_target + latched_yaw_angle_imu);
                    end            
                    else if((latched_yaw_angle_imu > ANGLE_270_DEG) && (body_yaw_angle_target <  ANGLE_90_DEG)) begin
                        next_yaw_angle_error = (ANGLE_360_DEG - latched_yaw_angle_imu + body_yaw_angle_target);
                    end

                    else begin
                        next_yaw_angle_error = yaw_angle_error_temp;
                    end
                end
                STATE_LIMIT_ERROR_CHANGE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error  = `TRUE;

                    if ((old_yaw_angle_error >= ANGLE_90_DEG) && (yaw_angle_error <= -ANGLE_90_DEG))
                        next_yaw_angle_error = ANGLE_90_DEG;

                    else if ((old_yaw_angle_error <= -ANGLE_90_DEG) && (yaw_angle_error >= ANGLE_90_DEG))
                        next_yaw_angle_error = -ANGLE_90_DEG;
`ifdef USE_ERROR_RATE_LIMIT

                    else if (yaw_angle_error < (old_yaw_angle_error - YAW_ERROR_MAX_CHANGE))
                        next_yaw_angle_error = old_yaw_angle_error - YAW_ERROR_MAX_CHANGE;

                    else if (yaw_angle_error > (old_yaw_angle_error + YAW_ERROR_MAX_CHANGE))
                        next_yaw_angle_error = old_yaw_angle_error + YAW_ERROR_MAX_CHANGE;
`endif

                    else
                        next_yaw_angle_error = yaw_angle_error;
                end

                STATE_LIMIT_ERROR: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error_out = `TRUE;

                    if(yaac_enable_n) begin
                        next_body_yaw_angle_target = latched_yaw_pwm_value;
                        next_yaw_angle_error_out = 0;
                    end
`ifdef USE_ERROR_VALUE_LIMIT
                    else if (yaw_angle_error >  ANGLE_90_DEG)
                        next_yaw_angle_error_out =  ANGLE_90_DEG;
                    else if (yaw_angle_error < -ANGLE_90_DEG)
                        next_yaw_angle_error_out = -ANGLE_90_DEG;
`endif
                    else
                        next_yaw_angle_error_out = yaw_angle_error;
                end
                STATE_COMPLETE: begin
                    next_complete_signal = `TRUE;
                    next_active_signal   = `FALSE;    

                end
                default: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `FALSE;
                    next_body_yaw_angle_target                  = `ALL_ZERO_2BYTE;
                    next_body_yaw_angle_target_tracking         = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error                        = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error_out                    = `ALL_ZERO_2BYTE;
                    next_yaw_stick_out_of_neutral_window        = `FALSE;
                    trigger_body_yaw_angle_target_tracking      = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                    trigger_yaw_stick_normalized                = `TRUE;
                    trigger_body_yaw_angle_target               = `TRUE;
                    trigger_yaw_angle_error                     = `TRUE;
                    trigger_yaw_angle_error_out                 = `TRUE;
                    trigger_yaw_stick_neutral_value             = `TRUE;
                    trigger_latch_new_values                    = `TRUE;
                    trigger_yaw_angle_error_temp                = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window     = `TRUE;
                end
            endcase
        end
    end

    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn)
            start_flag         <= `FALSE;
        else if(start_signal && !start_flag)
            start_flag         <= `TRUE;
        else if(!start_signal && start_flag) begin
            if(state != STATE_WAITING)
                start_flag     <= `FALSE;
        end
        else
            start_flag         <= start_flag;
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target_tracking_temp     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target_tracking_temp == `TRUE)
                body_yaw_angle_target_tracking_temp <= (body_yaw_angle_target_tracking + latched_yaw_stick_normalized);
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target_tracking     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target_tracking == `TRUE)
                body_yaw_angle_target_tracking <= next_body_yaw_angle_target_tracking;
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            latched_yaw_stick_normalized         <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_normalized == `TRUE) begin
                if (switch_a[0])
                    latched_yaw_stick_normalized <= (latched_yaw_pwm_value - yaw_stick_neutral_value)*`YAW_ACCUMULATOR_INPUT_MULTIPLIER;
                else
                    latched_yaw_stick_normalized <= (latched_yaw_pwm_value - yaw_stick_neutral_value)>>>1;
                
            end
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_stick_neutral_value     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_neutral_value == `TRUE)
                yaw_stick_neutral_value <= latched_yaw_pwm_value;
        end
    end
        

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target == `TRUE)
                body_yaw_angle_target <= next_body_yaw_angle_target;
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_angle_error_temp     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_angle_error_temp == `TRUE)
                yaw_angle_error_temp <= (body_yaw_angle_target - latched_yaw_angle_imu);
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_angle_error     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_angle_error == `TRUE)
                yaw_angle_error <= next_yaw_angle_error;
        end
    end



    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_stick_out_of_neutral_window     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_out_of_neutral_window == `TRUE)
                yaw_stick_out_of_neutral_window <= next_yaw_stick_out_of_neutral_window;
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            old_yaw_angle_imu                   <= `ALL_ZERO_2BYTE;
            old_yaw_angle_error                 <= `ALL_ZERO_2BYTE;
            old_yaw_stick_normalized            <= `ALL_ZERO_2BYTE;
            old_body_yaw_angle_target_tracking  <= `ALL_ZERO_2BYTE;
            yaw_angle_error_out                 <= `ALL_ZERO_2BYTE;
            old_yaw_stick_out_of_neutral_window <= `FALSE;
        end
        else begin
            if(trigger_yaw_angle_error_out == `TRUE) begin
                old_yaw_angle_imu                   <= latched_yaw_angle_imu;
                old_yaw_angle_error                 <= yaw_angle_error;
                old_yaw_stick_normalized            <= latched_yaw_stick_normalized;
                old_body_yaw_angle_target_tracking  <= body_yaw_angle_target_tracking;
                yaw_angle_error_out                 <= next_yaw_angle_error_out;
                old_yaw_stick_out_of_neutral_window <= yaw_stick_out_of_neutral_window;
            end
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            latched_throttle_pwm_value <= `ALL_ZERO_2BYTE;
            latched_yaw_pwm_value      <= `ALL_ZERO_2BYTE;
            latched_yaw_angle_imu      <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_latch_new_values == `TRUE) begin
                latched_throttle_pwm_value <= {8'd0, throttle_pwm_value_input};
                latched_yaw_pwm_value      <= {8'd0, yaw_pwm_value_input};
                if ( (yaw_angle_imu > ANGLE_360_DEG) || (yaw_angle_imu < ANGLE_0_DEG) )
                    latched_yaw_angle_imu <= old_yaw_angle_imu;
                else
                    latched_yaw_angle_imu <= yaw_angle_imu;
            end
        end
    end



endmodule
module yaw_angle_accumulator (
    output reg  signed [`RATE_BIT_WIDTH-1:0]body_yaw_angle_target,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_out,
    output wire [`RATE_BIT_WIDTH-1:0] debug_out,
    output reg  active_signal,
    output reg  complete_signal,
    output reg yaw_stick_out_of_neutral_window,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_pwm_value_input,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] yaw_pwm_value_input,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_imu,
    input  wire yaac_enable_n,
    input  wire [2:0] switch_a,

    input  wire start_signal,
    input  wire imu_ready,
    input  wire resetn,
    input  wire us_clk
    );

    
    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking_temp;
    reg signed [`RATE_BIT_WIDTH-1:0]    body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_throttle_pwm_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_pwm_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_stick_neutral_value;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_stick_normalized;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_stick_normalized;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_imu;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_imu;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error_temp;
    reg signed [`RATE_BIT_WIDTH-1:0]    yaw_angle_error;
    reg signed [`RATE_BIT_WIDTH-1:0]    old_yaw_angle_error;
    reg old_yaw_stick_out_of_neutral_window;
    

    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target_tracking;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_body_yaw_angle_target;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error;
    reg signed [`RATE_BIT_WIDTH-1:0]    next_yaw_angle_error_out;

    reg next_yaw_stick_out_of_neutral_window;
    reg next_complete_signal;
    reg next_active_signal;
    reg trigger_body_yaw_angle_target_tracking;
    reg trigger_yaw_stick_normalized;
    reg trigger_body_yaw_angle_target;
    reg trigger_yaw_angle_error;
    reg trigger_yaw_angle_error_out;
    reg trigger_yaw_stick_neutral_value;
    reg trigger_latch_new_values;
    reg trigger_body_yaw_angle_target_tracking_temp;
    reg trigger_yaw_angle_error_temp;
    reg trigger_yaw_stick_out_of_neutral_window;
    reg start_flag = `FALSE;


    `define NUM_STATES 15


    localparam [`NUM_STATES-1:0]

        STATE_INIT                    = `NUM_STATES'b1<<0,
        STATE_WAITING                 = `NUM_STATES'b1<<1,
        STATE_LATCH_VALUES            = `NUM_STATES'b1<<2,
        STATE_GET_NEUTRAL_YAW         = `NUM_STATES'b1<<3,
        STATE_NORMALIZE_STICK         = `NUM_STATES'b1<<4,
        STATE_TRACK_STICK             = `NUM_STATES'b1<<5,
        STATE_CALC_TRACK_ANGLE_TEMP   = `NUM_STATES'b1<<6,
        STATE_CALC_TRACK_ANGLE        = `NUM_STATES'b1<<7,
        STATE_LIMIT_ERROR_ACCUM       = `NUM_STATES'b1<<8,
        STATE_SCALE_TRACK_ANGLE       = `NUM_STATES'b1<<9,
        STATE_CALC_ANGLE_ERROR_TEMP   = `NUM_STATES'b1<<10,
        STATE_CALC_ANGLE_ERROR        = `NUM_STATES'b1<<11,
        STATE_LIMIT_ERROR_CHANGE      = `NUM_STATES'b1<<12,
        STATE_LIMIT_ERROR             = `NUM_STATES'b1<<13,
        STATE_COMPLETE                = `NUM_STATES'b1<<14;
        
    localparam SCALE_BITS = 2;
    localparam signed [`RATE_BIT_WIDTH-1:0] YAW_ERROR_MAX_CHANGE = 200;





    localparam signed [`RATE_BIT_WIDTH-1:0]
        ANGLE_360_DEG = 5760, 
        ANGLE_270_DEG = 4320, 
        ANGLE_180_DEG = 2880, 
        ANGLE_90_DEG  = 1440,
        ANGLE_10_DEG  = 160,
        ANGLE_0_DEG   = 0,
        
        ANGLE_360_DEG_SCALED = (ANGLE_360_DEG <<<SCALE_BITS), 
        ANGLE_270_DEG_SCALED = (ANGLE_270_DEG <<<SCALE_BITS), 
        ANGLE_180_DEG_SCALED = (ANGLE_180_DEG <<<SCALE_BITS), 
        ANGLE_90_DEG_SCALED  = (ANGLE_90_DEG  <<<SCALE_BITS),
        ANGLE_10_DEG_SCALED  = (ANGLE_10_DEG  <<<SCALE_BITS),
        ANGLE_0_DEG_SCALED   = ANGLE_0_DEG;
    

    reg [`NUM_STATES-1:0] state, next_state;    
    



    assign debug_out = yaw_angle_error_temp;


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            state           <= STATE_INIT;
            complete_signal <= `FALSE;
            active_signal   <= `FALSE;
        end
        else begin
            state           <= next_state;
            complete_signal <= next_complete_signal;
            active_signal   <= next_active_signal;
        end
    end


    always @(state or start_flag or imu_ready) begin
        case(state)
            STATE_INIT: begin
                if (imu_ready == `FALSE)
                    next_state = STATE_INIT;
                else
                    next_state = STATE_WAITING;
            end
            STATE_WAITING: begin
                if(start_flag)
                    next_state = STATE_LATCH_VALUES;
                else
                    next_state = STATE_WAITING;
            end
            STATE_LATCH_VALUES: begin
                next_state = STATE_GET_NEUTRAL_YAW;
            end
            STATE_GET_NEUTRAL_YAW: begin
                next_state = STATE_NORMALIZE_STICK;
            end
            STATE_NORMALIZE_STICK: begin
                next_state = STATE_TRACK_STICK;
            end
            STATE_TRACK_STICK: begin
                next_state = STATE_CALC_TRACK_ANGLE_TEMP;
            end
            STATE_CALC_TRACK_ANGLE_TEMP: begin
                next_state = STATE_CALC_TRACK_ANGLE;
            end
            STATE_CALC_TRACK_ANGLE: begin
                next_state = STATE_LIMIT_ERROR_ACCUM;
            end
            STATE_LIMIT_ERROR_ACCUM: begin
                next_state = STATE_SCALE_TRACK_ANGLE;
            end
            STATE_SCALE_TRACK_ANGLE: begin
                next_state = STATE_CALC_ANGLE_ERROR_TEMP;
            end
            STATE_CALC_ANGLE_ERROR_TEMP: begin
                next_state = STATE_CALC_ANGLE_ERROR;
            end
            STATE_CALC_ANGLE_ERROR: begin
                next_state = STATE_LIMIT_ERROR_CHANGE;
            end
            STATE_LIMIT_ERROR_CHANGE: begin
                next_state = STATE_LIMIT_ERROR;
            end
            STATE_LIMIT_ERROR: begin
                next_state = STATE_COMPLETE;
            end
            STATE_COMPLETE: begin
                next_state = STATE_WAITING;
            end
            default: begin
                next_state = STATE_INIT;
            end
        endcase
    end


    always @(*) begin
        next_yaw_stick_out_of_neutral_window        = `FALSE;
        trigger_body_yaw_angle_target_tracking      = `FALSE;
        trigger_body_yaw_angle_target_tracking_temp = `FALSE;
        trigger_yaw_stick_normalized                = `FALSE;
        trigger_body_yaw_angle_target               = `FALSE;
        trigger_yaw_angle_error                     = `FALSE;
        trigger_yaw_angle_error_out                 = `FALSE;
        trigger_yaw_stick_neutral_value             = `FALSE;
        trigger_latch_new_values                    = `FALSE;
        trigger_yaw_angle_error_temp                = `FALSE;
        trigger_yaw_stick_out_of_neutral_window     = `FALSE;
        if(!resetn) begin

            next_complete_signal                     = `FALSE;
            next_active_signal                       = `FALSE;
            next_body_yaw_angle_target               = `ALL_ZERO_2BYTE;
            next_body_yaw_angle_target_tracking      = `ALL_ZERO_2BYTE;
            next_yaw_angle_error                     = `ALL_ZERO_2BYTE;
            next_yaw_angle_error_out                 = `ALL_ZERO_2BYTE;
        end
        else begin
            case(state)
                STATE_INIT: begin

                    next_complete_signal                        = `TRUE;
                    next_active_signal                          = `FALSE;
                    next_body_yaw_angle_target                  = `ALL_ZERO_2BYTE;
                    next_body_yaw_angle_target_tracking         = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error                        = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error_out                    = `ALL_ZERO_2BYTE;
                    next_yaw_stick_out_of_neutral_window        = `FALSE;
                    trigger_body_yaw_angle_target_tracking      = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                    trigger_yaw_stick_normalized                = `TRUE;
                    trigger_body_yaw_angle_target               = `TRUE;
                    trigger_yaw_angle_error                     = `TRUE;
                    trigger_yaw_angle_error_out                 = `TRUE;
                    trigger_yaw_stick_neutral_value             = `TRUE;
                    trigger_latch_new_values                    = `TRUE;
                    trigger_yaw_angle_error_temp                = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window     = `TRUE;
                end
                STATE_WAITING: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `FALSE;
                end
                STATE_LATCH_VALUES: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_latch_new_values = `TRUE;
                end
                STATE_GET_NEUTRAL_YAW: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    if (latched_throttle_pwm_value < 10)
                        trigger_yaw_stick_neutral_value = `TRUE;

                end
                STATE_NORMALIZE_STICK: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_stick_normalized = `TRUE;
                end
                STATE_TRACK_STICK: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window   = `TRUE;

                    if (latched_throttle_pwm_value < 10)
                        next_yaw_stick_out_of_neutral_window = `FALSE;


                    else if ((latched_yaw_stick_normalized  < 4 ) && (latched_yaw_stick_normalized > -4 ))
                        next_yaw_stick_out_of_neutral_window = `FALSE;

                    else
                        next_yaw_stick_out_of_neutral_window = `TRUE;
                end
                STATE_CALC_TRACK_ANGLE_TEMP: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                end
                STATE_CALC_TRACK_ANGLE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target_tracking   = `TRUE;
                    

                    if (latched_throttle_pwm_value < 10)
                        next_body_yaw_angle_target_tracking    =(latched_yaw_angle_imu<<<SCALE_BITS);

                    else if (yaw_stick_out_of_neutral_window == `FALSE) begin
                        next_body_yaw_angle_target_tracking = body_yaw_angle_target_tracking;
                    end

                    else begin

                        if (body_yaw_angle_target_tracking_temp > ANGLE_360_DEG_SCALED )
                            next_body_yaw_angle_target_tracking    = (body_yaw_angle_target_tracking_temp - ANGLE_360_DEG_SCALED);

                        else if (body_yaw_angle_target_tracking_temp < ANGLE_0_DEG_SCALED )
                            next_body_yaw_angle_target_tracking    = (body_yaw_angle_target_tracking_temp + ANGLE_360_DEG_SCALED);
                        else
                            next_body_yaw_angle_target_tracking    = body_yaw_angle_target_tracking_temp;
                    end
                end
                STATE_LIMIT_ERROR_ACCUM: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;

                    if( ( yaw_angle_error >= ANGLE_90_DEG) && (latched_yaw_stick_normalized > 0) ) begin
                        next_body_yaw_angle_target_tracking      = old_body_yaw_angle_target_tracking;
                        trigger_body_yaw_angle_target_tracking   = `TRUE;
                    end
                    else if (( yaw_angle_error <= -ANGLE_90_DEG) && (latched_yaw_stick_normalized < 0)) begin
                        next_body_yaw_angle_target_tracking      = old_body_yaw_angle_target_tracking;
                        trigger_body_yaw_angle_target_tracking   = `TRUE;
                    end
                end
                STATE_SCALE_TRACK_ANGLE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_body_yaw_angle_target = `TRUE;
                    

                    if (latched_throttle_pwm_value < 10)
                        next_body_yaw_angle_target = latched_yaw_angle_imu;
                    else
                        next_body_yaw_angle_target = (body_yaw_angle_target_tracking>>>SCALE_BITS);
                end
                STATE_CALC_ANGLE_ERROR_TEMP: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error_temp = `TRUE;
                end
                STATE_CALC_ANGLE_ERROR: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error  = `TRUE;

                    if (latched_throttle_pwm_value < 10) begin
                        next_yaw_angle_error = 0;
                    end
                    else if((body_yaw_angle_target > ANGLE_270_DEG) && (latched_yaw_angle_imu <  ANGLE_90_DEG)) begin
                        next_yaw_angle_error = -(ANGLE_360_DEG - body_yaw_angle_target + latched_yaw_angle_imu);
                    end            
                    else if((latched_yaw_angle_imu > ANGLE_270_DEG) && (body_yaw_angle_target <  ANGLE_90_DEG)) begin
                        next_yaw_angle_error = (ANGLE_360_DEG - latched_yaw_angle_imu + body_yaw_angle_target);
                    end

                    else begin
                        next_yaw_angle_error = yaw_angle_error_temp;
                    end
                end
                STATE_LIMIT_ERROR_CHANGE: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error  = `TRUE;

                    if ((old_yaw_angle_error >= ANGLE_90_DEG) && (yaw_angle_error <= -ANGLE_90_DEG))
                        next_yaw_angle_error = ANGLE_90_DEG;

                    else if ((old_yaw_angle_error <= -ANGLE_90_DEG) && (yaw_angle_error >= ANGLE_90_DEG))
                        next_yaw_angle_error = -ANGLE_90_DEG;
`ifdef USE_ERROR_RATE_LIMIT

                    else if (yaw_angle_error < (old_yaw_angle_error - YAW_ERROR_MAX_CHANGE))
                        next_yaw_angle_error = old_yaw_angle_error - YAW_ERROR_MAX_CHANGE;

                    else if (yaw_angle_error > (old_yaw_angle_error + YAW_ERROR_MAX_CHANGE))
                        next_yaw_angle_error = old_yaw_angle_error + YAW_ERROR_MAX_CHANGE;
`endif

                    else
                        next_yaw_angle_error = yaw_angle_error;
                end

                STATE_LIMIT_ERROR: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `TRUE;
                    trigger_yaw_angle_error_out = `TRUE;

                    if(yaac_enable_n) begin
                        next_body_yaw_angle_target = latched_yaw_pwm_value;
                        next_yaw_angle_error_out = 0;
                    end
`ifdef USE_ERROR_VALUE_LIMIT
                    else if (yaw_angle_error >  ANGLE_90_DEG)
                        next_yaw_angle_error_out =  ANGLE_90_DEG;
                    else if (yaw_angle_error < -ANGLE_90_DEG)
                        next_yaw_angle_error_out = -ANGLE_90_DEG;
`endif
                    else
                        next_yaw_angle_error_out = yaw_angle_error;
                end
                STATE_COMPLETE: begin
                    next_complete_signal = `TRUE;
                    next_active_signal   = `FALSE;    

                end
                default: begin
                    next_complete_signal = `FALSE;
                    next_active_signal   = `FALSE;
                    next_body_yaw_angle_target                  = `ALL_ZERO_2BYTE;
                    next_body_yaw_angle_target_tracking         = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error                        = `ALL_ZERO_2BYTE;
                    next_yaw_angle_error_out                    = `ALL_ZERO_2BYTE;
                    next_yaw_stick_out_of_neutral_window        = `FALSE;
                    trigger_body_yaw_angle_target_tracking      = `TRUE;
                    trigger_body_yaw_angle_target_tracking_temp = `TRUE;
                    trigger_yaw_stick_normalized                = `TRUE;
                    trigger_body_yaw_angle_target               = `TRUE;
                    trigger_yaw_angle_error                     = `TRUE;
                    trigger_yaw_angle_error_out                 = `TRUE;
                    trigger_yaw_stick_neutral_value             = `TRUE;
                    trigger_latch_new_values                    = `TRUE;
                    trigger_yaw_angle_error_temp                = `TRUE;
                    trigger_yaw_stick_out_of_neutral_window     = `TRUE;
                end
            endcase
        end
    end

    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn)
            start_flag         <= `FALSE;
        else if(start_signal && !start_flag)
            start_flag         <= `TRUE;
        else if(!start_signal && start_flag) begin
            if(state != STATE_WAITING)
                start_flag     <= `FALSE;
        end
        else
            start_flag         <= start_flag;
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target_tracking_temp     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target_tracking_temp == `TRUE)
                body_yaw_angle_target_tracking_temp <= (body_yaw_angle_target_tracking + latched_yaw_stick_normalized);
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target_tracking     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target_tracking == `TRUE)
                body_yaw_angle_target_tracking <= next_body_yaw_angle_target_tracking;
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            latched_yaw_stick_normalized         <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_normalized == `TRUE) begin
                if (switch_a[0])
                    latched_yaw_stick_normalized <= (latched_yaw_pwm_value - yaw_stick_neutral_value)*`YAW_ACCUMULATOR_INPUT_MULTIPLIER;
                else
                    latched_yaw_stick_normalized <= (latched_yaw_pwm_value - yaw_stick_neutral_value)>>>1;
                
            end
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_stick_neutral_value     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_neutral_value == `TRUE)
                yaw_stick_neutral_value <= latched_yaw_pwm_value;
        end
    end
        

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            body_yaw_angle_target     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_body_yaw_angle_target == `TRUE)
                body_yaw_angle_target <= next_body_yaw_angle_target;
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_angle_error_temp     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_angle_error_temp == `TRUE)
                yaw_angle_error_temp <= (body_yaw_angle_target - latched_yaw_angle_imu);
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_angle_error     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_angle_error == `TRUE)
                yaw_angle_error <= next_yaw_angle_error;
        end
    end



    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            yaw_stick_out_of_neutral_window     <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_yaw_stick_out_of_neutral_window == `TRUE)
                yaw_stick_out_of_neutral_window <= next_yaw_stick_out_of_neutral_window;
        end
    end
    

    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            old_yaw_angle_imu                   <= `ALL_ZERO_2BYTE;
            old_yaw_angle_error                 <= `ALL_ZERO_2BYTE;
            old_yaw_stick_normalized            <= `ALL_ZERO_2BYTE;
            old_body_yaw_angle_target_tracking  <= `ALL_ZERO_2BYTE;
            yaw_angle_error_out                 <= `ALL_ZERO_2BYTE;
            old_yaw_stick_out_of_neutral_window <= `FALSE;
        end
        else begin
            if(trigger_yaw_angle_error_out == `TRUE) begin
                old_yaw_angle_imu                   <= latched_yaw_angle_imu;
                old_yaw_angle_error                 <= yaw_angle_error;
                old_yaw_stick_normalized            <= latched_yaw_stick_normalized;
                old_body_yaw_angle_target_tracking  <= body_yaw_angle_target_tracking;
                yaw_angle_error_out                 <= next_yaw_angle_error_out;
                old_yaw_stick_out_of_neutral_window <= yaw_stick_out_of_neutral_window;
            end
        end
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin
            latched_throttle_pwm_value <= `ALL_ZERO_2BYTE;
            latched_yaw_pwm_value      <= `ALL_ZERO_2BYTE;
            latched_yaw_angle_imu      <= `ALL_ZERO_2BYTE;
        end
        else begin
            if(trigger_latch_new_values == `TRUE) begin
                latched_throttle_pwm_value <= {8'd0, throttle_pwm_value_input};
                latched_yaw_pwm_value      <= {8'd0, yaw_pwm_value_input};
                if ( (yaw_angle_imu > ANGLE_360_DEG) || (yaw_angle_imu < ANGLE_0_DEG) )
                    latched_yaw_angle_imu <= old_yaw_angle_imu;
                else
                    latched_yaw_angle_imu <= yaw_angle_imu;
            end
        end
    end



endmodule