data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v
 line:526: before: "signed"
1: b'%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:33: 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/angle_controller.v:34: 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/angle_controller.v:37: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] throttle_rate_out,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:38: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_rate_out,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:39: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_rate_out,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:40: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_rate_out,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:41: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:42: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_angle_error,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:43: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_angle_error,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:46: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_target,\n                 ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:47: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    input  wire [`REC_VAL_BIT_WIDTH-1:0] pitch_target,\n                 ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:48: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    input  wire [`REC_VAL_BIT_WIDTH-1:0] roll_target,\n                 ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:50: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_target,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:51: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_in,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:52: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    input  wire signed [`RATE_BIT_WIDTH-1:0] pitch_actual,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:53: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    input  wire signed [`RATE_BIT_WIDTH-1:0] roll_actual,\n                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:61: Define or directive not defined: \'`OPS_BIT_WIDTH\'\n    localparam signed [`OPS_BIT_WIDTH-1:0]\n                       ^~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:62: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        THROTTLE_MAX    =  250 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:62: syntax error, unexpected \',\', expecting TYPE-IDENTIFIER\n        THROTTLE_MAX    =  250 << `FIXED_POINT_SHIFT,\n                                                    ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:63: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        YAW_MAX         =  200 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:64: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        YAW_MIN         = -200 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:65: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        PITCH_MAX       =  200 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:66: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        PITCH_MIN       = -200 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:67: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        ROLL_MAX        =  200 << `FIXED_POINT_SHIFT,\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:68: Define or directive not defined: \'`FIXED_POINT_SHIFT\'\n        ROLL_MIN        = -200 << `FIXED_POINT_SHIFT;\n                                  ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:72: Define or directive not defined: \'`OPS_BIT_WIDTH\'\n    localparam signed [`OPS_BIT_WIDTH-1:0]\n                       ^~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:83: Define or directive not defined: \'`OPS_BIT_WIDTH\'\n    reg signed [`OPS_BIT_WIDTH-1:0]     mapped_throttle, mapped_yaw, mapped_roll, mapped_pitch;\n                ^~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:84: Define or directive not defined: \'`OPS_BIT_WIDTH\'\n    reg signed [`OPS_BIT_WIDTH-1:0]     scaled_throttle, scaled_yaw, scaled_roll, scaled_pitch;\n                ^~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:85: Define or directive not defined: \'`REC_VAL_BIT_WIDTH\'\n    reg signed [`REC_VAL_BIT_WIDTH-1:0] latched_throttle, latched_pitch, latched_roll;\n                ^~~~~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:86: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_target;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:87: Define or directive not defined: \'`RATE_BIT_WIDTH\'\n    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_error;\n                ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:88: Define or directive not defined: \'`OPS_BIT_WIDTH\'\n    reg [`OPS_BIT_WIDTH-1:0]            multiplier;\n         ^~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:101: Define or directive not defined: \'`FALSE\'\n    reg start_flag = `FALSE;\n                     ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:101: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n    reg start_flag = `FALSE;\n                           ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:106: Define or directive not defined: \'`FALSE\'\n            start_flag     <= `FALSE;\n                              ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:106: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            start_flag     <= `FALSE;\n                                    ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:108: Define or directive not defined: \'`TRUE\'\n            start_flag     <= `TRUE;\n                              ^~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:108: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            start_flag     <= `TRUE;\n                                   ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:111: Define or directive not defined: \'`FALSE\'\n                start_flag <= `FALSE;\n                              ^~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:111: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n                start_flag <= `FALSE;\n                                    ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:121: Define or directive not defined: \'`ALL_ZERO_2BYTE\'\n            latched_throttle         <= `ALL_ZERO_2BYTE;\n                                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:121: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            latched_throttle         <= `ALL_ZERO_2BYTE;\n                                                       ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:122: Define or directive not defined: \'`ALL_ZERO_2BYTE\'\n            latched_yaw_angle_error  <= `ALL_ZERO_2BYTE;\n                                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:122: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            latched_yaw_angle_error  <= `ALL_ZERO_2BYTE;\n                                                       ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:123: Define or directive not defined: \'`ALL_ZERO_2BYTE\'\n            latched_pitch            <= `ALL_ZERO_2BYTE;\n                                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:123: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            latched_pitch            <= `ALL_ZERO_2BYTE;\n                                                       ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:124: Define or directive not defined: \'`ALL_ZERO_2BYTE\'\n            latched_roll             <= `ALL_ZERO_2BYTE;\n                                        ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:124: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            latched_roll             <= `ALL_ZERO_2BYTE;\n                                                       ^\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:167: Define or directive not defined: \'`ALL_ZERO_2BYTE\'\n            yaw_rate_out   <= `ALL_ZERO_2BYTE;\n                              ^~~~~~~~~~~~~~~\n%Error: data/full_repos/permissive/114714167/Source-Code/Drone2/impl1/source/angle_controller.v:167: syntax error, unexpected \';\', expecting TYPE-IDENTIFIER\n            yaw_rate_out   <= `ALL_ZERO_2BYTE;\n                                             ^\n%Error: Exiting due to too many errors encountered; --error-limit=50\n'
module angle_controller (
    output reg  signed [`RATE_BIT_WIDTH-1:0] throttle_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error,
    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_angle_error,
    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_angle_error,
    output reg  active_signal,
    output reg  complete_signal,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_target,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] pitch_target,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] roll_target,
    input  wire yaac_enable_n,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_target,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_in,
    input  wire signed [`RATE_BIT_WIDTH-1:0] pitch_actual,
    input  wire signed [`RATE_BIT_WIDTH-1:0] roll_actual,
    input  wire [2:0] switch_a,

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


    localparam signed [`OPS_BIT_WIDTH-1:0]
        THROTTLE_MAX    =  250 << `FIXED_POINT_SHIFT,
        YAW_MAX         =  200 << `FIXED_POINT_SHIFT,
        YAW_MIN         = -200 << `FIXED_POINT_SHIFT,
        PITCH_MAX       =  200 << `FIXED_POINT_SHIFT,
        PITCH_MIN       = -200 << `FIXED_POINT_SHIFT,
        ROLL_MAX        =  200 << `FIXED_POINT_SHIFT,
        ROLL_MIN        = -200 << `FIXED_POINT_SHIFT;



    localparam signed [`OPS_BIT_WIDTH-1:0]
        MAPPING_SUBS    = 500;


    localparam signed
        END_PAD         = 2'b0,
        FRONT_PAD       = 6'b0,
        THROTTLE_F_PAD  = 4'b0,
        THROTTLE_R_PAD  = 4'b0;


    reg signed [`OPS_BIT_WIDTH-1:0]     mapped_throttle, mapped_yaw, mapped_roll, mapped_pitch;
    reg signed [`OPS_BIT_WIDTH-1:0]     scaled_throttle, scaled_yaw, scaled_roll, scaled_pitch;
    reg signed [`REC_VAL_BIT_WIDTH-1:0] latched_throttle, latched_pitch, latched_roll;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_target;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_error;
    reg [`OPS_BIT_WIDTH-1:0]            multiplier;


    localparam
        STATE_WAITING  = 5'b00001,
        STATE_MAPPING  = 5'b00010,
        STATE_SCALING  = 5'b00100,
        STATE_LIMITING = 5'b01000,
        STATE_COMPLETE = 5'b10000;


    reg [4:0] state, next_state;

    reg start_flag = `FALSE;


    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
            state                    <= STATE_WAITING;
            latched_throttle         <= `ALL_ZERO_2BYTE;
            latched_yaw_angle_error  <= `ALL_ZERO_2BYTE;
            latched_pitch            <= `ALL_ZERO_2BYTE;
            latched_roll             <= `ALL_ZERO_2BYTE;
        end
        else begin
            state                    <= next_state;
            latched_yaw_angle_target <= yaw_angle_target;
            latched_yaw_angle_error  <= yaw_angle_error_in;
            latched_roll             <= roll_target;
            latched_pitch            <= pitch_target;
            latched_throttle         <= throttle_target;
        end
    end


    always @(*) begin
        case(state)
            STATE_WAITING: begin
                if(start_flag)
                    next_state = STATE_MAPPING;
                else
                    next_state = STATE_WAITING;
            end
            STATE_MAPPING: begin
                next_state = STATE_SCALING;
            end
            STATE_SCALING: begin
                next_state = STATE_LIMITING;
            end
            STATE_LIMITING: begin
                next_state = STATE_COMPLETE;
            end
            STATE_COMPLETE: begin
                next_state = STATE_WAITING;
            end
            default: begin
                next_state = STATE_WAITING;
            end
        endcase
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin

            yaw_rate_out   <= `ALL_ZERO_2BYTE;
            roll_rate_out  <= `ALL_ZERO_2BYTE;
            pitch_rate_out <= `ALL_ZERO_2BYTE;
            multiplier     <= 0;

        end
        else begin
            case(state)
                STATE_WAITING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `FALSE;
                    if (switch_a[0])
                        multiplier <= 16'sd2;
                    else
                        multiplier <= 16'sd1;
                end
                STATE_MAPPING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;    


                    if(yaac_enable_n)
                        mapped_yaw          <= $signed({FRONT_PAD, latched_yaw_angle_target[7:0], END_PAD}) - MAPPING_SUBS;

                    else
                        mapped_yaw          <= latched_yaw_angle_error;
                    mapped_throttle         <= $signed({THROTTLE_F_PAD, latched_throttle, THROTTLE_R_PAD});

                    mapped_roll             <= ($signed({FRONT_PAD, latched_roll,  END_PAD}) - MAPPING_SUBS)*multiplier + roll_actual;
                    mapped_pitch            <= ($signed({FRONT_PAD, latched_pitch, END_PAD}) - MAPPING_SUBS)*multiplier - pitch_actual;
                    
                end
                STATE_SCALING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;

                    if(yaac_enable_n)
                        scaled_yaw          <= scale_val(mapped_yaw, `YAW_OLD_AC_K_P, `YAW_AC_K_P_SHIFT);
                    else
                        scaled_yaw          <= scale_val(mapped_yaw, `YAW_YAAC_AC_K_P, `YAW_AC_K_P_SHIFT);
                    scaled_roll             <= scale_val(mapped_roll, `ROLL_AC_K_P, `ROLL_AC_K_P_SHIFT);
                    scaled_pitch            <= scale_val(mapped_pitch, `PITCH_AC_K_P, `PITCH_AC_K_P_SHIFT);
                    scaled_throttle         <= scale_val(mapped_throttle, `THROTTLE_AC_K_P, `THROTTLE_AC_K_P_SHIFT);
                end
                STATE_LIMITING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;


                    if(scaled_throttle > THROTTLE_MAX)
                        throttle_rate_out   <= THROTTLE_MAX;
                    else if(scaled_throttle < `MOTOR_VAL_MIN)
                        throttle_rate_out   <= `MOTOR_VAL_MIN;
                    else
                        throttle_rate_out   <= scaled_throttle;


                    if(scaled_yaw > YAW_MAX)
                        yaw_rate_out        <= YAW_MAX;
                    else if(scaled_yaw < YAW_MIN)
                        yaw_rate_out        <= YAW_MIN;
                    else
                        yaw_rate_out        <= scaled_yaw;


                    if(scaled_roll > ROLL_MAX)
                        roll_rate_out       <= ROLL_MAX;
                    else if(scaled_roll < ROLL_MIN)
                        roll_rate_out       <= ROLL_MIN;
                    else
                        roll_rate_out       <= scaled_roll;


                    if(scaled_pitch > PITCH_MAX)
                        pitch_rate_out      <= PITCH_MAX;
                    else if(scaled_pitch < PITCH_MIN)
                        pitch_rate_out      <= PITCH_MIN;
                    else
                        pitch_rate_out      <= scaled_pitch;

                    yaw_angle_error         <= latched_yaw_angle_error;
                    pitch_angle_error       <= mapped_pitch;
                    roll_angle_error        <= mapped_roll;
                end
                STATE_COMPLETE: begin
                    complete_signal         <= `TRUE;
                    active_signal           <= `FALSE;
                end
                default: begin
                    pitch_rate_out          <= `ALL_ZERO_2BYTE;
                    yaw_rate_out            <= `ALL_ZERO_2BYTE;
                    roll_rate_out           <= `ALL_ZERO_2BYTE;
                    throttle_rate_out       <= `ALL_ZERO_2BYTE;
                end
            endcase
        end
    end


function automatic signed [`RATE_BIT_WIDTH-1:0] scale_val;
    input reg signed [`RATE_BIT_WIDTH-1:0]        val;
    input reg signed [`OPS_BIT_WIDTH-1:0]          k_mult;
    input reg signed [`SHIFT_OP_BIT_WIDTH-1:0]    k_shift;

    reg signed [31:0]
        val_32,
        scaled;

    localparam
        SHIFT_BACK = 7'd16,
        ZERO_PAD   = 16'd0;

    localparam signed [31:0]
        OVERFLOW_PROTECTION_MIN = 32'shFFFF8000,
        OVERFLOW_PROTECTION_MAX = 32'sh00007FFF;

    begin

        val_32 = $signed({val, ZERO_PAD}) >>> SHIFT_BACK;

        scaled = (val_32 * k_mult) >>> k_shift;


        if (scaled <= OVERFLOW_PROTECTION_MIN)
            scale_val = OVERFLOW_PROTECTION_MIN;
        else if (scaled >= OVERFLOW_PROTECTION_MAX)
            scale_val = OVERFLOW_PROTECTION_MAX;
        else
            scale_val = $signed(scaled[`RATE_BIT_WIDTH-1:0]);
    end
