запущен проект motor identification c терминалкой

This commit is contained in:
2026-06-05 12:15:36 +03:00
commit 177431f3d2
1383 changed files with 840275 additions and 0 deletions

View 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

View 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

View 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);
}

View 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);
}