запущен проект motor identification c терминалкой
This commit is contained in:
129
Core/Inc/ad_parameter_identification.h
Normal file
129
Core/Inc/ad_parameter_identification.h
Normal file
@@ -0,0 +1,129 @@
|
||||
#ifndef AD_PARAMETER_IDENTIFICATION_H
|
||||
#define AD_PARAMETER_IDENTIFICATION_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifndef AD_PARAM_ID_ENABLE_POWER_TESTS
|
||||
#define AD_PARAM_ID_ENABLE_POWER_TESTS 0
|
||||
#endif
|
||||
|
||||
#define AD_PARAM_ID_TIMEOUT_US_DEFAULT (5000000UL)
|
||||
|
||||
#define AD_MOTOR_PARAM_VALID_RS (1UL << 0)
|
||||
#define AD_MOTOR_PARAM_VALID_RR (1UL << 1)
|
||||
#define AD_MOTOR_PARAM_VALID_LS (1UL << 2)
|
||||
#define AD_MOTOR_PARAM_VALID_LR (1UL << 3)
|
||||
#define AD_MOTOR_PARAM_VALID_LM (1UL << 4)
|
||||
#define AD_MOTOR_PARAM_VALID_LL (1UL << 5)
|
||||
#define AD_MOTOR_PARAM_VALID_J (1UL << 6)
|
||||
#define AD_MOTOR_PARAM_VALID_B (1UL << 7)
|
||||
#define AD_MOTOR_PARAM_VALID_NOMINALS (1UL << 8)
|
||||
|
||||
#define AD_MEAS_STATUS_OVERCURRENT (1UL << 0)
|
||||
#define AD_MEAS_STATUS_OVERVOLTAGE (1UL << 1)
|
||||
#define AD_MEAS_STATUS_UNDERVOLTAGE (1UL << 2)
|
||||
#define AD_MEAS_STATUS_OVERTEMPERATURE (1UL << 3)
|
||||
#define AD_MEAS_STATUS_DRIVER_FAULT (1UL << 4)
|
||||
#define AD_MEAS_STATUS_EMERGENCY_STOP (1UL << 5)
|
||||
|
||||
#define AD_PARAM_ID_STATUS_ACTIVE (1UL << 0)
|
||||
#define AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED (1UL << 1)
|
||||
#define AD_PARAM_ID_STATUS_POWER_STAGE_ARMED (1UL << 2)
|
||||
#define AD_PARAM_ID_STATUS_FAULT_LATCHED (1UL << 3)
|
||||
#define AD_PARAM_ID_STATUS_TIMEOUT (1UL << 4)
|
||||
#define AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED (1UL << 5)
|
||||
#define AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN (1UL << 6)
|
||||
#define AD_PARAM_ID_STATUS_DATA_VALID (1UL << 7)
|
||||
|
||||
#define AD_PARAM_ID_FAULT_OVERCURRENT (1UL << 0)
|
||||
#define AD_PARAM_ID_FAULT_OVERVOLTAGE (1UL << 1)
|
||||
#define AD_PARAM_ID_FAULT_UNDERVOLTAGE (1UL << 2)
|
||||
#define AD_PARAM_ID_FAULT_OVERTEMPERATURE (1UL << 3)
|
||||
#define AD_PARAM_ID_FAULT_DRIVER (1UL << 4)
|
||||
#define AD_PARAM_ID_FAULT_EMERGENCY_STOP (1UL << 5)
|
||||
#define AD_PARAM_ID_FAULT_TIMEOUT (1UL << 6)
|
||||
#define AD_PARAM_ID_FAULT_NULL_INPUT (1UL << 7)
|
||||
|
||||
typedef enum
|
||||
{
|
||||
AD_PARAM_ID_MODE_IDLE = 0,
|
||||
AD_PARAM_ID_MODE_STATOR_RESISTANCE = 1,
|
||||
AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING = 2,
|
||||
AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE = 3,
|
||||
AD_PARAM_ID_MODE_INERTIA_FRICTION = 4,
|
||||
AD_PARAM_ID_MODE_DATA_LOGGING = 5
|
||||
} AD_ParamID_Mode_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float Rs_ohm;
|
||||
float Rr_ohm;
|
||||
float Ls_H;
|
||||
float Lr_H;
|
||||
float Lm_H;
|
||||
float Ll_H;
|
||||
float J_kg_m2;
|
||||
float B_Nm_s;
|
||||
float pole_pairs;
|
||||
float nominal_voltage_V;
|
||||
float nominal_current_A;
|
||||
float nominal_frequency_Hz;
|
||||
float nominal_power_W;
|
||||
float nominal_speed_rpm;
|
||||
uint32_t valid_mask;
|
||||
} AD_MotorParameters_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float ia_A;
|
||||
float ib_A;
|
||||
float ic_A;
|
||||
float vdc_V;
|
||||
float va_V;
|
||||
float vb_V;
|
||||
float vc_V;
|
||||
float speed_rpm;
|
||||
float temperature_C;
|
||||
uint32_t timestamp_us;
|
||||
uint32_t status_flags;
|
||||
} AD_Measurements_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t enable;
|
||||
uint8_t test_mode;
|
||||
uint8_t reset_faults;
|
||||
float pwm_duty_limit;
|
||||
float current_limit_A;
|
||||
float voltage_limit_V;
|
||||
float speed_limit_rpm;
|
||||
} AD_Command_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float overcurrent_A;
|
||||
float overvoltage_V;
|
||||
float undervoltage_V;
|
||||
float overtemperature_C;
|
||||
float speed_limit_rpm;
|
||||
uint32_t timeout_us;
|
||||
} AD_ParamID_SafetyLimits_t;
|
||||
|
||||
void AD_ParamID_Init(void);
|
||||
void AD_ParamID_Reset(void);
|
||||
void AD_ParamID_Start(uint8_t mode);
|
||||
void AD_ParamID_Stop(void);
|
||||
void AD_ParamID_StepFast(const AD_Measurements_t *meas);
|
||||
void AD_ParamID_StepSlow(void);
|
||||
const AD_MotorParameters_t* AD_ParamID_GetParameters(void);
|
||||
uint32_t AD_ParamID_GetStatus(void);
|
||||
|
||||
uint8_t AD_ParamID_GetMode(void);
|
||||
uint32_t AD_ParamID_GetFaults(void);
|
||||
void AD_ParamID_SetSafetyLimits(const AD_ParamID_SafetyLimits_t *limits);
|
||||
void AD_ParamID_SetSoftwareEnable(uint8_t enable);
|
||||
void AD_ParamID_SetLockedRotorAllowed(uint8_t enable);
|
||||
uint8_t AD_ParamID_IsPowerStageAllowed(void);
|
||||
void AD_ParamID_EmergencyStop(uint32_t fault_flags);
|
||||
|
||||
#endif
|
||||
75
Core/Inc/simulink_interface.h
Normal file
75
Core/Inc/simulink_interface.h
Normal file
@@ -0,0 +1,75 @@
|
||||
#ifndef SIMULINK_INTERFACE_H
|
||||
#define SIMULINK_INTERFACE_H
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "ad_parameter_identification.h"
|
||||
|
||||
#define SIMULINK_INTERFACE_VERSION (1U)
|
||||
#define SIMULINK_TELEMETRY_MAGIC (0x41444944UL)
|
||||
#define SIMULINK_TELEMETRY_MAX_BYTES (256U)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
AD_Measurements_t measurements;
|
||||
AD_MotorParameters_t motor_parameters;
|
||||
uint32_t param_id_status;
|
||||
uint32_t param_id_faults;
|
||||
uint8_t param_id_mode;
|
||||
uint8_t reserved[3];
|
||||
} SimulinkInterface_InputBus_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
AD_Command_t command;
|
||||
uint32_t telemetry_size_bytes;
|
||||
uint8_t telemetry_ready;
|
||||
uint8_t reserved[3];
|
||||
} SimulinkInterface_OutputBus_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint32_t magic;
|
||||
uint16_t version;
|
||||
uint16_t payload_size;
|
||||
uint32_t sequence;
|
||||
uint16_t crc16;
|
||||
uint16_t reserved;
|
||||
} SimulinkTelemetryHeader_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
AD_Measurements_t measurements;
|
||||
AD_MotorParameters_t motor_parameters;
|
||||
uint32_t param_id_status;
|
||||
uint32_t param_id_faults;
|
||||
uint8_t param_id_mode;
|
||||
uint8_t command_enable;
|
||||
uint8_t command_test_mode;
|
||||
uint8_t reserved;
|
||||
} SimulinkTelemetryPayload_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SimulinkTelemetryHeader_t header;
|
||||
SimulinkTelemetryPayload_t payload;
|
||||
} SimulinkTelemetryPacket_t;
|
||||
|
||||
void SimulinkInterface_Init(void);
|
||||
void SimulinkInterface_StepFast(void);
|
||||
void SimulinkInterface_StepSlow(void);
|
||||
void SimulinkInterface_UpdateInputs(void);
|
||||
void SimulinkInterface_UpdateOutputs(void);
|
||||
void SimulinkInterface_PackTelemetry(void);
|
||||
void SimulinkInterface_UnpackCommand(void);
|
||||
|
||||
void SimulinkInterface_SetMeasurements(const AD_Measurements_t *meas);
|
||||
void SimulinkInterface_SetCommand(const AD_Command_t *command);
|
||||
const SimulinkInterface_InputBus_t* SimulinkInterface_GetInputBus(void);
|
||||
SimulinkInterface_OutputBus_t* SimulinkInterface_GetOutputBus(void);
|
||||
const SimulinkTelemetryPacket_t* SimulinkInterface_GetTelemetryPacket(void);
|
||||
const uint8_t* SimulinkInterface_GetTelemetryBytes(void);
|
||||
size_t SimulinkInterface_GetTelemetrySize(void);
|
||||
|
||||
#endif
|
||||
283
Core/Src/ad_parameter_identification.c
Normal file
283
Core/Src/ad_parameter_identification.c
Normal file
@@ -0,0 +1,283 @@
|
||||
#include "ad_parameter_identification.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
static AD_MotorParameters_t g_params;
|
||||
static AD_Measurements_t g_last_meas;
|
||||
static AD_ParamID_SafetyLimits_t g_limits;
|
||||
static uint32_t g_status;
|
||||
static uint32_t g_faults;
|
||||
static uint32_t g_start_timestamp_us;
|
||||
static uint8_t g_mode;
|
||||
static uint8_t g_software_enable;
|
||||
static uint8_t g_locked_rotor_allowed;
|
||||
|
||||
static uint8_t mode_requires_power(uint8_t mode)
|
||||
{
|
||||
return (uint8_t)((mode == AD_PARAM_ID_MODE_STATOR_RESISTANCE) ||
|
||||
(mode == AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING) ||
|
||||
(mode == AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE) ||
|
||||
(mode == AD_PARAM_ID_MODE_INERTIA_FRICTION));
|
||||
}
|
||||
|
||||
static uint32_t elapsed_us(uint32_t now_us, uint32_t start_us)
|
||||
{
|
||||
return now_us - start_us;
|
||||
}
|
||||
|
||||
static float absf_local(float value)
|
||||
{
|
||||
return (value < 0.0f) ? -value : value;
|
||||
}
|
||||
|
||||
static void set_fault(uint32_t fault_flags)
|
||||
{
|
||||
g_faults |= fault_flags;
|
||||
if (fault_flags != 0UL) {
|
||||
g_status |= AD_PARAM_ID_STATUS_FAULT_LATCHED;
|
||||
AD_ParamID_Stop();
|
||||
}
|
||||
}
|
||||
|
||||
static void update_safety_from_measurements(const AD_Measurements_t *meas)
|
||||
{
|
||||
uint32_t faults = 0UL;
|
||||
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_OVERCURRENT) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERCURRENT;
|
||||
}
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_OVERVOLTAGE) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERVOLTAGE;
|
||||
}
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_UNDERVOLTAGE) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_UNDERVOLTAGE;
|
||||
}
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_OVERTEMPERATURE) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERTEMPERATURE;
|
||||
}
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_DRIVER_FAULT) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_DRIVER;
|
||||
}
|
||||
if ((meas->status_flags & AD_MEAS_STATUS_EMERGENCY_STOP) != 0UL) {
|
||||
faults |= AD_PARAM_ID_FAULT_EMERGENCY_STOP;
|
||||
}
|
||||
|
||||
if (g_limits.overcurrent_A > 0.0f) {
|
||||
if ((absf_local(meas->ia_A) > g_limits.overcurrent_A) ||
|
||||
(absf_local(meas->ib_A) > g_limits.overcurrent_A) ||
|
||||
(absf_local(meas->ic_A) > g_limits.overcurrent_A)) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERCURRENT;
|
||||
}
|
||||
}
|
||||
|
||||
if ((g_limits.overvoltage_V > 0.0f) && (meas->vdc_V > g_limits.overvoltage_V)) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERVOLTAGE;
|
||||
}
|
||||
|
||||
if ((g_limits.undervoltage_V > 0.0f) && (meas->vdc_V < g_limits.undervoltage_V)) {
|
||||
faults |= AD_PARAM_ID_FAULT_UNDERVOLTAGE;
|
||||
}
|
||||
|
||||
if ((g_limits.overtemperature_C > 0.0f) &&
|
||||
(meas->temperature_C > g_limits.overtemperature_C)) {
|
||||
faults |= AD_PARAM_ID_FAULT_OVERTEMPERATURE;
|
||||
}
|
||||
|
||||
if ((g_limits.speed_limit_rpm > 0.0f) &&
|
||||
(absf_local(meas->speed_rpm) > g_limits.speed_limit_rpm)) {
|
||||
faults |= AD_PARAM_ID_FAULT_EMERGENCY_STOP;
|
||||
}
|
||||
|
||||
set_fault(faults);
|
||||
}
|
||||
|
||||
static void update_limit_status(void)
|
||||
{
|
||||
if ((g_limits.overcurrent_A <= 0.0f) ||
|
||||
(g_limits.overvoltage_V <= 0.0f) ||
|
||||
(g_limits.undervoltage_V <= 0.0f) ||
|
||||
(g_limits.overtemperature_C <= 0.0f)) {
|
||||
g_status |= AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
|
||||
} else {
|
||||
g_status &= ~AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
|
||||
}
|
||||
}
|
||||
|
||||
void AD_ParamID_Init(void)
|
||||
{
|
||||
memset(&g_params, 0, sizeof(g_params));
|
||||
memset(&g_last_meas, 0, sizeof(g_last_meas));
|
||||
memset(&g_limits, 0, sizeof(g_limits));
|
||||
g_limits.timeout_us = AD_PARAM_ID_TIMEOUT_US_DEFAULT;
|
||||
g_status = AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
|
||||
g_faults = 0UL;
|
||||
g_start_timestamp_us = 0UL;
|
||||
g_mode = AD_PARAM_ID_MODE_IDLE;
|
||||
g_software_enable = 0U;
|
||||
g_locked_rotor_allowed = 0U;
|
||||
}
|
||||
|
||||
void AD_ParamID_Reset(void)
|
||||
{
|
||||
AD_ParamID_Stop();
|
||||
g_faults = 0UL;
|
||||
g_status &= ~(AD_PARAM_ID_STATUS_FAULT_LATCHED |
|
||||
AD_PARAM_ID_STATUS_TIMEOUT |
|
||||
AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED |
|
||||
AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED);
|
||||
update_limit_status();
|
||||
}
|
||||
|
||||
void AD_ParamID_Start(uint8_t mode)
|
||||
{
|
||||
g_status &= ~(AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED |
|
||||
AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED |
|
||||
AD_PARAM_ID_STATUS_TIMEOUT);
|
||||
|
||||
if ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
|
||||
g_mode = AD_PARAM_ID_MODE_IDLE;
|
||||
return;
|
||||
}
|
||||
|
||||
if (mode == AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE) {
|
||||
if (g_locked_rotor_allowed == 0U) {
|
||||
g_status |= AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED;
|
||||
g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#if AD_PARAM_ID_ENABLE_POWER_TESTS == 0
|
||||
if (mode_requires_power(mode) != 0U) {
|
||||
g_status |= AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED;
|
||||
g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
|
||||
g_status |= AD_PARAM_ID_STATUS_ACTIVE;
|
||||
g_start_timestamp_us = g_last_meas.timestamp_us;
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
if ((mode_requires_power(mode) != 0U) && (g_software_enable == 0U)) {
|
||||
g_status |= AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED;
|
||||
g_mode = AD_PARAM_ID_MODE_DATA_LOGGING;
|
||||
} else {
|
||||
g_mode = mode;
|
||||
}
|
||||
|
||||
if (g_mode == AD_PARAM_ID_MODE_IDLE) {
|
||||
AD_ParamID_Stop();
|
||||
} else {
|
||||
g_status |= AD_PARAM_ID_STATUS_ACTIVE;
|
||||
g_start_timestamp_us = g_last_meas.timestamp_us;
|
||||
}
|
||||
}
|
||||
|
||||
void AD_ParamID_Stop(void)
|
||||
{
|
||||
g_mode = AD_PARAM_ID_MODE_IDLE;
|
||||
g_status &= ~(AD_PARAM_ID_STATUS_ACTIVE | AD_PARAM_ID_STATUS_POWER_STAGE_ARMED);
|
||||
}
|
||||
|
||||
void AD_ParamID_StepFast(const AD_Measurements_t *meas)
|
||||
{
|
||||
uint32_t timeout_us;
|
||||
|
||||
if (meas == 0) {
|
||||
set_fault(AD_PARAM_ID_FAULT_NULL_INPUT);
|
||||
return;
|
||||
}
|
||||
|
||||
g_last_meas = *meas;
|
||||
update_safety_from_measurements(meas);
|
||||
update_limit_status();
|
||||
|
||||
if ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
|
||||
return;
|
||||
}
|
||||
|
||||
if ((g_status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) {
|
||||
timeout_us = (g_limits.timeout_us != 0UL) ?
|
||||
g_limits.timeout_us :
|
||||
AD_PARAM_ID_TIMEOUT_US_DEFAULT;
|
||||
if (elapsed_us(meas->timestamp_us, g_start_timestamp_us) > timeout_us) {
|
||||
g_status |= AD_PARAM_ID_STATUS_TIMEOUT;
|
||||
set_fault(AD_PARAM_ID_FAULT_TIMEOUT);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AD_ParamID_StepSlow(void)
|
||||
{
|
||||
if (g_mode == AD_PARAM_ID_MODE_DATA_LOGGING) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Parameter estimation commands are intentionally not implemented here.
|
||||
* Hardware-specific PWM/ADC stimulus must be added in the STM32G474
|
||||
* motor-driver layer after pinout and protections are verified.
|
||||
*/
|
||||
}
|
||||
|
||||
const AD_MotorParameters_t* AD_ParamID_GetParameters(void)
|
||||
{
|
||||
return &g_params;
|
||||
}
|
||||
|
||||
uint32_t AD_ParamID_GetStatus(void)
|
||||
{
|
||||
return g_status;
|
||||
}
|
||||
|
||||
uint8_t AD_ParamID_GetMode(void)
|
||||
{
|
||||
return g_mode;
|
||||
}
|
||||
|
||||
uint32_t AD_ParamID_GetFaults(void)
|
||||
{
|
||||
return g_faults;
|
||||
}
|
||||
|
||||
void AD_ParamID_SetSafetyLimits(const AD_ParamID_SafetyLimits_t *limits)
|
||||
{
|
||||
if (limits != 0) {
|
||||
g_limits = *limits;
|
||||
}
|
||||
update_limit_status();
|
||||
}
|
||||
|
||||
void AD_ParamID_SetSoftwareEnable(uint8_t enable)
|
||||
{
|
||||
g_software_enable = (enable != 0U) ? 1U : 0U;
|
||||
if (g_software_enable == 0U) {
|
||||
g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
|
||||
} else if (((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) == 0UL) &&
|
||||
(AD_PARAM_ID_ENABLE_POWER_TESTS != 0)) {
|
||||
g_status |= AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
|
||||
}
|
||||
}
|
||||
|
||||
void AD_ParamID_SetLockedRotorAllowed(uint8_t enable)
|
||||
{
|
||||
g_locked_rotor_allowed = (enable != 0U) ? 1U : 0U;
|
||||
}
|
||||
|
||||
uint8_t AD_ParamID_IsPowerStageAllowed(void)
|
||||
{
|
||||
if ((AD_PARAM_ID_ENABLE_POWER_TESTS == 0) ||
|
||||
(g_software_enable == 0U) ||
|
||||
((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL)) {
|
||||
return 0U;
|
||||
}
|
||||
|
||||
return 1U;
|
||||
}
|
||||
|
||||
void AD_ParamID_EmergencyStop(uint32_t fault_flags)
|
||||
{
|
||||
if (fault_flags == 0UL) {
|
||||
fault_flags = AD_PARAM_ID_FAULT_EMERGENCY_STOP;
|
||||
}
|
||||
set_fault(fault_flags);
|
||||
}
|
||||
173
Core/Src/simulink_interface.c
Normal file
173
Core/Src/simulink_interface.c
Normal file
@@ -0,0 +1,173 @@
|
||||
#include "simulink_interface.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
static SimulinkInterface_InputBus_t g_input_bus;
|
||||
static SimulinkInterface_OutputBus_t g_output_bus;
|
||||
static SimulinkTelemetryPacket_t g_telemetry_packet;
|
||||
static uint32_t g_telemetry_sequence;
|
||||
static uint8_t g_last_enable;
|
||||
static uint8_t g_last_test_mode;
|
||||
|
||||
static uint16_t crc16_ccitt(const uint8_t *data, size_t size)
|
||||
{
|
||||
uint16_t crc = 0xFFFFU;
|
||||
size_t i;
|
||||
uint8_t bit;
|
||||
|
||||
for (i = 0U; i < size; i++) {
|
||||
crc ^= (uint16_t)data[i] << 8;
|
||||
for (bit = 0U; bit < 8U; bit++) {
|
||||
if ((crc & 0x8000U) != 0U) {
|
||||
crc = (uint16_t)((crc << 1) ^ 0x1021U);
|
||||
} else {
|
||||
crc = (uint16_t)(crc << 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
void SimulinkInterface_Init(void)
|
||||
{
|
||||
memset(&g_input_bus, 0, sizeof(g_input_bus));
|
||||
memset(&g_output_bus, 0, sizeof(g_output_bus));
|
||||
memset(&g_telemetry_packet, 0, sizeof(g_telemetry_packet));
|
||||
g_telemetry_sequence = 0UL;
|
||||
g_last_enable = 0U;
|
||||
g_last_test_mode = AD_PARAM_ID_MODE_IDLE;
|
||||
AD_ParamID_Init();
|
||||
SimulinkInterface_PackTelemetry();
|
||||
}
|
||||
|
||||
void SimulinkInterface_StepFast(void)
|
||||
{
|
||||
SimulinkInterface_UnpackCommand();
|
||||
AD_ParamID_StepFast(&g_input_bus.measurements);
|
||||
SimulinkInterface_UpdateInputs();
|
||||
SimulinkInterface_UpdateOutputs();
|
||||
}
|
||||
|
||||
void SimulinkInterface_StepSlow(void)
|
||||
{
|
||||
AD_ParamID_StepSlow();
|
||||
SimulinkInterface_UpdateInputs();
|
||||
SimulinkInterface_PackTelemetry();
|
||||
}
|
||||
|
||||
void SimulinkInterface_UpdateInputs(void)
|
||||
{
|
||||
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
|
||||
|
||||
if (params != 0) {
|
||||
g_input_bus.motor_parameters = *params;
|
||||
}
|
||||
g_input_bus.param_id_status = AD_ParamID_GetStatus();
|
||||
g_input_bus.param_id_faults = AD_ParamID_GetFaults();
|
||||
g_input_bus.param_id_mode = AD_ParamID_GetMode();
|
||||
}
|
||||
|
||||
void SimulinkInterface_UpdateOutputs(void)
|
||||
{
|
||||
g_output_bus.telemetry_size_bytes = (uint32_t)SimulinkInterface_GetTelemetrySize();
|
||||
}
|
||||
|
||||
void SimulinkInterface_PackTelemetry(void)
|
||||
{
|
||||
uint16_t crc;
|
||||
|
||||
g_telemetry_packet.header.magic = SIMULINK_TELEMETRY_MAGIC;
|
||||
g_telemetry_packet.header.version = SIMULINK_INTERFACE_VERSION;
|
||||
g_telemetry_packet.header.payload_size = (uint16_t)sizeof(g_telemetry_packet.payload);
|
||||
g_telemetry_packet.header.sequence = g_telemetry_sequence++;
|
||||
g_telemetry_packet.header.crc16 = 0U;
|
||||
g_telemetry_packet.header.reserved = 0U;
|
||||
|
||||
g_telemetry_packet.payload.measurements = g_input_bus.measurements;
|
||||
g_telemetry_packet.payload.motor_parameters = g_input_bus.motor_parameters;
|
||||
g_telemetry_packet.payload.param_id_status = g_input_bus.param_id_status;
|
||||
g_telemetry_packet.payload.param_id_faults = g_input_bus.param_id_faults;
|
||||
g_telemetry_packet.payload.param_id_mode = g_input_bus.param_id_mode;
|
||||
g_telemetry_packet.payload.command_enable = g_output_bus.command.enable;
|
||||
g_telemetry_packet.payload.command_test_mode = g_output_bus.command.test_mode;
|
||||
g_telemetry_packet.payload.reserved = 0U;
|
||||
|
||||
crc = crc16_ccitt((const uint8_t *)&g_telemetry_packet.payload,
|
||||
sizeof(g_telemetry_packet.payload));
|
||||
g_telemetry_packet.header.crc16 = crc;
|
||||
g_output_bus.telemetry_ready = 1U;
|
||||
g_output_bus.telemetry_size_bytes = (uint32_t)sizeof(g_telemetry_packet);
|
||||
}
|
||||
|
||||
void SimulinkInterface_UnpackCommand(void)
|
||||
{
|
||||
AD_ParamID_SafetyLimits_t limits;
|
||||
const AD_Command_t *command = &g_output_bus.command;
|
||||
|
||||
if (command->reset_faults != 0U) {
|
||||
AD_ParamID_Reset();
|
||||
g_output_bus.command.reset_faults = 0U;
|
||||
}
|
||||
|
||||
limits.overcurrent_A = command->current_limit_A;
|
||||
limits.overvoltage_V = command->voltage_limit_V;
|
||||
limits.undervoltage_V = 0.0f;
|
||||
limits.overtemperature_C = 0.0f;
|
||||
limits.speed_limit_rpm = command->speed_limit_rpm;
|
||||
limits.timeout_us = AD_PARAM_ID_TIMEOUT_US_DEFAULT;
|
||||
AD_ParamID_SetSafetyLimits(&limits);
|
||||
|
||||
if (command->enable == 0U) {
|
||||
AD_ParamID_SetSoftwareEnable(0U);
|
||||
AD_ParamID_Stop();
|
||||
} else {
|
||||
AD_ParamID_SetSoftwareEnable(1U);
|
||||
if ((command->enable != g_last_enable) ||
|
||||
(command->test_mode != g_last_test_mode)) {
|
||||
AD_ParamID_Start(command->test_mode);
|
||||
}
|
||||
}
|
||||
|
||||
g_last_enable = command->enable;
|
||||
g_last_test_mode = command->test_mode;
|
||||
}
|
||||
|
||||
void SimulinkInterface_SetMeasurements(const AD_Measurements_t *meas)
|
||||
{
|
||||
if (meas != 0) {
|
||||
g_input_bus.measurements = *meas;
|
||||
}
|
||||
}
|
||||
|
||||
void SimulinkInterface_SetCommand(const AD_Command_t *command)
|
||||
{
|
||||
if (command != 0) {
|
||||
g_output_bus.command = *command;
|
||||
}
|
||||
}
|
||||
|
||||
const SimulinkInterface_InputBus_t* SimulinkInterface_GetInputBus(void)
|
||||
{
|
||||
return &g_input_bus;
|
||||
}
|
||||
|
||||
SimulinkInterface_OutputBus_t* SimulinkInterface_GetOutputBus(void)
|
||||
{
|
||||
return &g_output_bus;
|
||||
}
|
||||
|
||||
const SimulinkTelemetryPacket_t* SimulinkInterface_GetTelemetryPacket(void)
|
||||
{
|
||||
return &g_telemetry_packet;
|
||||
}
|
||||
|
||||
const uint8_t* SimulinkInterface_GetTelemetryBytes(void)
|
||||
{
|
||||
return (const uint8_t *)&g_telemetry_packet;
|
||||
}
|
||||
|
||||
size_t SimulinkInterface_GetTelemetrySize(void)
|
||||
{
|
||||
return sizeof(g_telemetry_packet);
|
||||
}
|
||||
Reference in New Issue
Block a user