<|start_filename|>sdk/bare/common/drv/io.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

#include "drv/io.h"
#include "xgpiops.h"
#include <stdio.h>

#define IO_LED_R_MIO_PIN (41)
#define IO_LED_G_MIO_PIN (43)
#define IO_LED_B_MIO_PIN (45)

#define IO_BTN1_MIO_PIN (47)

#define IO_SW1_MIO_PIN (42)
#define IO_SW2_MIO_PIN (44)

static XGpioPs Gpio;

void io_init(void)
{
    printf("IO:\tInitializing...\n");

    int Status;
    XGpioPs_Config *GPIOConfigPtr;

    // GPIO Initialization
    GPIOConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
    Status = XGpioPs_CfgInitialize(&Gpio, GPIOConfigPtr, GPIOConfigPtr->BaseAddr);
    if (Status != XST_SUCCESS) {
        // Just hang here if error...
        while (1) {
        }
    }

    // Set RGB LED direction as output
    XGpioPs_SetDirectionPin(&Gpio, IO_LED_R_MIO_PIN, 1);
    XGpioPs_SetDirectionPin(&Gpio, IO_LED_G_MIO_PIN, 1);
    XGpioPs_SetDirectionPin(&Gpio, IO_LED_B_MIO_PIN, 1);

    // Set switches and button as input
    XGpioPs_SetDirectionPin(&Gpio, IO_SW1_MIO_PIN, 0);
    XGpioPs_SetDirectionPin(&Gpio, IO_SW2_MIO_PIN, 0);
    XGpioPs_SetDirectionPin(&Gpio, IO_BTN1_MIO_PIN, 0);

    // Set LED output enabled
    XGpioPs_SetOutputEnablePin(&Gpio, IO_LED_R_MIO_PIN, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, IO_LED_G_MIO_PIN, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, IO_LED_B_MIO_PIN, 1);

    // Start with RGB LED off
    XGpioPs_WritePin(&Gpio, IO_LED_R_MIO_PIN, 0);
    XGpioPs_WritePin(&Gpio, IO_LED_G_MIO_PIN, 0);
    XGpioPs_WritePin(&Gpio, IO_LED_B_MIO_PIN, 0);
}

void io_led_set(io_led_color_t *color)
{
    io_led_set_c(1, 1, 1, color);
}

void io_led_set_c(uint8_t r, uint8_t g, uint8_t b, io_led_color_t *color)
{
    uint8_t rr = color->r > 0 ? 1 : 0;
    uint8_t gg = color->g > 0 ? 1 : 0;
    uint8_t bb = color->b > 0 ? 1 : 0;

    if (r) {
        XGpioPs_WritePin(&Gpio, IO_LED_R_MIO_PIN, rr);
    }

    if (g) {
        XGpioPs_WritePin(&Gpio, IO_LED_G_MIO_PIN, gg);
    }

    if (b) {
        XGpioPs_WritePin(&Gpio, IO_LED_B_MIO_PIN, bb);
    }
}

void io_switch_get(uint8_t *sw1, uint8_t *sw2)
{
    *sw1 = XGpioPs_ReadPin(&Gpio, IO_SW1_MIO_PIN);
    *sw2 = XGpioPs_ReadPin(&Gpio, IO_SW2_MIO_PIN);
}

void io_button_get(uint8_t *btn1)
{
    *btn1 = XGpioPs_ReadPin(&Gpio, IO_BTN1_MIO_PIN);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/sys/debug.c<|end_filename|>
#include "sys/debug.h"
#include "sys/serial.h"
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#define BUFFER_LENGTH (1024)
static char buffer[BUFFER_LENGTH] = { 0 };

void debug_print(char *msg)
{
    serial_write(msg, strlen(msg));
}

void debug_printf(const char *format, ...)
{
    va_list vargs;
    va_start(vargs, format);

    vsnprintf(buffer, BUFFER_LENGTH, format, vargs);
    debug_print(buffer);

    va_end(vargs);
}


<|start_filename|>sdk/bare/common/sys/util.h<|end_filename|>
#ifndef UTIL_H
#define UTIL_H

#include <stdbool.h>
#include <string.h>

#define MIN(x, y) (((x) > (y)) ? (y) : (x))
#define MAX(x, y) (((x) > (y)) ? (x) : (y))

#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))

#define BITSET(word, nbit)   ((word) |= (1 << (nbit)))
#define BITCLEAR(word, nbit) ((word) &= ~(1 << (nbit)))
#define BITFLIP(word, nbit)  ((word) ^= (1 << (nbit)))
#define BITCHECK(word, nbit) ((word) & (1 << (nbit)))

static inline bool STREQ(char *in1, char *in2)
{
    return (strcmp(in1, in2) == 0) ? true : false;
}

#endif // UTIL_H


<|start_filename|>sdk/bare/common/drv/analog_defs.h<|end_filename|>
#ifndef ANALOG_DEFS_H
#define ANALOG_DEFS_H

#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

// ===============
// Number of slave registers in this IP
// ===============
#define ANALOG_DEFS_NUM_SLAVE_REGS (17)

// ===============
// Offsets for the relevant slave registers (in bytes).
// Separated by 4 bytes since each register is 32 bits.
// ===============
#define ANALOG_DEFS_OFFSET_CHANNEL1  (0 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL2  (1 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL3  (2 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL4  (3 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL5  (4 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL6  (5 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL7  (6 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL8  (7 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL9  (8 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL10 (9 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL11 (10 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL12 (11 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL13 (12 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL14 (13 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL15 (14 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL16 (15 * 4)
#define ANALOG_DEFS_OFFSET_CONTROL   (16 * 4)

#endif // USER_CONFIG_HARDWARE_TARGET

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

// ===============
// Number of slave registers in this IP
// ===============
#define ANALOG_DEFS_NUM_SLAVE_REGS (10)

// ===============
// Offsets for the relevant slave registers (in bytes).
// Separated by 4 bytes since each register is 32 bits.
// ===============
#define ANALOG_DEFS_OFFSET_CHANNEL1 (0 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL2 (1 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL3 (2 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL4 (3 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL5 (4 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL6 (5 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL7 (6 * 4)
#define ANALOG_DEFS_OFFSET_CHANNEL8 (7 * 4)
#define ANALOG_DEFS_OFFSET_CONTROL  (8 * 4)
#define ANALOG_DEFS_OFFSET_UNUSED1  (9 * 4)

#endif // USER_CONFIG_HARDWARE_TARGET

// ===============
// CONTROL register
// ===============
#define ANALOG_DEFS_CONTROL_CLKDIV_WIDTH (2) // in bits
#define ANALOG_DEFS_CONTROL_CLKDIV_SHIFT (0) // up from reg[0]
#define ANALOG_DEFS_CONTROL_CLKDIV_MASK  (0x3)

#define ANALOG_DEFS_CONTROL_PWM_SYNC_HIGH_WIDTH (1) // in bits
#define ANALOG_DEFS_CONTROL_PWM_SYNC_HIGH_SHIFT (2) // up from reg[0]
#define ANALOG_DEFS_CONTROL_PWM_SYNC_HIGH_MASK  (0x1)

#define ANALOG_DEFS_CONTROL_PWM_SYNC_LOW_WIDTH (1) // in bits
#define ANALOG_DEFS_CONTROL_PWM_SYNC_LOW_SHIFT (3) // up from reg[0]
#define ANALOG_DEFS_CONTROL_PWM_SYNC_LOW_MASK  (0x1)

#endif // ANALOG_DEFS_H


<|start_filename|>sdk/bare/user/usr/blink/task_vsi.c<|end_filename|>
#ifdef APP_BLINK

#include "usr/blink/task_vsi.h"
#include "drv/led.h"
#include "sys/scheduler.h"
#include <math.h>
#include <stdint.h>

// Scheduler TCB which holds task "context"
static task_control_block_t tcb;

// Example logging variables for testing
double LOG_vsi_a = 0;
float LOG_vsi_b = 0;
int LOG_vsi_c = 0;

static double Ts = 1.0 / 10000.0; // [sec]
static double theta = 0.0;        // [rad]
static double omega = 10.0;       // [rad/s]
static double Do = 0.75;          // [--]

void task_vsi_init(void)
{
    if (scheduler_tcb_is_registered(&tcb)) {
        return;
    }

    // Fill TCB with parameters
    scheduler_tcb_init(&tcb, task_vsi_callback, NULL, "vsi", TASK_VSI_INTERVAL_USEC);

    // Register task with scheduler
    scheduler_tcb_register(&tcb);
}

void task_vsi_callback(void *arg)
{
    // Update theta
    theta += (Ts * omega);
    theta = fmod(theta, 2.0 * M_PI); // Wrap to 2*pi

    // Calculate desired duty ratios
    double vsi_a = Do * cos(theta);
    double vsi_b = Do * cos(theta + 2.0 * M_PI / 3.0);
    double vsi_c = Do * cos(theta + 4.0 * M_PI / 3.0);

    // Update logging variables
    LOG_vsi_a = (double) (10e3 * vsi_a);
    LOG_vsi_b = (float) (10e3 * vsi_b);
    LOG_vsi_c = (int) (10e3 * vsi_c);
}

#endif // APP_BLINK


<|start_filename|>sdk/bare/user/usr/blink/task_vsi.h<|end_filename|>
#ifndef TASK_VSI_H
#define TASK_VSI_H

#include "sys/scheduler.h"

#define TASK_VSI_UPDATES_PER_SEC (10000)
#define TASK_VSI_INTERVAL_USEC   (USEC_IN_SEC / TASK_VSI_UPDATES_PER_SEC)

void task_vsi_init(void);

void task_vsi_callback(void *arg);

#endif // TASK_VSI_H


<|start_filename|>sdk/bare/common/usr/demo/cmd/cmd_cc.c<|end_filename|>
#ifdef APP_DEMO

#include "usr/demo/cmd/cmd_cc.h"
#include "drv/analog.h"
#include "drv/pwm.h"
#include "sys/commands.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include "usr/demo/task_cc.h"
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

// To initialize this module for CC 0:
//
// Configure I/O
// > cc 0 pwm a 2
// > cc 0 pwm b 1
// > cc 0 pwm c 0
// > cc 0 adc a 16 -2.5 0
// > cc 0 adc b 15 -2.5 0
// > cc 0 adc c 13 -2.5 0
//
// Configure tuning
// > cc 0 vdc 10
// > cc 0 tune 1 0.0015 0.0015 628
//
// Start current controller
// > cc 0 init
//
// Set operating point
// > cc 0 set 1 1 10
//
// Stop current controller
// > cc 0 deinit

float value;

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "setup [ashad|yusuke]", "Set-up specific bench configuration" },
    { "<cc_idx> init", "Initialize current controller" },
    { "<cc_idx> deinit", "Deinitialize current controller" },
    { "<cc_idx> vdc <volts>", "Set DC link voltage" },
    { "<cc_idx> pwm <a|b|c> <pwm_chnl>", "Configure PWM outputs per phase" },
    { "<cc_idx> adc <a|b|c> <adc_chnl> <adc_gain> <adc_offset>", "Configure ADC input per phase" },
    { "<cc_idx> tune <Rs> <Ld> <Lq> <bw>", "Tune with Rs [ohms], Ldq [H], and bandwidth [rad/s]" },
    { "<cc_idx> setmode <angle|speed>", "Set mode to control angle or speed directly" },
    { "<cc_idx> set <Id*> <Iq*>", "Set operating point for Idq* [Adq]" },
    { "<cc_idx> omega <omega_e>", "Set omega_e [rad/s]" },
    { "<cc_idx> theta <theta_e>", "Set theta_e [degrees]" },
};

void cmd_cc_register(void)
{
    commands_cmd_init(
        &cmd_entry, "cc", "Current control (D/Q) related commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_cc);

    // Register the command with the system
    commands_cmd_register(&cmd_entry);
}

int cmd_cc(int argc, char **argv)
{
    if (argc == 3 && STREQ("init", argv[2])) {
        // Enable current controller
        int cc_idx = atoi(argv[1]);
        task_cc_init(cc_idx);
        return CMD_SUCCESS;
    }

    if (argc == 3 && STREQ("deinit", argv[2])) {
        // Disable current controller
        int cc_idx = atoi(argv[1]);
        task_cc_deinit(cc_idx);
        return CMD_SUCCESS;
    }

    if (argc == 3 && STREQ("setup", argv[1]) && STREQ("ashad", argv[2])) {
        // Setup for Ashad bench
        task_cc_vdc_set(0, 10.0);

        task_cc_adc(0, 0, 16, -2.5, 0.0); // A
        task_cc_adc(0, 1, 15, -2.5, 0.0); // B
        task_cc_adc(0, 2, 13, -2.5, 0.0); // C

        task_cc_pwm(0, 0, 2); // A
        task_cc_pwm(0, 1, 1); // B
        task_cc_pwm(0, 2, 0); // C

        task_cc_tune(0, 1.0, 0.0015, 0.0015, 628.0);

        return CMD_SUCCESS;
    }

    if (argc == 3 && STREQ("setup", argv[1]) && STREQ("yusuke", argv[2])) {
        // Setup for Ashad bench
        task_cc_vdc_set(0, 10.0);
        task_cc_vdc_set(1, 10.0);

        task_cc_adc(0, 0, 3, 1.434, -0.01); // TA
        task_cc_adc(0, 1, 2, 1.434, -0.08); // TB
        task_cc_adc(0, 2, 1, 1.434, -0.00); // TC
        task_cc_adc(1, 0, 6, 2.199, -0.01); // SA
        task_cc_adc(1, 1, 5, 2.199, -0.04); // SB
        task_cc_adc(1, 2, 4, 2.199, -0.07); // SC

        task_cc_pwm(0, 0, 5); // TA
        task_cc_pwm(0, 1, 4); // TB
        task_cc_pwm(0, 2, 3); // TC
        task_cc_pwm(1, 0, 1); // SA
        task_cc_pwm(1, 1, 2); // SB
        task_cc_pwm(1, 2, 0); // SC

        task_cc_tune(0, 0.6, 0.0025, 0.0025, 6283.0); // Torque
        task_cc_tune(1, 2.4, 0.010, 0.010, 6283.0);   // Suspension

        return CMD_SUCCESS;
    }

    if (argc == 4 && STREQ("vdc", argv[2])) {
        // Set DC link voltage
        int cc_idx = atoi(argv[1]);

        double vdc = strtod(argv[3], NULL);

        // Sanitize inputs
        if (vdc <= 0.0 || vdc > 1000.0)
            return CMD_INVALID_ARGUMENTS;

        task_cc_vdc_set(cc_idx, vdc);
        return CMD_SUCCESS;
    }

    if (argc == 5 && STREQ("pwm", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // Extract phase user is configuring
        uint8_t phase;
        if (STREQ("a", argv[3])) {
            phase = 0;
        } else if (STREQ("b", argv[3])) {
            phase = 1;
        } else if (STREQ("c", argv[3])) {
            phase = 2;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        uint8_t pwm_chnl = atoi(argv[4]);
        if (pwm_chnl >= 24) {
            return CMD_INVALID_ARGUMENTS;
        }

        task_cc_pwm(cc_idx, phase, pwm_chnl);

        return CMD_SUCCESS;
    }

    if (argc == 7 && STREQ("adc", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // Extract phase user is configuring
        uint8_t phase;
        if (STREQ("a", argv[3])) {
            phase = 0;
        } else if (STREQ("b", argv[3])) {
            phase = 1;
        } else if (STREQ("c", argv[3])) {
            phase = 2;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        uint8_t adc_chnl = atoi(argv[4]);
        if (adc_chnl <= 0 || adc_chnl >= ANALOG_NUM_CHANNELS) {
            return CMD_INVALID_ARGUMENTS;
        }

        double adc_gain = strtod(argv[5], NULL);
        double adc_offset = strtod(argv[6], NULL);

        if (adc_gain <= -1e6 || adc_gain >= 1e6)
            return CMD_INVALID_ARGUMENTS;
        if (adc_offset <= -1e6 || adc_offset >= 1e6)
            return CMD_INVALID_ARGUMENTS;

        task_cc_adc(cc_idx, phase, adc_chnl, adc_gain, adc_offset);

        return CMD_SUCCESS;
    }

    if (argc == 7 && STREQ("tune", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // Tune current controller
        double Rs, Ld, Lq, bw;

        // Extract arguments
        Rs = strtod(argv[3], NULL);
        Ld = strtod(argv[4], NULL);
        Lq = strtod(argv[5], NULL);
        bw = strtod(argv[6], NULL);

        // Sanitize inputs
        if (Rs <= 0.0 || Rs > 100.0)
            return CMD_INVALID_ARGUMENTS;
        if (Ld <= 0.0 || Ld > 100.0)
            return CMD_INVALID_ARGUMENTS;
        if (Lq <= 0.0 || Lq > 100.0)
            return CMD_INVALID_ARGUMENTS;
        if (bw <= 0.0 || bw > 100000.0)
            return CMD_INVALID_ARGUMENTS;

        task_cc_tune(cc_idx, Rs, Ld, Lq, bw);
        return CMD_SUCCESS;
    }

    if (argc == 4 && STREQ("setmode", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // Extract arguments
        if (STREQ("angle", argv[3])) {
            task_cc_setmode(cc_idx, "angle");
        } else if (STREQ("speed", argv[3])) {
            task_cc_setmode(cc_idx, "speed");
        }

        return CMD_SUCCESS;
    }

    if (argc == 5 && STREQ("set", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // Tune current controller
        double Id_star, Iq_star;

        // Extract arguments
        Id_star = strtod(argv[3], NULL);
        Iq_star = strtod(argv[4], NULL);

        // Sanitize inputs
        if (Id_star <= -100.0 || Id_star >= 100.0)
            return CMD_INVALID_ARGUMENTS;
        if (Iq_star <= -100.0 || Iq_star >= 100.0)
            return CMD_INVALID_ARGUMENTS;

        task_cc_set_currents(cc_idx, Id_star, Iq_star);
        return CMD_SUCCESS;
    }

    if (argc == 4 && STREQ("omega", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // extract arguments
        double omega_e;
        omega_e = strtod(argv[3], NULL);

        // sanitize inputs
        if (omega_e <= -1e6 || omega_e >= 1e6)
            return CMD_INVALID_ARGUMENTS;

        // set omega
        task_cc_set_omega(cc_idx, omega_e);

        return CMD_SUCCESS;
    }

    if (argc == 4 && STREQ("theta", argv[2])) {
        int cc_idx = atoi(argv[1]);

        // extract arguments
        double theta_e;
        theta_e = strtod(argv[3], NULL);
        theta_e = theta_e * PI / 180; // degree to radians

        // sanitize inputs
        if (theta_e <= -PI2 || theta_e >= PI2)
            return CMD_INVALID_ARGUMENTS;

        // set theta
        task_cc_set_theta(cc_idx, theta_e);

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_DEMO


<|start_filename|>sdk/basic/src/project_include.h<|end_filename|>
#ifndef __PROJECT_INCLUDE_H
#define __PROJECT_INCLUDE_H
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "platform.h"
#include "xparameters.h"
#include "xparameters_ps.h"	/* defines XPAR values */
#include "xil_cache.h"
#include "netif/xadapter.h"
// #include "xgpio.h"
#include "xscutimer.h"
#include "xscugic.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include "xuartps_hw.h"
#include "xtmrctr.h"

//LWIP
#include "lwip/tcp.h"
#include "lwip/tcp_impl.h"
#include "lwip/init.h"

#include "platform_config.h"
#include "project_settings.h"
#include "commands.h"
#include "main.h"
#include "log.h"
#include "prog_timer.h"
#include "axi_timer.h"
#include "lwip_glue.h"
#include "control.h"
#include "io_pecb.h"

#include "../bsp/bsp.h"

//Control applications (maybe make this conditional)
#include "../control_apps/VSI_DQ_SVPWM.h"
#include "../control_apps/HBridge_CC.h"
#include "../control_apps/PosContTry1.h"
#include "../control_apps/TorqueVSI.h"
#include "ControlApp.h"



#endif //__PROJECT_INCLUDE_H



<|start_filename|>sdk/bare/common/sys/transform.c<|end_filename|>
#include "transform.h"
#include "defines.h"
#include <math.h>

void transform_clarke(transform_dqz_type_e C, double *abc, double *xyz)
{
    if (C == TRANS_DQZ_C_INVARIANT_AMPLITUDE) {
        xyz[0] = C_INVARIANT_AMPLITUDE * (1.0 * abc[0] - 0.5 * abc[1] - 0.5 * abc[2]);
        xyz[1] = C_INVARIANT_AMPLITUDE * (0.0 * abc[0] + SQRT3 / 2.0 * abc[1] - SQRT3 / 2.0 * abc[2]);
        xyz[2] = (1.0 / 3.0 * abc[0] + 1.0 / 3.0 * abc[1] + 1.0 / 3.0 * abc[2]);
    }
    if (C == TRANS_DQZ_C_INVARIANT_POWER) {
        xyz[0] = C_INVARIANT_POWER * (1.0 * abc[0] - 0.5 * abc[1] - 0.5 * abc[2]);
        xyz[1] = C_INVARIANT_POWER * (0.0 * abc[0] + SQRT3 / 2.0 * abc[1] - SQRT3 / 2.0 * abc[2]);
        xyz[2] = (1.0 / SQRT3 * abc[0] + 1.0 / SQRT3 * abc[1] + 1.0 / SQRT3 * abc[2]);
    }
}

void transform_park(double theta, double *xyz, double *dqz)
{
    dqz[0] = +cos(theta) * xyz[0] + sin(theta) * xyz[1] + 0.0 * xyz[2];
    dqz[1] = -sin(theta) * xyz[0] + cos(theta) * xyz[1] + 0.0 * xyz[2];
    dqz[2] = 0.0 * xyz[0] + 0.0 * xyz[1] + 1.0 * xyz[2];
}

void transform_dqz(transform_dqz_type_e C, double theta, double *abc, double *dqz)
{
    double xyz[3];
    transform_clarke(C, abc, xyz);
    transform_park(theta, xyz, dqz);
}

void transform_clarke_inverse(transform_dqz_type_e C, double *abc, double *xyz)
{
    if (C == TRANS_DQZ_C_INVARIANT_AMPLITUDE) {
        abc[0] = 1.0 * xyz[0] + 0.0 * xyz[1] + 1.0 * xyz[2];
        abc[1] = -1.0 / 2.0 * xyz[0] + SQRT3 / 2.0 * xyz[1] + 1.0 * xyz[2];
        abc[2] = -1.0 / 2.0 * xyz[0] - SQRT3 / 2.0 * xyz[1] + 1.0 * xyz[2];
    }
    if (C == TRANS_DQZ_C_INVARIANT_POWER) {
        abc[0] = C_INVARIANT_POWER * (1.0 * xyz[0] + 0.0 * xyz[1] + 1.0 / SQRT2 * xyz[2]);
        abc[1] = C_INVARIANT_POWER * (-1.0 / 2.0 * xyz[0] + SQRT3 / 2.0 * xyz[1] + 1.0 / SQRT2 * xyz[2]);
        abc[2] = C_INVARIANT_POWER * (-1.0 / 2.0 * xyz[0] - SQRT3 / 2.0 * xyz[1] + 1.0 / SQRT2 * xyz[2]);
    }
}

void transform_park_inverse(double theta, double *xyz, double *dqz)
{
    // Rotate backwards
    transform_park(-theta, dqz, xyz);
}

void transform_dqz_inverse(transform_dqz_type_e C, double theta, double *abc, double *dqz)
{
    double xyz[3];
    transform_park_inverse(theta, xyz, dqz);
    transform_clarke_inverse(C, abc, xyz);
}


<|start_filename|>sdk/bare/common/sys/task_stats.c<|end_filename|>
#include "sys/task_stats.h"
#include "drv/fpga_timer.h"
#include "sys/debug.h"
#include "sys/scheduler.h"
#include <assert.h>
#include <stdint.h>

void tast_stats_init(task_stats_t *stats)
{
    assert(stats);

    task_stats_reset(stats);
}

void task_stats_reset(task_stats_t *stats)
{
    assert(stats);

    stats->is_init = false;

    statistics_clear(&stats->loop_time);
    statistics_clear(&stats->run_time);
}

void task_stats_pre_task(task_stats_t *stats)
{
    assert(stats);

    if (stats->enabled) {
        uint32_t task_start_ticks = fpga_timer_now();

        if (stats->is_init) {
            double loop_time_us = fpga_timer_ticks_to_usec(task_start_ticks - stats->last_fpga_ticks);
            statistics_push(&stats->loop_time, loop_time_us);
        } else {
            stats->is_init = true;
        }

        stats->last_fpga_ticks = task_start_ticks;
    }
}

void task_stats_post_task(task_stats_t *stats)
{
    assert(stats);

    if (stats->enabled && stats->is_init) {
        uint32_t task_end_ticks = fpga_timer_now();

        double run_time_us = fpga_timer_ticks_to_usec(task_end_ticks - stats->last_fpga_ticks);

        statistics_push(&stats->run_time, run_time_us);
    }
}

// ***************************
// Code for printing stats
// ***************************

typedef enum sm_states_e {
    PRINT_HEADER = 0,

    PRINT_LOOP_NUM_SAMPLES,
    PRINT_LOOP_MIN,
    PRINT_LOOP_MAX,
    PRINT_LOOP_MEAN,
    PRINT_LOOP_VARIANCE,

    PRINT_RUN_NUM_SAMPLES,
    PRINT_RUN_MIN,
    PRINT_RUN_MAX,
    PRINT_RUN_MEAN,
    PRINT_RUN_VARIANCE,

    REMOVE_TASK,
} sm_states_e;

typedef struct sm_ctx_t {
    sm_states_e state;
    task_control_block_t tcb;

    task_stats_t *stats;
} sm_ctx_t;

#define SM_UPDATES_PER_SEC (200)
#define SM_INTERVAL_USEC   (USEC_IN_SEC / SM_UPDATES_PER_SEC)

static void state_machine_callback(void *arg)
{
    sm_ctx_t *ctx = (sm_ctx_t *) arg;

    switch (ctx->state) {
    case PRINT_HEADER:
        debug_printf("Task Stats:\r\n");
        ctx->state = PRINT_LOOP_NUM_SAMPLES;
        break;

    // Loop timings
    // ...
    case PRINT_LOOP_NUM_SAMPLES:
        debug_printf("Loop Num:\t%d samples\r\n", ctx->stats->loop_time.num_samples);
        ctx->state = PRINT_LOOP_MIN;
        break;

    case PRINT_LOOP_MIN:
        debug_printf("Loop Min:\t%.2f usec\r\n", ctx->stats->loop_time.min);
        ctx->state = PRINT_LOOP_MAX;
        break;

    case PRINT_LOOP_MAX:
        debug_printf("Loop Max:\t%.2f usec\r\n", ctx->stats->loop_time.max);
        ctx->state = PRINT_LOOP_MEAN;
        break;

    case PRINT_LOOP_MEAN:
        debug_printf("Loop Mean:\t%.2f usec\r\n", ctx->stats->loop_time.mean);
        ctx->state = PRINT_LOOP_VARIANCE;
        break;

    case PRINT_LOOP_VARIANCE:
        debug_printf("Loop Var:\t%.2f usec\r\n", statistics_variance(&ctx->stats->loop_time));
        ctx->state = PRINT_RUN_NUM_SAMPLES;
        break;

    // Run timings
    // ...
    case PRINT_RUN_NUM_SAMPLES:
        debug_printf("Run Num:\t%d samples\r\n", ctx->stats->run_time.num_samples);
        ctx->state = PRINT_RUN_MIN;
        break;

    case PRINT_RUN_MIN:
        debug_printf("Run Min:\t%.2f usec\r\n", ctx->stats->run_time.min);
        ctx->state = PRINT_RUN_MAX;
        break;

    case PRINT_RUN_MAX:
        debug_printf("Run Max:\t%.2f usec\r\n", ctx->stats->run_time.max);
        ctx->state = PRINT_RUN_MEAN;
        break;

    case PRINT_RUN_MEAN:
        debug_printf("Run Mean:\t%.2f usec\r\n", ctx->stats->run_time.mean);
        ctx->state = PRINT_RUN_VARIANCE;
        break;

    case PRINT_RUN_VARIANCE:
        debug_printf("Run Var:\t%.2f usec\r\n", statistics_variance(&ctx->stats->run_time));
        ctx->state = REMOVE_TASK;
        break;

    case REMOVE_TASK:
        debug_printf("\r\n");
        scheduler_tcb_unregister(&ctx->tcb);
        break;

    default:
        // Can't happen
        HANG;
        break;
    }
}

static sm_ctx_t ctx;

void task_stats_print(task_stats_t *stats)
{
    // Initialize the state machine context
    ctx.state = PRINT_HEADER;
    ctx.stats = stats;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx.tcb, state_machine_callback, &ctx, "printstats", SM_INTERVAL_USEC);
    scheduler_tcb_register(&ctx.tcb);
}


<|start_filename|>sdk/bare/user/usr/beta_labs/task_mc.h<|end_filename|>
#ifndef TASK_MC_H
#define TASK_MC_H

#include "sys/scheduler.h"
#include <stdint.h>

#define TASK_MC_UPDATES_PER_SEC (4000)
#define TASK_MC_INTERVAL_USEC   (USEC_IN_SEC / TASK_MC_UPDATES_PER_SEC)

uint8_t task_mc_is_inited(void);
void task_mc_init(void);
void task_mc_deinit(void);
void task_mc_callback(void *arg);

void task_mc_set_omega_m_star(double omega_m);
void task_mc_set_cff_enabled(uint32_t enabled);
void task_mc_set_omega_m_src(uint8_t use_encoder);

#endif // TASK_MC_H


<|start_filename|>sdk/bare/common/drv/io.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

#ifndef IO_H
#define IO_H

#include <stdint.h>

typedef struct io_led_color_t {
    uint8_t r;
    uint8_t g;
    uint8_t b;
} io_led_color_t;

void io_init(void);

void io_led_set_c(uint8_t r, uint8_t g, uint8_t b, io_led_color_t *color);
void io_led_set(io_led_color_t *color);

void io_switch_get(uint8_t *sw1, uint8_t *sw2);
void io_button_get(uint8_t *btn1);

#endif // IO_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/usr/demo/task_cc.c<|end_filename|>
#ifdef APP_DEMO

#include "usr/demo/task_cc.h"
#include "drv/analog.h"
#include "sys/scheduler.h"
#include "sys/transform.h"
#include "usr/demo/inverter.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

typedef struct pwm_config_t {
    uint8_t pwm_chnl;
} pwm_config_t;

typedef struct adc_config_t {
    uint8_t adc_chnl;
    double adc_gain;
    double adc_offset;
} adc_config_t;

typedef struct vec_dq_t {
    double d;
    double q;
} vec_dq_t;

typedef struct vec_abc_t {
    double a;
    double b;
    double c;
} vec_abc_t;

typedef struct cc_context_t {
    // Scheduler TCB which holds task "context"
    task_control_block_t tcb;

    adc_config_t adc_config[3];
    pwm_config_t pwm_config[3];

    vec_dq_t Kp;
    vec_dq_t Ki;

    // Controller references
    double Id_star;
    double Iq_star;
    double omega_e;

    // Controller state
    vec_dq_t Idq_err;
    vec_dq_t Idq_err_acc;
    double theta_e;
    enum setmode { MODE_ANGLE, MODE_SPEED } mode;
} cc_context_t;

// Period between controller updates
const double Ts = 1.0 / TASK_CC_UPDATES_PER_SEC;

#define MAX_NUM_CC_CTXS (8)
static cc_context_t cc_ctxs[MAX_NUM_CC_CTXS] = { 0 };

static inline bool STREQ(char *in1, char *in2)
{
    return (strcmp(in1, in2) == 0) ? true : false;
}

void task_cc_init(int cc_idx)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    if (!scheduler_tcb_is_registered(&ctx->tcb)) {
        scheduler_tcb_init(&ctx->tcb, task_cc_callback, ctx, "cc", TASK_CC_INTERVAL_USEC);
        scheduler_tcb_register(&ctx->tcb);
    }
    ctx->mode = MODE_SPEED;
}

void task_cc_deinit(int cc_idx)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    if (scheduler_tcb_is_registered(&ctx->tcb)) {
        scheduler_tcb_unregister(&ctx->tcb);
    }

    inverter_set_voltage(cc_idx, ctx->pwm_config[0].pwm_chnl, 0.0);
    inverter_set_voltage(cc_idx, ctx->pwm_config[1].pwm_chnl, 0.0);
    inverter_set_voltage(cc_idx, ctx->pwm_config[2].pwm_chnl, 0.0);
}

void task_cc_setmode(int cc_idx, char *argMode)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    if (STREQ("angle", argMode))
        ctx->mode = MODE_ANGLE;
    else if (STREQ("speed", argMode))
        ctx->mode = MODE_SPEED;
}
void task_cc_callback(void *arg)
{
    cc_context_t *ctx = (cc_context_t *) arg;
    int cc_idx = 0;
    while (&cc_ctxs[cc_idx] != ctx)
        cc_idx++;

    // ---------------------
    // Update position based on user specified speed
    // ---------------------
    if (ctx->mode == MODE_SPEED) {
        ctx->theta_e += ctx->omega_e * Ts;
    }
    if (ctx->theta_e > PI2)
        ctx->theta_e -= PI2;
    if (ctx->theta_e < -PI2)
        ctx->theta_e += PI2;

    // ---------------------
    // Read currents from ADC inputs
    // ---------------------

    // Read from ADCs
    float Iabc_f[3];
    for (uint8_t i = 0; i < 3; i++) {
        analog_getf(ctx->adc_config[i].adc_chnl, &Iabc_f[i]);
    }

    // Convert ADC values to raw currents
    vec_abc_t Iabc;
    Iabc.a = ((double) Iabc_f[0] * ctx->adc_config[0].adc_gain) + ctx->adc_config[0].adc_offset;
    Iabc.b = ((double) Iabc_f[1] * ctx->adc_config[1].adc_gain) + ctx->adc_config[1].adc_offset;
    Iabc.c = ((double) Iabc_f[2] * ctx->adc_config[2].adc_gain) + ctx->adc_config[2].adc_offset;

    // ---------------------
    // Convert ABC to DQ
    // ---------------------
    double Iabc1[3];
    Iabc1[0] = Iabc.a;
    Iabc1[1] = Iabc.b;
    Iabc1[2] = Iabc.c;
    double Ixyz[3]; // alpha beta gamma currents
    double Idq0[3]; // d q 0 currents
    transform_dqz(TRANS_DQZ_C_INVARIANT_AMPLITUDE, ctx->theta_e, Iabc1, Idq0);

    // -----------------------------
    // Run through block diagram of CVCR to get Vdq*
    // -----------------------------
    vec_dq_t Vdq_star;

    vec_dq_t Idq;
    Idq.d = Idq0[0];
    Idq.q = Idq0[1];

    // d-axis
    ctx->Idq_err.d = ctx->Id_star - Idq.d;
    ctx->Idq_err_acc.d += ctx->Idq_err.d;
    Vdq_star.d = (ctx->Kp.d * ctx->Idq_err.d) + (ctx->Ki.d * Ts * ctx->Idq_err_acc.d);

    // q-axis
    ctx->Idq_err.q = ctx->Iq_star - Idq.q;
    ctx->Idq_err_acc.q += ctx->Idq_err.q;
    Vdq_star.q = (ctx->Kp.q * ctx->Idq_err.q) + (ctx->Ki.q * Ts * ctx->Idq_err_acc.q);

    // --------------------------------
    // Perform inverse DQ transform of Vdq_star
    // --------------------------------
    double Vabc_star1[3];
    double Vdq0[3];
    Vdq0[0] = Vdq_star.d;
    Vdq0[1] = Vdq_star.q;
    Vdq0[2] = 0.0;
    transform_dqz_inverse(TRANS_DQZ_C_INVARIANT_AMPLITUDE, ctx->theta_e, Vabc_star1, Vdq0);
    vec_abc_t Vabc_star;
    Vabc_star.a = Vabc_star1[0];
    Vabc_star.b = Vabc_star1[1];
    Vabc_star.c = Vabc_star1[2];

    // ------------------------------------
    // Saturate Vabc_star to inverter bus voltage
    // ------------------------------------
    inverter_saturate_to_Vdc(cc_idx, &Vabc_star.a);
    inverter_saturate_to_Vdc(cc_idx, &Vabc_star.b);
    inverter_saturate_to_Vdc(cc_idx, &Vabc_star.c);

    // --------------------------------------
    // Write voltages out to PWM hardware
    // --------------------------------------
    inverter_set_voltage(cc_idx, ctx->pwm_config[0].pwm_chnl, Vabc_star.a);
    inverter_set_voltage(cc_idx, ctx->pwm_config[1].pwm_chnl, Vabc_star.b);
    inverter_set_voltage(cc_idx, ctx->pwm_config[2].pwm_chnl, Vabc_star.c);
}

void task_cc_vdc_set(int cc_idx, double vdc)
{
    inverter_set_Vdc(cc_idx, vdc);
}

void task_cc_pwm(int cc_idx, uint8_t phase, uint8_t pwm_chnl)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    ctx->pwm_config[phase].pwm_chnl = pwm_chnl;
}

void task_cc_adc(int cc_idx, uint8_t phase, uint8_t adc_chnl, double adc_gain, double adc_offset)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    ctx->adc_config[phase].adc_chnl = adc_chnl;
    ctx->adc_config[phase].adc_gain = adc_gain;
    ctx->adc_config[phase].adc_offset = adc_offset;
}

void task_cc_tune(int cc_idx, double Rs, double Ld, double Lq, double bw)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    ctx->Kp.d = bw * Ld;
    ctx->Kp.q = bw * Lq;
    ctx->Ki.d = ctx->Kp.d * (Rs / Ld);
    ctx->Ki.q = ctx->Kp.q * (Rs / Lq);
}

void task_cc_set_currents(int cc_idx, double Id_star, double Iq_star)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    ctx->Id_star = Id_star;
    ctx->Iq_star = Iq_star;
}

void task_cc_set_omega(int cc_idx, double omega_e)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    if (ctx->mode == MODE_SPEED) {
        ctx->omega_e = omega_e;
        if (ctx->omega_e == 0.0) {
            ctx->theta_e = 0.0;
        }
    }
}

void task_cc_set_theta(int cc_idx, double theta_e)
{
    if (cc_idx < 0 || cc_idx >= MAX_NUM_CC_CTXS) {
        return;
    }

    cc_context_t *ctx = &cc_ctxs[cc_idx];

    if (ctx->mode == MODE_ANGLE) {
        ctx->theta_e = theta_e;
    }
}

#endif // APP_DEMO


<|start_filename|>sdk/bare/common/sys/log.c<|end_filename|>
#include "usr/user_config.h"

#if USER_CONFIG_ENABLE_LOGGING == 1

#include "sys/cmd/cmd_log.h"
#include "sys/crc32.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/log.h"
#include "sys/scheduler.h"
#include "sys/serial.h"
#include "sys/util.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>

// Used for debugging the logging system (preloads last slot with known data)
#define LOG_DEBUG_ENABLE_PRELOAD_SLOT (0)

// This ought to be larger than the `sys/commands.c` max char per argument
#define LOG_VAR_NAME_MAX_CHARS (40)

typedef struct buffer_entry_t {
    uint32_t timestamp;
    uint32_t value;
} buffer_entry_t;

typedef struct log_var_t {
    bool is_registered;
    char name[LOG_VAR_NAME_MAX_CHARS];
    void *addr;
    var_type_e type;

    uint32_t log_interval_usec;
    uint32_t last_logged_usec;

    int num_samples;
    buffer_entry_t buffer[LOG_SAMPLE_DEPTH_PER_VARIABLE];
    int buffer_idx;
} log_var_t;

static log_var_t vars[LOG_MAX_NUM_VARIABLES] = { 0 };

static bool is_log_running = false;

static task_control_block_t tcb;

void log_init(void)
{
    // Register task which samples variables etc
    // NOTE: this runs at the base scheduler time quantum,
    //       or as fast as possible!
    scheduler_tcb_init(&tcb, log_callback, NULL, "log", LOG_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);

    // Initialize all the variables to NULL address,
    // which indicates they aren't active
    for (int i = 0; i < LOG_MAX_NUM_VARIABLES; i++) {
        vars[i].addr = NULL;
    }

    // Start with logging disabled
    log_stop();

    // Register command
    cmd_log_register();

#if LOG_DEBUG_ENABLE_PRELOAD_SLOT == 1
    // Initialize one log variable slot (last one) with known information to test dumping
    log_var_register(LOG_MAX_NUM_VARS - 1, "LOG_testvar", (void *) 0x1, 1000, LOG_INT);

    // Get point to slot
    log_var_t *v = &vars[LOG_MAX_NUM_VARS - 1];

    v->buffer_idx = 0;
    v->num_samples = 0;

    // Put fake sample entries into slot buffer
    for (int i = 0; i < 100; i++) {
        v->buffer[v->buffer_idx].timestamp = 2000 + i;

        int out = 2000 + i;
        v->buffer[v->buffer_idx].value = *((uint32_t *) &out);

        v->buffer_idx++;
        v->num_samples++;
    }
#endif // LOG_DEBUG_ENABLE_PRELOAD_SLOT
}

void log_callback(void *arg)
{
    if (!is_log_running) {
        return;
    }

    uint32_t elapsed_usec = scheduler_get_elapsed_usec();

    for (uint8_t i = 0; i < LOG_MAX_NUM_VARIABLES; i++) {
        log_var_t *v = &vars[i];

        if (!v->is_registered) {
            // Variable not active for logging, so skip
            continue;
        }

        uint32_t usec_since_last_run = elapsed_usec - v->last_logged_usec;

        if (usec_since_last_run >= v->log_interval_usec) {
            // Time to log this variable!
            v->last_logged_usec = elapsed_usec;

            v->buffer[v->buffer_idx].timestamp = (uint32_t) elapsed_usec;

            if (v->type == LOG_INT) {
                v->buffer[v->buffer_idx].value = *((uint32_t *) v->addr);
            } else if (v->type == LOG_FLOAT) {
                float *f = (float *) &(v->buffer[v->buffer_idx].value);
                *f = *((float *) v->addr);
            } else if (v->type == LOG_DOUBLE) {
                float *f = (float *) &(v->buffer[v->buffer_idx].value);
                double value = *((double *) v->addr);
                *f = (float) value;
            }

            v->buffer_idx++;
            if (v->buffer_idx >= LOG_SAMPLE_DEPTH_PER_VARIABLE) {
                v->buffer_idx = 0;
            }

            if (v->num_samples < LOG_SAMPLE_DEPTH_PER_VARIABLE) {
                v->num_samples++;
            }
        }
    }
}

void log_start(void)
{
    is_log_running = true;
}

void log_stop(void)
{
    is_log_running = false;
}

bool log_is_logging(void)
{
    return is_log_running;
}

int log_var_register(int idx, char *name, void *addr, uint32_t samples_per_sec, var_type_e type)
{
    // Sanity check variable idx
    if (idx < 0 || idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    // Populate variable entry...
    strncpy(vars[idx].name, name, LOG_VAR_NAME_MAX_CHARS);
    vars[idx].addr = addr;
    vars[idx].type = type;

    // Calculate 'log_interval_usec' from samples per second
    vars[idx].log_interval_usec = USEC_IN_SEC / samples_per_sec;
    vars[idx].last_logged_usec = 0;

    // Mark as registered
    vars[idx].is_registered = true;

    return SUCCESS;
}

int log_var_unregister(int idx)
{
    // Sanity check variable idx
    if (idx < 0 || idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    // Mark as unregistered, this allows slot to be reused
    vars[idx].is_registered = false;

    // Also, empty slot so when variable is rereg the next time,
    // previous samples aren't still stored.
    log_var_empty(idx);

    return SUCCESS;
}

int log_var_is_registered(int idx, bool *is_registered)
{
    // Sanity check variable idx
    if (idx < 0 || idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    // Set output variable
    *is_registered = vars[idx].is_registered;

    return SUCCESS;
}

int log_var_empty(int idx)
{
    // Sanity check variable idx
    if (idx < 0 || idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    // Result metadata
    vars[idx].buffer_idx = 0;
    vars[idx].last_logged_usec = 0;
    vars[idx].num_samples = 0;

    // Note: we don't have to actually clear the memory buffer since we only dump
    // the number of samples we recorded! So the old data can live in memory
    // and not be an issue.

    return SUCCESS;
}

int log_var_empty_all(void)
{
    for (int idx = 0; idx < LOG_MAX_NUM_VARIABLES; idx++) {
        vars[idx].buffer_idx = 0;
        vars[idx].last_logged_usec = 0;
        vars[idx].num_samples = 0;
    }

    return SUCCESS;
}

// ***************************
// Code for running the state machine to
// dump the log buffers to the UART
// using ASCII characters (human readable)
// ***************************

typedef enum sm_states_dump_ascii_e {
    DUMP_ASCII_TITLE = 1,
    DUMP_ASCII_NUM_SAMPLES,
    DUMP_ASCII_HEADER,
    DUMP_ASCII_VARIABLES_TS,
    DUMP_ASCII_VARIABLES_VALUE,
    DUMP_ASCII_FOOTER,
    DUMP_ASCII_REMOVE_TASK
} sm_states_dump_ascii_e;

typedef struct sm_ctx_dump_ascii_t {
    sm_states_dump_ascii_e state;
    int var_idx;
    int sample_idx;
    task_control_block_t tcb;
} sm_ctx_dump_ascii_t;

#define SM_DUMP_ASCII_UPDATES_PER_SEC (200)
#define SM_DUMP_ASCII_INTERVAL_USEC   (USEC_IN_SEC / SM_DUMP_ASCII_UPDATES_PER_SEC)

void state_machine_dump_ascii_callback(void *arg)
{
    sm_ctx_dump_ascii_t *ctx = (sm_ctx_dump_ascii_t *) arg;

    log_var_t *v = &vars[ctx->var_idx];
    buffer_entry_t *e = &v->buffer[ctx->sample_idx];

    switch (ctx->state) {
    case DUMP_ASCII_TITLE:
        debug_printf("LOG OF VARIABLE: '%s'\r\n", v->name);
        ctx->state = DUMP_ASCII_NUM_SAMPLES;
        break;

    case DUMP_ASCII_NUM_SAMPLES:
        debug_printf("NUM SAMPLES: %d\r\n", v->num_samples);
        ctx->state = DUMP_ASCII_HEADER;
        break;

    case DUMP_ASCII_HEADER:
        debug_printf("-------START-------\r\n");

        if (v->num_samples == 0) {
            // Nothing to dump!
            ctx->state = DUMP_ASCII_FOOTER;
        } else {
            ctx->state = DUMP_ASCII_VARIABLES_TS;
        }
        break;

    case DUMP_ASCII_VARIABLES_TS:
        // Print just the timestamp
        debug_printf("> %ld\t\t", e->timestamp);

        ctx->state = DUMP_ASCII_VARIABLES_VALUE;
        break;

    case DUMP_ASCII_VARIABLES_VALUE:
        // Print just the value
        if (v->type == LOG_INT) {
            debug_printf("%ld\r\n", e->value);
        } else if (v->type == LOG_FLOAT || v->type == LOG_DOUBLE) {
            float *f = (float *) &(e->value);
            debug_printf("%f\r\n", *f);
        }

        ctx->sample_idx++;

        if (ctx->sample_idx >= v->num_samples) {
            ctx->state = DUMP_ASCII_FOOTER;
        } else {
            ctx->state = DUMP_ASCII_VARIABLES_TS;
        }
        break;

    case DUMP_ASCII_FOOTER:
        debug_printf("-------END-------\r\n\r\n");

        ctx->state = DUMP_ASCII_REMOVE_TASK;
        break;

    default:
    case DUMP_ASCII_REMOVE_TASK:
        scheduler_tcb_unregister(&ctx->tcb);
        break;
    }
}

static sm_ctx_dump_ascii_t ctx_dump_ascii;

int log_var_dump_uart_ascii(int log_var_idx)
{
    // Sanity check variable idx
    if (log_var_idx < 0 || log_var_idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    if (!vars[log_var_idx].is_registered) {
        return FAILURE;
    }

    // Initialize the state machine context
    ctx_dump_ascii.state = DUMP_ASCII_TITLE;
    ctx_dump_ascii.var_idx = log_var_idx;
    ctx_dump_ascii.sample_idx = 0;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx_dump_ascii.tcb,
                       state_machine_dump_ascii_callback,
                       &ctx_dump_ascii,
                       "logdascii",
                       SM_DUMP_ASCII_INTERVAL_USEC);
    scheduler_tcb_register(&ctx_dump_ascii.tcb);

    return SUCCESS;
}

// ***************************
// Code for running the state machine to
// dump the log buffers to the UART using binary
// ***************************

typedef enum sm_states_dump_binary_e {
    DUMP_BINARY_MAGIC_HEADER = 1,
    DUMP_BINARY_NUM_SAMPLES,
    DUMP_BINARY_SAMPLE_INTERVAL_USEC,
    DUMP_BINARY_DATA_TYPE,
    DUMP_BINARY_SAMPLE_VALUE,
    DUMP_BINARY_MAGIC_FOOTER,
    DUMP_BINARY_PRINT_FOOTER_SPACE,
    DUMP_BINARY_REMOVE_TASK
} sm_states_dump_binary_e;

typedef struct sm_ctx_dump_binary_t {
    sm_states_dump_binary_e state;
    int var_idx;
    int sample_idx;
    uint32_t crc;
    task_control_block_t tcb;
} sm_ctx_dump_binary_t;

// State machine at 2kHz results in 2kSPS dumping,
// which is nearly full UART bandwidth at 112500 baud
#define SM_DUMP_BIANRY_UPDATES_PER_SEC (2000)
#define SM_DUMP_BINARY_INTERVAL_USEC   (USEC_IN_SEC / SM_DUMP_BIANRY_UPDATES_PER_SEC)

static const uint32_t MAGIC_HEADER = 0x12345678;
static const uint32_t MAGIC_FOOTER = 0x11223344;

void state_machine_dump_binary_callback(void *arg)
{
    sm_ctx_dump_binary_t *ctx = (sm_ctx_dump_binary_t *) arg;

    log_var_t *v = &vars[ctx->var_idx];
    buffer_entry_t *e = &v->buffer[ctx->sample_idx];

    switch (ctx->state) {

    case DUMP_BINARY_MAGIC_HEADER:
    {
        // Write to data stream output (UART)
        serial_write((char *) &MAGIC_HEADER, 4);
        serial_write((char *) &MAGIC_HEADER, 4);
        serial_write((char *) &MAGIC_HEADER, 4);
        serial_write((char *) &MAGIC_HEADER, 4);

        // Run data through CRC
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_HEADER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_HEADER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_HEADER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_HEADER, 4, ctx->crc);

        ctx->state = DUMP_BINARY_NUM_SAMPLES;
        break;
    }

    case DUMP_BINARY_NUM_SAMPLES:
    {
        uint32_t out = (uint32_t) v->num_samples;

        // Write to output data stream (UART)
        serial_write((char *) &out, 4);

        // Run data through CRC
        ctx->crc = crc32_calc_part((uint8_t *) &out, 4, ctx->crc);

        ctx->state = DUMP_BINARY_SAMPLE_INTERVAL_USEC;
        break;
    }

    case DUMP_BINARY_SAMPLE_INTERVAL_USEC:
    {
        uint32_t interval_usec = (uint32_t) v->log_interval_usec;

        // Write to output data stream (UART)
        serial_write((char *) &interval_usec, 4);

        // Run data through CRC
        ctx->crc = crc32_calc_part((uint8_t *) &interval_usec, 4, ctx->crc);

        ctx->state = DUMP_BINARY_DATA_TYPE;
        break;
    }

    case DUMP_BINARY_DATA_TYPE:
    {
        uint32_t var_type = (uint32_t) v->type;

        // Write to output data stream (UART)
        serial_write((char *) &var_type, 4);

        // Run data through CRC
        ctx->crc = crc32_calc_part((uint8_t *) &var_type, 4, ctx->crc);

        if (v->num_samples == 0) {
            // Nothing to dump!
            ctx->state = DUMP_BINARY_MAGIC_FOOTER;
        } else {
            ctx->state = DUMP_BINARY_SAMPLE_VALUE;
        }
        break;
    }

    case DUMP_BINARY_SAMPLE_VALUE:
    {
        // Dump the sampled value
        if (v->type == LOG_INT) {
            int32_t out = (int32_t) e->value;
            serial_write((char *) &out, 4);
            ctx->crc = crc32_calc_part((uint8_t *) &out, 4, ctx->crc);
        } else if (v->type == LOG_FLOAT || v->type == LOG_DOUBLE) {
            // During the sampling, the distinction between float and double variable types
            // was accounted for. The data is stored in the log array in the float format.
            float out = *((float *) &(e->value));
            serial_write((char *) &out, 4);
            ctx->crc = crc32_calc_part((uint8_t *) &out, 4, ctx->crc);
        }

        ctx->sample_idx++;

        // Stay in the current state until we have dumped enough samples
        if (ctx->sample_idx >= v->num_samples) {
            ctx->state = DUMP_BINARY_MAGIC_FOOTER;
        }

        break;
    }

    case DUMP_BINARY_MAGIC_FOOTER:
    {
        serial_write((char *) &MAGIC_FOOTER, 4);
        serial_write((char *) &MAGIC_FOOTER, 4);
        serial_write((char *) &MAGIC_FOOTER, 4);
        serial_write((char *) &MAGIC_FOOTER, 4);

        // Calculate CRC across footer bytes
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_FOOTER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_FOOTER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_FOOTER, 4, ctx->crc);
        ctx->crc = crc32_calc_part((uint8_t *) &MAGIC_FOOTER, 4, ctx->crc);

        // After we are done calculating the CRC, we must
        // invert it to match the Python implementation.
        ctx->crc = ~(ctx->crc);

        // The CRC includes the footer, so the client must
        // read an extra 4 bytes (the CRC bytes!)
        serial_write((char *) &(ctx->crc), 4);

        ctx->state = DUMP_BINARY_PRINT_FOOTER_SPACE;
        break;
    }

    case DUMP_BINARY_PRINT_FOOTER_SPACE:
    {
        debug_print("\r\n\r\n");
        ctx->state = DUMP_BINARY_REMOVE_TASK;
        break;
    }

    case DUMP_BINARY_REMOVE_TASK:
    default:
    {
        scheduler_tcb_unregister(&ctx->tcb);
        break;
    }
    }
}

static sm_ctx_dump_binary_t ctx_dump_binary;

int log_var_dump_uart_binary(int log_var_idx)
{
    // Sanity check variable idx
    if (log_var_idx < 0 || log_var_idx >= LOG_MAX_NUM_VARIABLES) {
        return FAILURE;
    }

    if (!vars[log_var_idx].is_registered) {
        return FAILURE;
    }

    // Initialize the state machine context
    ctx_dump_binary.state = DUMP_BINARY_MAGIC_HEADER;
    ctx_dump_binary.var_idx = log_var_idx;
    ctx_dump_binary.sample_idx = 0;

    // CRC-32 must be seeded as follows:
    ctx_dump_binary.crc = CRC32_DEFAULT_INIT;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx_dump_binary.tcb,
                       state_machine_dump_binary_callback,
                       &ctx_dump_binary,
                       "logdbin",
                       SM_DUMP_BINARY_INTERVAL_USEC);
    scheduler_tcb_register(&ctx_dump_binary.tcb);

    return SUCCESS;
}

// ***************************
// Code for printing log info
// ***************************

typedef enum sm_states_info_e {
    INFO_HEAD = 1,
    INFO_MAX_SLOTS,
    INFO_MAX_DEPTH,
    INFO_MAX_SAMPLE_RATE,

    INFO_VAR_TITLE,
    INFO_VAR_DATA1,
    INFO_VAR_DATA2,
    INFO_VAR_DATA3,
    INFO_VAR_DATA4,
    INFO_VAR_DATA5,

    INFO_NEXT_VAR,

    INFO_REMOVE_TASK
} sm_states_info_e;

typedef struct sm_ctx_info_t {
    sm_states_info_e state;
    task_control_block_t tcb;

    int var_idx;
} sm_ctx_info_t;

#define SM_INFO_UPDATES_PER_SEC SYS_TICK_FREQ
#define SM_INFO_INTERVAL_USEC   (USEC_IN_SEC / SM_INFO_UPDATES_PER_SEC)

void state_machine_info_callback(void *arg)
{
    sm_ctx_info_t *ctx = (sm_ctx_info_t *) arg;

    log_var_t *v = &vars[ctx->var_idx];

    switch (ctx->state) {
    case INFO_HEAD:
    {
        debug_printf("Log Info\r\n");
        debug_printf("--------\r\n");
        ctx->state = INFO_MAX_SLOTS;
        break;
    }

    case INFO_MAX_SLOTS:
    {
        debug_printf("Max slots: %d\r\n", LOG_MAX_NUM_VARIABLES);
        ctx->state = INFO_MAX_DEPTH;
        break;
    }

    case INFO_MAX_DEPTH:
    {
        debug_printf("Max sample depth: %d\r\n", LOG_SAMPLE_DEPTH_PER_VARIABLE);
        ctx->state = INFO_MAX_SAMPLE_RATE;
        break;
    }

    case INFO_MAX_SAMPLE_RATE:
    {
        debug_printf("Max sample rate: %d Hz\r\n", LOG_UPDATES_PER_SEC);
        debug_printf("--------\r\n");
        ctx->state = INFO_VAR_TITLE;
        break;
    }

    case INFO_VAR_TITLE:
    {
        if (v->is_registered) {
            debug_printf("Slot %d:\r\n", ctx->var_idx);
            ctx->state = INFO_VAR_DATA1;
        } else {
            debug_printf("Slot %d: unused\r\n", ctx->var_idx);
            ctx->state = INFO_NEXT_VAR;
        }
        break;
    }

    case INFO_VAR_DATA1:
    {
        debug_printf("  Name: %s\r\n", v->name);
        ctx->state = INFO_VAR_DATA2;
        break;
    }

    case INFO_VAR_DATA2:
    {
        if (v->type == LOG_INT) {
            debug_printf("  Type: int\r\n");
        } else if (v->type == LOG_FLOAT) {
            debug_printf("  Type: float\r\n");
        } else {
            debug_printf("  Type: double\r\n");
        }
        ctx->state = INFO_VAR_DATA3;
        break;
    }

    case INFO_VAR_DATA3:
    {
        debug_printf("  Memory address: 0x%X\r\n", v->addr);
        ctx->state = INFO_VAR_DATA4;
        break;
    }

    case INFO_VAR_DATA4:
    {
        debug_printf("  Sampling interval (usec): %d\r\n", v->log_interval_usec);
        ctx->state = INFO_VAR_DATA5;
        break;
    }

    case INFO_VAR_DATA5:
    {
        debug_printf("  Num samples: %d\r\n", v->num_samples);
        ctx->state = INFO_NEXT_VAR;
        break;
    }

    case INFO_NEXT_VAR:
    {
        ctx->var_idx++;
        if (ctx->var_idx >= LOG_MAX_NUM_VARIABLES) {
            debug_printf("\r\n");
            ctx->state = INFO_REMOVE_TASK;
        } else {
            ctx->state = INFO_VAR_TITLE;
        }
        break;
    }

    case INFO_REMOVE_TASK:
    default:
        scheduler_tcb_unregister(&ctx->tcb);
        break;
    }
}

static sm_ctx_info_t ctx_info;

int log_print_info(void)
{
    if (scheduler_tcb_is_registered(&ctx_info.tcb)) {
        // Already in process of printing something!!
        return FAILURE;
    }

    // Initialize the state machine context
    ctx_info.state = INFO_HEAD;
    ctx_info.var_idx = 0;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx_info.tcb, state_machine_info_callback, &ctx_info, "loginfo", SM_INFO_INTERVAL_USEC);
    scheduler_tcb_register(&ctx_info.tcb);

    return SUCCESS;
}

#endif // USER_CONFIG_ENABLE_LOGGING


<|start_filename|>sdk/bare/common/usr/pcbtest/sm_test.c<|end_filename|>
#ifdef APP_PCBTEST

#include "usr/pcbtest/sm_test.h"
#include "drv/analog.h"
#include "drv/hardware_targets.h"
#include "drv/pwm.h"
#include "sys/debug.h"
#include "sys/scheduler.h"
#include "sys/util.h"
#include "usr/user_config.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

static bool test_check_voltage(bool quiet_mode, analog_channel_e channel, float *voltage_out, float goal, float tol);

typedef enum sm_states_e {
    PRINT_HEADER = 0,

    // Verify +/- 10V functionality
    TEST1,
    TEST1_POS_10V_RAIL,
    TEST1_NEG_10V_RAIL,

    TEST2,
    TEST2_SET_PWM,
    TEST2_CHECK_VOUT1,
    TEST2_CHECK_VOUT2,

    END_TESTING,
    PRINT_SMART_DEBUG,
    REMOVE_TASK,
} sm_states_e;

typedef struct sm_ctx_t {
    sm_states_e state;
    task_control_block_t tcb;

    // Top level test config
    bool quiet_mode;
    int num_passes;
    int stack;
    char tb;
    float vdrive;

    // Old PWM info
    bool old_pwm_en;
    double old_fsw;
    uint16_t old_dt;

    // Test stats
    int num_completed_tests;
    int num_passed_tests;

    // TEST2 data
    int test2_bulk_runs;
    float curr_pwm_duty_random;
    float curr_pwm_goal_voltage1;
    float curr_pwm_goal_voltage2;
    int curr_pwm;
    int num_fails_by_vout[6];
} sm_ctx_t;

#define SM_UPDATES_PER_SEC (400)
#define SM_INTERVAL_USEC   (USEC_IN_SEC / SM_UPDATES_PER_SEC)

static void state_machine_callback(void *arg)
{
    sm_ctx_t *ctx = (sm_ctx_t *) arg;

    switch (ctx->state) {
    case PRINT_HEADER:
    {
        debug_printf("Automated PCB Test: Inverter Port %d%c\r\n", ctx->stack, ctx->tb);
        debug_printf("------------------------------------\r\n");
        debug_printf("Test started...\r\n");
        ctx->state = TEST1;
        break;
    }

    case TEST1:
    case TEST1_POS_10V_RAIL:
    {
        float voltage;
        bool pass = test_check_voltage(ctx->quiet_mode, ANALOG_IN7, &voltage, 10.0, 0.050);
        if (pass) {
            ctx->num_passed_tests++;
        }

        if (!ctx->quiet_mode) {
            debug_printf("+10V rail reads %8.4fV\r\n", voltage);
        }

        if (!pass) {
            // FAIL ... we can't read >0 voltages, so stop testing!
            debug_printf("PCB BROKEN: +10V rail read failure. Stopping test.\r\n\n");
            ctx->state = REMOVE_TASK;
        } else {
            // PASS
            ctx->state = TEST1_NEG_10V_RAIL;
        }

        ctx->num_completed_tests++;
        break;
    }

    case TEST1_NEG_10V_RAIL:
    {
        float voltage;
        bool pass = test_check_voltage(ctx->quiet_mode, ANALOG_IN8, &voltage, -10.0, 0.050);
        if (pass) {
            ctx->num_passed_tests++;
        }

        if (!ctx->quiet_mode) {
            debug_printf("-10V rail reads %8.4fV\r\n", voltage);
        }

        if (!pass) {
            // FAIL ... we can't read <0 voltages, so stop testing!
            debug_printf("PCB BROKEN: -10V rail read failure. Stopping test.\r\n\n");
            ctx->state = REMOVE_TASK;
        } else {
            // PASS
            ctx->state = TEST2;
        }

        ctx->num_completed_tests++;
        break;
    }

    case TEST2:
    {
        ctx->curr_pwm = 0;
        ctx->test2_bulk_runs = 0;

        for (int i = 0; i < 6; i++) {
            ctx->num_fails_by_vout[i] = 0;
        }

        ctx->state = TEST2_SET_PWM;
        break;
    }

    case TEST2_SET_PWM:
    {
        // Initialize PWM for TEST2
        int base = 6 * (ctx->stack - 1);
        if (ctx->tb == 'B') {
            base += 3;
        }

        // Set PWM output
        ctx->curr_pwm_duty_random = (float) rand() / (float) RAND_MAX;
        pwm_set_duty(base + ctx->curr_pwm, ctx->curr_pwm_duty_random);

        // Compute goal voltage based on VDRIVE and PWM parameters
        float pwm_dt = pwm_get_deadtime_ns();
        float pwm_fsw = pwm_get_switching_freq();
        float pwm_dt_percent = (((float) pwm_dt) / 1e9) / (1.0 / pwm_fsw);
        float pwm_dt_delta = pwm_dt_percent * ctx->vdrive;

        float goal1 = (ctx->vdrive * ctx->curr_pwm_duty_random) - pwm_dt_delta;
        float goal2 = -((ctx->vdrive * (1.0 - ctx->curr_pwm_duty_random)) - pwm_dt_delta);
        ctx->curr_pwm_goal_voltage1 = goal1;
        ctx->curr_pwm_goal_voltage2 = goal2;

        ctx->state = TEST2_CHECK_VOUT1;
        break;
    }

    case TEST2_CHECK_VOUT1:
    {
        int vout = 2 * ctx->curr_pwm;
        float voltage;
        bool pass = test_check_voltage(ctx->quiet_mode, vout, &voltage, ctx->curr_pwm_goal_voltage1, 0.100);
        if (pass) {
            ctx->num_passed_tests++;
        } else {
            ctx->num_fails_by_vout[vout]++;
        }

        if (!ctx->quiet_mode) {
            debug_printf("VOUT%d: goal = %8.4fV, meas = %8.4f\r\n", vout + 1, ctx->curr_pwm_goal_voltage1, voltage);
        }

        ctx->num_completed_tests++;
        ctx->state = TEST2_CHECK_VOUT2;
        break;
    }

    case TEST2_CHECK_VOUT2:
    {
        int vout = (2 * ctx->curr_pwm) + 1;
        float voltage;
        bool pass = test_check_voltage(ctx->quiet_mode, vout, &voltage, ctx->curr_pwm_goal_voltage2, 0.100);
        if (pass) {
            ctx->num_passed_tests++;
        } else {
            ctx->num_fails_by_vout[vout]++;
        }

        if (!ctx->quiet_mode) {
            debug_printf("VOUT%d: goal = %8.4fV, meas = %8.4f\r\n", vout + 1, ctx->curr_pwm_goal_voltage2, voltage);
        }

        ctx->num_completed_tests++;
        ctx->state = TEST2_SET_PWM;

        // Update state for next test
        ctx->curr_pwm++;
        if (ctx->curr_pwm >= 3) {
            ctx->curr_pwm = 0;
            ctx->test2_bulk_runs++;

            if (ctx->test2_bulk_runs >= ctx->num_passes) {
                ctx->state = END_TESTING;
            }
        }

        break;
    }

    case END_TESTING:
    {
        // Print status message to user
        debug_printf("%d of %d tests passed\r\n", ctx->num_passed_tests, ctx->num_completed_tests);

        if (!ctx->quiet_mode) {
            for (int i = 0; i < 6; i++) {
                debug_printf("VOUT%d fails: %d\r\n", i + 1, ctx->num_fails_by_vout[i]);
            }
        }

        debug_printf("\n");

        // Restore old PWM settings
        pwm_disable();
        pwm_set_switching_freq(ctx->old_fsw);
        pwm_set_deadtime_ns(ctx->old_dt);
        if (ctx->old_pwm_en) {
            pwm_enable();
        }

        ctx->state = PRINT_SMART_DEBUG;
        break;
    }

    case PRINT_SMART_DEBUG:
    {
        // Give the user some smart debugging info so they don't have to compute this stuff... :)
        debug_printf("Smart Debug\r\n");
        debug_printf("-----------\r\n");

        bool any_issues = false;
        bool pwm_line_is_broken[6] = { 0 };

        // Give info on analog input status
        for (int i = 0; i < 6; i++) {
            if (ctx->num_fails_by_vout[i] > ctx->num_passes / 2) {
                // VOUT is considered broken! > 50% failures
                debug_printf("ANALOG IN%d may be broken\r\n", i + 1);
                pwm_line_is_broken[i] = true;
                any_issues = true;
            }
        }

        // Give info on PWM output status

        int rawbase = 12 * (ctx->stack - 1);
        if (ctx->tb == 'B') {
            rawbase += 6;
        }

        int invnum = 2 * (ctx->stack - 1);
        if (ctx->tb == 'B') {
            invnum += 1;
        }

        for (int i = 0; i < 6; i++) {
            if (pwm_line_is_broken[i]) {
                // PWM output broken
                debug_printf("PWM OUT%d may be broken (INV%d_PWM%d)\r\n", rawbase + i + 1, invnum + 1, i + 1);
            }
        }

        if (any_issues) {
            debug_printf("\n");
            debug_printf("Good luck debugging! :(\r\n\n");
        } else {
            debug_printf("No hardware issues found! :)\r\n\n");
        }

        ctx->state = REMOVE_TASK;
        break;
    }

    case REMOVE_TASK:
    default:
    {
        scheduler_tcb_unregister(&ctx->tcb);
        break;
    }
    }
}

static sm_ctx_t ctx;

int sm_test_start_auto_test(bool quiet_mode, int num_passes, int stack, char tb, double vdrive)
{
    // Ensure test is not already running
    if (scheduler_tcb_is_registered(&ctx.tcb)) {
        return FAILURE;
    }

    // Store old PWM settings so we can revert to them later
    ctx.old_pwm_en = pwm_is_enabled();
    ctx.old_fsw = pwm_get_switching_freq();
    ctx.old_dt = pwm_get_deadtime_ns();

    // Update PWM to as fast as possible
    pwm_disable();

    // NOTE: as we increase Fsw, the dead-time percentage
    // increases, so the voltage we see in the tests goes
    // further from ideal...
    //
    // At 300kHz, this is 30x the LPF bandwidth, so we should
    // see fairly DC at the ADC.
    double fsw = MIN(300e3, PWM_MAX_SWITCHING_FREQ_HZ);
    if (pwm_set_switching_freq(fsw) != SUCCESS) {
        return FAILURE;
    }

    if (pwm_set_deadtime_ns(PWM_MIN_DEADTIME_NS) != SUCCESS) {
        return FAILURE;
    }

    pwm_enable();

    // Initialize state machine variables
    ctx.state = PRINT_HEADER;
    ctx.quiet_mode = quiet_mode;
    ctx.num_passes = num_passes;
    ctx.stack = stack;
    ctx.tb = tb;
    ctx.vdrive = vdrive;
    ctx.num_completed_tests = 0;
    ctx.num_passed_tests = 0;

    // Register state machine
    scheduler_tcb_init(&ctx.tcb, state_machine_callback, &ctx, "testsm", SM_INTERVAL_USEC);
    int err = scheduler_tcb_register(&ctx.tcb);

    return err;
}

// Helper function

static bool test_check_voltage(bool quiet_mode, analog_channel_e channel, float *voltage_out, float goal, float tol)
{
    float voltage;
    analog_getf(channel, &voltage);

    *voltage_out = voltage;

    bool is_pass = false;
    if (fabsf(voltage - goal) <= tol) {
        is_pass = true;
    }

    if (!quiet_mode) {
        if (is_pass) {
            debug_printf("\tPASS: ");
        } else {
            debug_printf("X\tFAIL: ");
        }
    }

    return is_pass;
}

#endif // APP_PCBTEST


<|start_filename|>sdk/bare/common/usr/demo/inverter.h<|end_filename|>
#ifndef INVERTER_H
#define INVERTER_H

#include <stdint.h>

void inverter_saturate_to_Vdc(int inv_idx, double *phase_voltage);
void inverter_set_voltage(int inv_idx, uint8_t pwm_idx, double phase_voltage);

void inverter_set_Vdc(int inv_idx, double Vdc);
double inverter_get_Vdc(int inv_idx);

#endif // INVERTER_H


<|start_filename|>sdk/bare/user/usr/blink/cmd/cmd_blink.h<|end_filename|>
#ifndef CMD_BLINK_H
#define CMD_BLINK_H

// Called in app init function to register command with system
void cmd_blink_register(void);

// Function called when user types "blink" command into command prompt
// i.e., this is the blink command handler function
int cmd_blink(int argc, char **argv);

#endif // CMD_BLINK_H


<|start_filename|>ip_repo/amdc_adc_1.0/drivers/amdc_adc_v1_0/src/amdc_adc.c<|end_filename|>


/***************************** Include Files *******************************/
#include "amdc_adc.h"

/************************** Function Definitions ***************************/


<|start_filename|>sdk/bare/common/drv/motherboard.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if (USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D) && (USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT == 1)

#include "drv/motherboard.h"
#include "drv/motherboard_defs.h"
#include "sys/cmd/cmd_mb.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include <stdbool.h>
#include <stdint.h>

static volatile uint32_t *m_motherboard;

void motherboard_init(uint32_t base_addr)
{
    // Store base address for IP
    m_motherboard = (volatile uint32_t *) base_addr;

    cmd_mb_register();

#if USER_CONFIG_ENABLE_MOTHERBOARD_AUTO_TX == 1
    motherboard_set_adc_sampling(true);
#else
    motherboard_set_adc_sampling(false);
#endif
}

void motherboard_set_adc_sampling(bool enable)
{
    // Read register from FPGA
    uint32_t reg = m_motherboard[MOTHERBOARD_DEFS_OFFSET_CONTROL / 4];

    // Clear bit
    BITCLEAR(reg, MOTHERBOARD_DEFS_CONTROL_SYNC_ADC_EN_SHIFT);

    // Set new bits based on user enable flag
    if (enable) {
        BITSET(reg, MOTHERBOARD_DEFS_CONTROL_SYNC_ADC_EN_SHIFT);
    }

    // Write register to FPGA
    m_motherboard[MOTHERBOARD_DEFS_OFFSET_CONTROL / 4] = reg;
}

void motherboard_request_new_data(void)
{
    // Read register from FPGA
    uint32_t reg = m_motherboard[MOTHERBOARD_DEFS_OFFSET_CONTROL / 4];

    // Toggle the bit
    BITFLIP(reg, MOTHERBOARD_DEFS_CONTROL_SYNC_TX_SHIFT);

    // Write register to FPGA
    m_motherboard[MOTHERBOARD_DEFS_OFFSET_CONTROL / 4] = reg;
}

int motherboard_get_data(mb_channel_e channel, int32_t *out)
{
    if (!motherboard_is_valid_channel(channel)) {
        return FAILURE;
    } else {
        *out = (int32_t) m_motherboard[channel];
        return SUCCESS;
    }
}

void motherboard_print_samples(void)
{
    for (int i = 0; i < 8; i++) {
        uint32_t val = m_motherboard[i];
        debug_printf("%i: %04X\r\n", i, val);
    }
}

void motherboard_print_counters(void)
{
    debug_printf("V: %08X\r\n", m_motherboard[MOTHERBOARD_DEFS_OFFSET_COUNT_VALID / 4]);
    debug_printf("C: %08X\r\n", m_motherboard[MOTHERBOARD_DEFS_OFFSET_COUNT_CORRUPT / 4]);
    debug_printf("T: %08X\r\n", m_motherboard[MOTHERBOARD_DEFS_OFFSET_COUNT_TIMEOUT / 4]);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/basic/src/prog_timer.c<|end_filename|>
/*
 * timer.c
 *
 *  Created on: Jun 13, 2014
 *      Author: sever212
 */
#if 0
#include "platform.h"
#include "xparameters.h"
#include "netif/xadapter.h"
#include "xgpio.h"
#include "xscutimer.h"
#include "xscugic.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include "xuartps_hw.h"
#include <stdio.h>
#include "commands.h"
#include "Log.h"
#include "prog_timer.h"
#include "lwip_glue.h"
#endif
#include "project_include.h"

static void TimerIntrHandler(void *CallBackRef);
static int TimerSetupIntrSystem(XScuGic *IntcInstancePtr,
								XScuTimer *TimerInstancePtr, u16 TimerIntrId);
//static void TimerDisableIntrSystem(XScuGic *IntcInstancePtr, u16 TimerIntrId);

volatile u32 MainTimerTimerTick;
volatile char bTimer100usTick = 0;
volatile u32 Timing10usErrorCount = 0;

int SetupTimer(XScuGic *IntcInstancePtr, XScuTimer * TimerInstancePtr,
		u16 TimerDeviceId, u16 TimerIntrId)
{
	int Status;
		//int Timer10ms = 0;
		XScuTimer_Config *ConfigPtr;

		/*
		 * Initialize the Scu Private Timer driver.
		 */
		ConfigPtr = XScuTimer_LookupConfig(TimerDeviceId);

		/*
		 * This is where the virtual address would be used, this example
		 * uses physical address.
		 */
		Status = XScuTimer_CfgInitialize(TimerInstancePtr, ConfigPtr,
						ConfigPtr->BaseAddr);
		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

		/*
		 * Perform a self-test to ensure that the hardware was built correctly.
		 */
		Status = XScuTimer_SelfTest(TimerInstancePtr);
		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

		/*
		 * Connect the device to interrupt subsystem so that interrupts
		 * can occur.
		 */
		Status = TimerSetupIntrSystem(IntcInstancePtr,
						TimerInstancePtr, TimerIntrId);
		if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

		/*
		 * Enable Auto reload mode.
		 */
		XScuTimer_EnableAutoReload(TimerInstancePtr);

		/*
		 * Load the timer counter register.
		 */
		XScuTimer_LoadTimer(TimerInstancePtr, TIMER_LOAD_VALUE);

		/*
		 * Start the timer counter and then wait for it
		 * to timeout a number of times.
		 */
		XScuTimer_Start(TimerInstancePtr);

		return XST_SUCCESS;
}



/*****************************
 * TimeElapsed10us()
 * Return the time elapsed since old in units of 10us
 *
 *
 * old: an old timer tick value
 */
u32 TimeElapsed10us(u32 old)
{
	if (MainTimerTimerTick >= old)
		return (MainTimerTimerTick - old);
	else
		return (0xFFFFFFFF - (old - MainTimerTimerTick)) + 1;
}

/*****************************************************************************/
/**
*
* This function sets up the interrupt system such that interrupts can occur
* for the device.
*
* @param	IntcInstancePtr is a pointer to the instance of XScuGic driver.
* @param	TimerInstancePtr is a pointer to the instance of XScuTimer
*		driver.
* @param	TimerIntrId is the Interrupt Id of the XScuTimer device.
*
* @return	XST_SUCCESS if successful, otherwise XST_FAILURE.
*
* @note		None.
*
******************************************************************************/
static int TimerSetupIntrSystem(XScuGic *IntcInstancePtr,
			      XScuTimer *TimerInstancePtr, u16 TimerIntrId)
{
	int Status;

	XScuGic_SetPriorityTriggerType(IntcInstancePtr, TimerIntrId, /*0xA8*/ 0 /*0xA0*/, 0x3);

	/*
	 * Connect the device driver handler that will be called when an
	 * interrupt for the device occurs, the handler defined above performs
	 * the specific interrupt processing for the device.
	 */
	Status = XScuGic_Connect(IntcInstancePtr, TimerIntrId,
				(Xil_ExceptionHandler)TimerIntrHandler,
				(void *)TimerInstancePtr);
	if (Status != XST_SUCCESS) {
		return Status;
	}

	/*
	 * Enable the interrupt for the device.
	 */
	XScuGic_Enable(IntcInstancePtr, TimerIntrId);

	/*
	 * Enable the timer interrupts for timer mode.
	 */
	XScuTimer_EnableInterrupt(TimerInstancePtr);


	return XST_SUCCESS;
}

/*****************************************************************************/
/**
*
* This function is the Interrupt handler for the Timer interrupt of the
* Timer device. It is called on the expiration of the timer counter in
* interrupt context.
*
* @param	CallBackRef is a pointer to the callback function.
*
* @return	None.
*
* @note		None.
*
******************************************************************************/
static void TimerIntrHandler(void *CallBackRef)
{

	XScuTimer *TimerInstancePtr = (XScuTimer *) CallBackRef;
	//static u32 local1us = 0;
	static u32 tickTo10Counter = 0, tickTo1msCounter = 0;
	static u32 Eth_1ms = 0;
	/*
	 * Check if the timer counter has expired, checking is not necessary
	 * since that's the reason this function is executed, this just shows
	 * how the callback reference can be used as a pointer to the instance
	 * of the timer counter that expired, increment a shared variable so
	 * the main thread of execution can see the timer expired.
	 */
	if (XScuTimer_IsExpired(TimerInstancePtr)) {
		XScuTimer_ClearInterruptStatus(TimerInstancePtr);

		MainTimerTimerTick++;
		//local1us += 10;

		if (++tickTo10Counter > 9)
		{
			//every 100 us code

			bTimer100usTick = 1;
			tickTo10Counter = 0;

			if (++tickTo1msCounter > 9)
			{
				//every 1ms code

				if ( (++Eth_1ms > 249) && (bEnableEthTimer == TRUE) )
				{
					Eth_1ms = 0;

					//Ethernet stuff needs to be dealt with every 250ms
					bEth250ms = TRUE;
				}
			}

		}

		DoLogging();

		/*if (Timer10ms == 500)
		{
			Timer10ms = 0;
			if (bTimer)
				print("5 seconds elapsed\r\n");
		}
		if (local1us >= 1000)
		{
			local1us = 0;
		}*/

		/*TimerExpired++;
		if (TimerExpired == 3) {
			XScuTimer_DisableAutoReload(TimerInstancePtr);
		}*/

		if (XScuTimer_IsExpired(TimerInstancePtr))
			Timing10usErrorCount++;
	}

}

/*****************************************************************************/
/**
*
* This function disables the interrupts that occur for the device.
*
* @param	IntcInstancePtr is the pointer to the instance of XScuGic
*		driver.
* @param	TimerIntrId is the Interrupt Id for the device.
*
* @return	None.
*
* @note		None.
*
******************************************************************************/
//#if 0
void TimerDisableIntrSystem(XScuGic *IntcInstancePtr, u16 TimerIntrId)
{
	/*
	 * Disconnect and disable the interrupt for the Timer.
	 */
	XScuGic_Disconnect(IntcInstancePtr, TimerIntrId);
}
//#endif



<|start_filename|>sdk/bare/common/drv/pwm.c<|end_filename|>
#include "drv/pwm.h"
#include "drv/hardware_targets.h"
#include "sys/defines.h"
#include "usr/user_config.h"
#include "xil_io.h"
#include <stdbool.h>
#include <stdio.h>

#define PWM_BASE_ADDR     (0x43C20000)
#define PWM_MUX_BASE_ADDR (0x43C40000)

static int pwm_set_carrier_divisor(uint8_t divisor);
static int pwm_set_carrier_max(uint16_t max);
static int pwm_set_duty_raw(pwm_channel_e channel, uint16_t value);

// Store current state of FPGA registers
static uint8_t carrier_divisor;
static uint16_t carrier_max;
static uint16_t deadtime;

// Store current state of user params
static uint16_t now_deadtime;
static double now_fsw;

// switching_freq = (200e6 / divisor) / (2*carrier_max)
// or
// carrier_max = ((200e6 / divisor) / (switching_freq)) / 2

void pwm_init(void)
{
    printf("PWM:\tInitializing...\n");

    // Default to no switching (all PWM outputs are logic LOW)
    // Opens all switches...
    pwm_disable();

    pwm_toggle_reset();

    pwm_set_switching_freq(PWM_DEFAULT_SWITCHING_FREQ_HZ);
    pwm_set_deadtime_ns(PWM_DEFAULT_DEADTIME_NS);

    // Initialize all PWMs to 50% duty output.
    // Since PWM is disabled, this has no effect now,
    // but if user enables PWM without updating registers,
    // 50% will appear at outputs.
    pwm_set_all_duty_midscale();

    // Initialize the PWM mux block in the FPGA to basic pass-through
    uint32_t mux_config_data[48];
    for (int i = 0; i < 48; i++) {
        mux_config_data[i] = i;
    }
    pwm_mux_set_all_pins(mux_config_data);
}

void pwm_set_all_duty_midscale(void)
{
    for (int i = 0; i < PWM_NUM_CHANNELS; i++) {
        pwm_set_duty(i, 0.5);
    }
}

void pwm_toggle_reset(void)
{
    // Toggles RST on all inverter outputs for 1 ms
    pwm_set_all_rst(0xFF);
    for (int i = 0; i < 83250; i++) {
        asm("nop");
    }
    pwm_set_all_rst(0x00);
    for (int i = 0; i < 83250; i++) {
        asm("nop");
    }
    pwm_set_all_rst(0xFF);
}

void pwm_set_all_rst(uint8_t rst)
{
    uint32_t value = 0;
    value |= (uint32_t) rst;

    // Offset 27 is rst output reg
    Xil_Out32(PWM_BASE_ADDR + (27 * sizeof(uint32_t)), value);
}

int pwm_enable(void)
{
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    // Write to slave reg 31 LSB to enable PWM switching
    Xil_Out32(PWM_BASE_ADDR + (31 * sizeof(uint32_t)), 0x00000001);

    return SUCCESS;
}

int pwm_disable(void)
{
    if (!pwm_is_enabled()) {
        return FAILURE;
    }

    // Write to slave reg 31 LSB to enable PWM switching
    Xil_Out32(PWM_BASE_ADDR + (31 * sizeof(uint32_t)), 0x00000000);

    return SUCCESS;
}

bool pwm_is_enabled(void)
{
    uint32_t reg31 = Xil_In32(PWM_BASE_ADDR + (31 * sizeof(uint32_t)));

    // LSB of reg 31 is enable bit for PWM
    return reg31 & 0x00000001;
}

int pwm_set_switching_freq(double freq_hz)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    // Based on FPGA, freq_hz can be in range: 1526Hz ... ~100MHz
    // For sanity, we limit this to: 2kHz to 2MHz
    if (freq_hz < PWM_MIN_SWITCHING_FREQ_HZ || freq_hz > PWM_MAX_SWITCHING_FREQ_HZ) {
        return FAILURE;
    }

    // Always set carrier_divisor to 0... anything else reduces resolution!
    pwm_set_carrier_divisor(0);

    // Calculate what the carrier_max should be to achieve the right switching freq
    carrier_max = (uint16_t)(((200e6 / (carrier_divisor + 1)) / (freq_hz)) / 2);
    pwm_set_carrier_max(carrier_max);

    // Store current freq so we can access later
    now_fsw = freq_hz;

    return SUCCESS;
}

int pwm_set_deadtime_ns(uint16_t time_ns)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    // FPGA only supports deadtime reg value from 5 to 2^16 - 1 (naturally supported using uint16_t)
    // Deadtime in ns is just reg value * 5. So, minimum deadtime is 25ns.

    // Ensure requested deadtime is >= 25 ns
    if (time_ns < PWM_MIN_DEADTIME_NS) {
        // Throw error so user knows this didn't work!
        return FAILURE;
    }

    // Convert time in ns to FPGA clock cycles
    deadtime = time_ns / 5;

    // NOTE: FPGA enforces minimum register value of 5
    // This should help prevent shoot-through events.

    // Write to slave reg 26 to set deadtime value
    Xil_Out32(PWM_BASE_ADDR + (26 * sizeof(uint32_t)), deadtime);

    // Store current deadtime so we can access later
    now_deadtime = time_ns;

    return SUCCESS;
}

double pwm_get_switching_freq(void)
{
    return now_fsw;
}

uint16_t pwm_get_deadtime_ns(void)
{
    return now_deadtime;
}

int pwm_set_duty(pwm_channel_e channel, double duty)
{
    if (!pwm_is_valid_channel(channel)) {
        return FAILURE;
    }

    if (duty >= 1.0) {
        pwm_set_duty_raw(channel, carrier_max);
    } else if (duty <= 0.0) {
        pwm_set_duty_raw(channel, 0);
    } else {
        pwm_set_duty_raw(channel, duty * carrier_max);
    }

    return SUCCESS;
}

static int pwm_set_duty_raw(pwm_channel_e channel, uint16_t value)
{
    if (!pwm_is_valid_channel(channel)) {
        return FAILURE;
    }

    // Write to offset 0 to control PWM 0
    Xil_Out32(PWM_BASE_ADDR + (channel * sizeof(uint32_t)), value);

    return SUCCESS;
}

static int pwm_set_carrier_divisor(uint8_t divisor)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    // FPGA only supports divisor from 0 to 255 (naturally supported using uint8_t)

    carrier_divisor = divisor;

    // Write to slave reg 24 to set triangle carrier clk divisor
    Xil_Out32(PWM_BASE_ADDR + (24 * sizeof(uint32_t)), divisor);

    return SUCCESS;
}

static int pwm_set_carrier_max(uint16_t max)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    // FPGA only supports carrier max from 0 to 2^16 - 1 (naturally supported using uint16_t)

    carrier_max = max;

    // Write to slave reg 25 to set triangle carrier max value
    Xil_Out32(PWM_BASE_ADDR + (25 * sizeof(uint32_t)), max);

    // Since we updated carrier max value, reset PWMs to new 50%
    pwm_set_all_duty_midscale();

    return SUCCESS;
}

// NOTE: we assume config is an array of length >= 48
int pwm_mux_set_all_pins(uint32_t *config)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    for (int i = 0; i < (PWM_NUM_CHANNELS * 2); i++) {
        Xil_Out32(PWM_MUX_BASE_ADDR + (i * sizeof(uint32_t)), config[i]);
    }

    return SUCCESS;
}

int pwm_mux_set_one_pin(uint32_t pwm_pin_idx, uint32_t config)
{
    // Only allow PWM configuration changes when switching is off
    if (pwm_is_enabled()) {
        return FAILURE;
    }

    if (pwm_pin_idx < 0 || pwm_pin_idx >= 48) {
        return FAILURE;
    }

    Xil_Out32(PWM_MUX_BASE_ADDR + (pwm_pin_idx * sizeof(uint32_t)), config);

    return SUCCESS;
}

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

void pwm_get_all_flt_temp(uint8_t *flt_temp)
{
    uint32_t value;

    // Offset 28 is flt_temp
    value = Xil_In32(PWM_BASE_ADDR + (28 * sizeof(uint32_t)));

    // Only look at bottom 8 bits
    value &= 0x000000FF;

    *flt_temp = (uint8_t) value;
}

void pwm_get_all_flt_desat(uint8_t *flt_desat)
{
    uint32_t value;

    // Offset 29 is flt_desat
    value = Xil_In32(PWM_BASE_ADDR + (29 * sizeof(uint32_t)));

    // Only look at bottom 8 bits
    value &= 0x000000FF;

    *flt_desat = (uint8_t) value;
}

void pwm_get_all_rdy(uint8_t *rdy)
{
    uint32_t value;

    // Offset 30 is rdy
    value = Xil_In32(PWM_BASE_ADDR + (30 * sizeof(uint32_t)));

    // Only look at bottom 8 bits
    value &= 0x000000FF;

    *rdy = (uint8_t) value;
}

int pwm_get_status(pwm_channel_e channel, pwm_status_t *status)
{
    if (!pwm_is_valid_channel(channel)) {
        return FAILURE;
    }

    // Read status signals from hardware
    uint8_t flt_temp, flt_desat, rdy;
    pwm_get_all_flt_temp(&flt_temp);
    pwm_get_all_flt_desat(&flt_desat);
    pwm_get_all_rdy(&rdy);

    uint8_t bit_mask = (1 << channel);

    status->fault_temp = (flt_temp & bit_mask) ? 1 : 0;
    status->fault_desat = (flt_desat & bit_mask) ? 1 : 0;
    status->ready = (rdy & bit_mask) ? 1 : 0;

    return SUCCESS;
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/sys/injection.h<|end_filename|>
#ifndef INJECTION_H
#define INJECTION_H

#include <stdint.h>

typedef enum inj_func_e {
    CONST = 1,
    NOISE,
    CHIRP,
    TRIANGLE,
    SQUARE,
    NONE,
} inj_func_e;

typedef enum inj_op_e {
    ADD = 1,
    SUB,
    SET,
} inj_op_e;

typedef struct inj_func_constant_t {
    double value;
} inj_func_constant_t;

typedef struct inj_func_noise_t {
    double gain;
    double offset;
} inj_func_noise_t;

typedef struct inj_func_chirp_t {
    double gain;
    double freqMin;
    double freqMax;
    double period;
} inj_func_chirp_t;

typedef struct inj_func_triangle_t {
    double valueMin;
    double valueMax;
    double period;
} inj_func_triangle_t;

typedef struct inj_func_square_t {
    double valueMin;
    double valueMax;
    double period;
} inj_func_square_t;

#define INJ_MAX_NAME_LENGTH (24)

typedef struct inj_ctx_t {
    int id;
    uint8_t registered;
    char name[INJ_MAX_NAME_LENGTH];
    struct inj_ctx_t *next;

    inj_func_e inj_func;
    inj_op_e operation;

    inj_func_constant_t constant;
    inj_func_noise_t noise;
    inj_func_chirp_t chirp;
    inj_func_triangle_t triangle;
    inj_func_square_t square;

    double curr_time;
} inj_ctx_t;

void injection_init(void);

void injection_ctx_init(inj_ctx_t *inj_ctx, char *name);
void injection_ctx_register(inj_ctx_t *inj_ctx);
void injection_ctx_unregister(inj_ctx_t *inj_ctx);
void injection_ctx_clear(inj_ctx_t *inj_ctx);

void injection_inj(double *output, inj_ctx_t *ctx, double Ts);

void injection_clear(void);
void injection_list(void);
void injection_const(inj_ctx_t *ctx, inj_op_e op, double value);
void injection_noise(inj_ctx_t *ctx, inj_op_e op, double gain, double offset);
void injection_chirp(inj_ctx_t *ctx, inj_op_e op, double gain, double freqMin, double freqMax, double period);
void injection_triangle(inj_ctx_t *ctx, inj_op_e op, double valueMin, double valueMax, double period);
void injection_square(inj_ctx_t *ctx, inj_op_e op, double valueMin, double valueMax, double period);

inj_ctx_t *injection_find_ctx_by_name(char *name);

#endif // INJECTION_H


<|start_filename|>sdk/bare/common/usr/demo/app_demo.h<|end_filename|>
#ifndef APP_DEMO_H
#define APP_DEMO_H

void app_demo_init(void);

#endif // APP_DEMO_H


<|start_filename|>sdk/basic/bsp/io.h<|end_filename|>
#ifndef IO_H
#define IO_H

#include <stdint.h>

typedef struct io_led_color_t {
	uint8_t r;
	uint8_t g;
	uint8_t b;
} io_led_color_t;

void io_init(void);

void io_led_set_c(uint8_t r, uint8_t g, uint8_t b, io_led_color_t *color);
void io_led_set(io_led_color_t *color);

void io_switch_get(uint8_t *sw1, uint8_t *sw2);
void io_button_get(uint8_t *btn1);

#endif // IO_H


<|start_filename|>ip_repo/amdc_motherboard_1.0/drivers/amdc_motherboard_v1_0/src/amdc_motherboard.h<|end_filename|>

#ifndef AMDC_MOTHERBOARD_H
#define AMDC_MOTHERBOARD_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_MOTHERBOARD_S00_AXI_SLV_REG15_OFFSET 60


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_MOTHERBOARD register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_MOTHERBOARDdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_MOTHERBOARD_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_MOTHERBOARD_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_MOTHERBOARD register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_MOTHERBOARD device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_MOTHERBOARD_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_MOTHERBOARD_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_MOTHERBOARD instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_MOTHERBOARD_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_MOTHERBOARD_H


<|start_filename|>sdk/bare/common/usr/dac/task_dac.c<|end_filename|>
#ifdef APP_DAC

#include "usr/dac/task_dac.h"
#include "drv/dac.h"
#include "drv/hardware_targets.h"
#include "sys/scheduler.h"
#include "usr/user_config.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>

// Scheduler TCB which holds task "context"
static task_control_block_t tcb;

// Scales the minimum phase step each callback cycle
// The minimum phase step is defined as 2 PI / callback frequency
static double FREQ = 1;

#define NUM_CHANNELS 3

int task_dac_init(void)
{
    if (scheduler_tcb_is_registered(&tcb)) {
        return FAILURE;
    }

    // Fill TCB with parameters
    scheduler_tcb_init(&tcb, task_dac_callback, NULL, "dac", TASK_DAC_INTERVAL_USEC);

    // Register task with scheduler
    return scheduler_tcb_register(&tcb);
}

int task_dac_deinit(void)
{
    // Unregister task with scheduler
    return scheduler_tcb_unregister(&tcb);
}

void task_dac_trigger()
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Initiate a trigger
    dac_set_trigger();
#endif // USER_CONFIG_HARDWARE_TARGET
}
void task_dac_frequency(double freq)
{
    // Update the frequency
    FREQ = freq;
}

void task_dac_broadcast(double voltage)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Initiate a broadcast
    dac_set_broadcast(voltage);
#endif // USER_CONFIG_HARDWARE_TARGET
}

void task_dac_set_voltage(uint8_t ch, double voltage)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Set the given channels voltage to given voltage
    dac_set_voltage(ch, voltage);
#endif // USER_CONFIG_HARDWARE_TARGET
}

void task_dac_set_reg(uint8_t reg, uint32_t value)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Write the raw value given in hex to the given register
    dac_set_raw(reg, value);
#endif // USER_CONFIG_HARDWARE_TARGET
}

void task_dac_callback(void *arg)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    static int divider = 0; // Divides the frequency
    static double step = 0;

    divider++;

    // If the frequency is less than 1Hz (default) the callback will return and do
    // nothing until the appropriate amount of cycles have passed for the desired
    // frequency to be met
    if (divider >= 1 / FREQ) {

        // If the frequency is greater than 1Hz (default) the step is scaled by the
        // desired frequency
        if (FREQ > 1) {
            step += FREQ;
        } else {
            step++;
        }

        // Reset the divider for the next cycle
        divider = 0;

        // Reset the step for the next cycle
        if (step > TASK_DAC_UPDATES_PER_SEC)
            step = 0;

        // Generates a 3-phase +/- 10V output on channels 0, 2, and 4
        for (int i = 0; i < 3; i++) {
            dac_set_voltage(i * 2, 10 * sin(TASK_DAC_MIN_PHASE * step + ((double) i / 3 * 2 * PI)));
        }

        // Send all channel updates through
        dac_set_trigger();
    }
#endif // USER_CONFIG_HARDWARE_TARGET
}

#endif // APP_BLINK


<|start_filename|>sdk/basic/src/commands.h<|end_filename|>
#ifndef __COMMANDS_H
#define __COMMANDS_H

extern void *StreamFIFOComm;
extern u32 StreamFIFO_MaxPacket;
extern char bPrimeStreamFIFO;	//set this to true if the streaming FIFO needs to be loaded from the main loop (instead of the callback)

extern int MakeMostFullLFR(char *szResponse, u32 uiMaxLength, u32 *SQnum);
extern int ReloadStreamFIFO(void * comm);

extern int CommandParser(const char * szCmd, char *szResponse, void *CommDevice);
extern u32 HandleCommandsUart(char *szRcvCmd, u32 state);

extern u32 HandleCommandsGeneral(char *szOutData, char *szRcvCmd, char *szNewData, u32 NewDataLength, u32 *state, void *ComDevice);

extern void HBA_100usTick();
extern void VSI_100usTick(void);
#endif //__COMMANDS_H




<|start_filename|>sdk/bare/common/drv/led.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#include "drv/led.h"
#include "xil_io.h"
#include <stdint.h>

#define LED_BASE_ADDR (0x43C30000)

// Color Data Bit Format
// =====================
//
// NOTE: see datasheet for bit ordering:
//
// 24-bit data format
//
// MSB -- G8R8B8 -- LSB
//
// Bits 23-16 : green
// Bits 15-8  : red
// Bits 7-0   : blue

void led_init(void)
{
    // Turn off all LEDs to start
    for (uint8_t i = 0; i < NUM_LEDS; i++) {
        led_set_raw(i, 0);
    }
}

void led_set_color(led_t idx, led_color_t color)
{
    uint32_t raw = 0;
    raw |= ((uint32_t) color.g & 0x000000FF) << 16;
    raw |= ((uint32_t) color.r & 0x000000FF) << 8;
    raw |= ((uint32_t) color.b & 0x000000FF) << 0;

    led_set_raw(idx, raw);
}

void led_set_color_bytes(led_t idx, uint8_t r, uint8_t g, uint8_t b)
{
    uint32_t raw = 0;
    raw |= ((uint32_t) g & 0x000000FF) << 16;
    raw |= ((uint32_t) r & 0x000000FF) << 8;
    raw |= ((uint32_t) b & 0x000000FF) << 0;

    led_set_raw(idx, raw);
}

void led_set_raw(led_t idx, uint32_t color)
{
    // NOTE: Writing color values to the device registers will
    // automatically trigger the transmission of color data
    // out to the LEDs.
    //
    // The user does not have to manually trigger the transmission.

    Xil_Out32(LED_BASE_ADDR + (idx * sizeof(uint32_t)), color);
}

led_color_t led_get_color(led_t idx)
{
    uint32_t raw = led_get_raw(idx);

    led_color_t ret;
    ret.g = (raw & 0x00FF0000) >> 16;
    ret.r = (raw & 0x0000FF00) >> 8;
    ret.b = (raw & 0x000000FF) >> 0;

    return ret;
}

uint32_t led_get_raw(led_t idx)
{
    return Xil_In32(LED_BASE_ADDR + (idx * sizeof(uint32_t)));
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/sys/cmd/cmd_hw.h<|end_filename|>
#ifndef CMD_HW_H
#define CMD_HW_H

void cmd_hw_register(void);

int cmd_hw(int argc, char **argv);

#endif // CMD_HW_H


<|start_filename|>sdk/bare/common/sys/cmd/cmd_mb.h<|end_filename|>
#ifndef CMD_MB_H
#define CMD_MB_H

void cmd_mb_register(void);

int cmd_mb(int argc, char **argv);

#endif // CMD_MB_H


<|start_filename|>sdk/bare/common/drv/ild1420.c<|end_filename|>
#include "drv/ild1420.h"
#include "xil_io.h"
#include <stdint.h>

// IP base addresses per mapping in Vivado
static const uint32_t addr_mapping[ILD1420_NUM_SENSORS] = {
    0x43CA0000,
    0x43CB0000,
    0x43CC0000,
    0x43CD0000,
};

ild1420_packet_t ild1420_get_latest_packet(ild1420_sensor_t sensor)
{
    // Read the raw output of the first slave register in the FPGA
    uint32_t reg = Xil_In32(addr_mapping[sensor]);

    // Parse the raw output into our data
    ild1420_packet_t ret;
    ret.distance = reg & 0x0000FFFF;
    ret.error = (reg >> 16) & 0x3;
    ret.fresh = (reg >> 18) & 0x1;

    return ret;
}


<|start_filename|>sdk/bare/common/usr/dac/app_dac.c<|end_filename|>
#ifdef APP_DAC

#include "usr/dac/app_dac.h"
#include "usr/dac/cmd/cmd_dac.h"
#include "usr/dac/task_dac.h"

void app_dac_init(void)
{
    // Registers the "dac" command with the system
    cmd_dac_register();

    // Initialize the dac task with the system
    task_dac_init();
}

#endif // APP_DAC


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_mc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_mc.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/task_mc.h"
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (5)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "init", "Start motion controller" },
    { "deinit", "Stop motion controller" },
    { "rpm <rpms>", "Set commanded speed" },
    { "cff <on|off>", "Enable / disable command feed-forward" },
    { "omega_m_src <enc|est>", "Set omega_m source between encoder and estimation" },
};

void cmd_mc_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "mc", "Motion controller commands", cmd_help, NUM_HELP_ENTRIES, cmd_mc);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

int cmd_mc(int argc, char **argv)
{
    // Handle 'init' sub-command
    if (argc == 2 && strcmp("init", argv[1]) == 0) {
        // Make sure mc task was not already inited
        if (task_mc_is_inited())
            return CMD_FAILURE;

        task_mc_init();
        return CMD_SUCCESS;
    }

    // Handle 'deinit' sub-command
    if (argc == 2 && strcmp("deinit", argv[1]) == 0) {
        // Make sure mc task was already inited
        if (!task_mc_is_inited())
            return CMD_FAILURE;

        task_mc_deinit();
        return CMD_SUCCESS;
    }

    // Handle 'rpm ...' sub-command
    if (argc == 3 && strcmp("rpm", argv[1]) == 0) {
        // Pull out rpms argument
        // and saturate to -10,000 .. 10,000 RPMs
        double rpms = (double) atoi(argv[2]);
        if (rpms < -10000.0)
            return CMD_INVALID_ARGUMENTS;
        if (rpms > 10000.0)
            return CMD_INVALID_ARGUMENTS;

        task_mc_set_omega_m_star(PI2 * rpms / 60.0);

        return CMD_SUCCESS;
    }

    // Handle 'cff ...' sub-command
    if (argc == 3 && strcmp("omega_m_src", argv[1]) == 0) {
        // Pull out est/enc argument
        uint8_t use_encoder;
        if (strcmp("enc", argv[2]) == 0) {
            use_encoder = 1;
        } else if (strcmp("est", argv[2]) == 0) {
            use_encoder = 0;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        task_mc_set_omega_m_src(use_encoder);

        return CMD_SUCCESS;
    }

    // Handle 'cff ...' sub-command
    if (argc == 3 && strcmp("cff", argv[1]) == 0) {
        // Pull out onoff argument
        uint8_t enabled;
        if (strcmp("on", argv[2]) == 0) {
            // On
            enabled = 1;
        } else if (strcmp("off", argv[2]) == 0) {
            enabled = 0;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        task_mc_set_cff_enabled(enabled);

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/sys/cmd/cmd_help.h<|end_filename|>
#ifndef CMD_HELP_H
#define CMD_HELP_H

void cmd_help_register(void);

int cmd_help(int argc, char **argv);

#endif // CMD_HELP_H


<|start_filename|>sdk/bare/common/drv/hardware_targets.h<|end_filename|>
#ifndef HARDWARE_TARGETS_H
#define HARDWARE_TARGETS_H

// This file holds the defines for all possible hardware targets.

// As the AMDC platform matures, the supported hardware targets will change.

#define AMDC_REV_C (3)
#define AMDC_REV_D (4)

#endif // HARDWARE_TARGETS_H


<|start_filename|>sdk/bare/common/usr/dac/task_dac.h<|end_filename|>
#ifndef TASK_DAC_H
#define TASK_DAC_H

#include "sys/scheduler.h"

// Frequency that this task is called (in Hz)
//
// Must be less than or equal to scheduler updates per second
// This value is defined in sys/scheduler.h and defaults to 10kHz.
// Note that it can be overridden via usr/user_config.h
#define TASK_DAC_UPDATES_PER_SEC (10000)

// Microseconds interval between when task is called
//
// This is what scheduler actually uses to run task,
// but is generated via define above
#define TASK_DAC_INTERVAL_USEC (USEC_IN_SEC / TASK_DAC_UPDATES_PER_SEC)

// The minimum phase step between each time the task is called
// The default output of the any channel updated in the callback will
// always be 1Hz regardless of the task updates per second.
#define TASK_DAC_MIN_PHASE (2 * PI) / (TASK_DAC_UPDATES_PER_SEC)

// Called in app init function to set up task (or via command)
int task_dac_init(void);
int task_dac_deinit(void);
void task_dac_trigger(void);
void task_dac_frequency(double);
void task_dac_broadcast(double);
void task_dac_set_voltage(uint8_t, double);
void task_dac_set_reg(uint8_t, uint32_t);

// Callback function which scheduler calls periodically
void task_dac_callback(void *arg);

#endif // TASK_BLINK_H


<|start_filename|>sdk/bare/common/sys/serial.h<|end_filename|>
#ifndef SERIAL_H
#define SERIAL_H

#include "sys/scheduler.h"

#define SERIAL_UPDATES_PER_SEC (10000)
#define SERIAL_INTERVAL_USEC   (USEC_IN_SEC / SERIAL_UPDATES_PER_SEC)

void serial_init(void);
void serial_callback(void *arg);

void serial_write(char *msg, int len);

#endif // SERIAL_H


<|start_filename|>sdk/bare/user/usr/user_apps.c<|end_filename|>
#include "usr/user_apps.h"

// Adding applications:
//
// To add a new application, create a block
// like the other example(s) below which
// conditionally includes the app's header file.
//
// Also, conditionally call the app's init function.

#ifdef APP_PCBTEST
#include "usr/pcbtest/app_pcbtest.h"
#endif

#ifdef APP_BLINK
#include "usr/blink/app_blink.h"
#endif

#ifdef APP_BETA_LABS
#include "usr/beta_labs/app_beta_labs.h"
#endif

#ifdef APP_DEMO
#include "usr/demo/app_demo.h"
#endif

#ifdef APP_DAC
#include "usr/dac/app_dac.h"
#endif

void user_apps_init(void)
{
#ifdef APP_PCBTEST
    app_pcbtest_init();
#endif

#ifdef APP_BLINK
    app_blink_init();
#endif

#ifdef APP_BETA_LABS
    app_beta_labs_init();
#endif

#ifdef APP_DEMO
    app_demo_init();
#endif

#ifdef APP_DAC
    app_dac_init();
#endif
}


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_inv.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_inv.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/inverter.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (2)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "dtc <uDcomp> <mCurrLimit>", "Set deadtime compensation" },
    { "Vdc <mVdc>", "Set Vdc" },
};

void cmd_inv_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "inv", "Inverter commands", cmd_help, NUM_HELP_ENTRIES, cmd_inv);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

//
// Handles the 'inv' command
// and all sub-commands
//
int cmd_inv(int argc, char **argv)
{
    // Handle 'dtc' sub-command
    if (strcmp("dtc", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 4)
            return CMD_INVALID_ARGUMENTS;

        // Pull out uDcomp argument
        // and saturate to 0 .. 0.2
        double uDcomp = (double) atoi(argv[2]);
        if (uDcomp > 200000.0)
            return CMD_INVALID_ARGUMENTS;
        if (uDcomp < 0.0)
            return CMD_INVALID_ARGUMENTS;

        // Pull out mCurrLimit argument
        // and saturate to 0 ... 4A
        double mCurrLimit = (double) atoi(argv[3]);
        if (mCurrLimit > 4000.0)
            return CMD_INVALID_ARGUMENTS;
        if (mCurrLimit < 0.0)
            return CMD_INVALID_ARGUMENTS;

        inverter_set_dtc(uDcomp / 1000000.0, mCurrLimit / 1000.0);
        return CMD_SUCCESS;
    }

    // Handle 'Vdc' sub-command
    if (strcmp("Vdc", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 3)
            return CMD_INVALID_ARGUMENTS;

        // Pull out mVdc argument
        // and saturate to 0 .. 100V
        double mVdc = (double) atoi(argv[2]);
        if (mVdc > 100000.0)
            return CMD_INVALID_ARGUMENTS;
        if (mVdc < 0.0)
            return CMD_INVALID_ARGUMENTS;

        inverter_set_Vdc(mVdc / 1000.0);
        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>ip_repo/amdc_inverters_1.0/drivers/amdc_inverters_v1_0/src/amdc_inverters.h<|end_filename|>

#ifndef AMDC_INVERTERS_H
#define AMDC_INVERTERS_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_INVERTERS_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_INVERTERS_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_INVERTERS_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_INVERTERS_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_INVERTERS_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_INVERTERS_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_INVERTERS_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_INVERTERS_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_INVERTERS_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_INVERTERS_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_INVERTERS_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_INVERTERS_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_INVERTERS_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_INVERTERS_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_INVERTERS_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_INVERTERS_S00_AXI_SLV_REG15_OFFSET 60
#define AMDC_INVERTERS_S00_AXI_SLV_REG16_OFFSET 64
#define AMDC_INVERTERS_S00_AXI_SLV_REG17_OFFSET 68
#define AMDC_INVERTERS_S00_AXI_SLV_REG18_OFFSET 72
#define AMDC_INVERTERS_S00_AXI_SLV_REG19_OFFSET 76
#define AMDC_INVERTERS_S00_AXI_SLV_REG20_OFFSET 80
#define AMDC_INVERTERS_S00_AXI_SLV_REG21_OFFSET 84
#define AMDC_INVERTERS_S00_AXI_SLV_REG22_OFFSET 88
#define AMDC_INVERTERS_S00_AXI_SLV_REG23_OFFSET 92
#define AMDC_INVERTERS_S00_AXI_SLV_REG24_OFFSET 96
#define AMDC_INVERTERS_S00_AXI_SLV_REG25_OFFSET 100
#define AMDC_INVERTERS_S00_AXI_SLV_REG26_OFFSET 104
#define AMDC_INVERTERS_S00_AXI_SLV_REG27_OFFSET 108
#define AMDC_INVERTERS_S00_AXI_SLV_REG28_OFFSET 112
#define AMDC_INVERTERS_S00_AXI_SLV_REG29_OFFSET 116
#define AMDC_INVERTERS_S00_AXI_SLV_REG30_OFFSET 120
#define AMDC_INVERTERS_S00_AXI_SLV_REG31_OFFSET 124
#define AMDC_INVERTERS_S00_AXI_SLV_REG32_OFFSET 128
#define AMDC_INVERTERS_S00_AXI_SLV_REG33_OFFSET 132
#define AMDC_INVERTERS_S00_AXI_SLV_REG34_OFFSET 136
#define AMDC_INVERTERS_S00_AXI_SLV_REG35_OFFSET 140
#define AMDC_INVERTERS_S00_AXI_SLV_REG36_OFFSET 144
#define AMDC_INVERTERS_S00_AXI_SLV_REG37_OFFSET 148
#define AMDC_INVERTERS_S00_AXI_SLV_REG38_OFFSET 152
#define AMDC_INVERTERS_S00_AXI_SLV_REG39_OFFSET 156
#define AMDC_INVERTERS_S00_AXI_SLV_REG40_OFFSET 160
#define AMDC_INVERTERS_S00_AXI_SLV_REG41_OFFSET 164
#define AMDC_INVERTERS_S00_AXI_SLV_REG42_OFFSET 168
#define AMDC_INVERTERS_S00_AXI_SLV_REG43_OFFSET 172
#define AMDC_INVERTERS_S00_AXI_SLV_REG44_OFFSET 176
#define AMDC_INVERTERS_S00_AXI_SLV_REG45_OFFSET 180
#define AMDC_INVERTERS_S00_AXI_SLV_REG46_OFFSET 184
#define AMDC_INVERTERS_S00_AXI_SLV_REG47_OFFSET 188


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_INVERTERS register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_INVERTERSdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_INVERTERS_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_INVERTERS_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_INVERTERS register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_INVERTERS device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_INVERTERS_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_INVERTERS_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_INVERTERS instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_INVERTERS_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_INVERTERS_H


<|start_filename|>sdk/bare/common/drv/bsp.h<|end_filename|>
#ifndef BSP_H
#define BSP_H

void bsp_init(void);

#endif // BSP_H


<|start_filename|>sdk/bare/common/sys/util.c<|end_filename|>
#include "sys/util.h"


<|start_filename|>sdk/bare/user/usr/beta_labs/co.h<|end_filename|>
#ifndef CO_H
#define CO_H

void co_update(double *Idq0, double *Vdq0, double omega_e);

void co_get_Idq_hat(double *Id_hat, double *Iq_hat);
void co_get_Esal_hat(double *Esal_d_hat, double *Esal_q_hat);

#endif // CO_H


<|start_filename|>sdk/bare/common/sys/statistics.h<|end_filename|>
#ifndef STATISTICS_H
#define STATISTICS_H

#include <stdbool.h>
#include <stdint.h>

typedef struct {
    double min;
    double max;
    double mean;
    double value;
    uint32_t num_samples;

    // Private variables
    double __old_mean;
    double __old_s;
    double __s;
} statistics_t;

void statistics_init(statistics_t *stats);
void statistics_clear(statistics_t *stats);
void statistics_push(statistics_t *stats, double val);
double statistics_variance(statistics_t *stats);

#endif // STATISTICS_H


<|start_filename|>sdk/bare/common/usr/pcbtest/sm_test.h<|end_filename|>
#ifndef SM_TEST_H
#define SM_TEST_H

#include <stdbool.h>

int sm_test_start_auto_test(bool quiet_mode, int num_passes, int stack, char tb, double vdrive);

#endif // SM_TEST_H


<|start_filename|>sdk/bare/common/sys/transform.h<|end_filename|>
#ifndef TRANSFORM_H
#define TRANSFORM_H

#define C_INVARIANT_POWER     (0.816496580927726032732) // sqrt(2/3)
#define C_INVARIANT_AMPLITUDE (0.666666666666666666667) // 2/3

typedef enum { TRANS_DQZ_C_INVARIANT_AMPLITUDE = 0, TRANS_DQZ_C_INVARIANT_POWER } transform_dqz_type_e;

void transform_dqz(transform_dqz_type_e C, double theta, double *abc, double *dqz);
void transform_dqz_inverse(transform_dqz_type_e C, double theta, double *abc, double *dqz);

void transform_clarke(transform_dqz_type_e C, double *abc, double *xyz);
void transform_park(double theta, double *xyz, double *dqz);

void transform_clarke_inverse(transform_dqz_type_e C, double *abc, double *xyz);
void transform_park_inverse(double theta, double *xyz, double *dqz);

#endif // TRANSFORM_H


<|start_filename|>sdk/basic/src/lwip_glue.h<|end_filename|>

#ifndef __LWIP_GLUE_H
#define __LWIP_GLUE_H
extern void lwip_timer_callback();
extern int SetupLWIP();
extern int start_tcpip();

extern volatile char bEnableEthTimer;
extern struct netif *echo_netif;
extern char bEth250ms;

#endif //__LWIP_GLUE_H


<|start_filename|>sdk/bare/user/usr/beta_labs/bemfo.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/bemfo.h"
#include "usr/beta_labs/machine.h"
#include "usr/beta_labs/task_cc.h"
#include <math.h>

// Tuning for Back EMF Observer with Ts = 1/20000 sec
// Bandwidth is tuned to 20 Hz
#define Ts  (5e-05)
#define Jp  (0.00636)
#define Bo  (0.7253749394602439)
#define Ko  (7.474395723324704)
#define Kio (6.938868479366282)

typedef struct complex_t {
    double d;
    double q;
} complex_t;

// Forward declarations
static inline double sign(double x);
static inline double cross_product(complex_t *v1, complex_t *v2);

static double theta_m_error_acc;
static double Tem_hat;
static double alpha_hat;

static double omega_m_hat_l;
static double omega_m_hat_l_next;
static double omega_m_hat_r;
static double omega_m_hat_r_last;
static double theta_m_hat_last;
static double theta_m_hat;
static double theta_e_hat_last;
static double theta_e_hat;
static double delta_theta_m_hat;

// Logging variables
double LOG_Esal_vec_d = 0.0;
double LOG_Esal_vec_q = 0.0;
double LOG_unit_vec_d = 0.0;
double LOG_unit_vec_q = 0.0;

complex_t unit_vec;
complex_t Esal_vec;

void bemfo_init(void)
{
    // Clear states
    theta_m_error_acc = 0.0;
    omega_m_hat_l = 0.0;
    omega_m_hat_r = 0.0;
    theta_m_hat = 0.0;
    theta_e_hat = 0.0;
}

void bemfo_start(void)
{
    // Reset the state
    theta_m_error_acc = 0.0;
    omega_m_hat_l = 0.0;
    omega_m_hat_r = 0.0;

    // Pre-load theta_m_hat
    theta_m_hat = task_cc_get_theta_e_enc() / POLE_PAIRS;
}

void bemfo_update(double Esal_alpha, double Esal_beta, double Tcff_hat)
{
    // -------------------------
    // Save 'last' values
    // -------------------------

    omega_m_hat_r_last = omega_m_hat_r;
    theta_m_hat_last = theta_m_hat;
    theta_e_hat_last = theta_e_hat;

    // -------------------------
    // Heterodyning Demodulation
    // -------------------------

    theta_e_hat = theta_m_hat_last * POLE_PAIRS;
    theta_e_hat = fmod(theta_e_hat, PI2);

    // NOTE: If theta_e_hat comes from encoder, then the unit vector and Esal should be in-phase.
    // Because of our system setup, we must phase shift theta_e_hat by -90 degrees.
    // (This is because of the way we define our DQ transformation.)
    unit_vec.d = sign(omega_m_hat_r_last) * cos(theta_e_hat + (PI / 2.0));
    unit_vec.q = sign(omega_m_hat_r_last) * sin(theta_e_hat + (PI / 2.0));

    // Normalize Esal vector to -1..1 in magnitude
    double Esal_mag = sqrt((Esal_alpha * Esal_alpha) + (Esal_beta * Esal_beta));
    Esal_vec.d = Esal_alpha / Esal_mag;
    Esal_vec.q = Esal_beta / Esal_mag;

    LOG_Esal_vec_d = Esal_vec.d;
    LOG_Esal_vec_q = Esal_vec.q;
    LOG_unit_vec_d = unit_vec.d;
    LOG_unit_vec_q = unit_vec.q;

    // Generate error term (in mechanical degrees)
    double theta_e_error = cross_product(&Esal_vec, &unit_vec);
    double theta_m_error = theta_e_error / POLE_PAIRS;

    theta_m_error_acc += theta_m_error;

    // -------------------------
    // Saliency Tracking Observer
    // -------------------------

    Tem_hat = theta_m_error_acc * Kio * Ts + theta_m_error * Ko + 0;

    alpha_hat = Tem_hat * Ts / Jp; // note: alpha has Ts multiplied in

    omega_m_hat_l = omega_m_hat_l_next;
    omega_m_hat_l_next = omega_m_hat_l_next + alpha_hat;
    omega_m_hat_r = omega_m_hat_l + theta_m_error * Bo / Jp;
    delta_theta_m_hat = (omega_m_hat_r + omega_m_hat_r_last) * Ts / 2.0;
    theta_m_hat = theta_m_hat + delta_theta_m_hat;

    theta_m_hat = fmod(theta_m_hat, PI2);
}

double bemfo_get_omega_m_hat(void)
{
    return omega_m_hat_r_last;
}

double bemfo_get_omega_e_hat(void)
{
    return omega_m_hat_r_last * POLE_PAIRS;
}

double bemfo_get_theta_e_hat(void)
{
    return theta_e_hat_last;
}

static inline double sign(double x)
{
    if (x >= 0.0)
        return 1.0;
    return -1.0;
}

static inline double cross_product(complex_t *v1, complex_t *v2)
{
    return (v1->q * v2->d) - (v1->d * v2->q);
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/motherboard.h<|end_filename|>
#ifndef MOTHERBOARD_H
#define MOTHERBOARD_H

#include "drv/hardware_targets.h"
#include "usr/user_config.h"
#include <stdbool.h>
#include <stdint.h>

// Throw compiler error if the motherboard is not supported on the hardware target
// NOTE: this won't be an issue if the user code never includes this file!
#if USER_CONFIG_HARDWARE_TARGET != AMDC_REV_D
#error "SensorCard motherboard interface is only supported on the AMDC REV D hardware target."
#endif

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
#define MOTHERBOARD_BASE_ADDR (0x43C90000)
#endif

typedef enum {
    // Keep first channel index at 0!
    MB_IN1 = 0,
    MB_IN2,
    MB_IN3,
    MB_IN4,
    MB_IN5,
    MB_IN6,
    MB_IN7,
    MB_IN8,

    // Keep this as last entry!
    MB_NUM_CHANNELS,
} mb_channel_e;

static inline bool motherboard_is_valid_channel(mb_channel_e channel)
{
    if (channel >= MB_IN1 && channel < MB_NUM_CHANNELS) {
        return true;
    }

    return false;
}

void motherboard_init(uint32_t base_addr);

void motherboard_set_adc_sampling(bool enable);
void motherboard_request_new_data(void);

int motherboard_get_data(mb_channel_e channel, int32_t *out);

void motherboard_print_samples(void);
void motherboard_print_counters(void);

#endif // MOTHERBOARD_H


<|start_filename|>sdk/bare/common/sys/crc32.c<|end_filename|>
#include "sys/crc32.h"
#include <stddef.h>
#include <stdint.h>

static const uint32_t crc_table[256]
    = { 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 0x0edb8832,
        0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
        0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 0x136c9856, 0x646ba8c0, 0xfd62f97a,
        0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
        0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3,
        0x45df5c75, 0xdcd60dcf, 0xabd13d59, 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
        0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab,
        0xb6662d3d, 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
        0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 0x6b6b51f4,
        0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
        0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, 0x4db26158, 0x3ab551ce, 0xa3bc0074,
        0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
        0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525,
        0x206f85b3, 0xb966d409, 0xce61e49f, 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
        0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615,
        0x73dc1683, 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
        0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 0xfed41b76,
        0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
        0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 0xd80d2bda, 0xaf0a1b4c, 0x36034af6,
        0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
        0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7,
        0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
        0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7,
        0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
        0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 0xa00ae278,
        0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
        0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 0xbdbdf21c, 0xcabac28a, 0x53b39330,
        0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
        0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d };

static inline uint32_t crc32_next(uint32_t crc, uint8_t data)
{
    return (crc >> 8) ^ crc_table[(crc & 0xff) ^ data];
}

static inline uint32_t crc32_final(uint32_t crc)
{
    return ~crc;
}

uint32_t crc32_calc_part(const uint8_t *data, size_t len, uint32_t init)
{
    uint32_t crc = init;

    if (len)
        do {
            crc = crc32_next(crc, *data++);
        } while (--len);

    return crc;
}

uint32_t crc32_calc_full(const uint8_t *data, size_t len)
{
    uint32_t crc = CRC32_DEFAULT_INIT;

    if (len)
        do {
            crc = crc32_next(crc, *data++);
        } while (--len);

    return crc32_final(crc);
}


<|start_filename|>sdk/bare/user/usr/user_apps.h<|end_filename|>
#ifndef USER_APPS_H
#define USER_APPS_H

void user_apps_init(void);

#endif // USER_APPS_H


<|start_filename|>sdk/bare/common/drv/motherboard_defs.h<|end_filename|>
#ifndef MOTHERBOARD_DEFS_H
#define MOTHERBOARD_DEFS_H

// ===============
// Number of slave registers in this IP
// ===============
#define MOTHERBOARD_DEFS_NUM_SLAVE_REGS (16)

// ===============
// Offsets for the relevant slave registers (in bytes).
// Separated by 4 bytes since each register is 32 bits.
// ===============
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH1       (0 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH2       (1 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH3       (2 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH4       (3 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH5       (4 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH6       (5 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH7       (6 * 4)
#define MOTHERBOARD_DEFS_OFFSET_ADC_CH8       (7 * 4)
#define MOTHERBOARD_DEFS_OFFSET_CONTROL       (8 * 4)
#define MOTHERBOARD_DEFS_OFFSET_STATUS        (9 * 4)
#define MOTHERBOARD_DEFS_OFFSET_COUNT_VALID   (10 * 4)
#define MOTHERBOARD_DEFS_OFFSET_COUNT_CORRUPT (11 * 4)
#define MOTHERBOARD_DEFS_OFFSET_COUNT_TIMEOUT (12 * 4)
#define MOTHERBOARD_DEFS_OFFSET_UNUSED5       (13 * 4)
#define MOTHERBOARD_DEFS_OFFSET_UNUSED6       (14 * 4)
#define MOTHERBOARD_DEFS_OFFSET_UNUSED7       (15 * 4)

// ===============
// CONTROL register
// ===============
#define MOTHERBOARD_DEFS_CONTROL_SYNC_ADC_EN_WIDTH (1) // in bits
#define MOTHERBOARD_DEFS_CONTROL_SYNC_ADC_EN_SHIFT (0) // up from reg[0]
#define MOTHERBOARD_DEFS_CONTROL_SYNC_ADC_EN_MASK  (0x1)

#define MOTHERBOARD_DEFS_CONTROL_SYNC_TX_WIDTH (1) // in bits
#define MOTHERBOARD_DEFS_CONTROL_SYNC_TX_SHIFT (1) // up from reg[0]
#define MOTHERBOARD_DEFS_CONTROL_SYNC_TX_MASK  (0x1)

#endif // MOTHERBOARD_DEFS_H


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_bemfo.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_bemfo.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/bemfo.h"
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (1)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "start", "Reset controller state" },
};

void cmd_bemfo_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "bemfo", "BEMFO commands", cmd_help, NUM_HELP_ENTRIES, cmd_bemfo);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

int cmd_bemfo(int argc, char **argv)
{
    // Handle 'start' sub-command
    if (argc == 2 && strcmp("start", argv[1]) == 0) {
        bemfo_start();
        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/led.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#ifndef LED_H
#define LED_H

#include <stdint.h>

typedef struct led_color_t {
    uint8_t r;
    uint8_t g;
    uint8_t b;
} led_color_t;

// clang-format off
static const led_color_t LED_COLOR_RED     = { 020, 000, 000 };
static const led_color_t LED_COLOR_GREEN   = { 000, 020, 000 };
static const led_color_t LED_COLOR_BLUE    = { 000, 000, 020 };
static const led_color_t LED_COLOR_YELLOW  = { 020, 020, 000 };
static const led_color_t LED_COLOR_CYAN    = { 000, 020, 020 };
static const led_color_t LED_COLOR_MAGENTA = { 020, 000, 020 };
static const led_color_t LED_COLOR_WHITE   = { 020, 020, 020 };
static const led_color_t LED_COLOR_BLACK   = { 000, 000, 000 };
// clang-format on

typedef enum {
    LED0 = 0,
    LED1,
    LED2,
    LED3,
    NUM_LEDS,
} led_t;

void led_init(void);

void led_set_color(led_t idx, led_color_t color);
void led_set_color_bytes(led_t idx, uint8_t r, uint8_t g, uint8_t b);
void led_set_raw(led_t idx, uint32_t color);

led_color_t led_get_color(led_t idx);
uint32_t led_get_raw(led_t idx);

#endif // LED_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/sys/cmd/cmd_mb.c<|end_filename|>
#include "usr/user_config.h"

#if USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT == 1

#include "drv/motherboard.h"
#include "sys/cmd/cmd_mb.h"
#include "sys/commands.h"
#include "sys/util.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "adc <on|off>", "Controls motherboard ADC sampling" },
    { "tx", "Manually trigger motherboard to send latest ADC samples" },
    { "samples", "Display latest samples from MB" },
    { "counters", "Display debug counters from MB UART RX" },
};

void cmd_mb_register(void)
{
    commands_cmd_init(&cmd_entry, "mb", "Motherboard commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_mb);

    commands_cmd_register(&cmd_entry);
}

int cmd_mb(int argc, char **argv)
{
    // Handle 'mb adc <on|off>' command
    if (argc == 3 && STREQ("adc", argv[1])) {
        if (STREQ("on", argv[2])) {
            motherboard_set_adc_sampling(true);
            return CMD_SUCCESS;
        }

        if (STREQ("off", argv[2])) {
            motherboard_set_adc_sampling(false);
            return CMD_SUCCESS;
        }
    }

    // Handle 'mb tx' command
    if (argc == 2 && STREQ("tx", argv[1])) {
        motherboard_request_new_data();
        return CMD_SUCCESS;
    }

    // Handle 'mb samples' command
    if (argc == 2 && STREQ("samples", argv[1])) {
        motherboard_print_samples();
        return CMD_SUCCESS;
    }

    // Handle 'mb counters' command
    if (argc == 2 && STREQ("counters", argv[1])) {
        motherboard_print_counters();
        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT


<|start_filename|>sdk/basic/control_apps/VSI_DQ_SVPWM.c<|end_filename|>
/*
 * VSI_DQ_SVPWM.c
 *
 *  Created on: Jun 30, 2014
 *      Author: sever212
 */

#include "../src/project_include.h"
//#include <tgmath.h>


#define KiTs_Q_GAIN 0.005
#define KiTs_D_GAIN 0.005

#define KP_Q_GAIN 0.452
#define KP_D_GAIN 0.452

#define DEFAULT_SINE_TRIANGLE 1		//0 for SVPWM, 1 for SINE TRIANGLE


//assumes the dq voltages are a fraction of the dc bus voltage
int SaturateDQ0Voltages(double *inputDQ, double *outputDQ, u8 bSineTriangle)
{
	double ml, ml_0, a;
	if (bSineTriangle)
	{
		ml = 1.632993161855452*sqrt(inputDQ[0]*inputDQ[0] + inputDQ[1]*inputDQ[1]);
		ml_0 = 0.816496580927726*fabs(inputDQ[2]);
		a = 0.816496580927726;
	}
	else
	{
		ml = sqrt(2.0*(inputDQ[0]*inputDQ[0] + inputDQ[1]*inputDQ[1]));
		ml_0 = 0.707106781186548*fabs(inputDQ[2]);
		a = 0.707106781186548;
	}

	if (ml > 1.0) //case 1: limit d and q, no zero sequence
	{
		outputDQ[0] = inputDQ[0]/ml;
		outputDQ[1] = inputDQ[1]/ml;
		outputDQ[2] = 0; //Zero seq reference set to 0
	}
	else if (ml + ml_0 > 1.0) //case 2: no limit on d and q, limit zero sequence
	{
		outputDQ[0] = inputDQ[0];
		outputDQ[1] = inputDQ[1];
		if (inputDQ[2] > 0.0)
			outputDQ[2] = (1.0-ml)/a;
		else
			outputDQ[2] = -(1.0-ml)/a;
	}
	else //case 3: no limits.
	{
		outputDQ[0] = inputDQ[0];
		outputDQ[1] = inputDQ[1];
		outputDQ[2] = inputDQ[2];

	}
	return SUCCESS;
}

//assumes the dq voltages are a fraction of the dc bus voltage
int SaturateDQVoltages(double *inputDQ, double *outputDQ, u8 bSineTriangle)
{
	double ml;
	if (bSineTriangle)
		ml = 1.632993161855452*sqrt(inputDQ[0]*inputDQ[0] + inputDQ[1]*inputDQ[1]);
	else
		ml = sqrt(2.0*(inputDQ[0]*inputDQ[0] + inputDQ[1]*inputDQ[1]));

	if (ml > 1.0)
	{
		outputDQ[0] = inputDQ[0]/ml;
		outputDQ[1] = inputDQ[1]/ml;
	}
	else
	{
		outputDQ[0] = inputDQ[0];
		outputDQ[1] = inputDQ[1];
	}
	return SUCCESS;
}

int DQ0_Transform(double theta_da, double *abc, double *dq0)
{
	dq0[0] = 	DQK*(cos(theta_da)*abc[0] 	+cos(theta_da - PI23)*abc[1] 	+cos(theta_da + PI23)*abc[2]);
	dq0[1] =   -DQK*(sin(theta_da)*abc[0] 	+sin(theta_da - PI23)*abc[1] 	+sin(theta_da + PI23)*abc[2]);
	dq0[2] = 	DQK*(			abc[0] 		+abc[1] 						+abc[2]);
	return SUCCESS;
}

int InverseDQ0_Transform(double theta_da, double *abc, double *dq0)
{
	abc[0] = DQK*(cos(theta_da)*dq0[0] 			-sin(theta_da)*dq0[1] 			+0.5*dq0[2]);
	abc[1] = DQK*(cos(theta_da - PI23)*dq0[0] 	-sin(theta_da - PI23)*dq0[1] 	+0.5*dq0[2]);
	abc[2] = DQK*(cos(theta_da + PI23)*dq0[0] 	-sin(theta_da + PI23)*dq0[1] 	+0.5*dq0[2]);
	return SUCCESS;
}

double GetMid(double m1, double m2, double m3)
{
	if ( (m2 >= m1) && (m1 >= m3) )
		return m1;
	if ( (m3 >= m1) && (m1 >= m2) )
		return m1;
	if ( (m1 >= m2) && (m2 >= m3))
		return m2;
	if ( (m3 >= m2) && (m2 >= m1))
		return m2;
	if ( (m1 >= m3) && (m3 >= m2) )
		return m3;
	if ( (m2 >= m3) && (m3 >= m1))
		return m3;
	return m1;		//should never get here!
}

//change the reference frame of a dq0 value.
int ChangeDQ_ReferenceFrame(double *new_dq0, double new_theta_da, double *old_dq0, double old_theta_da)
{
	double theta = old_theta_da - new_theta_da;

	new_dq0[0] = old_dq0[0]*cos(theta) - old_dq0[1]*sin(theta);
	new_dq0[1] = old_dq0[0]*sin(theta) + old_dq0[1]*cos(theta);
	new_dq0[2] = old_dq0[2];

	return SUCCESS;
}

#define OUTPUT_FREQ 120.0		//Hz
//#define THETA_DA  GetTheta_DA() /*0.0 //212 //*/ //((double) 2.0*PI*OUTPUT_FREQ*((double)MainTimerTimerTick)/((double)100000.0))


double GetTheta_DA()
{
	static double theta_da = 0;
	double update_da = 0;
	static u32 LastTickCNT = 0;
	u32 elapsed;
	elapsed = TimeElapsed10us(LastTickCNT);
	LastTickCNT = MainTimerTimerTick;
	update_da = ((double) 2.0*PI*OUTPUT_FREQ*((double)elapsed)/((double)100000.0));
	theta_da += update_da;
	while (theta_da > PI2)
		theta_da = theta_da - PI2;

	return theta_da;
}

u32 vsi_theta_da;


static u8 ExampleVSI_NeedsInput(void *arg, void *VSIContext);
static void ExampleVSI_WriteOutput(void *arg, void *VSIContext);

VSI_context VSI_DQ_Cur_Controllers[VSI_DQ_SVPWM_NUM_VSI];
static controller_linked_list_item cont_items [3*VSI_DQ_SVPWM_NUM_VSI];

//callbacks:
static void VSI_Input(void *arg, struct controller_context *cnt);
static void VSI_Output(void *arg, struct controller_context *cnt);

//logging:
u32 VSI_V1_da, VSI_V1_db, VSI_V1_dc;
s32 VSI_V1_d_error, VSI_V1_q_error, VSI_V1_d_current, VSI_V1_q_current;


//non-control -- allows d and q voltages to be forced to a certain value (fraction of dc bus)
double force_vd[VSI_DQ_SVPWM_NUM_VSI];
double force_vq[VSI_DQ_SVPWM_NUM_VSI];
u8 bForceDQ_Voltages = 0;

/*****************************
 * Init_VSI_DQ_VSPWM()
 * Sets up the controllers memory to default values
 * and optionally starts the controllers running.
 * The default settings can be overridden by manually
 * editing the VSI_DQ_Cur_Controllers structure.
 *
 *StartAllControllers: if this is set to TRUE, all the
 * controllers are registered in control.c and started.
 *returns a success code
 */
int Init_VSI_DQ_VSPWM(u8 StartAllControllers)
{
	//pi_house *pi_ptr = HB_Controllers;
	int i;
	for (i = 0; i<VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		force_vd[i] = 0;
		force_vq[i] = 0;

		VSI_DQ_Cur_Controllers[i].bSineTriangle = DEFAULT_SINE_TRIANGLE;

		//PI controllers
		VSI_DQ_Cur_Controllers[i].d.pi.kits = KiTs_D_GAIN;
		VSI_DQ_Cur_Controllers[i].q.pi.kits = KiTs_Q_GAIN;
		VSI_DQ_Cur_Controllers[i].zero.pi.kits = 0.0;
		VSI_DQ_Cur_Controllers[i].d.pi.kp = KP_D_GAIN;
		VSI_DQ_Cur_Controllers[i].q.pi.kp = KP_Q_GAIN;
		VSI_DQ_Cur_Controllers[i].zero.pi.kp = 0.0;



		VSI_DQ_Cur_Controllers[i].d.pi.lastInt = 0;
		VSI_DQ_Cur_Controllers[i].d.pi.plusSat = 0.5;		//limit integral to +- 0.5Vdcbus because this is the max value Vd and Vq can both be (max modulation index)
		VSI_DQ_Cur_Controllers[i].d.pi.negSat = -0.5;
		VSI_DQ_Cur_Controllers[i].q.pi.lastInt = 0;
		VSI_DQ_Cur_Controllers[i].q.pi.plusSat = 0.5;
		VSI_DQ_Cur_Controllers[i].q.pi.negSat = -0.5;
		VSI_DQ_Cur_Controllers[i].zero.pi.lastInt = 0;
		VSI_DQ_Cur_Controllers[i].zero.pi.plusSat = 0.5;
		VSI_DQ_Cur_Controllers[i].zero.pi.negSat = -0.5;

		//basic
		VSI_DQ_Cur_Controllers[i].d.io.inputs = &VSI_DQ_Cur_Controllers[i].d.input;
		VSI_DQ_Cur_Controllers[i].q.io.inputs = &VSI_DQ_Cur_Controllers[i].q.input;
		VSI_DQ_Cur_Controllers[i].zero.io.inputs = &VSI_DQ_Cur_Controllers[i].zero.input;
		VSI_DQ_Cur_Controllers[i].d.io.outputs = &VSI_DQ_Cur_Controllers[i].d.output;
		VSI_DQ_Cur_Controllers[i].q.io.outputs = &VSI_DQ_Cur_Controllers[i].q.output;
		VSI_DQ_Cur_Controllers[i].zero.io.outputs = &VSI_DQ_Cur_Controllers[i].zero.output;
		VSI_DQ_Cur_Controllers[i].d.io.NumInputs = 1;
		VSI_DQ_Cur_Controllers[i].q.io.NumInputs = 1;
		VSI_DQ_Cur_Controllers[i].zero.io.NumInputs = 1;
		VSI_DQ_Cur_Controllers[i].d.io.NumOutputs = 1;
		VSI_DQ_Cur_Controllers[i].q.io.NumOutputs = 1;
		VSI_DQ_Cur_Controllers[i].zero.io.NumOutputs = 1;
		VSI_DQ_Cur_Controllers[i].d.io.NumClocks = 1;
		VSI_DQ_Cur_Controllers[i].q.io.NumClocks = 1;
		VSI_DQ_Cur_Controllers[i].zero.io.NumClocks = 1;
		VSI_DQ_Cur_Controllers[i].d.io.ClockTime = 5; //5us
		VSI_DQ_Cur_Controllers[i].q.io.ClockTime = 5;
		VSI_DQ_Cur_Controllers[i].zero.io.ClockTime = 5;
		VSI_DQ_Cur_Controllers[i].d.io.clockCount = 0;
		VSI_DQ_Cur_Controllers[i].q.io.clockCount = 0;
		VSI_DQ_Cur_Controllers[i].zero.io.clockCount = 0;
		VSI_DQ_Cur_Controllers[i].d.io.Type = C_PI;
		VSI_DQ_Cur_Controllers[i].q.io.Type = C_PI;
		VSI_DQ_Cur_Controllers[i].zero.io.Type = C_PI;
		VSI_DQ_Cur_Controllers[i].d.io.Status = C_RUNNING;
		VSI_DQ_Cur_Controllers[i].q.io.Status = C_RUNNING;
		VSI_DQ_Cur_Controllers[i].zero.io.Status = C_RUNNING;

		//context
		VSI_DQ_Cur_Controllers[i].d.ctxt.arg_in = 0;
		VSI_DQ_Cur_Controllers[i].q.ctxt.arg_in = 0;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.arg_in = 0;
		VSI_DQ_Cur_Controllers[i].d.ctxt.arg_out = 0;
		VSI_DQ_Cur_Controllers[i].q.ctxt.arg_out = 0;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.arg_out = 0;
		VSI_DQ_Cur_Controllers[i].d.ctxt.control_io = &VSI_DQ_Cur_Controllers[i].d.io;
		VSI_DQ_Cur_Controllers[i].q.ctxt.control_io = &VSI_DQ_Cur_Controllers[i].q.io;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.control_io = &VSI_DQ_Cur_Controllers[i].zero.io;
		VSI_DQ_Cur_Controllers[i].d.ctxt.controller_info = &VSI_DQ_Cur_Controllers[i].d.pi;
		VSI_DQ_Cur_Controllers[i].q.ctxt.controller_info = &VSI_DQ_Cur_Controllers[i].q.pi;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.controller_info = &VSI_DQ_Cur_Controllers[i].zero.pi;

		VSI_DQ_Cur_Controllers[i].d.ctxt.input_callback = VSI_Input;
		VSI_DQ_Cur_Controllers[i].q.ctxt.input_callback = VSI_Input;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.input_callback = VSI_Input;

		VSI_DQ_Cur_Controllers[i].d.ctxt.output_callback = VSI_Output;
		VSI_DQ_Cur_Controllers[i].q.ctxt.output_callback = VSI_Output;
		VSI_DQ_Cur_Controllers[i].zero.ctxt.output_callback = VSI_Output;


		VSI_DQ_Cur_Controllers[i].d.reference = 0;
		VSI_DQ_Cur_Controllers[i].q.reference = 0;
		VSI_DQ_Cur_Controllers[i].zero.reference = 0;
		VSI_DQ_Cur_Controllers[i].dqComp[0] = 0;
		VSI_DQ_Cur_Controllers[i].dqComp[1] = 0;

		VSI_DQ_Cur_Controllers[i].bUseZeroSequence = 0;

		//callbacks
		VSI_DQ_Cur_Controllers[i].get_input = ExampleVSI_NeedsInput;
		VSI_DQ_Cur_Controllers[i].write_output = ExampleVSI_WriteOutput;

		//load it:
		cont_items[3*i].context = &VSI_DQ_Cur_Controllers[i].d.ctxt;
		cont_items[3*i+1].context = &VSI_DQ_Cur_Controllers[i].q.ctxt;
		cont_items[3*i+2].context = &VSI_DQ_Cur_Controllers[i].zero.ctxt;
		if (StartAllControllers)
		{
			Start_VSI_DQ_SVPWM_Controller(i+1);
		}
	}
	return SUCCESS;
}

/*****************************
 * Start_VSI_DQ_SVPWM_Controller()
 * Registers the VSI d and q current controllers with control.c.
 *
 *vsi: the VSI number to register (1 - VSI_DQ_SVPWM_NUM_VSI)
 *returns a success code
 */
int Start_VSI_DQ_SVPWM_Controller(u32 vsi)
{
	u32 i = vsi-1;
	if (i >= VSI_DQ_SVPWM_NUM_VSI)
		return INVALID_ARGUMENT;
	if (cont_items[3*i].bRegistered == TRUE)
		return INVALID_OPERATION;
	if (cont_items[3*i+1].bRegistered == TRUE)
		return INVALID_OPERATION;
	if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
		if (cont_items[3*i+2].bRegistered == TRUE)
			return INVALID_OPERATION;

	RegisterController(0, &cont_items[3*i]);
	RegisterController(0, &cont_items[3*i+1]);

	if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
		RegisterController(0, &cont_items[3*i+2]);

	return SUCCESS;
}

/*****************************
 * Stop_VSI_DQ_SVPWM_Controller()
 * Unregisters the VSI d and q current controllers from control.c.
 *
 *vsi: the VSI number to unregister (1 - VSI_DQ_SVPWM_NUM_VSI)
 *returns a success code
 */
int Stop_VSI_DQ_SVPWM_Controller(u32 vsi)
{
	u32 i = vsi-1;
	if (i >= VSI_DQ_SVPWM_NUM_VSI)
		return INVALID_ARGUMENT;
	if (cont_items[2*i].bRegistered == TRUE)
		UnregisterController(0, &cont_items[3*i]);
	if (cont_items[2*i+1].bRegistered == TRUE)
		UnregisterController(0, &cont_items[3*i+1]);
	if (cont_items[3*i+2].bRegistered == TRUE)
		UnregisterController(0, &cont_items[3*i+2]);

	return SUCCESS;
}

/*****************************
 * VSI_DQ_VSPW_StopCommand()
 * Process an ascii command for starting VSI controllers
 * Init_VSI_DQ_VSPWM is called
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int VSI_DQ_VSPW_StopCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	int i;
	for (i=0; i< VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		Stop_VSI_DQ_SVPWM_Controller(i+1);
	}

	strcpy(szResponse, "OK");
	return strlen(szResponse);
}

/*****************************
 * VSI_DQ_VSPW_StartCommand()
 * Process an ascii command for starting VSI controllers
 * Init_VSI_DQ_VSPWM is called
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int VSI_DQ_VSPW_StartCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	if (strstr(szCmd, "FORCEVDQ"))
	{
		strcpy(szResponse, "FORCEVDQ=1");
		bForceDQ_Voltages = TRUE;
	}
	else
	{
		bForceDQ_Voltages = FALSE;
		strcpy(szResponse, "OK");
	}

	Init_VSI_DQ_VSPWM(TRUE);

	return strlen(szResponse);
}

/*****************************
 * VSI_DQ_VSPW_Command()
 * Process an ascii command for setting a reference current
 * Syntax: n,r,val
 *  n is the Vbridge number (1 - VSI_DQ_SVPWM_NUM_VSI),
 *  r is d for direct axis or q for quadrature axis,
 *	val is the current reference value * 128
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int VSI_DQ_VSPW_Command(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *ptr;
	u32 vsi = atoi(szCmd);
	u8 bError = FALSE, bD = FALSE;
	if (vsi < 1 || vsi > VSI_DQ_SVPWM_NUM_VSI)
		bError = TRUE;
	else
	{
		for (ptr = szCmd; ptr != &szCmd[strlen(szCmd)-1]; ptr++)
			if (*ptr == ',')
				break;
		ptr++;	//advance past the comma
		if ( (ptr >= &szCmd[strlen(szCmd)]) || (ptr[1] != ',') || ((*ptr != 'd') && (*ptr != 'q')) )
			bError = TRUE;
		else
		{
			double current_ref;
			if (ptr[0] == 'd')
				bD = TRUE;

			ptr = &ptr[2];
			current_ref = ((double) atoi(ptr))/128;
			if (bForceDQ_Voltages == TRUE)
			{
				if (bD)
					force_vd[vsi-1] = current_ref;
				else
					force_vq[vsi-1] = current_ref;

				strcpy(szResponse, "OK");
			}
			else
			{
				if (VSI_DQ_VSPW_SetCurrentRef(vsi, bD, current_ref) == SUCCESS)
					strcpy(szResponse, "OK");
				else
					bError = TRUE;
			}
		}

	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * VSI_DQ_VSPW_SetCurrentRef()
 * Set a d or q axis reference current
 *
 *
 *vsi: index of the VSI to set the reference current of
 *bDaxis: 1 if setting a d-axis quantity, otherwise 0 for q
 *reference: the value of the new reference current
 */
int VSI_DQ_VSPW_SetCurrentRef(u32 vsi, u8 bDaxis, double reference)
{
	if (vsi < 1 || vsi > VSI_DQ_SVPWM_NUM_VSI)
		return INVALID_ARGUMENT;
	if (bDaxis)
		VSI_DQ_Cur_Controllers[vsi-1].d.reference = reference;
	else
		VSI_DQ_Cur_Controllers[vsi-1].q.reference = reference;

	return SUCCESS;
}

//Internal function called by control.c before executing a controller
static void VSI_Input(void *arg, struct controller_context *cnt)
{
	u32 i=0;

	for (i= 0; i< VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		if (&VSI_DQ_Cur_Controllers[i].d.io == cnt)
			break;
		else if (&VSI_DQ_Cur_Controllers[i].q.io == cnt)
			break;
		else if (&VSI_DQ_Cur_Controllers[i].zero.io == cnt)
			break;

	}

	//calculate the error and store it
	if (TimeElapsed10us(VSI_DQ_Cur_Controllers[i].InputAtTickCount) > 3)
	{

		VSI_DQ_Cur_Controllers[i].InputAtTickCount = MainTimerTimerTick;

		if (!VSI_DQ_Cur_Controllers[i].get_input(VSI_DQ_Cur_Controllers[i].input_arg, &VSI_DQ_Cur_Controllers[i]))
			DQ0_Transform(VSI_DQ_Cur_Controllers[i].theta_da, VSI_DQ_Cur_Controllers[i].Iabc, VSI_DQ_Cur_Controllers[i].Idq0);

		VSI_DQ_Cur_Controllers[i].d.input = VSI_DQ_Cur_Controllers[i].d.reference - VSI_DQ_Cur_Controllers[i].Idq0[0];
		VSI_DQ_Cur_Controllers[i].q.input = VSI_DQ_Cur_Controllers[i].q.reference - VSI_DQ_Cur_Controllers[i].Idq0[1];
		//if a zero sequence current controller:
		if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
			VSI_DQ_Cur_Controllers[i].zero.input = VSI_DQ_Cur_Controllers[i].zero.reference - VSI_DQ_Cur_Controllers[i].Idq0[2];

		//logging:
		if (i == 0)
		{
			VSI_V1_d_error = (s32) (VSI_DQ_Cur_Controllers[i].d.input*128.0);
			VSI_V1_q_error = (s32) (VSI_DQ_Cur_Controllers[i].q.input*128.0);
			VSI_V1_d_current = (s32) (VSI_DQ_Cur_Controllers[i].Idq0[0]*128.0);
			VSI_V1_q_current = (s32) (VSI_DQ_Cur_Controllers[i].Idq0[1]*128.0);
			vsi_theta_da = (u32) VSI_DQ_Cur_Controllers[i].theta_da*128.0;
		}

	}
}

//Internal function called by control.c when an output is ready
static void VSI_Output(void *arg, struct controller_context *cnt)
{
	u32 i = 0, n = 0;
	double m_dq0[3], mid_m, d_com;
	for (i= 0; i< VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		if (&VSI_DQ_Cur_Controllers[i].d.io == cnt)
		{
			VSI_DQ_Cur_Controllers[i].d.updated = TRUE;
			break;
		}
		else if (&VSI_DQ_Cur_Controllers[i].q.io == cnt)
		{
			VSI_DQ_Cur_Controllers[i].q.updated = TRUE;
			break;
		}
		else if (&VSI_DQ_Cur_Controllers[i].zero.io == cnt)
		{
			VSI_DQ_Cur_Controllers[i].zero.updated = TRUE;
			break;
		}
	}

	if ( (VSI_DQ_Cur_Controllers[i].d.updated == TRUE) && (VSI_DQ_Cur_Controllers[i].q.updated==TRUE) && (VSI_DQ_Cur_Controllers[i].zero.updated==TRUE || (!VSI_DQ_Cur_Controllers[i].bUseZeroSequence)))
	{
		double dq_raw[3], dq_saturated[3];
		//both d and q have been updated, do the inverse dq transform and write out the duty ratios

		VSI_DQ_Cur_Controllers[i].d.updated = FALSE;
		VSI_DQ_Cur_Controllers[i].q.updated = FALSE;
		VSI_DQ_Cur_Controllers[i].zero.updated = FALSE;

		//first saturate the outputs:
		if (bForceDQ_Voltages == FALSE)
		{
			dq_raw[0] = VSI_DQ_Cur_Controllers[i].d.output + VSI_DQ_Cur_Controllers[i].dqComp[0];
			dq_raw[1] = VSI_DQ_Cur_Controllers[i].q.output + VSI_DQ_Cur_Controllers[i].dqComp[1];
			if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
				dq_raw[2] = VSI_DQ_Cur_Controllers[i].zero.output;
		}
		else
		{
			//allow to operate VSI open loop
			dq_raw[0] = force_vd[i];
			dq_raw[1] = force_vq[i];
			dq_raw[2] = 0;
		}

		if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
			SaturateDQ0Voltages(dq_raw, dq_saturated, VSI_DQ_Cur_Controllers[i].bSineTriangle);
		else
			SaturateDQVoltages(dq_raw, dq_saturated, VSI_DQ_Cur_Controllers[i].bSineTriangle);

		//dq modulation indices
		m_dq0[0] = dq_saturated[0];
		m_dq0[1] = dq_saturated[1];
		if (VSI_DQ_Cur_Controllers[i].bUseZeroSequence)
			m_dq0[2] = dq_saturated[2];
		else
			m_dq0[2] = 0;

		//abc modulation indices
		InverseDQ0_Transform(VSI_DQ_Cur_Controllers[i].theta_da, VSI_DQ_Cur_Controllers[i].mabc, m_dq0);

		//mid modulation indices (for SVPWM)
		if (VSI_DQ_Cur_Controllers[i].bSineTriangle)
			d_com = 0.5;
		else
		{
			mid_m = GetMid(VSI_DQ_Cur_Controllers[i].mabc[0], VSI_DQ_Cur_Controllers[i].mabc[1], VSI_DQ_Cur_Controllers[i].mabc[2]);
			d_com = 0.5*(1+mid_m);
		}

		//calculate duty ratios
		//dtest = 256*(d_com + m_abc[0]);
		VSI_DQ_Cur_Controllers[i].dabc[0] = d_com + VSI_DQ_Cur_Controllers[i].mabc[0];
		VSI_DQ_Cur_Controllers[i].dabc[1] = d_com + VSI_DQ_Cur_Controllers[i].mabc[1];
		VSI_DQ_Cur_Controllers[i].dabc[2] = d_com + VSI_DQ_Cur_Controllers[i].mabc[2];

		for (n = 0; n < 3; n++) //SAFETY make sure we don't wrap around
		{
			if (VSI_DQ_Cur_Controllers[i].dabc[n] > 1.0)
				VSI_DQ_Cur_Controllers[i].dabc[n] = 1.0;
			else if (VSI_DQ_Cur_Controllers[i].dabc[n] < -1.0)
				VSI_DQ_Cur_Controllers[i].dabc[n] = -1.0;
		}

		//write them out:
		VSI_DQ_Cur_Controllers[i].write_output(VSI_DQ_Cur_Controllers[i].output_arg, &VSI_DQ_Cur_Controllers[i]);

	}
}

//return false if ABC values set, true if DQ values set
static u8 ExampleVSI_NeedsInput(void *arg, void *VSIContext)
{
	u32 i=0;
	u32 adc_num[3];
	struct VSI_context *cnt = VSIContext;
	//double meas_abc[3], meas_dq0[3];
	adc_num[0] = 4; adc_num[1] = 5; adc_num[2] = 6;

	for (i= 0; i< VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		if (&VSI_DQ_Cur_Controllers[i] == cnt)
		{
			if (i == 0)
			{
				adc_num[0] = 1; adc_num[1] = 2; adc_num[2] = 3;
				/*Verror = &VSI_V1_d_error;
				Vcurrent = &VSI_V1_d_current;*/
				//bD = TRUE;
			}
			break;
		}
	}

	cnt->Iabc[0] = ReadADC(adc_num[0]);
	cnt->Iabc[1] = ReadADC(adc_num[1]);
	cnt->Iabc[2] = ReadADC(adc_num[2]);
	cnt->theta_da = GetTheta_DA();

	return FALSE;
}

//return false if ABC values set, true if DQ values set
static void ExampleVSI_WriteOutput(void *arg, void *VSIContext)
{
	u32 legs[3];
	u8 duty1, duty2, duty3;
	struct VSI_context *cnt = VSIContext;
	int i;
	legs[0] = 0; legs[1] = 0; legs[2] = 0;
	for (i= 0; i< VSI_DQ_SVPWM_NUM_VSI; i++)
	{
		if (&VSI_DQ_Cur_Controllers[i] == cnt)
		{
			if (i == 0)
				legs[0] = 1; legs[1] = 2; legs[2] = 3;
			break;
		}
	}

	if (i==0)
	{
		//convert the duty ratios
		//dtest = 256*(d_com + m_abc[0]);
		duty1 = (unsigned char) 256.0*(cnt->dabc[0]);
		duty2 = (unsigned char) 256.0*(cnt->dabc[1]);
		duty3 = (unsigned char) 256.0*(cnt->dabc[2]);

		//write them out:
		WriteDutyRatio(legs[0], duty1);
		WriteDutyRatio(legs[1], duty2);
		WriteDutyRatio(legs[2], duty3);

		//logging
		VSI_V1_da = duty1;
		VSI_V1_db = duty2;
		VSI_V1_dc = duty3;

	}
}



<|start_filename|>sdk/bare/user/usr/beta_labs/task_cc.h<|end_filename|>
#ifndef TASK_CC_H
#define TASK_CC_H

#include "drv/analog.h"
#include "sys/defines.h"
#include "sys/scheduler.h"
#include <stdint.h>

#define TASK_CC_UPDATES_PER_SEC (20000)
#define TASK_CC_INTERVAL_USEC   (USEC_IN_SEC / TASK_CC_UPDATES_PER_SEC)

// Current = GAIN * ADC_Voltage + Offset

#define ADC_TO_AMPS_PHASE_A_GAIN (1.0056083)
#define ADC_TO_AMPS_PHASE_B_GAIN (1.0088819)
#define ADC_TO_AMPS_PHASE_C_GAIN (1.0100499)

#define ADC_TO_AMPS_PHASE_A_OFFSET (-0.0477295)
#define ADC_TO_AMPS_PHASE_B_OFFSET (-0.048415325)
#define ADC_TO_AMPS_PHASE_C_OFFSET (-0.01765254)

#define CC_PHASE_A_ADC (ANLG_CHNL1)
#define CC_PHASE_B_ADC (ANLG_CHNL2)
#define CC_PHASE_C_ADC (ANLG_CHNL3)

#define CC_PHASE_A_PWM_LEG_IDX (0)
#define CC_PHASE_B_PWM_LEG_IDX (1)
#define CC_PHASE_C_PWM_LEG_IDX (2)

void task_cc_init(void);
void task_cc_deinit(void);
void task_cc_callback(void *arg);

uint8_t task_cc_is_inited(void);

double task_cc_get_theta_e_enc(void);

void task_cc_clear(void);
void task_cc_set_dq_offset(int32_t offset);
void task_cc_set_bw(double bw);

void task_cc_set_Iq_star(double value);
void task_cc_set_Id_star(double value);

void task_cc_set_theta_e_src(uint8_t use_encoder);
void task_cc_set_omega_e_src(uint8_t use_encoder);

#endif // TASK_CC_H


<|start_filename|>sdk/basic/src/platform_config.h<|end_filename|>
#ifndef __PLATFORM_CONFIG_H_
#define __PLATFORM_CONFIG_H_

#define STDOUT_IS_PS7_UART
#define UART_DEVICE_ID 0
#ifdef __PPC__
#define CACHEABLE_REGION_MASK 0xf0000001
#endif

#define PLATFORM_EMAC_BASEADDR XPAR_XEMACPS_0_BASEADDR

#endif


<|start_filename|>sdk/bare/common/drv/cpu_timer.h<|end_filename|>
#ifndef CPU_TIMER_H
#define CPU_TIMER_H

#include <stdint.h>

void cpu_timer_init(void);

// Return the current time in CPU clock cycles (666.666 MHZ clock)
uint32_t cpu_timer_now(void);

// Convert CPU clock ticks to usec
double cpu_timer_ticks_to_usec(uint32_t ticks);

// Convert CPU clock ticks to sec
double cpu_timer_ticks_to_sec(uint32_t ticks);

#endif // CPU_TIMER_H


<|start_filename|>sdk/basic/control_apps/PosContTry1.c<|end_filename|>
/*
 * PosContTry1.c
 *
 *  Created on: Jul 10, 2014
 *      Author: sever212
 */
#include "../src/project_include.h"

#if defined (USE_VSI_CC)
	//(these are all scaled so that we can take sensor inputs in units of mm)
	#define KKpf_xo_GAIN 47.905474024799268 //38.9693  //Kxo_forcenorm*Kpf_xo
	#define KKiTs_xo_GAIN 0.011041196186847 //0.0075708324	//Kxo_forcenorm*Kif_xo*Ts
	//derivative gain for current error: derivative difference EQ: D = kdn*E[n] + kdn1*E[n-1] + kdm1*D[n-1]
	#define KKdn_xo_GAIN 398.2600565348860 //349.43217 //Kxo_forcenorm*er_coefs_xo(1)
	#define KKdn1_xo_GAIN -398.2600565348860 //-349.43217 //Kxo_forcenorm*er_coefs_xo(2)
	#define KKdm1_xo_GAIN 0.909993668824279 //0.905664 //Dcoefs_xo(2)

	#define KKpf_th_GAIN 4.70 //3.45  //Kth_forcenorm*Kpf_th
	#define KKiTs_th_GAIN 0.001060151124033 //.000517474016	//Kth_forcenorm*Kif_th*Ts
	//derivative gain for current error: derivative difference EQ: D = kdn*E[n] + kdn1*E[n-1] + kdm1*D[n-1]
	#define KKdn_th_GAIN 76.670981745634862 //76.2908 //Kth_forcenorm*er_coefs_th(1)
	#define KKdn1_th_GAIN -76.670981745634862 //-76.2908 //Kth_forcenorm*er_coefs_th(2)
	#define KKdm1_th_GAIN 0.909993668824279 //0.857308 //Dcoefs_th(2)


	#define CUR_KP_GAIN  0.441786 //Old val (works): 0.9990 //WORKS: 0.499513231920777 // 0.16650441064	//Kpv
	#define CUR_KiTs_GAIN 0.014726 //Old val (works): 0.0251 //WORKS: 0.012566370614359 // 0.006283185307180	//Kiv*Ts


	#define BOT_US_ADC 1
	#define BOT_VS_ADC 2
	#define BOT_WS_ADC 3
	#define TOP_US_ADC 4
	#define TOP_VS_ADC 5
	#define TOP_WS_ADC 6

	#define BOT_US_LEG 1
	#define BOT_VS_LEG 2
	#define BOT_WS_LEG 3
	#define TOP_US_LEG 4
	#define TOP_VS_LEG 5
	#define TOP_WS_LEG 6


	#define BOTX_ADC 7
	#define BOTY_ADC 8
	#define TOPX_ADC 9
	#define TOPY_ADC 10

	#define BOT_ADC_MM_V 0.201005		//mm/V (1/4.975) for SN1309172-01-01
	#define TOP_ADC_MM_V 0.201491		//mm/V (1/4.963) for SN1207054-01-01

	#define INV_2RX 2.617801047120	//1/(2*rx)  //rx should be in units of m (even though sensors are in units of mm -- because gains were divided by 1000)

	#define MAX_PHASE_CURRENT 3.6 //max that the sensor can read (8 turns --> 0.375 A/V, so max it can read is 3.75A)

	#define CS_ADC_AMP_PER_VOLT 0.375

	static u8 POS_VSI_Input(void *arg, void *VSIContext);
	static void POS_VSI_Output(void *arg, void *VSIContext);
#endif

#if defined (USE_H_BRIDGE_CC) || defined(USE_H_BRIDGE_DQ_CC)
	//(these are all scaled so that we can take sensor inputs in units of mm)
	#define KKpf_xo_GAIN 61.669050462043472 //WORKS: 77.938586884587338  //Kxo_forcenorm*Kpf_xo
	#define KKiTs_xo_GAIN 0.010458260654729 //0.015141664815228	//Kxo_forcenorm*Kif_xo*Ts
	//derivative gain for current error: derivative difference EQ: D = kdn*E[n] + kdn1*E[n-1] + kdm1*D[n-1]
	#define KKdn_xo_GAIN 546.4352960041329 //698.86434078013 //Kxo_forcenorm*er_coefs_xo(1)
	#define KKdn1_xo_GAIN -546.4352960041329 //-698.8643407801309 //Kxo_forcenorm*er_coefs_xo(2)
	#define KKdm1_xo_GAIN  0.917183613124594 //0.905664 //Dcoefs_xo(2)

	#define KKpf_th_GAIN 7.183817503137941 //6.90  //Kth_forcenorm*Kpf_th
	#define KKiTs_th_GAIN 0.001101464122195 //0.001034948	//Kth_forcenorm*Kif_th*Ts
	//derivative gain for current error: derivative difference EQ: D = kdn*E[n] + kdn1*E[n-1] + kdm1*D[n-1]
	#define KKdn_th_GAIN 159.0181780942346 //152.581543975 //Kth_forcenorm*er_coefs_th(1)
	#define KKdn1_th_GAIN -159.0181780942346 //-152.581543975 //Kth_forcenorm*er_coefs_th(2)
	#define KKdm1_th_GAIN 0.854358985825323 //0.857308 //Dcoefs_th(2)


	#define CUR_KP_GAIN  0.161 //Works (Lext): 0.3338 //Works (IAS): 0.1047 //0.1665 //Kpv
	#define CUR_KiTs_GAIN 0.0210 //Works (Lext): 0.0105 //Works (IAS): 0.02094 //0.006283 //Kiv*Ts


	#define BOT_US_ADC 1
	#define BOT_VS_ADC 2
	#define BOT_WS_ADC 3
	#define TOP_US_ADC 4
	#define TOP_VS_ADC 5
	#define TOP_WS_ADC 6

	#define BOT_US_L1 11
	#define BOT_US_L2 12
	#define BOT_VS_L1 15
	#define BOT_VS_L2 16
	#define BOT_WS_L1 19
	#define BOT_WS_L2 20
	#define TOP_US_L1 5
	#define TOP_US_L2 6
	#define TOP_VS_L1 13
	#define TOP_VS_L2 14
	#define TOP_WS_L1 17
	#define TOP_WS_L2 18

	#define BOTX_ADC 7
	#define BOTY_ADC 8
	#define TOPX_ADC 9
	#define TOPY_ADC 10

	#define BOT_ADC_MM_V 0.201005		//mm/V (1/4.975) for SN1309172-01-01
	#define TOP_ADC_MM_V 0.201491		//mm/V (1/4.963) for SN1207054-01-01

	#define INV_2RX 2.617801047120	//1/(2*rx)  //rx should be in units of m (even though sensors are in units of mm -- because gains were divided by 1000)

	#define MAX_PHASE_CURRENT 5.8 //max that the sensor can read (5 turns --> 0.6 A/V, so max it can read is 6A)

	#define CS_ADC_AMP_PER_VOLT 0.6

#if defined(USE_H_BRIDGE_CC)
	static u8 POS_HB_Input(void *arg, void *HBContext);
	static void POS_HB_Output(void *arg, void *HBContext);
#endif
#if defined(USE_H_BRIDGE_DQ_CC)
	static u8 POS_VSI_Input(void *arg, void *VSIContext);
	static void POS_VSI_Output(void *arg, void *VSIContext);
#endif
#endif



static void POS_Input(void *arg, struct controller_context *cnt);
static void POS_Output(void *arg, struct controller_context *cnt);



//Logging:
s32 x0_meas, y0_meas, x0Error, y0Error, thx, thy, thxError, thyError;
s32 Ixt_meas, Iyt_meas, Ixb_meas, Iyb_meas, Ixt_ref, Iyt_ref, Ixb_ref, Iyb_ref;
s32 Ixb_ref_usat, Iyb_ref_usat, Ixt_ref_usat, Iyt_ref_usat;

#define DEFAULT_X_TOP_REF 0.251
#define DEFAULT_Y_TOP_REF -0.053
#define DEFAULT_X_BOT_REF 0.089
#define DEFAULT_Y_BOT_REF -0.140

//double default_ref_pos[4] = {DEFAULT_X_TOP_REF, DEFAULT_Y_TOP_REF, DEFAULT_X_BOT_REF, DEFAULT_Y_BOT_REF};

//non-control -- allows x and y, top and bottom currents to be forced to a certain value
double force_ixy_top[2] = {0.0, 0.0};
double force_ixy_bot[2] = {0.0, 0.0};
u8 bForceXY_Currents = 0;

int SaturateDQCurrents(double MaxPhaseCurrent, double *inputDQ, double *outputDQ)
{
	double mc = sqrt(0.6667*(inputDQ[0]*inputDQ[0] + inputDQ[1]*inputDQ[1]));
	if (mc > MaxPhaseCurrent)
	{
		outputDQ[0] = inputDQ[0]/mc*MaxPhaseCurrent;
		outputDQ[1] = inputDQ[1]/mc*MaxPhaseCurrent;
	}
	else
	{
		outputDQ[0] = inputDQ[0];
		outputDQ[1] = inputDQ[1];
	}
	return SUCCESS;
}

int SaturatePhaseCurrents(double MaxPhaseCurrent, double *inputUVW, double *outputUVW)
{
	int i = 0;
	double max_c = inputUVW[0];
	double scale = 1.0;
	for (i = 1; i < 3; i++)
	{
		if (fabs(inputUVW[i]) > fabs(max_c))
			max_c = inputUVW[i];
	}
	if (fabs(max_c) > MaxPhaseCurrent)
	{
		scale = fabs(MaxPhaseCurrent/max_c);
		for (i = 0; i < 3; i++)
			outputUVW[i] = scale*inputUVW[i];
	}
	else
	{
		for (i = 0; i < 3; i++)
			outputUVW[i] = inputUVW[i];
	}

	return SUCCESS;
}

PosCnt_context PosControllers;
static controller_linked_list_item pos_cont_items [4];

int ConvertFromX1X2(double *x1x2, double *x0th)
{
	x0th[0] = 0.5*(x1x2[0] + x1x2[1]);
	x0th[1] = INV_2RX*(x1x2[0] - x1x2[1]);
	return SUCCESS;
}

int ConvertFromSumDiff(double *SumDiff, double *OneTwo)
{
	OneTwo[0] = 0.5*(SumDiff[0] + SumDiff[1]);
	OneTwo[1] = 0.5*(SumDiff[0] - SumDiff[1]);
	return SUCCESS;
}

/*****************************
 * Init_PosContTry1()
 * Sets up the controllers memory to default values
 * and optionally starts the controllers running.
 * The default settings can be overridden by manually
 * editing the VSI_DQ_Cur_Controllers structure.
 *
 *StartAllControllers: this is ignored
 *returns a success code
 */
int Init_PosContTry1(u8 StartAllControllers)
{
	struct pid_house *pid;
	int i;

	PosControllers.MaxPhaseCurrent = MAX_PHASE_CURRENT;
#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
	PosControllers.VSItop = &VSI_DQ_Cur_Controllers[VSI_TOP_INDEX];
	PosControllers.VSIbot = &VSI_DQ_Cur_Controllers[VSI_BOT_INDEX];
#endif


	//setup and register PID controllers
	for (i = 0; i < 4; i++)
	{
		if (i < 2)
			pid = &PosControllers.x[i];
		else
			pid = &PosControllers.y[i-2];

		//position controller
		if ((i == 0) || (i == 2))
		{
			pid->pid.kp = KKpf_xo_GAIN;
			pid->pid.kits = KKiTs_xo_GAIN;
			pid->pid.kdn = KKdn_xo_GAIN;
			pid->pid.kdn1 = KKdn1_xo_GAIN;
			pid->pid.kdm1 = KKdm1_xo_GAIN;
		}
		else
		{
			pid->pid.kp = KKpf_th_GAIN;
			pid->pid.kits = KKiTs_th_GAIN;
			pid->pid.kdn = KKdn_th_GAIN;
			pid->pid.kdn1 = KKdn1_th_GAIN;
			pid->pid.kdm1 = KKdm1_th_GAIN;
		}
		pid->pid.lastInt = 0;
		pid->pid.lastErr = 0;
		pid->pid.lastD = 0;
		pid->pid.plusSat = 2.4495*MAX_PHASE_CURRENT;  //2*sqrt(3/2)*Kf*Iph_max
		pid->pid.negSat = -2.4495*MAX_PHASE_CURRENT;  //-2*sqrt(3/2)*Kf*Iph_max

		//house
		pid->reference = 0;
		pid->updated = FALSE;
		pid->input = 0;
		pid->output = 0;

		//IO (controller_context)
		pid->io.inputs = &pid->input;
		pid->io.outputs = &pid->output;
		pid->io.NumInputs = 1;
		pid->io.NumOutputs = 1;
		pid->io.ClockTime = 5;	//uS
		pid->io.clockCount = 0;
		pid->io.NumClocks = 1;
		pid->io.Type = C_PID;
		pid->io.Status = C_RUNNING;

		//CTXT (controller_holder)
		pid->ctxt.arg_in = (void *)i;
		pid->ctxt.arg_out = (void *)i;
		pid->ctxt.control_io = &pid->io;
		pid->ctxt.controller_info = &pid->pid;
		pid->ctxt.input_callback = POS_Input;
		pid->ctxt.output_callback = POS_Output;

		//register the controller
		pos_cont_items[i].context = &pid->ctxt;
		RegisterController(0, &pos_cont_items[i]);
	}

	//setup VSI's:
#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
	Init_VSI_DQ_VSPWM(FALSE);		//set to defaults (but don't register controllers)
	for (i = 0; i < 2; i++)
	{
		u32 vsi_index;
		struct VSI_context *VSI;
		if (i == 0)
		{
			VSI = PosControllers.VSIbot;
			vsi_index = VSI_BOT_INDEX;
		}
		else
		{
			VSI = PosControllers.VSItop;
			vsi_index = VSI_TOP_INDEX;
#if defined(VSI_ISOLATED)
			VSI->bUseZeroSequence = 0;
#else
			VSI->bUseZeroSequence = 1;
			VSI->zero.pi.kits = CUR_KiTs_GAIN/2.0;
			VSI->zero.pi.kp = CUR_KP_GAIN/2.0;
#endif
		}

		VSI->input_arg = (void*) vsi_index;
		VSI->output_arg = (void*) vsi_index;

		VSI->d.pi.kits = CUR_KiTs_GAIN;
		VSI->d.pi.kp = CUR_KP_GAIN;
		VSI->q.pi.kits = CUR_KiTs_GAIN;
		VSI->q.pi.kp = CUR_KP_GAIN;

		//callbacks
		VSI->get_input = POS_VSI_Input;
		VSI->write_output = POS_VSI_Output;

		//start it up
		Start_VSI_DQ_SVPWM_Controller(vsi_index+1);
	}
#endif
#if defined(USE_H_BRIDGE_CC)

	Init_HBridge_CC(FALSE);		//set to defaults (but don't register controllers)
	for (i = 0; i < 6; i++)
	{
		u32 hb_index;
		struct HB_context *HB;

		if (i < 3)
		{
			PosControllers.HBtop[i] = &HB_Controllers[i];
			HB = PosControllers.HBtop[i]; //i = 0: u, i = 1: v, i = 2: w
			hb_index = i;
		}
		else
		{
			PosControllers.HBbot[i-3] = &HB_Controllers[i];
			HB = PosControllers.HBbot[i-3]; //i = 3: u, i = 4: v, i = 5: w
			hb_index = i;
		}

		HB->input_arg = (void*) hb_index;
		HB->output_arg = (void*) hb_index;

		HB->c.pi.kits = CUR_KiTs_GAIN;
		HB->c.pi.kp = CUR_KP_GAIN;

		//callbacks
		HB->get_input = POS_HB_Input;
		HB->write_output = POS_HB_Output;

		//start it up
		Start_HBridge_CC_Controller(hb_index+1);
	}
#endif

	//adjust references of position controllers
	//x1 is top, x2 is bot
	double xs[2] = {DEFAULT_X_TOP_REF, DEFAULT_X_BOT_REF};
	double ys[2] = {DEFAULT_Y_TOP_REF, DEFAULT_Y_BOT_REF};
	double xthx[2];
	double ythy[2];
	ConvertFromX1X2(xs, xthx);
	ConvertFromX1X2(ys, ythy);

	PosControllers.x[0].reference = xthx[0];
	PosControllers.x[1].reference = xthx[1];

	PosControllers.y[0].reference = ythy[0];
	PosControllers.y[1].reference = ythy[1];
	return SUCCESS;
}


/*****************************
 * Stop_PosContTry1_Controller()
 * Unregisters the position controller. Note that this does
 * not stop any corresponding current controller
 *
 *vsi: the VSI number to unregister (1 - VSI_DQ_SVPWM_NUM_VSI)
 *returns a success code
 */
int Stop_PosContTry1_Controller(u32 poscnt)
{
	u32 i = poscnt-1;
	if (i >= 4)
		return INVALID_ARGUMENT;
	if (pos_cont_items[2*i].bRegistered == TRUE)
		UnregisterController(0, &pos_cont_items[2*i]);
	if (pos_cont_items[2*i+1].bRegistered == TRUE)
		UnregisterController(0, &pos_cont_items[2*i+1]);
	return SUCCESS;
}

/*****************************
 * PosContTry1_StopCommand()
 * Process an ascii command for stopping the position controllers
 * and any associated current controllers
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int PosContTry1_StopCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	int i;
	for (i=0; i< 4; i++)
	{
		Stop_PosContTry1_Controller(i+1);
	}
#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
	Stop_VSI_DQ_SVPWM_Controller(VSI_BOT_INDEX);
	Stop_VSI_DQ_SVPWM_Controller(VSI_TOP_INDEX);
#endif
#if defined(USE_H_BRIDGE_CC)
	for (i=0; i < 6; i++)
		Stop_HBridge_CC_Controller(i);
#endif

	strcpy(szResponse, "OK");
	return strlen(szResponse);
}


/*****************************
 * PosContTry1_StartCommand()
 * Process an ascii command for starting the position controllers
 * Init_PosContTry1 is called
 *
 * Example: CST=1,FORCEI=1
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int PosContTry1_StartCommand(const char *szCmd, char *szResponse, void *CommDevice)
{

	if (strstr(szCmd, "FORCEI"))
	{
		strcpy(szResponse, "FORCEI=1");
		bForceXY_Currents = TRUE;
	}
	else
	{
		bForceXY_Currents = FALSE;
		strcpy(szResponse, "OK");
	}


	Init_PosContTry1(TRUE);

	return strlen(szResponse);
}

/*****************************
 * PosContTry1_Command()
 * Process an ascii command for setting a reference position (or current)
 * Syntax: n,tb,val
 *  n is the controller axis (x for x-axis, y for y-axis),
 *  tb is t for top, b for bottom,
 *	val is the mm reference (*128)
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int PosContTry1_Command(const char * szCmd, char *szResponse, void *CommDevice)
{
	u8 tb = szCmd[0];
	u8 bError = FALSE;
	double ref;
	if ((tb != 'b')  && (tb != 't') )
		bError = TRUE;
	else if (szCmd[1] != ',')
		bError = TRUE;
	else
	{
		u8 xy = szCmd[2];
		if ((xy != 'x')  && (xy != 'y') )
			bError = TRUE;
		else if (szCmd[3] != ',')
			bError = TRUE;
		else if (szCmd[4] == 0)
			bError = TRUE;
		else
		{
			ref = ((double) atoi(&szCmd[4]))/128;
			if (bForceXY_Currents)
			{
				if (tb == 't')
				{
					if (xy == 'x')
						force_ixy_top[0] = ref;
					else
						force_ixy_top[1] = ref;
				}
				else
				{
					if (xy == 'x')
						force_ixy_bot[0] = ref;
					else
						force_ixy_bot[1] = ref;
				}
				strcpy(szResponse, "OK");
			}
			else //not supported yet
				bError = TRUE;
		}

	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}




//Internal function called by control.c before executing a controller
static void POS_Input(void *arg, struct controller_context *cnt)
{
//	u32 i= (u32) arg;

	//calculate the error and store it
	if (TimeElapsed10us(PosControllers.InputAtTickCount) > 5)
	{

		PosControllers.InputAtTickCount = MainTimerTimerTick;
		PosControllers.xs[POS_BOT_INDEX] = BOT_ADC_MM_V*ReadADC(BOTX_ADC);
		PosControllers.xs[POS_TOP_INDEX] = TOP_ADC_MM_V*ReadADC(TOPX_ADC);
		PosControllers.ys[POS_BOT_INDEX] = BOT_ADC_MM_V*ReadADC(BOTY_ADC);
		PosControllers.ys[POS_TOP_INDEX] = TOP_ADC_MM_V*ReadADC(TOPY_ADC);
		double x1x2[2] = {PosControllers.xs[POS_TOP_INDEX], PosControllers.xs[POS_BOT_INDEX]};
		double y1y2[2] = {PosControllers.ys[POS_TOP_INDEX], PosControllers.ys[POS_BOT_INDEX]};
		ConvertFromX1X2(x1x2, PosControllers.x_thx);
		ConvertFromX1X2(y1y2, PosControllers.y_thy);

		PosControllers.x[0].input = PosControllers.x[0].reference - PosControllers.x_thx[0];
		PosControllers.x[1].input = PosControllers.x[1].reference - PosControllers.x_thx[1];
		PosControllers.y[0].input = PosControllers.y[0].reference - PosControllers.y_thy[0];
		PosControllers.y[1].input = PosControllers.y[1].reference - PosControllers.y_thy[1];

		//logging
		x0_meas = (s32) 128.0*PosControllers.x_thx[0];
		y0_meas = (s32) 128.0*PosControllers.y_thy[0];
		thx = (s32) 128.0*PosControllers.x_thx[1];
		thy = (s32) 128.0*PosControllers.y_thy[1];
		x0Error = (s32) 128.0*PosControllers.x[0].input;
		y0Error = (s32) 128.0*PosControllers.y[0].input;
		thxError = (s32) 128.0*PosControllers.x[1].input;
		thyError = (s32) 128.0*PosControllers.y[1].input;

	}
}

//Internal function called by control.c when an output is ready
static void POS_Output(void *arg, struct controller_context *cnt)
{
	u32 i= (u32) arg;

	switch (i)
	{
		case 0:
			PosControllers.x[0].updated = TRUE;
			break;

		case 1:
			PosControllers.x[1].updated = TRUE;
			break;

		case 2:
			PosControllers.y[0].updated = TRUE;
			break;

		case 3:
			PosControllers.y[1].updated = TRUE;
			break;

		default:
		break;
	}


	if ( (PosControllers.x[0].updated == TRUE) && (PosControllers.x[1].updated==TRUE)
			&& (PosControllers.y[0].updated == TRUE) && (PosControllers.y[1].updated==TRUE))
	{
		double current_xy_top[2], current_xy_bot[2], current_12x[2], current_12y[2], current_sumdiff[2];
		//all controller output current references have been updated,
		//update the xy current holder taking into account maximum current values
		PosControllers.x[0].updated = FALSE;
		PosControllers.x[1].updated = FALSE;
		PosControllers.y[0].updated = FALSE;
		PosControllers.y[1].updated = FALSE;

		//get top bot x currents
		current_sumdiff[0] = PosControllers.x[POS_XY0_INDEX].output;
		current_sumdiff[1] = PosControllers.x[POS_THXY_INDEX].output;
		ConvertFromSumDiff(current_sumdiff, current_12x);
		current_xy_top[0] = current_12x[0];
		current_xy_bot[0] = current_12x[1];	//NOT negative because the bottom force is NOT created in the opposite direction

		//get top bot y currents
		current_sumdiff[0] = PosControllers.y[POS_XY0_INDEX].output;
		current_sumdiff[1] = PosControllers.y[POS_THXY_INDEX].output;
		ConvertFromSumDiff(current_sumdiff, current_12y);
		current_xy_top[1] = current_12y[0];
		current_xy_bot[1] = current_12y[1]; //NOT negative because the bottom force is NOT created in the opposite direction

		//if we are forcing the currents, override the above calculations
		if (bForceXY_Currents)
		{
			current_xy_top[0] = force_ixy_top[0];
			current_xy_top[1] = force_ixy_top[1];
			current_xy_bot[0] = force_ixy_bot[0];
			current_xy_bot[1] = force_ixy_bot[1];
		}

		//saturate the top bot xy currents
#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
		SaturateDQCurrents(PosControllers.MaxPhaseCurrent, current_xy_top, PosControllers.Itop_xy);
		SaturateDQCurrents(PosControllers.MaxPhaseCurrent, current_xy_bot, PosControllers.Ibot_xy);
#else
		PosControllers.Itop_xy[0] = current_xy_top[0]; PosControllers.Itop_xy[1] = current_xy_top[1];
		PosControllers.Ibot_xy[0] = current_xy_bot[0]; PosControllers.Ibot_xy[1] = current_xy_bot[1];
#endif

#if defined(USE_H_BRIDGE_CC)
		{
			double i_xy0[3], i_uvw[3];
			i_xy0[0] = PosControllers.Itop_xy[0];
			i_xy0[1] = PosControllers.Itop_xy[1];
			i_xy0[2] = 0;
			InverseDQ0_Transform(0, i_uvw, i_xy0);
			SaturatePhaseCurrents(PosControllers.MaxPhaseCurrent, i_uvw, PosControllers.Itop_uvw_sus);
			i_xy0[0] = PosControllers.Ibot_xy[0]; i_xy0[1] = PosControllers.Ibot_xy[1]; i_xy0[2] = 0;
			InverseDQ0_Transform(0, i_uvw, i_xy0);
			SaturatePhaseCurrents(PosControllers.MaxPhaseCurrent, i_uvw, PosControllers.Ibot_uvw_sus);
		}
#endif

		//logging
		Ixb_ref_usat = (s32) 128.0*current_xy_bot[0];
		Iyb_ref_usat = (s32) 128.0*current_xy_bot[1];
		Ixt_ref_usat = (s32) 128.0*current_xy_top[0];
		Iyt_ref_usat = (s32) 128.0*current_xy_top[1];
	}
}


#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
//update reference and measured currents
//return false if ABC values set, true if DQ values set
static u8 POS_VSI_Input(void *arg, void *VSIContext)
{
	u32 vsi_index = (u32) arg;
	u32 adc_num[3];
	struct VSI_context *cnt = VSIContext;
	//double meas_abc[3], meas_dq0[3];
	double adc_gain = CS_ADC_AMP_PER_VOLT;

	//MEASURED CURRENTS
	if (vsi_index == VSI_BOT_INDEX)
	{
		adc_num[0] = BOT_US_ADC; adc_num[1] = BOT_VS_ADC; adc_num[2] = BOT_WS_ADC;
	}
	else
	{
		adc_num[0] = TOP_US_ADC; adc_num[1] = TOP_VS_ADC; adc_num[2] = TOP_WS_ADC;
	}

	cnt->Iabc[0] = adc_gain*ReadADC(adc_num[0]);
	cnt->Iabc[1] = adc_gain*ReadADC(adc_num[1]);
	cnt->Iabc[2] = adc_gain*ReadADC(adc_num[2]);
	cnt->theta_da = 0;

	//REFERENCE DQ CURRENTS
	if (vsi_index == VSI_BOT_INDEX)
	{
#if defined(USE_H_BRIDGE_DQ_CC)
		cnt->d.reference = PosControllers.Ibot_xy[0];
		cnt->q.reference = PosControllers.Ibot_xy[1];
#endif

#if defined(USE_VSI_CC)
		cnt->d.reference = PosControllers.Ibot_xy[0] - 0.5*PosControllers.TorqueVSI_botIxy[0];
		cnt->q.reference = PosControllers.Ibot_xy[1] - 0.5*PosControllers.TorqueVSI_botIxy[1];
#endif
		//logging
		Ixb_ref = (s32) 128.0*PosControllers.Ibot_xy[0]; //PosControllers.Ibot_xy[0] - 0.5*PosControllers.TorqueVSI_topIxy[0];
		Iyb_ref = (s32) 128.0*PosControllers.Ibot_xy[1]; //PosControllers.Ibot_xy[1] - 0.5*PosControllers.TorqueVSI_topIxy[1];
	}
	else
	{
#if defined(USE_H_BRIDGE_DQ_CC)
		cnt->d.reference = PosControllers.Itop_xy[0];
		cnt->q.reference = PosControllers.Itop_xy[1];
#endif
#if defined(USE_VSI_CC)
		cnt->d.reference = PosControllers.Itop_xy[0] - 0.5*PosControllers.TorqueVSI_topIxy[0];
		cnt->q.reference = PosControllers.Itop_xy[1] - 0.5*PosControllers.TorqueVSI_topIxy[1];
#endif
		//logging
		Ixt_ref = (s32) 128.0*PosControllers.Itop_xy[0];
		Iyt_ref = (s32) 128.0*PosControllers.Itop_xy[1];
	}
	return FALSE;
}

//write the duty ratio out to the legs
static void POS_VSI_Output(void *arg, void *VSIContext)
{
	u32 vsi_index = (u32) arg;
	u32 legs[6];
	u8 duty1, duty2, duty3;
	struct VSI_context *cnt = VSIContext;
	legs[0] = 0; legs[1] = 0; legs[2] = 0;
	int i;

#if defined(USE_H_BRIDGE_DQ_CC)
	if (vsi_index == VSI_BOT_INDEX)
	{
		legs[0] = BOT_US_L1; legs[1] = BOT_US_L2;
		legs[2] = BOT_VS_L1; legs[3] = BOT_VS_L2;
		legs[4] = BOT_WS_L1; legs[5] = BOT_WS_L2;
	}
	else
	{
		legs[0] = TOP_US_L1; legs[1] = TOP_US_L2;
		legs[2] = TOP_VS_L1; legs[3] = TOP_VS_L2;
		legs[4] = TOP_WS_L1; legs[5] = TOP_WS_L2;
	}

	//convert the modulation indices to duty ratios
	for (i = 0; i < 3; i++)
	{
		double m;
		u8 d1, d2;
		//safety to avoid roll-over:
		if (cnt->mabc[i] > 1.0)
			m = 1.0;
		else if (cnt->mabc[i] < -1.0)
			m = -1.0;
		else
			m = cnt->mabc[i];


		//calculate duty ratios
		d1 = (unsigned char) 127*(1 + m);
		d2 = (unsigned char) 127*(1 - m);

		//write it out
		WriteDutyRatio(legs[i*2], d1);
		WriteDutyRatio(legs[i*2+1], d2);

	}
#endif

#if defined(USE_VSI_CC)
	if (vsi_index == VSI_BOT_INDEX)
	{
		legs[0] = BOT_US_LEG; legs[1] = BOT_VS_LEG; legs[2] = BOT_WS_LEG;
	}
	else
	{
		legs[0] = TOP_US_LEG; legs[1] = TOP_VS_LEG; legs[2] = TOP_WS_LEG;
	}
	//convert the duty ratios
	//dtest = 256*(d_com + m_abc[0]);
	duty1 = (unsigned char) 256.0*(cnt->dabc[0]);
	duty2 = (unsigned char) 256.0*(cnt->dabc[1]);
	duty3 = (unsigned char) 256.0*(cnt->dabc[2]);

	//write them out:
	WriteDutyRatio(legs[0], duty1);
	WriteDutyRatio(legs[1], duty2);
	WriteDutyRatio(legs[2], duty3);
#endif
}
#endif

#if defined(USE_H_BRIDGE_CC)
//update reference and measured currents
//return false if ABC values set, true if DQ values set
static u8 POS_HB_Input(void *arg, void *HBContext)
{
	u32 hb_index = (u32) arg;
	u32 adc_num = 0;
	struct HB_context *cnt = HBContext;
	double adc_gain = CS_ADC_AMP_PER_VOLT;

	//MEASURED CURRENTS
	switch(hb_index)
	{
	case 0:
		adc_num = TOP_US_ADC;
		break;
	case 1:
		adc_num = TOP_VS_ADC;
		break;
	case 2:
		adc_num = TOP_WS_ADC;
		break;
	case 3:
		adc_num = BOT_US_ADC;
		break;
	case 4:
		adc_num = BOT_VS_ADC;
		break;
	case 5:
		adc_num = BOT_WS_ADC;
		break;
	default:
		//should not get here!
		break;
	}

	//update the measured current
	cnt->I = adc_gain*ReadADC(adc_num);

	//update reference currents
	if (hb_index < 3)
		HBridge_CC_SetCurrentRef(hb_index+1, PosControllers.Itop_uvw_sus[hb_index]);
	else
		HBridge_CC_SetCurrentRef(hb_index+1, PosControllers.Ibot_uvw_sus[hb_index-3]);

	//logging
		Ixt_ref = (s32) 128.0*PosControllers.Itop_xy[0];
		Iyt_ref = (s32) 128.0*PosControllers.Itop_xy[1];
		Ixb_ref = (s32) 128.0*PosControllers.Ibot_xy[0];
		Iyb_ref = (s32) 128.0*PosControllers.Ibot_xy[1];

	return FALSE;
}

//write the duty ratio out to the legs
static void POS_HB_Output(void *arg, void *HBContext)
{
	u32 hb_index = (u32) arg;
	u32 legs[2];

	struct HB_context *cnt = HBContext;
	legs[0] = 0; legs[1] = 0;

	switch(hb_index)
	{
	case 0:
		legs[0] = TOP_US_L1;
		legs[1] = TOP_US_L2;
		break;
	case 1:
		legs[0] = TOP_VS_L1;
		legs[1] = TOP_VS_L2;
		break;
	case 2:
		legs[0] = TOP_WS_L1;
		legs[1] = TOP_WS_L2;
		break;
	case 3:
		legs[0] = BOT_US_L1;
		legs[1] = BOT_US_L2;
		break;
	case 4:
		legs[0] = BOT_VS_L1;
		legs[1] = BOT_VS_L2;
		break;
	case 5:
		legs[0] = BOT_WS_L1;
		legs[1] = BOT_WS_L2;
		break;
	default:
		//should not get here!
		break;
	}

	//write them out:
	WriteDutyRatio(legs[0], cnt->d[0]);
	WriteDutyRatio(legs[1], cnt->d[1]);
}
#endif


<|start_filename|>sdk/basic/bsp/encoder.c<|end_filename|>
#include "encoder.h"
#include <stdio.h>
#include <math.h>
#include "xil_io.h"

#define ENCODER_BASE_ADDR		(0x43C10000)

void encoder_init(void)
{
	printf("ENC:\tInitializing...\n");
	encoder_set_pulses_per_rev_bits(ENCODER_PULSES_PER_REV_BITS);
}

void encoder_set_pulses_per_rev_bits(uint32_t bits)
{
	printf("ENC:\tSetting pulses per rev bits = %ld...\n", bits);

	Xil_Out32(ENCODER_BASE_ADDR + 2*sizeof(uint32_t), bits);
}

void encoder_get_steps(int32_t *steps)
{
	*steps = Xil_In32(ENCODER_BASE_ADDR);
}

void encoder_get_position(uint32_t *position)
{
	*position = Xil_In32(ENCODER_BASE_ADDR + 1*sizeof(uint32_t));
}


<|start_filename|>sdk/basic/control_apps/TorqueVSI.h<|end_filename|>
#ifndef __TORQUE_VSI_H
#define __TORQUE_VSI_H

#define NUM_TVSI 1

#if defined (USE_VSI_CC)
//#define TVSI_STATIONARY_FRAME		//comment this out to turn off the torque stationary frame controller
#endif

typedef struct TVSI_context
{
	struct VSI_context *VSI;
#if defined (TVSI_STATIONARY_FRAME)
	struct VSI_context *VSI_stationary;
#endif
	u32 InputAtTickCount;		//the tick count of the last time the inputs were updated (to avoid unnecessary dq transformations)
	double OmegaDA_Cmd;
	double OmegaDA_Current;
	double MaxPhaseCurrent;
	double Idq_ref[2];	
} TVSI_context;

extern int Init_TorqueVSI(u8 StartAllControllers);
extern int Stop_TorqueVSI_Controller(u32 tvsi);
extern int TorqueVSI_StopCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int TorqueVSI_StartCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int TorqueVSI_Command(const char * szCmd, char *szResponse, void *CommDevice);
#endif //__TORQUE_VSI_H


<|start_filename|>ip_repo/amdc_analog_1.0/drivers/amdc_analog_v1_0/src/amdc_analog.c<|end_filename|>


/***************************** Include Files *******************************/
#include "amdc_analog.h"

/************************** Function Definitions ***************************/


<|start_filename|>sdk/bare/user/usr/beta_labs/machine.h<|end_filename|>
#ifndef MACHINE_H
#define MACHINE_H

#include "sys/defines.h"

// Parameters for TG motor from Beta
//
// Started as datasheet values, but since
// have been updated to more accurate estimates

// Electrical
#define Ld_HAT        (18.709 / 1000000.0) // H
#define Lq_HAT        (19.667 / 1000000.0) // H
#define Rs_HAT        (260.0 / 1000.0)     // Ohms
#define Lambda_pm_HAT (7407.0 / 1000000.0) // Wb

// Mechanical
#define Jp_HAT (0.00636) // kg-m^2
#define Bp_HAT (0.00025) // kg-m^2/s
#define Tu_HAT (0.0025)  // kg-m/s^2

#define Kt_HAT (1.0 / (4.0 / (3.0 * POLES * Lambda_pm_HAT)))
//#define Kt_HAT        (0.177768)  // Nm / Arms
//#define Km_HAT        (0.330)     // Nm / sqrt(W)
//#define Ke_HAT        (0.160)     // Vpkll / (rads/s)

#define POLE_PAIRS (16.0)
#define POLES      (32.0)

// NOTE: use this to artificially limit current
#define I_rated_rms (10.0) // Arms

//#define I_rated_rms   (24.2) // Arms
#define I_rated_pk (I_rated_rms * SQRT2) // Apk
#define I_rated_dq (I_rated_pk / SQRT23) // A_dq

#endif // MACHINE_H


<|start_filename|>sdk/basic/src/commands.c<|end_filename|>

#if 0
#include "xparameters.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "commands.h"
#include "main.h"
#include "project_settings.h"
#include "log.h"
#include "prog_timer.h"
#endif

#include "project_include.h"

#include "../bsp/bsp.h"

/*****************************
 * MakeMostFullLFR()
 * Populate an LFR response from the most-full log
 *
 *
 * Response: LFR=sqn_num,index,flags,length,binary_data
 * **The response length will be different from the
 * **  command length if the end of the log is reached
 * **binary_data is byte-for-byte what is in the log.
 * **sqn_num is a sequence number that starts at 1 (reset when LFS is received)
 * 	and increments with each LFR
 * --> only 100us errors are but response is padded to 10 fields
 *szResponse: the response
 *uiMaxLength: the maximum length of the binary_data field (must be divisible by 4)
 */
int MakeMostFullLFR(char *szResponse, u32 uiMaxLength, u32 *SQnum)
{
	u32 uiLogIndex = 0, uiFlags = 0;
	int iStatus1 = ReadFromMostFullLog((unsigned char*) szResponse, &uiLogIndex, uiMaxLength);
	int iStatus2 = ReadLogFlags(&uiFlags, uiLogIndex);
	if ( (iStatus1 < 0) || (iStatus2 < 0))
		return -1;
	else if (iStatus1 == 0)
		return 0;
	else
	{
		char szTemp[64];
		if (uiFlags != 0)
			uiFlags = uiFlags; //breakpoint
		sprintf(szTemp, "LFR=%d,%d,%d,%d,", (int) *SQnum, (int) uiLogIndex, (int) uiFlags, (int) iStatus1);
		*SQnum = *SQnum + 1;
		memmove(&szResponse[strlen(szTemp)], szResponse, iStatus1);
		memcpy(szResponse, szTemp, strlen(szTemp));
		return strlen(szTemp) + iStatus1;
	}
}

//Streaming FIFO
u32 StreamFIFO_SqnNum = 0;
void *StreamFIFOComm = 0;
u32 StreamFIFO_MaxPacket = 0; //maximum LFR binary data field size in bytes
char bPrimeStreamFIFO = FALSE;	//set this to true if the streaming FIFO needs to be loaded from the main loop (instead of the callback)

int ReloadStreamFIFO(void * comm)
{
	int retVal = 0;
	int status;
	err_t err;
	if (comm > 0)
	{
		unsigned char ucSendAnother;
		char SendDataBuff[MAX_SEND_PACKET_SIZE];
		do
		{
			u32 tcp_space_avail = tcp_sndbuf((struct tcp_pcb *)comm);
			u32 uiMaxLen = ((u32)(tcp_space_avail/4) * 4)-64; //-64 is to account for the header
			status = 0;
			ucSendAnother = 0;
			if (uiMaxLen > StreamFIFO_MaxPacket)
				uiMaxLen = StreamFIFO_MaxPacket;
			if (uiMaxLen > 0)
				status = MakeMostFullLFR(&SendDataBuff[2], uiMaxLen, &StreamFIFO_SqnNum);

			if (status > 0)
			{
				u32 dummy;
				status += 4; //for \n\r
				SendDataBuff[0] = '\n'; SendDataBuff[1] = '\r';
				SendDataBuff[status-2] = '\n'; SendDataBuff[status-1] = '\r';
				if ( (tcp_space_avail - status >= MAX_SEND_PACKET_SIZE) && (DetermineMostFullLog(&dummy) >= MAX_SEND_PACKET_SIZE/4 )) 	//if we have at least a half packet that we can send
					ucSendAnother = 2;

				err = tcp_write(comm, SendDataBuff, status, 1+ ucSendAnother);
				if (err != ERR_OK)
					status = 0; //break point
				retVal += status;
			}

		} while(ucSendAnother > 0 );
	}
	return retVal;
}

//internal functions
static int CmdBMK(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdCFG(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdCNT(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdCST(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdCTT(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdHBA(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdHBV(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdLFH(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdLM(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdLFR(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdLFS(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdLS(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdPRE(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdReadADC(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdWD(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdTRE(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdTRT(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdTCE(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdVSL(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdVSI(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdFSW(const char * szCmd, char *szResponse, void *CommDevice);
static int CmdDEA(const char * szCmd, char *szResponse, void *CommDevice);

typedef struct command_table_entry {
	char *szCmd;
	char *szDescription;
	int (*cmd_function)(const char *, char *, void *);
} command_table_entry;

#define NUM_CMD 22
command_table_entry command_table[NUM_CMD] = {
		{"BMK", "Benchmark the PI controller logic", CmdBMK},
		{"CFG", "CFG=d: Write setting d to configuration register (d = 0 to 255)", CmdCFG},
		{"CNT", "CNT=n,cmd: Send the control application a message", CmdCNT},
		{"CST", "CST=n,cmd: Start control application n with startup command cmd", CmdCST},
		{"CTT", "CTT=n,cmd: Terminate control application n with command cmd", CmdCTT},
		{"HBA", "HBV=l1,l2,V,omega: Make an H-bridge from legs l1 and l2, output V (0-100), with omega (=int*128)", CmdHBA},
		{"HBV", "HBV=l1,l2,V: Make an H-bridge from legs l1 and l2 and output V (0-100)", CmdHBV},
		{"LFH", "Stop logging and hault sending unsolicited LFRs", CmdLFH},
		{"LFR", "Read a log output", CmdLFR},
		{"LFS", "Start logging and stream FIFO outputs (unsolicited LFRs sent until LFH recv'd)", CmdLFS},
		{"LM", "Log a memory address", CmdLM},
		{"LS", "Start logging", CmdLS},
		{"PRE", "PRE=toDev,fromDev: Read PECB data bus error count", CmdPRE},
		{"RADC", "Read ADC converter output", CmdReadADC},
		{"TCE", "Clear timing errors", CmdTCE},
		{"TRE", "Read timing errors", CmdTRE},
		{"TRT", "Read timer ticks", CmdTRT},
		{"WD", "WDn=d: Write d duty cycle register n (0 for off)", CmdWD},
		{"VSL", "VSL=l1,l2,l3: Set output legs for VSI", CmdVSL},
		{"VSI", "VSI=V,freq(,ramptime): Set V (percent output voltage, 0-100), with freq (Hz). Optional ramp time (ms)", CmdVSI},
		{"FSW", "FSW=frequency: Set PWM switching frequency (100Hz to 4MHz)", CmdFSW},
		{"DEA", "DEA=ns: Set PWM dead time in ns (25ns to ...)", CmdDEA}
	};





/*****************************
 * CmdLR()
 * Handle the LR command. Read portion of the log.
 * LR=index,start,length
 *
 * (must have already send LM commands to setup the log)
 *
 * Response: LR=index,start,length,binary_data
 * **The response length will be different from the
 * **  command length if the end of the log is reached
 * **binary_data is byte-for-byte what is in the log.
 * --> only 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
#if 0
static int CmdLR(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[3];
	char bError = 0;
	int retVal = 0;
	u32 uiLogIndex, uiStart, uiLength;

	//get the log index number
	uiLogIndex = atoi(szNum);

	if ( uiLogIndex >= MAX_LOG_VARS )
		bError = 1;
	else
	{
//get the number of bytes to read
		int i;
		for (i = 0; i < strlen(szCmd); i++)
			if (szCmd[i] == ',')
			{
				szNum = &szCmd[i+1];
				break;
			}
		if (i == strlen(szCmd))
			bError = 1;
		else
		{
			uiStart = atoi(szNum);

			for (i++; i < strlen(szCmd); i++)
				if (szCmd[i] == ',')
				{
					szNum = &szCmd[i+1];
					break;
				}
			if (i == strlen(szCmd))
				bError = 1;
			else
			{
				int iStatus;
				uiLength = atoi(szNum);
				iStatus = ReadFromLog((unsigned char*) szResponse, uiLogIndex, uiStart, uiLength);
				if (iStatus < 0)
					bError = 1;
				else
				{
					char szTemp[64];
					sprintf(szTemp, "LR=%d,%d,%d,", (int) uiLogIndex, (int) uiStart, (int) iStatus);
					memmove(&szResponse[strlen(szTemp)], szResponse, iStatus);
					memcpy(szResponse, szTemp, strlen(szTemp));
					retVal = strlen(szTemp) + iStatus;
				}
			}

		}
	}
	if (bError)
	{
		strcat(szResponse, "ERROR");
		retVal = strlen(szResponse);
	}

	return retVal;
}
#endif
/*****************************
 * CmdLLR()
 * Handle the LFR command. Read portion of the log.
 * LFR=index,length
 *
 * (must have already send LM commands to setup the log)
 *
 * Response: LFR=sqn_num, index,flags,length,binary_data
 * **The response length will be different from the
 * **  command length if the end of the log is reached
 * **binary_data is byte-for-byte what is in the log.
 * --> only 100us errors are but response is padded to 10 fields
 * **sqn_num is always 1
 *szCmd: the command
 *szResponse: the response
 */
static int CmdLFR(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[4];
	char bError = 0;
	int retVal = 0;
	int iStatus1, iStatus2;
	u32 uiLogIndex, uiLength, uiFlags;

	//get the log index number
	uiLogIndex = atoi(szNum);

	if ( uiLogIndex >= MAX_LOG_VARS )
		bError = 1;
	else
	{
//get the number of bytes to read
		int i;
		for (i = 0; i < strlen(szCmd); i++)
			if (szCmd[i] == ',')
			{
				szNum = &szCmd[i+1];
				break;
			}
		if (i == strlen(szCmd))
			bError = 1;
		else
		{
			uiLength = atoi(szNum);

			iStatus1 = ReadFromLog((unsigned char*) szResponse, uiLogIndex, uiLength);
			iStatus2 = ReadLogFlags(&uiFlags, uiLogIndex);
			if ( (iStatus1 < 0) || (iStatus2 < 0))
				bError = 1;
			else
			{
				char szTemp[64];
				sprintf(szTemp, "LFR=1,%d,%d,%d,", (int) uiLogIndex, (int) uiFlags, (int) iStatus1);
				memmove(&szResponse[strlen(szTemp)], szResponse, iStatus1);
				memcpy(szResponse, szTemp, strlen(szTemp));
				retVal = strlen(szTemp) + iStatus1;
			}
		}
	}
	if (bError)
	{
		strcat(szResponse, "ERROR");
		retVal = strlen(szResponse);
	}

	return retVal;
}


/*****************************
 * CmdBMK()
 * Benchmark a PI controller
 * BMK
 *
 * (must have already send LM commands to setup the log)
 *
 * Response: BMK=num
 * number of time the PI controller was called in 1 second.
 *szCmd: the command
 *szResponse: the response
 */
static int CmdBMK(const char * szCmd, char *szResponse, void *CommDevice)
{
	u32 started = MainTimerTimerTick;
	u32 count = 0;
	double input;
	double output;
	double abc[3], dq0[3];
	pi_controller cont;
	controller_context io;
	controller_holder ctxt;
	io.ClockTime = 10;
	io.NumClocks = 1;
	cont.kits = 10.315;
	cont.kp = 1.42;
	cont.plusSat = 5.12;
	cont.negSat = -100.91;
	cont.lastInt = 0;
	io.inputs = &input;
	io.outputs = &output;
	io.NumInputs = 1;
	io.NumOutputs = 1;
	io.Status = C_RUNNING;
	ctxt.control_io = &io;
	ctxt.controller_info = &cont;
	while (MainTimerTimerTick - started < 100000)
	{
		/*input += 0.1;
		if (input > cont.plusSat + 0.5)
			input = cont.negSat;
		RunPI_Controller(&ctxt);*/
		abc[0] = count/1000.0;
		abc[1] = (double) MainTimerTimerTick/100000.0;
		abc[2] = 3*abc[0];
		DQ0_Transform(((double) MainTimerTimerTick)/100000.0, abc, dq0);
		count++;
	}

	sprintf(szResponse, "BMK=%d", (int) count);
	return strlen(szResponse);
}


/*****************************
 * CmdLS()
 * Handle the LS command. Start logging.
 * LS
 *
 * (must have already send LM commands to setup the log)
 *
 * Response: OK
 * --> only 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdLS(const char * szCmd, char *szResponse, void *CommDevice)
{
	if (StartLogging() == SUCCESS)
		strcat(szResponse, "OK");
	else
		strcat(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdLFS()
 * Handle the LFS command. Start logging and start sending unsolicited
 * LFRs.
 * LFS=max_packet_size (in bytes)
 *
 * (must have already sent LM commands to setup the log)
 *
 * Response: OK
 *szCmd: the command
 *szResponse: the response
 */
static int CmdLFS(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[4];
	u32 tempLen = 0;

	//get the log index number
	tempLen = atoi(szNum);

	if ( (StartLogging() == SUCCESS) && (tempLen > 0) && (tempLen % 4 == 0))
	{
		strcat(szResponse, "OK");
		StreamFIFO_SqnNum = 1;
		StreamFIFO_MaxPacket = tempLen;
		StreamFIFOComm = CommDevice;
		bPrimeStreamFIFO = TRUE;
	}
	else
		strcat(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdLFH()
 * Handle the LFH command. Hault sending unsolicited LFRs
 * LFH
 *
 *
 * Response: OK
 * --> only 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdLFH(const char * szCmd, char *szResponse, void *CommDevice)
{
	StreamFIFOComm = 0;
	bPrimeStreamFIFO = FALSE;

	StopAllLogging();
	strcat(szResponse, "OK");

	return strlen(szResponse);
}




/*****************************
 * CmdLM()
 * Handle the LM command. Log a memory address
 * LM=index,address,settings
 * index: log file index (0 to MAX_LOG_VARS-1)
 * Response: OK
 * --> only 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdLM(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[3];
	char bError = 0;
	u32 uiLogIndex, uiAddr, uiSettings;

	//get the log index number
	uiLogIndex = atoi(szNum);

	if ( uiLogIndex >= MAX_LOG_VARS )
		bError = 1;
	else
	{
//get the memory address
		int i;
		for (i = 0; i < strlen(szCmd); i++)
			if (szCmd[i] == ',')
			{
				szNum = &szCmd[i+1];
				break;
			}
		if (i == strlen(szCmd))
			bError = 1;
		else
		{
			uiAddr = atoi(szNum);
			for (i++; i < strlen(szCmd); i++)
				if (szCmd[i] == ',')
				{
					szNum = &szCmd[i+1];
					break;
				}
			if (i == strlen(szCmd))
				bError = 1;
			else
			{
				uiSettings = atoi(szNum);

				if (SetupLogVariable(uiLogIndex, (u32 *) uiAddr, uiSettings) != SUCCESS)
					bError = 1;
				else
					strcat(szResponse, "OK");
			}
		}
	}
	if (bError)
		strcat(szResponse, "ERROR");

	return strlen(szResponse);
}


unsigned char HBA_bEnabled = 0;
u32 HBA_uiLeg1, HBA_uiLeg2;
double HBA_dbVPercent, HBA_dbOmega, HBA_dbTheta;

/*****************************
 * CmdHBA()
 * Handle the 100uS tick for the ac HBridge function.
 */
void HBA_100usTick()
{
	//generate ac for Hbridge
	if (HBA_bEnabled){
		u8 duty1, duty2;
		double update_da = ((double) HBA_dbOmega/( (double) 10000));
		double dbVPercent;
		HBA_dbTheta += update_da;
		if (HBA_dbTheta > 6.283185307179586)
			HBA_dbTheta -= 6.283185307179586;

		dbVPercent = HBA_dbVPercent*cos(HBA_dbTheta);

		duty1 = (unsigned char) 127*(1 + dbVPercent);
		duty2 = (unsigned char) 127*(1 - dbVPercent);

		WriteDutyRatio(HBA_uiLeg1, duty1);
		WriteDutyRatio(HBA_uiLeg2, duty2);
	}
}


// Ramps between `current` and `setpoint` values assuming Fs = 10kHz
// Updates `current` as it ramps.
//
// `ramprate` is in (units of `current`) / sec
//     0 implies no ramp
//
//
// Returns delta in Ts for theta update
//
double omega_ramp_fcn(double *current, double *setpoint, double ramprate)
{
	double ret;

	if (ramprate != 0 && *current != *setpoint) {
		// Ramping
		double dir = (*setpoint > *current) ? 1 : -1;
		double del = ramprate / 10000.0;

		*current += (dir * del);
		ret = *current / 10000.0;

		// Check if done ramping
		if ((dir == 1)  && (*current > *setpoint)) *current = *setpoint;
		if ((dir == -1) && (*current < *setpoint)) *current = *setpoint;
	} else {
		// Not ramping
		ret = *setpoint / 10000.0;
	}

	return ret;
}

uint8_t VSI_enabled = 0;
uint8_t VSI_leg1;
uint8_t VSI_leg2;
uint8_t VSI_leg3;
double VSI_Vpercent = 0;
double VSI_omega = 0;
double theta = 0;

double VSI_omega_ramp = 0;
double VSI_old_Vpercent = 0;
double VSI_old_omega = 0;

double VSI_R;
double VSI_V0;

/*****************************
 * CmdVSI()
 * Handle the 100uS tick for the VSI function.
 */
void VSI_100usTick(void)
{
	if (VSI_enabled) {
//		uint8_t duty1, duty2, duty3;

		// Calculate `da`
		double update_da = omega_ramp_fcn(&VSI_old_omega, &VSI_omega, VSI_omega_ramp);

		// Calculate dv
		// ramp(&VSI_old_Vpercent, &VSI_Vpercent, 0.1);

		theta += update_da;
		if (theta > 6.283185307179586)
			theta -= 6.283185307179586;

		double v = (VSI_R * VSI_old_omega) + VSI_V0;

		double percent1 = v*cos(theta);
		double percent2 = v*cos(theta - PI23);
		double percent3 = v*cos(theta + PI23);

//		duty1 = (unsigned char) 127*(1 + percent1);
//		duty2 = (unsigned char) 127*(1 + percent2);
//		duty3 = (unsigned char) 127*(1 + percent3);

		pwm_set_duty(VSI_leg1 - 1, (1 + percent1) / 2.0);
		pwm_set_duty(VSI_leg2 - 1, (1 + percent2) / 2.0);
		pwm_set_duty(VSI_leg3 - 1, (1 + percent3) / 2.0);

//		WriteDutyRatio(VSI_leg1, duty1);
//		WriteDutyRatio(VSI_leg2, duty2);
//		WriteDutyRatio(VSI_leg3, duty3);
	} else {
		WriteDutyRatio(VSI_leg1, 0);
		WriteDutyRatio(VSI_leg2, 0);
		WriteDutyRatio(VSI_leg3, 0);
	}
}


/*****************************
 * CmdHBA()
 * Handle the HBA command. Make an HBridge from two legs and
 * output the specified voltage as a sine wave varying from 0
 * to the peak value specified
 * HBV=leg1,leg2,voltage,omega
 * leg1: number of leg 1 (1 to NUM_LEGS)
 * leg2: number of leg 2 (1 to NUM_LEGS)
 * voltage: percent of bus voltage (0 to 100)
 * omega: 0 to 2*pi*10000, specified as an integer which must
 * 			be divided by 128
 * Response: OK
 *szCmd: the command
 *szResponse: the response
 */
static int CmdHBA(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[4];
	char bError = 0;
	u32 uiLeg1, uiLeg2, uiVoltagePercent;

//get the leg 1 number
	uiLeg1 = atoi(szNum);

	if ( (uiLeg1 > NUM_LEGS) || (uiLeg1 == 0) )
		bError = 1;
	else
	{
//get the leg 2 number
		int i;
		for (i = 0; i < strlen(szCmd); i++)
			if (szCmd[i] == ',')
			{
				szNum = &szCmd[i+1];
				break;
			}
		if (i == strlen(szCmd))
			bError = 1;
		else
		{
			uiLeg2 = atoi(szNum);
			if ( (uiLeg2 > NUM_LEGS) || (uiLeg2 == 0) )
				bError = 1;
			else
			{
				for (i++; i < strlen(szCmd); i++)
					if (szCmd[i] == ',')
					{
						szNum = &szCmd[i+1];
						break;
					}
				if (i == strlen(szCmd))
					bError = 1;
				else
				{
					double dbVPercent;
					uiVoltagePercent = atoi(szNum);
					dbVPercent = ((double) uiVoltagePercent)/100;

					for (i++; i < strlen(szCmd); i++)
						if (szCmd[i] == ',')
						{
							szNum = &szCmd[i+1];
							break;
						}
					if (i == strlen(szCmd))
						bError = 1;
					else
					{
						HBA_dbOmega = ((double) atoi(szNum)) / ((double) 128);
						HBA_dbVPercent = dbVPercent;
						HBA_uiLeg1 = uiLeg1;
						HBA_uiLeg2 = uiLeg2;
						HBA_bEnabled = 1;
						strcat(szResponse, "OK");
					}
				}
			}
		}
	}
	if (bError)
		strcat(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdHBV()
 * Handle the HBV command. Make an HBridge from two legs and
 * output the specified voltage
 * HBV=leg1,leg2,voltage
 * leg1: number of leg 1 (1 to NUM_LEGS)
 * leg2: number of leg 2 (1 to NUM_LEGS)
 * voltage: percent of bus voltage (0 to 100)
 * Response: OK
 *szCmd: the command
 *szResponse: the response
 */
static int CmdHBV(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[4];
	char bError = 0;
	u32 uiLeg1, uiLeg2, uiVoltagePercent;

//get the leg 1 number
	uiLeg1 = atoi(szNum);

	if ( (uiLeg1 > NUM_LEGS) || (uiLeg1 == 0) )
		bError = 1;
	else
	{
//get the leg 2 number
		int i;
		for (i = 0; i < strlen(szCmd); i++)
			if (szCmd[i] == ',')
			{
				szNum = &szCmd[i+1];
				break;
			}
		if (i == strlen(szCmd))
			bError = 1;
		else
		{
			uiLeg2 = atoi(szNum);
			if ( (uiLeg2 > NUM_LEGS) || (uiLeg2 == 0) )
				bError = 1;
			else
			{
				for (i++; i < strlen(szCmd); i++)
					if (szCmd[i] == ',')
					{
						szNum = &szCmd[i+1];
						break;
					}
				if (i == strlen(szCmd))
					bError = 1;
				else
				{
					double dbVPercent;
					u8 duty1, duty2;
					uiVoltagePercent = atoi(szNum);
					dbVPercent = ((double) uiVoltagePercent)/100;
					duty1 = (unsigned char) 127*(1 + dbVPercent);
					duty2 = (unsigned char) 127*(1 - dbVPercent);

					WriteDutyRatio(uiLeg1, duty1);
					WriteDutyRatio(uiLeg2, duty2);
					strcat(szResponse, "OK");
				}
			}
		}
	}
	if (bError)
		strcat(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * CmdReadADC()
 * Handle the RADC command
 * RADCn - n is adc number (1 - 12)
 * Response: RADCn=xx.xxxxV
 *szCmd: the command
 *szResponse: the response
 */
static int CmdReadADC(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szADCnum = &szCmd[4];
	double dbADC = 0;
	u32 uiADCnum = atoi(szADCnum);
	if ( (uiADCnum > 0) && (uiADCnum < 13) )
	{
		dbADC = ReadADC(uiADCnum);
		sprintf(szResponse, "ADC%d=%2.4fV", (int)uiADCnum,dbADC); //reads +-10V, accurate to 0.0049V
	}
	else
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdTCE()
 * Handle the TCE command. Clear timing errors.
 * RTE
 * Response: OK
 * --> only 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdTCE(const char * szCmd, char *szResponse, void *CommDevice)
{
	Timing10usErrorCount = 0;
	Timing100usErrorCount = 0;
	ControlTimer0ErrorCount = 0;
	ControlTimer1ErrorCount = 0;
	strcpy(szResponse, "OK");

	return strlen(szResponse);
}


/*****************************
 * CmdTRT()
 * Handle the TRT command. Read timing ticks.
 * RTE
 * Response: TRT=main,control0,control1,,,,,,,
 * --> only first 3 are defined but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdTRT(const char * szCmd, char *szResponse, void *CommDevice)
{
	/*u32 TimerCount1 = XTmrCtr_ReadReg(0x42800000,
			TIMER_CNTR_0, XTC_TCR_OFFSET);*/

	sprintf(szResponse, "TRT=%d,%d,%d,,,,,,,", (int) MainTimerTimerTick, (int) ControlTimer0_TickCount, (int) ControlTimer1_TickCount);

	return strlen(szResponse);
}

/*****************************
 * CmdPRE()
 * Handle the PRE command. Read PECB databus errors.
 * RTE
 * Response: PRE=ToPECB,FromPECB
 *szCmd: the command
 *szResponse: the response
 */
static int CmdPRE(const char * szCmd, char *szResponse, void *CommDevice)
{
	/*u32 TimerCount1 = XTmrCtr_ReadReg(0x42800000,
			TIMER_CNTR_0, XTC_TCR_OFFSET);*/
	u32 ec_to_dev, ec_from_dev;
	ec_to_dev = PECB[ER_CNT] & 0xFFFF;
	ec_from_dev = (PECB[ER_CNT] >> 16) & 0xFFFF;
	sprintf(szResponse, "TRE=%d,%d,%d,%d", (int) ec_to_dev, (int) ec_from_dev, (int) PECB[ER_CNT-1], (int) PECB[ER_CNT-2]);

	return strlen(szResponse);
}


/*****************************
 * CmdTRE()
 * Handle the TRE command. Read timing errors.
 * RTE
 * Response: TRE=10us,100us,,,,,,,,
 * --> only 10us, 100us errors are but response is padded to 10 fields
 *szCmd: the command
 *szResponse: the response
 */
static int CmdTRE(const char * szCmd, char *szResponse, void *CommDevice)
{
	/*u32 TimerCount1 = XTmrCtr_ReadReg(0x42800000,
			TIMER_CNTR_0, XTC_TCR_OFFSET);*/

	sprintf(szResponse, "TRE=%d,%d,%d,%d,,,,,,", (int) Timing10usErrorCount, (int) Timing100usErrorCount, (int) ControlTimer0ErrorCount, (int) ControlTimer1ErrorCount);

	return strlen(szResponse);
}


/*****************************
 * CmdCFG()
 * Handle the CFG command. Write setting to config register.
 * CFG=d
 * --> d: (0 to 255)
 *szCmd: the command
 *szResponse: the response
 */
static int CmdCFG(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[4];
	char bError = 0;
	u32 uiValue;
	if ( (szNum[0] < 0x30) || (szNum[0] > 0x39) )
		bError = 1;
	else
	{
//get the value
		uiValue = atoi(szNum);
		WriteConfigRegister((u8) uiValue);
		sprintf(szResponse, "OK");
	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * CmdWD()
 * Handle the WD command. Write duty ratio.
 * WDn=d
 * --> n: duty ratio register to write to
 * --> d: (0 to 255)
 *szCmd: the command
 *szResponse: the response
 */
static int CmdWD(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szNum = &szCmd[2];
	char bError = 0;
	u32 uiLeg, uiValue;
	if ( (szNum[0] < 0x31) )
		bError = 1;
	else
	{
//get the leg number
		uiLeg = atoi(szNum);

		if ( (uiLeg < 1) || (uiLeg > NUM_LEGS) )
			bError = 1;
		else
		{
//get new duty ratio
			int i;
			for (i = 0; i < strlen(szCmd); i++)
				if (szCmd[i] == '=')
				{
					szNum = &szCmd[i+1];
					break;
				}
			if (i == strlen(szCmd))
				bError = 1;
			else
			{
				uiValue = atoi(szNum);
				if (uiValue > 255)
					bError = 1;
				else
				{
					WriteDutyRatio(uiLeg, (unsigned char) uiValue);
					sprintf(szResponse, "OK");
				}
			}
		}
	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * CmdCNT()
 * Handle the CNT command. Send a control application a command.
 * CNT=d,CMD
 * --> n: which control application to send the command to (number)
 * --> CMD: The command to send
 *szCmd: the command
 *szResponse: the response
 */
static int CmdCNT(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szAppCMD = &szCmd[4];

	u32 uiApp;
	{
//get the control application to send to
		uiApp = atoi(szAppCMD);

		if ( uiApp >= 1) //|| (uiLeg > NUM_LEGS) )
		{
//get new duty ratio
			int i;
			for (i = 0; i < strlen(szCmd); i++)
				if (szCmd[i] == ',')
				{
					szAppCMD = &szCmd[i+1];
					break;
				}
			if (i != strlen(szCmd))
			{
				if (uiApp == 1)
					return ControlAppCommand(szAppCMD, szResponse, CommDevice);
				else if (uiApp == 2)
					return TorqueVSI_Command(szAppCMD, szResponse, CommDevice);
			}
		}
	}
	strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * CmdCTT()
 * Handle the CTT command. Start a control application with a command.
 * CTT=n,CMD
 * --> n: which control application to start (number)
 * --> CMD: The start command to send
 *szCmd: the command
 *szResponse: the response
 */
static int CmdCTT(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szAppCMD = &szCmd[4];

	u32 uiApp;
	{
//get the control application to send to
		uiApp = atoi(szAppCMD);

		if ( uiApp >= 1) //|| (uiLeg > NUM_LEGS) )
		{
//get new duty ratio
			int i;
			for (i = 0; i < strlen(szCmd); i++)
				if (szCmd[i] == ',')
				{
					szAppCMD = &szCmd[i+1];
					break;
				}
			if (i != strlen(szCmd))
			{
				if (uiApp == 1)
					return ControlStopAppCommand(szAppCMD, szResponse, CommDevice);
				else if (uiApp == 2)
					return TorqueVSI_StopCommand(szAppCMD, szResponse, CommDevice);
			}
		}
	}
	strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdCST()
 * Handle the CST command. Start a control application with a command.
 * CST=n,CMD
 * --> n: which control application to start (number)
 * --> CMD: The start command to send
 *szCmd: the command
 *szResponse: the response
 */
static int CmdCST(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *szAppCMD = &szCmd[4];

	u32 uiApp;
	{
//get the control application to send to
		uiApp = atoi(szAppCMD);

		if ( uiApp >= 1) //|| (uiLeg > NUM_LEGS) )
		{
//get new duty ratio
			int i;
			for (i = 0; i < strlen(szCmd); i++)
				if (szCmd[i] == ',')
				{
					szAppCMD = &szCmd[i+1];
					break;
				}
			if (i != strlen(szCmd))
			{
				if (uiApp == 1)
					return ControlStartAppCommand(szAppCMD, szResponse, CommDevice);
				else if (uiApp == 2)
					return TorqueVSI_StartCommand(szAppCMD, szResponse, CommDevice);
			}
		}
	}
	strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}

/*****************************
 * CmdVSL()
 * Handle the VSL command.
 *
 * VSL=leg1,leg2,leg3
 *
 * leg1: number of leg 1 (1 to NUM_LEGS)
 * leg2: number of leg 2 (1 to NUM_LEGS)
 * leg3: number of leg 3 (1 to NUM_LEGS)
 *
 * Response: OK
 *
 *szCmd: the command
 *szResponse: the response
 */

static char bufferVSL[128];
static int CmdVSL(const char * szCmd, char *szResponse, void *CommDevice)
{
	char *p;
	int i = 0;

	// Create copy of cmd for parsing
	memset(bufferVSL, 0, 128);
	strcpy(bufferVSL, szCmd);

	// Parse out tokens
	int iLeg1 = 0;
	int iLeg2 = 0;
	int iLeg3 = 0;

	p = strtok(bufferVSL, "=,");
	while (p != NULL) {
		// Use current token...
		switch (i) {
		case 0:
			// Ignore 'VSL'
			break;
		case 1:
			iLeg1 = atoi(p);
			break;
		case 2:
			iLeg2 = atoi(p);
			break;
		case 3:
			iLeg3 = atoi(p);
			break;
		default:
			// This is an error!
			// Force error below
			iLeg1 = 0;
			iLeg2 = 1;
			iLeg3 = 1;
			break;
		}

		// Get next token
		p = strtok(NULL, "=,");
		i++;
	}

	// Check if all 0 => disable VSI
	if (iLeg1 == 0 && iLeg2 == 0 && iLeg3 == 0) {
		VSI_enabled = 0;
		strcat(szResponse, "OK");
		return strlen(szResponse);
	}

	// Check if errors in leg numbers
	int bError = 0;
	if (iLeg1 <= 0 || iLeg1 > NUM_LEGS) bError = 1;
	if (iLeg2 <= 0 || iLeg2 > NUM_LEGS) bError = 1;
	if (iLeg3 <= 0 || iLeg3 > NUM_LEGS) bError = 1;

	if (bError) {
		strcat(szResponse, "ERROR");
		return strlen(szResponse);
	}

	// Set legs for VSI
	VSI_leg1 = iLeg1;
	VSI_leg2 = iLeg2;
	VSI_leg3 = iLeg3;

	// Enable VSI
	VSI_enabled = 1;

	strcat(szResponse, "OK");
	return strlen(szResponse);
}


/*****************************
 * CmdVSI()
 * Handle the VSI command.
 *
 * VSI=voltage,freq(,ramp)
 *
 * voltage: peak voltage output percent of Vbus (0 to 100)
 * freq: frequency of output (Hz)
 * ramptime: (optional) duration of ramping (ms)
 *
 * Response: OK
 *
 *szCmd: the command
 *szResponse: the response
 */

static char bufferVSI[128];
static int CmdVSI(const char * szCmd, char *szResponse, void *CommDevice)
{
	char *p;
	int i = 0;

	// Create copy of cmd for parsing
	memset(bufferVSI, 0, 128);
	strcpy(bufferVSI, szCmd);

	// Parse out tokens
	int iVoltagePercent = 0;
	int iHz = 0;
	int iRamptime = 0;

	p = strtok(bufferVSI, "=,");
	while (p != NULL) {
		// Use current token...
		switch (i) {
		case 0:
			// Ignore 'VSI'
			break;
		case 1:
			iVoltagePercent = atoi(p);
			break;
		case 2:
			iHz = atoi(p);
			break;
		case 3:
			iRamptime = atoi(p);
			break;
		default:
			// This is an error!
			// Force error below
			iHz = 0;
			break;
		}

		// Get next token
		p = strtok(NULL, "=,");
		i++;
	}

	// Check for errors while parsing
//	if (iHz == 0 || iVoltagePercent == 0) {
//		strcat(szResponse, "ERROR");
//		return strlen(szResponse);
//	}

	// Convert voltage percent input to double percentage
	VSI_old_Vpercent = VSI_Vpercent;
	VSI_Vpercent = iVoltagePercent / 100.0;
	if (VSI_Vpercent > 100) VSI_Vpercent = 100.0;
	if (VSI_Vpercent < 0)   VSI_Vpercent = 0.0;


	// Convert freq input to rad/sec
	VSI_old_omega = VSI_omega;
	VSI_omega = 2 * PI * iHz;

	if (iRamptime > 0) {
		VSI_omega_ramp = abs(VSI_old_omega - VSI_omega) / (iRamptime / 1000.0); // (rad/sec) / sec
	} else {
		VSI_omega_ramp = 0;
	}


	if (VSI_omega != VSI_old_omega) {
		// There is delta freq requested
		VSI_R = (VSI_Vpercent - VSI_old_Vpercent) / (VSI_omega - VSI_old_omega);
		VSI_V0 = VSI_Vpercent - (VSI_R * VSI_omega);
	} else {
		// Instaneous voltage change
		VSI_R = 0;
		VSI_V0 = VSI_Vpercent;
	}

	strcat(szResponse, "OK");
	return strlen(szResponse);
}

static char bufferFSW[128];
static int CmdFSW(const char * szCmd, char *szResponse, void *CommDevice)
{
	char *p;
	int i = 0;

	// Create copy of cmd for parsing
	memset(bufferFSW, 0, 128);
	strcpy(bufferFSW, szCmd);

	// Parse out tokens
	int iHz = 0;

	p = strtok(bufferFSW, "=,");
	while (p != NULL) {
		// Use current token...
		switch (i) {
		case 0:
			// Ignore 'FSW'
			break;
		case 1:
			iHz = atoi(p);
			break;
		default:
			// This is an error!
			// Force error below
			iHz = 0;
			break;
		}

		// Get next token
		p = strtok(NULL, "=,");
		i++;
	}

	// Check for errors while parsing
	if (iHz < 100 || iHz > 4000000) {
		strcat(szResponse, "ERROR");
		return strlen(szResponse);
	}

	// do work
	pwm_set_switching_freq((double) iHz);

	strcat(szResponse, "OK");
	return strlen(szResponse);
}



static char bufferDEA[128];
static int CmdDEA(const char * szCmd, char *szResponse, void *CommDevice)
{
	char *p;
	int i = 0;

	// Create copy of cmd for parsing
	memset(bufferDEA, 0, 128);
	strcpy(bufferDEA, szCmd);

	// Parse out tokens
	int iNs = 0;

	p = strtok(bufferDEA, "=,");
	while (p != NULL) {
		// Use current token...
		switch (i) {
		case 0:
			// Ignore 'DEA'
			break;
		case 1:
			iNs = atoi(p);
			break;
		default:
			// This is an error!
			// Force error below
			iNs = 0;
			break;
		}

		// Get next token
		p = strtok(NULL, "=,");
		i++;
	}

	// Check for errors while parsing
	if (iNs < 25 || iNs > 1e9) {
		strcat(szResponse, "ERROR");
		return strlen(szResponse);
	}

	// do work
	pwm_set_deadtime_ns(iNs);

	strcat(szResponse, "OK");
	return strlen(szResponse);
}







//call this with the CR removed
//load the response into szResponse
int CommandParser(const char * szCmd, char *szResponse, void *CommDevice)
{
	char szTemp[256];
	int retVal = 0;
	//read ADC
	int i;
	char bFoundCmd = 0;
	szResponse[0] = 0; //put a leading null in to prevent any issues
	for (i = 0; i<NUM_CMD; i++)
		if (strstr(szCmd, command_table[i].szCmd) == szCmd)
		{
			bFoundCmd = 1;
			retVal = command_table[i].cmd_function(szCmd, szResponse, CommDevice);
			break;
		}
	if (!bFoundCmd)
	{
		if (strstr(szCmd, "?") == szCmd)
			for (i = 0; i < NUM_CMD; i++)
			{
				memset(szTemp, 0, sizeof(szTemp));
				sprintf(szTemp, "\r\n%s: %s", command_table[i].szCmd, command_table[i].szDescription);
				strcat(szResponse, szTemp);
			}
		else
			sprintf(szResponse, "Bad command: \"%s\"", szCmd);

		retVal = strlen(szResponse);
	}
	return retVal;
}



/*****************************
 * HandleCommands()
 * Call this function to handle incoming data
 *
 *
 *szRecvCmd: a pointer to memory that the function can use
 *state: the previously returned value (0 the first time the function is called)
 */
u32 HandleCommandsUart(char *szRcvCmd, u32 state)
{
	//static char szRcvCmd[4096];
	char szSendCmd[4096];
	//static char init = 0;
	char cRcvd[2];


	if (XUartPs_IsReceiveData(STDIN_BASEADDRESS))
	{
		cRcvd[0] = inbyte();
		cRcvd[1] = 0;
		print(cRcvd);

		if (cRcvd[0] == '\r')
		{
			int outLen = 0;
			memset (szSendCmd, 0, sizeof(szSendCmd));
			outLen = CommandParser(szRcvCmd, szSendCmd, 0);
			state = 0;

			//if sending string data
			/*if (outLen == strlen(szSendCmd))
				printf("\n%s\n", szSendCmd);
			//otherwise binary
			else
			{*/
				int i;
				print("\r\n");
				for (i = 0; i < outLen; i++)
					outbyte(szSendCmd[i]);
				print("\r\n");
			//}
		}
		else if (cRcvd[0] != '\n')	//ignore line feeds
		{
			szRcvCmd[state++] = cRcvd[0];
			szRcvCmd[state] = 0; //always null terminate

			//strcat(szRcvCmd, cRcvd);
		}
	}
	return state;
}

/*****************************
 * HandleCommandsGeneral()
 * Call this function to handle incoming data and get a response
 *
 *
 *szRecvCmd: a pointer to memory that the function can use
 *state: the previously returned value (0 the first time the function is called)
 */
u32 HandleCommandsGeneral(char *szOutData, char *szRcvCmd, char *szNewData, u32 NewDataLength, u32 *state, void *ComDevice)
{
	char *szSendCmd = szOutData;
	int i;
	char cRcvd[2];
	u32 outLen = 0;
	for (i = 0; i < NewDataLength; i++)
	{
		cRcvd[0] = *szNewData;
		cRcvd[1] = 0;
		szNewData++;

		*szSendCmd = cRcvd[0];
		outLen++;
		szSendCmd++;

		if (cRcvd[0] == '\r')
		{
			int newLen;
			//empty received data
			*state = 0;
			*szSendCmd = '\r';
			szSendCmd++;
			*szSendCmd = '\n';
			szSendCmd++;
			newLen = CommandParser(szRcvCmd, szSendCmd, ComDevice);
			if (newLen > 0)
			{
				szSendCmd[newLen++] = '\r';
				szSendCmd[newLen++] = '\n';
				outLen += newLen + 2;
			}
		}
		else if (cRcvd[0] != '\n')	//ignore line feeds
		{
			szRcvCmd[*state] = cRcvd[0];
			*state += 1;
			szRcvCmd[*state] = 0; //always null terminate

			//strcat(szRcvCmd, cRcvd);
		}
	}
	return outLen;
}


<|start_filename|>sdk/bare/common/drv/timer.c<|end_filename|>
#include "drv/timer.h"
#include "xparameters.h"
#include "xscugic.h"
#include "xtmrctr.h"
#include <stdio.h>

// Get the TIMER_0 device from the FPGA
// Parameter definitions
#define TMR_DEVICE_ID                 XPAR_CONTROL_TIMER_0_DEVICE_ID
#define INTC_TMR_INTERRUPT_ID         XPAR_FABRIC_CONTROL_TIMER_0_INTERRUPT_INTR
#define TIMER_LOAD_VALUE(period_usec) ((0xFFFFFFFF) - (((period_usec) *200) - 2))

// PERIOD = ((2^32-1) - (TMR_LOAD_VALUE) + 2) * 5e-9

static XScuGic intCtrl;
static XTmrCtr timer;

void fatalError(char *str)
{
    printf("ERROR: %s\n", str);
    while (1) {
    }
}

void timer_init(XTmrCtr_Handler timer_isr, uint32_t timer_period_usec)
{
    int xStatus = XTmrCtr_Initialize(&timer, TMR_DEVICE_ID);
    if (xStatus != XST_SUCCESS) {
        fatalError("Could not initialize timer");
    }
    XTmrCtr_SetHandler(&timer, timer_isr, (void *) 0x12345678);

    // Initialize the interrupt controller driver so that it is ready to use.
    XScuGic_Config *IntcConfig = XScuGic_LookupConfig(XPAR_PS7_SCUGIC_0_DEVICE_ID);
    if (NULL == IntcConfig) {
        fatalError("XScuGic_LookupConfig() failed");
    }

    int Status = XScuGic_CfgInitialize(&intCtrl, IntcConfig, IntcConfig->CpuBaseAddress);
    if (Status != XST_SUCCESS) {
        fatalError("XScuGic_CfgInitialize() failed");
    }

    XScuGic_SetPriorityTriggerType(&intCtrl, INTC_TMR_INTERRUPT_ID, 0xA0, 0x3);
    Status = XScuGic_Connect(&intCtrl, INTC_TMR_INTERRUPT_ID, (Xil_InterruptHandler) XTmrCtr_InterruptHandler, &timer);
    if (Status != XST_SUCCESS) {
        fatalError("XScuGic_Connect() failed");
    }
    XScuGic_Enable(&intCtrl, INTC_TMR_INTERRUPT_ID);

    // Initialize the exception table.
    Xil_ExceptionInit();

    // Register the interrupt controller handler with the exception table.
    Xil_ExceptionRegisterHandler(
        XIL_EXCEPTION_ID_INT, (Xil_ExceptionHandler) XScuGic_InterruptHandler, (void *) &intCtrl);

    // Enable exceptions.
    Xil_ExceptionEnable();

    XTmrCtr_SetOptions(&timer, 0, XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION);
    XTmrCtr_SetResetValue(&timer, 0, TIMER_LOAD_VALUE(timer_period_usec));
    XTmrCtr_Start(&timer, 0);
}


<|start_filename|>sdk/bare/common/drv/ild1420.h<|end_filename|>
#ifndef ILD1420_H
#define ILD1420_H

#include <stdint.h>

typedef enum {
    // Keep starting at 0
    ILD1420_SENSOR1 = 0,
    ILD1420_SENSOR2 = 1,
    ILD1420_SENSOR3 = 2,
    ILD1420_SENSOR4 = 3,

    // Keep at end
    ILD1420_NUM_SENSORS
} ild1420_sensor_t;

typedef struct ild1420_packet_t {
    // 16-bit raw data from the sensor
    uint16_t distance;

    // 2-bit raw error code from sensor
    uint8_t error;

    // 1 if data was received in the last 300us, 0 if stale
    uint8_t fresh;
} ild1420_packet_t;

// User calls this function to receive the latest
// packet from the ILD1420 sensor. The FPGA is constantly
// listening for data and parsing it. The last valid
// packet is returned, which may be OLD! Use the "fresh"
// bit to determine recency.
//
// NOTE: the C code does NOT have to initialize this driver,
// because the FPGA takes care of aligning its acquisitions
// to the UART data stream.
ild1420_packet_t ild1420_get_latest_packet(ild1420_sensor_t sensor);

#endif // ILD1420_H


<|start_filename|>sdk/bare/user/usr/user_config.h<|end_filename|>
#ifndef USER_CONFIG_H
#define USER_CONFIG_H

#include "drv/hardware_targets.h"

// This file is used to override system defines and conditionally enable
// various system-level firmware features.

// Specify hardware revision (i.e. REV C, REV D, etc)
#define USER_CONFIG_HARDWARE_TARGET (AMDC_REV_C)

// Override the default scheduler elementary frequency by defining SYS_TICK_FREQ here.
// System uses 10kHz by default
//#define SYS_TICK_FREQ (20000) // Hz

// Enforce time quantum limits
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_TIME_QUANTUM_CHECKING (1)

// Enable task statistic collection by default
// NOTE: The user can still go and enable the stats themselves if this is set to 0!
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_TASK_STATISTICS_BY_DEFAULT (0)

// Enable the watchdog timer
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_WATCHDOG (0)

// Enable logging functionality
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_LOGGING (0)

// Override default number of available logging variables
// when defined, this takes precedence over system default of 32
//#define USER_CONFIG_LOGGING_MAX_NUM_VARIABLES (150)

// Override default logging sample depth per variable
// when defined, this takes precedence over system default of 100k
//#define USER_CONFIG_LOGGING_SAMPLE_DEPTH_PER_VARIABLE (50000)

// Enable injection functionality
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_INJECTION (0)

// Enable SensorCard platform motherboard support
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT (0)

// Enable SensorCard platform motherboard auto request new ADC sample data
// set to 1 for enabled, 0 for disabled
#define USER_CONFIG_ENABLE_MOTHERBOARD_AUTO_TX (0)

#endif // USER_CONFIG_H


<|start_filename|>sdk/bare/common/sys/statistics.c<|end_filename|>
/**
 * This code uses the standard deviation and rolling statistics code
 * described at https://www.johndcook.com/blog/standard_deviation/
 * and in Knuth TAOCP vol 2, 3rd edition, page 232
 */
#include "sys/statistics.h"
#include <math.h>

void statistics_init(statistics_t *stats)
{
    statistics_clear(stats);
}

void statistics_clear(statistics_t *stats)
{
    stats->min = 0;
    stats->max = 0;
    stats->mean = 0;
    stats->__old_mean = 0;
    stats->__s = 0;
    stats->__old_s = 0;

    stats->num_samples = 0;
    stats->value = 0;
}

void statistics_push(statistics_t *stats, double val)
{
    // TODO: should return an error
    if (isnan(val) || isinf(val)) {
        return;
    }

    // See Knuth TAOCP vol 2, 3rd edition, page 232
    stats->num_samples++;
    stats->value = val;

    if (stats->num_samples == 1) {
        stats->min = val;
        stats->max = val;
        stats->__s = 0;
        stats->__old_s = 0;
        stats->mean = val;
        stats->__old_mean = val;
    } else {
        stats->mean = stats->__old_mean + (val - stats->__old_mean) / stats->num_samples;
        stats->__s = stats->__old_s + (val - stats->__old_mean) * (val - stats->mean);

        stats->__old_mean = stats->mean;
        stats->__old_s = stats->__s;

        if (val > stats->max) {
            stats->max = val;
        }

        if (val < stats->min) {
            stats->min = val;
        }
    }
}

double statistics_variance(statistics_t *stats)
{
    if (stats->num_samples > 1) {
        return stats->__s / (stats->num_samples);
    } else {
        return 0;
    }
}


<|start_filename|>sdk/bare/user/usr/beta_labs/inverter.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/inverter.h"
#include "drv/io.h"
#include "drv/pwm.h"
#include <math.h>

static double dtc_dcomp = 0.0;
static double dtc_currLimit = 0.0;

// Don't init to 0 since we divide by this! (User should override this value)
static double inverter_Vdc = 1.0;

inline static int saturate(double min, double max, double *value)
{
    if (*value < min) {
        // Lower bound saturation
        *value = min;
        return -1;
    } else if (*value > max) {
        // Upper bound saturation
        *value = max;
        return 1;
    } else {
        // No saturation
        return 0;
    }
}

void inverter_saturate_to_Vdc(double *voltage)
{
    io_led_color_t color = { 0, 0, 0 };
    if (saturate(-inverter_Vdc, inverter_Vdc, voltage) != 0)
        color.g = 255;
    io_led_set_c(0, 1, 0, &color);
}

void inverter_set_voltage(uint8_t pwm_idx, double voltage, double current)
{
    double duty = 0.5 + (voltage / inverter_Vdc);

    // Calculate duty compensation
    double dcomp = 0.0;

    if (dtc_dcomp != 0.0 && dtc_currLimit != 0.0) {
        if (current < -dtc_currLimit) {
            dcomp = -dtc_dcomp;
        } else if (current > dtc_currLimit) {
            dcomp = dtc_dcomp;
        } else {
            dcomp = (dtc_dcomp / dtc_currLimit) * current;
        }
    }

    pwm_set_duty(pwm_idx, duty + dcomp);
}

void inverter_set_dtc(double dcomp, double currLimit)
{
    dtc_dcomp = dcomp;
    dtc_currLimit = currLimit;
}

void inverter_set_Vdc(double Vdc)
{
    inverter_Vdc = Vdc;
}

double inverter_get_Vdc(void)
{
    return inverter_Vdc;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/gpio_mux.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#ifndef GPIO_MUX_H
#define GPIO_MUX_H

#include <stdbool.h>
#include <stdint.h>

typedef enum {
    GPIO_MUX_UNUSED = 0,
    GPIO_MUX_DEVICE1 = 1,
    GPIO_MUX_DEVICE2 = 2,
    GPIO_MUX_DEVICE3 = 3,
    GPIO_MUX_DEVICE4 = 4
} gpio_mux_device_t;

#define GPIO_PORT1 GPIO_MUX_UNUSED
#define GPIO_PORT2 GPIO_MUX_UNUSED

void gpio_mux_init(void);

void gpio_mux_set_device(uint8_t, gpio_mux_device_t);

#endif // GPIO_MUX_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/basic/src/lwip_glue.c<|end_filename|>
/*
 * lwip_glue.c
 *
 *  Created on: Jun 13, 2014
 *      Author: sever212
 */
#ifdef __arm__
#if 0
#include "xparameters.h"
#include "xparameters_ps.h"	/* defines XPAR values */
#include "xil_cache.h"
#include "xscugic.h"
#include "xscutimer.h"
#include "lwip/tcp.h"
#include "lwip/tcp_impl.h"
#include "xil_printf.h"
#include "platform_config.h"
#include "netif/xadapter.h"
#endif
#include "project_include.h"

#define RESET_RX_CNTR_LIMIT	400

static int ResetRxCntr = 0;
volatile char bEnableEthTimer = FALSE;
static struct netif server_netif;
struct netif *echo_netif;

char bEth250ms = FALSE;

typedef struct lwip_comm_glue {
	char szRcvCmdEth[4096];
	void *socket;
	u32 CmdHndlStateEth;
} lwip_comm_glue;


lwip_comm_glue EthComm[MAX_BASE_SOCKETS];

/*****************************
 * init_tcpip_comm_data()
 * Set up the communication data structures.
 * Call on power up.
 */
static void init_tcpip_comm_data()
{
	int i;
	for (i = 0; i < MAX_BASE_SOCKETS; i++)
	{
		EthComm[i].socket = 0;
		EthComm[i].CmdHndlStateEth = 0;
	}
}

/*****************************
 * get_tcpip_comm_data()
 * Get a pointer to the receive data and the receive state for
 * this socket.
 *
 * socket: unique pointer to the socket (doesn't need to point
 * 	to the socket, just needs to be unique to the socket)
 * data: returned pointer to the data
 * state: returned pointer to the state
 *
 * Returns a status code: NOT_FOUND or SUCCESS
 */
static int get_tcpip_comm_data(void *socket, char **data, u32 **state)
{
	int retVal = NOT_FOUND;
	int i;
	int protect = EnterProtection();
	for (i = 0; i < MAX_BASE_SOCKETS; i++)
	{
		if (EthComm[i].socket == socket)
		{
			*data = EthComm[i].szRcvCmdEth;
			*state = &EthComm[i].CmdHndlStateEth;
			retVal = SUCCESS;
			break;
		}
	}
	LeaveProtection(protect);
	return retVal;
}

/*****************************
 * put_tcpip_comm_data()
 * Call this upon the connection of a new socket to setup a data
 * structure for it.
 *
 * socket: unique pointer to the socket (doesn't need to point
 * 	to the socket, just needs to be unique to the socket)
 *
 * Returns a status code: OVERFLOW (if no space) or SUCCESS
 */
static int put_tcpip_comm_data(void *socket)
{
	int retVal = OVERFLOW;
	int i;
	int protect = EnterProtection();
	for (i = 0; i < MAX_BASE_SOCKETS; i++)
	{
		if ((EthComm[i].socket == 0) || (EthComm[i].socket == socket))
		{
			EthComm[i].socket = socket;
			EthComm[i].CmdHndlStateEth = 0;
			retVal = SUCCESS;
			break;
		}
	}
	LeaveProtection(protect);
	return retVal;
}

/*****************************
 * remove_tcpip_comm_data()
 * Call this upon the disconnection of a socket to free up
 * the memory used for its data structure
 *
 * socket: unique pointer to the socket (doesn't need to point
 * 	to the socket, just needs to be unique to the socket)
 *
 * Returns a status code: NOT_FOUND or SUCCESS
 */
static int remove_tcpip_comm_data(void *socket)
{
	int retVal = NOT_FOUND;
	int i;
	int protect = EnterProtection();
	for (i = 0; i < MAX_BASE_SOCKETS; i++)
	{
		if (EthComm[i].socket == socket)
		{
			EthComm[i].socket = 0;
			EthComm[i].CmdHndlStateEth = 0;
			retVal = SUCCESS;
			break;
		}
	}
	LeaveProtection(protect);
	return retVal;
}



void lwip_timer_callback()
{
	/* we need to call tcp_fasttmr & tcp_slowtmr at intervals specified
	 * by lwIP. It is not important that the timing is absoluetly accurate.
	 */
	static int odd = 1;

	odd = !odd;
#ifndef USE_SOFTETH_ON_ZYNQ
	ResetRxCntr++;
#endif
	tcp_fasttmr();
	if (odd) {
		tcp_slowtmr();
	}

	/* For providing an SW alternative for the SI #692601. Under heavy
	 * Rx traffic if at some point the Rx path becomes unresponsive, the
	 * following API call will ensures a SW reset of the Rx path. The
	 * API xemacpsif_resetrx_on_no_rxdata is called every 100 milliseconds.
	 * This ensures that if the above HW bug is hit, in the worst case,
	 * the Rx path cannot become unresponsive for more than 100
	 * milliseconds.
	 */
#ifndef USE_SOFTETH_ON_ZYNQ
	if (ResetRxCntr >= RESET_RX_CNTR_LIMIT) {
		xemacpsif_resetrx_on_no_rxdata(echo_netif);
		ResetRxCntr = 0;
	}
#endif
}


void
print_ip(char *msg, struct ip_addr *ip)
{
	print(msg);
	xil_printf("%d.%d.%d.%d\n\r", ip4_addr1(ip), ip4_addr2(ip),
			ip4_addr3(ip), ip4_addr4(ip));
}

void
print_ip_settings(struct ip_addr *ip, struct ip_addr *mask, struct ip_addr *gw)
{

	print_ip("Board IP: ", ip);
	print_ip("Netmask : ", mask);
	print_ip("Gateway : ", gw);
}


int SetupLWIP()
{
	struct ip_addr ipaddr, netmask, gw;

	/* the mac address of the board. this should be unique per board */
		unsigned char mac_ethernet_address[] =
		{ 0x00, 0x0a, 0x35, 0x00, 0x01, 0x02 };

		echo_netif = &server_netif;

	/* initliaze IP addresses to be used */
		IP4_ADDR(&ipaddr,  192, 168,   1, 10);
		IP4_ADDR(&netmask, 255, 255, 255,  0);
		IP4_ADDR(&gw,      192, 168,   1,  1);

		print_ip_settings(&ipaddr, &netmask, &gw);

		lwip_init();

	/* Add network interface to the netif_list, and set it as default */
		if (!xemac_add(echo_netif, &ipaddr, &netmask,
							&gw, mac_ethernet_address,
							PLATFORM_EMAC_BASEADDR)) {
			xil_printf("Error adding N/W interface\n\r");
			return -1;
		}
		netif_set_default(echo_netif);

		bEnableEthTimer = TRUE;

		/* specify that the network if is up */
		netif_set_up(echo_netif);

		/*Setup the application's comm data memory*/
		init_tcpip_comm_data();
	return XST_SUCCESS;
}

//for now lump all ethernet commands together:

void tcpip_disconnect_callback(void *arg, err_t err)
{
	remove_tcpip_comm_data(arg);
	//if we are streaming out this port, stop the streaming.
	if (StreamFIFOComm == arg)
		StreamFIFOComm = 0;
}

err_t recv_callback(void *arg, struct tcp_pcb *tpcb,
                               struct pbuf *p, err_t err)
{
	char szOutDataEth[8192];
	int status;
	char *szRcvCmdEth;
	u32 *CmdHndlStateEth;

	status = get_tcpip_comm_data(arg, &szRcvCmdEth, &CmdHndlStateEth);
	/* do not read the packet if we are not in ESTABLISHED state */
	/* Also, abort connection if unable to find the comm data*/
	if ((!p) || (status != SUCCESS)) {
		tcp_close(tpcb);
		tcp_recv(tpcb, NULL);
		remove_tcpip_comm_data(tpcb);
		return ERR_OK;
	}
	u32 outLen = HandleCommandsGeneral(szOutDataEth, szRcvCmdEth, p->payload, p->len, CmdHndlStateEth, arg);
	/* indicate that the packet has been received */
	tcp_recved(tpcb, p->len);


	/* echo back the payload */
	/* in this case, we assume that the payload is < TCP_SND_BUF */
	if (outLen > 0)
	{
		if (tcp_sndbuf(tpcb) > outLen) { //(tcp_sndbuf(tpcb) > p->len) {
			err = tcp_write(tpcb, szOutDataEth, outLen, 1);
		} else
			xil_printf("no space in tcp_sndbuf\n\r");
	}
	/* free the received pbuf */
	pbuf_free(p);

	return ERR_OK;
}

err_t sent_callback(void *arg, struct tcp_pcb *pcb, unsigned short len)
{
	//if streaming data, send data
	if (StreamFIFOComm == pcb)
	{
		if (ReloadStreamFIFO(pcb) > 0)
			bPrimeStreamFIFO = FALSE;
		else
			bPrimeStreamFIFO = TRUE;
	}
	return ERR_OK;
}

err_t accept_callback(void *arg, struct tcp_pcb *newpcb, err_t err)
{
	static int connection = 1;

	put_tcpip_comm_data(newpcb);

	/* set the receive callback for this connection */
	tcp_recv(newpcb, recv_callback);

	/* just use an integer number indicating the connection id as the
	   callback argument */
	tcp_arg(newpcb, /*(void*)connection*/ newpcb);

	//eric added:
	tcp_sent(newpcb, sent_callback);

	tcp_err(newpcb, tcpip_disconnect_callback);
	/* increment for subsequent accepted connections */
	connection++;

	return ERR_OK;
}


int start_tcpip()
{
	struct tcp_pcb *pcb;
	err_t err;
	unsigned port = 7;

	/* create new TCP PCB structure */
	pcb = tcp_new();
	if (!pcb) {
		xil_printf("Error creating PCB. Out of Memory\n\r");
		return -1;
	}

	pcb->so_options |= SOF_KEEPALIVE; //ELS added to try to get sockets to detect a disconnect

	/* bind to specified @port */
	err = tcp_bind(pcb, IP_ADDR_ANY, port);
	if (err != ERR_OK) {
		xil_printf("Unable to bind to port %d: err = %d\n\r", port, err);
		return -2;
	}

	/* we do not need any arguments to callback functions */
	tcp_arg(pcb, NULL);

	/* listen for connections */
	pcb = tcp_listen(pcb);
	if (!pcb) {
		xil_printf("Out of memory while tcp_listen\n\r");
		return -3;
	}

	/* specify callback to use for incoming connections */
	tcp_accept(pcb, accept_callback);

	xil_printf("TCP echo server started @ port %d\n\r", port);

	return XST_SUCCESS;
}


#endif



<|start_filename|>sdk/basic/src/Log.c<|end_filename|>
/*
 * Log.c
 *
 *  Created on: Jun 6, 2014
 *      Author: sever212
 */


#include "xparameters.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include <stdio.h>
#include <stdlib.h>
#include "commands.h"
#include "main.h"
#include "project_settings.h"
#include "Log.h"




//log_entry TheLog[MAX_LOG_VARS];
log_entry TheLog[MAX_LOG_VARS];


static inline int ProtectLog(u32 LogIndex)
{
	return 1; //Protection not needed since we aren't allowing overruns EnterProtection();
}

static inline int UnprotectLog(u32 LogIndex, int State)
{
	return 1; //protection not needed since we aren't allowing overruns LeaveProtection(State);
}
/*****************************
 * InitLog()
 * Call this to initialize the log.
 *
 */
int InitLog()
{
	int i;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		int protect = ProtectLog(i);
		TheLog[i].InPos = 0;
		TheLog[i].OutPos = 0;
		TheLog[i].Settings = 0;
		TheLog[i].Flags = 0;
		TheLog[i].VarToLog = 0;
		TheLog[i].LogStatus = L_IDLE;
		UnprotectLog(i, protect);
	}

	return SUCCESS;
}

/*****************************
 * ReadFromLog()
 * Call this to read data from the log
 *
 * output: where the data is copied to
 * LogIndex: which log to read from (0 to MAX_LOG_VARS-1)
 * Index: zero-based index in the log to start reading from
 * Length: The maximum length that can be written to output (in bytes)
 * ---NOTE: since everything is 32 bit aligned, length must be
 * 	divisible by 4
 *
 * Returns number of bytes read (if positive)
 * Error code (if negative)
 */
int ReadFromLog(unsigned char *output, u32 LogIndex, u32 Length)
{
	u32 avail, in, out;
	int protect;
	if (LogIndex >= MAX_LOG_VARS)
		return INVALID_ARGUMENT;
	if ( (TheLog[LogIndex].LogStatus != L_DONE) && (TheLog[LogIndex].LogStatus != L_LOGGING) )
		return INVALID_OPERATION;
	if (Length % 4 != 0)
		return INVALID_ARGUMENT; //must by 32bit aligned

	protect = ProtectLog(LogIndex);
	//set up internal variables and adjust length
	avail = LogDataAvail(LogIndex); //in u32 (for bytes multiply by 4)
	in = TheLog[LogIndex].InPos;
	out = TheLog[LogIndex].OutPos;
	if (Length > avail*4)
		Length = avail*4;

	//Okay, now read it out
	if (out + Length/4 < LOG_LENGTH)
	{
		memcpy(output, &TheLog[LogIndex].LogEntry[out], Length);
		TheLog[LogIndex].OutPos += Length/4;
	}
	else if (out > in)
	{
		u32 lenTop = 4*(LOG_LENGTH - out); //number of bytes
		u32 lenBot = Length - lenTop; //number of bytes
		memcpy(output, &TheLog[LogIndex].LogEntry[out], lenTop);
		memcpy(&output[lenTop], TheLog[LogIndex].LogEntry, lenBot);
		TheLog[LogIndex].OutPos = lenBot/4;
	}

	UnprotectLog(LogIndex, protect);
	return Length;
}

/*****************************
 * DetermineMostFullLog()
 * Call this to determine which log has the most data and find out how many
 * bytes can be read.
 *
 * LogIndex: set to the log that has the most data (0 to MAX_LOG_VARS-1)
 *
 * Returns number of bytes available (if positive)
 * Error code (if negative)
 */
int DetermineMostFullLog(u32 *LogIndex)
{
	int i;
	u32 max_avail = 0, cur_avail = 0;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		cur_avail = LogDataAvail(i);
		if (cur_avail > max_avail)
		{
			max_avail = cur_avail;
			*LogIndex = i;
		}
	}
	return max_avail;
}

/*****************************
 * ReadFromMostFullLog()
 * Call this to read data from the log that has the most data ready
 * to be read out.
 *
 * output: where the data is copied to
 * LogIndex: set to the log that was read from (0 to MAX_LOG_VARS-1)
 * Length: The maximum length that can be written to output (in bytes)
 * ---NOTE: since everything is 32 bit aligned, length must be
 * 	divisible by 4
 *
 * Returns number of bytes read (if positive)
 * Error code (if negative)
 */
int ReadFromMostFullLog(unsigned char *output, u32 *LogIndex, u32 Length)
{
	int i;
	u32 max_entry = 0, max_avail = 0, cur_avail = 0;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		cur_avail = LogDataAvail(i);
		if (cur_avail > max_avail)
		{
			max_avail = cur_avail;
			max_entry = i;
			*LogIndex = i;
		}
	}
	if (max_avail > 0)
		return ReadFromLog(output, max_entry, Length);
	else
		return 0;
}

/*****************************
 * ReadLogFlags()
 * Call this to read a log's flags. Doing so
 * clears the flags
 *
 * *flags: pointer to where the flags should be written to
 * LogIndex: which log to read from (0 to MAX_LOG_VARS-1)
 *
 * Returns number of bytes read (if positive)
 * Error code (if negative)
 */
int ReadLogFlags(u32 *flags, u32 LogIndex)
{

	if (LogIndex >= MAX_LOG_VARS)
		return INVALID_ARGUMENT;
	*flags = TheLog[LogIndex].Flags;
	TheLog[LogIndex].Flags = 0;
	return SUCCESS;
}

/*****************************
 * LogDataAvail()
 * Determine how many u32 values can be read from the log
 * (the number of bytes is 4 * this)
 *
 * LogIndex: Which log the caller is inquiring about
 * Returns: number of u32's available
 */
u32 inline LogDataAvail(u32 LogIndex)
{
	u32 in, out;
	int protect;

	if (LogIndex >= MAX_LOG_VARS)
			return 0;
	protect = ProtectLog(LogIndex);
	in = TheLog[LogIndex].InPos;
	out = TheLog[LogIndex].OutPos;
	UnprotectLog(LogIndex, protect);
	if (in >= out)
		return in - out;
	else
		return LOG_LENGTH - (out - in);
}

/*****************************
 * LogInputSpaceAvail()
 * Determine how many u32 values can be stored in the log
 * (the number of bytes is 4 * this)
 *
 * LogIndex: Which log the caller is inquiring about
 * Returns: number of u32's available
 */
static inline u32 LogInputSpaceAvail(u32 LogIndex)
{
	u32 in, out;
	if (LogIndex >= MAX_LOG_VARS)
			return 0;
	in = TheLog[LogIndex].InPos;
	out = TheLog[LogIndex].OutPos;

	if (out > in)
		return (out - in) - 1;
	else
		return (LOG_LENGTH - (in - out)) - 1;
}

/*****************************
 * SetupLogVariable()
 * Call this prepare a variable for logging
 *
 * LogIndex: Which index in the log to use (0 to MAX_LOG_VARS-1)
 * Variable: Pointer to the variable to log
 * Returns: Success code
 */
int SetupLogVariable(u32 LogIndex, u32 *Variable, u32 Settings)
{
	if (LogIndex >= MAX_LOG_VARS)
		return INVALID_ARGUMENT;

	else
	{
		TheLog[LogIndex].InPos = 0;
		TheLog[LogIndex].OutPos = 0;
		TheLog[LogIndex].Settings = Settings;
		TheLog[LogIndex].Flags = 0;
		TheLog[LogIndex].VarToLog = Variable;
		TheLog[LogIndex].LogStatus = L_READY;
	}
	return SUCCESS;
}

/*****************************
 * StartLogging()
 * Call this to start the data logging
 *
 * Returns: Success code
 */
int StartLogging()
{
	int i;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		if (TheLog[i].LogStatus == L_READY)
			TheLog[i].LogStatus = L_LOGGING;
	}

	return SUCCESS;
}

/*****************************
 * StopAllLogging()
 * Call this to stop all data logging. Anything
 * in progress will be marked as done.
 *
 * Returns: Success code
 */
int StopAllLogging()
{
	int i;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		if (TheLog[i].LogStatus == L_LOGGING)
			TheLog[i].LogStatus = L_DONE;
	}

	return SUCCESS;
}

/*****************************
 * DoLogging()
 * Call this at fixed intervals to handle logging of variables (move to DMA??)
 *
 * Returns: Success code
 */
int DoLogging()
{
	int i;
	for (i = 0; i < MAX_LOG_VARS; i++)
	{
		if (TheLog[i].LogStatus == L_LOGGING)
		{
			//basic case: we have space:
			if ( LogInputSpaceAvail(i) )
				TheLog[i].LogEntry[TheLog[i].InPos++] = *TheLog[i].VarToLog;
			else if (TheLog[i].Settings & FIFO_RUN_ONCE) //just run once
				TheLog[i].LogStatus = L_DONE;
			else if ( !(TheLog[i].Settings & FIFO_PREVENT_OVERRUN) ) //just write over
			{
				//if we don't write over, than there is no need to protect the fifo!
				TheLog[i].Flags |= FIFO_OVERRUN_OCCURED;
				/*
				TheLog[i].OutPos++;
				TheLog[i].OutPos %= LOG_LENGTH;
				TheLog[i].LogEntry[TheLog[i].InPos++] = *TheLog[i].VarToLog;*/
			}

			/*if (TheLog[i].Flags != 0)
				TheLog[i].Flags = TheLog[i].Flags;
			if (TheLog[i].InPos >= LOG_LENGTH)
				TheLog[i].InPos = TheLog[i].InPos;*/

			//adjust indices:
			TheLog[i].InPos %= LOG_LENGTH;
		}
	}

	return SUCCESS;
}


<|start_filename|>sdk/bare/common/sys/defines.h<|end_filename|>
#ifndef DEFINES_H
#define DEFINES_H

#include <stdio.h>

#define UNUSED(x) (void) (x)

#define HANG                                                                                                           \
    printf("HANG!!!\n");                                                                                               \
    while (1)

#define SUCCESS (0)
#define FAILURE (1)

#define PI   (3.141592653589793238463) // pi
#define PI23 (2.094395102393195492308) // 2*pi/3
#define PI2  (6.283185307179586476925) // 2*pi

#define SQRT23 (0.816496580927726032732) // sqrt(2/3)
#define SQRT2  (1.414213562373095048802) // sqrt(2)
#define SQRT3  (1.732050807568877293528) // sqrt(3)

#define DEG_TO_RAD(deg)         (deg * PI / 180.0)
#define RAD_TO_DEG(rad)         (rad * 180.0 / PI)
#define RPM_TO_RAD_PER_SEC(rpm) (rpm * PI2 / 60.0)
#define RAD_PER_SEC_TO_RPM(rps) (rps * 60.0 / PI2)

#endif // DEFINES_H


<|start_filename|>sdk/basic/bsp/gpio.c<|end_filename|>
#include "../bsp/gpio.h"
#include <stdio.h>

void gpio_init(void)
{
	printf("GPIO:\tInitializing...\n");
}

void gpio_pin_read(uint8_t port, uint8_t pin, uint8_t *value)
{

}

void gpio_pin_write(uint8_t port, uint8_t pin, uint8_t value)
{

}

void gpio_port_read(uint8_t port, uint16_t *value)
{

}

void gpio_port_write(uint8_t port, uint16_t value)
{

}

void gpio_set_dir(uint8_t group, uint8_t dir)
{

}


<|start_filename|>sdk/basic/bsp/gpio.h<|end_filename|>
#ifndef GPIO_H
#define GPIO_H

#include <stdint.h>

void gpio_init(void);

void gpio_pin_read(uint8_t port, uint8_t pin, uint8_t *value);
void gpio_pin_write(uint8_t port, uint8_t pin, uint8_t value);

void gpio_port_read(uint8_t port, uint16_t *value);
void gpio_port_write(uint8_t port, uint16_t value);

void gpio_set_dir(uint8_t group, uint8_t dir);

#endif // GPIO_H


<|start_filename|>sdk/bare/user/usr/beta_labs/app_beta_labs.h<|end_filename|>
#ifndef APP_BETA_LABS_H
#define APP_BETA_LABS_H

void app_beta_labs_init(void);

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_mc.h<|end_filename|>
#ifndef CMD_MC_H
#define CMD_MC_H

void cmd_mc_register(void);

int cmd_mc(int argc, char **argv);

#endif // CMD_MC_H


<|start_filename|>sdk/bare/common/sys/log.h<|end_filename|>
#ifndef LOG_H
#define LOG_H

#include "sys/scheduler.h"
#include "usr/user_config.h"
#include <stdbool.h>
#include <stdint.h>

#ifdef USER_CONFIG_LOGGING_MAX_NUM_VARIABLES
#define LOG_MAX_NUM_VARIABLES (USER_CONFIG_LOGGING_MAX_NUM_VARIABLES)
#else
#define LOG_MAX_NUM_VARIABLES (32)
#endif

#ifdef USER_CONFIG_LOGGING_SAMPLE_DEPTH_PER_VARIABLE
#define LOG_SAMPLE_DEPTH_PER_VARIABLE (USER_CONFIG_LOGGING_SAMPLE_DEPTH_PER_VARIABLE)
#else
#define LOG_SAMPLE_DEPTH_PER_VARIABLE (100000)
#endif

// Check the memory usage of the logging buffers... enforce that the total number
// of variable entries is reasonably small, i.e. expected to fit in memory.
//
// The PicoZed 7030 device has 1GB of external memory. When compiling with 100M
// logging slot sample locations, the resulting binary consumes 760MB of RAM.
#if (LOG_SAMPLE_DEPTH_PER_VARIABLE * LOG_MAX_NUM_VARIABLES) > (100 * 1000 * 1000)
#error "Logging memory usage too high! Reduce sample depth or max number of variables."
#endif

// Limit the max number of logging slots. If there are too many possible variables,
// the firmware can't log them all in one time quantum!
//
// This number was found experimentally when no control algorithms were running.
// There is some head room in the limit.
//
// Interestingly, the max number of variables doesn't depend on the scheduler
// time quantum, like you'd think... I think this is because in the logging task
// callback, it iterates over the whole log (all slots)... Since they are stored
// in external memory and the stride of access is VERY large, each memory access
// results in a cache miss in the SoC, so we are seeing ~100s of memory access
// latencies stack up (from CPU core all the way out to external RAM device),
// thus overruns the time slice!
#if LOG_MAX_NUM_VARIABLES > 150
#error "Max number of logging variables too large to meet timing! Please reduce."
#endif

#define LOG_UPDATES_PER_SEC (SYS_TICK_FREQ)
#define LOG_INTERVAL_USEC   (USEC_IN_SEC / LOG_UPDATES_PER_SEC)

typedef enum var_type_e {
    LOG_INT = 1,
    LOG_FLOAT,
    LOG_DOUBLE,
} var_type_e;

void log_init(void);
void log_callback(void *arg);

void log_start(void);
void log_stop(void);
bool log_is_logging(void);

int log_var_register(int idx, char *name, void *addr, uint32_t samples_per_sec, var_type_e type);

int log_var_empty(int idx);
int log_var_empty_all(void);

int log_var_dump_uart_ascii(int idx);
int log_var_dump_uart_binary(int idx);

int log_var_unregister(int idx);
int log_var_is_registered(int idx, bool *is_registered);

int log_print_info(void);

#endif // LOG_H


<|start_filename|>sdk/bare/common/usr/dac/cmd/cmd_dac.h<|end_filename|>
#ifndef CMD_DAC_H
#define CMD_DAC_H

// Called in app init function to register command with system
void cmd_dac_register(void);

// Function called when user types "dac" command into command prompt
int cmd_dac(int argc, char **argv);

#endif // CMD_DAC_H


<|start_filename|>sdk/bare/common/sys/scheduler.h<|end_filename|>
#ifndef SCHEDULER_H
#define SCHEDULER_H

#include <stdbool.h>
#include <stdint.h>

#include "sys/defines.h"
#include "sys/task_stats.h"
#include "usr/user_config.h"

// Utility defines for time conversions
#define USEC_IN_SEC       (1000000)
#define SEC_TO_USEC(sec)  (sec * USEC_IN_SEC)
#define USEC_TO_SEC(usec) (usec / USEC_IN_SEC)

// SysTick
//
// The basic time quantum is defined to be SYS_TICK_FREQ.
//
// As an example, say we set this to 10kHz. At 666.6 MHz DSP clock,
// we will have 66.6k cycles per time slice, meaning all tasks
// combined have to consume <= 66.6k cycles
#ifndef SYS_TICK_FREQ
#define SYS_TICK_FREQ (10000) // Hz
#endif
#define SYS_TICK_USEC (SEC_TO_USEC(1) / SYS_TICK_FREQ)

// Callback into application when task is run:
typedef void (*task_callback_t)(void *);

// TCB of each task
typedef struct task_control_block_t {
    int id;
    const char *name;
    bool is_registered;
    task_callback_t callback;
    void *callback_arg;
    uint32_t interval_usec;
    uint32_t last_run_usec;

    task_stats_t stats;

    struct task_control_block_t *next;
} task_control_block_t;

void scheduler_init(void);
void scheduler_run(void);

void scheduler_tcb_init(
    task_control_block_t *tcb, task_callback_t callback, void *callback_arg, const char *name, uint32_t interval_usec);
int scheduler_tcb_register(task_control_block_t *tcb);
int scheduler_tcb_unregister(task_control_block_t *tcb);
bool scheduler_tcb_is_registered(task_control_block_t *tcb);

uint32_t scheduler_get_elapsed_usec(void);

#endif // SCHEDULER_H


<|start_filename|>sdk/basic/bsp/analog.h<|end_filename|>
#ifndef ANALOG_H
#define ANALOG_H

#include <stdint.h>

typedef enum {
	ANLG_CHNL1 = 1,
	ANLG_CHNL2,
	ANLG_CHNL3,
	ANLG_CHNL4,
	ANLG_CHNL5,
	ANLG_CHNL6,
	ANLG_CHNL7,
	ANLG_CHNL8,
	ANLG_CHNL9,
	ANLG_CHNL10,
	ANLG_CHNL11,
	ANLG_CHNL12,
	ANLG_CHNL13,
	ANLG_CHNL14,
	ANLG_CHNL15,
	ANLG_CHNL16
} analog_channel_e;

typedef enum {
	ANLG_CLKDIV2 = 0,
	ANLG_CLKDIV4,
	ANLG_CLKDIV8,
	ANLG_CLKDIV16
} analog_clkdiv_e;

#define ANALOG_NUM_CHANNELS				(16)
#define ANALOG_CALIBRATE_NUM_SAMPLES	(256)

void analog_init(void);

void analog_calibrate(uint32_t num);

void analog_set_clkdiv(analog_clkdiv_e div);
void analog_get_clkdiv(analog_clkdiv_e *div);

void analog_getf(analog_channel_e channel, float *value);
void analog_geti(analog_channel_e channel, int16_t *value);

// void analog_set_filter(analog_channel_e channel, ...);

#endif // ANALOG_H


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_cc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_cc.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/task_cc.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (6)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "init", "Start current controller" },
    { "deinit", "Stop current controller" },
    { "bw <mFreq>", "Set controller bandwidth" },
    { "offset <enc_pulses>", "Set DQ frame offset" },
    { "theta_e_src <enc|est>", "Set theta_e source between encoder and estimation" },
    { "omega_e_src <enc|est>", "Set omega_e source between encoder and estimation" },
};

void cmd_cc_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "cc", "Current controller commands", cmd_help, NUM_HELP_ENTRIES, cmd_cc);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

//
// Handles the 'cc' command
// and all sub-commands
//
int cmd_cc(int argc, char **argv)
{
    // Handle 'init' sub-command
    if (strcmp("init", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure cc task was not already inited
        if (task_cc_is_inited())
            return CMD_FAILURE;

        task_cc_init();
        return CMD_SUCCESS;
    }

    // Handle 'deinit' sub-command
    if (strcmp("deinit", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure cc task was already inited
        if (!task_cc_is_inited())
            return CMD_FAILURE;

        task_cc_deinit();
        task_cc_clear();
        return CMD_SUCCESS;
    }

    // Handle 'bw' sub-command
    if (strcmp("bw", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 3)
            return CMD_INVALID_ARGUMENTS;

        // Pull out mAmp argument
        // and saturate to 1 .. 500Hz
        double mBw = (double) atoi(argv[2]);
        if (mBw < 1000.0)
            return CMD_INVALID_ARGUMENTS;
        if (mBw > 500000.0)
            return CMD_INVALID_ARGUMENTS;

        task_cc_set_bw(mBw / 1000.0);
        return CMD_SUCCESS;
    }

    // Handle 'offset' sub-command
    if (strcmp("offset", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 3)
            return CMD_INVALID_ARGUMENTS;

        // Pull out offset argument
        int32_t offset = atoi(argv[2]);

        task_cc_set_dq_offset(offset);
        return CMD_SUCCESS;
    }

    // Handle 'theta_e_src' sub-command
    if (argc == 3 && strcmp("theta_e_src", argv[1]) == 0) {
        uint8_t use_encoder = 1;
        if (strcmp("enc", argv[2]) == 0) {
            use_encoder = 1;
        } else if (strcmp("est", argv[2]) == 0) {
            use_encoder = 0;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        task_cc_set_theta_e_src(use_encoder);

        return CMD_SUCCESS;
    }

    // Handle 'omega_e_src' sub-command
    if (argc == 3 && strcmp("omega_e_src", argv[1]) == 0) {
        uint8_t use_encoder = 1;
        if (strcmp("enc", argv[2]) == 0) {
            use_encoder = 1;
        } else if (strcmp("est", argv[2]) == 0) {
            use_encoder = 0;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        task_cc_set_omega_e_src(use_encoder);

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/user/usr/beta_labs/inverter.h<|end_filename|>
#ifndef INVERTER_H
#define INVERTER_H

#include <stdint.h>

void inverter_saturate_to_Vdc(double *voltage);
void inverter_set_voltage(uint8_t pwm_idx, double voltage, double current);

void inverter_set_dtc(double dcomp, double currLimit);
void inverter_set_Vdc(double Vdc);
double inverter_get_Vdc(void);

#endif // INVERTER_H


<|start_filename|>sdk/bare/user/usr/beta_labs/task_mc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/task_mc.h"
#include "drv/dac.h"
#include "drv/encoder.h"
#include "drv/io.h"
#include "machine.h"
#include "sys/injection.h"
#include "sys/scheduler.h"
#include "usr/beta_labs/bemfo.h"
#include "usr/beta_labs/mc.h"
#include "usr/beta_labs/mcff.h"
#include "usr/beta_labs/msf.h"
#include "usr/beta_labs/task_cc.h"
#include "usr/beta_labs/task_mo.h"

#define Ts (1.0 / TASK_MC_UPDATES_PER_SEC)

// Current limits which mc can command to cc
#define MIN_CC_CURRENT (-I_rated_dq) // Amps, in DQ frame
#define MAX_CC_CURRENT (+I_rated_dq) // Amps, in DQ frame

// Forward declarations
inline static int saturate(double min, double max, double *value);

// Injection contexts for motion controller
inj_ctx_t task_mc_inj_omega_m_star;
inj_ctx_t task_mc_inj_Td_star;

// Command for controller
static double omega_m_star = 0.0;
static uint8_t mcff_enabled = 0;
static uint8_t omega_m_src_use_encoder = 1;

static task_control_block_t tcb;

// Logging variables
double LOG_omega_m_star = 0.0;
double LOG_omega_m = 0.0;
double LOG_T_sfb = 0.0;
double LOG_T_cff = 0.0;
double LOG_T_d = 0.0;
double LOG_msf_w_m = 0.0;
double LOG_msf_w_m_dot = 0.0;

uint8_t task_mc_is_inited(void)
{
    return scheduler_tcb_is_registered(&tcb);
}

void task_mc_init(void)
{
    // Register scheduler task
    scheduler_tcb_init(&tcb, task_mc_callback, NULL, "mc", TASK_MC_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);

    // Initialize injection contexts
    injection_ctx_init(&task_mc_inj_omega_m_star, "omega_m*");
    injection_ctx_init(&task_mc_inj_Td_star, "Td*");

    // Register injection contexts
    injection_ctx_register(&task_mc_inj_omega_m_star);
    injection_ctx_register(&task_mc_inj_Td_star);

    // Initialize motion controller
    mc_init();

    // Clear commanded speed
    omega_m_star = 0.0;

    // Initialize motion state filter
    msf_init();
}

void task_mc_deinit(void)
{
    // Unregister scheduler task
    scheduler_tcb_unregister(&tcb);

    // Clear current command
    task_cc_set_Iq_star(0.0);

    // Unregister injection context
    injection_ctx_unregister(&task_mc_inj_omega_m_star);
    injection_ctx_unregister(&task_mc_inj_Td_star);

    // Clear injection context
    injection_ctx_clear(&task_mc_inj_omega_m_star);
    injection_ctx_clear(&task_mc_inj_Td_star);

    // Initialize motion controller
    mc_init();

    // Clear commanded speed
    omega_m_star = 0.0;
}

void task_mc_callback(void *arg)
{
    // Inject signal into omega_m*
    injection_inj(&omega_m_star, &task_mc_inj_omega_m_star, Ts);

    // Get speed from encoder
    double omega_m = 0.0;
    if (omega_m_src_use_encoder) {
        omega_m = task_mo_get_omega_m();
    } else {
        omega_m = bemfo_get_omega_m_hat();
    }

    double delta_theta_m = omega_m * Ts;

    // Motion State Filter
    double delta_theta_m_star = msf_update(omega_m_star);

    // Motion Command Feed-Forward
    mcff_update(msf_get_omega_m(), msf_get_omega_m_dot());
    double Tem_cff = 0.0;
    if (mcff_enabled) {
        Tem_cff = mcff_get_Te_cff_total();
    }

    // Motion Controller
    double Tem_SFB_star = mc_update(delta_theta_m_star, delta_theta_m, Ts);

    // Torque Disturbance
    // Inject user defined signal into Td*
    double Td_star = 0.0;
    injection_inj(&Td_star, &task_mc_inj_Td_star, Ts);

    // Sum up Tem*
    double Tem_star_total = Tem_SFB_star + Tem_cff + Td_star;

    // Convert to commanded current, Iq*
    double Iq_star = Tem_star_total / Kt_HAT;

    // Saturate Iq_star to limit
    io_led_color_t color = { 0, 0, 0 };
    if (saturate(-I_rated_dq, +I_rated_dq, &Iq_star) != 0)
        color.r = 255;
    io_led_set_c(1, 0, 0, &color);

    // Set value in current controller
    task_cc_set_Iq_star(Iq_star);

    // Update logging variables
    LOG_omega_m_star = omega_m_star;
    LOG_omega_m = omega_m;
    LOG_T_sfb = Tem_SFB_star;
    LOG_T_cff = Tem_cff;
    LOG_T_d = Td_star;
    LOG_msf_w_m = msf_get_omega_m();
    LOG_msf_w_m_dot = msf_get_omega_m_dot();

    // Output to DAC
    dac_set_output(0, LOG_omega_m, -50, 50);
    dac_set_output(1, LOG_T_sfb, -1, 1);
    dac_set_output(2, LOG_T_cff, -1, 1);
}

void task_mc_set_omega_m_star(double omega_m)
{
    omega_m_star = omega_m;
}

void task_mc_set_cff_enabled(uint32_t enabled)
{
    mcff_enabled = enabled;
}

void task_mc_set_omega_m_src(uint8_t use_encoder)
{
    omega_m_src_use_encoder = use_encoder;
}

inline static int saturate(double min, double max, double *value)
{
    if (*value < min) {
        // Lower bound saturation
        *value = min;
        return -1;
    } else if (*value > max) {
        // Upper bound saturation
        *value = max;
        return 1;
    } else {
        // No saturation
        return 0;
    }
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/uart.c<|end_filename|>
#include "drv/uart.h"
#include "sys/defines.h"
#include "xparameters.h"
#include "xuartps.h"
#include <stdio.h>

#define UART_DEVICE_ID XPAR_XUARTPS_0_DEVICE_ID

#define TEST_BUFFER_SIZE (26)

// Instance of the UART Device
XUartPs UartPs;

// The following buffers are used in this example to send
// and receive data with the UART.
static u8 SendBuffer[TEST_BUFFER_SIZE]; // Buffer for Transmitting Data
static u8 RecvBuffer[TEST_BUFFER_SIZE]; // Buffer for Receiving Data

// The following counters are used to determine when the entire
// buffer has been sent and received.
volatile int TotalReceivedCount;
volatile int TotalSentCount;
int TotalErrorCount;

int uart_init(void)
{
    XUartPs *UartInstPtr = &UartPs;
    u16 DeviceId = UART_DEVICE_ID;

    u32 LoopCount = 0;
    unsigned int SentCount;
    unsigned int ReceivedCount;

    int Status;
    XUartPs_Config *Config;
    int Index;

    /*
     * Initialize the UART driver so that it's ready to use
     * Look up the configuration in the config table, then initialize it.
     */
    Config = XUartPs_LookupConfig(DeviceId);
    if (NULL == Config) {
        return FAILURE;
    }

    Status = XUartPs_CfgInitialize(UartInstPtr, Config, Config->BaseAddress);
    if (Status != XST_SUCCESS) {
        return FAILURE;
    }

    /* Check hardware build */
    Status = XUartPs_SelfTest(UartInstPtr);
    if (Status != XST_SUCCESS) {
        return FAILURE;
    }

    /* Use local loopback mode. */
    XUartPs_SetOperMode(UartInstPtr, XUARTPS_OPER_MODE_LOCAL_LOOP);

    /*
     * Initialize the send buffer bytes with a pattern and zero out
     * the receive buffer.
     */
    for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {
        SendBuffer[Index] = (Index % 26) + 'A';
        RecvBuffer[Index] = 0;
    }

    /* Block sending the buffer. */
    SentCount = XUartPs_Send(UartInstPtr, SendBuffer, TEST_BUFFER_SIZE);
    if (SentCount != TEST_BUFFER_SIZE) {
        return FAILURE;
    }

    /*
     * Wait while the UART is sending the data so that we are guaranteed
     * to get the data the 1st time we call receive, otherwise this function
     * may enter receive before the data has arrived
     */
    while (XUartPs_IsSending(UartInstPtr)) {
        LoopCount++;
    }

    /* Block receiving the buffer. */
    ReceivedCount = 0;
    while (ReceivedCount < TEST_BUFFER_SIZE) {
        ReceivedCount += XUartPs_Recv(UartInstPtr, &RecvBuffer[ReceivedCount], (TEST_BUFFER_SIZE - ReceivedCount));
    }

    /*
     * Check the receive buffer against the send buffer and verify the
     * data was correctly received
     */
    for (Index = 0; Index < TEST_BUFFER_SIZE; Index++) {
        if (SendBuffer[Index] != RecvBuffer[Index]) {
            return FAILURE;
        }
    }

    /* Restore to normal mode. */
    XUartPs_SetOperMode(UartInstPtr, XUARTPS_OPER_MODE_NORMAL);

    return SUCCESS;
}

int uart_send(char *msg, int len)
{
    return XUartPs_Send(&UartPs, (uint8_t *) msg, len);
}

int uart_recv(char *msg, int len)
{
    return XUartPs_Recv(&UartPs, (uint8_t *) msg, len);
}


<|start_filename|>sdk/basic/control_apps/HBridge_CC.c<|end_filename|>
/*
 * HBridge_CC_8Leg.c
 *
 *  Created on: Jun 27, 2014
 *      Author: sever212
 */
#include "../src/project_include.h"

#define KP_GAIN 0.452
#define KiTs_GAIN 0.005
#define ADC_SCALE 1 		//(important to make sure this is recognized as a double and not an int) new: 3 amps per volt, wound around three times gives me a factor of 1 --OLD:3 amps per volt, wound around twice gives me a factor of 3/2



HB_context HB_Controllers[NUM_HBRIDGE];
controller_linked_list_item cont_items [NUM_HBRIDGE];


//Logging
u32 HB_H1_d1, HB_H1_d2, HB_H1_error, HB_H1_output, HB_H1_reference, HB_H1_Current, dummy;
u32 HB_H2_d1, HB_H2_d2, HB_H2_error, HB_H2_output, HB_H2_reference, HB_H2_Current;
u32 HB_H3_d1, HB_H3_d2, HB_H3_error, HB_H3_output, HB_H3_reference, HB_H3_Current;
u32 HB_H4_d1, HB_H4_d2, HB_H4_error, HB_H4_output, HB_H4_reference, HB_H4_Current;

//callback declarations:
static void HB_Input(void *arg, struct controller_context *cnt);
static void HB_Output(void *arg, struct controller_context *cnt);

static u8 ExampleHB_NeedsInput(void *arg, void *HBContext);
static void ExampleHB_WriteOutput(void *arg, void *HBContext);


/*****************************
 * Init_VSI_DQ_VSPWM()
 * Sets up the controllers memory to default values
 * and optionally starts the controllers running.
 * The default settings can be overridden by manually
 * editing the HB_Controllers structure.
 *
 *StartAllControllers: if this is set to TRUE, all the
 * controllers are registered in control.c and started.
 *returns a success code
 */
int Init_HBridge_CC(u8 StartAllControllers)
{
	//pi_house *pi_ptr = HB_Controllers;
	int i;
	for (i = 0; i<NUM_HBRIDGE; i++)
	{
		//PI controller
		HB_Controllers[i].c.pi.kits = KiTs_GAIN;
		HB_Controllers[i].c.pi.kp = KP_GAIN;
		HB_Controllers[i].c.pi.lastInt = 0;
		HB_Controllers[i].c.pi.plusSat = 0.9;
		HB_Controllers[i].c.pi.negSat = -0.9;

		//basic
		HB_Controllers[i].c.io.inputs = &HB_Controllers[i].c.input;
		HB_Controllers[i].c.io.outputs = &HB_Controllers[i].c.output;
		HB_Controllers[i].c.io.NumInputs = 1;
		HB_Controllers[i].c.io.NumOutputs = 1;
		HB_Controllers[i].c.io.NumClocks = 1;
		HB_Controllers[i].c.io.ClockTime = 5; //5 us
		HB_Controllers[i].c.io.clockCount = 0;
		HB_Controllers[i].c.io.Type = C_PI;
		HB_Controllers[i].c.io.Status = C_RUNNING;

		//context
		HB_Controllers[i].c.ctxt.arg_in = 0;
		HB_Controllers[i].c.ctxt.arg_out = 0;
		HB_Controllers[i].c.ctxt.control_io = &HB_Controllers[i].c.io;
		HB_Controllers[i].c.ctxt.controller_info = &HB_Controllers[i].c.pi;

		HB_Controllers[i].c.ctxt.input_callback = HB_Input;
		HB_Controllers[i].c.ctxt.output_callback = HB_Output;

		//HB_Controllers[i].c.adc_scale = ADC_SCALE;
		HB_Controllers[i].c.reference = 0;

		//callbacks
		HB_Controllers[i].get_input = ExampleHB_NeedsInput;
		HB_Controllers[i].write_output = ExampleHB_WriteOutput;

		//load it:
		cont_items[i].context = &HB_Controllers[i].c.ctxt;
		if (StartAllControllers)
			Start_HBridge_CC_Controller(i+1);

	}
	return XST_SUCCESS;
}


/*****************************
 * Start_HBridge_CC_Controller()
 * Registers the HBridge current controller with control.c.
 *
 *HB: the HB number to register (1 - NUM_HBRIDGE)
 *returns a success code
 */
int Start_HBridge_CC_Controller(u32 HB)
{
	u32 i = HB-1;
	if (i >= NUM_HBRIDGE)
		return INVALID_ARGUMENT;
	if (cont_items[i].bRegistered == TRUE)
		return INVALID_OPERATION;

	RegisterController(0, &cont_items[i]);

	return SUCCESS;
}


/*****************************
 * Stop_HBridge_CC_Controller()
 * Unregisters the HBridge current controller from control.c.
 *
 *HB: the HB number to unregister (1 - NUM_HBRIDGE)
 *returns a success code
 */
int Stop_HBridge_CC_Controller(u32 HB)
{
	u32 i = HB-1;
	if (i >= NUM_HBRIDGE)
		return INVALID_ARGUMENT;
	if (cont_items[i].bRegistered == TRUE)
		UnregisterController(0, &cont_items[i]);

	return SUCCESS;
}

/*****************************
 * HBridge_CC_StopCommand()
 * Process an ascii command for stopping the HB controllers
 * Stop_HBridge_CC_Controller is called
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int HBridge_CC_StopCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	int i;
	for (i=0; i< NUM_HBRIDGE; i++)
		Stop_HBridge_CC_Controller(i+1);

	strcpy(szResponse, "OK");
	return strlen(szResponse);
}


/*****************************
 * HBridge_CC_StartCommand()
 * Process an ascii command for starting HB current controllers
 * Init_HBridge_CC_8Leg is called
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int HBridge_CC_StartCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	Init_HBridge_CC(TRUE);
	strcpy(szResponse, "OK");
	return strlen(szResponse);
}

/*****************************
 * HBridge_CC_Command()
 * Process an ascii command for setting a reference current
 * Syntax: n,val
 *  n is the Hbridge number (1 - NUM_HBRIDGE),
 *	val is the current reference value * 128
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int HBridge_CC_Command(const char * szCmd, char *szResponse, void *CommDevice)
{
	const char *ptr;
	u32 hbridge = atoi(szCmd);
	u8 bError = FALSE;
	if (hbridge < 1 || hbridge > NUM_HBRIDGE)
		bError = TRUE;
	else
	{
		for (ptr = szCmd; ptr != &szCmd[strlen(szCmd)-1]; ptr++)
			if (*ptr == ',')
				break;
		ptr++;	//advance past the comma
		if (ptr >= &szCmd[strlen(szCmd)])
			bError = TRUE;
		else
		{
			double current_ref = ((double) atoi(ptr))/128;
			if (HBridge_CC_SetCurrentRef(hbridge, current_ref) == SUCCESS)
				strcpy(szResponse, "OK");
			else
				bError = TRUE;
		}

	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}


/*****************************
 * HBridge_CC_SetCurrentRef()
 * Set a reference current for an H-Bridge
 *
 *hbridge: index of the HBridge to set the reference current of
 *			Should be 1 - NUM_HBRIDGE
 *reference: the value of the new reference current
 */
int HBridge_CC_SetCurrentRef(u32 hbridge, double reference)
{
	if (hbridge < 1 || hbridge > NUM_HBRIDGE)
		return INVALID_ARGUMENT;
	HB_Controllers[hbridge-1].c.reference = reference;

	//logging:
	switch(hbridge)
	{
	case 1:
		HB_H1_reference = (s32)(128 * reference);
		break;
	case 2:
		HB_H2_reference = (s32)(128 * reference);
		break;
	case 3:
		HB_H3_reference = (s32)(128 * reference);
		break;
	case 4:
		HB_H4_reference = (s32)(128 * reference);
		break;
	}
	return SUCCESS;
}


//Internal function called by control.c before executing a controller
static void HB_Input(void *arg, struct controller_context *cnt)
{
	u32 i=0;
	u32 *Herror, *Hcurrent;
	Herror = &dummy;
	Hcurrent = &dummy;

	for (i= 0; i< NUM_HBRIDGE; i++)
		if (&HB_Controllers[i].c.io == cnt)
		{
			switch(i)
			{
			case 0:
				Herror = &HB_H1_error;
				Hcurrent = &HB_H1_Current;
				break;

			case 1:
				Herror = &HB_H2_error;
				Hcurrent = &HB_H2_Current;
				break;

			case 2:
				Herror = &HB_H3_error;
				Hcurrent = &HB_H3_Current;
				break;

			case 3:
				Herror = &HB_H4_error;
				Hcurrent = &HB_H4_Current;
				break;

			}
			break;	//we have our match
		}

	if (i < NUM_HBRIDGE)
	{
		HB_Controllers[i].InputAtTickCount = MainTimerTimerTick;

		//get the input
		HB_Controllers[i].get_input(HB_Controllers[i].input_arg, &HB_Controllers[i]);

		//calculate the error and store it
		HB_Controllers[i].c.input = HB_Controllers[i].c.reference - HB_Controllers[i].I;
		*Herror = (s32) (HB_Controllers[i].c.input*128);
		*Hcurrent =  (s32) (HB_Controllers[i].I*128);
	}
	else
		; //should not happen!!
}

//Internal function called by control.c when an output is ready
static void HB_Output(void *arg, struct controller_context *cnt)
{
	u32 i = 0;

	//logging
	u32 *Hd1, *Hd2, *Houtput;
	Hd1 = &dummy;
	Hd2 = &dummy;
	Houtput = &dummy;

	for (i= 0; i< NUM_HBRIDGE; i++)
		if (&HB_Controllers[i].c.io == cnt)
		{
			//logging
			switch(i)
			{
			case 0:
				Hd1 = &HB_H1_d1;
				Hd2 = &HB_H1_d2;
				Houtput = &HB_H1_output;
				break;

			case 1:
				Hd1 = &HB_H2_d1;
				Hd2 = &HB_H2_d2;
				Houtput = &HB_H2_output;
				break;

			case 2:
				Hd1 = &HB_H3_d1;
				Hd2 = &HB_H3_d2;
				Houtput = &HB_H3_output;
				break;

			case 3:
				Hd1 = &HB_H4_d1;
				Hd2 = &HB_H4_d2;
				Houtput = &HB_H4_output;
				break;
			}
			break;
		}

	if (i < NUM_HBRIDGE)
	{
		//safety to avoid roll-over:
		if (HB_Controllers[i].c.output > 1.0)
			HB_Controllers[i].c.output = 1.0;
		if (HB_Controllers[i].c.output < -1.0)
			HB_Controllers[i].c.output = -1.0;

		//calculate duty ratios
		HB_Controllers[i].d[0] = (unsigned char) 127*(1 + HB_Controllers[i].c.output);
		HB_Controllers[i].d[1] = (unsigned char) 127*(1 - HB_Controllers[i].c.output);

		//write them out:
		HB_Controllers[i].write_output(HB_Controllers[i].output_arg, &HB_Controllers[i]);

		//logging
		*Hd1 = HB_Controllers[i].d[0];
		*Hd2 = HB_Controllers[i].d[1];
		*Houtput = (s32) (HB_Controllers[i].c.output*128);
	}
	else
		; //should not happen!!
}

//HB_Controllers[i].get_input = ExampleHB_NeedsInput;
//HB_Controllers[i].write_output = ExampleHB_WriteOutput;

//return false
static u8 ExampleHB_NeedsInput(void *arg, void *HBContext)
{
	u32 i=0;
	u32 adc_num = 0;
	struct HB_context *cnt = HBContext;

	for (i= 0; i< NUM_HBRIDGE; i++)
		if (&HB_Controllers[i] == cnt)
		{
			if (i == 0)
				adc_num = 4;
			else if (i == 1)
				adc_num = 5;
			else if (i == 2)
				adc_num = 6;
			else if (i == 3)
				adc_num = 1;
			break;	//we have our match
		}

	//measure the current and store it
	if (i < NUM_HBRIDGE)
		HB_Controllers[i].I = 0.6*ReadADC(adc_num);
	else
		; //uh oh! should not get here!

	return FALSE;
}


//return false if ABC values set, true if DQ values set
static void ExampleHB_WriteOutput(void *arg, void *HBContext)
{
	u32 legs[2];
	struct HB_context *cnt = HBContext;
	int i;

	legs[0] = 0; legs[1] = 0; legs[2] = 0;
	for (i= 0; i< NUM_HBRIDGE; i++)
	{
		if (&HB_Controllers[i] == cnt)
		{
			if (i == 0)
				{ legs[0] = 5; legs[1] = 6; }
			else if (i == 1)
				{ legs[0] = 13; legs[1] = 14; }
			else if (i == 2)
				{ legs[0] = 17; legs[1] = 18; }
			else if (i == 3)
				{ legs[0] = 11; legs[1] = 12; }
			else
				i = NUM_HBRIDGE;	//safety, don't write out to other legs.
			break;
		}
	}

	if (i < NUM_HBRIDGE)
	{
		//write them out:
		WriteDutyRatio(legs[0], HB_Controllers[i].d[0]);
		WriteDutyRatio(legs[1], HB_Controllers[i].d[1]);
	}
}



<|start_filename|>sdk/bare/user/usr/beta_labs/co.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/co.h"
#include "usr/beta_labs/machine.h"

// Current observer gains assuming Ts = 5e-05
// Tuned to 10 Hz
#define Kpo_d   (0.0008127575979211963)
#define Kio_dTs (0.0008155323839119321)
#define Kpo_q   (0.0008706077043813599)
#define Kio_qTs (0.0008155323839119323)
#define A_d     (0.4991479447697602)
#define A_q     (0.5163317748186554)
#define B_d     (1.9263540585778451)
#define B_q     (1.8602624045436327)

typedef struct complex_t {
    double d;
    double q;
} complex_t;

// Forward declarations
static void _update_from_err(complex_t Idq_err, complex_t Idq, complex_t Vdq, double omega_e);

// States
static complex_t Idq_hat;
static complex_t Idq_err_acc_last;
static complex_t Idq_hat_last;
static complex_t Idq_err;
static complex_t Idq_err_acc;
static double Vq_sfb;
static double Vq_tot;
static double Iq_1;
static double Iq_hat;
static double Vd_sfb;
static double Vd_tot;
static double Id_1;
static double Id_hat;

void co_update(double *Idq0, double *Vdq0, double omega_e)
{
    complex_t Idq;
    complex_t Vdq;

    Idq.d = Idq0[0]; // direct
    Idq.q = Idq0[1]; // quadrature

    Vdq.d = Vdq0[0]; // direct
    Vdq.q = Vdq0[1]; // quadrature

    // Find current error
    Idq_err.d = Idq.d - Idq_hat.d;
    Idq_err.q = Idq.q - Idq_hat.q;

    _update_from_err(Idq_err, Idq, Vdq, omega_e);
}

static void _update_from_err(complex_t Idq_err, complex_t Idq, complex_t Vdq, double omega_e)
{
    // Saving Last States
    Idq_err_acc_last.d = Idq_err_acc.d;
    Idq_err_acc_last.q = Idq_err_acc.q;
    Idq_hat_last.d = Idq_hat.d;
    Idq_hat_last.q = Idq_hat.q;

    // Error and Accumulators
    Idq_err_acc.d = Idq_err_acc.d + Idq_err.d;
    Idq_err_acc.q = Idq_err_acc.q + Idq_err.q;

    // Q-axis
    Vq_sfb = Kpo_q * Idq_err.q + Kio_qTs * Idq_err_acc.q;
    Vq_tot = Vq_sfb - (Ld_HAT * Idq.d) * omega_e + Vdq.q;
    Iq_1 = B_q * Vq_tot;
    Iq_hat = Iq_1 + Idq_hat_last.q * A_q;

    // D-axis
    Vd_sfb = Kpo_d * Idq_err.d + Kio_dTs * Idq_err_acc.d;
    Vd_tot = Vd_sfb + Lq_HAT * Idq.q * omega_e + Vdq.d;
    Id_1 = B_d * Vd_tot;
    Id_hat = Id_1 + Idq_hat_last.d * A_d;

    Idq_hat.d = Id_hat;
    Idq_hat.q = Iq_hat;
}

void co_get_Idq_hat(double *Id_hat, double *Iq_hat)
{
    *Id_hat = Idq_hat.d;
    *Iq_hat = Idq_hat.q;
}

void co_get_Esal_hat(double *Esal_d_hat, double *Esal_q_hat)
{
    *Esal_d_hat = -Vd_sfb;
    *Esal_q_hat = -Vq_sfb;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/sys/injection.c<|end_filename|>
#include "usr/user_config.h"

#if USER_CONFIG_ENABLE_INJECTION == 1

#include "sys/cmd/cmd_inj.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/injection.h"
#include "sys/scheduler.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>

// Linked list of all registered injection contexts
static inj_ctx_t *inj_ctxs = NULL;

// Used to give each registered ctx a unique ID
static int next_ctx_id = 0;

// Chirp function
//
// Generates the chirp signal value given:
// - time: current time instant
// - w1:   low freq (rad)
// - w2:   high freq (rad)
// - A:    amplitude
// - M:    time period
static inline double _chirp(double w1, double w2, double A, double M, double time)
{
    double out;
    out = A * cos(w1 * time + (w2 - w1) * time * time / (2 * M));
    return out;
}

static inline double _triangle(double min, double max, double period, double time)
{
    // NOTE: Assumes the user set min = -max. Therefore, the mid value is 0.
    //       Function starts at mid value at t = 0
    //
    // Let mid = min + ((max - min) / 2)
    //
    // Triangle waveform output:
    //        starts at mid at t = 0,
    // S1: increases to max at t = T/4,
    // S2: decreases to mid at t = T/2
    // S3: decreases to min at t = 3T/4
    // S4: increases to mid at t = T

    double out = 0.0;
    double mid = min + ((max - min) / 2.0);

    // Calculate slopes
    double m_pos = (max - min) / (period / 2.0);
    double m_neg = -m_pos;

    // y-intercept
    double x1, y1;

    if (0.0 <= time && time < period / 4.0) {
        // State S1
        out = m_pos * time + mid;
    } else if (period / 4.0 <= time && time < period / 2.0) {
        // State S2
        // y = m(x - x1) + y1
        x1 = period / 4.0;
        y1 = max;
        out = m_neg * (time - x1) + y1;
    } else if (period / 2.0 <= time && time < 3.0 * period / 4.0) {
        // State S3
        // y = m(x - x1) + y1
        x1 = period / 2.0;
        y1 = mid;
        out = m_neg * (time - x1) + y1;
    } else {
        // State S4
        // y = m(x - x1) + y1
        x1 = 3.0 * period / 4.0;
        y1 = min;
        out = m_pos * (time - x1) + y1;
    }

    return out;
}

static inline double _square(double min, double max, double period, double time)
{
    // Output defaults to min value
    double out = min;

    // Output becomes max value for second half of period
    if (time >= period / 2.0) {
        out = max;
    }

    return out;
}

void injection_init(void)
{
    cmd_inj_register();
}

void injection_ctx_init(inj_ctx_t *ctx, char *name)
{
    ctx->id = next_ctx_id++;
    strncpy(ctx->name, name, INJ_MAX_NAME_LENGTH);
}

void injection_ctx_register(inj_ctx_t *ctx)
{
    // Don't let clients re-register their ctx
    if (ctx->registered) {
        HANG;
    }

    // Mark as registered
    ctx->registered = 1;

    // Base case: there are no tasks in linked list
    if (inj_ctxs == NULL) {
        inj_ctxs = ctx;
        inj_ctxs->next = NULL;
        return;
    }

    // Find end of list
    inj_ctx_t *curr = inj_ctxs;
    while (curr->next != NULL)
        curr = curr->next;

    // Append new ctx to end of list
    curr->next = ctx;
    ctx->next = NULL;
}

void injection_ctx_unregister(inj_ctx_t *ctx)
{
    // Don't let clients unregister their already unregistered ctx
    if (!ctx->registered) {
        HANG;
    }

    // Mark as unregistered
    ctx->registered = 0;

    // Make sure list isn't empty
    if (inj_ctxs == NULL) {
        HANG;
    }

    // Special case: trying to remove the head of the list
    if (inj_ctxs->id == ctx->id) {
        inj_ctxs = inj_ctxs->next;
        return;
    }

    // Now we know that 'ctx' to remove is NOT first node

    inj_ctx_t *prev = NULL;
    inj_ctx_t *curr = inj_ctxs;

    // Find spot in linked list to remove ctx
    while (curr->id != ctx->id) {
        prev = curr;
        curr = curr->next;
    }

    // 'curr' is now the one we want to remove!

    prev->next = curr->next;
}

void injection_inj(double *output, inj_ctx_t *ctx, double Ts)
{
    if (!ctx->registered) {
        return;
    }

    double value = 0.0;

    switch (ctx->inj_func) {
    case CONST:
    {
        value = ctx->constant.value;
        break;
    }

    case NOISE:
    {
        // Generate random number between 0..1
        double r = (double) rand() / (double) RAND_MAX;

        // Make between -1.0 .. 1.0
        r = (2.0 * r) - 1.0;

        value = ctx->noise.gain * r;
        value += ctx->noise.offset;
        break;
    }

    case CHIRP:
    {
        ctx->curr_time += Ts;
        if (ctx->curr_time >= ctx->chirp.period) {
            ctx->curr_time = 0.0;
        }

        value = _chirp(
            PI2 * ctx->chirp.freqMin, PI2 * ctx->chirp.freqMax, ctx->chirp.gain, ctx->chirp.period, ctx->curr_time);
        break;
    }

    case TRIANGLE:
    {
        ctx->curr_time += Ts;
        if (ctx->curr_time >= ctx->triangle.period) {
            ctx->curr_time = 0.0;
        }

        value = _triangle(ctx->triangle.valueMin, ctx->triangle.valueMax, ctx->triangle.period, ctx->curr_time);
        break;
    }

    case SQUARE:
    {
        ctx->curr_time += Ts;
        if (ctx->curr_time >= ctx->square.period) {
            ctx->curr_time = 0.0;
        }

        value = _square(ctx->square.valueMin, ctx->square.valueMax, ctx->square.period, ctx->curr_time);
        break;
    }

    case NONE:
    default:
        // Injection function not set by user,
        // so don't do anything to the output signal
        return;
    }

    // Perform operation to do injection
    switch (ctx->operation) {
    case ADD:
        *output += value;
        break;
    case SUB:
        *output -= value;
        break;
    case SET:
        *output = value;
        break;
    }
}

inj_ctx_t *injection_find_ctx_by_name(char *name)
{
    inj_ctx_t *curr = inj_ctxs;

    // Try to find entry which is "name"
    while (curr != NULL) {
        if (strcmp(name, curr->name) == 0) {
            // Found it!
            return curr;
        }

        // Move to next entry in linked list
        curr = curr->next;
    }

    return NULL;
}

void injection_ctx_clear(inj_ctx_t *inj_ctx)
{
    inj_ctx->inj_func = NONE;
}

void injection_clear(void)
{
    inj_ctx_t *curr = inj_ctxs;

    // Operate on all registered contexts
    while (curr != NULL) {
        // Disable injection function
        curr->inj_func = NONE;

        // Move to next entry in linked list
        curr = curr->next;
    }
}

void injection_const(inj_ctx_t *ctx, inj_op_e op, double value)
{
    ctx->inj_func = CONST;
    ctx->operation = op;
    ctx->curr_time = 0.0;
    ctx->constant.value = value;
}

void injection_noise(inj_ctx_t *ctx, inj_op_e op, double gain, double offset)
{
    ctx->inj_func = NOISE;
    ctx->operation = op;
    ctx->curr_time = 0.0;
    ctx->noise.gain = gain;
    ctx->noise.offset = offset;
}

void injection_chirp(inj_ctx_t *ctx, inj_op_e op, double gain, double freqMin, double freqMax, double period)
{
    ctx->inj_func = CHIRP;
    ctx->operation = op;
    ctx->curr_time = 0.0;
    ctx->chirp.gain = gain;
    ctx->chirp.freqMin = freqMin;
    ctx->chirp.freqMax = freqMax;
    ctx->chirp.period = period;
}

void injection_triangle(inj_ctx_t *ctx, inj_op_e op, double valueMin, double valueMax, double period)
{
    ctx->inj_func = TRIANGLE;
    ctx->operation = op;
    ctx->curr_time = 0.0;
    ctx->triangle.valueMin = valueMin;
    ctx->triangle.valueMax = valueMax;
    ctx->triangle.period = period;
}

void injection_square(inj_ctx_t *ctx, inj_op_e op, double valueMin, double valueMax, double period)
{
    ctx->inj_func = SQUARE;
    ctx->operation = op;
    ctx->curr_time = 0.0;
    ctx->square.valueMin = valueMin;
    ctx->square.valueMax = valueMax;
    ctx->square.period = period;
}

// ***************************
// Code for running the state machine to
// list the registered injection contexts
// to the UART
// ***************************

typedef enum sm_states_list_e { LISTING = 1, REMOVE_TASK } sm_states_list_e;

typedef struct sm_ctx_list_t {
    sm_states_list_e state;
    task_control_block_t tcb;

    inj_ctx_t *curr;
} sm_ctx_list_t;

#define SM_LIST_UPDATES_PER_SEC (10000)
#define SM_LIST_INTERVAL_USEC   (USEC_IN_SEC / SM_LIST_UPDATES_PER_SEC)

void state_machine_list_callback(void *arg)
{
    sm_ctx_list_t *ctx = (sm_ctx_list_t *) arg;

    switch (ctx->state) {
    case LISTING:
        // Print entry
        debug_printf("%s\r\n", ctx->curr->name);

        // Move to next entry
        ctx->curr = ctx->curr->next;
        if (ctx->curr == NULL)
            ctx->state = REMOVE_TASK;
        break;

    case REMOVE_TASK:
        debug_printf("\r\n");
        scheduler_tcb_unregister(&ctx->tcb);
        break;

    default:
        // Can't happen
        HANG;
        break;
    }
}

static sm_ctx_list_t ctx_list;

void injection_list(void)
{
    if (inj_ctxs == NULL) {
        // Don't try to print empty list!
        return;
    }

    // Initialize the state machine context
    ctx_list.state = LISTING;
    ctx_list.curr = inj_ctxs;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx_list.tcb, state_machine_list_callback, &ctx_list, "inj_list", SM_LIST_INTERVAL_USEC);
    scheduler_tcb_register(&ctx_list.tcb);
}

#endif // USER_CONFIG_ENABLE_INJECTION


<|start_filename|>sdk/bare/common/drv/pwm.h<|end_filename|>
#ifndef PWM_H
#define PWM_H

#include "drv/hardware_targets.h"
#include "usr/user_config.h"
#include <stdbool.h>
#include <stdint.h>

#define PWM_DEFAULT_SWITCHING_FREQ_HZ (100000.0)
#define PWM_DEFAULT_DEADTIME_NS       (100.0)

#define PWM_MAX_SWITCHING_FREQ_HZ (2e6)
#define PWM_MIN_SWITCHING_FREQ_HZ (2e3)

#define PWM_MAX_DEADTIME_NS ((1 << 16) - 1)
#define PWM_MIN_DEADTIME_NS (25)

typedef enum {
    // Keep first channel index at 0!
    PWM_OUT1 = 0,
    PWM_OUT2,
    PWM_OUT3,
    PWM_OUT4,
    PWM_OUT5,
    PWM_OUT6,
    PWM_OUT7,
    PWM_OUT8,
    PWM_OUT9,
    PWM_OUT10,
    PWM_OUT11,
    PWM_OUT12,
    PWM_OUT13,
    PWM_OUT14,
    PWM_OUT15,
    PWM_OUT16,
    PWM_OUT17,
    PWM_OUT18,
    PWM_OUT19,
    PWM_OUT20,
    PWM_OUT21,
    PWM_OUT22,
    PWM_OUT23,
    PWM_OUT24,

    // Keep this as last entry!
    PWM_NUM_CHANNELS,
} pwm_channel_e;

typedef struct pwm_status_t {
    uint8_t ready;
    uint8_t fault_desat;
    uint8_t fault_temp;
} pwm_status_t;

static inline bool pwm_is_valid_channel(pwm_channel_e channel)
{
    if (channel >= PWM_OUT1 && channel < PWM_NUM_CHANNELS) {
        return true;
    }

    return false;
}

void pwm_init(void);

void pwm_toggle_reset(void);

int pwm_enable(void);
int pwm_disable(void);
bool pwm_is_enabled(void);

int pwm_set_switching_freq(double freq_hz);
int pwm_set_deadtime_ns(uint16_t deadtime);

double pwm_get_switching_freq(void);
uint16_t pwm_get_deadtime_ns(void);

int pwm_set_duty(pwm_channel_e channel, double duty);

void pwm_set_all_duty_midscale(void);

// Mux for PWM output pins
int pwm_mux_set_all_pins(uint32_t *config);
int pwm_mux_set_one_pin(uint32_t pwm_pin_idx, uint32_t config);

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

int pwm_get_status(pwm_channel_e channel, pwm_status_t *status);
void pwm_get_all_flt_temp(uint8_t *flt_temp);
void pwm_get_all_flt_desat(uint8_t *flt_desat);
void pwm_get_all_rdy(uint8_t *rdy);
void pwm_set_all_rst(uint8_t rst);

#endif // USER_CONFIG_HARDWARE_TARGET

#endif // PWM_H


<|start_filename|>sdk/basic/control_apps/PosContTry1.h<|end_filename|>
#ifndef __POS_CONT_TRY1_H
#define __POS_CONT_TRY1_H

//#define USE_H_BRIDGE_CC			//uncomment this for HBridge CC (Bridge config)
//#define USE_H_BRIDGE_DQ_CC		//uncomment this for using Hbridges controlled by a dq0 controller
#define USE_VSI_CC					//uncomment this for VSI CC (Parallel config)
#define VSI_ISOLATED				//uncomment this if using isolated VSI's in parallel config

#define VSI_BOT_INDEX 0
#define VSI_TOP_INDEX 1


#define POS_BOT_INDEX 0
#define POS_TOP_INDEX 1

#define POS_XY0_INDEX 0
#define POS_THXY_INDEX 1

typedef struct pid_house
{
	pid_controller pid;
	controller_context io;
	controller_holder ctxt;
	double input;
	double output;
	double reference;
	u8 updated;	//set to TRUE everytime this controller output is updated, cleared everytime the output is used
	//1 to 1 double adc_scale;		//3 amps per volt, wound around twice gives me a factor of 3/2
} pid_house;

typedef struct PosCntVSI_context
{
	pid_house x[2];
	pid_house y[2];
#if defined(USE_VSI_CC) || defined(USE_H_BRIDGE_DQ_CC)
	struct VSI_context *VSItop;
	struct VSI_context *VSIbot;
#endif
#if defined(USE_H_BRIDGE_CC)
	struct HB_context *HBtop[3];
	struct HB_context *HBbot[3];
	double Itop_uvw_sus[3];	//reference suspension uvw currents
	double Ibot_uvw_sus[3]; //reference suspension uvw currents
#endif
	u32 InputAtTickCount;		//the tick count of the last time the inputs were updated (to avoid unnecessary dq transformations)
	double x_thx[2];
	double y_thy[2];
	double xs[2];
	double ys[2];
	double Itop_xy[2];
	double Ibot_xy[2];
	double MaxPhaseCurrent;
	double TorqueVSI_topIxy[2];
	double TorqueVSI_botIxy[2];
} PosCnt_context;

extern PosCnt_context PosControllers;

extern int Init_PosContTry1(u8 StartAllControllers);
extern int Stop_PosContTry1_Controller(u32 poscnt);
extern int PosContTry1_StopCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int PosContTry1_StartCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int PosContTry1_Command(const char * szCmd, char *szResponse, void *CommDevice);
#endif //__POS_CONT_TRY1_H



<|start_filename|>sdk/bare/common/drv/fpga_timer.c<|end_filename|>
#include "drv/fpga_timer.h"
#include "sys/defines.h"
#include "xparameters.h"
#include "xtmrctr.h"
#include <stdio.h>

// Get the TIMER_1 device from the FPGA
#define TMR_DEVICE_ID XPAR_CONTROL_TIMER_1_DEVICE_ID

static XTmrCtr timer;

void fpga_timer_init(void)
{
    int xStatus = XTmrCtr_Initialize(&timer, TMR_DEVICE_ID);
    if (xStatus != XST_SUCCESS) {
        HANG;
    }

    // Configure timer to count up and auto-restart when it hits max value
    XTmrCtr_SetOptions(&timer, 0, XTC_AUTO_RELOAD_OPTION);

    // Reset timer to value of 0
    XTmrCtr_SetResetValue(&timer, 0, 0);

    XTmrCtr_Start(&timer, 0);
}

uint32_t fpga_timer_now(void)
{
    return XTmrCtr_GetValue(&timer, 0);
}

double fpga_timer_ticks_to_usec(uint32_t ticks)
{
    // FPGA clock is 200MHz on AMDC
    double usec = (double) ticks / 200;
    return usec;
}

double fpga_timer_ticks_to_sec(uint32_t ticks)
{
    // FPGA clock is 200MHz on AMDC
    double sec = (double) ticks / 200e6;
    return sec;
}


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_vsi.h<|end_filename|>
#ifndef CMD_VSI_H
#define CMD_VSI_H

void cmd_vsi_register(void);

int cmd_vsi(int argc, char **argv);

#endif // CMD_VSI_H


<|start_filename|>ip_repo/amdc_analog_1.0/drivers/amdc_analog_v1_0/src/amdc_analog.h<|end_filename|>

#ifndef AMDC_ANALOG_H
#define AMDC_ANALOG_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_ANALOG_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_ANALOG_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_ANALOG_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_ANALOG_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_ANALOG_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_ANALOG_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_ANALOG_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_ANALOG_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_ANALOG_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_ANALOG_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_ANALOG_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_ANALOG_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_ANALOG_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_ANALOG_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_ANALOG_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_ANALOG_S00_AXI_SLV_REG15_OFFSET 60
#define AMDC_ANALOG_S00_AXI_SLV_REG16_OFFSET 64


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_ANALOG register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_ANALOGdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_ANALOG_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_ANALOG_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_ANALOG register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_ANALOG device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_ANALOG_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_ANALOG_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_ANALOG instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_ANALOG_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_ANALOG_H


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_dtc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_dtc.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/task_dtc.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (3)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "init", "Start current controller" },
    { "deinit", "Stop current controller" },
    { "set <mA> <mHz>", "Command sinusoidal current" },
};

void cmd_dtc_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "dtc", "Dead time compensation commands", cmd_help, NUM_HELP_ENTRIES, cmd_dtc);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

//
// Handles the 'dtc' command
// and all sub-commands
//
int cmd_dtc(int argc, char **argv)
{
    // Handle 'init' sub-command
    if (strcmp("init", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure dtc task was not already inited
        if (task_dtc_is_inited())
            return CMD_FAILURE;

        task_dtc_init();
        return CMD_SUCCESS;
    }

    // Handle 'deinit' sub-command
    if (strcmp("deinit", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure dtc task was already inited
        if (!task_dtc_is_inited())
            return CMD_FAILURE;

        task_dtc_clear();
        task_dtc_deinit();
        return CMD_SUCCESS;
    }

    // Handle 'set' sub-command
    if (strcmp("set", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 4)
            return CMD_INVALID_ARGUMENTS;

        // Pull out mA argument
        // and saturate to 0 .. 8A
        double mA = (double) atoi(argv[2]);
        if (mA > 8000.0)
            return CMD_INVALID_ARGUMENTS;
        if (mA < 0.0)
            return CMD_INVALID_ARGUMENTS;

        // Pull out mHz argument
        // and saturate to 0 .. 10Hz
        double mHz = (double) atoi(argv[3]);
        if (mHz > 10000.0)
            return CMD_INVALID_ARGUMENTS;
        if (mHz < 0.0)
            return CMD_INVALID_ARGUMENTS;

        double Hz = mHz / 1000;
        double A = mA / 1000;

        task_dtc_set_I_star(A, Hz);
        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/user/usr/beta_labs/bemfo.h<|end_filename|>
#ifndef BEMFO_H
#define BEMFO_H

void bemfo_init(void);
void bemfo_update(double Esal_alpha, double Esal_beta, double Te_hat);

void bemfo_start(void);

double bemfo_get_omega_m_hat(void);
double bemfo_get_omega_e_hat(void);
double bemfo_get_theta_e_hat(void);

#endif // BEMFO_H


<|start_filename|>sdk/bare/common/drv/analog.c<|end_filename|>
#include "drv/analog.h"
#include "drv/analog_defs.h"
#include "sys/defines.h"
#include <stdbool.h>

static volatile uint32_t *m_adc;

void analog_init(uint32_t base_addr)
{
    // Store base address for IP
    m_adc = (volatile uint32_t *) base_addr;

    // Set SCK to 50MHz (200MHz / 4)
    analog_set_clkdiv(ANALOG_CLKDIV4);

    // Set PWM sync to both high and low of triangle carrier
    bool sync_to_carrier_high = true;
    bool sync_to_carrier_low = true;
    analog_set_pwm_sync(sync_to_carrier_high, sync_to_carrier_low);
}

int analog_set_clkdiv(analog_clkdiv_e div)
{
    // Make sure the divisor is valid
    if (!analog_is_valid_clkdiv(div)) {
        return FAILURE;
    }

    // Read in CONTROL register
    uint32_t reg = m_adc[ANALOG_DEFS_OFFSET_CONTROL / 4];

    // Clear lower 2 bits
    reg &= ~ANALOG_DEFS_CONTROL_CLKDIV_MASK;

    // Set lower 2 bits
    reg |= (div & ANALOG_DEFS_CONTROL_CLKDIV_MASK);

    // Write out CONTROL register
    m_adc[ANALOG_DEFS_OFFSET_CONTROL / 4] = reg;

    return SUCCESS;
}

void analog_get_clkdiv(analog_clkdiv_e *out_div)
{
    // Read in CONTROL register
    uint32_t reg = m_adc[ANALOG_DEFS_OFFSET_CONTROL / 4];

    // Make sure we only look at lower 2 bits
    uint32_t value = reg & ANALOG_DEFS_CONTROL_CLKDIV_MASK;

    *out_div = value;
}

int analog_getf(analog_channel_e channel, float *out_value)
{
    // Read raw binary value from ADC
    int16_t val;
    if (analog_geti(channel, &val) != SUCCESS) {
        return FAILURE;
    }

    // Conversion from raw bits to voltage
    *out_value = (float) (val) / 400.0;

    return SUCCESS;
}

int analog_geti(analog_channel_e channel, int16_t *out_value)
{
    // Make sure channel in valid
    if (!analog_is_valid_channel(channel)) {
        return FAILURE;
    }

    // Read in ADC data register
    uint32_t reg = m_adc[channel];

    *out_value = (int16_t) reg;

    return SUCCESS;
}

void analog_set_pwm_sync(bool sync_to_carrier_high, bool sync_to_carrier_low)
{
    // Read in CONTROL register
    uint32_t reg = m_adc[ANALOG_DEFS_OFFSET_CONTROL / 4];

    if (sync_to_carrier_high) {
        reg |= (1 << ANALOG_DEFS_CONTROL_PWM_SYNC_HIGH_SHIFT);
    } else {
        reg &= ~(1 << ANALOG_DEFS_CONTROL_PWM_SYNC_HIGH_SHIFT);
    }

    if (sync_to_carrier_low) {
        reg |= (1 << ANALOG_DEFS_CONTROL_PWM_SYNC_LOW_SHIFT);
    } else {
        reg &= ~(1 << ANALOG_DEFS_CONTROL_PWM_SYNC_LOW_SHIFT);
    }

    // Write out CONTROL register
    m_adc[ANALOG_DEFS_OFFSET_CONTROL / 4] = reg;
}


<|start_filename|>sdk/bare/common/drv/eddy_current_sensor.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#include "drv/eddy_current_sensor.h"
#include "xil_io.h"
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>

#define EDDY_CURRENT_SENSOR_BASE_ADDR (0x43C80000)

void eddy_current_sensor_init(void)
{
    printf("EDDY CURRENT SENSOR:\tInitializing...\n");

    // Set sampling rate to 20kHz
    eddy_current_sensor_set_sample_rate(20000);
}

void eddy_current_sensor_enable(void)
{
    Xil_Out32(EDDY_CURRENT_SENSOR_BASE_ADDR + (3 * sizeof(uint32_t)), 1);
}

void eddy_current_sensor_disable(void)
{
    Xil_Out32(EDDY_CURRENT_SENSOR_BASE_ADDR + (3 * sizeof(uint32_t)), 0);
}

void eddy_current_sensor_set_sample_rate(double sample_rate)
{
    uint8_t divider = (uint8_t)(500000 / sample_rate);

    eddy_current_sensor_set_divider(divider - 1);
}

void eddy_current_sensor_set_divider(uint8_t divider)
{
    Xil_Out32(EDDY_CURRENT_SENSOR_BASE_ADDR + (2 * sizeof(uint32_t)), divider);
}

static double bits_to_voltage(uint32_t data)
{
    bool is_negative = 0x20000 & data;

    // Convert 2's compliment to positive data
    if (is_negative) {
        data = ~data;
        data += 1;
    }

    // Convert data to voltage (+/-5V)
    double resolution = 0.00003814697;              // 5V / 2^17
    double voltage = (0x1FFFF & data) * resolution; // 17-bit data

    if (is_negative) {
        voltage = -voltage;
    }

    return voltage;
}

double eddy_current_sensor_read_x_voltage(void)
{
    uint32_t x_data = Xil_In32(EDDY_CURRENT_SENSOR_BASE_ADDR);

    return bits_to_voltage(x_data);
}

double eddy_current_sensor_read_y_voltage(void)
{
    uint32_t y_data = Xil_In32(EDDY_CURRENT_SENSOR_BASE_ADDR + (1 * sizeof(uint32_t)));

    return bits_to_voltage(y_data);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/user/usr/beta_labs/task_vsi.h<|end_filename|>
#ifndef TASK_VSI_H
#define TASK_VSI_H

#include "sys/scheduler.h"
#include <stdint.h>

#define TASK_VSI_UPDATES_PER_SEC (10000)
#define TASK_VSI_INTERVAL_USEC   (USEC_IN_SEC / TASK_VSI_UPDATES_PER_SEC)

uint8_t task_vsi_is_inited(void);
void task_vsi_init(void);
void task_vsi_deinit(void);
void task_vsi_callback(void *arg);

void task_vsi_set_legs(int phAIdx, int phBIdx, int phCIdx);
void task_vsi_set(int vPercent, int freq, double ramptime);

#endif // TASK_VSI_H


<|start_filename|>sdk/basic/src/control.h<|end_filename|>
/*
 * control.h
 *
 *  Created on: Jun 24, 2014
 *      Author: sever212
 */

#ifndef CONTROL_H_
#define CONTROL_H_

typedef enum {C_IDLE, C_READY, C_RUNNING} controller_status;
typedef enum {C_PI, C_PID} controller_type;

typedef struct controller_context{
	double *inputs;	//reference - measured value of quantity being controlled
	double *outputs;	//output of the pi controller
	u32 NumInputs;	//number of inputs
	u32	NumOutputs;	//number of outputs
	controller_status Status;	//the status of the controller
	u32 NumClocks;	//run this controller every NumClocks count (1 for every time)
	u32 ClockTime;	//duration (in microseconds) of a clock edge
	u32 clockCount;	//current clock count
	controller_type Type;		//the type of controller (PI, PID)
} controller_context;

typedef void (*get_controller_inputs)(void *arg, struct controller_context *cnt);
typedef void (*take_controller_output)(void *arg, struct controller_context *cnt);

typedef struct pid_controller{
	double kp;		//proportional gain
	double kits;	//integral gain * sample time
	double kdn;		//derivative gain for current error: derivative difference EQ: D = kdn*E[n] + kdn1*E[n-1] + kdm1*D[n-1]
	double kdn1;	//derivative gain for previous error
	double kdm1;	//derivative gain for previous derivative
	double lastInt;	//value of last integral
	double lastErr;	//value of the last error
	double lastD;	//value of the last derivative output
	double plusSat;	//positive integral saturation limit
	double negSat;	//negative integral saturation limit
} pid_controller;

typedef struct pi_controller{
	double kp;		//proportional gain
	double kits;	//integral gain * sample time
	double lastInt;	//value of last integral
	double plusSat;	//positive integral saturation limit
	double negSat;	//negative integral saturation limit
} pi_controller;

typedef struct controller_holder{
	void *controller_info;					//specific to the type of controller
	struct controller_context *control_io;			//the io of the controller (general to all controllers)
	get_controller_inputs input_callback;	//function to call to get measured and reference values
	take_controller_output output_callback;	//function to call to take the controller output
	void *arg_in;	//argument to pass input_callback when called
	void *arg_out;	//argument to pass output_callback when called
} controller_holder;

typedef struct controller_linked_list_item
{
	struct controller_holder *context;
	struct controller_linked_list_item *next;
	struct controller_linked_list_item *prev;
	u8 bRegistered;		//set to true when added to the list, false when removed
} controller_linked_list_item;

extern volatile u32 ControlTimer0ErrorCount;
extern volatile u32 ControlTimer1ErrorCount;

extern int InitControllers(XScuGic* IntcInstancePtr);
//extern int InitControlTimers(XScuGic* IntcInstancePtr);
extern void RunPI_Controller (controller_holder* ctxt);
extern void RunPID_Controller (controller_holder* ctxt);
extern int UnregisterController(int contNum, struct controller_linked_list_item *cnt);
extern int RegisterController(int contNum, struct controller_linked_list_item *cnt);

#endif /* CONTROL_H_ */


<|start_filename|>sdk/bare/common/drv/analog.h<|end_filename|>
#ifndef ANALOG_H
#define ANALOG_H

#include "drv/hardware_targets.h"
#include "usr/user_config.h"
#include <stdbool.h>
#include <stdint.h>

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
#define ANALOG_BASE_ADDR (0x43C00000)
#endif

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
#define ANALOG_BASE_ADDR (0x43C00000)
#endif

typedef enum {
    // Keep first channel index at 0!
    ANALOG_IN1 = 0,
    ANALOG_IN2,
    ANALOG_IN3,
    ANALOG_IN4,
    ANALOG_IN5,
    ANALOG_IN6,
    ANALOG_IN7,
    ANALOG_IN8,

// REV C target hardware is the only platform
// which supports more than 8 analog inputs.
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
    ANALOG_IN9,
    ANALOG_IN10,
    ANALOG_IN11,
    ANALOG_IN12,
    ANALOG_IN13,
    ANALOG_IN14,
    ANALOG_IN15,
    ANALOG_IN16,
#endif // USER_CONFIG_HARDWARE_TARGET

    // Keep this as last entry!
    ANALOG_NUM_CHANNELS,
} analog_channel_e;

typedef enum {
    ANALOG_CLKDIV2 = 0,
    ANALOG_CLKDIV4,
    ANALOG_CLKDIV8,
    ANALOG_CLKDIV16,
} analog_clkdiv_e;

static inline bool analog_is_valid_channel(analog_channel_e channel)
{
    if (channel >= ANALOG_IN1 && channel < ANALOG_NUM_CHANNELS) {
        return true;
    }

    return false;
}

static inline bool analog_is_valid_clkdiv(analog_clkdiv_e div)
{
    if (div == ANALOG_CLKDIV2 || div == ANALOG_CLKDIV4 || div == ANALOG_CLKDIV8 || div == ANALOG_CLKDIV16) {
        return true;
    }

    return false;
}

void analog_init(uint32_t base_addr);

int analog_set_clkdiv(analog_clkdiv_e div);
void analog_get_clkdiv(analog_clkdiv_e *out_div);

int analog_getf(analog_channel_e channel, float *out_value);
int analog_geti(analog_channel_e channel, int16_t *out_value);

void analog_set_pwm_sync(bool sync_to_carrier_high, bool sync_to_carrier_low);

// TODO(NP): Implement digital low-pass filtering in FPGA,
//           then, implement C driver which looks like this:
// void analog_set_filter(analog_channel_e channel, ...);

#endif // ANALOG_H


<|start_filename|>sdk/bare/common/drv/sts_mux.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#include "drv/sts_mux.h"
#include "xil_io.h"
#include <stdint.h>
#include <stdio.h>

#define STS_MUX_BASE_ADDR (0x43C50000)

static int ports[] = {
    INVERTER_PORT1, INVERTER_PORT2, INVERTER_PORT3, INVERTER_PORT4,
    INVERTER_PORT5, INVERTER_PORT6, INVERTER_PORT7, INVERTER_PORT8,
};

void sts_mux_init(void)
{
    printf("STATUS LINES:\tInitializing...\n");

    // Set status line direction according to direction defined by the macros in the header file
    sts_mux_set_output(STS_MUX_A, STS_MUX_A_DIR);
    sts_mux_set_output(STS_MUX_B, STS_MUX_B_DIR);
    sts_mux_set_output(STS_MUX_C, STS_MUX_C_DIR);
    sts_mux_set_output(STS_MUX_D, STS_MUX_D_DIR);

    for (int i = 0; i < 8; i++) {
        if (ports[i]) {
            // Map the port status lines to the corresponding device defined in the header file
            sts_mux_set_device(i, ports[i]);
        }
    }
}

void sts_mux_set_line(uint8_t port, sts_mux_device_t device, sts_mux_line_t line)
{
    Xil_Out32(STS_MUX_BASE_ADDR + ((port + (line * 8)) * sizeof(uint32_t)), device);
}

void sts_mux_set_output(sts_mux_line_t line, bool output)
{
    int dir = output ? 1 : 0;
    Xil_Out32(STS_MUX_BASE_ADDR + ((32 + line) * sizeof(uint32_t)), dir);
}

void sts_mux_set_device(uint8_t port, sts_mux_device_t device)
{
    sts_mux_set_line(port, device, STS_MUX_A);
    sts_mux_set_line(port, device, STS_MUX_B);
    sts_mux_set_line(port, device, STS_MUX_C);
    sts_mux_set_line(port, device, STS_MUX_D);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/drv/uart.h<|end_filename|>
#ifndef UART_H
#define UART_H

#include <stdint.h>

#define UART_RX_FIFO_LENGTH (64)
#define UART_TX_FIFO_LENGTH (64)

int uart_init(void);

int uart_send(char *msg, int len);
int uart_recv(char *msg, int len);

#endif // UART_H


<|start_filename|>sdk/bare/common/drv/eddy_current_sensor.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#ifndef eddy_current_sensor_H
#define eddy_current_sensor_H

#include <stdbool.h>
#include <stdint.h>

void eddy_current_sensor_init(void);

void eddy_current_sensor_enable(void);

void eddy_current_sensor_disable(void);

void eddy_current_sensor_set_sample_rate(double);

void eddy_current_sensor_set_divider(uint8_t);

double eddy_current_sensor_read_x_voltage(void);

double eddy_current_sensor_read_y_voltage(void);

#endif // EDDY_CURRENT_SENSOR_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/basic/src/ControlApp.c<|end_filename|>
/*
 * ControlApp.c
 *
 * This file houses the control application
 *  Created on: Jun 24, 2014
 *      Author: sever212
 */

#include "project_include.h"

typedef struct c_pi_house
{
	pi_controller pi;
	controller_context io;
	controller_holder ctxt;
	double input;
	double output;
	double reference;
	double adc_scale;		//3 amps per volt, wound around twice gives me a factor of 3/2
} c_pi_house;

c_pi_house PI_Controllers[NUM_PI_CONTROLLERS];
controller_linked_list_item cont_items [NUM_PI_CONTROLLERS];

//Logging
u32 Leg1_d1;
u32 Leg1_d2;
s32 Leg1_error;
s32 Leg1_output;
s32 Leg1_reference = 0;
s32 Leg1_ADC;
s32 Leg1_Current;

static void HBridgeCurrent_Input(void *arg, struct controller_context *cnt);
static void HBridgeCurrent_Output(void *arg, struct controller_context *cnt);

//#define HB8L
//#define VSI_DQ_SVPWM
#define PARALLEL_SUSPENSION

int ControlStopAppCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
#if defined(HB8L)
	return HBridge_CC_8Leg_StopCommand(szCmd, szResponse, CommDevice);
#elif defined(VSI_DQ_SVPWM)
	return VSI_DQ_VSPW_StopCommand(szCmd, szResponse, CommDevice);
#elif defined (PARALLEL_SUSPENSION)
	return PosContTry1_StopCommand(szCmd, szResponse, CommDevice);
#else
	int i;
	for (i = 0; i< NUM_PI_CONTROLLERS; i++)
		UnregisterPIController(0, &cont_items[i]);

	strcpy(szResponse, "OK");
	return strlen(szResponse);
#endif
}

int ControlStartAppCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
#if defined(HB8L)
	return HBridge_CC_8Leg_StartCommand(szCmd, szResponse, CommDevice);
#elif defined(VSI_DQ_SVPWM)
	return VSI_DQ_VSPW_StartCommand(szCmd, szResponse, CommDevice);
#elif defined (PARALLEL_SUSPENSION)
	return PosContTry1_StartCommand(szCmd, szResponse, CommDevice);
#else
	InitControlApp();
	strcpy(szResponse, "OK");
	return strlen(szResponse);
#endif
}

int ControlAppCommand(const char * szCmd, char *szResponse, void *CommDevice)
{
#if defined(HB8L)
	return HBridge_CC_8Leg_Command(szCmd, szResponse, CommDevice);
#elif defined (VSI_DQ_SVPWM)
	return VSI_DQ_VSPW_Command(szCmd, szResponse, CommDevice);
#elif defined (PARALLEL_SUSPENSION)
	return PosContTry1_Command(szCmd, szResponse, CommDevice);
#else
	s32 current = atoi(szCmd);
	s32 *L1R = &Leg1_reference;
	PI_Controllers[0].reference = ((double) current)/128;
	strcpy(szResponse, "OK");
	*L1R = current;
	return strlen(szResponse);
#endif
}

#if !defined(HB8L) && !defined(VSI_DQ_SVPWM) && !defined(PARALLEL_SUSPENSION)
int InitControlApp()
{
	/*int i;
	for(i = 0; i<NUM_PI_CONTROLLERS; i++)
	{
		PI_Controllers[i].pi
	}*/

	//PI controller
	PI_Controllers[0].pi.kits = 0.005;
	PI_Controllers[0].pi.kp = 0.452;
	PI_Controllers[0].pi.lastInt = 0;
	PI_Controllers[0].pi.plusSat = 0.9;
	PI_Controllers[0].pi.negSat = -0.9;

	//basic
	PI_Controllers[0].io.inputs = &PI_Controllers[0].input;
	PI_Controllers[0].io.outputs = &PI_Controllers[0].output;
	PI_Controllers[0].io.NumInputs = 1;
	PI_Controllers[0].io.NumOutputs = 1;
	PI_Controllers[0].io.NumClocks = 1;
	PI_Controllers[0].io.ClockTime = 10; //10 us
	PI_Controllers[0].io.clockCount = 0;
	PI_Controllers[0].io.Type = C_PI;
	PI_Controllers[0].io.Status = C_RUNNING;

	//context
	PI_Controllers[0].ctxt.arg_in = 0;
	PI_Controllers[0].ctxt.arg_out = 0;
	PI_Controllers[0].ctxt.control_io = &PI_Controllers[0].io;
	PI_Controllers[0].ctxt.controller_info = &PI_Controllers[0].pi;
	PI_Controllers[0].ctxt.input_callback = HBridgeCurrent_Input;
	PI_Controllers[0].ctxt.output_callback = HBridgeCurrent_Output;

	PI_Controllers[0].adc_scale = 1;  //new: 3 amps per volt, wound around three times gives me a factor of 1 --OLD:3 amps per volt, wound around twice gives me a factor of 3/2
	PI_Controllers[0].reference = 0;

	//load it:
	cont_items[0].context = &PI_Controllers[0].ctxt;
	RegisterPIController(0, &cont_items[0]);
	return XST_SUCCESS;
}


static void HBridgeCurrent_Input(void *arg, struct controller_context *cnt)
{
	//calculate the error and store it
	PI_Controllers[0].input = PI_Controllers[0].reference - PI_Controllers[0].adc_scale*ReadADC(1);
	Leg1_error = (s32) (PI_Controllers[0].input*128);
	Leg1_ADC = (s32) (ReadADC(1)*128);
	Leg1_Current =  (s32) (PI_Controllers[0].adc_scale*ReadADC(1)*128);
}

static void HBridgeCurrent_Output(void *arg, struct controller_context *cnt)
{
	u8 duty1 = (unsigned char) 127*(1 + PI_Controllers[0].output);
	u8 duty2 = (unsigned char) 127*(1 - PI_Controllers[0].output);

	//logging
	Leg1_d1 = duty1;
	Leg1_d2 = duty2;
	Leg1_output = (s32) (PI_Controllers[0].output*128);

	//plus voltage
	WriteDutyRatio(1, duty1 );
	//minus voltage
	WriteDutyRatio(2, duty2 );
}
#endif






<|start_filename|>sdk/bare/user/usr/beta_labs/task_mo.h<|end_filename|>
#ifndef TASK_MO_H
#define TASK_MO_H

#include "sys/scheduler.h"

#define TASK_MO_UPDATES_PER_SEC (10000)
#define TASK_MO_INTERVAL_USEC   (USEC_IN_SEC / TASK_MO_UPDATES_PER_SEC)

void task_mo_init(void);
void task_mo_callback(void *arg);

double task_mo_get_omega_e(void);
double task_mo_get_omega_m(void);

#endif // TASK_MO_H


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_bemfo.h<|end_filename|>
#ifndef CMD_BEMFO_H
#define CMD_BEMFO_H

void cmd_bemfo_register(void);

int cmd_bemfo(int argc, char **argv);

#endif // CMD_BEMFO_H


<|start_filename|>sdk/bare/common/sys/crc32.h<|end_filename|>
// Code generated by universal_crc by <NAME>
// Website: http://www.mcgougan.se/universal_crc/
//
// Run the C script using the following arguments:
// ./universal_crc -b 32 -p 0x04c11db7 -i 0xffffffff -x 0xffffffff -r -a tab
//
// CRC parameters used:
//   bits:       32
//   poly:       0x04c11db7
//   init:       0xffffffff
//   xor:        0xffffffff
//   reverse:    true
//   non-direct: false
//
// CRC of the string "123456789" is 0xcbf43926

#ifndef CRC32_H
#define CRC32_H

#include <stddef.h>
#include <stdint.h>

#define CRC32_DEFAULT_INIT (0xffffffff)

// Perform full CRC-32 calculation (matches Python zlib.crc32 or binascii.crc32)
uint32_t crc32_calc_full(const uint8_t *data, size_t len);

// Let user pass in initial value for CRC (perform CRC in chunks)
// NOTE: User must invert final output!
uint32_t crc32_calc_part(const uint8_t *data, size_t len, uint32_t init);

#endif // CRC32_H


<|start_filename|>sdk/bare/common/usr/dac/cmd/cmd_dac.c<|end_filename|>
#ifdef APP_DAC

#include "usr/dac/cmd/cmd_dac.h"
#include "sys/commands.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include "usr/dac/task_dac.h"
#include <stdlib.h>
#include <string.h>

// Stores command entry for command system module
static command_entry_t cmd_entry;

// Defines help content displayed for this command
// when user types "help" at command prompt
static command_help_t cmd_help[] = {
    { "init", "Start task" },
    { "deinit", "Stop task" },
    { "freq <frequency(Hz)>", "Set Frequency" },
    { "ch <channel> <voltage>", "Set channel to voltage" },
    { "reg <register> <value(in hex)>", "Write value to register" },
    { "trigger", "Trigger synchronous channels" },
    { "broadcast <voltage>", "Broadcast voltage" },
};

void cmd_dac_register(void)
{
    // Populate the command entry block
    //
    // Here is where you define the base command string: "dac"
    // and what function is called to handle command
    commands_cmd_init(&cmd_entry, "dac", "DAC application commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_dac);

    // Register the command with the system
    commands_cmd_register(&cmd_entry);
}

int cmd_dac(int argc, char **argv)
{
    // This function is called every time the user types "dac"
    // followed by a space and any number of characters.

    // Handle 'init' sub-command
    //
    // First, verify correct number of arguments (2)
    // Second, verify second argument is "init"
    if (argc == 2 && strcmp("init", argv[1]) == 0) {
        if (task_dac_init() != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'deinit' sub-command
    //
    // First, verify correct number of arguments (2)
    // Second, verify second argument is "deinit"
    if (argc == 2 && strcmp("deinit", argv[1]) == 0) {
        if (task_dac_deinit() != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'freq' sub-command
    //
    // First, verify correct number of arguments (3)
    // Second, verify second argument is "freq"
    // Third, scan for a double
    if (argc == 3 && strcmp("freq", argv[1]) == 0) {
        double frequency;
        sscanf(argv[2], "%lf", &frequency);
        task_dac_frequency(frequency);
        return CMD_SUCCESS;
    }

    // Handle 'broadcast' sub-command
    //
    // First, verify correct number of arguments (3)
    // Second, verify second argument is "broadcast"
    // Third, scan for a double
    if (argc == 3 && strcmp("broadcast", argv[1]) == 0) {
        double voltage;
        sscanf(argv[2], "%lf", &voltage);

        if (abs(voltage) > 10)
            return CMD_INVALID_ARGUMENTS;

        task_dac_broadcast(voltage);
        return CMD_SUCCESS;
    }

    // Handle 'trigger' sub-command
    //
    // First, verify correct number of arguments (2)
    // Second, verify second argument is "trigger"
    if (argc == 2 && strcmp("trigger", argv[1]) == 0) {
        task_dac_trigger();
        return CMD_SUCCESS;
    }

    // Handle 'ch' sub-command
    //
    // First, verify correct number of arguments (4)
    // Second, verify second argument is "ch"
    // Third, scan for an int
    // Fourth, scan for a double
    if (argc == 4 && strcmp("ch", argv[1]) == 0) {
        int ch;
        sscanf(argv[2], "%i", &ch);

        if (ch < 0 || ch > 7)
            return CMD_INVALID_ARGUMENTS;

        double voltage;
        sscanf(argv[3], "%lf", &voltage);

        if (abs(voltage) > 10)
            return CMD_INVALID_ARGUMENTS;

        task_dac_set_voltage((uint8_t) ch, voltage);

        return CMD_SUCCESS;
    }

    // Handle 'reg' sub-command
    //
    // First, verify correct number of arguments (4)
    // Second, verify second argument is "reg"
    // Third, scan for an int
    // Fourth, scan for a hex string (uint32_t)
    if (argc == 4 && strcmp("reg", argv[1]) == 0) {
        int reg;
        sscanf(argv[2], "%i", &reg);

        if (reg < 0 || reg > 15)
            return CMD_INVALID_ARGUMENTS;

        uint32_t value;
        sscanf(argv[3], "%lX", &value);

        task_dac_set_reg((uint8_t) reg, value);

        return CMD_SUCCESS;
    }

    // At any point, if an error is detected in given input command,
    // simply return an error code (defined in sys/defines.h)
    //
    // The return statement below is used to catch all user input which
    // didn't match the if statements above. In general, to handle commands,
    // assume they are invalid. Only after checking if each argument is
    // valid should you trust them.
    //
    // Common error return values are: FAILURE, INVALID_ARGUMENTS
    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BLINK


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_dtc.h<|end_filename|>
#ifndef CMD_DTC_H
#define CMD_DTC_H

void cmd_dtc_register(void);

int cmd_dtc(int argc, char **argv);

#endif // CMD_DTC_H


<|start_filename|>sdk/bare/common/sys/cmd/cmd_inj.h<|end_filename|>
#ifndef CMD_INJ_H
#define CMD_INJ_H

void cmd_inj_register(void);

int cmd_inj(int argc, char **argv);

#endif // CMD_INJ_H


<|start_filename|>sdk/basic/src/Log.h<|end_filename|>

#ifndef __LOG_H
#define __LOG_H

#define MAX_LOG_VARS 14
#define LOG_LENGTH (100000 + 1) //+1 is for empty position of circular buffer

typedef enum {L_IDLE, L_READY, L_LOGGING, L_DONE} LogStatus_t;

//FIFO settings:
#define FIFO_PREVENT_OVERRUN 1 //set this to prevent the log from overwriting old data (error will be flagged)
#define FIFO_RUN_ONCE 2		//set this to let the log fill up once and then stop

// FIFO flags:
#define FIFO_OVERRUN_OCCURED 1

typedef struct log_entry {
	u32 LogEntry[LOG_LENGTH];
	u32 InPos;
	u32 OutPos;
	u32 Settings;
	u32 Flags;
	u32 *VarToLog;
	LogStatus_t LogStatus;
} log_entry;


extern int InitLog();
extern int ReadFromLog(unsigned char *output, u32 LogIndex, u32 Length);
extern int DetermineMostFullLog(u32 *LogIndex);
extern int ReadFromMostFullLog(unsigned char *output, u32 *LogIndex, u32 Length);
extern int ReadLogFlags(u32 *flags, u32 LogIndex);
extern u32 LogDataAvail(u32 LogIndex);
extern int SetupLogVariable(u32 LogIndex, u32 *Variable, u32 Settings);
extern int StartLogging();
extern int StopAllLogging();
extern int DoLogging();

#endif //__LOG_H



<|start_filename|>sdk/basic/bsp/bsp.c<|end_filename|>
#include "bsp.h"
#include <stdio.h>

void bsp_init(void)
{
	printf("BSP:\tInitializing...\n");

    encoder_init();
    analog_init();
    pwm_init();
    io_init();
    gpio_init();
}


<|start_filename|>sdk/bare/common/drv/cpu_timer.c<|end_filename|>
#include "drv/cpu_timer.h"
#include "sys/defines.h"
//#include "xparameters.h"
//#include "xtmrctr.h"

// Code from StackOverflow post:
// https://stackoverflow.com/questions/3247373/3250835#3250835

static inline void init_perfcounters(int32_t do_reset, int32_t enable_divider);

static const double CPU_CLOCK_FREQ_MHZ = 666.666;
static const double CPU_CLOCK_FREQ_HZ = CPU_CLOCK_FREQ_MHZ * 1e6;

void cpu_timer_init(void)
{
    /* enable user-mode access to the performance counter*/
    asm("MCR p15, 0, %0, C9, C14, 0\n\t" ::"r"(1));

    /* disable counter overflow interrupts (just in case)*/
    asm("MCR p15, 0, %0, C9, C14, 2\n\t" ::"r"(0x8000000f));

    init_perfcounters(1, 0);
}

uint32_t cpu_timer_now(void)
{
    uint32_t value;
    // Read CCNT Register
    asm volatile("MRC p15, 0, %0, c9, c13, 0\t\n" : "=r"(value));
    return value;
}

double cpu_timer_ticks_to_usec(uint32_t ticks)
{
    double usec = (double) ticks / CPU_CLOCK_FREQ_MHZ;
    return usec;
}

double cpu_timer_ticks_to_sec(uint32_t ticks)
{
    double sec = (double) ticks / CPU_CLOCK_FREQ_HZ;
    return sec;
}

static inline void init_perfcounters(int32_t do_reset, int32_t enable_divider)
{
    // in general enable all counters (including cycle counter)
    int32_t value = 1;

    // peform reset:
    if (do_reset) {
        value |= 2; // reset all counters to zero.
        value |= 4; // reset cycle counter to zero.
    }

    if (enable_divider)
        value |= 8; // enable "by 64" divider for CCNT.

    value |= 16;

    // program the performance-counter control-register:
    asm volatile("MCR p15, 0, %0, c9, c12, 0\t\n" ::"r"(value));

    // enable all counters:
    asm volatile("MCR p15, 0, %0, c9, c12, 1\t\n" ::"r"(0x8000000f));

    // clear overflows:
    asm volatile("MCR p15, 0, %0, c9, c12, 3\t\n" ::"r"(0x8000000f));
}


<|start_filename|>sdk/bare/user/usr/beta_labs/mcff.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/mcff.h"
#include "usr/beta_labs/machine.h"

// Output States
static double Te_cff_jp = 0.0;
static double Te_cff_bp = 0.0;
static double Te_cff_total = 0.0;

void mcff_update(double omega_m_star, double omega_dot_m_star)
{
    Te_cff_jp = Jp_HAT * omega_dot_m_star;
    Te_cff_bp = Bp_HAT * omega_m_star;
    Te_cff_total = Te_cff_jp + Te_cff_bp;
}

double mcff_get_Te_cff_total(void)
{
    return Te_cff_total;
}

double mcff_get_Te_cff_bp(void)
{
    return Te_cff_bp;
}

double mcff_get_Te_cff_jp(void)
{
    return Te_cff_jp;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/usr/pcbtest/app_pcbtest.c<|end_filename|>
#ifdef APP_PCBTEST

#include "usr/pcbtest/app_pcbtest.h"
#include "drv/pwm.h"
#include "usr/pcbtest/cmd/cmd_test.h"

void app_pcbtest_init(void)
{
    cmd_test_register();
}

#endif // APP_PCBTEST


<|start_filename|>sdk/bare/common/usr/demo/inverter.c<|end_filename|>
#ifdef APP_DEMO

#include "usr/demo/inverter.h"
#include "drv/hardware_targets.h"
#include "drv/io.h"
#include "drv/pwm.h"
#include "usr/user_config.h"
#include <math.h>

typedef struct inverter_ctx_t {
    double Vdc;
} inverter_ctx_t;

#define MAX_NUM_INVERTER_CTXS (8)
static inverter_ctx_t inverter_ctxs[MAX_NUM_INVERTER_CTXS] = { 0 };

inline static int saturate(double min, double max, double *value)
{
    if (*value < min) {
        // Lower bound saturation
        *value = min;
        return -1;
    } else if (*value > max) {
        // Upper bound saturation
        *value = max;
        return 1;
    } else {
        // No saturation
        return 0;
    }
}

void inverter_saturate_to_Vdc(int inv_idx, double *phase_voltage)
{
    if (inv_idx < 0 || inv_idx >= MAX_NUM_INVERTER_CTXS) {
        return;
    }

    inverter_ctx_t *ctx = &inverter_ctxs[inv_idx];

    // Enforce voltage is positive so we can divide by it... User should have set this up (i.e. > 1V)
    if (ctx->Vdc < 1.0) {
        ctx->Vdc = 1.0;
    }

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
    io_led_color_t color = { 0, 0, 0 };
    if (saturate(-ctx->Vdc / 2.0, ctx->Vdc / 2.0, phase_voltage) != 0)
        color.g = 255;
    io_led_set_c(0, 1, 0, &color);
#endif // USER_CONFIG_HARDWARE_TARGET
}

void inverter_set_voltage(int inv_idx, uint8_t pwm_idx, double phase_voltage)
{
    if (inv_idx < 0 || inv_idx >= MAX_NUM_INVERTER_CTXS) {
        return;
    }

    inverter_ctx_t *ctx = &inverter_ctxs[inv_idx];

    // Enforce voltage is positive so we can divide by it... User should have set this up (i.e. > 1V)
    if (ctx->Vdc < 1.0) {
        ctx->Vdc = 1.0;
    }

    double duty = 0.5 + (phase_voltage / ctx->Vdc);
    pwm_set_duty(pwm_idx, duty);
}

void inverter_set_Vdc(int inv_idx, double Vdc)
{
    if (inv_idx < 0 || inv_idx >= MAX_NUM_INVERTER_CTXS) {
        return;
    }

    inverter_ctx_t *ctx = &inverter_ctxs[inv_idx];

    ctx->Vdc = Vdc;
}

double inverter_get_Vdc(int inv_idx)
{
    if (inv_idx < 0 || inv_idx >= MAX_NUM_INVERTER_CTXS) {
        return -1.0;
    }

    inverter_ctx_t *ctx = &inverter_ctxs[inv_idx];

    return ctx->Vdc;
}

#endif // APP_DEMO


<|start_filename|>sdk/bare/user/usr/beta_labs/app_beta_labs.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/app_beta_labs.h"
#include "usr/beta_labs/cmd/cmd_bemfo.h"
#include "usr/beta_labs/cmd/cmd_cc.h"
#include "usr/beta_labs/cmd/cmd_dtc.h"
#include "usr/beta_labs/cmd/cmd_inv.h"
#include "usr/beta_labs/cmd/cmd_mc.h"
#include "usr/beta_labs/cmd/cmd_vsi.h"
#include "usr/beta_labs/task_mo.h"

void app_beta_labs_init(void)
{
    task_mo_init();

    cmd_inv_register();
    cmd_dtc_register();
    cmd_vsi_register();
    cmd_cc_register();
    cmd_mc_register();
    cmd_bemfo_register();
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/user/usr/blink/cmd/cmd_blink.c<|end_filename|>
#ifdef APP_BLINK

#include "usr/blink/cmd/cmd_blink.h"
#include "sys/commands.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include "usr/blink/task_blink.h"
#include <stdlib.h>
#include <string.h>

// Stores command entry for command system module
static command_entry_t cmd_entry;

// Defines help content displayed for this command
// when user types "help" at command prompt
static command_help_t cmd_help[] = {
    { "hello <name>", "Print hello to screen" },
    { "init", "Start task" },
    { "deinit", "Stop task" },
    { "stats print", "Print stats" },
    { "stats reset", "Reset stats" },
};

void cmd_blink_register(void)
{
    // Populate the command entry block
    //
    // Here is where you define the base command string: "blink"
    // and what function is called to handle command
    commands_cmd_init(&cmd_entry, "blink", "Blink application commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_blink);

    // Register the command with the system
    commands_cmd_register(&cmd_entry);
}

int cmd_blink(int argc, char **argv)
{
    // This function is called every time the user types "blink"
    // followed by a space and any number of characters.
    //
    // Example user input: blink foo 123
    //
    // It is up to the application developer to handle this
    // incoming command in a manner that reflects this command's
    // help message, as to not confuse the user.
    //
    // The arguments passed into this function (argc & argv)
    // follow standard C convention for main() programs called
    // via command line interface (CLI).
    //
    // 'argc' is the number of CLI arguments, including the base command
    // For above example: argc = 3
    //
    // 'argv' is an array of char strings which contain the CLI arguments
    // For above example:
    // - argv[0] => "blink"
    // - argv[1] => "foo"
    // - argv[2] => "123"
    //
    // NOTE: The system constrains user CLI input to ensure responsive
    // behavior for arbitrary commands. This involves limiting individual
    // command argument length as well as the total number of arguments
    // (this is defined in sys/commands.c)

    // Handle 'hello' sub-command
    //
    // First, verify correct number of arguments (2)
    // Second, verify second argument is "hello"
    if (argc == 3 && strcmp("hello", argv[1]) == 0) {
        // Perform desired action for command...

        // Fun example: hello message based on name
        if (strcmp("nathan", argv[2]) == 0) {
            // Wow, happy to talk to Nathan!
            debug_printf("Welcome, sir Nathan!!!\r\n");
            debug_print("\r\n");

            // Indicate success, but hide SUCCESS message
            return CMD_SUCCESS_QUIET;

        } else if (strcmp("fred", argv[2]) == 0) {
            // We don't want to talk to Fred... :(
            // Treat this case as an invalid command input from user
            return CMD_INVALID_ARGUMENTS;

        } else {
            // Normal hello for anyone else
            debug_printf("Hello, %s\r\n", argv[2]);
            debug_print("\r\n");

            // Indicate success, but hide SUCCESS message
            return CMD_SUCCESS_QUIET;
        }

        // If user typed a valid command, return SUCCESS
        return CMD_SUCCESS;
    }

    if (argc == 2 && strcmp("init", argv[1]) == 0) {
        if (task_blink_init() != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    if (argc == 2 && strcmp("deinit", argv[1]) == 0) {
        if (task_blink_deinit() != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    if (argc >= 2 && strcmp("stats", argv[1]) == 0) {
        if (argc == 3 && strcmp("print", argv[2]) == 0) {
            task_blink_stats_print();
            return CMD_SUCCESS;
        }

        if (argc == 3 && strcmp("reset", argv[2]) == 0) {
            task_blink_stats_reset();
            return CMD_SUCCESS;
        }
    }

    // At any point, if an error is detected in given input command,
    // simply return an error code (defined in sys/defines.h)
    //
    // The return statement below is used to catch all user input which
    // didn't match the if statements above. In general, to handle commands,
    // assume they are invalid. Only after checking if each argument is
    // valid should you trust them.
    //
    // Common error return values are: FAILURE, INVALID_ARGUMENTS
    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BLINK


<|start_filename|>sdk/basic/src/main.h<|end_filename|>
#ifndef __MAIN_H
#define __MAIN_H

extern volatile u32* PECB;
extern u32 Timing100usErrorCount;

extern int EnterProtection();
extern int LeaveProtection(int State);
#endif //__MAIN_H


<|start_filename|>ip_repo/amdc_gpio_mux_1.0/drivers/amdc_gpio_mux_v1_0/src/amdc_gpio_mux.h<|end_filename|>

#ifndef AMDC_GPIO_MUX_H
#define AMDC_GPIO_MUX_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_GPIO_MUX_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG15_OFFSET 60
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG16_OFFSET 64
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG17_OFFSET 68
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG18_OFFSET 72
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG19_OFFSET 76
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG20_OFFSET 80
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG21_OFFSET 84
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG22_OFFSET 88
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG23_OFFSET 92
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG24_OFFSET 96
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG25_OFFSET 100
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG26_OFFSET 104
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG27_OFFSET 108
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG28_OFFSET 112
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG29_OFFSET 116
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG30_OFFSET 120
#define AMDC_GPIO_MUX_S00_AXI_SLV_REG31_OFFSET 124


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_GPIO_MUX register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_GPIO_MUXdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_GPIO_MUX_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_GPIO_MUX_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_GPIO_MUX register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_GPIO_MUX device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_GPIO_MUX_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_GPIO_MUX_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_GPIO_MUX instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_GPIO_MUX_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_GPIO_MUX_H


<|start_filename|>sdk/bare/user/usr/blink/task_blink.h<|end_filename|>
#ifndef TASK_BLINK_H
#define TASK_BLINK_H

#include "sys/scheduler.h"

// Frequency that this task is called (in Hz)
//
// Must be less than or equal to scheduler updates per second
// This value is defined in sys/scheduler.h and defaults to 10kHz.
// Note that it can be overridden via usr/user_config.h
#define TASK_BLINK_UPDATES_PER_SEC (5)

// Microseconds interval between when task is called
//
// This is what scheduler actually uses to run task,
// but is generated via define above
#define TASK_BLINK_INTERVAL_USEC (USEC_IN_SEC / TASK_BLINK_UPDATES_PER_SEC)

// Called in app init function to set up task (or via command)
int task_blink_init(void);
int task_blink_deinit(void);

// Callback function which scheduler calls periodically
void task_blink_callback(void *arg);

// Print the statistics gathered by the scheduler
void task_blink_stats_print(void);

// Reset the statistics gathered by the scheduler
void task_blink_stats_reset(void);

#endif // TASK_BLINK_H


<|start_filename|>sdk/bare/user/usr/blink/app_blink.c<|end_filename|>
#ifdef APP_BLINK

#include "usr/blink/app_blink.h"
#include "usr/blink/cmd/cmd_blink.h"
#include "usr/blink/task_blink.h"
#include "usr/blink/task_vsi.h"

void app_blink_init(void)
{
    // Register "blink" command with system
    cmd_blink_register();

    // Initialize blink task with system
    task_blink_init();

    task_vsi_init();
}

#endif // APP_BLINK


<|start_filename|>ip_repo/amdc_inv_status_mux_1.0/drivers/amdc_inv_status_mux_v1_0/src/amdc_inv_status_mux.h<|end_filename|>

#ifndef AMDC_INV_STATUS_MUX_H
#define AMDC_INV_STATUS_MUX_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG15_OFFSET 60
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG16_OFFSET 64
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG17_OFFSET 68
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG18_OFFSET 72
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG19_OFFSET 76
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG20_OFFSET 80
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG21_OFFSET 84
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG22_OFFSET 88
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG23_OFFSET 92
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG24_OFFSET 96
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG25_OFFSET 100
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG26_OFFSET 104
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG27_OFFSET 108
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG28_OFFSET 112
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG29_OFFSET 116
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG30_OFFSET 120
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG31_OFFSET 124
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG32_OFFSET 128
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG33_OFFSET 132
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG34_OFFSET 136
#define AMDC_INV_STATUS_MUX_S00_AXI_SLV_REG35_OFFSET 140


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_INV_STATUS_MUX register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_INV_STATUS_MUXdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_INV_STATUS_MUX_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_INV_STATUS_MUX_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_INV_STATUS_MUX register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_INV_STATUS_MUX device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_INV_STATUS_MUX_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_INV_STATUS_MUX_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_INV_STATUS_MUX instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_INV_STATUS_MUX_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_INV_STATUS_MUX_H


<|start_filename|>sdk/bare/user/usr/beta_labs/task_dtc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/task_dtc.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/scheduler.h"
#include "usr/beta_labs/inverter.h"
#include "usr/beta_labs/machine.h"
#include <math.h>

#define L_HAT ((Ld_HAT + Lq_HAT) / 2.0)
#define R_HAT (Rs_HAT)

#define Wb (DTC_BANDWIDTH * PI2) // rad/s
#define Ts (1.0 / TASK_DTC_UPDATES_PER_SEC)
#define Kp (Wb * L_HAT)
#define Ki ((R_HAT / L_HAT) * Kp)

// Global variables for logging
double LOG_Ia = 0.0;
double LOG_Vab_star = 0.0;

// Set by command
static double I_mag_star = 0.0;
static double I_freq_star = 0.0;

// Used by controller
static double I_star = 0.0;
static double I_err_acc = 0.0;
static double theta = 0.0;

static task_control_block_t tcb;

uint8_t task_dtc_is_inited(void)
{
    return scheduler_tcb_is_registered(&tcb);
}

void task_dtc_init(void)
{
    scheduler_tcb_init(&tcb, task_dtc_callback, NULL, "dtc", TASK_DTC_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);
}

void task_dtc_deinit(void)
{
    scheduler_tcb_unregister(&tcb);
}

static void _get_Iabc(double *Iabc)
{
    // Read from ADCs
    float Iabc_f[3];
    analog_getf(CC_PHASE_A_ADC, &Iabc_f[0]);
    analog_getf(CC_PHASE_B_ADC, &Iabc_f[1]);
    analog_getf(CC_PHASE_C_ADC, &Iabc_f[2]);

    // Convert ADC values to raw currents
    Iabc[0] = ((double) Iabc_f[0] * ADC_TO_AMPS_PHASE_A_GAIN) + ADC_TO_AMPS_PHASE_A_OFFSET;
    Iabc[1] = ((double) Iabc_f[1] * ADC_TO_AMPS_PHASE_B_GAIN) + ADC_TO_AMPS_PHASE_B_OFFSET;
    Iabc[2] = ((double) Iabc_f[2] * ADC_TO_AMPS_PHASE_C_GAIN) + ADC_TO_AMPS_PHASE_C_OFFSET;
}

void task_dtc_callback(void *arg)
{
    // ------------------------------------
    // Get currents
    // ------------------------------------

    double Iabc[3];
    _get_Iabc(Iabc);
    double Ia = Iabc[0];

    // ------------------------------------
    // Calculate I_star based on commanded I_mag and I_freq
    // ------------------------------------
    theta += (PI2 * I_freq_star * Ts);
    I_star = I_mag_star * cos(theta);

    // ------------------------------------
    // PI stationary frame current regulator Ia
    // ------------------------------------

    double I_err;
    double Va_star, Vb_star;
    I_err = I_star - Ia;
    I_err_acc += I_err;
    Va_star = (Kp * I_err) + (Ki * Ts * I_err_acc);
    Vb_star = -Va_star;

    // ------------------------------------
    // Write commanded voltages to inverter
    // ------------------------------------

    inverter_saturate_to_Vdc(&Va_star);
    inverter_saturate_to_Vdc(&Vb_star);

    inverter_set_voltage(CC_PHASE_A_PWM_LEG_IDX, Va_star, Iabc[0]);
    inverter_set_voltage(CC_PHASE_B_PWM_LEG_IDX, Vb_star, Iabc[1]);

    // ------------------------------------
    // Update log variables
    // ------------------------------------

    LOG_Ia = Iabc[0];
    LOG_Vab_star = Va_star - Vb_star;
}

void task_dtc_set_I_star(double A, double Hz)
{
    I_mag_star = A;
    I_freq_star = Hz;
}

void task_dtc_clear(void)
{
    task_dtc_set_I_star(0.0, 0.0);

    inverter_set_voltage(CC_PHASE_A_PWM_LEG_IDX, 0.0, 0.0);
    inverter_set_voltage(CC_PHASE_B_PWM_LEG_IDX, 0.0, 0.0);
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/gpio_mux.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#include "drv/gpio_mux.h"
#include "xil_io.h"
#include <stdint.h>
#include <stdio.h>

#define GPIO_MUX_BASE_ADDR (0x43C70000)

static int ports[] = { GPIO_PORT1, GPIO_PORT2 };

void gpio_mux_init(void)
{
    printf("GPIO LINES:\tInitializing...\n");

    for (int i = 0; i < 2; i++) {
        if (ports[i]) {
            // Map the port status lines to the corresponding device defined in the header file
            gpio_mux_set_device(i, ports[i]);
        }
    }
}

void gpio_mux_set_device(uint8_t port, gpio_mux_device_t device)
{
    Xil_Out32(GPIO_MUX_BASE_ADDR + (port * sizeof(uint32_t)), device);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_cc.h<|end_filename|>
#ifndef CMD_CC_H
#define CMD_CC_H

void cmd_cc_register(void);

int cmd_cc(int argc, char **argv);

#endif // CMD_CC_H


<|start_filename|>sdk/bare/user/usr/beta_labs/mc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/mc.h"

// Tuning for motion controller with Ts = 0.00025
// Bandwidth is tuned to 5 Hz
#define Ba   (0.18121428301337322)
#define Ksa  (0.4668134916780646)
#define Kisa (0.10834187149852403)

// Static variables for controller
static double delta_theta_m_error_acc;
static double delta_theta_m_error_acc_acc;

void mc_init(void)
{
    delta_theta_m_error_acc = 0.0;
    delta_theta_m_error_acc_acc = 0.0;
}

double mc_update(double delta_theta_m_star, double delta_theta_m, double Ts)
{
    // Run through motion controller block diagram
    double delta_theta_m_error = delta_theta_m_star - delta_theta_m;
    delta_theta_m_error_acc += delta_theta_m_error;
    delta_theta_m_error_acc_acc += delta_theta_m_error_acc;

    // Calculate commanded torque
    double term1 = (Ba / Ts * delta_theta_m_error);
    double term2 = (Ksa * delta_theta_m_error_acc);
    double term3 = (Kisa * Ts * delta_theta_m_error_acc_acc);
    double Tem_SFB_star = term1 + term2 + term3;

    return Tem_SFB_star;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/basic/src/main.c<|end_filename|>
/*
 * Copyright (c) 2009-2012 Xilinx, Inc.  All rights reserved.
 *
 * Xilinx, Inc.
 * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A
 * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
 * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR
 * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
 * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE
 * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.
 * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO
 * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO
 * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE
 * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 */

/*
 * helloworld.c: simple test application
 *
 * This application configures UART 16550 to baud rate 9600.
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

//#include <stdio.h>
#if 0
#include "platform.h"
#include "xparameters.h"
#include "netif/xadapter.h"
#include "xgpio.h"
#include "xscutimer.h"
#include "xscugic.h"
#include "xil_exception.h"
#include "xil_printf.h"
#include "xuartps_hw.h"
#include <stdio.h>
#include "commands.h"
#include "Log.h"
#include "prog_timer.h"
#endif
#include "project_include.h"
/************************** Constant Definitions *****************************/
/*
 * The following constant maps to the name of the hardware instances that
 * were created in the EDK XPS system.
 */

//#define GPIO_EXAMPLE_DEVICE_ID          XPAR_AXI_GPIO_0_DEVICE_ID
//#define LED_CHANNEL                     1
//#define BUTTON_CHANNEL                  2
#define INTC_DEVICE_ID                  XPAR_PS7_SCUGIC_0_DEVICE_ID
//#define INTC_GPIO_INTERRUPT_ID          XPAR_FABRIC_AXI_GPIO_0_IP2INTC_IRPT_INTR
#define INTC                            XScuGic
#define INTC_HANDLER                    XScuGic_InterruptHandler
//#define BUTTON_INTERRUPT                XGPIO_IR_CH2_MASK
//#define INTENSITY_STEP                  110000

//Internal functions
//int SetupGPIO_Interrupt(XScuGic *IntcInstancePtr);
int SetupInterruptSystem(XScuGic *IntcInstancePtr);
//void GpioIsr(void *InstancePtr);
void HandleTimer100usTick();
//	/void CommandParser(const char * szCmd, char *szResponse);
//void HandleCommands();
//void UpdateLEDs();

/* missing declaration in lwIP */
void lwip_init();

////GPIO
//XGpio Gpio; /* The Instance of the GPIO Driver */
//u32 count = 0;

//Interrupts
XScuGic IntcInstance; /* Interrupt Controller Instance */


//UART stuff
char szRcvCmdUart[4096];

//timer stuff:
/*static int SetupTimer(XScuGic *IntcInstancePtr, XScuTimer * TimerInstancePtr,
 u16 TimerDeviceId, u16 TimerIntrId);*/

XScuTimer TimerInstance; /* Cortex A9 Scu Private Timer Instance */

volatile u32 Timer10ms;

u32 Timing100usErrorCount = 0;

u32 bTimer = 0;

u32 ConvertCharToByte(u32 i) {
	u32 res = i;
	if (res <= 0x39)
		res -= 0x30;
	else if (res <= 'F')
		res -= 55;
	else if (res <= 'f')
		res -= 87;

	return res;
}

char TimerDisabled = TRUE;
int EnterProtection() {
	if (!TimerDisabled)
	{
		XScuGic_Disable(&IntcInstance, TIMER_IRPT_INTR);
		TimerDisabled = TRUE;
		return TRUE;
	}
	else
		return FALSE;
}

int LeaveProtection(int State) {
	if (State)
	{
		XScuGic_Enable(&IntcInstance, TIMER_IRPT_INTR);
		TimerDisabled = FALSE;
	}
	return State;
}

	extern void outbyte(char c);
	int main() {
		u32 HandleCommandState = 0;
		u32 counter;
		u32 ec_to_dev, ec_from_dev; //, failure;
		int Status;

		init_platform();

		bsp_init();

		InitPECB_IO();
		InitLog();

		/*
		 * Setup the interrupts such that interrupt processing can occur.
		 * If an error occurs then exit
		 */
//ISR
		print("Setting up the interrupts... ");
		Status = SetupInterruptSystem(&IntcInstance);
		if (Status != XST_SUCCESS)
			return XST_FAILURE;
		print("Success!\n\r");

//TIMER
		print("Setting up the timer...");
		Status = SetupTimer(&IntcInstance, &TimerInstance, TIMER_DEVICE_ID,
				TIMER_IRPT_INTR);
		if (Status != XST_SUCCESS)
			return XST_FAILURE;
		print("Success!\n\r");
		TimerDisabled = FALSE;

////GPIO
//		print("Setting up the GPIO...");
//		Status = XGpio_Initialize(&Gpio, GPIO_EXAMPLE_DEVICE_ID);
//		if (Status != XST_SUCCESS)
//			return XST_FAILURE;
//		print("Success!\n\r");

////GPIO ISR
//		print("Setting up the GPIO Interrupt...");
//		Status = SetupGPIO_Interrupt(&IntcInstance);
//		if (Status != XST_SUCCESS)
//			return XST_FAILURE;
//		print("Success!\n\r");

//CONTROL
		print("Setting up the controllers...");
		Status = InitControllers(&IntcInstance);
		if (Status != XST_SUCCESS)
			return XST_FAILURE;
		print("Success!\n\r");

//CONTROL app
		/*print("Initializing the control application...");
			Status = InitControlApp();
			if (Status != XST_SUCCESS)
				return XST_FAILURE;
			print("Success!\n\r");*/

//CLEAR PECB error count
		print("Clearing PECB error count...");
		PECB[CONTROL] = 1;
		for (counter = 0; counter < 0xFFFF; counter++)
			;
		PECB[CONTROL] = 0;

		ec_to_dev = PECB[ER_CNT] & 0xFFFF;
		ec_from_dev = (PECB[ER_CNT] >> 16) & 0xFFFF;
		if (ec_to_dev + ec_from_dev == 0)
			print("Success!\r\n");
		else
			printf("Uh oh... found %d errors!\r\n",
					(unsigned int) (ec_to_dev + ec_from_dev));

//PECB self-test
#if 0
		print("PECB self test...");
		for (counter = 0; counter < 12; counter++)
		PECB[WR_REG0+counter] = 0x03 + counter;

		for (counter = 0; counter < 0xFFFF; counter++)
		;

		failure = 0;

		for (counter = 0; counter < 12; counter++)
		if (PECB[RD_REG0+counter] != 0x03 + counter)
		{
			failure = 1;
			printf("\r\nPECB Self-Test Failure: Register %X reads %08X, expected %08X", (unsigned int)counter, (unsigned int)PECB[RD_REG0+counter], (unsigned int) (0x03+counter));
		}

		if (failure == 0)
		printf("Success!\r\n");
		else
		printf("\r\n");
#endif
		print("Setting up ethernet and lwip...");
		if (SetupLWIP() == XST_SUCCESS)
			printf("Success!\r\n");
		else
			printf("Failure\r\n");

		print("Setting up listening sockets...");
		if (start_tcpip() == XST_SUCCESS)
			printf("Success!\r\n");
		else
			printf("Failure\r\n");

		//so we have two options to address the IO -- either directly with its address (0x41200000)
		//or via XGpio functions.

		//Option 1:
		//print("Writing to LEDs directly: 0x24\n\r");

		//*LEDs = 0x24;

		print("Completing the initialization of the PECB interface...");
		FinishInitPECB_IO();

		print("Enter a command followed by CR\n\r");
		while (1) {
			xemacif_input(echo_netif);

			HandleCommandState = HandleCommandsUart(szRcvCmdUart,
					HandleCommandState);

			if (bTimer100usTick)
				HandleTimer100usTick();

			if (bPrimeStreamFIFO)
			{
				if (ReloadStreamFIFO(StreamFIFOComm) > 0)
					bPrimeStreamFIFO = FALSE;
			}

			if (bEth250ms)
			{
				bEth250ms = FALSE;
				lwip_timer_callback();
			}
		}

		return 0;
	}

	void HandleTimer100usTick() {
		static u32 countTo10ms;

		//Clear this immediately so we can make sure the function doesn't take more than 100us
		bTimer100usTick = 0;

		//code every 10ms
		if (++countTo10ms) {
			countTo10ms = 0;
//			UpdateLEDs();
		}

		HBA_100usTick();
		VSI_100usTick();

		if (bTimer100usTick)
			Timing100usErrorCount++;
	}

//	void UpdateLEDs()
//	{
//		u32 ec_to_dev, ec_from_dev;
//		ec_to_dev = PECB[ER_CNT] & 0xFFFF;
//		ec_from_dev = (PECB[ER_CNT] >> 16) & 0xFFFF;
//		WriteToZB_LEDs(ec_to_dev + ec_from_dev);
//	}




//	void GpioIsr(void *InstancePtr) {
//		u32 Buttons;
//
//		XGpio *GpioPtr = (XGpio *) InstancePtr;
//
//		// Disable the interrupt
//		XGpio_InterruptDisable(GpioPtr, BUTTON_INTERRUPT);
//
//		// There should not be any other interrupts occuring other than the the button changes
//		if ((XGpio_InterruptGetStatus(GpioPtr) & BUTTON_INTERRUPT)
//				!= BUTTON_INTERRUPT) {
//			return;
//		}
//
//		// Read state of push buttons and determine which ones changed
//		// states from the previous interrupt. Save a copy of the buttons
//		// for the next interrupt
//
//		Buttons = XGpio_DiscreteRead(&Gpio, BUTTON_CHANNEL);
//
//		if (Buttons == 1 && (count < 0xFF)) {
//			u32 counter;
//			count++;
//			PECB[CONTROL] = 1;
//			fprintf(stdout, "Clearing the error count\n");
//			for (counter = 0; counter < 0xFFFF; counter++)
//				;
//			PECB[CONTROL] = 0;
//			fprintf(stdout, "Error count cleared, now reads: %d\n",
//					(int) PECB[ER_CNT]);
//		}
//
//		if (Buttons == 2 && (count < 0xFF)) {
//			count++;
//		}
//
//		if (Buttons == 4 && (count >= 1)) {
//			count--;
//		}
//
//		if (Buttons == 8 && (count >= 1)) {
//			count--;
//		}
//
//		if (Buttons == 16) {
//			count = 0;
//		}
//
//		//fprintf(stdout,"Count : %X\n",(int)count);
//
//		//XGpio_DiscreteWrite(&Gpio, LED_CHANNEL, count);
//
//		// Clear the interrupt such that it is no longer pending in the GPIO
//		(void) XGpio_InterruptClear(GpioPtr, BUTTON_INTERRUPT);
//
//		// Enable the interrupt
//		XGpio_InterruptEnable(GpioPtr, BUTTON_INTERRUPT);
//
//	}

	/*******************************************************************************/
	/*              S E T U P   I N T E R R U P T   S Y S T E M                    */
	/*******************************************************************************/
	int SetupInterruptSystem(XScuGic *IntcInstancePtr) {
		int Result;

		XScuGic_Config *IntcConfig;

		// Initialize the interrupt controller driver so that it is ready to use.

		IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);//ELS -- this is misleading! there is only one XScuGic device (0)
		if (NULL == IntcConfig) {
			return XST_FAILURE;
		}

		Result = XScuGic_CfgInitialize(IntcInstancePtr, IntcConfig,
				IntcConfig->CpuBaseAddress);
		if (Result != XST_SUCCESS) {
			return XST_FAILURE;
		}

		// Initialize the exception table and register the interrupt
		// controller handler with the exception table

		Xil_ExceptionInit();

		Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
				(Xil_ExceptionHandler) INTC_HANDLER, IntcInstancePtr);

		// Enable non-critical exceptions

		Xil_ExceptionEnable();

		return XST_SUCCESS;
	}

//	int SetupGPIO_Interrupt(XScuGic *IntcInstancePtr) {
//		int Result;
//		XScuGic_SetPriorityTriggerType(IntcInstancePtr, INTC_GPIO_INTERRUPT_ID,
//				0xA0, 0x3);
//
//		// Connect the interrupt handler that will be called when an
//		// interrupt occurs for the device.
//
//		Result = XScuGic_Connect(IntcInstancePtr, INTC_GPIO_INTERRUPT_ID,
//				(Xil_ExceptionHandler) GpioIsr, &Gpio);
//		if (Result != XST_SUCCESS) {
//			return Result;
//		}
//
//		// Enable the interrupt for the GPIO device.
//
//		XScuGic_Enable(IntcInstancePtr, INTC_GPIO_INTERRUPT_ID);
//
//		// Enable the GPIO channel interrupts so that push button can be
//		// detected and enable interrupts for the GPIO device
//
//		XGpio_InterruptEnable(&Gpio, BUTTON_INTERRUPT);
//		XGpio_InterruptGlobalEnable(&Gpio);
//
//		return XST_SUCCESS;
//	}



<|start_filename|>sdk/bare/common/sys/serial.c<|end_filename|>
#include "sys/serial.h"
#include "drv/uart.h"
#include "sys/scheduler.h"
#include "sys/util.h"
#include <string.h>

#define OUTPUT_BUFFER_LENGTH (32 * 1024)

static char output_buffer[OUTPUT_BUFFER_LENGTH] = { 0 };
static int output_idx = 0;

// Used to mark position in `output_buffer` of char which needs to output
//
// -1 means not currently outputting anything
// >= 0 means trying to print at that idx
static int print_idx = -1;

// Amount of chars we need to print at the moment
static int print_amount = 0;

static task_control_block_t tcb;

void serial_init(void)
{
    printf("DB:\tInitializing serial task...\n");
    scheduler_tcb_init(&tcb, serial_callback, NULL, "serial", SERIAL_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);
}

void serial_callback(void *arg)
{
    if (print_amount > 0) {
        // Have work to do!

        // Determine amount of chars to print before buffer wrap
        int try_to_send = MIN(print_amount, OUTPUT_BUFFER_LENGTH - print_idx);

        // Try to send that amount of chars to the UART
        int bytes_sent = uart_send(&output_buffer[print_idx], try_to_send);
        print_amount -= bytes_sent;

        // UART TX FIFO might have been full, so didn't print all we tried
        print_idx += bytes_sent;
        if (print_idx >= OUTPUT_BUFFER_LENGTH) {
            print_idx = 0;
        }

        // Check if done outputting data
        if (print_amount == 0) {
            print_idx = -1;
        }
    }
}

static void _append_to_output_buffer(char c)
{
    output_buffer[output_idx] = c;

    output_idx++;
    if (output_idx >= OUTPUT_BUFFER_LENGTH) {
        output_idx = 0;
    }
}

void serial_write(char *msg, int len)
{
    // Mark index to start outputting at in the callback
    if (print_idx == -1) {
        print_idx = output_idx;
    }

    print_amount += len;

    // Copy contents into circular output buffer
    for (int i = 0; i < len; i++) {
        _append_to_output_buffer(msg[i]);
    }
}


<|start_filename|>sdk/basic/src/axi_timer.c<|end_filename|>
/*
 * control_timer.c
 *
 *  Created on: Jun 23, 2014
 *      Author: sever212
 */

#include "project_include.h"



static int AXI_TimerSetupIntrSystem(XScuGic *IntcInstancePtr,
		XTmrCtr *TimerInstancePtr, u16 TimerIntrId, u8 priority);
/*static int Init_AXI_Timer(XScuGic* IntcInstancePtr, XTmrCtr* TmrCtrInstancePtr,
		u16 DeviceId, u16 IntrId, u8 TmrCtrNumber,
		XTmrCtr_Handler InterruptHandler, u8 IntrPriority, u32 ResetValue);*/


/*****************************************************************************/
/**
 * It initializes a timer counter and then sets it up in
 * compare mode with auto reload such that a periodic interrupt is generated.
 *
 * This function uses interrupt driven mode of the timer counter.
 *
 * @param	IntcInstancePtr is a pointer to the Interrupt Controller
 *		driver Instance
 * @param	TmrCtrInstancePtr is a pointer to the XTmrCtr driver Instance
 * @param	DeviceId is the XPAR_<TmrCtr_instance>_DEVICE_ID value from
 *		xparameters.h
 * @param	IntrId is XPAR_<INTC_instance>_<TmrCtr_instance>_INTERRUPT_INTR
 *		value from xparameters.h
 * @param	TmrCtrNumber is the number of the timer to which this
 *		handler is associated with.
 *
 * @return	XST_SUCCESS if the Test is successful, otherwise XST_FAILURE
 *
 * @note		This function contains an infinite loop such that if interrupts
 *		are not working it may never return.
 *
 *****************************************************************************/
int Init_AXI_Timer(XScuGic* IntcInstancePtr, XTmrCtr* TmrCtrInstancePtr,
		u16 DeviceId, u16 IntrId, u8 TmrCtrNumber,
		XTmrCtr_Handler InterruptHandler, u8 IntrPriority, u32 ResetValue) {
	int Status;

	/*
	 * Initialize the timer counter so that it's ready to use,
	 * specify the device ID that is generated in xparameters.h
	 */
	Status = XTmrCtr_Initialize(TmrCtrInstancePtr, DeviceId);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Perform a self-test to ensure that the hardware was built
	 * correctly, use the 1st timer in the device (0)
	 */
	Status = XTmrCtr_SelfTest(TmrCtrInstancePtr, TmrCtrNumber);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Connect the timer counter to the interrupt subsystem such that
	 * interrupts can occur.  This function is application specific.
	 */
	Status = AXI_TimerSetupIntrSystem(IntcInstancePtr, TmrCtrInstancePtr,
			IntrId, IntrPriority);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Setup the handler for the timer counter that will be called from the
	 * interrupt context when the timer expires, specify a pointer to the
	 * timer counter driver instance as the callback reference so the handler
	 * is able to access the instance data
	 */
	XTmrCtr_SetHandler(TmrCtrInstancePtr, InterruptHandler, TmrCtrInstancePtr);

	/*
	 * Enable the interrupt of the timer counter so interrupts will occur
	 * and use auto reload mode such that the timer counter will reload
	 * itself automatically and continue repeatedly, without this option
	 * it would expire once only
	 */
	XTmrCtr_SetOptions(TmrCtrInstancePtr, TmrCtrNumber,
			XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION);

	/*
	 * Set a reset value for the timer counter such that it will expire
	 * eariler than letting it roll over from 0, the reset value is loaded
	 * into the timer counter when it is started
	 */
	XTmrCtr_SetResetValue(TmrCtrInstancePtr, TmrCtrNumber, ResetValue);

	/*
	 * Start the timer counter such that it's incrementing by default,
	 * then wait for it to timeout a number of times
	 */
	XTmrCtr_Start(TmrCtrInstancePtr, TmrCtrNumber);

	return XST_SUCCESS;
}

/*****************************************************************************/
/**
 *
 * This function sets up the interrupt system such that interrupts can occur
 * for the device.
 *
 * @param	IntcInstancePtr is a pointer to the instance of XScuGic driver.
 * @param	TimerInstancePtr is a pointer to the instance of XScuTimer
 *		driver.
 * @param	TimerIntrId is the Interrupt Id of the XScuTimer device.
 * @param 	InterruptHandler is the function to handle the interrupts
 * @param 	priority is the priority for the interrupts (0 is highest
 * 			priority, 0xF8 (248) is lowest.
 *
 * @return	XST_SUCCESS if successful, otherwise XST_FAILURE.
 *
 * @note		None.
 *
 ******************************************************************************/
static int AXI_TimerSetupIntrSystem(XScuGic *IntcInstancePtr,
		XTmrCtr *TimerInstancePtr, u16 TimerIntrId, u8 priority) {
	int Status;

	/*
	 * Connect the device driver handler that will be called when an
	 * interrupt for the device occurs, the handler defined above performs
	 * the specific interrupt processing for the device.
	 */
	XScuGic_SetPriorityTriggerType(IntcInstancePtr, TimerIntrId, /*0xA8*/ priority /*0xA0*/, 0x3);

	Status = XScuGic_Connect(IntcInstancePtr, TimerIntrId,
			(XInterruptHandler) XTmrCtr_InterruptHandler,
			(void *) TimerInstancePtr);
	if (Status != XST_SUCCESS) {
		return Status;
	}

	/*
	 * Enable the interrupt for the device.
	 */
	XScuGic_Enable(IntcInstancePtr, TimerIntrId);

	/*
	 * Enable the timer interrupts for timer mode.
	 */
	//should I do this?? XScuTimer_EnableInterrupt(TimerInstancePtr);

	return XST_SUCCESS;
}





<|start_filename|>sdk/bare/common/drv/fpga_timer.h<|end_filename|>
#ifndef FPGA_TIMER_H
#define FPGA_TIMER_H

#include <stdint.h>

void fpga_timer_init(void);

// Return the current time in FPGA clock cycles (200MHz clock)
uint32_t fpga_timer_now(void);

// Convert FPGA clock ticks to usec
double fpga_timer_ticks_to_usec(uint32_t ticks);

// Convert FPGA clock ticks to sec
double fpga_timer_ticks_to_sec(uint32_t ticks);

#endif // FPGA_TIMER_H


<|start_filename|>sdk/bare/user/usr/beta_labs/task_dtc.h<|end_filename|>
#ifndef TASK_DTC_H
#define TASK_DTC_H

#include "drv/analog.h"
#include "sys/scheduler.h"
#include <stdint.h>

#define TASK_DTC_UPDATES_PER_SEC (10000)
#define TASK_DTC_INTERVAL_USEC   (USEC_IN_SEC / TASK_DTC_UPDATES_PER_SEC)

#define DTC_BANDWIDTH (200.0) // Hz

// Current = GAIN * ADC_Voltage + Offset

#define ADC_TO_AMPS_PHASE_A_GAIN (1.0100499)
#define ADC_TO_AMPS_PHASE_B_GAIN (1.0088819)
#define ADC_TO_AMPS_PHASE_C_GAIN (1.0056083)

#define ADC_TO_AMPS_PHASE_A_OFFSET (-0.01765254)
#define ADC_TO_AMPS_PHASE_B_OFFSET (-0.048415325)
#define ADC_TO_AMPS_PHASE_C_OFFSET (-0.0477295)

#define CC_PHASE_A_ADC (ANLG_CHNL1)
#define CC_PHASE_B_ADC (ANLG_CHNL2)
#define CC_PHASE_C_ADC (ANLG_CHNL3)

#define CC_PHASE_A_PWM_LEG_IDX (0)
#define CC_PHASE_B_PWM_LEG_IDX (1)
#define CC_PHASE_C_PWM_LEG_IDX (2)

uint8_t task_dtc_is_inited(void);

void task_dtc_init(void);
void task_dtc_deinit(void);
void task_dtc_callback(void *arg);

void task_dtc_set_I_star(double A, double Hz);
void task_dtc_clear(void);

#endif // TASK_DTC_H


<|start_filename|>sdk/bare/common/drv/dac.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#ifndef DAC_H
#define DAC_H

#include <stdint.h>

typedef enum {
    DAC_REG_SYNC = 2,
    DAC_REG_CONFIG = 3,
    DAC_REG_GAIN = 4,
    DAC_REG_TRIGGER = 5,
    DAC_REG_BRDCAST = 6,
    DAC_REG_DAC0 = 8,
    DAC_REG_DAC1 = 9,
    DAC_REG_DAC2 = 10,
    DAC_REG_DAC3 = 11,
    DAC_REG_DAC4 = 12,
    DAC_REG_DAC5 = 13,
    DAC_REG_DAC6 = 14,
    DAC_REG_DAC7 = 15,
} dac_reg_t;

#define DAC_MAX_CODE (4095)

void dac_init(void);

void dac_set_voltage(uint8_t ch, double voltage);

void dac_set_broadcast(double voltage);

void dac_set_trigger(void);

void dac_set_raw(dac_reg_t reg, uint32_t value);

#endif // DAC_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/main.c<|end_filename|>
// Bare AMDC Application
//
// <NAME>, 5/22/2019
//
// -----------
//
// This is a bare application which can be used as
// a starting point for AMDC applications.
//
// It uses a custom "RTOS" -- a simple cooperative
// scheduler which manages device tasks. Users must
// ensure that their tasks do not consume too many
// cycles in a single iteration, else the scheduler
// time quantum will be overrun.
//
// NOTE: UART uses 115200 baud

#include "drv/bsp.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "sys/injection.h"
#include "sys/log.h"
#include "sys/platform.h"
#include "sys/scheduler.h"
#include "sys/serial.h"
#include "usr/user_apps.h"
#include "usr/user_config.h"
#include <stdio.h>

#if USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT == 1
#include "drv/motherboard.h"
#endif

int main()
{
    // Required system initialization
    init_platform();

    // User BSP library initialization
    bsp_init();

    // Initialize system tasks
    serial_init();
    commands_init();
#if USER_CONFIG_ENABLE_LOGGING == 1
    log_init();
#endif

#if USER_CONFIG_ENABLE_INJECTION == 1
    injection_init();
#endif

#if USER_CONFIG_ENABLE_MOTHERBOARD_SUPPORT == 1
    // Initialize motherboard driver and register command
    motherboard_init(MOTHERBOARD_BASE_ADDR);
#endif

    // Initialize user applications
    user_apps_init();

    // Show start message to user, asking for cmds
    commands_start_msg();

    // Initialize scheduler (sets up h/w timer, interrupt, etc)
    scheduler_init();

    // Run scheduler => this takes over the system and never returns!
    scheduler_run();

    cleanup_platform();
    return 0;
}


<|start_filename|>sdk/basic/control_apps/HBridge_CC.h<|end_filename|>
#ifndef __HBRIDGE_CC_H
#define  __HBRIDGE_CC_H

extern int Init_HBridge_CC(u8 StartAllControllers);
extern int HBridge_CC_StartCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int Start_HBridge_CC_Controller(u32 HB);
extern int Stop_HBridge_CC_Controller(u32 HB);
extern int HBridge_CC_Command(const char * szCmd, char *szResponse, void *CommDevice);
extern int HBridge_CC_SetCurrentRef(u32 hbridge, double reference);
extern int HBridge_CC_StopCommand(const char *szCmd, char *szResponse, void *CommDevice);

typedef u8 (*HBridge_input)(void *arg, void *VSIcontext);
typedef void (*Hbridge_output)(void *arg, void *VSIcontext);

#define NUM_HBRIDGE 6

typedef struct HB_context
{
	pi_house c;
	u32 InputAtTickCount;		//the tick count of the last time the inputs were updated (to avoid unnecessary computation)
	double I;
	u8 d[2];				//[0] for leg 1, [1] for leg 2
	HBridge_input get_input;
	void * input_arg;
	Hbridge_output write_output;
	void * output_arg;
} HB_context;

extern HB_context HB_Controllers[NUM_HBRIDGE];
#endif 


<|start_filename|>sdk/bare/user/usr/beta_labs/task_vsi.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/task_vsi.h"
#include "drv/pwm.h"
#include "sys/scheduler.h"
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

// Forward declarations
static double omega_ramp_fcn(double *current, double *setpoint, double ramprate);

static uint8_t VSI_leg1;
static uint8_t VSI_leg2;
static uint8_t VSI_leg3;
static double VSI_Vpercent = 0;
static double VSI_omega = 0;
static double theta = 0;

static double VSI_omega_ramp = 0;
static double VSI_old_Vpercent = 0;
static double VSI_old_omega = 0;

static double VSI_R;
static double VSI_V0;

static task_control_block_t tcb;

static void _clear_state(void)
{
    task_vsi_set(0, 0, 0);
    pwm_set_duty(VSI_leg1, 0.0);
    pwm_set_duty(VSI_leg2, 0.0);
    pwm_set_duty(VSI_leg3, 0.0);
}

uint8_t task_vsi_is_inited(void)
{
    return scheduler_tcb_is_registered(&tcb);
}

void task_vsi_init(void)
{
    // Register scheduler task
    scheduler_tcb_init(&tcb, task_vsi_callback, NULL, "vsi", TASK_VSI_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);

    // Clear output
    _clear_state();
}

void task_vsi_deinit(void)
{
    // Unregister scheduler task
    scheduler_tcb_unregister(&tcb);

    // Clear output
    _clear_state();
}

void task_vsi_callback(void *arg)
{
    double update_da = omega_ramp_fcn(&VSI_old_omega, &VSI_omega, VSI_omega_ramp);

    theta += update_da;
    if (theta > 6.283185307179586)
        theta -= 6.283185307179586;

    double v = (VSI_R * VSI_old_omega) + VSI_V0;

    double percent1 = v * cos(theta);
    double percent2 = v * cos(theta - PI23);
    double percent3 = v * cos(theta + PI23);

    pwm_set_duty(VSI_leg1, (1 + percent1) / 2.0);
    pwm_set_duty(VSI_leg2, (1 + percent2) / 2.0);
    pwm_set_duty(VSI_leg3, (1 + percent3) / 2.0);
}

void task_vsi_set_legs(int phAIdx, int phBIdx, int phCIdx)
{
    VSI_leg1 = phAIdx;
    VSI_leg2 = phBIdx;
    VSI_leg3 = phCIdx;
}

void task_vsi_set(int vPercent, int freq, double ramptime)
{
    // Convert voltage percent input to double percentage
    VSI_old_Vpercent = VSI_Vpercent;
    VSI_Vpercent = vPercent / 100.0;

    // Convert freq input to rad/sec
    VSI_old_omega = VSI_omega;
    VSI_omega = 2 * PI * freq;

    if (ramptime > 0.0) {
        VSI_omega_ramp = abs(VSI_old_omega - VSI_omega) / ramptime; // (rad/sec) / sec
    } else {
        VSI_omega_ramp = 0;
    }

    if (VSI_omega != VSI_old_omega) {
        // There is delta freq requested
        VSI_R = (VSI_Vpercent - VSI_old_Vpercent) / (VSI_omega - VSI_old_omega);
        VSI_V0 = VSI_Vpercent - (VSI_R * VSI_omega);
    } else {
        // Instaneous voltage change
        VSI_R = 0;
        VSI_V0 = VSI_Vpercent;
    }
}

// Ramps between `current` and `setpoint` values assuming Fs = 10kHz
// Updates `current` as it ramps.
//
// `ramprate` is in (units of `current`) / sec
//     0 implies no ramp
//
//
// Returns delta in Ts for theta update
//
static double omega_ramp_fcn(double *current, double *setpoint, double ramprate)
{
    double ret;

    if (ramprate != 0 && *current != *setpoint) {
        // Ramping
        double dir = (*setpoint > *current) ? 1 : -1;
        double del = ramprate / TASK_VSI_UPDATES_PER_SEC;

        *current += (dir * del);
        ret = *current / TASK_VSI_UPDATES_PER_SEC;

        // Check if done ramping
        if ((dir == 1) && (*current > *setpoint))
            *current = *setpoint;
        if ((dir == -1) && (*current < *setpoint))
            *current = *setpoint;
    } else {
        // Not ramping
        ret = *setpoint / TASK_VSI_UPDATES_PER_SEC;
    }

    return ret;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/basic/bsp/timer.h<|end_filename|>
#ifndef TIMER_H
#define TIMER_H

#include "xtmrctr.h"

void timer_init(XTmrCtr_Handler timer_isr, uint32_t timer_period_usec);

#endif // TIMER_H


<|start_filename|>sdk/bare/common/sys/cmd/cmd_log.c<|end_filename|>
#include "usr/user_config.h"

#if USER_CONFIG_ENABLE_LOGGING == 1

#include "cmd_log.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "sys/log.h"
#include "sys/util.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "reg <log_var_idx> <name> <memory_addr> <samples_per_sec> <type>", "Register memory address for logging" },
    { "unreg <log_var_idx>", "Unregister variable slot" },
    { "start", "Start logging" },
    { "stop", "Stop logging" },
    { "dump <bin|text> <log_var_idx>", "Dump log data to console" },
    { "empty <log_var_idx>", "Empty log for a previously logged variable (stays registered)" },
    { "empty_all", "Empty all slots" },
    { "info", "Print status of logging engine" },
};

void cmd_log_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "log", "Logging engine commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_log);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

//
// Handles the 'log' command
// and all sub-commands
//
int cmd_log(int argc, char **argv)
{
    // Handle 'reg' sub-command
    if (strcmp("reg", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 7)
            return CMD_INVALID_ARGUMENTS;

        // Parse arg1: log_var_idx
        int log_var_idx = atoi(argv[2]);
        if (log_var_idx >= LOG_MAX_NUM_VARIABLES || log_var_idx < 0) {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse arg2: name
        char *name = argv[3];

        // Parse arg3: memory_addr
        void *memory_addr = (void *) atoi(argv[4]);

        // Parse arg4: samples_per_sec
        int samples_per_sec = atoi(argv[5]);
        if (samples_per_sec > LOG_UPDATES_PER_SEC || samples_per_sec <= 0) {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse arg5: type
        var_type_e type;
        if (strcmp("int", argv[6]) == 0) {
            type = LOG_INT;
        } else if (strcmp("float", argv[6]) == 0) {
            type = LOG_FLOAT;
        } else if (strcmp("double", argv[6]) == 0) {
            type = LOG_DOUBLE;
        } else {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        // Register the variable with the logging engine
        int err = log_var_register(log_var_idx, name, memory_addr, samples_per_sec, type);
        if (err != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'unreg' sub-command
    if (strcmp("unreg", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 3)
            return CMD_INVALID_ARGUMENTS;

        // Parse log_var_idx arg
        int log_var_idx = atoi(argv[2]);
        if (log_var_idx >= LOG_MAX_NUM_VARIABLES || log_var_idx < 0) {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        // Register the variable with the logging engine
        int err = log_var_unregister(log_var_idx);
        if (err != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'start' sub-command
    if (strcmp("start", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure log was stopped before this
        if (log_is_logging())
            return CMD_FAILURE;

        log_start();
        return CMD_SUCCESS;
    }

    // Handle 'stop' sub-command
    if (strcmp("stop", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        // Make sure log was running before this
        if (!log_is_logging())
            return CMD_FAILURE;

        log_stop();
        return CMD_SUCCESS;
    }

    // Handle 'dump' sub-command
    if (strcmp("dump", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 4)
            return CMD_INVALID_ARGUMENTS;

        // Ensure logging was stopped before this
        if (log_is_logging())
            return CMD_FAILURE;

        // Parse log_var_idx
        int log_var_idx = atoi(argv[3]);
        if (log_var_idx >= LOG_MAX_NUM_VARIABLES || log_var_idx < 0) {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        if (strcmp("text", argv[2]) == 0) {
            // Dump using text (human-readable)
            int err = log_var_dump_uart_ascii(log_var_idx);
            if (err != SUCCESS) {
                return CMD_FAILURE;
            }
        } else if (strcmp("bin", argv[2]) == 0) {
            // Dump using binary
            int err = log_var_dump_uart_binary(log_var_idx);
            if (err != SUCCESS) {
                return CMD_FAILURE;
            }
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        return CMD_SUCCESS_QUIET;
    }

    // Handle 'empty' sub-command
    if (strcmp("empty", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 3)
            return CMD_INVALID_ARGUMENTS;

        // Parse arg1: log_var_idx
        int log_var_idx = atoi(argv[2]);
        if (log_var_idx >= LOG_MAX_NUM_VARIABLES || log_var_idx < 0) {
            // ERROR
            return CMD_INVALID_ARGUMENTS;
        }

        int err = log_var_empty(log_var_idx);
        if (err != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'empty_all' sub-command
    if (argc == 2 && strcmp("empty_all", argv[1]) == 0) {
        int err = log_var_empty_all();
        if (err != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    // Handle 'info' sub-command
    if (strcmp("info", argv[1]) == 0) {
        // Check correct number of arguments
        if (argc != 2)
            return CMD_INVALID_ARGUMENTS;

        int err = log_print_info();
        if (err != SUCCESS) {
            return CMD_FAILURE;
        }

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // USER_CONFIG_ENABLE_LOGGING


<|start_filename|>sdk/basic/src/io_pecb.c<|end_filename|>
/*
 * io_pecb.c
 *
 *  Created on: Jun 25, 2014
 *      Author: sever212
 */

#include "project_include.h"

volatile u32 *PECB;
volatile u32 *LEDs;

/*****************************
 * ReadADC()
 * Call this function to read in the ADC voltage
 *
 * reads +-10V, accurate to 0.0049V
 *uiADCnum: the ADC to use (between 1 and 12)
 */
double ReadADC(u32 uiADCnum)
{
	float v;
	analog_getf(uiADCnum, &v);
	return v;

//	signed int iADC = 0;
//	double dbADCval = 0, dbTemp;
//
//	if ( (uiADCnum > 0) && (uiADCnum < 13) )
//		iADC = PECB[uiADCnum-1];
//	dbTemp = iADC;
//	dbADCval = (dbTemp / 0x7FF) * 10;
//	return dbADCval;
}

/*void WriteDutyFraction(u32 Leg, float frac)
{
	unsigned char value = (unsigned char) (frac*256);
	PECB[Leg-1] = value;
}*/

/*****************************
 * WriteDutyRatio()
 * Call this function to write a duty ratio out to
 * a single leg. The value should be between 0 and 255
 * as where 255 is always on and 0 is always off
 *
 * Leg: 1 - ??
 * value: fractional duty ratio between 0 and 255
 *
 */
void WriteDutyRatio(u32 Leg, unsigned char value)
{
	pwm_set_duty_raw(Leg-1, value);

//	PECB[Leg-1] = value;

}

/*****************************
 * WriteConfigRegister()
 * Call this function to write a setting to the PECB config
 * register.
 *
 * value: value between 0 and 255
 *
 */
void WriteConfigRegister(u8 value)
{
	PECB[NUM_LEGS] = value;
}

/*****************************
 * InitPECB_IO()
 * Call this function to start setting up the
 * PECB IO. After approximately 0.5 seconds have
 * passed, call FinishInitPECB_IO() to finish
 * the initialization
 *
 */
int InitPECB_IO()
{
	u32 i;
	LEDs = (u32 *) 0x41200000;
	PECB = (u32 *) 0x43c00000;

	//turn all of the PECB legs on.
	//this is to get the leg's dc/dc converters up and running properly
	//in accordance with the email from <EMAIL> on Dec. 10, 2014
	//will turn all the legs off later, in FinishInitPECB_IO()
	for (i = 0; i < NUM_LEGS; i++)
		WriteDutyRatio(i+1, 255);

	return XST_SUCCESS;
}

/*****************************
 * FinishInitPECB_IO()
 * Call this function approximately 0.5 seconds
 * after calling InitPECB_IO() to finish the
 * initialization.
 *
 */
int FinishInitPECB_IO()
{
	u32 i;
	//turn all of the legs off
	for (i = 0; i < NUM_LEGS; i++)
			WriteDutyRatio(i+1, 0);
}

void WriteToZB_LEDs(u32 LED_value)
{
	*LEDs = LED_value;
}


<|start_filename|>sdk/bare/user/usr/beta_labs/mc.h<|end_filename|>
#ifndef MC_H
#define MC_H

void mc_init(void);
double mc_update(double delta_theta_m_star, double delta_theta_m, double Ts);

#endif // MC_H


<|start_filename|>sdk/bare/user/usr/beta_labs/msf.h<|end_filename|>
#ifndef MSF_H
#define MSF_H

void msf_init(void);

double msf_update(double omega_m_star);

double msf_get_omega_m(void);
double msf_get_omega_m_dot(void);

#endif // MSF_H


<|start_filename|>sdk/bare/user/usr/beta_labs/task_mo.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/task_mo.h"
#include "drv/dac.h"
#include "drv/encoder.h"
#include "sys/scheduler.h"
#include "usr/beta_labs/machine.h"

static task_control_block_t tcb;

static double task_mo_omega_m = 0.0;

// Forward declarations
inline static double filter(double input);

void task_mo_init(void)
{
    scheduler_tcb_init(&tcb, task_mo_callback, NULL, "mo", TASK_MO_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);
}

double LOG_enc_pos = 0.0;

void task_mo_callback(void *arg)
{
    // TODO: change this to use "position"
    // and wrap things etc
    // Also be aware of negative wraps

    static int32_t last_steps = 0;
    int32_t steps;
    encoder_get_steps(&steps);
    int32_t delta = steps - last_steps;
    double rads = PI2 * ((double) delta / (double) (1 << ENCODER_PULSES_PER_REV_BITS));

    last_steps = steps;

    double omega_m = rads * (double) TASK_MO_UPDATES_PER_SEC;

    task_mo_omega_m = filter(omega_m);

    // Update log variables
    uint32_t pos;
    encoder_get_position(&pos);
    LOG_enc_pos = pos;

    double p = (double) pos / (double) ENCODER_PULSES_PER_REV;

    dac_set_output(0, p, 0.0, 1.0);
}

double task_mo_get_omega_e(void)
{
    return task_mo_omega_m * POLE_PAIRS;
}

double task_mo_get_omega_m(void)
{
    return task_mo_omega_m;
}

// Tuning for LPF based on Ts = 0.0001
#define A_1Hz   (0.9993718788200349)
#define A_5Hz   (0.9968633369849541)
#define A_10Hz  (0.9937365126247782)
#define A_50Hz  (0.9690724263048106)
#define A_100Hz (0.9391013674242926)
#define A_500Hz (0.7304026910486456)

#define A (A_100Hz)

inline static double filter(double input)
{
    static double z1 = 0.0;

    double output = (input * (1 - A)) + z1;

    z1 = output * A;

    return output;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/drv/dac.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#include "drv/dac.h"
#include "xil_io.h"
#include <stdint.h>
#include <stdio.h>

#define DAC_BASE_ADDR (0x43C60000)

void dac_init(void)
{
    printf("DAC:\tInitializing...\n");

    // Default Reference divider and each channel Gain to 2/2
    dac_set_raw(DAC_REG_GAIN, 0x000401FF);

    // Default all channels to be synchronously triggered
    dac_set_raw(DAC_REG_SYNC, 0x0002FFFF);

    // Default all channel values to 0V (redundant to power on reset)
    dac_set_broadcast(0);
}

void dac_set_voltage(uint8_t ch, double voltage)
{
    double range = 20;
    uint32_t value = (voltage + 10) * DAC_MAX_CODE / range;

    dac_set_raw(ch + 0x8, (ch + 0x8) << 16 | (value << 4 & 0xFFF0));
}

void dac_set_broadcast(double voltage)
{
    double range = 20;
    uint32_t value = (voltage + 10) * DAC_MAX_CODE / range;

    dac_set_raw(0x6, (0x6) << 16 | (value << 4 & 0xFFF0));
}

void dac_set_trigger(void)
{
    Xil_Out32(DAC_BASE_ADDR + (DAC_REG_TRIGGER * sizeof(uint32_t)), 0x00000010);
}

void dac_set_raw(dac_reg_t reg, uint32_t value)
{
    Xil_Out32(DAC_BASE_ADDR + (reg * sizeof(uint32_t)), value);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/user/usr/blink/app_blink.h<|end_filename|>
#ifndef APP_BLINK_H
#define APP_BLINK_H

void app_blink_init(void);

#endif // APP_BLINK_H


<|start_filename|>sdk/bare/common/drv/bsp.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if (USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C) || (USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D)
// Ensure a valid hardware target is specified
// NOTE: this firmware only supports REV C hardware onward
#else
#error "ERROR: Hardware target not specified correctly"
// If you have this error, please define USER_CONFIG_HARDWARE_TARGET in your usr/user_config.h file!
#endif

#include "drv/analog.h"
#include "drv/bsp.h"
#include "drv/cpu_timer.h"
#include "drv/dac.h"
#include "drv/encoder.h"
#include "drv/fpga_timer.h"
#include "drv/pwm.h"
#include "drv/timer.h"
#include "drv/uart.h"
#include "drv/watchdog.h"
#include "sys/cmd/cmd_hw.h"
#include "sys/defines.h"
#include <stdio.h>

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
#include "drv/gpio.h"
#include "drv/io.h"
#endif

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
#include "drv/dac.h"
#include "drv/eddy_current_sensor.h"
#include "drv/gpio_mux.h"
#include "drv/led.h"
#include "drv/sts_mux.h"
#endif

void bsp_init(void)
{
    if (uart_init() != SUCCESS) {
        printf("ERROR: UART init failed!\n");
        while (1) {
        };
    }

    // Busy loop to wait for UART to warm up... :)
    volatile int i;
    for (i = 0; i < 10e6; i++) {
        asm("nop");
    }

    // Print welcome message to user
    printf("Advanced Motor Drive Controller\n");

    switch (USER_CONFIG_HARDWARE_TARGET) {
    case AMDC_REV_C:
        printf("Hardware Target: REV C\n");
        break;

    case AMDC_REV_D:
        printf("Hardware Target: REV D\n");
        break;

    default:
        printf("ERROR: Unknown AMDC hardware revision\n");
        while (1) {
        }
    }

    printf("(C) 2020 Severson Research Group\n");
    printf("--------------------------------\n");
    printf("\n");

    printf("BSP:\tInitializing...\n");

    encoder_init();
    analog_init(ANALOG_BASE_ADDR);
    pwm_init();

    fpga_timer_init();
    cpu_timer_init();

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    led_init();
    sts_mux_init();
    gpio_mux_init();
    dac_init();
    eddy_current_sensor_init();
#endif

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
    io_init();
    gpio_init();
#endif

#if USER_CONFIG_ENABLE_WATCHDOG == 1
    watchdog_init();
#endif

    cmd_hw_register();
}


<|start_filename|>sdk/basic/bsp/encoder.h<|end_filename|>
#ifndef ENCODER_H
#define ENCODER_H

#include <stdint.h>

#define ENCODER_PULSES_PER_REV_BITS		(14)
#define ENCODER_PULSES_PER_REV			(1 << ENCODER_PULSES_PER_REV_BITS)

void encoder_init(void);

void encoder_set_pulses_per_rev_bits(uint32_t bits);

void encoder_get_steps(int32_t *steps);
void encoder_get_position(uint32_t *position);


#endif // ENCODER_H


<|start_filename|>sdk/basic/bsp/analog.c<|end_filename|>
#include "../bsp/analog.h"
#include "xil_io.h"
#include <stdio.h>


#define ANALOG_BASE_ADDR				(0x43C00000)

float analog_offsetf[ANALOG_NUM_CHANNELS] = {0};

void analog_init(void)
{
	printf("ANLG:\tInitializing...\n");

	// Set SCK to 50MHz
	analog_set_clkdiv(ANLG_CLKDIV4);
}

void analog_calibrate(uint32_t num)
{
    for (int i = 0; i < ANALOG_NUM_CHANNELS; i++) {
    	float value = 0;

    	// Average ANALOG_CALIBRATE_NUM_SAMPLES reads
    	for (int j = 0; j < ANALOG_CALIBRATE_NUM_SAMPLES; j++) {
    		float tmp;
            analog_getf(i+1, &tmp);
            value += tmp;
    	}

    	analog_offsetf[i] = value / ANALOG_CALIBRATE_NUM_SAMPLES;
    }

    printf("Analog Calibration Done:\n");
    for (int i = 0; i < ANALOG_NUM_CHANNELS; i++) {
    	printf("Channel #%d offset: %f volts\n", i+1, analog_offsetf[i]);
    }
}

void analog_set_clkdiv(analog_clkdiv_e div)
{
	printf("ANLG:\tSetting clkdiv to %d...\n", div);

	// Make sure we are only writing lower 2 bits
	uint32_t value = div & 0x00000003;

	// Register 16 is read/write clkdiv value
	Xil_Out32(ANALOG_BASE_ADDR + (16 * sizeof(uint32_t)), value);
}

void analog_get_clkdiv(analog_clkdiv_e *div)
{
	// Register 16 is read/write clkdiv value
	uint32_t tmp = Xil_In32(ANALOG_BASE_ADDR + (16 * sizeof(uint32_t)));

	*div = tmp;
}

void analog_getf(analog_channel_e channel, float *value)
{
	// Registers 0..15 are read-only values from ADC
	uint32_t out = Xil_In32(ANALOG_BASE_ADDR + (sizeof(uint32_t) * (channel-1)));

	// Conversion from raw bits to input voltage
	float tmp = (float)((int16_t) out) / 400;

	// Subtract off the measured offset bias
	*value = tmp - analog_offsetf[channel-1];
}

void analog_geti(analog_channel_e channel, int16_t *value)
{
	// Registers 0..15 are read-only values from ADC
	uint32_t out = Xil_In32(ANALOG_BASE_ADDR + (sizeof(uint32_t) * (channel-1)));

	*value = (int16_t) out;
}


<|start_filename|>sdk/basic/src/io_pecb.h<|end_filename|>
#ifndef __PECB_IO_H
#define __PECB_IO_H

//custom PL stuff
#define ER_CNT 31
#define CONTROL 31
#define WR_REG0 0
#define RD_REG0 0



//extern void WriteDutyFraction(u32 Leg, float frac);
extern void WriteConfigRegister(u8 value);
extern void WriteDutyRatio(u32 Leg, unsigned char value);
extern double ReadADC(u32 uiADCnum);
extern int InitPECB_IO();
extern int FinishInitPECB_IO();
extern void WriteToZB_LEDs(u32 LED_value);
#endif


<|start_filename|>sdk/basic/src/axi_timer.h<|end_filename|>
#ifndef __AXI_TIMER_H
#define __AXI_TIMER_H
extern volatile u32 ControlTimer0_TickCount;
extern volatile u32 ControlTimer1_TickCount;
#include "xparameters.h"



extern int Init_AXI_Timer(XScuGic* IntcInstancePtr, XTmrCtr* TmrCtrInstancePtr,
		u16 DeviceId, u16 IntrId, u8 TmrCtrNumber,
		XTmrCtr_Handler InterruptHandler, u8 IntrPriority, u32 ResetValue);
#endif //__AXI_TIMER_H


<|start_filename|>sdk/bare/common/sys/commands.c<|end_filename|>
#include "sys/commands.h"
#include "drv/encoder.h"
#include "drv/uart.h"
#include "sys/cmd/cmd_help.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/log.h"
#include "sys/scheduler.h"
#include "sys/serial.h"
#include "sys/util.h"
#include <ctype.h>
#include <stdlib.h>
#include <string.h>

#define RECV_BUFFER_LENGTH (4 * 1024)
static char recv_buffer[RECV_BUFFER_LENGTH] = { 0 };
static int recv_buffer_idx = 0;

#define CMD_MAX_ARGC       (16) // # of args accepted
#define CMD_MAX_ARG_LENGTH (32) // max chars of any arg
typedef struct pending_cmd_t {
    int argc;
    char *argv[CMD_MAX_ARGC];

    // Used by parser to keep track of how
    // long the argument is. If too long,
    // throws an error!
    int curr_arg_length;

    // Error flag set by parser task
    int err;

    // Set to 1 to indicate ready to execute,
    // 0 means not valid
    int ready;
} pending_cmd_t;

// Note, this must be >= 2.
//
// We always need a buffer to be filling while we
// execute the previous command
//
#define MAX_PENDING_CMDS (8)
static pending_cmd_t pending_cmds[MAX_PENDING_CMDS] = { 0 };
static int pending_cmd_write_idx = 0;
static int pending_cmd_read_idx = 0;

static int _command_handler(int argc, char **argv);

// Head of linked list of commands
static command_entry_t *cmds = NULL;

static task_control_block_t tcb_parse;
static task_control_block_t tcb_exec;

void commands_init(void)
{
    printf("CMD:\tInitializing command tasks...\n");

    // Command parse task
    scheduler_tcb_init(&tcb_parse, commands_callback_parse, NULL, "command_parse", COMMANDS_INTERVAL_USEC);
    scheduler_tcb_register(&tcb_parse);

    // Command exec task
    scheduler_tcb_init(&tcb_exec, commands_callback_exec, NULL, "command_exec", COMMANDS_INTERVAL_USEC);
    scheduler_tcb_register(&tcb_exec);

    cmd_help_register();
}

typedef enum state_e { BEGIN = 1, LOOKING_FOR_SPACE, LOOKING_FOR_CHAR } state_e;

state_e state = BEGIN;

void _create_pending_cmds(char *buffer, int length)
{
    // Get current pending cmd slot
    pending_cmd_t *p = &pending_cmds[pending_cmd_write_idx];

    for (int i = 0; i < length; i++) {
        char c = buffer[i];

        if (state != BEGIN && (c == '\n' || c == '\r')) {
            // End of a command!

            // Set error flag if this arg was too long
            if (p->curr_arg_length > CMD_MAX_ARG_LENGTH) {
                p->err = CMD_INPUT_TOO_LONG;
            }

            // Put a NULL at the end of the last cmd arg
            // (replaces a \r or \n, so nbd
            buffer[i] = 0;

            // Make console go to beginning of next line
            debug_printf("\r\n");

            p->ready = 1;

            // Update current pending cmd slot
            if (++pending_cmd_write_idx >= MAX_PENDING_CMDS)
                pending_cmd_write_idx = 0;
            p = &pending_cmds[pending_cmd_write_idx];
            p->ready = 0;

            // Move on to next char, which starts
            // next command sequence
            state = BEGIN;
            continue;
        }

        // Echo character back to terminal
        serial_write(&c, 1);

        // Process incoming char `c`
        switch (state) {
        case BEGIN:
            if (!isspace(c)) {
                // Populate first argument
                p->argc = 1;
                p->argv[0] = &buffer[i];
                p->err = CMD_SUCCESS; // Assume the parsing will work!
                p->curr_arg_length = 1;
                state = LOOKING_FOR_SPACE;
            }
            break;

        case LOOKING_FOR_SPACE:
            if (c != ' ') {
                p->curr_arg_length++;
            }

            if (c == ' ') {
                // End of chars for the arg

                // Set error flag if this arg was too long
                if (p->curr_arg_length > CMD_MAX_ARG_LENGTH) {
                    p->err = CMD_INPUT_TOO_LONG;
                }

                // Put NULL at end of arg (replaces ' ')
                buffer[i] = 0;

                state = LOOKING_FOR_CHAR;
            }
            break;

        case LOOKING_FOR_CHAR:
            if (c != ' ') {
                p->argv[p->argc] = &buffer[i];
                p->argc++;

                // Check if argc too big!
                if (p->argc > CMD_MAX_ARGC) {
                    p->err = CMD_INPUT_TOO_LONG;

                    // Put argc back to zero...
                    // NOTE: this ensure no buffer overruns,
                    //       but, screws up previous args.
                    //       This is okay as we are going to
                    //       throw away this pending cmd!
                    p->argc = 0;
                }

                p->curr_arg_length = 1;
                state = LOOKING_FOR_SPACE;
            }
            break;

        default:
            // Impossible!
            HANG;
            break;
        }
    }
}

void commands_callback_parse(void *arg)
{
    // Read in bounded chunk of new chars
    //
    // NOTE: careful not to try and read too much!
    //       would cause a buffer overrun!
    //
    int try_to_read = MIN(UART_RX_FIFO_LENGTH, RECV_BUFFER_LENGTH - recv_buffer_idx);
    int num_bytes = uart_recv(&recv_buffer[recv_buffer_idx], try_to_read);

    // Run state machine to create pending cmds to execute
    _create_pending_cmds(&recv_buffer[recv_buffer_idx], num_bytes);

    // Move along in recv buffer
    recv_buffer_idx += num_bytes;
    if (recv_buffer_idx >= RECV_BUFFER_LENGTH) {
        recv_buffer_idx = 0;
    }
}

void commands_callback_exec(void *arg)
{
    int err;

    // Get current pending cmd slot
    pending_cmd_t *p = &pending_cmds[pending_cmd_read_idx];

    if (p->ready) {
        // Run me!

        // Don't run a cmd that has errors
        err = p->err;
        if (err == CMD_SUCCESS) {
            err = _command_handler(p->argc, p->argv);
        }

        // Display command status to user
        switch (err) {
        case CMD_SUCCESS_QUIET:
            // Don't print anything
            break;

        case CMD_SUCCESS:
            debug_printf("SUCCESS\r\n\n");
            break;

        case CMD_FAILURE:
            debug_printf("FAILURE\r\n\n");
            break;

        case CMD_INVALID_ARGUMENTS:
            debug_printf("INVALID ARGUMENTS\r\n\n");
            break;

        case CMD_INPUT_TOO_LONG:
            debug_printf("INPUT TOO LONG\r\n\n");
            break;

        case CMD_UNKNOWN_CMD:
            debug_printf("UNKNOWN CMD\r\n\n");
            break;

        default:
            debug_printf("UNKNOWN ERROR\r\n\n");
            break;
        }

        p->ready = 0;

        // Update READ index
        if (++pending_cmd_read_idx >= MAX_PENDING_CMDS) {
            pending_cmd_read_idx = 0;
        }
    }
}

void commands_cmd_init(command_entry_t *cmd_entry,
                       const char *cmd,
                       const char *desc,
                       command_help_t *help,
                       int num_help_cmds,
                       int (*cmd_function)(int, char **))
{
    cmd_entry->cmd = cmd;
    cmd_entry->desc = desc;
    cmd_entry->help = help;
    cmd_entry->num_help_cmds = num_help_cmds;
    cmd_entry->cmd_function = cmd_function;
    cmd_entry->next = NULL;
}

void commands_cmd_register(command_entry_t *cmd_entry)
{
    // Base case: there are no tasks in linked list
    if (cmds == NULL) {
        cmds = cmd_entry;
        cmds->next = NULL;
        return;
    }

    // Find end of list
    command_entry_t *curr = cmds;
    while (curr->next != NULL)
        curr = curr->next;

    // Append new cmd to end of list
    curr->next = cmd_entry;
    cmd_entry->next = NULL;
}

void commands_start_msg(void)
{
    debug_printf("\r\n");
    commands_display_help();
}

// _command_handler
//
// Takes `argc` and `argv` and finds
// the command function to call.
//
int _command_handler(int argc, char **argv)
{
    command_entry_t *c = cmds;

    while (c != NULL) {
        if (strcmp(argv[0], c->cmd) == 0) {
            // Found command to run!
            return c->cmd_function(argc, argv);
        }

        c = c->next;
    }

    return CMD_UNKNOWN_CMD;
}

// ****************
// State Machine
// which outputs
// the help messages
// ****************

typedef enum sm_states_e { TITLE1 = 1, TITLE2, TITLE3, CMD_HEADER, SUB_CMD, REMOVE_TASK } sm_states_e;

typedef struct sm_ctx_t {
    sm_states_e state;
    command_entry_t *curr;
    int sub_cmd_idx;
    task_control_block_t tcb;
} sm_ctx_t;

#define SM_UPDATES_PER_SEC (10000)
#define SM_INTERVAL_USEC   (USEC_IN_SEC / SM_UPDATES_PER_SEC)

void help_state_machine_callback(void *arg)
{
    sm_ctx_t *ctx = (sm_ctx_t *) arg;

    switch (ctx->state) {
    case TITLE1:
        debug_printf("\r\n");

        ctx->curr = cmds;
        ctx->state = TITLE2;
        break;

    case TITLE2:
        debug_printf("Available commands:\r\n");
        ctx->state = TITLE3;
        break;

    case TITLE3:
        debug_printf("-------------------\r\n");
        ctx->state = CMD_HEADER;
        break;

    case CMD_HEADER:
        if (ctx->curr == NULL) {
            // DONE!
            debug_printf("\r\n");
            ctx->state = REMOVE_TASK;
        } else {
            debug_printf("%s -- %s\r\n", ctx->curr->cmd, ctx->curr->desc);
            ctx->sub_cmd_idx = 0;
            ctx->state = SUB_CMD;
        }
        break;

    case SUB_CMD:
        if (ctx->sub_cmd_idx >= ctx->curr->num_help_cmds) {
            ctx->curr = ctx->curr->next;
            ctx->state = CMD_HEADER;
        } else {
            command_help_t *h = &ctx->curr->help[ctx->sub_cmd_idx++];
            debug_printf("\t%s -- %s\r\n", h->subcmd, h->desc);
        }
        break;

    case REMOVE_TASK:
        scheduler_tcb_unregister(&ctx->tcb);
        break;

    default:
        // Can't happen
        HANG;
        break;
    }
}

static sm_ctx_t ctx;

void commands_display_help(void)
{
    // Initialize the state machine context
    ctx.state = TITLE1;
    ctx.curr = cmds;
    ctx.sub_cmd_idx = 0;

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx.tcb, help_state_machine_callback, &ctx, "help_sm", SM_INTERVAL_USEC);
    scheduler_tcb_register(&ctx.tcb);
}


<|start_filename|>sdk/bare/common/usr/dac/app_dac.h<|end_filename|>
#ifndef APP_DAC_H
#define APP_DAC_H

void app_dac_init(void);

#endif // APP_DAC_H


<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_vsi.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/cmd/cmd_vsi.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "usr/beta_labs/task_vsi.h"
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

#define NUM_HELP_ENTRIES (4)
static command_help_t cmd_help[NUM_HELP_ENTRIES] = {
    { "init", "Start VSI task" },
    { "deinit", "Stop VSI task" },
    { "legs <phAIdx> <phBIdx> <phCIdx>", "Set the output leg indices" },
    { "set <vPercent> <freq> [mRamptime]", "Set operating point (optional transition ramp time)" },
};

void cmd_vsi_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "vsi", "Voltage source inverter commands", cmd_help, NUM_HELP_ENTRIES, cmd_vsi);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

int cmd_vsi(int argc, char **argv)
{
    // Handle 'init' sub-command
    if (argc == 2 && strcmp("init", argv[1]) == 0) {
        // Make sure task was not already inited
        if (task_vsi_is_inited())
            return CMD_FAILURE;

        task_vsi_init();
        return CMD_SUCCESS;
    }

    // Handle 'deinit' sub-command
    if (argc == 2 && strcmp("deinit", argv[1]) == 0) {
        // Make sure task was not already inited
        if (!task_vsi_is_inited())
            return CMD_FAILURE;

        task_vsi_deinit();
        return CMD_SUCCESS;
    }

    // Handle 'legs ...' sub-command
    if (argc == 5 && strcmp("legs", argv[1]) == 0) {
        // Parse out phAIdx
        // and clip to 0..23
        int phAIdx = atoi(argv[2]);
        if (phAIdx < 0)
            return CMD_INVALID_ARGUMENTS;
        if (phAIdx > 23)
            return CMD_INVALID_ARGUMENTS;

        // Parse out phBIdx
        // and clip to 0..23
        int phBIdx = atoi(argv[3]);
        if (phBIdx < 0)
            return CMD_INVALID_ARGUMENTS;
        if (phBIdx > 23)
            return CMD_INVALID_ARGUMENTS;

        // Parse out phCIdx
        // and clip to 0..23
        int phCIdx = atoi(argv[4]);
        if (phCIdx < 0)
            return CMD_INVALID_ARGUMENTS;
        if (phCIdx > 23)
            return CMD_INVALID_ARGUMENTS;

        task_vsi_set_legs(phAIdx, phBIdx, phCIdx);

        return CMD_SUCCESS;
    }

    // Handle 'set ...' sub-command
    if ((argc == 4 || argc == 5) && strcmp("set", argv[1]) == 0) {
        // Parse out vPercent
        // and clip to 0..100
        int vPercent = atoi(argv[2]);
        if (vPercent < 0)
            return CMD_INVALID_ARGUMENTS;
        if (vPercent > 100)
            return CMD_INVALID_ARGUMENTS;

        // Parse out freq
        // and clip to 0..10,000
        int freq = atoi(argv[3]);
        if (freq < 0)
            return CMD_INVALID_ARGUMENTS;
        if (freq > 10000)
            return CMD_INVALID_ARGUMENTS;

        // Parse out mRamptime (optional)
        // and clip to 0..60sec
        int mRamptime = 0;
        if (argc == 5) {
            mRamptime = atoi(argv[4]);
            if (mRamptime < 0)
                return CMD_INVALID_ARGUMENTS;
            if (mRamptime > 60000)
                return CMD_INVALID_ARGUMENTS;
        }

        task_vsi_set(vPercent, freq, (double) mRamptime / 1000.0);

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/usr/demo/task_cc.h<|end_filename|>
#ifndef TASK_CC_H
#define TASK_CC_H

#include "sys/scheduler.h"
#include <stdint.h>
#include <stdlib.h>

#define TASK_CC_UPDATES_PER_SEC (10000)
#define TASK_CC_INTERVAL_USEC   (USEC_IN_SEC / TASK_CC_UPDATES_PER_SEC)

void task_cc_callback(void *arg);

void task_cc_init(int cc_idx);
void task_cc_deinit(int cc_idx);

void task_cc_vdc_set(int cc_idx, double vdc);
void task_cc_pwm(int cc_idx, uint8_t phase, uint8_t pwm_chnl);
void task_cc_adc(int cc_idx, uint8_t phase, uint8_t adc_chnl, double adc_gain, double adc_offset);
void task_cc_tune(int cc_idx, double Rs, double Ld, double Lq, double bw);
void task_cc_setmode(int cc_idx, char *argMode);
void task_cc_set_currents(int cc_idx, double Id_star, double Iq_star);
void task_cc_set_omega(int cc_idx, double omega_e);
void task_cc_set_theta(int cc_idx, double theta_e);

#endif // TASK_CC_H


<|start_filename|>sdk/basic/bsp/pwm.c<|end_filename|>
#include "pwm.h"
#include "xil_io.h"
#include <stdio.h>

#define PWM_BASE_ADDR		(0x43C20000)

static uint8_t carrier_divisor;
static uint16_t carrier_max;
static uint16_t deadtime;

// switching_freq = (200e6 / divisor) / (2*carrier_max)
// or
// carrier_max = ((200e6 / divisor) / (switching_freq)) / 2

void pwm_init(void)
{
	printf("PWM:\tInitializing...\n");

	pwm_set_switching_freq(100000.0);
//	pwm_set_carrier_divisor(0);
//	pwm_set_carrier_max(255);

	pwm_set_deadtime_ns(100);

	// Turn off all PWM outputs
	for (int i = 0; i < 24; i++) {
		pwm_set_duty_raw(i, 0);
	}
}

void pwm_set_switching_freq(double freq_hz)
{
	printf("PWM:\tSetting switching freq to %f...\n", freq_hz);

	// NOTE: freq_hz can be in range:
	// 1526Hz ... 100MHz

	// Always set carrier_divisor to 0... anything else reduces resolution!
	pwm_set_carrier_divisor(0);

	// Calculate what the carrier_max should be to achieve the right switching freq
	carrier_max = (uint16_t) (((200e6 / (carrier_divisor + 1)) / (freq_hz)) / 2);
	pwm_set_carrier_max(carrier_max);

	printf("PWM:\tCarrier divisor: %d, carrier_max: %d\n", carrier_divisor, carrier_max);
}

void pwm_set_duty(uint8_t idx, double duty)
{
	if (duty >= 1.0) {
		pwm_set_duty_raw(idx, carrier_max);
	} else if (duty <= 0.0) {
		pwm_set_duty_raw(idx, 0);
	} else {
		pwm_set_duty_raw(idx, duty * carrier_max);
	}

}

void pwm_set_duty_raw(uint8_t idx, uint16_t value)
{
	// Write to offset 0 to control PWM 0
	Xil_Out32(PWM_BASE_ADDR + (idx * sizeof(uint32_t)), value);
}

void pwm_set_carrier_divisor(uint8_t divisor)
{
	carrier_divisor = divisor;

	// Write to slave reg 24 to set triangle carrier clk divisor
	Xil_Out32(PWM_BASE_ADDR + (24 * sizeof(uint32_t)), divisor);
}

void pwm_set_carrier_max(uint16_t max)
{
	carrier_max = max;

	// Write to slave reg 25 to set triangle carrier max value
	Xil_Out32(PWM_BASE_ADDR + (25 * sizeof(uint32_t)), max);
}

void pwm_set_deadtime_ns(uint16_t time_ns)
{
	printf("PWM:\tSetting deadtime to %d ns...\n", time_ns);

	deadtime = time_ns / 5;

	// NOTE: FPGA enforces minimum register value of 5
	// This should prevent shoot-through events.

	// Write to slave reg 26 to set deadtime value
	Xil_Out32(PWM_BASE_ADDR + (26 * sizeof(uint32_t)), deadtime);
}

//void inverter_get_status(uint8_t idx, inverter_status_t *status)
//{
//
//}


<|start_filename|>sdk/bare/common/usr/pcbtest/cmd/cmd_test.c<|end_filename|>
#ifdef APP_PCBTEST

#include "usr/pcbtest/cmd/cmd_test.h"
#include "drv/analog.h"
#include "drv/pwm.h"
#include "sys/commands.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include "usr/pcbtest/sm_test.h"
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "analog", "Print all analog voltages" },
    { "auto <quiet|loud> <num_passes> <#[T|B]> <VDRIVE>",
      "Perform automated test of specified power stack port (e.g. 1T is top-left)" },
};

void cmd_test_register(void)
{
    commands_cmd_init(&cmd_entry, "test", "Test commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_test);

    // Register the command with the system
    commands_cmd_register(&cmd_entry);
}

int cmd_test(int argc, char **argv)
{
    if (argc == 2 && STREQ("analog", argv[1])) {
        // Print analog voltages
        for (int i = 0; i < ANALOG_NUM_CHANNELS; i++) {
            float voltage;
            analog_getf(i, &voltage);
            debug_printf("%d: %8.4f\r\n", i, voltage);
        }

        return CMD_SUCCESS;
    }

    if (argc == 6 && STREQ("auto", argv[1])) {
        // Parse verboseness mode
        bool quiet_mode;
        if (STREQ("quiet", argv[2])) {
            quiet_mode = true;
        } else if (STREQ("loud", argv[2])) {
            quiet_mode = false;
        } else {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse num passes
        int num_passes = atoi(argv[3]);
        if (num_passes <= 0 || num_passes >= 1e3) {
            return CMD_INVALID_ARGUMENTS;
        }

        if (strlen(argv[4]) == 2) {
            // Parse stack number
            int stack = argv[4][0] - '0';
            if (stack < 1 || stack > 4) {
                return CMD_INVALID_ARGUMENTS;
            }

            // Parse top or bottom (T or B)
            char tb = argv[4][1];
            bool valid = (tb == 'T') | (tb == 'B');
            if (!valid) {
                return CMD_INVALID_ARGUMENTS;
            }

            // Parse VDRIVE voltage
            double vdrive = strtod(argv[5], NULL);
            if (vdrive < 5.0 || vdrive > 18.0) {
                return CMD_INVALID_ARGUMENTS;
            }

            if (sm_test_start_auto_test(quiet_mode, num_passes, stack, tb, vdrive) != SUCCESS) {
                return CMD_FAILURE;
            }

            return CMD_SUCCESS;
        }
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // APP_PCBTEST


<|start_filename|>sdk/bare/common/sys/cmd/cmd_help.c<|end_filename|>
#include "cmd_help.h"
#include "sys/commands.h"
#include <stdint.h>
#include <string.h>

static command_entry_t cmd_entry;

void cmd_help_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "help", "Display this help message", NULL, 0, cmd_help);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

//
// Handles the 'help' command
//
int cmd_help(int argc, char **argv)
{
    if (argc > 1) {
        return CMD_INVALID_ARGUMENTS;
    }

    commands_display_help();

    return CMD_SUCCESS_QUIET;
}


<|start_filename|>sdk/basic/bsp/bsp.h<|end_filename|>
#ifndef BSP_H
#define BSP_H

#include "encoder.h"
#include "analog.h"
#include "io.h"
#include "gpio.h"
#include "pwm.h"
#include "timer.h"
#include "uart.h"

void bsp_init(void);

#endif // BSP_H


<|start_filename|>sdk/bare/user/usr/beta_labs/msf.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/msf.h"

// Motion state filter gains with Ts = 0.00025
// Bandwidth tuned to 200.0 Hz
#define Ts     (0.00025)
#define Ksf2Ts (1.226383390687582)
#define Ksf1   (841.3256566809094)

// Output States
static double msf_omega_m_dot;
static double msf_omega_m;
static double msf_omega_m_ave;
static double msf_delta_theta;

// Internal States
static double msf_omega_m_last;
static double msf_omega_m_dot_next;
static double msf_omega_m_next;
static double msf_omega_err;
static double msf_omega_dot_err;
static double msf_delta_omega;

double msf_get_omega_m(void)
{
    return msf_omega_m;
}

double msf_get_omega_m_dot(void)
{
    return msf_omega_m_dot;
}

void msf_init(void)
{
    msf_omega_m = 0.0;
    msf_omega_m_dot_next = 0.0;
    msf_omega_m_next = 0.0;
}

double msf_update(double omega_m_star)
{
    msf_omega_m_last = msf_omega_m;
    msf_omega_m_dot = msf_omega_m_dot_next;
    msf_omega_m = msf_omega_m_next;

    msf_omega_err = (omega_m_star - msf_omega_m);
    msf_omega_dot_err = Ksf1 * msf_omega_err - msf_omega_m_dot;
    msf_omega_m_dot_next = msf_omega_m_dot + msf_omega_dot_err * Ksf2Ts;
    msf_delta_omega = Ts * msf_omega_m_dot;
    msf_omega_m_next = msf_delta_omega + msf_omega_m;
    msf_omega_m_ave = 0.5 * (msf_omega_m + msf_omega_m_last);
    msf_delta_theta = msf_omega_m_ave * Ts;

    return msf_delta_theta;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/sys/task_stats.h<|end_filename|>
#ifndef TASK_STATS_H
#define TASK_STATS_H

#include <stdbool.h>
#include <stdint.h>

#include "sys/statistics.h"

typedef struct {
    bool enabled;
    bool is_init;
    uint32_t last_fpga_ticks;

    statistics_t loop_time;
    statistics_t run_time;
} task_stats_t;

void task_stats_init(task_stats_t *stats);
void task_stats_reset(task_stats_t *stats);

void task_stats_print(task_stats_t *stats);

static inline void task_stats_enable(task_stats_t *stats)
{
    stats->enabled = true;
}

static inline void task_stats_disable(task_stats_t *stats)
{
    stats->enabled = false;
}

/**
 * Before the task you are analyzing runs, run this function. If it is enabled it will:
 *
 * 1. Measure the time at the start of the function in FPGA clock ticks
 * 2. Calculate the delta from the last time period in us.
 * 3. Calculate the statistics on the loop timing (time between callings of this function)
 * 4. Save the current time as the last fpga ticks time.
 *
 * To run the runtime (will include the time it takes to calculate statistics).
 * you must run task_stats_post_task() after the task runs.
 */
void task_stats_pre_task(task_stats_t *stats);

/**
 * Run this after the function you are analyzing. If it is enabled.
 *
 * It will calculate the time since task_stats_pre_task() was run.
 * Calculate runtime statistics on them.
 */
void task_stats_post_task(task_stats_t *stats);

#endif // TASK_STATS_H


<|start_filename|>sdk/bare/common/sys/cmd/cmd_log.h<|end_filename|>
#ifndef CMD_LOG_H
#define CMD_LOG_H

void cmd_log_register(void);

int cmd_log(int argc, char **argv);

#endif // CMD_LOG_H


<|start_filename|>sdk/bare/common/sys/scheduler.c<|end_filename|>
#include "sys/scheduler.h"
#include "drv/hardware_targets.h"
#include "drv/io.h"
#include "drv/led.h"
#include "drv/motherboard.h"
#include "drv/timer.h"
#include "drv/watchdog.h"
#include "usr/user_config.h"
#include <stdbool.h>
#include <stdio.h>

// Used to give each task a unique ID
static int next_tcb_id = 0;

// Linked list of all registered tasks
static task_control_block_t *tasks = NULL;

// For debugging, this variable is set to point
// at the currently running task
static task_control_block_t *running_task = NULL;

// Incremented every SysTick interrupt to track time
static volatile uint32_t elapsed_usec = 0;

static bool tasks_running = false;
static volatile bool scheduler_idle = false;

void scheduler_timer_isr(void *userParam, uint8_t TmrCtrNumber)
{
#if USER_CONFIG_ENABLE_TIME_QUANTUM_CHECKING == 1
    // We should be done running tasks in a time slice before this fires,
    // so if tasks are still running, we consumed too many cycles per slice
    if (tasks_running) {
        // Use raw printf so this goes directly to the UART device
        printf("ERROR: OVERRUN SCHEDULER TIME QUANTUM!\n");
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
        io_led_color_t color;
        color.r = 255;
        color.g = 0;
        color.b = 0;
        io_led_set(&color);
#elif USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
        led_set_color(0, LED_COLOR_RED);
        led_set_color(1, LED_COLOR_RED);
        led_set_color(2, LED_COLOR_RED);
        led_set_color(3, LED_COLOR_RED);
#endif // USER_CONFIG_HARDWARE_TARGET

        // Hang here so the user can debug why the code took so long
        // and overran the time slice! See the `running_task` variable.
        while (1) {
        }
    }
#endif // USER_CONFIG_ENABLE_TIME_QUANTUM_CHECKING

    elapsed_usec += SYS_TICK_USEC;
    scheduler_idle = false;
}

uint32_t scheduler_get_elapsed_usec(void)
{
    return elapsed_usec;
}

void scheduler_init(void)
{
    printf("SCHED:\tInitializing scheduler...\n");

    // Start system timer for periodic interrupts
    timer_init(scheduler_timer_isr, SYS_TICK_USEC);
    printf("SCHED:\tTasks per second: %d\n", SYS_TICK_FREQ);
}

void scheduler_tcb_init(
    task_control_block_t *tcb, task_callback_t callback, void *callback_arg, const char *name, uint32_t interval_usec)
{
    tcb->id = next_tcb_id++;
    tcb->name = name;
    tcb->callback = callback;
    tcb->callback_arg = callback_arg;
    tcb->interval_usec = interval_usec;
    tcb->last_run_usec = 0;

#if USER_CONFIG_ENABLE_TASK_STATISTICS_BY_DEFAULT == 1
    tcb->stats.enabled = true;
#else
    tcb->stats.enabled = false;
#endif // USER_CONFIG_ENABLE_TASK_STATISTICS_BY_DEFAULT
}

int scheduler_tcb_register(task_control_block_t *tcb)
{
    // Don't let clients re-register their tcb
    if (tcb->is_registered) {
        return FAILURE;
    }

    // Mark as registered
    tcb->is_registered = true;

    if (tasks == NULL) {
        // There are no tasks in linked list
        tasks = tcb;
        tasks->next = NULL;
    } else {
        // Find end of list
        task_control_block_t *curr = tasks;
        while (curr->next != NULL) {
            curr = curr->next;
        }

        // Append new tcb to end of list
        curr->next = tcb;
        tcb->next = NULL;
    }

    return SUCCESS;
}

int scheduler_tcb_unregister(task_control_block_t *tcb)
{
    // Don't let clients unregister their already unregistered tcb
    if (!tcb->is_registered) {
        return FAILURE;
    }

    // Make sure list isn't empty
    if (tasks == NULL) {
        return FAILURE;
    }

    // Mark as unregistered
    tcb->is_registered = false;

    // Special case: trying to remove the head of the list
    if (tasks->id == tcb->id) {
        tasks = tasks->next;
        return SUCCESS;
    }

    // Now we know that 'tcb' to remove is NOT first node

    task_control_block_t *prev = NULL;
    task_control_block_t *curr = tasks;

    // Find spot in linked list to remove tcb
    while (curr->id != tcb->id) {
        prev = curr;
        curr = curr->next;
    }

    // 'curr' is now the one we want to remove!

    prev->next = curr->next;

    return SUCCESS;
}

bool scheduler_tcb_is_registered(task_control_block_t *tcb)
{
    return tcb->is_registered;
}

void scheduler_run(void)
{
    printf("SCHED:\tRunning scheduler...\n");

    // This is the main event loop that runs the device
    while (1) {
        uint32_t my_elapsed_usec = elapsed_usec;
        tasks_running = true;

        task_control_block_t *t = tasks;
        while (t != NULL) {
            uint32_t usec_since_last_run = my_elapsed_usec - t->last_run_usec;

            if (usec_since_last_run >= t->interval_usec) {
                // Time to run this task!
                task_stats_pre_task(&t->stats);
                running_task = t;
                t->callback(t->callback_arg);
                running_task = NULL;
                task_stats_post_task(&t->stats);

                t->last_run_usec = my_elapsed_usec;
            }

            // Go to next task in linked list
            t = t->next;
        }

        tasks_running = false;

#if USER_CONFIG_ENABLE_MOTHERBOARD_AUTO_TX == 1
        // Request motherboard to send its latest ADC sample data back to the AMDC
        //
        // NOTE: this is specifically before the while loop below so that the new
        // data arrives before it is needed in the next control loop.
        motherboard_request_new_data();
#endif

        // Wait here until unpaused (i.e. when SysTick fires)
        scheduler_idle = true;
        while (scheduler_idle) {
        }

#if USER_CONFIG_ENABLE_WATCHDOG == 1
        // Reset the watchdog timer after SysTick fires
        watchdog_reset();
#endif
    }
}


<|start_filename|>sdk/bare/common/drv/gpio.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

#ifndef GPIO_H
#define GPIO_H

#include <stdint.h>

void gpio_init(void);

void gpio_pin_read(uint8_t port, uint8_t pin, uint8_t *value);
void gpio_pin_write(uint8_t port, uint8_t pin, uint8_t value);

void gpio_port_read(uint8_t port, uint16_t *value);
void gpio_port_write(uint8_t port, uint16_t value);

void gpio_set_dir(uint8_t group, uint8_t dir);

#endif // GPIO_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/basic/src/project_settings.h<|end_filename|>
#ifndef __PROJECT_SETTINGS_H
#define __PROJECT_SETTINGS_H

#define NUM_ADCS 12
#define NUM_LEGS 20

//communications
#define MAX_BASE_SOCKETS	5
#define MAX_SEND_PACKET_SIZE 	(2*8192)

#define TRUE 1
#define FALSE 0

//general status codes
#define SUCCESS 1
#define INVALID_ARGUMENT -2
#define INVALID_OPERATION -3
#define NOT_FOUND			-4
#define OVERFLOW			-5

#endif //__PROJECT_SETTINGS_H



<|start_filename|>sdk/bare/user/usr/beta_labs/cmd/cmd_inv.h<|end_filename|>
#ifndef CMD_INV_H
#define CMD_INV_H

void cmd_inv_register(void);

int cmd_inv(int argc, char **argv);

#endif // CMD_INV_H


<|start_filename|>sdk/basic/control_apps/VSI_DQ_SVPWM.h<|end_filename|>
#ifndef __VSI_DQ_SVPWM_H
#define __VSI_DQ_SVPWM_H


#define PI 3.14159265
#define PI2 6.283185307179586
#define PI23 2.094395102393195
#define DQK 0.816496580927726

#define VSI_DQ_SVPWM_NUM_VSI 5

typedef struct pi_house
{
	pi_controller pi;
	controller_context io;
	controller_holder ctxt;
	double input;
	double output;
	double reference;
	u8 updated;	//set to TRUE everytime this controller output is updated, cleared everytime the output is used
	//1 to 1 double adc_scale;		//3 amps per volt, wound around twice gives me a factor of 3/2
} pi_house;

typedef u8 (*VSI_DQ_SVPWM_input)(void *arg, void *VSIcontext);
typedef void (*VSI_DQ_SVPWM_output)(void *arg, void *VSIcontext);

typedef struct VSI_context
{
	pi_house d;
	pi_house q;
	pi_house zero;
	u32 InputAtTickCount;		//the tick count of the last time the inputs were updated (to avoid unnecessary dq transformations)
	double Iabc[3];
	double Idq0[3];
	double dabc[3];
	double mabc[3];				//the 3 output modulation indices
	double dqComp[2];			//compensation d and q terms. These are added to the controller outputs before saturating the result.
	double theta_da;
	u8 bUseZeroSequence;		//set to 1 to use a zero sequence controller, otherwise 0
	VSI_DQ_SVPWM_input get_input;
	void * input_arg;
	VSI_DQ_SVPWM_output write_output;
	void * output_arg;
	u8 bSineTriangle;			//0 for CSVPWM, 1 for SineTriangle
} VSI_context;




//move these to a library:
int ChangeDQ_ReferenceFrame(double *new_dq0, double new_theta_da, double *old_dq0, double old_theta_da);
int SaturateDQVoltages(double *inputDQ, double *outputDQ, u8 bSineTriangle);
extern int DQ0_Transform(double theta_da, double *abc, double *dq0);
extern int InverseDQ0_Transform(double theta_da, double *abc, double *dq0);

extern int Start_VSI_DQ_SVPWM_Controller(u32 vsi);
extern int Stop_VSI_DQ_SVPWM_Controller(u32 vsi);
extern int Init_VSI_DQ_VSPWM(u8 StartAllControllers);
extern int VSI_DQ_VSPW_StopCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int VSI_DQ_VSPW_StartCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int VSI_DQ_VSPW_Command(const char * szCmd, char *szResponse, void *CommDevice);
extern int VSI_DQ_VSPW_SetCurrentRef(u32 vsi, u8 bDaxis, double reference);

extern VSI_context VSI_DQ_Cur_Controllers[VSI_DQ_SVPWM_NUM_VSI];
#endif


<|start_filename|>sdk/bare/common/usr/pcbtest/cmd/cmd_test.h<|end_filename|>
#ifndef CMD_TEST_H
#define CMD_TEST_H

void cmd_test_register(void);

int cmd_test(int argc, char **argv);

#endif // CMD_TEST_H


<|start_filename|>sdk/bare/common/sys/debug.h<|end_filename|>
#ifndef DEBUG_H
#define DEBUG_H

void debug_print(char *msg);
void debug_printf(const char *msg, ...);

#endif // DEBUG_H


<|start_filename|>sdk/basic/bsp/pwm.h<|end_filename|>
#ifndef PWM_H
#define PWM_H

#include <stdint.h>

//typedef enum {
//	INV_1 = 1,
//	INV_2,
//	INV_3,
//	INV_4,
//	INV_5,
//	INV_6,
//	INV_7,
//	INV_8,
//} inverter_e;
//
//typedef struct inverter_status_t {
//	uint8_t ready;
//	uint8_t fault_desat;
//	uint8_t fault_temp;
//} inverter_status_t;

void pwm_init(void);

void pwm_set_switching_freq(double freq_hz);

void pwm_set_duty_raw(uint8_t idx, uint16_t value);
void pwm_set_duty(uint8_t idx, double duty);

void pwm_set_carrier_divisor(uint8_t divisor);
void pwm_set_carrier_max(uint16_t max);
void pwm_set_deadtime_ns(uint16_t deadtime);

//void inverter_set_duty_ratio(inverter_e inv, uint8_t pwm_idx, uint8_t value);
//void inverter_set_dead_time (uint8_t inv_idx, uint8_t pwm_idx, uint8_t value);

//void inverter_get_status(uint8_t idx, inverter_status_t *status);

#endif // PWM_H


<|start_filename|>sdk/bare/user/usr/beta_labs/task_cc.c<|end_filename|>
#ifdef APP_BETA_LABS

#include "usr/beta_labs/task_cc.h"
#include "drv/analog.h"
#include "drv/dac.h"
#include "drv/encoder.h"
#include "drv/io.h"
#include "drv/pwm.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/injection.h"
#include "sys/scheduler.h"
#include "sys/transform.h"
#include "usr/beta_labs/bemfo.h"
#include "usr/beta_labs/cmd/cmd_cc.h"
#include "usr/beta_labs/co.h"
#include "usr/beta_labs/inverter.h"
#include "usr/beta_labs/machine.h"
#include "usr/beta_labs/task_mo.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

// Controller tuning
#define Wb   (controller_bw * PI2) // rad/s
#define Ts   (1.0 / TASK_CC_UPDATES_PER_SEC)
#define Kp_d (Wb * Ld_HAT)
#define Kp_q (Wb * Lq_HAT)
#define Ki_d ((Rs_HAT / Ld_HAT) * Kp_d)
#define Ki_q ((Rs_HAT / Lq_HAT) * Kp_q)

// Variables for logging
double LOG_Id = 0.0;
double LOG_Iq = 0.0;
double LOG_Id_star = 0.0;
double LOG_Iq_star = 0.0;
double LOG_Vd_star = 0.0;
double LOG_Vq_star = 0.0;
double LOG_theta_e_enc = 0.0;
double LOG_theta_e_hat = 0.0;
double LOG_omega_m_hat = 0.0;
double LOG_omega_m_enc = 0.0;

// Commands for Id and Iq -- Idq*
static double Id_star = 0.0;
static double Iq_star = 0.0;

static int32_t dq_offset = 9980;

// Note: user should override this initial value
static double controller_bw = 1.0;

// Used to switch controller to use estimated position / speed
static uint8_t theta_e_src_use_encoder = 1;
static uint8_t omega_e_src_use_encoder = 1;

// Static variables for controller
static double Id_err_acc;
static double Iq_err_acc;

// Injection contexts for current controller
inj_ctx_t cc_inj_ctx_Id_star;
inj_ctx_t cc_inj_ctx_Iq_star;
inj_ctx_t cc_inj_ctx_Vd_star;
inj_ctx_t cc_inj_ctx_Vq_star;

// Scheduler TCB which holds task "context"
static task_control_block_t tcb;

uint8_t task_cc_is_inited(void)
{
    return scheduler_tcb_is_registered(&tcb);
}

static void _clear_state(void)
{
    // Clear controller static variables
    Id_err_acc = 0.0;
    Iq_err_acc = 0.0;

    bemfo_init();
}

void task_cc_init(void)
{
    // Register task with scheduler
    scheduler_tcb_init(&tcb, task_cc_callback, NULL, "cc", TASK_CC_INTERVAL_USEC);
    scheduler_tcb_register(&tcb);

    // Initialize all cc signal injection points
    injection_ctx_init(&cc_inj_ctx_Id_star, "Id*");
    injection_ctx_init(&cc_inj_ctx_Iq_star, "Iq*");
    injection_ctx_init(&cc_inj_ctx_Vd_star, "Vd*");
    injection_ctx_init(&cc_inj_ctx_Vq_star, "Vq*");

    // Register all cc signal injection points
    injection_ctx_register(&cc_inj_ctx_Id_star);
    injection_ctx_register(&cc_inj_ctx_Iq_star);
    injection_ctx_register(&cc_inj_ctx_Vd_star);
    injection_ctx_register(&cc_inj_ctx_Vq_star);

    // Clear controller state
    _clear_state();
}

void task_cc_deinit(void)
{
    // Unregister task with scheduler
    scheduler_tcb_unregister(&tcb);

    // Unregister all cc signal injection points
    injection_ctx_unregister(&cc_inj_ctx_Id_star);
    injection_ctx_unregister(&cc_inj_ctx_Iq_star);
    injection_ctx_unregister(&cc_inj_ctx_Vd_star);
    injection_ctx_unregister(&cc_inj_ctx_Vq_star);

    // Clear all injection points
    injection_ctx_clear(&cc_inj_ctx_Id_star);
    injection_ctx_clear(&cc_inj_ctx_Iq_star);
    injection_ctx_clear(&cc_inj_ctx_Vd_star);
    injection_ctx_clear(&cc_inj_ctx_Vq_star);

    // Clear controller state
    _clear_state();
}

double task_cc_get_theta_e_enc(void)
{
    // Get raw encoder position
    uint32_t position;
    encoder_get_position(&position);

    // Add offset (align to DQ frame)
    position += dq_offset;

    while (position >= ENCODER_PULSES_PER_REV) {
        position -= ENCODER_PULSES_PER_REV;
    }

    double theta_e_enc = 0.0;

    // Convert to radians
    theta_e_enc = (double) PI2 * ((double) position / (double) ENCODER_PULSES_PER_REV);

    // Multiple by pole pairs to convert mechanical to electrical degrees
    theta_e_enc *= POLE_PAIRS;

    // Mod by 2 pi
    while (theta_e_enc > PI2) {
        theta_e_enc -= PI2;
    }

    return theta_e_enc;
}

static void _get_Iabc(double *Iabc)
{
    // Read from ADCs
    float Iabc_f[3];
    analog_getf(CC_PHASE_A_ADC, &Iabc_f[0]);
    analog_getf(CC_PHASE_B_ADC, &Iabc_f[1]);
    analog_getf(CC_PHASE_C_ADC, &Iabc_f[2]);

    // Convert ADC values to raw currents
    Iabc[0] = ((double) Iabc_f[0] * ADC_TO_AMPS_PHASE_A_GAIN) + ADC_TO_AMPS_PHASE_A_OFFSET;
    Iabc[1] = ((double) Iabc_f[1] * ADC_TO_AMPS_PHASE_B_GAIN) + ADC_TO_AMPS_PHASE_B_OFFSET;
    Iabc[2] = ((double) Iabc_f[2] * ADC_TO_AMPS_PHASE_C_GAIN) + ADC_TO_AMPS_PHASE_C_OFFSET;
}

void task_cc_callback(void *arg)
{
    // -------------------
    // Inject signals into Idq*
    // (constants, chirps, noise, etc)
    // -------------------
    injection_inj(&Id_star, &cc_inj_ctx_Id_star, Ts);
    injection_inj(&Iq_star, &cc_inj_ctx_Iq_star, Ts);

    // -------------------
    // Update theta_e using either encoder or estimation
    // -------------------
    double theta_e = 0.0;
    if (theta_e_src_use_encoder) {
        theta_e = task_cc_get_theta_e_enc();
    } else {
        theta_e = bemfo_get_theta_e_hat();
    }

    LOG_theta_e_enc = task_cc_get_theta_e_enc();
    LOG_theta_e_hat = bemfo_get_theta_e_hat();

    LOG_omega_m_enc = task_mo_get_omega_m();
    LOG_omega_m_hat = bemfo_get_omega_m_hat();

    // ------------------------------
    // Update omega_e_avg in rads/sec
    // ------------------------------
    double omega_e_avg = 0.0;
    if (omega_e_src_use_encoder) {
        omega_e_avg = task_mo_get_omega_e();
    } else {
        omega_e_avg = bemfo_get_omega_e_hat();
    }

    // ----------------------
    // Get current values
    // ----------------------
    double Iabc[3];
    _get_Iabc(Iabc);

    // ---------------------
    // Convert ABC to DQ
    // ---------------------
    double Ixyz[3]; // alpha beta gamma currents
    double Idq0[3]; // d q 0 currents
    transform_clarke(TRANS_DQZ_C_INVARIANT_POWER, Iabc, Ixyz);
    transform_park(theta_e, Ixyz, Idq0);

    // -----------------------------
    // Run through block diagram of CVCR to get Vdq_star
    // -----------------------------
    double Id = Idq0[0];
    double Iq = Idq0[1];

    // d-axis
    double Id_err;
    double Vd_star;
    Id_err = Id_star - Id;
    Id_err_acc += Id_err;
    Vd_star = (Kp_d * Id_err) + (Ki_d * Ts * Id_err_acc) - (omega_e_avg * Kp_q * Ts * Iq_err_acc);

    // q-axis
    double Iq_err;
    double Vq_star;
    Iq_err = Iq_star - Iq;
    Iq_err_acc += Iq_err;
    Vq_star = (Kp_q * Iq_err) + (Ki_q * Ts * Iq_err_acc) + (omega_e_avg * Kp_d * Ts * Id_err_acc)
        + (omega_e_avg * Lambda_pm_HAT);

    // -------------------
    // Inject signals into Vdq*
    // (constants, chirps, noise, etc)
    // -------------------
    injection_inj(&Vd_star, &cc_inj_ctx_Vd_star, Ts);
    injection_inj(&Vq_star, &cc_inj_ctx_Vq_star, Ts);

    // --------------------------------
    // Perform inverse DQ transform of Vdq_star
    // --------------------------------
    double Vabc_star[3];
    double Vdq0[3];
    Vdq0[0] = Vd_star;
    Vdq0[1] = Vq_star;
    Vdq0[2] = 0.0;
    transform_dqz_inverse(TRANS_DQZ_C_INVARIANT_POWER, theta_e, Vabc_star, Vdq0);

    // ------------------------------------
    // Saturate Vabc_star to inverter bus voltage
    // ------------------------------------
    inverter_saturate_to_Vdc(&Vabc_star[0]);
    inverter_saturate_to_Vdc(&Vabc_star[1]);
    inverter_saturate_to_Vdc(&Vabc_star[2]);

    // --------------------------------------
    // Write voltages out to PWM hardware
    // --------------------------------------
    inverter_set_voltage(CC_PHASE_A_PWM_LEG_IDX, Vabc_star[0], Iabc[0]);
    inverter_set_voltage(CC_PHASE_B_PWM_LEG_IDX, Vabc_star[1], Iabc[1]);
    inverter_set_voltage(CC_PHASE_C_PWM_LEG_IDX, Vabc_star[2], Iabc[2]);

    // -------------------
    // Store LOG variables
    // -------------------
    LOG_Vd_star = Vd_star;
    LOG_Vq_star = Vq_star;
    LOG_Id_star = Id_star;
    LOG_Iq_star = Iq_star;
    LOG_Id = Id;
    LOG_Iq = Iq;

    // -------------------
    // Self-sensing:
    // -------------------

    // Back-EMF State Filter
    co_update(Idq0, Vdq0, omega_e_avg);

    // Pull out Esal (sync ref frame)
    double Esal_d_hat, Esal_q_hat;
    co_get_Esal_hat(&Esal_d_hat, &Esal_q_hat);

    // Convert Esal to stationary ref frame (alpha-beta)
    //
    // (NOTE: this is hacking the transform_park(...) function,
    //        since I didn't have an inverse one. Just rotate by -theta)
    double Esal_dq0[3];
    double Esal_xyz[3];
    Esal_dq0[0] = Esal_d_hat;
    Esal_dq0[1] = Esal_q_hat;
    Esal_dq0[2] = 0.0;
    transform_park(-theta_e, Esal_dq0, Esal_xyz);
    double Esal_alpha = Esal_xyz[0];
    double Esal_beta = Esal_xyz[1];

    // Back-EMF Tracking Position State Filter
    bemfo_update(Esal_alpha, Esal_beta, 0.0);
}

void task_cc_clear(void)
{
    Id_star = 0.0;
    Iq_star = 0.0;

    inverter_set_voltage(CC_PHASE_A_PWM_LEG_IDX, 0.0, 0.0);
    inverter_set_voltage(CC_PHASE_B_PWM_LEG_IDX, 0.0, 0.0);
    inverter_set_voltage(CC_PHASE_C_PWM_LEG_IDX, 0.0, 0.0);
}

void task_cc_set_dq_offset(int32_t offset)
{
    dq_offset = offset;
}

void task_cc_set_bw(double bw)
{
    controller_bw = bw;
}

void task_cc_set_Iq_star(double value)
{
    Iq_star = value;
}

void task_cc_set_Id_star(double value)
{
    Id_star = value;
}

void task_cc_set_theta_e_src(uint8_t use_encoder)
{
    theta_e_src_use_encoder = use_encoder;
}

void task_cc_set_omega_e_src(uint8_t use_encoder)
{
    omega_e_src_use_encoder = use_encoder;
}

#endif // APP_BETA_LABS


<|start_filename|>sdk/bare/common/usr/pcbtest/app_pcbtest.h<|end_filename|>
#ifndef APP_PCBTEST_H
#define APP_PCBTEST_H

void app_pcbtest_init(void);

#endif // APP_PCBTEST_H


<|start_filename|>sdk/basic/control_apps/TorqueVSI.c<|end_filename|>
/*
 * TorqueVSI.c
 *
 *  Created on: Jul 17, 2014
 *      Author: sever212
 */

#include "../src/project_include.h"


#define OMEGA_DA_RAMP_RATE 0.001

#define DEFAULT_T_SINE_TRIANGLE 1
#define MAX_PHASE_CURRENT 1
const u8 TVSI_INDEX[NUM_TVSI] = {2};



TVSI_context TVSIs[NUM_TVSI];

#define IField 1

#if defined (USE_VSI_CC)
	#define TVSI_KiTs_Q_GAIN 0.001675516 //0.0010053 //0.001675 //0.0032 //0.0063 //Old val (works): 0.0151 //0.0008 //0.005 //Ki_t*Ts/Vbus
	#define TVSI_KiTs_D_GAIN 0.001675516 //0.0010053 //0.001675 //0.0032 //0.0063 //0.0151 //0.0008 //0.005 //Ki_t*Ts/Vbus

	#define TVSI_KP_Q_GAIN 0.1590065 //0.09540388 //0.159006 //0.2981 //0.5963 //Old val (works): 1.431 //.076 //0.452 //Kp_t/Vbus
	#define TVSI_KP_D_GAIN 0.1590065 //0.09540388 //0.159006 //0.2981 //0.5963 //1.431 //.076 //0.452 //Kp_t/Vbus


	#define T0_U_ADC 11
	#define T0_V_ADC 12

	#define T0_U_LEG 11
	#define T0_V_LEG 12
	#define T0_W_LEG 13

	#define Lq_div_Vbus 0.00002531 //0.000126533 //Lq/Vbus
	#define Ld_div_Vbus 0.00002531 //0.000126533 //Ld/Vbus
	#define sqrt6_Lfg_div_Vbus 0.00009219 //0.0004609576 //sqrt(6)*Lfg_m/Vbus

#endif

#if defined(TVSI_STATIONARY_FRAME)
	const u8 TVSI_STATIONARY_INDEX[NUM_TVSI] = {3};

	#define TVSI_KiTs_X_GAIN TVSI_KiTs_D_GAIN
	#define TVSI_KiTs_Y_GAIN TVSI_KiTs_D_GAIN

	#define TVSI_KP_X_GAIN 0
	#define TVSI_KP_Y_GAIN 0

	static u8 T_VSI_Input_Stationary(void *arg, void *VSIContext);
	static void T_VSI_Output_Stationary(void *arg, void *VSIContext);

	//logging
	u32 Itx_measured, Ity_measured;
	u32 Itx_ref, Ity_ref;
	u32 x_out_ref, y_out_ref;
	u32 dxy_out_ref, qxy_out_ref;
#endif

#if defined (USE_H_BRIDGE_CC) || defined(USE_H_BRIDGE_DQ_CC)
	//wc = 2*pi*750
	#define TVSI_KiTs_Q_GAIN 0.00837758 //0.0047 //0.0151 //Ki_t*Ts/Vbus
	#define TVSI_KiTs_D_GAIN 0.00837758 //0.0047 //0.0151 //Ki_t*Ts/Vbus

	#define TVSI_KP_Q_GAIN 0.7950 //0.13499 //1.431 //Kp_t/Vbus
	#define TVSI_KP_D_GAIN 0.7950 //0.13499 //1.431 //Kp_t/Vbus

	#define T0_U_ADC 11
	#define T0_V_ADC 12

	#define T0_U_LEG 1
	#define T0_V_LEG 2
	#define T0_W_LEG 3

	#define Lq_div_Vbus 0.000126533 //Lq/Vbus
	#define Ld_div_Vbus 0.000126533 //Ld/Vbus
	#define sqrt6_Lfg_div_Vbus 0.0004609576 //sqrt(6)*Lfg_m/Vbus
#endif

s32 OmegaDA_Log = 0;

static u8 T_VSI_Input(void *arg, void *VSIContext);
static void T_VSI_Output(void *arg, void *VSIContext);

static double GetTheta_DA(double theta_da, double omega_da, u32 LastTickCNT)
{
	double update_da = 0;
	u32 elapsed;
	elapsed = TimeElapsed10us(LastTickCNT);
	update_da = ((double) omega_da*((double)elapsed)/((double)100000.0));
	theta_da += update_da;
	while (theta_da > PI2)
		theta_da = theta_da - PI2;

	return theta_da;
}
u32 t_vsi_theta_da;

/*****************************
 * Init_TorqueVSI()
 * Sets up the controllers memory to default values
 * and optionally starts the controllers running.
 * The default settings can be overridden by manually
 * editing the VSI_DQ_Cur_Controllers structure.
 *
 *StartAllControllers: this is ignored
 *returns a success code
 */
int Init_TorqueVSI(u8 StartAllControllers)
{
	int i;

#if defined(USE_H_BRIDGE_CC)
	Init_VSI_DQ_VSPWM(FALSE);		//We need to init the VSI controllers in the HBridge case because the position controller has not done that.
#endif

	for (i = 0; i < NUM_TVSI; i++)
	{
		TVSIs[i].MaxPhaseCurrent = MAX_PHASE_CURRENT;
		TVSIs[i].InputAtTickCount = MainTimerTimerTick;
		TVSIs[i].OmegaDA_Cmd = 0;
		TVSIs[i].OmegaDA_Current = 0;
		TVSIs[i].VSI = &VSI_DQ_Cur_Controllers[TVSI_INDEX[i]];
		TVSIs[i].VSI->theta_da = 0;
		TVSIs[i].Idq_ref[0] = 0;
		TVSIs[i].Idq_ref[1] = 0;

		//assume init_vsi_dq_VSPWM has already been called
		TVSIs[i].VSI->bSineTriangle = DEFAULT_T_SINE_TRIANGLE;

		//controller gains
		TVSIs[i].VSI->d.pi.kits = TVSI_KiTs_D_GAIN;
		TVSIs[i].VSI->d.pi.kp = TVSI_KP_D_GAIN;
		TVSIs[i].VSI->q.pi.kits = TVSI_KiTs_Q_GAIN;
		TVSIs[i].VSI->q.pi.kp = TVSI_KP_Q_GAIN;

		//callbacks
		TVSIs[i].VSI->input_arg = (void*) i;
		TVSIs[i].VSI->output_arg = (void*) i;
		TVSIs[i].VSI->get_input = T_VSI_Input;
		TVSIs[i].VSI->write_output = T_VSI_Output;

#if defined (TVSI_STATIONARY_FRAME)
		TVSIs[i].VSI_stationary = &VSI_DQ_Cur_Controllers[TVSI_STATIONARY_INDEX[i]];
		TVSIs[i].VSI_stationary->theta_da = 0;

		//assume init_vsi_dq_VSPWM has already been called
		TVSIs[i].VSI_stationary->bSineTriangle = DEFAULT_T_SINE_TRIANGLE;

		//controller gains
		TVSIs[i].VSI_stationary->d.pi.kits = TVSI_KiTs_X_GAIN;
		TVSIs[i].VSI_stationary->d.pi.kp = TVSI_KP_X_GAIN;
		TVSIs[i].VSI_stationary->q.pi.kits = TVSI_KiTs_Y_GAIN;
		TVSIs[i].VSI_stationary->q.pi.kp = TVSI_KP_Y_GAIN;

		//callbacks
		TVSIs[i].VSI_stationary->input_arg = (void*) i;
		TVSIs[i].VSI_stationary->output_arg = (void*) i;
		TVSIs[i].VSI_stationary->get_input = T_VSI_Input_Stationary;
		TVSIs[i].VSI_stationary->write_output = T_VSI_Output_Stationary;

		Start_VSI_DQ_SVPWM_Controller(TVSI_STATIONARY_INDEX[i]+1);
#endif

		//start up the current controller
		Start_VSI_DQ_SVPWM_Controller(TVSI_INDEX[i]+1);
	}
	return SUCCESS;
}


/*****************************
 * Stop_TorqueVSI_Controller()
 * Unregisters the VSI's belonging to the TVSI controller.
 *
 *tvsi: the TVSI number to unregister (1 - NUM_TVSI)
 *returns a success code
 */
int Stop_TorqueVSI_Controller(u32 tvsi)
{
	u32 i = tvsi-1;
	if (i >= NUM_TVSI)
		return INVALID_ARGUMENT;

#if defined(TVSI_STATIONARY_FRAME)
	Stop_VSI_DQ_SVPWM_Controller(TVSI_STATIONARY_INDEX[i]+1);
#endif
	return Stop_VSI_DQ_SVPWM_Controller(TVSI_INDEX[i]+1);
}


/*****************************
 * TorqueVSI_StopCommand()
 * Process an ascii command for stopping the torque VSI current controllers
 * and any associated current controllers
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int TorqueVSI_StopCommand(const char *szCmd, char *szResponse, void *CommDevice)
{
	int i;
	for (i=0; i< 4; i++)
	{
		Stop_TorqueVSI_Controller(i+1);
	}
	strcpy(szResponse, "OK");
	return strlen(szResponse);
}


/*****************************
 * TorqueVSI_StartCommand()
 * Process an ascii command for starting the torque VSI controllers
 * Init_PosContTry1 is called
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int TorqueVSI_StartCommand(const char *szCmd, char *szResponse, void *CommDevice)
{

	/*if (strstr(szCmd, "FORCEI"))
	{
		strcpy(szResponse, "FORCEI=1");
		bForceXY_Currents = TRUE;
	}
	else
	{
		bForceXY_Currents = FALSE;*/
		strcpy(szResponse, "OK");
	//}


	Init_TorqueVSI(TRUE);

	return strlen(szResponse);
}


/*****************************
 * PosContTry1_Command()
 * Process an ascii command for setting a reference position (or current)
 * Syntax: n,v,val
 *  n is the controller index (1 or 2),
 *  v is the variable to set (d for d axis current, q for q axis current, w for reference OmegaDa command)
 *	val is the value (*128)
 *
 *szCmd: incoming command
 *szResponse: the response to the command
 *CommDevice: pointer to information about the communication device
 * this was received on
 */
int TorqueVSI_Command(const char * szCmd, char *szResponse, void *CommDevice)
{
	u8 num = atoi(szCmd);
	u8 bError = FALSE;
	double ref;
	if ((num != 1) )
		bError = TRUE;
	else if (szCmd[1] != ',')
		bError = TRUE;
	else
	{
		u8 v = szCmd[2];
		if ((v != 'd')  && (v != 'q') && (v != 'w'))
			bError = TRUE;
		else if (szCmd[3] != ',')
			bError = TRUE;
		else if (szCmd[4] == 0)
			bError = TRUE;
		else
		{
			ref = ((double) atoi(&szCmd[4]))/128;
			if (v == 'd')
			{
				TVSIs[num-1].Idq_ref[0] = ref;
				strcpy(szResponse, "OK");
			}
			else if (v == 'q')
			{
				TVSIs[num-1].Idq_ref[1] = ref;
				strcpy(szResponse, "OK");
			}
			else if (v == 'w')
			{
				TVSIs[num-1].OmegaDA_Cmd = ref;
				strcpy(szResponse, "OK");
			}
			else //not supported yet
				bError = TRUE;
		}

	}
	if (bError)
		strcpy(szResponse, "ERROR");

	return strlen(szResponse);
}

//update reference and measured currents
//return false if ABC values set, true if DQ values set
static u8 T_VSI_Input(void *arg, void *VSIContext)
{
	u32 t_index = (u32) arg;
	u32 adc_num[3];
	struct VSI_context *cnt = VSIContext;
	//double meas_abc[3], meas_dq0[3];

	//MEASURED CURRENTS
	if (t_index == 0)
	{
		adc_num[0] = T0_U_ADC; adc_num[1] = T0_V_ADC;
		adc_num[2] = 0; //use a+b+c =0
	}
	else
	{
		; //adc_num[0] = TOP_US_ADC; adc_num[1] = TOP_VS_ADC; adc_num[2] = TOP_WS_ADC; //should not get here!
	}

	cnt->Iabc[0] = 0.6*ReadADC(adc_num[0]);
	cnt->Iabc[1] = 0.6*ReadADC(adc_num[1]);
	cnt->Iabc[2] = -cnt->Iabc[0] -cnt->Iabc[1]; //ReadADC(adc_num[2]);
#if defined (USE_VSI_CC) && !defined(VSI_ISOLATED)
	cnt->Iabc[2] = cnt->Iabc[2] - (PosControllers.VSItop->Iabc[0] + PosControllers.VSItop->Iabc[1] + PosControllers.VSItop->Iabc[2]);
#endif

	cnt->theta_da = GetTheta_DA(cnt->theta_da, TVSIs[t_index].OmegaDA_Current, TVSIs[t_index].InputAtTickCount);
	DQ0_Transform(cnt->theta_da, cnt->Iabc, cnt->Idq0);

	//compensation terms:
	cnt->dqComp[0] = 0; //-TVSIs[t_index].OmegaDA_Current*Lq_div_Vbus*cnt->Idq0[1];
	cnt->dqComp[1] = 0; //TVSIs[t_index].OmegaDA_Current*(Ld_div_Vbus*cnt->Idq0[0] + sqrt6_Lfg_div_Vbus*IField);

#if defined (TVSI_STATIONARY_FRAME)
	//add the stationary frame terms as additional compensation
	{
		//cnt->dqComp[0] += TVSIs[t_index].VSI_stationary->d.output*cos(cnt->theta_da) + TVSIs[t_index].VSI_stationary->q.output*sin(cnt->theta_da);
		//cnt->dqComp[1] += TVSIs[t_index].VSI_stationary->q.output*cos(cnt->theta_da) - TVSIs[t_index].VSI_stationary->d.output*sin(cnt->theta_da);
		double new_dq0[3], old_dq0[3], test_uvw[3], alt_dq0[3];
		old_dq0[0] = TVSIs[t_index].VSI_stationary->d.output;
		old_dq0[1] = TVSIs[t_index].VSI_stationary->q.output;
		old_dq0[2] = 0;
		ChangeDQ_ReferenceFrame(new_dq0, cnt->theta_da, old_dq0, 0.0);
		cnt->dqComp[0] = cnt->dqComp[0] + new_dq0[0];
		cnt->dqComp[1] = cnt->dqComp[1] + new_dq0[1];

		//debugging
		//InverseDQ0_Transform(0.0, test_uvw, old_dq0);
		//DQ0_Transform(cnt->theta_da, test_uvw, alt_dq0);

		//logging
		/*x_out_ref = (s32)(128.0*old_dq0[0]);
		y_out_ref = (s32)(128.0*old_dq0[1]);
		dxy_out_ref = (s32)(128.0*(alt_dq0[0] - new_dq0[0])); //(s32)(128.0*new_dq0[0]);
		qxy_out_ref = (s32)(128.0*(alt_dq0[1] - new_dq0[1])); //(s32)(128.0*new_dq0[1]);*/

	}
#endif

	//acceleration
	if (TVSIs[t_index].OmegaDA_Cmd != TVSIs[t_index].OmegaDA_Current)
	{
		if (TVSIs[t_index].OmegaDA_Cmd > TVSIs[t_index].OmegaDA_Current)
		{
			TVSIs[t_index].OmegaDA_Current += OMEGA_DA_RAMP_RATE*TimeElapsed10us(TVSIs[t_index].InputAtTickCount);
			if (TVSIs[t_index].OmegaDA_Current > TVSIs[t_index].OmegaDA_Cmd)
				TVSIs[t_index].OmegaDA_Current = TVSIs[t_index].OmegaDA_Cmd;
		}
		else if (TVSIs[t_index].OmegaDA_Cmd < TVSIs[t_index].OmegaDA_Current)
		{
			TVSIs[t_index].OmegaDA_Current -= OMEGA_DA_RAMP_RATE*TimeElapsed10us(TVSIs[t_index].InputAtTickCount);
			if (TVSIs[t_index].OmegaDA_Current < TVSIs[t_index].OmegaDA_Cmd)
				TVSIs[t_index].OmegaDA_Current = TVSIs[t_index].OmegaDA_Cmd;
		}
	}

	//should limit the reference currents, but for now don't worry about it:
	cnt->d.reference = TVSIs[t_index].Idq_ref[0];
	cnt->q.reference = TVSIs[t_index].Idq_ref[1];

#if defined(USE_VSI_CC)
	//inform the position controller what we are up to
	{
		double new_dq0[3], old_dq0[3];
		old_dq0[0] = cnt->d.reference;
		old_dq0[1] = cnt->q.reference;
		old_dq0[2] = 0;
	//PosControllers.TorqueVSI_topIxy[0] = cnt->d.reference*cos(cnt->theta_da) - cnt->q.reference*sin(cnt->theta_da);
	//PosControllers.TorqueVSI_topIxy[1] = cnt->q.reference*cos(cnt->theta_da) + cnt->d.reference*sin(cnt->theta_da);
		ChangeDQ_ReferenceFrame(new_dq0, 0.0, old_dq0, cnt->theta_da);
		PosControllers.TorqueVSI_topIxy[0] = new_dq0[0];
		PosControllers.TorqueVSI_topIxy[1] = new_dq0[1];
	}
#endif

	//update the tick count:
	TVSIs[t_index].InputAtTickCount = MainTimerTimerTick;

	//logging
	if (t_index == 0)
		OmegaDA_Log = (s32)(128.0*TVSIs[t_index].OmegaDA_Current);
	return TRUE;
}

//write the duty ratio out to the legs
static void T_VSI_Output(void *arg, void *VSIContext)
{
	u32 tvsi_index = (u32) arg;
	u32 legs[3];
	u8 duty1, duty2, duty3;
	struct VSI_context *cnt = VSIContext;
	legs[0] = 0; legs[1] = 0; legs[2] = 0;

	if (tvsi_index == 0)
	{
		legs[0] = T0_U_LEG; legs[1] = T0_V_LEG; legs[2] = T0_W_LEG;
	}
	else
	{
		; //should not get here legs[0] = TOP_US_LEG; legs[1] = TOP_VS_LEG; legs[2] = TOP_WS_LEG;
	}


	//convert the duty ratios
	//dtest = 256*(d_com + m_abc[0]);
	duty1 = (unsigned char) 256.0*(cnt->dabc[0]);
	duty2 = (unsigned char) 256.0*(cnt->dabc[1]);
	duty3 = (unsigned char) 256.0*(cnt->dabc[2]);

	//write them out:
	WriteDutyRatio(legs[0], duty1);
	WriteDutyRatio(legs[1], duty2);
	WriteDutyRatio(legs[2], duty3);
}

#if defined(TVSI_STATIONARY_FRAME)
//update reference and measured currents
//return false if ABC values set, true if DQ values set
static u8 T_VSI_Input_Stationary(void *arg, void *VSIContext)
{
	u32 t_index = (u32) arg;
	struct VSI_context *cnt = VSIContext;
	double new_dq0[3], old_dq0[3];

	//use the measured VSI currents (transform them into the stationary reference frame)
	ChangeDQ_ReferenceFrame(TVSIs[t_index].VSI_stationary->Idq0, 0.0, TVSIs[t_index].VSI->Idq0, TVSIs[t_index].VSI->theta_da);

	//the reference currents also need to be brought over from the other frame
	old_dq0[0] = TVSIs[t_index].VSI->d.reference;
	old_dq0[1] = TVSIs[t_index].VSI->q.reference;
	old_dq0[2] = 0;
	ChangeDQ_ReferenceFrame(new_dq0, 0.0, old_dq0, TVSIs[t_index].VSI->theta_da);

	cnt->d.reference = new_dq0[0];
	cnt->q.reference = new_dq0[1];

	//logging:
	/*Itx_measured = (s32)(128.0*TVSIs[t_index].VSI_stationary->Idq0[0]);
	Ity_measured = (s32)(128.0*TVSIs[t_index].VSI_stationary->Idq0[1]);
	Itx_ref = (s32)(128.0*TVSIs[t_index].VSI_stationary->d.reference);
	Ity_ref = (s32)(128.0*TVSIs[t_index].VSI_stationary->q.reference);*/

	return TRUE;
}

//normally would write the duty ratio out to the legs, but don't do anything, let the other output function handle all of this.
static void T_VSI_Output_Stationary(void *arg, void *VSIContext)
{

}

#endif



<|start_filename|>sdk/bare/common/drv/sts_mux.h<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D

#ifndef STS_MUX_H
#define STS_MUX_H

#include <stdbool.h>
#include <stdint.h>

typedef enum {
    STS_MUX_UNUSED = 0,
    STS_MUX_DEVICE1 = 1, // DAC expansion board by default
    STS_MUX_DEVICE2 = 2,
    STS_MUX_DEVICE3 = 3,
    STS_MUX_DEVICE4 = 4,
    STS_MUX_DEVICE5 = 5,
    STS_MUX_DEVICE6 = 6,
    STS_MUX_DEVICE7 = 7,
    STS_MUX_DEVICE8 = 8
} sts_mux_device_t;

#define STS_MUX_OUTPUT true
#define STS_MUX_INPUT  false

typedef enum { STS_MUX_A, STS_MUX_B, STS_MUX_C, STS_MUX_D } sts_mux_line_t;

// These macros are used to map the connected devices in the FPGA IP to the desired port on the
// AMDC. All ports are re-mapped to the defined device in the init function. The
// STS_MUX_UNUSED enum maps the port to the default connections from the inverter IP

#define INVERTER_PORT1 STS_MUX_UNUSED
#define INVERTER_PORT2 STS_MUX_UNUSED
#define INVERTER_PORT3 STS_MUX_UNUSED
#define INVERTER_PORT4 STS_MUX_UNUSED
#define INVERTER_PORT5 STS_MUX_UNUSED
#define INVERTER_PORT6 STS_MUX_UNUSED
#define INVERTER_PORT7 STS_MUX_UNUSED
#define INVERTER_PORT8 STS_MUX_UNUSED

// These macros are used to configure the direction of the status lines in the FPGA IP

#define STS_MUX_A_DIR STS_MUX_OUTPUT
#define STS_MUX_B_DIR STS_MUX_INPUT
#define STS_MUX_C_DIR STS_MUX_INPUT
#define STS_MUX_D_DIR STS_MUX_INPUT

void sts_mux_init(void);

void sts_mux_set_line(uint8_t, sts_mux_device_t, sts_mux_line_t);

void sts_mux_set_device(uint8_t, sts_mux_device_t);

void sts_mux_set_output(sts_mux_line_t, bool);

#endif // STS_MUX_H

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/drv/watchdog.h<|end_filename|>
#ifndef WATCHDOG_H
#define WATCHDOG_H

void watchdog_init(void);
void watchdog_reset(void);

#endif // WATCHDOG_H


<|start_filename|>sdk/bare/common/drv/encoder.c<|end_filename|>
#include "drv/encoder.h"
#include "drv/hardware_targets.h"
#include "drv/io.h"
#include "sys/defines.h"
#include "sys/scheduler.h"
#include "usr/user_config.h"
#include "xil_io.h"
#include <math.h>
#include <stdio.h>

#define ENCODER_BASE_ADDR (0x43C10000)

void encoder_init(void)
{
    printf("ENC:\tInitializing...\n");
    encoder_set_pulses_per_rev_bits(ENCODER_PULSES_PER_REV_BITS);
}

void encoder_set_pulses_per_rev_bits(uint32_t bits)
{
    printf("ENC:\tSetting pulses per rev bits = %ld...\n", bits);

    Xil_Out32(ENCODER_BASE_ADDR + 2 * sizeof(uint32_t), bits);
}

void encoder_get_steps(int32_t *steps)
{
    *steps = Xil_In32(ENCODER_BASE_ADDR);
}

void encoder_get_position(uint32_t *position)
{
    *position = Xil_In32(ENCODER_BASE_ADDR + 1 * sizeof(uint32_t));
}

// ****************
// State Machine which finds z pulse
// ****************

typedef enum sm_states_e {
    WAIT_UNTIL_Z,
    REMOVE_TASK,
} sm_states_e;

typedef struct sm_ctx_t {
    sm_states_e state;
    double vPercent;
    double theta;
    double theta_delta;
    int counter;
    task_control_block_t tcb;
} sm_ctx_t;

#define SM_UPDATES_PER_SEC (10000)
#define SM_INTERVAL_USEC   (USEC_IN_SEC / SM_UPDATES_PER_SEC)

static void _find_z_callback(void *arg)
{
    sm_ctx_t *ctx = (sm_ctx_t *) arg;

    switch (ctx->state) {
    case WAIT_UNTIL_Z:
    {
        uint32_t pos;
        encoder_get_position(&pos);
        if (pos != -1) {
            ctx->state = REMOVE_TASK;
        }

        break;
    }

    case REMOVE_TASK:
    {
        scheduler_tcb_unregister(&ctx->tcb);

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
        io_led_color_t color;
        color.b = 0;
        io_led_set_c(0, 0, 1, &color);
#endif // USER_CONFIG_HARDWARE_TARGET
        break;
    }
    }
}

static sm_ctx_t ctx;

void encoder_find_z(void)
{
    // Initialize the state machine context
    ctx.state = WAIT_UNTIL_Z;

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
    io_led_color_t color;
    color.b = 255;
    io_led_set_c(0, 0, 1, &color);
#endif // USER_CONFIG_HARDWARE_TARGET

    // Initialize the state machine callback tcb
    scheduler_tcb_init(&ctx.tcb, _find_z_callback, &ctx, "find_z", SM_INTERVAL_USEC);
    scheduler_tcb_register(&ctx.tcb);
}


<|start_filename|>ip_repo/amdc_pwm_mux_1.0/drivers/amdc_pwm_mux_v1_0/src/amdc_pwm_mux.h<|end_filename|>

#ifndef AMDC_PWM_MUX_H
#define AMDC_PWM_MUX_H


/****************** Include Files ********************/
#include "xil_types.h"
#include "xstatus.h"

#define AMDC_PWM_MUX_S00_AXI_SLV_REG0_OFFSET 0
#define AMDC_PWM_MUX_S00_AXI_SLV_REG1_OFFSET 4
#define AMDC_PWM_MUX_S00_AXI_SLV_REG2_OFFSET 8
#define AMDC_PWM_MUX_S00_AXI_SLV_REG3_OFFSET 12
#define AMDC_PWM_MUX_S00_AXI_SLV_REG4_OFFSET 16
#define AMDC_PWM_MUX_S00_AXI_SLV_REG5_OFFSET 20
#define AMDC_PWM_MUX_S00_AXI_SLV_REG6_OFFSET 24
#define AMDC_PWM_MUX_S00_AXI_SLV_REG7_OFFSET 28
#define AMDC_PWM_MUX_S00_AXI_SLV_REG8_OFFSET 32
#define AMDC_PWM_MUX_S00_AXI_SLV_REG9_OFFSET 36
#define AMDC_PWM_MUX_S00_AXI_SLV_REG10_OFFSET 40
#define AMDC_PWM_MUX_S00_AXI_SLV_REG11_OFFSET 44
#define AMDC_PWM_MUX_S00_AXI_SLV_REG12_OFFSET 48
#define AMDC_PWM_MUX_S00_AXI_SLV_REG13_OFFSET 52
#define AMDC_PWM_MUX_S00_AXI_SLV_REG14_OFFSET 56
#define AMDC_PWM_MUX_S00_AXI_SLV_REG15_OFFSET 60
#define AMDC_PWM_MUX_S00_AXI_SLV_REG16_OFFSET 64
#define AMDC_PWM_MUX_S00_AXI_SLV_REG17_OFFSET 68
#define AMDC_PWM_MUX_S00_AXI_SLV_REG18_OFFSET 72
#define AMDC_PWM_MUX_S00_AXI_SLV_REG19_OFFSET 76
#define AMDC_PWM_MUX_S00_AXI_SLV_REG20_OFFSET 80
#define AMDC_PWM_MUX_S00_AXI_SLV_REG21_OFFSET 84
#define AMDC_PWM_MUX_S00_AXI_SLV_REG22_OFFSET 88
#define AMDC_PWM_MUX_S00_AXI_SLV_REG23_OFFSET 92
#define AMDC_PWM_MUX_S00_AXI_SLV_REG24_OFFSET 96
#define AMDC_PWM_MUX_S00_AXI_SLV_REG25_OFFSET 100
#define AMDC_PWM_MUX_S00_AXI_SLV_REG26_OFFSET 104
#define AMDC_PWM_MUX_S00_AXI_SLV_REG27_OFFSET 108
#define AMDC_PWM_MUX_S00_AXI_SLV_REG28_OFFSET 112
#define AMDC_PWM_MUX_S00_AXI_SLV_REG29_OFFSET 116
#define AMDC_PWM_MUX_S00_AXI_SLV_REG30_OFFSET 120
#define AMDC_PWM_MUX_S00_AXI_SLV_REG31_OFFSET 124
#define AMDC_PWM_MUX_S00_AXI_SLV_REG32_OFFSET 128
#define AMDC_PWM_MUX_S00_AXI_SLV_REG33_OFFSET 132
#define AMDC_PWM_MUX_S00_AXI_SLV_REG34_OFFSET 136
#define AMDC_PWM_MUX_S00_AXI_SLV_REG35_OFFSET 140
#define AMDC_PWM_MUX_S00_AXI_SLV_REG36_OFFSET 144
#define AMDC_PWM_MUX_S00_AXI_SLV_REG37_OFFSET 148
#define AMDC_PWM_MUX_S00_AXI_SLV_REG38_OFFSET 152
#define AMDC_PWM_MUX_S00_AXI_SLV_REG39_OFFSET 156
#define AMDC_PWM_MUX_S00_AXI_SLV_REG40_OFFSET 160
#define AMDC_PWM_MUX_S00_AXI_SLV_REG41_OFFSET 164
#define AMDC_PWM_MUX_S00_AXI_SLV_REG42_OFFSET 168
#define AMDC_PWM_MUX_S00_AXI_SLV_REG43_OFFSET 172
#define AMDC_PWM_MUX_S00_AXI_SLV_REG44_OFFSET 176
#define AMDC_PWM_MUX_S00_AXI_SLV_REG45_OFFSET 180
#define AMDC_PWM_MUX_S00_AXI_SLV_REG46_OFFSET 184
#define AMDC_PWM_MUX_S00_AXI_SLV_REG47_OFFSET 188
#define AMDC_PWM_MUX_S00_AXI_SLV_REG48_OFFSET 192
#define AMDC_PWM_MUX_S00_AXI_SLV_REG49_OFFSET 196


/**************************** Type Definitions *****************************/
/**
 *
 * Write a value to a AMDC_PWM_MUX register. A 32 bit write is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is written.
 *
 * @param   BaseAddress is the base address of the AMDC_PWM_MUXdevice.
 * @param   RegOffset is the register offset from the base to write to.
 * @param   Data is the data written to the register.
 *
 * @return  None.
 *
 * @note
 * C-style signature:
 * 	void AMDC_PWM_MUX_mWriteReg(u32 BaseAddress, unsigned RegOffset, u32 Data)
 *
 */
#define AMDC_PWM_MUX_mWriteReg(BaseAddress, RegOffset, Data) \
  	Xil_Out32((BaseAddress) + (RegOffset), (u32)(Data))

/**
 *
 * Read a value from a AMDC_PWM_MUX register. A 32 bit read is performed.
 * If the component is implemented in a smaller width, only the least
 * significant data is read from the register. The most significant data
 * will be read as 0.
 *
 * @param   BaseAddress is the base address of the AMDC_PWM_MUX device.
 * @param   RegOffset is the register offset from the base to write to.
 *
 * @return  Data is the data from the register.
 *
 * @note
 * C-style signature:
 * 	u32 AMDC_PWM_MUX_mReadReg(u32 BaseAddress, unsigned RegOffset)
 *
 */
#define AMDC_PWM_MUX_mReadReg(BaseAddress, RegOffset) \
    Xil_In32((BaseAddress) + (RegOffset))

/************************** Function Prototypes ****************************/
/**
 *
 * Run a self-test on the driver/device. Note this may be a destructive test if
 * resets of the device are performed.
 *
 * If the hardware system is not built correctly, this function may never
 * return to the caller.
 *
 * @param   baseaddr_p is the base address of the AMDC_PWM_MUX instance to be worked on.
 *
 * @return
 *
 *    - XST_SUCCESS   if all self-test code passed
 *    - XST_FAILURE   if any self-test code failed
 *
 * @note    Caching must be turned off for this function to work.
 * @note    Self test may fail if data memory and device are not on the same bus.
 *
 */
XStatus AMDC_PWM_MUX_Reg_SelfTest(void * baseaddr_p);

#endif // AMDC_PWM_MUX_H


<|start_filename|>sdk/basic/src/prog_timer.h<|end_filename|>

#ifndef __PROG_TIMER_H
#define __PROG_TIMER_H
#include "xscutimer.h"
#include "xscugic.h"

#define TIMER_DEVICE_ID		XPAR_XSCUTIMER_0_DEVICE_ID
#define TIMER_IRPT_INTR		XPAR_SCUTIMER_INTR
#define TIMER_INTERVAL		10 //value in us
#define TIMER_LOAD_VALUE	(TIMER_INTERVAL*666.66666667)/2 //0xFFFF // = timerInterval*ClockFreq/2


extern volatile u32 MainTimerTimerTick;
extern volatile char bTimer100usTick;
extern volatile  u32 Timing10usErrorCount;


extern int SetupTimer(XScuGic *IntcInstancePtr, XScuTimer * TimerInstancePtr,
		u16 TimerDeviceId, u16 TimerIntrId);
extern u32 TimeElapsed10us(u32 old);
extern void TimerDisableIntrSystem(XScuGic *IntcInstancePtr, u16 TimerIntrId);
#endif //__PROG_TIMER_H


<|start_filename|>sdk/bare/common/usr/demo/app_demo.c<|end_filename|>
#ifdef APP_DEMO

#include "usr/demo/app_demo.h"
#include "drv/pwm.h"
#include "usr/demo/cmd/cmd_cc.h"
#include "usr/demo/task_cc.h"

void app_demo_init(void)
{
    // Register "cc" command with system
    cmd_cc_register();

    // Set all PWM outputs to 50% duty ratio by default
    // TODO: we ought to open all switches, but FPGA does not support this yet...
    for (int i = 0; i < 24; i++) {
        pwm_set_duty(i, 0.5);
    }
}

#endif // APP_DEMO


<|start_filename|>sdk/basic/src/control.c<|end_filename|>
/*
 * control.c
 *
 *  Created on: Jun 24, 2014
 *      Author: sever212
 */

#include "project_include.h"

/*4294947296 for 10kHz*/
/*4294957296 for 20kHz*/
#define TIMER0_TICK_FREQ 20000
#define TIMER0_RESET_VAL ((0xFFFFFFFF-XPAR_CONTROL_TIMER_0_CLOCK_FREQ_HZ/TIMER0_TICK_FREQ) + 1) //4294957296 //32bit counter, 100MHz clock, time = (0x1_0000_0000-RESET_VALUE)/200e6 --> RESET_VALUE = 4294967296 - time*200e6
									//I don't know why the clock is 200MHz, maybe it is clocked from the AXI clock?

#define TIMER1_TICK_FREQ 1000
#define TIMER1_RESET_VAL ((0xFFFFFFFF-XPAR_CONTROL_TIMER_1_CLOCK_FREQ_HZ/TIMER1_TICK_FREQ) + 1) //4294957296 //32bit counter, 100MHz clock, time = (0x1_0000_0000-RESET_VALUE)/200e6 --> RESET_VALUE = 4294967296 - time*200e6
									//I don't know why the clock is 200MHz, maybe it is clocked from the AXI clock?


#define AXI_TIMER0_PRIORITY 0xA8		//0xF8 is lowest priority, 0 is highest: READ THIS: actually if the priority's upper nibble is F, it doesn't work at all!
#define AXI_TIMER1_PRIORITY 0xA9		//0xF8 is lowest priority, 0 is highest: READ THIS: actually if the priority's upper nibble is F, it doesn't work at all!


#define TMRCTR0_INTERRUPT_ID		61 //I don't know why, but it won't recognize these XPAR_FABRIC_AXI_TIMER_0_INTERRUPT_INTR
#define TMRCTR1_INTERRUPT_ID		62 //XPAR_FABRIC_AXI_TIMER_1_INTERRUPT_INTR

#define TMRCTR0_DEVICE_ID		XPAR_TMRCTR_0_DEVICE_ID
#define TMRCTR1_DEVICE_ID		XPAR_TMRCTR_1_DEVICE_ID

#define TIMER_CNTR_0	 0
#define TIMER_CNTR_1	 0

XTmrCtr TmrCtrInstance0;
XTmrCtr TmrCtrInstance1;
static void ControlTimer0_ISR(void *CallBackRef, u8 TmrCtrNumber);
static void ControlTimer1_ISR(void *CallBackRef, u8 TmrCtrNumber);
volatile u32 ControlTimer0_TickCount = 0;
volatile u32 ControlTimer1_TickCount = 0;

volatile u32 ControlTimer0ErrorCount = 0;
volatile u32 ControlTimer1ErrorCount = 0;

struct controller_linked_list_item *C0s = 0;		//list of the PI controllers for controller 0
//struct controller_linked_list_item *C0_PIDs = 0;		//list of the PI controllers for controller 0
struct controller_linked_list_item *C1s = 0;		//list of the PI controllers for controller 0
//struct controller_linked_list_item *C1_PIDs = 0;		//list of the PI controllers for controller 0

/************************TIMERS************************/
static int InitControlTimers(XScuGic* IntcInstancePtr)
{
	XTmrCtr *TmrCtrInstance0Ptr =  &TmrCtrInstance0;
	XTmrCtr *TmrCtrInstance1Ptr =  &TmrCtrInstance1;
	int Status;

	Status = Init_AXI_Timer(IntcInstancePtr,
			TmrCtrInstance0Ptr, TMRCTR0_DEVICE_ID, TMRCTR0_INTERRUPT_ID,
			TIMER_CNTR_0, ControlTimer0_ISR, AXI_TIMER0_PRIORITY, TIMER0_RESET_VAL);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*return Init_AXI_Timer(IntcInstancePtr,
			TmrCtrInstance1Ptr, TMRCTR1_DEVICE_ID, TMRCTR1_INTERRUPT_ID,
			TIMER_CNTR_1, ControlTimer1_ISR, AXI_TIMER1_PRIORITY, TIMER1_RESET_VAL);*/
	return XST_SUCCESS;

}

static void ServiceControllers(struct controller_linked_list_item *cnts)
{
	while(cnts)
	{
		struct controller_context *basic_cnt = cnts->context->control_io;
		if (++(basic_cnt->clockCount) >= basic_cnt->NumClocks)
		{
			basic_cnt->clockCount = 0;

			//input callback
			cnts->context->input_callback(cnts->context->arg_in, basic_cnt);

			//run the controller
			switch(basic_cnt->Type)
			{
				case C_PI:
					RunPI_Controller(cnts->context);
					break;
				case C_PID:
					RunPID_Controller (cnts->context);
				default:
					//UNSUPPORTED!
					break;
			}

			//output callback
			cnts->context->output_callback(cnts->context->arg_out, basic_cnt);
		}
		cnts = cnts->next;
		//basic_cnt = ((struct pi_controller *)cnts->controller)->controller;
	}
}

u32 starve = 0;
volatile unsigned char currentlyInISR = 0;
u32 LastMainTickCount = 0;
static void ControlTimer0_ISR(void *CallBackRef, u8 TmrCtrNumber) {
	XTmrCtr *InstancePtr = (XTmrCtr *)CallBackRef;
	u32 ControlStatusReg = XTmrCtr_ReadReg(InstancePtr->BaseAddress,
						TmrCtrNumber, XTC_TCSR_OFFSET);
	//clear the interrupt
	XTmrCtr_WriteReg(InstancePtr->BaseAddress,
							 TmrCtrNumber,
							 XTC_TCSR_OFFSET,
							 ControlStatusReg |
							 XTC_CSR_INT_OCCURED_MASK);
	if (currentlyInISR)
		ControlTimer0ErrorCount++;
	else
	{

		u32 startMainTicks = MainTimerTimerTick;
		if (TimeElapsed10us(LastMainTickCount) > 6)
			ControlTimer0ErrorCount++;

		LastMainTickCount = startMainTicks;
		//u32 i;
		ControlTimer0_TickCount++;

		currentlyInISR = 1;
		Xil_EnableNestedInterrupts();

		//do PIs:
		ServiceControllers(C0s);

		/*
		for (i=0; i<starve; i++)
			ServiceControllers(C0_PIs);
		starve++;
		//if (ControlTimer0_Count % 100000 == 0)*/
		/*if (ControlTimer0_TickCount == 200000 )
		{
			ControlTimer0_TickCount = 0;
			print("10 second tick - CISR0\r\n");
		}*/

		//if (XTmrCtr_IsExpired(InstancePtr, TmrCtrNumber))
		if (TimeElapsed10us(startMainTicks) > 5)
		{
			ControlTimer0ErrorCount++;
			starve = 0;
		}
		currentlyInISR = 0;
		Xil_DisableNestedInterrupts();
	}
}

static void ControlTimer1_ISR(void *CallBackRef, u8 TmrCtrNumber) {

	u32 startMainTicks = MainTimerTimerTick;
	ControlTimer1_TickCount++;
	//if (ControlTimer0_Count % 100000 == 0)
	/*if (ControlTimer1_TickCount == 10000 )
	{
		ControlTimer1_TickCount = 0;
		print("10 second tick - CISR1\r\n");
	}*/
	if (TimeElapsed10us(startMainTicks) >= 10)
			ControlTimer1ErrorCount++;
}
/*****************************************************/

static double SaturationBlock(double in, double sat_up, double sat_lower)
{
	if (in > sat_up)
		in = sat_up;
	else if (in < sat_lower)
		in = sat_lower;
	return in;
}

static double BasicIntegrator(double in, double last, double sat_up, double sat_lower)
{
	return SaturationBlock(in + last, sat_up, sat_lower);
}

int InitControllers(XScuGic* IntcInstancePtr)
{
	int Status = InitControlTimers(IntcInstancePtr);

	if (Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

	return XST_SUCCESS;
}

void RemoveLinkedListItem(struct controller_linked_list_item **list, struct controller_linked_list_item *item)
{
	if (*list)
	{
		struct controller_linked_list_item *head = *list;
		while ( (head != item) && (head->next) )
			head = head->next;

		//okay, we either found our item or it isnt here:
		if (head == item)
		{
			if (head->prev)
				head->prev->next = item->next;
			if (head->next)
				head->next->prev = item->prev;
			//update where the list points to
			if (head == *list)
				*list = head->next;
		}
		item->prev = 0;
		item->next = 0;
		item->bRegistered = FALSE;
	}
}

void AddLinkedListItem(struct controller_linked_list_item **list, struct controller_linked_list_item *item)
{
	if (*list)
	{
		struct controller_linked_list_item *head = *list;
		while (head->next)
			head = head->next;

		//okay, at the end
		head->next = item;
		item->prev = head;
		item->next = 0;
	}
	else
	{
		*list = item;
		(*list)->next = 0;
		(*list)->prev = 0;
	}
	item->bRegistered = TRUE;
}

int UnregisterController(int contNum, struct controller_linked_list_item *cnt)
{
	if (contNum == 0)
		RemoveLinkedListItem(&C0s, cnt);
	else if (contNum == 1)
		RemoveLinkedListItem(&C1s, cnt);
	else
		return XST_FAILURE;

	return XST_SUCCESS;
}

int RegisterController(int contNum, struct controller_linked_list_item *cnt)
{
	if (contNum == 0)
		AddLinkedListItem(&C0s, cnt);
	else if (contNum == 1)
		AddLinkedListItem(&C1s, cnt);
	else
		return XST_FAILURE;

	return XST_SUCCESS;
}

/*****************************
 * RunPI_Controller()
 * Do the computation for a pi controller
 *
 *
 *
 *pi: the pi controller context
 */
void RunPI_Controller (controller_holder* ctxt)
{
	struct pi_controller *pi = ctxt->controller_info;
	struct controller_context *cont_io = ctxt->control_io;
	double error = *cont_io->inputs;
	double p = pi->kp*error;
	//double kiTs = pi->ki* ((double)(cont_io->NumClocks*cont_io->ClockTime))*0.000001;
	double i_in = pi->kits*error;
	pi->lastInt = BasicIntegrator(i_in, pi->lastInt, pi->plusSat, pi->negSat);
	*(cont_io->outputs) = pi->lastInt + p; //don't saturate output, just prevent windup //SaturationBlock(pi->lastInt + p, pi->plusSat, pi->negSat);

	//check for NaN (conditions with NaN are always false)
	if (pi->lastInt != pi->lastInt)
		pi->lastInt = 0;
}

s32 PID_D, PID_I, PID_P, PID_LastD, PID_Err, PID_out;
/*****************************
 * RunPID_Controller()
 * Do the computation for our custom pid controller
 *
 *
 *
 *ctxt: the controller context
 */
void RunPID_Controller (controller_holder* ctxt)
{
	static int i = 0;
	struct pid_controller *pid = ctxt->controller_info;
	struct controller_context *cont_io = ctxt->control_io;
	double error = *cont_io->inputs;
	double p = pid->kp*error;
	double i_in = pid->kits*error;
	double d = pid->kdn*error + pid->kdn1*pid->lastErr + pid->kdm1*pid->lastD;
	pid->lastInt = BasicIntegrator(i_in, pid->lastInt, pid->plusSat, pid->negSat);
	pid->lastD = SaturationBlock(d, pid->plusSat, pid->negSat); //saturate to prevent windup
	pid->lastErr = error;

	*(cont_io->outputs) = pid->lastInt + p + d; //don't saturate output, just prevent windup //SaturationBlock(pid->lastInt + p + d, pid->plusSat, pid->negSat);
	//*(cont_io->outputs) = SaturationBlock(pid->lastInt + p + d, 10.0*pid->plusSat, 10.0*pid->negSat);

	//check for NaN (conditions with NaN are always false)
	if (pid->lastInt != pid->lastInt)
		pid->lastInt = 0;
	if (pid->lastD != pid->lastD)
		pid->lastD = 0;
	if (pid->lastErr != pid->lastErr)
		pid->lastErr = 0;

	//logging
	if (i == 0)
	{
		PID_D = (s32) (128*d);
		PID_I = (s32) (128*(pid->lastInt));
		PID_P = (s32) (128*p);
		PID_LastD = (s32) (128*(pid->lastD));
		PID_Err = (s32) (512*(error));
		PID_out = (s32) (128*(*(cont_io->outputs)));
	}
	i++;
	if (i == 4)
		i = 0;
}




<|start_filename|>sdk/bare/common/sys/cmd/cmd_hw.c<|end_filename|>
#include "sys/cmd/cmd_hw.h"
#include "drv/analog.h"
#include "drv/cpu_timer.h"
#include "drv/encoder.h"
#include "drv/fpga_timer.h"
#include "drv/gpio_mux.h"
#include "drv/hardware_targets.h"
#include "drv/ild1420.h"
#include "drv/pwm.h"
#include "drv/sts_mux.h"
#include "sys/commands.h"
#include "sys/debug.h"
#include "sys/defines.h"
#include "sys/util.h"
#include "usr/user_config.h"
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
#include "drv/led.h"
#endif // USER_CONFIG_HARDWARE_TARGET

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "pwm <on|off>", "Turn on/off PWM switching" },
    { "pwm sw <freq_switching> <deadtime_ns>", "Set the PWM switching characteristics" },
    { "pwm duty <pwm_idx> <percent>", "Set a duty ratio" },
    { "anlg read <chnl_idx>", "Read voltage on ADC channel" },
    { "ild read", "Read the latest packet from ILD1420 sensor" },
    { "enc steps", "Read encoder steps from power-up" },
    { "enc pos", "Read encoder position" },
    { "enc init", "Turn on blue LED until Z pulse found" },
    { "timer <fpga|cpu> now", "Read value from hardware timer" },

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    { "led set <led_idx> <r> <g> <b>", "Set LED color (color is 0..255)" },
    { "mux <gpio|sts> <port> <device>", "Map the device driver in the FPGA to the hardware port" },
#endif // USER_CONFIG_HARDWARE_TARGET
};

void cmd_hw_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "hw", "Hardware related commands", cmd_help, ARRAY_SIZE(cmd_help), cmd_hw);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

// Handles the 'hw' command and all sub-commands
int cmd_hw(int argc, char **argv)
{
    // Handle 'pwm' sub-command
    if (argc >= 2 && STREQ("pwm", argv[1])) {
        if (argc == 3 && STREQ("on", argv[2])) {
            if (pwm_enable() != SUCCESS) {
                return CMD_FAILURE;
            }

            return CMD_SUCCESS;
        }

        if (argc == 3 && STREQ("off", argv[2])) {
            if (pwm_disable() != SUCCESS) {
                return CMD_FAILURE;
            }

            return CMD_SUCCESS;
        }

        if (argc == 5 && STREQ("sw", argv[2])) {
            // Parse out switching freq arg
            double fsw = strtod(argv[3], NULL);
            if (fsw > 2e6 || fsw < 2e3) {
                return CMD_INVALID_ARGUMENTS;
            }

            // Parse out dead time arg
            int dt = atoi(argv[4]);
            if (dt > 5000 || dt < 25) {
                return CMD_INVALID_ARGUMENTS;
            }

            if (pwm_set_deadtime_ns(dt) != SUCCESS) {
                return CMD_FAILURE;
            }

            if (pwm_set_switching_freq(fsw) != SUCCESS) {
                return CMD_FAILURE;
            }

            return CMD_SUCCESS;
        }

        if (argc == 5 && STREQ("duty", argv[2])) {
            // Parse out switching pwm_idx arg
            int pwm_idx = atoi(argv[3]);

            if (!pwm_is_valid_channel(pwm_idx)) {
                return CMD_INVALID_ARGUMENTS;
            }

            // Parse out percent arg
            double percent = strtod(argv[4], NULL);
            if (percent > 1.0 || percent < 0.0) {
                return CMD_INVALID_ARGUMENTS;
            }

            pwm_set_duty(pwm_idx, percent);

            return CMD_SUCCESS;
        }
    }

    // Handle 'anlg' sub-command
    if (argc >= 2 && STREQ("anlg", argv[1])) {
        if (argc == 4 && STREQ("read", argv[2])) {
            // Parse out analog channel arg
            int anlg_idx = atoi(argv[3]);

            if (!analog_is_valid_channel(anlg_idx)) {
                return CMD_INVALID_ARGUMENTS;
            }

            float out_volts;
            analog_getf(anlg_idx, &out_volts);

            debug_printf("%fV\r\n", out_volts);

            return SUCCESS;
        }
    }

    // Handle 'ild' sub-command
    // hw ild read
    if (argc == 4 && strcmp("ild", argv[1]) == 0) {
        if (strcmp("read", argv[2]) == 0) {
            int sensor = atoi(argv[3]);

            if (sensor < 0 || sensor >= ILD1420_NUM_SENSORS)
                return CMD_INVALID_ARGUMENTS;

            ild1420_packet_t packet = ild1420_get_latest_packet(sensor);
            debug_printf("dist:  %x\r\n", packet.distance);
            debug_printf("err:   %X\r\n", packet.error);
            debug_printf("fresh: %X\r\n", packet.fresh);

            return CMD_SUCCESS;
        }
    }

    // Handle 'enc' sub-command
    if (argc >= 2 && STREQ("enc", argv[1])) {
        if (argc == 3 && STREQ("steps", argv[2])) {
            int32_t steps;
            encoder_get_steps(&steps);

            debug_printf("steps: %ld\r\n", steps);

            return CMD_SUCCESS;
        }

        if (argc == 3 && STREQ("pos", argv[2])) {
            uint32_t position;
            encoder_get_position(&position);

            debug_printf("pos: %ld\r\n", position);

            return CMD_SUCCESS;
        }

        if (argc == 3 && STREQ("init", argv[2])) {
            encoder_find_z();

            return CMD_SUCCESS;
        }
    }

    // Handle 'timer' sub-command
    if (argc >= 2 && STREQ("timer", argv[1])) {
        if (argc == 4 && STREQ("fpga", argv[2]) && STREQ("now", argv[3])) {
            uint32_t counts1 = fpga_timer_now();
            uint32_t counts2 = fpga_timer_now();

            debug_printf("counts1: %lu\r\n", counts1);
            debug_printf("counts2: %lu\r\n", counts2);
            debug_printf("time delta = %8.3f ns\r\n", 1e3 * fpga_timer_ticks_to_usec(counts2 - counts1));

            return CMD_SUCCESS;
        }

        if (argc == 4 && STREQ("cpu", argv[2]) && STREQ("now", argv[3])) {
            uint32_t counts1 = cpu_timer_now();
            uint32_t counts2 = cpu_timer_now();

            debug_printf("counts1: %lu\r\n", counts1);
            debug_printf("counts2: %lu\r\n", counts2);
            debug_printf("time delta = %8.3f ns\r\n", 1e3 * cpu_timer_ticks_to_usec(counts2 - counts1));

            return CMD_SUCCESS;
        }
    }

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Handle 'led' sub-command
    // hw led set <led_idx> <r> <g> <b>
    if (argc >= 2 && STREQ("led", argv[1])) {
        if (argc == 7 && STREQ("set", argv[2])) {
            int led_idx = atoi(argv[3]);
            if (led_idx < 0 || led_idx >= NUM_LEDS)
                return CMD_INVALID_ARGUMENTS;

            int r = atoi(argv[4]);
            int g = atoi(argv[5]);
            int b = atoi(argv[6]);

            if (r < 0 || r > 255)
                return CMD_INVALID_ARGUMENTS;
            if (g < 0 || g > 255)
                return CMD_INVALID_ARGUMENTS;
            if (b < 0 || b > 255)
                return CMD_INVALID_ARGUMENTS;

            led_set_color_bytes(led_idx, r, g, b);

            return CMD_SUCCESS;
        }
    }

    // Handle 'mux' sub-command
    // mux gpio <port#> <device#>
    if (argc >= 2 && STREQ("mux", argv[1])) {
        if (argc == 5 && STREQ("gpio", argv[2])) {
            int gpio_port = atoi(argv[3]);
            int device = atoi(argv[4]);

            if (gpio_port < 1 || gpio_port > 2)
                return CMD_INVALID_ARGUMENTS;
            if (device < 0 || device > 4)
                return CMD_INVALID_ARGUMENTS;

            gpio_mux_set_device(gpio_port - 1, device);

            return CMD_SUCCESS;
        }

        if (argc == 5 && STREQ("sts", argv[2])) {
            int sts_port = atoi(argv[3]);
            int device = atoi(argv[4]);

            if (sts_port < 1 || sts_port > 8)
                return CMD_INVALID_ARGUMENTS;
            if (device < 0 || device > 8)
                return CMD_INVALID_ARGUMENTS;

            sts_mux_set_device(sts_port - 1, device);

            return CMD_SUCCESS;
        }
    }
#endif // USER_CONFIG_HARDWARE_TARGET

    return CMD_INVALID_ARGUMENTS;
}


<|start_filename|>sdk/bare/user/usr/beta_labs/mcff.h<|end_filename|>
#ifndef MCFF_H
#define MCFF_H

void mcff_update(double omega_m_star, double omega_dot_m_star);

double mcff_get_Te_cff_total(void);
double mcff_get_Te_cff_bp(void);
double mcff_get_Te_cff_jp(void);

#endif // MCFF_H


<|start_filename|>sdk/bare/common/drv/watchdog.c<|end_filename|>
#include "drv/watchdog.h"
#include "xparameters.h"
#include "xscuwdt.h"

// Private watchdog defines
#define WDT_DEVICE_ID  (XPAR_SCUWDT_0_DEVICE_ID)
#define INTC_DEVICE_ID (XPAR_SCUGIC_SINGLE_DEVICE_ID)
#define WDT_IRPT_INTR  (XPAR_SCUWDT_INTR)
#define WDT_LOAD_VALUE (0xFFFFFFFF) // seems to be about 10 seconds

XScuWdt WdtInstance;

void watchdog_init(void)
{
    int Status;
    XScuWdt_Config *ConfigPtr;

    // Initialize the SCU Private Wdt driver so that it is ready to use.
    ConfigPtr = XScuWdt_LookupConfig(WDT_DEVICE_ID);
    Status = XScuWdt_CfgInitialize(&WdtInstance, ConfigPtr, ConfigPtr->BaseAddr);
    if (Status != XST_SUCCESS) {
        // Hang if error...
        while (1) {
        }
    }

    // Put the watchdog timer in watchdog mode.
    XScuWdt_SetWdMode(&WdtInstance);

    // Load the watchdog counter register.
    XScuWdt_LoadWdt(&WdtInstance, WDT_LOAD_VALUE);

    // Start the ScuWdt device.
    XScuWdt_Start(&WdtInstance);
}

void watchdog_reset(void)
{
    XScuWdt_RestartWdt(&WdtInstance);
}


<|start_filename|>sdk/bare/common/drv/gpio.c<|end_filename|>
#include "drv/hardware_targets.h"
#include "usr/user_config.h"

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C

#include "drv/gpio.h"
#include "xgpiops.h"
#include <stdio.h>

#define GPIO_DIR_1_MIO_PIN (40)
#define GPIO_DIR_2_MIO_PIN (49)
#define GPIO_DIR_3_MIO_PIN (46)
#define GPIO_DIR_4_MIO_PIN (48)

static XGpioPs Gpio;

void gpio_init(void)
{
    printf("GPIO:\tInitializing...\n");

    int Status;
    XGpioPs_Config *GPIOConfigPtr;

    // GPIO Initialization
    GPIOConfigPtr = XGpioPs_LookupConfig(XPAR_PS7_GPIO_0_DEVICE_ID);
    Status = XGpioPs_CfgInitialize(&Gpio, GPIOConfigPtr, GPIOConfigPtr->BaseAddr);
    if (Status != XST_SUCCESS) {
        // Just hang here if error...
        while (1) {
        }
    }

    // Set GPIO dir pins as outputs
    XGpioPs_SetDirectionPin(&Gpio, GPIO_DIR_1_MIO_PIN, 1);
    XGpioPs_SetDirectionPin(&Gpio, GPIO_DIR_2_MIO_PIN, 1);
    XGpioPs_SetDirectionPin(&Gpio, GPIO_DIR_3_MIO_PIN, 1);
    XGpioPs_SetDirectionPin(&Gpio, GPIO_DIR_4_MIO_PIN, 1);

    // Set LED output enabled
    XGpioPs_SetOutputEnablePin(&Gpio, GPIO_DIR_1_MIO_PIN, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, GPIO_DIR_2_MIO_PIN, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, GPIO_DIR_3_MIO_PIN, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, GPIO_DIR_4_MIO_PIN, 1);

    // Start with GPIO dir set as outputs
    gpio_set_dir(1, 1);
    gpio_set_dir(2, 1);
    gpio_set_dir(3, 1);
    gpio_set_dir(4, 1);
}

void gpio_pin_read(uint8_t port, uint8_t pin, uint8_t *value)
{
}

void gpio_pin_write(uint8_t port, uint8_t pin, uint8_t value)
{
}

void gpio_port_read(uint8_t port, uint16_t *value)
{
}

void gpio_port_write(uint8_t port, uint16_t value)
{
}

void gpio_set_dir(uint8_t group, uint8_t dir)
{
    int pin;
    if (group == 1) {
        pin = GPIO_DIR_1_MIO_PIN;
    } else if (group == 2) {
        pin = GPIO_DIR_2_MIO_PIN;
    } else if (group == 3) {
        pin = GPIO_DIR_3_MIO_PIN;
    } else if (group == 4) {
        pin = GPIO_DIR_4_MIO_PIN;
    } else {
        // ERROR
    }

    XGpioPs_WritePin(&Gpio, pin, dir);
}

#endif // USER_CONFIG_HARDWARE_TARGET


<|start_filename|>sdk/bare/common/sys/commands.h<|end_filename|>
#ifndef COMMANDS_H
#define COMMANDS_H

#include "sys/scheduler.h"

#define COMMANDS_UPDATES_PER_SEC (10000)
#define COMMANDS_INTERVAL_USEC   (USEC_IN_SEC / COMMANDS_UPDATES_PER_SEC)

// Supported command handler return codes
#define CMD_SUCCESS           (0)
#define CMD_FAILURE           (1)
#define CMD_SUCCESS_QUIET     (2)
#define CMD_INVALID_ARGUMENTS (3)
#define CMD_INPUT_TOO_LONG    (4)
#define CMD_UNKNOWN_CMD       (5)

// Forward declarations
typedef struct command_entry_t command_entry_t;
typedef struct command_help_t command_help_t;

typedef struct command_entry_t {
    const char *cmd;
    const char *desc;
    command_help_t *help;
    int num_help_cmds;
    int (*cmd_function)(int, char **);

    // Pointer to next cmd; set this to NULL in user code.
    // When cmd is registered, this will form a linked list.
    command_entry_t *next;
} command_entry_t;

typedef struct command_help_t {
    const char *subcmd;
    const char *desc;
} command_help_t;

void commands_init(void);
void commands_callback_parse(void *arg);
void commands_callback_exec(void *arg);

void commands_cmd_init(command_entry_t *cmd_entry,
                       const char *cmd,
                       const char *desc,
                       command_help_t *help,
                       int num_help_cmds,
                       int (*cmd_function)(int, char **));
void commands_cmd_register(command_entry_t *cmd_entry);

void commands_start_msg(void);
void commands_display_help(void);

#endif // COMMANDS_H


<|start_filename|>sdk/bare/common/sys/cmd/cmd_inj.c<|end_filename|>
#include "usr/user_config.h"

#if USER_CONFIG_ENABLE_INJECTION == 1

#include "cmd_inj.h"
#include "sys/commands.h"
#include "sys/defines.h"
#include "sys/injection.h"
#include "sys/util.h"
#include <stdlib.h>
#include <string.h>

static command_entry_t cmd_entry;

static command_help_t cmd_help[] = {
    { "clear", "Clear all injections" },
    { "list", "List all available injection points" },
    { "const <name> <set|add|sub> <value>", "Inject a constant" },
    { "noise <name> <set|add|sub> <gain> <offset>", "Inject noise" },
    { "chirp <name> <set|add|sub> <gain> <freqMin> <freqMax> <period>", "Inject chirp" },
    { "triangle <name> <set|add|sub> <valueMin> <valueMax> <period>", "Inject triangle" },
    { "square <name> <set|add|sub> <valueMin> <valueMax> <period>", "Inject square" },
};

void cmd_inj_register(void)
{
    // Populate the command entry block
    commands_cmd_init(&cmd_entry, "inj", "Inject signals into system", cmd_help, ARRAY_SIZE(cmd_help), cmd_inj);

    // Register the command
    commands_cmd_register(&cmd_entry);
}

static int _parse_op(char *op_str, inj_op_e *inj_op)
{
    // Default to error state:
    // -1 : error
    //  0 : no error
    int error = -1;

    if (STREQ("set", op_str)) {
        *inj_op = SET;
        error = 0;
    } else if (STREQ("add", op_str)) {
        *inj_op = ADD;
        error = 0;
    } else if (STREQ("sub", op_str)) {
        *inj_op = SUB;
        error = 0;
    }

    return error;
}

int cmd_inj(int argc, char **argv)
{
    // Handle 'inj clear' command
    if (argc == 2 && STREQ("clear", argv[1])) {
        injection_clear();
        return CMD_SUCCESS;
    }

    // Handle 'inj list' command
    if (argc == 2 && STREQ("list", argv[1])) {
        injection_list();
        return CMD_SUCCESS;
    }

    // Handle 'inj const ...' command
    if (argc == 5 && STREQ("const", argv[1])) {
        // Parse out name and convert to injection context
        inj_ctx_t *ctx = injection_find_ctx_by_name(argv[2]);
        if (ctx == NULL) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse out operation
        inj_op_e op;
        if (_parse_op(argv[3], &op) != 0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out value argument
        double value = strtod(argv[4], NULL);

        injection_const(ctx, op, value);

        return CMD_SUCCESS;
    }

    // Handle 'inj noise ...' command
    if (argc == 6 && STREQ("noise", argv[1])) {
        // Parse out name and convert to injection context
        inj_ctx_t *ctx = injection_find_ctx_by_name(argv[2]);
        if (ctx == NULL) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse out operation
        inj_op_e op;
        if (_parse_op(argv[3], &op) != 0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out gain argument
        double gain = strtod(argv[4], NULL);

        // Pull out offset argument
        double offset = strtod(argv[5], NULL);

        injection_noise(ctx, op, gain, offset);

        return CMD_SUCCESS;
    }

    // Handle 'inj chirp ...' command
    if (argc == 8 && STREQ("chirp", argv[1])) {
        // Parse out name and convert to injection context
        inj_ctx_t *ctx = injection_find_ctx_by_name(argv[2]);
        if (ctx == NULL) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse out operation
        inj_op_e op;
        if (_parse_op(argv[3], &op) != 0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out gain argument
        // and saturate to 0+
        double gain = strtod(argv[4], NULL);
        if (gain < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out freqMin argument
        // and saturate to 0+
        double freqMin = strtod(argv[5], NULL);
        if (freqMin < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out freqMax argument
        // and saturate to 0+
        double freqMax = strtod(argv[6], NULL);
        if (freqMax < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out period argument
        double period = strtod(argv[7], NULL);
        if (period < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        injection_chirp(ctx, op, gain, freqMin, freqMax, period);

        return CMD_SUCCESS;
    }

    // Handle 'inj triangle ...' command
    if (argc == 7 && STREQ("triangle", argv[1])) {
        // Parse out name and convert to injection context
        inj_ctx_t *ctx = injection_find_ctx_by_name(argv[2]);
        if (ctx == NULL) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse out operation
        inj_op_e op;
        if (_parse_op(argv[3], &op) != 0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out valueMin argument
        double valueMin = strtod(argv[4], NULL);

        // Pull out valueMax argument
        double valueMax = strtod(argv[5], NULL);

        // Pull out period argument
        double period = strtod(argv[6], NULL);
        if (period < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        injection_triangle(ctx, op, valueMin, valueMax, period);

        return CMD_SUCCESS;
    }

    // Handle 'inj square ...' command
    if (argc == 7 && STREQ("square", argv[1])) {
        // Parse out name and convert to injection context
        inj_ctx_t *ctx = injection_find_ctx_by_name(argv[2]);
        if (ctx == NULL) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Parse out operation
        inj_op_e op;
        if (_parse_op(argv[3], &op) != 0) {
            return CMD_INVALID_ARGUMENTS;
        }

        // Pull out valueMin argument
        double valueMin = strtod(argv[4], NULL);

        // Pull out valueMax argument
        double valueMax = strtod(argv[5], NULL);

        // Pull out period argument
        double period = strtod(argv[6], NULL);
        if (period < 0.0) {
            return CMD_INVALID_ARGUMENTS;
        }

        injection_square(ctx, op, valueMin, valueMax, period);

        return CMD_SUCCESS;
    }

    return CMD_INVALID_ARGUMENTS;
}

#endif // USER_CONFIG_ENABLE_INJECTION


<|start_filename|>sdk/basic/src/ControlApp.h<|end_filename|>
/*
 * ControlApp.h
 *
 *  Created on: Jun 24, 2014
 *      Author: sever212
 */

#ifndef CONTROLAPP_H_
#define CONTROLAPP_H_

#define NUM_PI_CONTROLLERS 12

extern int ControlStartAppCommand(const char *szCmd, char *szResponse, void *CommDevice);
extern int ControlAppCommand(const char * szCmd, char *szResponse, void *CommDevice);
extern int InitControlApp();

#endif /* CONTROLAPP_H_ */


<|start_filename|>sdk/bare/user/usr/blink/task_blink.c<|end_filename|>
#ifdef APP_BLINK

#include "usr/blink/task_blink.h"
#include "drv/hardware_targets.h"
#include "sys/scheduler.h"
#include "usr/user_config.h"
#include <math.h>
#include <stdint.h>

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
#include "drv/led.h"
#endif // USER_CONFIG_HARDWARE_TARGET

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
#include "drv/io.h"
#endif // USER_CONFIG_HARDWARE_TARGET

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
// Hold LED state (0: off, 1: red, 2: green, 3: blue)
static uint8_t led_state = 0;
#endif // USER_CONFIG_HARDWARE_TARGET

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
// Hold LED animation state
static uint8_t led_pos = 0;
static uint8_t led_color_idx = 0;
#define NUM_LED_COLORS (7)
static led_color_t led_colors[NUM_LED_COLORS] = {
    LED_COLOR_RED,     //
    LED_COLOR_GREEN,   //
    LED_COLOR_BLUE,    //
    LED_COLOR_YELLOW,  //
    LED_COLOR_CYAN,    //
    LED_COLOR_MAGENTA, //
    LED_COLOR_WHITE,   //
};
#endif // USER_CONFIG_HARDWARE_TARGET

// Scheduler TCB which holds task "context"
static task_control_block_t tcb;

int task_blink_init(void)
{
    if (scheduler_tcb_is_registered(&tcb)) {
        return FAILURE;
    }

    // Fill TCB with parameters
    scheduler_tcb_init(&tcb, task_blink_callback, NULL, "blink", TASK_BLINK_INTERVAL_USEC);

    // Register task with scheduler
    return scheduler_tcb_register(&tcb);
}

int task_blink_deinit(void)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    // Turn off all LEDs
    for (uint8_t i = 0; i < NUM_LEDS; i++) {
        led_set_color(i, LED_COLOR_BLACK);
    }

    // Reset state
    led_pos = 0;
    led_color_idx = 0;
#endif // USER_CONFIG_HARDWARE_TARGET

    // Unregister task with scheduler
    return scheduler_tcb_unregister(&tcb);
}

void task_blink_callback(void *arg)
{
#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_C
    // Set LED output via I/O driver
    io_led_color_t color = { 0 };
    color.r = led_state == 1 ? 1 : 0;
    color.g = led_state == 2 ? 1 : 0;
    color.b = led_state == 3 ? 1 : 0;
    io_led_set(&color);

    // Update LED state for next time task is called
    if (++led_state >= 4) {
        led_state = 0;
    }
#endif // USER_CONFIG_HARDWARE_TARGET

#if USER_CONFIG_HARDWARE_TARGET == AMDC_REV_D
    for (uint8_t i = 0; i < NUM_LEDS; i++) {
        led_set_color(i, led_pos == i ? led_colors[led_color_idx] : LED_COLOR_BLACK);
    }

    if (++led_pos == NUM_LEDS) {
        led_pos = 0;

        if (++led_color_idx >= NUM_LED_COLORS) {
            led_color_idx = 0;
        }
    }
#endif // USER_CONFIG_HARDWARE_TARGET
}

void task_blink_stats_print(void)
{
    task_stats_print(&tcb.stats);
}

void task_blink_stats_reset(void)
{
    task_stats_reset(&tcb.stats);
}

#endif // APP_BLINK
