scpi.cpp

Go to the documentation of this file.

/* This file has been prepared for Doxygen automatic documentation generation.*/
 
#include "scpi.h"
#include "config.h"
#include "scpi_config.h"
#include "scpi_helper.h"
 
// Forward declarations
static void ScpiCoreIdnQ(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ScpiSystemErrorCountQ(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ScpiSystemErrorNextQ(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void GetMotorEnable(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ConfigureMotorEnable(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void GetConfigureMotorDutyCycleSource(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ConfigureMotorDutyCycle(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ConfigureMotorFrequency(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void GetConfigureMotorFrequency(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ConfigureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void GetConfigureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorSpeed(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorCurrentVBus(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorCurrentPhaseU(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorCurrentPhaseV(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorCurrentPhaseW(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureMotorVoltage(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void MeasureGateDutyCycle(SCPI_C commands, SCPI_P parameters, Stream &interface);
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_OPEN_LOOP)
static void ConfigureMotorDutyCycleSource(SCPI_C commands, SCPI_P parameters, Stream &interface);
#elif (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
static void ConfigureMotorSpeedSource(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void GetMotorSpeedSource(SCPI_C commands, SCPI_P parameters, Stream &interface);
static void ConfigureMotorSpeed(SCPI_C commands, SCPI_P parameters, Stream &interface);
#endif
 
// Instantiate the SCPI Parser
SCPI_Parser scpiParser;

void ScpiInit(void)
{
    /* IEEE Mandated Commands (SCPI std V1999.0 4.1.1) */
    // "*CLS" and "*RST" are not supported
    scpiParser.RegisterCommand(F("*IDN?"), &ScpiCoreIdnQ);
 
    /* Required SCPI commands (SCPI std V1999.0 4.2.1) */
    // "SYSTem:VERSion?" and "SYSTem:ERRor:NEXT?" are not supported
    scpiParser.SetCommandTreeBase(F("SYSTem"));
    scpiParser.RegisterCommand(F(":ERRor?"), &ScpiSystemErrorNextQ);
    scpiParser.RegisterCommand(F(":ERRor:COUNt?"), &ScpiSystemErrorCountQ);
 
    /* Motor Configuration Commands */
    scpiParser.SetCommandTreeBase(F("CONFigure"));
    scpiParser.RegisterCommand(F(":ENABle"), &ConfigureMotorEnable);
    scpiParser.RegisterCommand(F(":ENABle?"), &GetMotorEnable);
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_OPEN_LOOP)
    scpiParser.RegisterCommand(F(":DUTYcycle:SOURce"), &ConfigureMotorDutyCycleSource);
    scpiParser.RegisterCommand(F(":DUTYcycle:SOURce?"), &GetConfigureMotorDutyCycleSource);
    scpiParser.RegisterCommand(F(":DUTYcycle"), &ConfigureMotorDutyCycle);
#else
    scpiParser.RegisterCommand(F(":SPEED:SOURce"), &ConfigureMotorSpeedSource);
    scpiParser.RegisterCommand(F(":SPEED:SOURce?"), &GetMotorSpeedSource);
    scpiParser.RegisterCommand(F(":SPEED"), &ConfigureMotorSpeed);
#endif
    scpiParser.RegisterCommand(F(":FREQuency"), &ConfigureMotorFrequency);
    scpiParser.RegisterCommand(F(":FREQuency?"), &GetConfigureMotorFrequency);
    // scpiParser.RegisterCommand(F("DEADtime"), &ConfigureMotorDeadTime);
    // scpiParser.RegisterCommand(F("DEADtime?"), &GetConfigureMotorDeadTime);
    scpiParser.RegisterCommand(F(":DIREction"), &ConfigureMotorDirection);
    scpiParser.RegisterCommand(F(":DIREction?"), &GetConfigureMotorDirection);
 
    /* Motor Measurement Commands */
    scpiParser.SetCommandTreeBase(F("MEASure"));
    scpiParser.RegisterCommand(F(":SPEEd?"), &MeasureMotorSpeed);
    scpiParser.RegisterCommand(F(":CURRent:IBUS?"), &MeasureMotorCurrentVBus);
    scpiParser.RegisterCommand(F(":CURRent:IPHU?"), &MeasureMotorCurrentPhaseU);
    scpiParser.RegisterCommand(F(":CURRent:IPHV?"), &MeasureMotorCurrentPhaseV);
    scpiParser.RegisterCommand(F(":CURRent:IPHW?"), &MeasureMotorCurrentPhaseW);
    scpiParser.RegisterCommand(F(":VOLTage?"), &MeasureMotorVoltage);
    scpiParser.RegisterCommand(F(":DIREction?"), &MeasureMotorDirection);
    scpiParser.RegisterCommand(F(":DUTYcycle?"), &MeasureGateDutyCycle);
}

void ScpiInput(Stream &interface)
{
    scpiParser.ProcessInput(interface, "\n");
}

static void ScpiCoreIdnQ(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.print(F(SCPI_IDN1));
    interface.print(F(","));
    interface.print(F(SCPI_IDN2));
    interface.print(F(","));
    if (SCPI_IDN3 != NULL && strlen(SCPI_IDN3) > 0)
    {
        interface.print(F(SCPI_IDN3));
        interface.print(F(","));
    }
    interface.println(F(SCPI_IDN4));
}

static void ScpiSystemErrorCountQ(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(scpiParser.last_error == ErrorCode::NoError ? 0 : 1);
}

static void ScpiSystemErrorNextQ(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    switch (scpiParser.last_error)
    {
    case ErrorCode::BufferOverflow:
        interface.println(F("Buffer overflow error"));
        break;
    case ErrorCode::Timeout:
        interface.println(F("Communication timeout error"));
        break;
    case ErrorCode::UnknownCommand:
        interface.println(F("Unknown command received"));
        break;
    case ErrorCode::NoError:
        interface.println(F("No Error"));
        break;
    case ErrorCode::MissingOrInvalidParameter:
        interface.println(F("Missing or invalid parameter"));
        break;
    default:
        interface.println(F("Unknown error"));
    }
    scpiParser.last_error = ErrorCode::NoError;
}

static void GetMotorEnable(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(motorFlags.enable);
}

static void ConfigureMotorEnable(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    bool param;
    if (!ScpiParamBool(parameters, param))
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
    if (param)
    {
        // Set the enable pin
        PORTD |= (1 << ENABLE_PIN);
    }
    else
    {
        // Clear the enable pin
        PORTD &= ~(1 << ENABLE_PIN);
    }
 
    scpiParser.last_error = ErrorCode::NoError;
}

#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_OPEN_LOOP)
static void ConfigureMotorDutyCycleSource(SCPI_C commands, SCPI_P parameters, Stream &interface)
#elif (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
static void ConfigureMotorSpeedSource(SCPI_C commands, SCPI_P parameters, Stream &interface)
#endif
{
    uint8_t param;
 
    // read first parameter if present
    if (!ScpiParamChoice(parameters, inputSources, INPUT_SOURCE_OPTIONS, param))
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
 
    if (param == SPEED_INPUT_SOURCE_LOCAL)
    {
        motorConfigs.speedInputSource = SPEED_INPUT_SOURCE_LOCAL;
        return;
    }
    else
    {
        motorConfigs.speedInputSource = SPEED_INPUT_SOURCE_REMOTE;
        speedInput = 0;
        return;
    }
    scpiParser.last_error = ErrorCode::NoError;
}
 
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_OPEN_LOOP)
static void GetConfigureMotorDutyCycleSource(SCPI_C commands, SCPI_P parameters, Stream &interface)
#elif (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
static void GetMotorSpeedSource(SCPI_C commands, SCPI_P parameters, Stream &interface)
#endif
{
    String name;
    ScpiChoiceToName(inputSources, INPUT_SOURCE_OPTIONS, motorConfigs.speedInputSource, name);
    interface.println(name);
}
 
#if (SPEED_CONTROL_METHOD == SPEED_CONTROL_OPEN_LOOP)
static void ConfigureMotorDutyCycle(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    double param;
    if (!ScpiParamDouble(parameters, param) || param < 0.0 || param > 100.0)
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
    speedInput = param;
    scpiParser.last_error = ErrorCode::NoError;
}
#elif (SPEED_CONTROL_METHOD == SPEED_CONTROL_CLOSED_LOOP)
static void ConfigureMotorSpeed(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    double param;
    if (!ScpiParamDouble(parameters, param) || param > ((((uint32_t)SPEED_CONTROLLER_MAX_SPEED * 15) << 3) / MOTOR_POLES))
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
    speedInput = ((param * SPEED_CONTROLLER_MAX_INPUT * MOTOR_POLES) >> 3) / ((uint32_t)SPEED_CONTROLLER_MAX_SPEED * 15);
    scpiParser.last_error = ErrorCode::NoError;
}
#endif

static void ConfigureMotorFrequency(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    // Clear the enable pin to stop the motor before changing frequency
    PORTD &= ~(1 << ENABLE_PIN);
 
    uint32_t param;
 
    // Read first parameter if present and within range
    if (!ScpiParamUInt32(parameters, param) || param < 7183 || param > 100000)
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
 
    // Reload the configs
    motorConfigs.tim4Freq = param;
    motorConfigs.tim4Top = (uint16_t)TIM4_TOP(motorConfigs.tim4Freq);
 
    // Wait until motor is stopped
    while (faultFlags.motorStopped == FALSE)
    {
        ;
    }
 
    // Re-init timers with the new frequency
    TimersInit();
    scpiParser.last_error = ErrorCode::NoError;
}

static void GetConfigureMotorFrequency(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(motorConfigs.tim4Freq);
}

static void ConfigureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    uint8_t param;
 
    // read first parameter if present
    if (!ScpiParamChoice(parameters, motorDirections, MOTOR_DIRECTION_OPTIONS, param))
    {
        scpiParser.last_error = ErrorCode::MissingOrInvalidParameter;
        return;
    }
 
    if (param)
    {
        // Set the direction pin if param is 1 (DIRECTION_REVERSE)
        PORTD |= (1 << DIRECTION_COMMAND_PIN);
    }
    else
    {
        // Clear the direction pin if param is 0 (DIRECTION_FORWARD)
        PORTD &= ~(1 << DIRECTION_COMMAND_PIN);
    }
    scpiParser.last_error = ErrorCode::NoError;
}

static void GetConfigureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    String name;
    ScpiChoiceToName(motorDirections, MOTOR_DIRECTION_OPTIONS, motorFlags.desiredDirection, name);
    interface.println(name);
}

static void MeasureMotorSpeed(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    if (lastCommutationTicks == 0xffff)
    {
        interface.println(0.0);
    }
    else
    {
        interface.println(
            (motorConfigs.tim4Freq * 20.0) / (lastCommutationTicks * MOTOR_POLES));
    }
}

static void MeasureMotorCurrentVBus(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(((double)ibus * 5.0 * 1000000.0) / ((double)1023.0 * IBUS_GAIN * IBUS_SENSE_RESISTOR));
}

static void MeasureMotorCurrentPhaseU(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(((double)(iphaseU - 511) * 5.0 * 1000000.0) / ((double)1023.0 * IPHASE_GAIN * IPHASE_SENSE_RESISTOR));
}

static void MeasureMotorCurrentPhaseV(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(((double)(iphaseV - 511) * 5.0 * 1000000.0) / ((double)1023.0 * IPHASE_GAIN * IPHASE_SENSE_RESISTOR));
}

static void MeasureMotorCurrentPhaseW(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(((double)(iphaseW - 511) * 5.0 * 1000000.0) / ((double)1023.0 * IPHASE_GAIN * IPHASE_SENSE_RESISTOR));
}

static void MeasureMotorDirection(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    if (motorFlags.actualDirection == DIRECTION_UNKNOWN)
    {
        interface.println(F("UNKN"));
    }
    else
    {
        String name;
        ScpiChoiceToName(motorDirections, MOTOR_DIRECTION_OPTIONS, motorFlags.actualDirection, name);
        interface.println(name);
    }
}

static void MeasureMotorVoltage(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    interface.println(((double)vbusVref * 5.0 * (VBUS_RTOP + VBUS_RBOTTOM)) / ((double)1023.0 * VBUS_RBOTTOM));
}

static void MeasureGateDutyCycle(SCPI_C commands, SCPI_P parameters, Stream &interface)
{
    if (motorFlags.enable == TRUE)
    {
        // Reading 16 bit register so disabling interrupts for atomic operation
        cli();
        uint16_t duty = 0xff & OCR4A;
        duty |= (0x03 & TC4H) << 8;
        sei();
 
        interface.println((double)duty / (double)motorConfigs.tim4Top * 100.0);
    }
    else
    {
        interface.println(0.0);
    }
}

const SCPI_choice_def_t motorDirections[MOTOR_DIRECTION_OPTIONS] = {
    {"FORW", "ard", DIRECTION_FORWARD},
    {"REVE", "rse", DIRECTION_REVERSE},
};

const SCPI_choice_def_t inputSources[INPUT_SOURCE_OPTIONS] = {
    {"LOCA", "l", SPEED_INPUT_SOURCE_LOCAL},
    {"REMO", "te", SPEED_INPUT_SOURCE_REMOTE},
};