endfunction

endmodule
module angle_controller (
    output reg  signed [`RATE_BIT_WIDTH-1:0] throttle_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_rate_out,
    output reg  signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error,
    output reg  signed [`RATE_BIT_WIDTH-1:0] pitch_angle_error,
    output reg  signed [`RATE_BIT_WIDTH-1:0] roll_angle_error,
    output reg  active_signal,
    output reg  complete_signal,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] throttle_target,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] pitch_target,
    input  wire [`REC_VAL_BIT_WIDTH-1:0] roll_target,
    input  wire yaac_enable_n,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_target,
    input  wire signed [`RATE_BIT_WIDTH-1:0] yaw_angle_error_in,
    input  wire signed [`RATE_BIT_WIDTH-1:0] pitch_actual,
    input  wire signed [`RATE_BIT_WIDTH-1:0] roll_actual,
    input  wire [2:0] switch_a,

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



    localparam signed [`OPS_BIT_WIDTH-1:0]
        THROTTLE_MAX    =  250 << `FIXED_POINT_SHIFT,
        YAW_MAX         =  200 << `FIXED_POINT_SHIFT,
        YAW_MIN         = -200 << `FIXED_POINT_SHIFT,
        PITCH_MAX       =  200 << `FIXED_POINT_SHIFT,
        PITCH_MIN       = -200 << `FIXED_POINT_SHIFT,
        ROLL_MAX        =  200 << `FIXED_POINT_SHIFT,
        ROLL_MIN        = -200 << `FIXED_POINT_SHIFT;



    localparam signed [`OPS_BIT_WIDTH-1:0]
        MAPPING_SUBS    = 500;


    localparam signed
        END_PAD         = 2'b0,
        FRONT_PAD       = 6'b0,
        THROTTLE_F_PAD  = 4'b0,
        THROTTLE_R_PAD  = 4'b0;


    reg signed [`OPS_BIT_WIDTH-1:0]     mapped_throttle, mapped_yaw, mapped_roll, mapped_pitch;
    reg signed [`OPS_BIT_WIDTH-1:0]     scaled_throttle, scaled_yaw, scaled_roll, scaled_pitch;
    reg signed [`REC_VAL_BIT_WIDTH-1:0] latched_throttle, latched_pitch, latched_roll;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_target;
    reg signed [`RATE_BIT_WIDTH-1:0]    latched_yaw_angle_error;
    reg [`OPS_BIT_WIDTH-1:0]            multiplier;


    localparam
        STATE_WAITING  = 5'b00001,
        STATE_MAPPING  = 5'b00010,
        STATE_SCALING  = 5'b00100,
        STATE_LIMITING = 5'b01000,
        STATE_COMPLETE = 5'b10000;


    reg [4:0] state, next_state;

    reg start_flag = `FALSE;


    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
            state                    <= STATE_WAITING;
            latched_throttle         <= `ALL_ZERO_2BYTE;
            latched_yaw_angle_error  <= `ALL_ZERO_2BYTE;
            latched_pitch            <= `ALL_ZERO_2BYTE;
            latched_roll             <= `ALL_ZERO_2BYTE;
        end
        else begin
            state                    <= next_state;
            latched_yaw_angle_target <= yaw_angle_target;
            latched_yaw_angle_error  <= yaw_angle_error_in;
            latched_roll             <= roll_target;
            latched_pitch            <= pitch_target;
            latched_throttle         <= throttle_target;
        end
    end


    always @(*) begin
        case(state)
            STATE_WAITING: begin
                if(start_flag)
                    next_state = STATE_MAPPING;
                else
                    next_state = STATE_WAITING;
            end
            STATE_MAPPING: begin
                next_state = STATE_SCALING;
            end
            STATE_SCALING: begin
                next_state = STATE_LIMITING;
            end
            STATE_LIMITING: begin
                next_state = STATE_COMPLETE;
            end
            STATE_COMPLETE: begin
                next_state = STATE_WAITING;
            end
            default: begin
                next_state = STATE_WAITING;
            end
        endcase
    end


    always @(posedge us_clk or negedge resetn) begin
        if(!resetn) begin

            yaw_rate_out   <= `ALL_ZERO_2BYTE;
            roll_rate_out  <= `ALL_ZERO_2BYTE;
            pitch_rate_out <= `ALL_ZERO_2BYTE;
            multiplier     <= 0;

        end
        else begin
            case(state)
                STATE_WAITING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `FALSE;
                    if (switch_a[0])
                        multiplier <= 16'sd2;
                    else
                        multiplier <= 16'sd1;
                end
                STATE_MAPPING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;    


                    if(yaac_enable_n)
                        mapped_yaw          <= $signed({FRONT_PAD, latched_yaw_angle_target[7:0], END_PAD}) - MAPPING_SUBS;

                    else
                        mapped_yaw          <= latched_yaw_angle_error;
                    mapped_throttle         <= $signed({THROTTLE_F_PAD, latched_throttle, THROTTLE_R_PAD});

                    mapped_roll             <= ($signed({FRONT_PAD, latched_roll,  END_PAD}) - MAPPING_SUBS)*multiplier + roll_actual;
                    mapped_pitch            <= ($signed({FRONT_PAD, latched_pitch, END_PAD}) - MAPPING_SUBS)*multiplier - pitch_actual;
                    
                end
                STATE_SCALING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;

                    if(yaac_enable_n)
                        scaled_yaw          <= scale_val(mapped_yaw, `YAW_OLD_AC_K_P, `YAW_AC_K_P_SHIFT);
                    else
                        scaled_yaw          <= scale_val(mapped_yaw, `YAW_YAAC_AC_K_P, `YAW_AC_K_P_SHIFT);
                    scaled_roll             <= scale_val(mapped_roll, `ROLL_AC_K_P, `ROLL_AC_K_P_SHIFT);
                    scaled_pitch            <= scale_val(mapped_pitch, `PITCH_AC_K_P, `PITCH_AC_K_P_SHIFT);
                    scaled_throttle         <= scale_val(mapped_throttle, `THROTTLE_AC_K_P, `THROTTLE_AC_K_P_SHIFT);
                end
                STATE_LIMITING: begin
                    complete_signal         <= `FALSE;
                    active_signal           <= `TRUE;


                    if(scaled_throttle > THROTTLE_MAX)
                        throttle_rate_out   <= THROTTLE_MAX;
                    else if(scaled_throttle < `MOTOR_VAL_MIN)
                        throttle_rate_out   <= `MOTOR_VAL_MIN;
                    else
                        throttle_rate_out   <= scaled_throttle;


                    if(scaled_yaw > YAW_MAX)
                        yaw_rate_out        <= YAW_MAX;
                    else if(scaled_yaw < YAW_MIN)
                        yaw_rate_out        <= YAW_MIN;
                    else
                        yaw_rate_out        <= scaled_yaw;


                    if(scaled_roll > ROLL_MAX)
                        roll_rate_out       <= ROLL_MAX;
                    else if(scaled_roll < ROLL_MIN)
                        roll_rate_out       <= ROLL_MIN;
                    else
                        roll_rate_out       <= scaled_roll;


                    if(scaled_pitch > PITCH_MAX)
                        pitch_rate_out      <= PITCH_MAX;
                    else if(scaled_pitch < PITCH_MIN)
                        pitch_rate_out      <= PITCH_MIN;
                    else
                        pitch_rate_out      <= scaled_pitch;

                    yaw_angle_error         <= latched_yaw_angle_error;
                    pitch_angle_error       <= mapped_pitch;
                    roll_angle_error        <= mapped_roll;
                end
                STATE_COMPLETE: begin
                    complete_signal         <= `TRUE;
                    active_signal           <= `FALSE;
                end
                default: begin
                    pitch_rate_out          <= `ALL_ZERO_2BYTE;
                    yaw_rate_out            <= `ALL_ZERO_2BYTE;
                    roll_rate_out           <= `ALL_ZERO_2BYTE;
                    throttle_rate_out       <= `ALL_ZERO_2BYTE;
                end
            endcase
        end
    end


function automatic signed [`RATE_BIT_WIDTH-1:0] scale_val;
    input reg signed [`RATE_BIT_WIDTH-1:0]        val;
    input reg signed [`OPS_BIT_WIDTH-1:0]          k_mult;
    input reg signed [`SHIFT_OP_BIT_WIDTH-1:0]    k_shift;

    reg signed [31:0]
        val_32,
        scaled;

    localparam
        SHIFT_BACK = 7'd16,
        ZERO_PAD   = 16'd0;

    localparam signed [31:0]
        OVERFLOW_PROTECTION_MIN = 32'shFFFF8000,
        OVERFLOW_PROTECTION_MAX = 32'sh00007FFF;

    begin

        val_32 = $signed({val, ZERO_PAD}) >>> SHIFT_BACK;

        scaled = (val_32 * k_mult) >>> k_shift;


        if (scaled <= OVERFLOW_PROTECTION_MIN)
            scale_val = OVERFLOW_PROTECTION_MIN;
        else if (scaled >= OVERFLOW_PROTECTION_MAX)
            scale_val = OVERFLOW_PROTECTION_MAX;
        else
            scale_val = $signed(scaled[`RATE_BIT_WIDTH-1:0]);
    end
endfunction

endmodule