запущен проект motor identification c терминалкой
This commit is contained in:
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