Andrey/motor_params
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
andrey
2026-06-05 12:15:36 +03:00
commit 177431f3d2
1383 changed files with 840275 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
AD_Keil_Project/Core/Inc/ad_binary_transport.h Normal file
View File

@@ -0,0 +1,61 @@
#ifndef AD_BINARY_TRANSPORT_H
#define AD_BINARY_TRANSPORT_H
#include <stdint.h>
typedef enum
{
AD_HOST_PROTOCOL_MODBUS = 0,
AD_HOST_PROTOCOL_BINARY_TELEMETRY = 1
} AD_HostProtocol_t;
typedef enum
{
AD_BINARY_TRANSPORT_USB = 0,
AD_BINARY_TRANSPORT_CAN = 1,
AD_BINARY_TRANSPORT_USB_AND_CAN = 2
} AD_BinaryTransportTarget_t;
typedef enum
{
AD_BINARY_TRANSPORT_STATUS_INITIALIZED = (1UL << 0),
AD_BINARY_TRANSPORT_STATUS_USB_READY = (1UL << 1),
AD_BINARY_TRANSPORT_STATUS_CAN_READY = (1UL << 2),
AD_BINARY_TRANSPORT_STATUS_USB_CONFIGURED = (1UL << 3),
AD_BINARY_TRANSPORT_STATUS_USB_TX_BUSY = (1UL << 4),
AD_BINARY_TRANSPORT_STATUS_CAN_TX_BUSY = (1UL << 5),
AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED = (1UL << 6)
} AD_BinaryTransportStatusFlag_t;
typedef struct
{
uint8_t initialized;
uint8_t host_protocol;
uint8_t binary_transport;
uint8_t usb_configured;
uint8_t usb_tx_busy;
uint8_t can_started;
uint8_t can_tx_busy;
uint8_t reserved;
uint32_t status_flags;
uint32_t period_ms;
uint32_t tx_packets;
uint32_t dropped_packets;
uint32_t usb_packets;
uint32_t can_packets;
uint32_t last_tx_tick_ms;
uint32_t last_size_bytes;
} AD_BinaryTransportState_t;
void AD_BinaryTransport_Init(void);
void AD_BinaryTransport_Loop(void);
uint8_t AD_BinaryTransport_IsProtocolValid(uint16_t protocol);
uint8_t AD_BinaryTransport_IsTransportValid(uint16_t transport);
uint8_t AD_BinaryTransport_SetProtocol(uint16_t protocol);
uint8_t AD_BinaryTransport_SetTransport(uint16_t transport);
uint8_t AD_BinaryTransport_GetProtocol(void);
uint8_t AD_BinaryTransport_GetTransport(void);
const AD_BinaryTransportState_t* AD_BinaryTransport_GetState(void);
#endif

51
AD_Keil_Project/Core/Inc/ad_board.h Normal file
View File

@@ -0,0 +1,51 @@
#ifndef AD_BOARD_H
#define AD_BOARD_H
#include <stdint.h>
#include "ad_parameter_identification.h"
#include "ad_inverter.h"
typedef struct
{
uint8_t initialized;
uint32_t faults;
uint8_t zero_calibrated;
uint16_t ia_raw;
uint16_t ib_raw;
uint16_t ic_raw;
uint16_t vdc_raw;
uint16_t vdc_raw_avg;
uint16_t mcu_temp_raw;
uint16_t ia_zero_raw;
uint16_t ib_zero_raw;
uint16_t ic_zero_raw;
uint16_t vdc_zero_raw;
float ia_adc_V;
float ib_adc_V;
float ic_adc_V;
float vdc_adc_V;
float vdc_adc_avg_V;
float mcu_temp_adc_V;
float mcu_temperature_C;
float ia_zero_adc_V;
float ib_zero_adc_V;
float ic_zero_adc_V;
float vdc_zero_adc_V;
float phase_shunt_ohm;
} AD_BoardAdcSnapshot_t;
void AD_Board_Init(void);
void AD_Board_Loop(void);
void AD_Board_ReadMeasurements(AD_Measurements_t *meas);
const AD_BoardAdcSnapshot_t* AD_Board_GetAdcSnapshot(void);
void AD_Board_ResetAdcFaults(void);
void AD_Board_SetPhaseShuntResistanceOhm(float resistance_ohm);
float AD_Board_GetPhaseShuntResistanceOhm(void);
void AD_Board_StartDataLogging(void);
void AD_Board_StartParamTest(uint8_t mode);
void AD_Board_StartParamTestWithDuty(uint8_t mode, float pwm_duty_limit);
void AD_Board_StopParamTest(void);
void AD_Board_ToggleDataLogging(void);
#endif

38
AD_Keil_Project/Core/Inc/ad_can_telemetry.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef AD_CAN_TELEMETRY_H
#define AD_CAN_TELEMETRY_H
#include <stdint.h>
typedef enum
{
AD_CAN_TELEMETRY_STATUS_INITIALIZED = (1UL << 0),
AD_CAN_TELEMETRY_STATUS_STARTED = (1UL << 1),
AD_CAN_TELEMETRY_STATUS_TX_BUSY = (1UL << 2),
AD_CAN_TELEMETRY_STATUS_TX_DROPPED = (1UL << 3),
AD_CAN_TELEMETRY_STATUS_ERROR = (1UL << 4)
} AD_CAN_TelemetryStatusFlag_t;
typedef struct
{
uint8_t initialized;
uint8_t started;
uint8_t tx_busy;
uint8_t reserved;
uint32_t status_flags;
uint32_t tx_packets;
uint32_t tx_frames;
uint32_t dropped_packets;
uint32_t error_count;
uint32_t last_error;
uint32_t base_id;
uint32_t last_size_bytes;
} AD_CAN_TelemetryState_t;
void AD_CAN_Telemetry_Init(void);
void AD_CAN_Telemetry_Loop(void);
uint8_t AD_CAN_Telemetry_QueuePacket(const uint8_t *data, uint16_t size);
uint8_t AD_CAN_Telemetry_IsStarted(void);
uint8_t AD_CAN_Telemetry_IsTxBusy(void);
const AD_CAN_TelemetryState_t* AD_CAN_Telemetry_GetState(void);
#endif

38
AD_Keil_Project/Core/Inc/ad_debug.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef AD_DEBUG_H
#define AD_DEBUG_H
#include <stdint.h>
#include "ad_board.h"
#include "ad_binary_transport.h"
#include "ad_inverter.h"
#include "ad_modbus.h"
#include "ad_parameter_identification.h"
#include "ad_project.h"
#include "simulink_interface.h"
typedef struct
{
AD_ProjectState_t project;
AD_InverterState_t inverter;
AD_BoardAdcSnapshot_t adc;
AD_Measurements_t measurements;
AD_MotorParameters_t motor_parameters;
AD_Command_t command;
AD_ModbusState_t modbus;
AD_ModbusRegisters_t modbus_regs;
AD_BinaryTransportState_t binary_transport;
uint32_t param_id_status;
uint32_t param_id_faults;
uint8_t param_id_mode;
uint8_t power_stage_allowed;
uint8_t test_running;
uint32_t tick_ms;
} AD_DebugView_t;
extern volatile AD_DebugView_t g_ad_debug;
void AD_Debug_Init(void);
void AD_Debug_Update(void);
#endif

78
AD_Keil_Project/Core/Inc/ad_inverter.h Normal file
View File

@@ -0,0 +1,78 @@
#ifndef AD_INVERTER_H
#define AD_INVERTER_H
#include <stdint.h>
#include "ad_project_config.h"
#ifndef AD_INVERTER_ENABLE_OUTPUTS
#define AD_INVERTER_ENABLE_OUTPUTS 0
#endif
#define AD_INVERTER_PWM_POLARITY_MAIN_INVERTED (1U << 0)
#define AD_INVERTER_PWM_POLARITY_COMP_INVERTED (1U << 1)
#define AD_INVERTER_PWM_POLARITY_MASK (AD_INVERTER_PWM_POLARITY_MAIN_INVERTED | \
AD_INVERTER_PWM_POLARITY_COMP_INVERTED)
#ifndef AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS
#define AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS AD_INVERTER_PWM_POLARITY_MAIN_INVERTED
#endif
typedef enum
{
AD_INVERTER_PWM_TIMING_UP = 0,
AD_INVERTER_PWM_TIMING_CENTER = 1
} AD_InverterPwmTimingMode_t;
typedef struct
{
uint8_t enable;
float duty_a;
float duty_b;
float duty_c;
} AD_InverterCommand_t;
typedef struct
{
uint8_t initialized;
uint8_t pwm_running;
uint8_t outputs_allowed_by_build;
float duty_a;
float duty_b;
float duty_c;
float electrical_frequency_Hz;
float electrical_angle_rad;
float requested_test_duty;
float applied_test_duty;
uint16_t pwm_polarity_flags;
uint16_t pwm_timing_mode;
uint8_t service_output;
uint8_t service_pwm_running;
uint8_t id_stage;
} AD_InverterState_t;
typedef enum
{
AD_INVERTER_PWM_OUTPUT_NONE = 0,
AD_INVERTER_PWM_OUTPUT_UH = 1,
AD_INVERTER_PWM_OUTPUT_UL = 2,
AD_INVERTER_PWM_OUTPUT_VH = 3,
AD_INVERTER_PWM_OUTPUT_VL = 4,
AD_INVERTER_PWM_OUTPUT_WH = 5,
AD_INVERTER_PWM_OUTPUT_WL = 6,
AD_INVERTER_PWM_OUTPUT_ALL = 7
} AD_InverterPwmOutput_t;
void AD_Inverter_Init(void);
void AD_Inverter_Disable(void);
uint8_t AD_Inverter_Enable(void);
uint8_t AD_Inverter_ApplyCommand(const AD_InverterCommand_t *command);
uint8_t AD_Inverter_StartPwmOutput(uint8_t output, float duty);
void AD_Inverter_SetDuty(float duty_a, float duty_b, float duty_c);
void AD_Inverter_SetPwmPolarityFlags(uint16_t flags);
uint16_t AD_Inverter_GetPwmPolarityFlags(void);
void AD_Inverter_SetPwmTimingMode(uint16_t mode);
uint16_t AD_Inverter_GetPwmTimingMode(void);
const AD_InverterState_t* AD_Inverter_GetState(void);
#endif

215
AD_Keil_Project/Core/Inc/ad_modbus.h Normal file
View File

@@ -0,0 +1,215 @@
#ifndef AD_MODBUS_H
#define AD_MODBUS_H
#include <stdint.h>
#include "main.h"
typedef enum
{
AD_MODBUS_FUNCTION_READ_HOLDING_REGISTERS = 3,
AD_MODBUS_FUNCTION_WRITE_SINGLE_REGISTER = 6,
AD_MODBUS_FUNCTION_WRITE_MULTIPLE_REGISTERS = 16
} AD_ModbusFunction_t;
typedef enum
{
AD_MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 1,
AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2,
AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE = 3,
AD_MODBUS_EXCEPTION_SERVER_DEVICE_FAILURE = 4
} AD_ModbusException_t;
typedef enum
{
AD_MODBUS_STATUS_INITIALIZED = (1UL << 0),
AD_MODBUS_STATUS_FRAME_READY = (1UL << 1),
AD_MODBUS_STATUS_LAST_FRAME_OK = (1UL << 2),
AD_MODBUS_STATUS_LAST_FRAME_ERROR = (1UL << 3),
AD_MODBUS_STATUS_UART_RX_ACTIVE = (1UL << 4),
AD_MODBUS_STATUS_RX_OVERFLOW = (1UL << 5)
} AD_ModbusStatusFlag_t;
typedef enum
{
AD_MODBUS_REG_DEVICE_ID = 0x0000,
AD_MODBUS_REG_PROTOCOL_VERSION = 0x0001,
AD_MODBUS_REG_CONTROL_ENABLE = 0x0002,
AD_MODBUS_REG_CONTROL_MODE = 0x0003,
AD_MODBUS_REG_CONTROL_RESET_FAULTS = 0x0004,
AD_MODBUS_REG_CONTROL_STOP = 0x0005,
AD_MODBUS_REG_CONTROL_LOCKED_ROTOR_ALLOWED = 0x0006,
AD_MODBUS_REG_CONTROL_PWM_DUTY_0P0001 = 0x0007,
AD_MODBUS_REG_LIMIT_CURRENT_0P01_A = 0x0008,
AD_MODBUS_REG_LIMIT_OVERVOLTAGE_0P1_V = 0x0009,
AD_MODBUS_REG_LIMIT_UNDERVOLTAGE_0P1_V = 0x000A,
AD_MODBUS_REG_LIMIT_SPEED_RPM = 0x000B,
AD_MODBUS_REG_LIMIT_TEMPERATURE_0P1_C = 0x000C,
AD_MODBUS_REG_CONTROL_ROTATION_FREQ_0P1_HZ = 0x000D,
AD_MODBUS_REG_CONTROL_ROTATION_MOD_0P0001 = 0x000E,
AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS = 0x000F,
AD_MODBUS_REG_STATUS_MODE = 0x0010,
AD_MODBUS_REG_STATUS_FLAGS_LO = 0x0011,
AD_MODBUS_REG_STATUS_FLAGS_HI = 0x0012,
AD_MODBUS_REG_FAULT_FLAGS_LO = 0x0013,
AD_MODBUS_REG_FAULT_FLAGS_HI = 0x0014,
AD_MODBUS_REG_POWER_STAGE_ALLOWED = 0x0015,
AD_MODBUS_REG_TEST_RUNNING = 0x0016,
AD_MODBUS_REG_INVERTER_PWM_RUNNING = 0x0017,
AD_MODBUS_REG_INVERTER_SERVICE_OUTPUT = 0x0018,
AD_MODBUS_REG_INVERTER_ID_STAGE = 0x0019,
AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE = 0x001A,
AD_MODBUS_REG_CONTROL_MOTOR_CONTROL_TYPE = 0x001B,
AD_MODBUS_REG_CONTROL_HOST_PROTOCOL = 0x001C,
AD_MODBUS_REG_CONTROL_BINARY_TRANSPORT = 0x001D,
AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS = 0x001E,
AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS = 0x001F,
AD_MODBUS_REG_MEAS_IA_0P001_A = 0x0020,
AD_MODBUS_REG_MEAS_IB_0P001_A = 0x0021,
AD_MODBUS_REG_MEAS_IC_0P001_A = 0x0022,
AD_MODBUS_REG_MEAS_VDC_0P1_V = 0x0023,
AD_MODBUS_REG_MEAS_TEMP_0P1_C = 0x0024,
AD_MODBUS_REG_MEAS_STATUS_LO = 0x0025,
AD_MODBUS_REG_MEAS_STATUS_HI = 0x0026,
AD_MODBUS_REG_MEAS_SPEED_RPM = 0x0027,
AD_MODBUS_REG_MEAS_IA_RMS_0P001_A = 0x0028,
AD_MODBUS_REG_MEAS_IB_RMS_0P001_A = 0x0029,
AD_MODBUS_REG_MEAS_IC_RMS_0P001_A = 0x002A,
AD_MODBUS_REG_MEAS_TORQUE_0P001_NM = 0x002B,
AD_MODBUS_REG_MEAS_IA_PEAK_0P001_A = 0x002C,
AD_MODBUS_REG_MEAS_IB_PEAK_0P001_A = 0x002D,
AD_MODBUS_REG_MEAS_IC_PEAK_0P001_A = 0x002E,
AD_MODBUS_REG_MEAS_SLIP_0P01_PERCENT = 0x002F,
AD_MODBUS_REG_PARAM_VALID_MASK_LO = 0x0030,
AD_MODBUS_REG_PARAM_VALID_MASK_HI = 0x0031,
AD_MODBUS_REG_PARAM_RS_MOHM = 0x0032,
AD_MODBUS_REG_PARAM_LS_UH_LO = 0x0033,
AD_MODBUS_REG_PARAM_LS_UH_HI = 0x0034,
AD_MODBUS_REG_PARAM_LL_UH_LO = 0x0035,
AD_MODBUS_REG_PARAM_LL_UH_HI = 0x0036,
AD_MODBUS_REG_PARAM_RR_MOHM = 0x0037,
AD_MODBUS_REG_PARAM_LR_UH_LO = 0x0038,
AD_MODBUS_REG_PARAM_LR_UH_HI = 0x0039,
AD_MODBUS_REG_PARAM_LM_UH_LO = 0x003A,
AD_MODBUS_REG_PARAM_LM_UH_HI = 0x003B,
AD_MODBUS_REG_PARAM_J_NKGM2_LO = 0x003C,
AD_MODBUS_REG_PARAM_J_NKGM2_HI = 0x003D,
AD_MODBUS_REG_PARAM_B_NNMS_LO = 0x003E,
AD_MODBUS_REG_PARAM_B_NNMS_HI = 0x003F,
AD_MODBUS_REG_PARAM_POLE_PAIRS = 0x0040,
AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM = 0x0041
} AD_ModbusRegister_t;
enum
{
AD_MODBUS_HOLDING_REGISTER_COUNT = 0x0042
};
typedef struct
{
uint16_t H000_device_id;
uint16_t H001_protocol_version;
uint16_t H002_control_enable;
uint16_t H003_control_mode;
uint16_t H004_control_reset_faults;
uint16_t H005_control_stop;
uint16_t H006_control_locked_rotor_allowed;
uint16_t H007_control_pwm_duty_0p0001;
uint16_t H008_limit_current_0p01_a;
uint16_t H009_limit_overvoltage_0p1_v;
uint16_t H010_limit_undervoltage_0p1_v;
uint16_t H011_limit_speed_rpm;
uint16_t H012_limit_temperature_0p1_c;
uint16_t H013_control_rotation_freq_0p1_hz;
uint16_t H014_control_rotation_mod_0p0001;
uint16_t H015_control_pwm_polarity_flags;
uint16_t H016_status_mode;
uint16_t H017_status_flags_lo;
uint16_t H018_status_flags_hi;
uint16_t H019_fault_flags_lo;
uint16_t H020_fault_flags_hi;
uint16_t H021_power_stage_allowed;
uint16_t H022_test_running;
uint16_t H023_inverter_pwm_running;
uint16_t H024_inverter_service_output;
uint16_t H025_inverter_id_stage;
uint16_t H026_control_pwm_timing_mode;
uint16_t H027_control_motor_control_type;
uint16_t H028_control_host_protocol;
uint16_t H029_control_binary_transport;
uint16_t H030_control_rotation_ramp_time_ms;
uint16_t H031_control_reset_phase_current_peaks;
uint16_t H032_meas_ia_0p001_a;
uint16_t H033_meas_ib_0p001_a;
uint16_t H034_meas_ic_0p001_a;
uint16_t H035_input_volt_0p1_v;
uint16_t H036_meas_temp_0p1_c;
uint16_t H037_meas_status_lo;
uint16_t H038_meas_status_hi;
uint16_t H039_meas_speed_rpm;
uint16_t H040_meas_ia_rms_0p001_a;
uint16_t H041_meas_ib_rms_0p001_a;
uint16_t H042_meas_ic_rms_0p001_a;
uint16_t H043_meas_torque_0p001_nm;
uint16_t H044_meas_ia_peak_0p001_a;
uint16_t H045_meas_ib_peak_0p001_a;
uint16_t H046_meas_ic_peak_0p001_a;
uint16_t H047_meas_slip_0p01_percent;
uint16_t H048_param_valid_mask_lo;
uint16_t H049_param_valid_mask_hi;
uint16_t H050_param_rs_mohm;
uint16_t H051_param_ls_uh_lo;
uint16_t H052_param_ls_uh_hi;
uint16_t H053_param_ll_uh_lo;
uint16_t H054_param_ll_uh_hi;
uint16_t H055_param_rr_mohm;
uint16_t H056_param_lr_uh_lo;
uint16_t H057_param_lr_uh_hi;
uint16_t H058_param_lm_uh_lo;
uint16_t H059_param_lm_uh_hi;
uint16_t H060_param_j_nkgm2_lo;
uint16_t H061_param_j_nkgm2_hi;
uint16_t H062_param_b_nnms_lo;
uint16_t H063_param_b_nnms_hi;
uint16_t H064_param_pole_pairs;
uint16_t H065_param_phase_shunt_mohm;
} AD_ModbusHoldingRegisters_t;
typedef union
{
AD_ModbusHoldingRegisters_t holding;
uint16_t holding_raw[AD_MODBUS_HOLDING_REGISTER_COUNT];
} AD_ModbusRegisters_t;
typedef struct
{
uint8_t initialized;
uint8_t slave_address;
uint32_t baudrate;
uint32_t status_flags;
uint32_t rx_frames;
uint32_t tx_frames;
uint32_t crc_errors;
uint32_t exception_errors;
uint32_t uart_errors;
uint32_t rx_overflows;
uint32_t write_count;
uint8_t last_function;
uint16_t last_address;
uint16_t last_quantity;
uint16_t rx_count;
} AD_ModbusState_t;
extern volatile AD_ModbusRegisters_t g_ad_modbus_regs;
extern volatile AD_ModbusRegisters_t * const MB_REGS;
void AD_Modbus_Init(UART_HandleTypeDef *uart);
void AD_Modbus_Loop(void);
const AD_ModbusState_t* AD_Modbus_GetState(void);
void AD_Modbus_RefreshRegisters(void);
const volatile AD_ModbusRegisters_t* AD_Modbus_GetRegisters(void);
#endif

242
AD_Keil_Project/Core/Inc/ad_parameter_identification.h Normal file
View File

@@ -0,0 +1,242 @@
#ifndef AD_PARAMETER_IDENTIFICATION_H
#define AD_PARAMETER_IDENTIFICATION_H
#include <stdint.h>
#include "ad_project_config.h"
#ifndef AD_PARAM_ID_ENABLE_POWER_TESTS
#define AD_PARAM_ID_ENABLE_POWER_TESTS 0
#endif
#ifndef AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT
#define AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT (0.08f)
#endif
#ifndef AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ
#define AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ (3.0f)
#endif
#ifndef AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ
#define AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ (200.0f)
#endif
#ifndef AD_PARAM_ID_DEFAULT_ROTATION_MODULATION
#define AD_PARAM_ID_DEFAULT_ROTATION_MODULATION (0.35f)
#endif
#ifndef AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS
#define AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS (3000U)
#endif
#ifndef AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS
#define AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS (100U)
#endif
#ifndef AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS
#define AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS (60000U)
#endif
#ifndef AD_PARAM_ID_DEFAULT_POLE_PAIRS
#define AD_PARAM_ID_DEFAULT_POLE_PAIRS (1U)
#endif
#ifndef AD_PARAM_ID_MAX_POLE_PAIRS
#define AD_PARAM_ID_MAX_POLE_PAIRS (64U)
#endif
#define AD_PARAM_ID_TIMEOUT_US_DEFAULT (20000000UL)
typedef enum
{
AD_PARAM_ID_MOTOR_CONTROL_AD = 0,
AD_PARAM_ID_MOTOR_CONTROL_BLDC = 1
} AD_ParamID_MotorControl_t;
typedef enum
{
AD_MOTOR_PARAM_VALID_RS = (1UL << 0),
AD_MOTOR_PARAM_VALID_RR = (1UL << 1),
AD_MOTOR_PARAM_VALID_LS = (1UL << 2),
AD_MOTOR_PARAM_VALID_LR = (1UL << 3),
AD_MOTOR_PARAM_VALID_LM = (1UL << 4),
AD_MOTOR_PARAM_VALID_LL = (1UL << 5),
AD_MOTOR_PARAM_VALID_J = (1UL << 6),
AD_MOTOR_PARAM_VALID_B = (1UL << 7),
AD_MOTOR_PARAM_VALID_NOMINALS = (1UL << 8),
AD_MOTOR_PARAM_VALID_POLE_PAIRS = (1UL << 9)
} AD_MotorParamValidFlag_t;
typedef enum
{
AD_MEAS_STATUS_OVERCURRENT = (1UL << 0),
AD_MEAS_STATUS_OVERVOLTAGE = (1UL << 1),
AD_MEAS_STATUS_UNDERVOLTAGE = (1UL << 2),
AD_MEAS_STATUS_OVERTEMPERATURE = (1UL << 3),
AD_MEAS_STATUS_DRIVER_FAULT = (1UL << 4),
AD_MEAS_STATUS_EMERGENCY_STOP = (1UL << 5)
} AD_MeasurementStatusFlag_t;
typedef enum
{
AD_PARAM_ID_STATUS_ACTIVE = (1UL << 0),
AD_PARAM_ID_STATUS_POWER_TEST_BLOCKED = (1UL << 1),
AD_PARAM_ID_STATUS_POWER_STAGE_ARMED = (1UL << 2),
AD_PARAM_ID_STATUS_FAULT_LATCHED = (1UL << 3),
AD_PARAM_ID_STATUS_TIMEOUT = (1UL << 4),
AD_PARAM_ID_STATUS_LOCKED_ROTOR_BLOCKED = (1UL << 5),
AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN = (1UL << 6),
AD_PARAM_ID_STATUS_DATA_VALID = (1UL << 7),
AD_PARAM_ID_STATUS_COMPLETE = (1UL << 8),
AD_PARAM_ID_STATUS_PARTIAL_COMPLETE = (1UL << 9),
AD_PARAM_ID_STATUS_STEP_FAILED = (1UL << 10),
AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED = (1UL << 11)
} AD_ParamID_StatusFlag_t;
typedef enum
{
AD_PARAM_ID_FAULT_OVERCURRENT = (1UL << 0),
AD_PARAM_ID_FAULT_OVERVOLTAGE = (1UL << 1),
AD_PARAM_ID_FAULT_UNDERVOLTAGE = (1UL << 2),
AD_PARAM_ID_FAULT_OVERTEMPERATURE = (1UL << 3),
AD_PARAM_ID_FAULT_DRIVER = (1UL << 4),
AD_PARAM_ID_FAULT_EMERGENCY_STOP = (1UL << 5),
AD_PARAM_ID_FAULT_TIMEOUT = (1UL << 6),
AD_PARAM_ID_FAULT_NULL_INPUT = (1UL << 7)
} AD_ParamID_FaultFlag_t;
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_PARAM_ID_MODE_PWM_TEST_UH = 6,
AD_PARAM_ID_MODE_PWM_TEST_UL = 7,
AD_PARAM_ID_MODE_PWM_TEST_VH = 8,
AD_PARAM_ID_MODE_PWM_TEST_VL = 9,
AD_PARAM_ID_MODE_PWM_TEST_WH = 10,
AD_PARAM_ID_MODE_PWM_TEST_WL = 11,
AD_PARAM_ID_MODE_PWM_TEST_ALL = 12,
AD_PARAM_ID_MODE_AUTO_IDENTIFICATION = 13,
AD_PARAM_ID_MODE_ROTATION_3HZ = 14
} 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 ia_rms_A;
float ib_rms_A;
float ic_rms_A;
float vdc_V;
float va_V;
float vb_V;
float vc_V;
float speed_rpm;
float torque_Nm;
float temperature_C;
uint32_t timestamp_us;
uint32_t status_flags;
float slip_percent;
} AD_Measurements_t;
typedef struct
{
float ia_peak_A;
float ib_peak_A;
float ic_peak_A;
} AD_PhaseCurrentPeaks_t;
typedef struct
{
uint8_t enable;
uint8_t test_mode;
uint8_t reset_faults;
uint16_t pwm_polarity_flags;
uint16_t pwm_timing_mode;
uint16_t motor_control_type;
uint16_t rotation_ramp_time_ms;
float pwm_duty_limit;
float rotation_frequency_Hz;
float rotation_modulation;
float current_limit_A;
float voltage_limit_V;
float undervoltage_limit_V;
float speed_limit_rpm;
float temperature_limit_C;
} 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);
const AD_Measurements_t* AD_ParamID_GetLastMeasurements(void);
const AD_PhaseCurrentPeaks_t* AD_ParamID_GetPhaseCurrentPeaks(void);
void AD_ParamID_ResetPhaseCurrentPeaks(void);
uint32_t AD_ParamID_GetStatus(void);
uint8_t AD_ParamID_GetMode(void);
uint32_t AD_ParamID_GetFaults(void);
void AD_ParamID_SetParameters(const AD_MotorParameters_t *params);
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_IsLockedRotorAllowed(void);
void AD_ParamID_SetPwmDutyLimit(float duty_limit);
float AD_ParamID_GetPwmDutyLimit(void);
void AD_ParamID_SetRotationFrequencyHz(float frequency_hz);
float AD_ParamID_GetRotationFrequencyHz(void);
void AD_ParamID_SetRotationModulation(float modulation);
float AD_ParamID_GetRotationModulation(void);
void AD_ParamID_SetRotationRampTimeMs(uint16_t ramp_time_ms);
uint16_t AD_ParamID_GetRotationRampTimeMs(void);
void AD_ParamID_SetMotorControlType(uint16_t motor_control_type);
uint16_t AD_ParamID_GetMotorControlType(void);
void AD_ParamID_SetPolePairs(uint16_t pole_pairs);
uint16_t AD_ParamID_GetPolePairs(void);
uint8_t AD_ParamID_IsPowerStageAllowed(void);
void AD_ParamID_EmergencyStop(uint32_t fault_flags);
void AD_ParamID_MarkComplete(void);
void AD_ParamID_MarkPartialComplete(void);
void AD_ParamID_MarkStepFailed(void);
void AD_ParamID_MarkLockedRotorSkipped(void);
uint8_t AD_ParamID_HardwareStep(uint8_t mode, const AD_Measurements_t *meas);
void AD_ParamID_HardwareDisable(void);
#endif

24
AD_Keil_Project/Core/Inc/ad_project.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef AD_PROJECT_H
#define AD_PROJECT_H
#include <stdint.h>
typedef struct
{
uint8_t initialized;
uint8_t board_initialized;
uint8_t inverter_initialized;
uint8_t simulink_initialized;
uint8_t modbus_initialized;
uint8_t binary_transport_initialized;
uint8_t outputs_allowed_by_build;
uint32_t param_id_status;
uint32_t param_id_faults;
} AD_ProjectState_t;
void AD_Project_Init(void);
void AD_Project_Loop(void);
void AD_Project_EmergencyStop(uint32_t fault_flags);
const AD_ProjectState_t* AD_Project_GetState(void);
#endif

62
AD_Keil_Project/Core/Inc/ad_project_config.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef AD_PROJECT_CONFIG_H
#define AD_PROJECT_CONFIG_H
/*
* Central switch for bench power tests.
*
* Keep the default at 0: the firmware can be flashed safely and will only log
* measurements. Define AD_PROJECT_POWER_TEST_ENABLE=1 in Keil C/C++ defines
* when the inverter, current limits, VBUS limit, BKIN and oscilloscope setup
* are ready for real PWM pulses.
*/
#ifndef AD_PROJECT_POWER_TEST_ENABLE
#define AD_PROJECT_POWER_TEST_ENABLE 0
#endif
#if AD_PROJECT_POWER_TEST_ENABLE != 0
#ifndef AD_PARAM_ID_ENABLE_POWER_TESTS
#define AD_PARAM_ID_ENABLE_POWER_TESTS 1
#endif
#ifndef AD_INVERTER_ENABLE_OUTPUTS
#define AD_INVERTER_ENABLE_OUTPUTS 1
#endif
#endif
#ifndef AD_MODBUS_ENABLE
#define AD_MODBUS_ENABLE 1
#endif
#ifndef AD_MODBUS_SLAVE_ADDRESS
#define AD_MODBUS_SLAVE_ADDRESS 1
#endif
#ifndef AD_MODBUS_UART_BAUDRATE
#define AD_MODBUS_UART_BAUDRATE 512000UL
#endif
#ifndef AD_HOST_PROTOCOL_DEFAULT
#define AD_HOST_PROTOCOL_DEFAULT 0U
#endif
#ifndef AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT
#define AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT 0U
#endif
#ifndef AD_BINARY_TELEMETRY_PERIOD_MS
#define AD_BINARY_TELEMETRY_PERIOD_MS 10UL
#endif
#ifndef AD_USB_CDC_ENABLE
#define AD_USB_CDC_ENABLE 1
#endif
#ifndef AD_CAN_TELEMETRY_ENABLE
#define AD_CAN_TELEMETRY_ENABLE 1
#endif
#ifndef AD_CAN_TELEMETRY_STD_ID_BASE
#define AD_CAN_TELEMETRY_STD_ID_BASE 0x5A0UL
#endif
#endif

38
AD_Keil_Project/Core/Inc/ad_usb_cdc.h Normal file
View File

@@ -0,0 +1,38 @@
#ifndef AD_USB_CDC_H
#define AD_USB_CDC_H
#include <stdint.h>
typedef enum
{
AD_USB_CDC_STATUS_INITIALIZED = (1UL << 0),
AD_USB_CDC_STATUS_CONFIGURED = (1UL << 1),
AD_USB_CDC_STATUS_TX_BUSY = (1UL << 2),
AD_USB_CDC_STATUS_ENUM_ERROR = (1UL << 3),
AD_USB_CDC_STATUS_TX_DROPPED = (1UL << 4)
} AD_USB_CDC_StatusFlag_t;
typedef struct
{
uint8_t initialized;
uint8_t configured;
uint8_t tx_busy;
uint8_t reserved;
uint32_t status_flags;
uint32_t tx_packets;
uint32_t tx_bytes;
uint32_t dropped_packets;
uint32_t setup_packets;
uint32_t rx_bytes;
uint32_t last_error;
} AD_USB_CDC_State_t;
void AD_USB_CDC_Init(void);
void AD_USB_CDC_Loop(void);
uint8_t AD_USB_CDC_WritePacket(const uint8_t *data, uint16_t size);
uint8_t AD_USB_CDC_IsConfigured(void);
uint8_t AD_USB_CDC_IsTxBusy(void);
const AD_USB_CDC_State_t* AD_USB_CDC_GetState(void);
void AD_USB_CDC_IRQHandler(void);
#endif

109
AD_Keil_Project/Core/Inc/main.h Normal file
View File

@@ -0,0 +1,109 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.h
* @brief : Header for main.c file.
* This file contains the common defines of the application.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32g4xx_hal.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
extern UART_HandleTypeDef huart2;
/* Exported functions prototypes ---------------------------------------------*/
void Error_Handler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
/* Private defines -----------------------------------------------------------*/
#define B1_Pin GPIO_PIN_13
#define B1_GPIO_Port GPIOC
#define B1_EXTI_IRQn EXTI15_10_IRQn
#define RCC_OSC32_IN_Pin GPIO_PIN_14
#define RCC_OSC32_IN_GPIO_Port GPIOC
#define RCC_OSC32_OUT_Pin GPIO_PIN_15
#define RCC_OSC32_OUT_GPIO_Port GPIOC
#define RCC_OSC_IN_Pin GPIO_PIN_0
#define RCC_OSC_IN_GPIO_Port GPIOF
#define RCC_OSC_OUT_Pin GPIO_PIN_1
#define RCC_OSC_OUT_GPIO_Port GPIOF
#define LD2_Pin GPIO_PIN_5
#define LD2_GPIO_Port GPIOA
#define T_SWDIO_Pin GPIO_PIN_13
#define T_SWDIO_GPIO_Port GPIOA
#define T_SWCLK_Pin GPIO_PIN_14
#define T_SWCLK_GPIO_Port GPIOA
#define T_SWO_Pin GPIO_PIN_3
#define T_SWO_GPIO_Port GPIOB
/* USER CODE BEGIN Private defines */
#define VCP_TX_Pin GPIO_PIN_2
#define VCP_TX_GPIO_Port GPIOA
#define VCP_RX_Pin GPIO_PIN_3
#define VCP_RX_GPIO_Port GPIOA
#define INV_PWM_UH_Pin GPIO_PIN_8
#define INV_PWM_UH_GPIO_Port GPIOA
#define INV_PWM_UL_Pin GPIO_PIN_7
#define INV_PWM_UL_GPIO_Port GPIOA
#define INV_PWM_VH_Pin GPIO_PIN_9
#define INV_PWM_VH_GPIO_Port GPIOA
#define INV_PWM_VL_Pin GPIO_PIN_0
#define INV_PWM_VL_GPIO_Port GPIOB
#define INV_PWM_WH_Pin GPIO_PIN_10
#define INV_PWM_WH_GPIO_Port GPIOA
#define INV_PWM_WL_Pin GPIO_PIN_1
#define INV_PWM_WL_GPIO_Port GPIOB
#define INV_PWM_BKIN_Pin GPIO_PIN_6
#define INV_PWM_BKIN_GPIO_Port GPIOA
/* USER CODE END Private defines */
#ifdef __cplusplus
}
#endif
#endif /* __MAIN_H */

77
AD_Keil_Project/Core/Inc/simulink_interface.h Normal file
View File

@@ -0,0 +1,77 @@
#ifndef SIMULINK_INTERFACE_H
#define SIMULINK_INTERFACE_H
#include <stddef.h>
#include <stdint.h>
#include "ad_parameter_identification.h"
#define SIMULINK_INTERFACE_VERSION (3U)
#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;
extern SimulinkTelemetryPacket_t g_telemetry_packet;
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

380
AD_Keil_Project/Core/Inc/stm32g4xx_hal_conf.h Normal file
View File

@@ -0,0 +1,380 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g4xx_hal_conf.h
* @author MCD Application Team
* @brief HAL configuration file
******************************************************************************
* @attention
*
* Copyright (c) 2019 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef STM32G4xx_HAL_CONF_H
#define STM32G4xx_HAL_CONF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* ########################## Module Selection ############################## */
/**
* @brief This is the list of modules to be used in the HAL driver
*/
#define HAL_MODULE_ENABLED
/*#define HAL_ADC_MODULE_ENABLED */
/*#define HAL_COMP_MODULE_ENABLED */
/*#define HAL_CORDIC_MODULE_ENABLED */
/*#define HAL_CRC_MODULE_ENABLED */
/*#define HAL_CRYP_MODULE_ENABLED */
/*#define HAL_DAC_MODULE_ENABLED */
#define HAL_FDCAN_MODULE_ENABLED
/*#define HAL_FMAC_MODULE_ENABLED */
/*#define HAL_HRTIM_MODULE_ENABLED */
/*#define HAL_IRDA_MODULE_ENABLED */
/*#define HAL_IWDG_MODULE_ENABLED */
/*#define HAL_I2C_MODULE_ENABLED */
/*#define HAL_I2S_MODULE_ENABLED */
/*#define HAL_LPTIM_MODULE_ENABLED */
/*#define HAL_NAND_MODULE_ENABLED */
/*#define HAL_NOR_MODULE_ENABLED */
/*#define HAL_OPAMP_MODULE_ENABLED */
#define HAL_PCD_MODULE_ENABLED
/*#define HAL_QSPI_MODULE_ENABLED */
/*#define HAL_RNG_MODULE_ENABLED */
/*#define HAL_RTC_MODULE_ENABLED */
/*#define HAL_SAI_MODULE_ENABLED */
/*#define HAL_SMARTCARD_MODULE_ENABLED */
/*#define HAL_SMBUS_MODULE_ENABLED */
/*#define HAL_SPI_MODULE_ENABLED */
/*#define HAL_SRAM_MODULE_ENABLED */
#define HAL_TIM_MODULE_ENABLED
#define HAL_UART_MODULE_ENABLED
/*#define HAL_USART_MODULE_ENABLED */
/*#define HAL_WWDG_MODULE_ENABLED */
#define HAL_GPIO_MODULE_ENABLED
#define HAL_EXTI_MODULE_ENABLED
#define HAL_DMA_MODULE_ENABLED
#define HAL_RCC_MODULE_ENABLED
#define HAL_FLASH_MODULE_ENABLED
#define HAL_PWR_MODULE_ENABLED
#define HAL_CORTEX_MODULE_ENABLED
/* ########################## Register Callbacks selection ############################## */
/**
* @brief This is the list of modules where register callback can be used
*/
#define USE_HAL_ADC_REGISTER_CALLBACKS 0U
#define USE_HAL_COMP_REGISTER_CALLBACKS 0U
#define USE_HAL_CORDIC_REGISTER_CALLBACKS 0U
#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U
#define USE_HAL_DAC_REGISTER_CALLBACKS 0U
#define USE_HAL_EXTI_REGISTER_CALLBACKS 0U
#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0U
#define USE_HAL_FMAC_REGISTER_CALLBACKS 0U
#define USE_HAL_HRTIM_REGISTER_CALLBACKS 0U
#define USE_HAL_I2C_REGISTER_CALLBACKS 0U
#define USE_HAL_I2S_REGISTER_CALLBACKS 0U
#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U
#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U
#define USE_HAL_NAND_REGISTER_CALLBACKS 0U
#define USE_HAL_NOR_REGISTER_CALLBACKS 0U
#define USE_HAL_OPAMP_REGISTER_CALLBACKS 0U
#define USE_HAL_PCD_REGISTER_CALLBACKS 0U
#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U
#define USE_HAL_RNG_REGISTER_CALLBACKS 0U
#define USE_HAL_RTC_REGISTER_CALLBACKS 0U
#define USE_HAL_SAI_REGISTER_CALLBACKS 0U
#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U
#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U
#define USE_HAL_SPI_REGISTER_CALLBACKS 0U
#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U
#define USE_HAL_TIM_REGISTER_CALLBACKS 0U
#define USE_HAL_UART_REGISTER_CALLBACKS 0U
#define USE_HAL_USART_REGISTER_CALLBACKS 0U
#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U
/* ########################## Oscillator Values adaptation ####################*/
/**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL).
*/
#if !defined (HSE_VALUE)
#define HSE_VALUE (24000000UL) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSI is used as system clock source, directly or through the PLL).
*/
#if !defined (HSI_VALUE)
#define HSI_VALUE (16000000UL) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
/**
* @brief Internal High Speed oscillator (HSI48) value for USB FS and RNG.
* This internal oscillator is mainly dedicated to provide a high precision clock to
* the USB peripheral by means of a special Clock Recovery System (CRS) circuitry.
* When the CRS is not used, the HSI48 RC oscillator runs on it default frequency
* which is subject to manufacturing process variations.
*/
#if !defined (HSI48_VALUE)
#define HSI48_VALUE (48000000UL) /*!< Value of the Internal High Speed oscillator for USB FS/RNG in Hz.
The real value my vary depending on manufacturing process variations.*/
#endif /* HSI48_VALUE */
/**
* @brief Internal Low Speed oscillator (LSI) value.
*/
#if !defined (LSI_VALUE)
/*!< Value of the Internal Low Speed oscillator in Hz
The real value may vary depending on the variations in voltage and temperature.*/
#define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/
#endif /* LSI_VALUE */
/**
* @brief External Low Speed oscillator (LSE) value.
* This value is used by the UART, RTC HAL module to compute the system frequency
*/
#if !defined (LSE_VALUE)
#define LSE_VALUE (32768UL) /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */
#if !defined (LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S and SAI peripherals
* This value is used by the I2S and SAI HAL modules to compute the I2S and SAI clock source
* frequency, this source is inserted directly through I2S_CKIN pad.
*/
#if !defined (EXTERNAL_CLOCK_VALUE)
#define EXTERNAL_CLOCK_VALUE (12288000UL) /*!< Value of the External oscillator in Hz*/
#endif /* EXTERNAL_CLOCK_VALUE */
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
/* ########################### System Configuration ######################### */
/**
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY (0UL) /*!< tick interrupt priority (lowest by default) */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 0U
#define INSTRUCTION_CACHE_ENABLE 1U
#define DATA_CACHE_ENABLE 1U
/* ########################## Assert Selection ############################## */
/**
* @brief Uncomment the line below to expanse the "assert_param" macro in the
* HAL drivers code
*/
/* #define USE_FULL_ASSERT 1U */
/* ################## SPI peripheral configuration ########################## */
/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
* Activated: CRC code is present inside driver
* Deactivated: CRC code cleaned from driver
*/
#define USE_SPI_CRC 0U
/* Includes ------------------------------------------------------------------*/
/**
* @brief Include module's header file
*/
#ifdef HAL_RCC_MODULE_ENABLED
#include "stm32g4xx_hal_rcc.h"
#endif /* HAL_RCC_MODULE_ENABLED */
#ifdef HAL_GPIO_MODULE_ENABLED
#include "stm32g4xx_hal_gpio.h"
#endif /* HAL_GPIO_MODULE_ENABLED */
#ifdef HAL_DMA_MODULE_ENABLED
#include "stm32g4xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32g4xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */
#ifdef HAL_ADC_MODULE_ENABLED
#include "stm32g4xx_hal_adc.h"
#endif /* HAL_ADC_MODULE_ENABLED */
#ifdef HAL_COMP_MODULE_ENABLED
#include "stm32g4xx_hal_comp.h"
#endif /* HAL_COMP_MODULE_ENABLED */
#ifdef HAL_CORDIC_MODULE_ENABLED
#include "stm32g4xx_hal_cordic.h"
#endif /* HAL_CORDIC_MODULE_ENABLED */
#ifdef HAL_CRC_MODULE_ENABLED
#include "stm32g4xx_hal_crc.h"
#endif /* HAL_CRC_MODULE_ENABLED */
#ifdef HAL_CRYP_MODULE_ENABLED
#include "stm32g4xx_hal_cryp.h"
#endif /* HAL_CRYP_MODULE_ENABLED */
#ifdef HAL_DAC_MODULE_ENABLED
#include "stm32g4xx_hal_dac.h"
#endif /* HAL_DAC_MODULE_ENABLED */
#ifdef HAL_EXTI_MODULE_ENABLED
#include "stm32g4xx_hal_exti.h"
#endif /* HAL_EXTI_MODULE_ENABLED */
#ifdef HAL_FDCAN_MODULE_ENABLED
#include "stm32g4xx_hal_fdcan.h"
#endif /* HAL_FDCAN_MODULE_ENABLED */
#ifdef HAL_FLASH_MODULE_ENABLED
#include "stm32g4xx_hal_flash.h"
#endif /* HAL_FLASH_MODULE_ENABLED */
#ifdef HAL_FMAC_MODULE_ENABLED
#include "stm32g4xx_hal_fmac.h"
#endif /* HAL_FMAC_MODULE_ENABLED */
#ifdef HAL_HRTIM_MODULE_ENABLED
#include "stm32g4xx_hal_hrtim.h"
#endif /* HAL_HRTIM_MODULE_ENABLED */
#ifdef HAL_IRDA_MODULE_ENABLED
#include "stm32g4xx_hal_irda.h"
#endif /* HAL_IRDA_MODULE_ENABLED */
#ifdef HAL_IWDG_MODULE_ENABLED
#include "stm32g4xx_hal_iwdg.h"
#endif /* HAL_IWDG_MODULE_ENABLED */
#ifdef HAL_I2C_MODULE_ENABLED
#include "stm32g4xx_hal_i2c.h"
#endif /* HAL_I2C_MODULE_ENABLED */
#ifdef HAL_I2S_MODULE_ENABLED
#include "stm32g4xx_hal_i2s.h"
#endif /* HAL_I2S_MODULE_ENABLED */
#ifdef HAL_LPTIM_MODULE_ENABLED
#include "stm32g4xx_hal_lptim.h"
#endif /* HAL_LPTIM_MODULE_ENABLED */
#ifdef HAL_NAND_MODULE_ENABLED
#include "stm32g4xx_hal_nand.h"
#endif /* HAL_NAND_MODULE_ENABLED */
#ifdef HAL_NOR_MODULE_ENABLED
#include "stm32g4xx_hal_nor.h"
#endif /* HAL_NOR_MODULE_ENABLED */
#ifdef HAL_OPAMP_MODULE_ENABLED
#include "stm32g4xx_hal_opamp.h"
#endif /* HAL_OPAMP_MODULE_ENABLED */
#ifdef HAL_PCD_MODULE_ENABLED
#include "stm32g4xx_hal_pcd.h"
#endif /* HAL_PCD_MODULE_ENABLED */
#ifdef HAL_PWR_MODULE_ENABLED
#include "stm32g4xx_hal_pwr.h"
#endif /* HAL_PWR_MODULE_ENABLED */
#ifdef HAL_QSPI_MODULE_ENABLED
#include "stm32g4xx_hal_qspi.h"
#endif /* HAL_QSPI_MODULE_ENABLED */
#ifdef HAL_RNG_MODULE_ENABLED
#include "stm32g4xx_hal_rng.h"
#endif /* HAL_RNG_MODULE_ENABLED */
#ifdef HAL_RTC_MODULE_ENABLED
#include "stm32g4xx_hal_rtc.h"
#endif /* HAL_RTC_MODULE_ENABLED */
#ifdef HAL_SAI_MODULE_ENABLED
#include "stm32g4xx_hal_sai.h"
#endif /* HAL_SAI_MODULE_ENABLED */
#ifdef HAL_SMARTCARD_MODULE_ENABLED
#include "stm32g4xx_hal_smartcard.h"
#endif /* HAL_SMARTCARD_MODULE_ENABLED */
#ifdef HAL_SMBUS_MODULE_ENABLED
#include "stm32g4xx_hal_smbus.h"
#endif /* HAL_SMBUS_MODULE_ENABLED */
#ifdef HAL_SPI_MODULE_ENABLED
#include "stm32g4xx_hal_spi.h"
#endif /* HAL_SPI_MODULE_ENABLED */
#ifdef HAL_SRAM_MODULE_ENABLED
#include "stm32g4xx_hal_sram.h"
#endif /* HAL_SRAM_MODULE_ENABLED */
#ifdef HAL_TIM_MODULE_ENABLED
#include "stm32g4xx_hal_tim.h"
#endif /* HAL_TIM_MODULE_ENABLED */
#ifdef HAL_UART_MODULE_ENABLED
#include "stm32g4xx_hal_uart.h"
#endif /* HAL_UART_MODULE_ENABLED */
#ifdef HAL_USART_MODULE_ENABLED
#include "stm32g4xx_hal_usart.h"
#endif /* HAL_USART_MODULE_ENABLED */
#ifdef HAL_WWDG_MODULE_ENABLED
#include "stm32g4xx_hal_wwdg.h"
#endif /* HAL_WWDG_MODULE_ENABLED */
/* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT
/**
* @brief The assert_param macro is used for function's parameters check.
* @param expr: If expr is false, it calls assert_failed function
* which reports the name of the source file and the source
* line number of the call that failed.
* If expr is true, it returns no value.
* @retval None
*/
#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
void assert_failed(uint8_t *file, uint32_t line);
#else
#define assert_param(expr) ((void)0U)
#endif /* USE_FULL_ASSERT */
#ifdef __cplusplus
}
#endif
#endif /* STM32G4xx_HAL_CONF_H */

71
AD_Keil_Project/Core/Inc/stm32g4xx_it.h Normal file
View File

@@ -0,0 +1,71 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g4xx_it.h
* @brief This file contains the headers of the interrupt handlers.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32G4xx_IT_H
#define __STM32G4xx_IT_H
#ifdef __cplusplus
extern "C" {
#endif
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes ---------------------------------------------*/
void NMI_Handler(void);
void HardFault_Handler(void);
void MemManage_Handler(void);
void BusFault_Handler(void);
void UsageFault_Handler(void);
void SVC_Handler(void);
void DebugMon_Handler(void);
void PendSV_Handler(void);
void SysTick_Handler(void);
void EXTI15_10_IRQHandler(void);
void TIM1_UP_TIM16_IRQHandler(void);
void USART2_IRQHandler(void);
void USB_LP_IRQHandler(void);
void USBWakeUp_IRQHandler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
#ifdef __cplusplus
}
#endif
#endif /* __STM32G4xx_IT_H */

215
AD_Keil_Project/Core/Src/ad_binary_transport.c Normal file
View File

@@ -0,0 +1,215 @@
#include "ad_binary_transport.h"
#include <stddef.h>
#include <string.h>
#include "ad_can_telemetry.h"
#include "ad_project_config.h"
#include "ad_usb_cdc.h"
#include "main.h"
#include "simulink_interface.h"
#ifndef AD_HOST_PROTOCOL_DEFAULT
#define AD_HOST_PROTOCOL_DEFAULT AD_HOST_PROTOCOL_MODBUS
#endif
#ifndef AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT
#define AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT AD_BINARY_TRANSPORT_USB
#endif
#ifndef AD_BINARY_TELEMETRY_PERIOD_MS
#define AD_BINARY_TELEMETRY_PERIOD_MS 10UL
#endif
static AD_BinaryTransportState_t g_binary_transport_state;
static void update_status_flags(void)
{
uint32_t flags = 0UL;
if (g_binary_transport_state.initialized != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_INITIALIZED;
}
if (AD_USB_CDC_GetState()->initialized != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_USB_READY;
}
if (AD_CAN_Telemetry_GetState()->started != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_CAN_READY;
}
if (AD_USB_CDC_IsConfigured() != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_USB_CONFIGURED;
}
if (AD_USB_CDC_IsTxBusy() != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_USB_TX_BUSY;
}
if (AD_CAN_Telemetry_IsTxBusy() != 0U) {
flags |= AD_BINARY_TRANSPORT_STATUS_CAN_TX_BUSY;
}
if ((g_binary_transport_state.status_flags & AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED) != 0UL) {
flags |= AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED;
}
g_binary_transport_state.usb_configured = AD_USB_CDC_IsConfigured();
g_binary_transport_state.usb_tx_busy = AD_USB_CDC_IsTxBusy();
g_binary_transport_state.can_started = AD_CAN_Telemetry_IsStarted();
g_binary_transport_state.can_tx_busy = AD_CAN_Telemetry_IsTxBusy();
g_binary_transport_state.status_flags = flags;
}
uint8_t AD_BinaryTransport_IsProtocolValid(uint16_t protocol)
{
return (uint8_t)(((protocol == (uint16_t)AD_HOST_PROTOCOL_MODBUS) ||
(protocol == (uint16_t)AD_HOST_PROTOCOL_BINARY_TELEMETRY)) ? 1U : 0U);
}
uint8_t AD_BinaryTransport_IsTransportValid(uint16_t transport)
{
return (uint8_t)(((transport == (uint16_t)AD_BINARY_TRANSPORT_USB) ||
(transport == (uint16_t)AD_BINARY_TRANSPORT_CAN) ||
(transport == (uint16_t)AD_BINARY_TRANSPORT_USB_AND_CAN)) ? 1U : 0U);
}
uint8_t AD_BinaryTransport_SetProtocol(uint16_t protocol)
{
if (AD_BinaryTransport_IsProtocolValid(protocol) == 0U) {
return 0U;
}
g_binary_transport_state.host_protocol = (uint8_t)protocol;
update_status_flags();
return 1U;
}
uint8_t AD_BinaryTransport_SetTransport(uint16_t transport)
{
if (AD_BinaryTransport_IsTransportValid(transport) == 0U) {
return 0U;
}
g_binary_transport_state.binary_transport = (uint8_t)transport;
update_status_flags();
return 1U;
}
uint8_t AD_BinaryTransport_GetProtocol(void)
{
return g_binary_transport_state.host_protocol;
}
uint8_t AD_BinaryTransport_GetTransport(void)
{
return g_binary_transport_state.binary_transport;
}
void AD_BinaryTransport_Init(void)
{
uint16_t default_protocol = (uint16_t)AD_HOST_PROTOCOL_DEFAULT;
uint16_t default_transport = (uint16_t)AD_BINARY_TELEMETRY_TRANSPORT_DEFAULT;
memset(&g_binary_transport_state, 0, sizeof(g_binary_transport_state));
g_binary_transport_state.period_ms = AD_BINARY_TELEMETRY_PERIOD_MS;
if (AD_BinaryTransport_IsProtocolValid(default_protocol) == 0U) {
default_protocol = (uint16_t)AD_HOST_PROTOCOL_MODBUS;
}
if (AD_BinaryTransport_IsTransportValid(default_transport) == 0U) {
default_transport = (uint16_t)AD_BINARY_TRANSPORT_USB;
}
g_binary_transport_state.host_protocol = (uint8_t)default_protocol;
g_binary_transport_state.binary_transport = (uint8_t)default_transport;
AD_USB_CDC_Init();
AD_CAN_Telemetry_Init();
g_binary_transport_state.initialized = 1U;
update_status_flags();
}
static uint8_t transport_uses_usb(void)
{
return (uint8_t)(((g_binary_transport_state.binary_transport == (uint8_t)AD_BINARY_TRANSPORT_USB) ||
(g_binary_transport_state.binary_transport == (uint8_t)AD_BINARY_TRANSPORT_USB_AND_CAN)) ? 1U : 0U);
}
static uint8_t transport_uses_can(void)
{
return (uint8_t)(((g_binary_transport_state.binary_transport == (uint8_t)AD_BINARY_TRANSPORT_CAN) ||
(g_binary_transport_state.binary_transport == (uint8_t)AD_BINARY_TRANSPORT_USB_AND_CAN)) ? 1U : 0U);
}
void AD_BinaryTransport_Loop(void)
{
const uint8_t *bytes;
size_t size;
uint32_t now;
uint8_t queued = 0U;
uint8_t dropped = 0U;
if (g_binary_transport_state.initialized == 0U) {
return;
}
AD_USB_CDC_Loop();
AD_CAN_Telemetry_Loop();
update_status_flags();
g_binary_transport_state.status_flags &= ~AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED;
if (g_binary_transport_state.host_protocol != (uint8_t)AD_HOST_PROTOCOL_BINARY_TELEMETRY) {
update_status_flags();
return;
}
now = HAL_GetTick();
if ((now - g_binary_transport_state.last_tx_tick_ms) < g_binary_transport_state.period_ms) {
update_status_flags();
return;
}
bytes = SimulinkInterface_GetTelemetryBytes();
size = SimulinkInterface_GetTelemetrySize();
if ((bytes == 0) || (size == 0U) || (size > SIMULINK_TELEMETRY_MAX_BYTES)) {
g_binary_transport_state.dropped_packets++;
g_binary_transport_state.status_flags |= AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED;
update_status_flags();
return;
}
if (transport_uses_usb() != 0U) {
if (AD_USB_CDC_WritePacket(bytes, (uint16_t)size) != 0U) {
queued = 1U;
g_binary_transport_state.usb_packets++;
} else {
dropped = 1U;
}
}
if (transport_uses_can() != 0U) {
if (AD_CAN_Telemetry_QueuePacket(bytes, (uint16_t)size) != 0U) {
queued = 1U;
g_binary_transport_state.can_packets++;
} else {
dropped = 1U;
}
}
if (queued != 0U) {
g_binary_transport_state.tx_packets++;
g_binary_transport_state.last_size_bytes = (uint32_t)size;
g_binary_transport_state.last_tx_tick_ms = now;
}
if (dropped != 0U) {
g_binary_transport_state.dropped_packets++;
g_binary_transport_state.status_flags |= AD_BINARY_TRANSPORT_STATUS_LAST_DROPPED;
}
update_status_flags();
}
const AD_BinaryTransportState_t* AD_BinaryTransport_GetState(void)
{
update_status_flags();
return &g_binary_transport_state;
}

1221
AD_Keil_Project/Core/Src/ad_board.c Normal file
View File

@@ -0,0 +1,1221 @@
#include "ad_board.h"
#include <string.h>
#include "ad_inverter.h"
#include "main.h"
#include "simulink_interface.h"
#include "stm32g4xx_ll_adc.h"
#define AD_BOARD_SLOW_PERIOD_MS (10UL)
#define AD_BOARD_IDLE_BLINK_MS (500UL)
#define AD_BOARD_FAULT_BLINK_MS (100UL)
#define AD_BOARD_BUTTON_DEBOUNCE_MS (200UL)
#define AD_BOARD_DEFAULT_CURRENT_LIMIT_A (10.0f)
#define AD_BOARD_DEFAULT_OVERVOLTAGE_V (60.0f)
#define AD_BOARD_DEFAULT_UNDERVOLTAGE_V (0.0f)
#define AD_BOARD_DEFAULT_SPEED_LIMIT_RPM (1000.0f)
#define AD_BOARD_DEFAULT_TEMPERATURE_C (85.0f)
#define AD_BOARD_DEFAULT_PWM_DUTY_LIMIT (AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT)
#ifndef AD_BOARD_ADC_ENABLE
#define AD_BOARD_ADC_ENABLE 1
#endif
#ifndef AD_BOARD_LED_ENABLE
#define AD_BOARD_LED_ENABLE 0
#endif
#ifndef AD_BOARD_ADC_VREF_V
#define AD_BOARD_ADC_VREF_V (3.3f)
#endif
#ifndef AD_BOARD_ADC_RAW_FULL_SCALE
#define AD_BOARD_ADC_RAW_FULL_SCALE (4095.0f)
#endif
#ifndef AD_BOARD_ADC_CURRENT_ZERO_V
#define AD_BOARD_ADC_CURRENT_ZERO_V (AD_BOARD_ADC_VREF_V * 0.5f)
#endif
#ifndef AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM (0.1f)
#endif
#ifndef AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM (0.001f)
#endif
#ifndef AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM
#define AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM (65.535f)
#endif
#ifndef AD_BOARD_ADC_CURRENT_A_PER_V
#define AD_BOARD_ADC_CURRENT_A_PER_V (1.0f / AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM)
#endif
#ifndef AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V
/* High range calibration. Above about 14.5 V this scale matches the bench supply. */
#define AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V (12.1770f)
#endif
#ifndef AD_BOARD_ADC_VDC_LOW_CAL_ADC_V
#define AD_BOARD_ADC_VDC_LOW_CAL_ADC_V (0.618f)
#endif
#ifndef AD_BOARD_ADC_VDC_LOW_CAL_BUS_V
#define AD_BOARD_ADC_VDC_LOW_CAL_BUS_V (10.0f)
#endif
#ifndef AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V
#define AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V (14.5f)
#endif
#ifndef AD_BOARD_ADC_VDC_AVG_SAMPLES
#define AD_BOARD_ADC_VDC_AVG_SAMPLES (16UL)
#endif
#ifndef AD_BOARD_ADC_VDC_FILTER_ALPHA
#define AD_BOARD_ADC_VDC_FILTER_ALPHA (0.125f)
#endif
#ifndef AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA
#define AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA (0.02f)
#endif
#ifndef AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA
#define AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA (0.05f)
#endif
#ifndef AD_BOARD_TORQUE_FILTER_ALPHA
#define AD_BOARD_TORQUE_FILTER_ALPHA (0.05f)
#endif
#ifndef AD_BOARD_TORQUE_MIN_SPEED_RAD_S
#define AD_BOARD_TORQUE_MIN_SPEED_RAD_S (1.0f)
#endif
#ifndef AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2
#define AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2 (0.1f)
#endif
#ifndef AD_BOARD_TORQUE_MIN_POWER_W
#define AD_BOARD_TORQUE_MIN_POWER_W (0.01f)
#endif
#ifndef AD_BOARD_TORQUE_MIN_CURRENT_RMS_A
#define AD_BOARD_TORQUE_MIN_CURRENT_RMS_A (0.01f)
#endif
#ifndef AD_BOARD_TORQUE_RMS_POWER_FACTOR
#define AD_BOARD_TORQUE_RMS_POWER_FACTOR (1.0f)
#endif
#ifndef AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS
#define AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS ((float)AD_PARAM_ID_DEFAULT_POLE_PAIRS)
#endif
#ifndef AD_BOARD_TORQUE_MAX_ABS_NM
#define AD_BOARD_TORQUE_MAX_ABS_NM (1000.0f)
#endif
#ifndef AD_BOARD_SLIP_MIN_SYNC_RPM
#define AD_BOARD_SLIP_MIN_SYNC_RPM (1.0f)
#endif
#ifndef AD_BOARD_SLIP_MAX_ABS_PERCENT
#define AD_BOARD_SLIP_MAX_ABS_PERCENT (327.67f)
#endif
#ifndef AD_BOARD_ADC_DEFAULT_MCU_TEMP_C
#define AD_BOARD_ADC_DEFAULT_MCU_TEMP_C (25.0f)
#endif
#ifndef AD_BOARD_ADC_ZERO_CAL_ENABLE
#define AD_BOARD_ADC_ZERO_CAL_ENABLE 1
#endif
#ifndef AD_BOARD_ADC_ZERO_CAL_SAMPLES
#define AD_BOARD_ADC_ZERO_CAL_SAMPLES (64UL)
#endif
#ifndef AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V
#define AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V (0.05f)
#endif
#ifndef AD_BOARD_ADC_TIMEOUT
#define AD_BOARD_ADC_TIMEOUT (1000000UL)
#endif
#ifndef AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD
#define AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD (3U)
#endif
/* X-NUCLEO-IHM08M1 schematic, Figure 6: PA0-PhA, PC1-PhB, PC0-PhC, PA1-BUSV. */
#define AD_BOARD_ADC_CH_IA (1UL)
#define AD_BOARD_ADC_CH_IB (7UL)
#define AD_BOARD_ADC_CH_IC (6UL)
#define AD_BOARD_ADC_CH_VDC (2UL)
#define AD_BOARD_ADC_CH_MCU_TEMP (16UL)
#define AD_BOARD_ADC_GPIOA_PIN_MASK (GPIO_PIN_0 | GPIO_PIN_1)
#define AD_BOARD_ADC_GPIOC_PIN_MASK (GPIO_PIN_0 | GPIO_PIN_1)
#define AD_BOARD_ADC_FAULT_INIT (1UL << 0)
#define AD_BOARD_ADC_FAULT_TIMEOUT (1UL << 1)
#define AD_BOARD_TWO_PI (6.28318530717958647692f)
static volatile uint8_t g_button_toggle_request;
static uint8_t g_logging_enabled;
static uint8_t g_led_state;
static uint32_t g_last_button_tick_ms;
static uint32_t g_last_slow_tick_ms;
static uint32_t g_last_led_tick_ms;
static AD_BoardAdcSnapshot_t g_adc_snapshot;
static uint8_t g_vdc_filter_ready;
static float g_vdc_adc_filtered_V;
static uint8_t g_current_rms_filter_ready;
static float g_ia_rms_sq_A2;
static float g_ib_rms_sq_A2;
static float g_ic_rms_sq_A2;
static uint8_t g_mcu_temp_filter_ready;
static float g_mcu_temperature_C;
static uint8_t g_adc_timeout_count;
static uint8_t g_torque_speed_ready;
static uint8_t g_torque_filter_ready;
static uint32_t g_torque_timestamp_us;
static float g_torque_speed_rad_s;
static float g_torque_filtered_Nm;
static float g_phase_shunt_resistance_ohm;
static uint8_t wait_bits_set(volatile uint32_t *reg, uint32_t mask, uint32_t timeout)
{
while (((*reg) & mask) == 0UL) {
if (timeout == 0UL) {
return 0U;
}
timeout--;
}
return 1U;
}
static uint8_t wait_bits_clear(volatile uint32_t *reg, uint32_t mask, uint32_t timeout)
{
while (((*reg) & mask) != 0UL) {
if (timeout == 0UL) {
return 0U;
}
timeout--;
}
return 1U;
}
static uint8_t board_adc_stop_regular(void)
{
#if AD_BOARD_ADC_ENABLE != 0
if ((ADC1->CR & ADC_CR_ADSTART) == 0UL) {
return 1U;
}
ADC1->CR |= ADC_CR_ADSTP;
if (wait_bits_clear(&ADC1->CR, ADC_CR_ADSTART, AD_BOARD_ADC_TIMEOUT) == 0U) {
return 0U;
}
if (wait_bits_clear(&ADC1->CR, ADC_CR_ADSTP, AD_BOARD_ADC_TIMEOUT) == 0U) {
return 0U;
}
#endif
return 1U;
}
static void board_adc_note_timeout(void)
{
if (g_adc_timeout_count < 0xFFU) {
g_adc_timeout_count++;
}
if (g_adc_timeout_count >= AD_BOARD_ADC_TIMEOUT_FAULT_THRESHOLD) {
g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_TIMEOUT;
}
}
static void board_adc_note_success(void)
{
g_adc_timeout_count = 0U;
g_adc_snapshot.faults &= ~AD_BOARD_ADC_FAULT_TIMEOUT;
}
static float adc_raw_to_voltage(uint16_t raw)
{
return ((float)raw * AD_BOARD_ADC_VREF_V) / AD_BOARD_ADC_RAW_FULL_SCALE;
}
static float board_normalize_phase_shunt_resistance(float resistance_ohm)
{
if (resistance_ohm <= 0.0f) {
return AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM;
}
if (resistance_ohm < AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM) {
return AD_BOARD_ADC_MIN_PHASE_SHUNT_OHM;
}
if (resistance_ohm > AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM) {
return AD_BOARD_ADC_MAX_PHASE_SHUNT_OHM;
}
return resistance_ohm;
}
static float board_phase_shunt_resistance_ohm(void)
{
return board_normalize_phase_shunt_resistance(g_phase_shunt_resistance_ohm);
}
static float board_current_a_per_v(void)
{
return AD_BOARD_ADC_CURRENT_A_PER_V *
(AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM / board_phase_shunt_resistance_ohm());
}
static float adc_current_from_voltage(float voltage, float zero_voltage)
{
if (zero_voltage <= 0.0f) {
zero_voltage = AD_BOARD_ADC_CURRENT_ZERO_V;
}
return (voltage - zero_voltage) * board_current_a_per_v();
}
static float adc_vdc_from_voltage(float vdc_adc_V)
{
float high_adc_V;
float low_slope;
float mid_slope;
float vdc_V;
if (vdc_adc_V <= 0.0f) {
return 0.0f;
}
high_adc_V = AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V / AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
if ((AD_BOARD_ADC_VDC_LOW_CAL_ADC_V <= 0.0f) ||
(AD_BOARD_ADC_VDC_LOW_CAL_BUS_V <= 0.0f) ||
(high_adc_V <= AD_BOARD_ADC_VDC_LOW_CAL_ADC_V)) {
return vdc_adc_V * AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
}
if (vdc_adc_V <= AD_BOARD_ADC_VDC_LOW_CAL_ADC_V) {
low_slope = AD_BOARD_ADC_VDC_LOW_CAL_BUS_V / AD_BOARD_ADC_VDC_LOW_CAL_ADC_V;
return vdc_adc_V * low_slope;
}
if (vdc_adc_V >= high_adc_V) {
return vdc_adc_V * AD_BOARD_ADC_VDC_BUS_V_PER_ADC_V;
}
mid_slope = (AD_BOARD_ADC_VDC_HIGH_CAL_BUS_V - AD_BOARD_ADC_VDC_LOW_CAL_BUS_V) /
(high_adc_V - AD_BOARD_ADC_VDC_LOW_CAL_ADC_V);
vdc_V = AD_BOARD_ADC_VDC_LOW_CAL_BUS_V +
((vdc_adc_V - AD_BOARD_ADC_VDC_LOW_CAL_ADC_V) * mid_slope);
return (vdc_V > 0.0f) ? vdc_V : 0.0f;
}
static float adc_mcu_temperature_from_raw(uint16_t raw)
{
float cal1;
float cal2;
float raw_at_cal_vref;
float temp_C;
cal1 = (float)(*TEMPSENSOR_CAL1_ADDR);
cal2 = (float)(*TEMPSENSOR_CAL2_ADDR);
if ((raw == 0U) || (cal2 <= cal1)) {
return AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
}
raw_at_cal_vref = ((float)raw * (AD_BOARD_ADC_VREF_V * 1000.0f)) /
(float)TEMPSENSOR_CAL_VREFANALOG;
temp_C = (((raw_at_cal_vref - cal1) *
(float)(TEMPSENSOR_CAL2_TEMP - TEMPSENSOR_CAL1_TEMP)) /
(cal2 - cal1)) +
(float)TEMPSENSOR_CAL1_TEMP;
if (temp_C < -40.0f) {
temp_C = -40.0f;
} else if (temp_C > 150.0f) {
temp_C = 150.0f;
}
return temp_C;
}
static float board_mcu_temperature_filter(float temp_C)
{
float alpha = AD_BOARD_ADC_MCU_TEMP_FILTER_ALPHA;
if (alpha < 0.0f) {
alpha = 0.0f;
} else if (alpha > 1.0f) {
alpha = 1.0f;
}
if (g_mcu_temp_filter_ready == 0U) {
g_mcu_temperature_C = temp_C;
g_mcu_temp_filter_ready = 1U;
} else {
g_mcu_temperature_C += alpha * (temp_C - g_mcu_temperature_C);
}
return g_mcu_temperature_C;
}
static uint16_t board_adc_read_channel(uint32_t channel)
{
#if AD_BOARD_ADC_ENABLE != 0
if (board_adc_stop_regular() == 0U) {
board_adc_note_timeout();
return 0U;
}
ADC1->SQR1 = ((channel & 0x1FUL) << 6);
ADC1->ISR = ADC_ISR_EOSMP | ADC_ISR_EOC | ADC_ISR_EOS | ADC_ISR_OVR;
ADC1->CR |= ADC_CR_ADSTART;
if (wait_bits_set(&ADC1->ISR, ADC_ISR_EOC, AD_BOARD_ADC_TIMEOUT) == 0U) {
(void)board_adc_stop_regular();
board_adc_note_timeout();
return 0U;
}
board_adc_note_success();
return (uint16_t)(ADC1->DR & 0xFFFFUL);
#else
(void)channel;
return 0U;
#endif
}
static uint16_t board_adc_read_channel_average(uint32_t channel, uint32_t samples, uint16_t *last_raw)
{
#if AD_BOARD_ADC_ENABLE != 0
uint32_t index;
uint32_t sum = 0UL;
uint16_t raw = 0U;
if (samples == 0UL) {
samples = 1UL;
}
for (index = 0UL; index < samples; index++) {
raw = board_adc_read_channel(channel);
sum += raw;
}
if (last_raw != 0) {
*last_raw = raw;
}
return (uint16_t)((sum + (samples / 2UL)) / samples);
#else
(void)channel;
(void)samples;
if (last_raw != 0) {
*last_raw = 0U;
}
return 0U;
#endif
}
static float adc_voltage_minus_offset(float voltage, float offset)
{
voltage -= offset;
return (voltage > 0.0f) ? voltage : 0.0f;
}
static float board_absf(float value)
{
return (value < 0.0f) ? -value : value;
}
static float board_rpm_to_rad_s(float rpm)
{
return rpm * (AD_BOARD_TWO_PI / 60.0f);
}
static float board_clamp_abs(float value, float limit)
{
if (limit <= 0.0f) {
return value;
}
if (value > limit) {
return limit;
}
if (value < -limit) {
return -limit;
}
return value;
}
static float board_get_pole_pairs(const AD_MotorParameters_t *params)
{
if ((params != 0) && (params->pole_pairs > 0.0f)) {
if (params->pole_pairs > (float)AD_PARAM_ID_MAX_POLE_PAIRS) {
return (float)AD_PARAM_ID_MAX_POLE_PAIRS;
}
return params->pole_pairs;
}
return AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS;
}
static float board_torque_omega_from_pwm(const AD_MotorParameters_t *params)
{
const AD_InverterState_t *inverter = AD_Inverter_GetState();
float pole_pairs;
if ((inverter == 0) ||
(inverter->pwm_running == 0U) ||
(inverter->electrical_frequency_Hz <= 0.0f)) {
return 0.0f;
}
pole_pairs = board_get_pole_pairs(params);
if (pole_pairs <= 0.0f) {
pole_pairs = AD_BOARD_TORQUE_DEFAULT_POLE_PAIRS;
}
return (AD_BOARD_TWO_PI * inverter->electrical_frequency_Hz) / pole_pairs;
}
static float board_torque_omega_rad_s(const AD_Measurements_t *meas,
const AD_MotorParameters_t *params)
{
float omega_rad_s;
if (meas == 0) {
return 0.0f;
}
omega_rad_s = board_rpm_to_rad_s(meas->speed_rpm);
if (board_absf(omega_rad_s) >= AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
return omega_rad_s;
}
omega_rad_s = board_torque_omega_from_pwm(params);
if (board_absf(omega_rad_s) >= AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
return omega_rad_s;
}
return 0.0f;
}
static void board_adc_zero_calibrate(void)
{
#if (AD_BOARD_ADC_ENABLE != 0) && (AD_BOARD_ADC_ZERO_CAL_ENABLE != 0)
g_adc_snapshot.ia_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IA,
AD_BOARD_ADC_ZERO_CAL_SAMPLES,
0);
g_adc_snapshot.ib_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IB,
AD_BOARD_ADC_ZERO_CAL_SAMPLES,
0);
g_adc_snapshot.ic_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_IC,
AD_BOARD_ADC_ZERO_CAL_SAMPLES,
0);
g_adc_snapshot.vdc_zero_raw = board_adc_read_channel_average(AD_BOARD_ADC_CH_VDC,
AD_BOARD_ADC_ZERO_CAL_SAMPLES,
0);
g_adc_snapshot.ia_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ia_zero_raw);
g_adc_snapshot.ib_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ib_zero_raw);
g_adc_snapshot.ic_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.ic_zero_raw);
g_adc_snapshot.vdc_zero_adc_V = adc_raw_to_voltage(g_adc_snapshot.vdc_zero_raw);
if (g_adc_snapshot.vdc_zero_adc_V > AD_BOARD_ADC_VDC_ZERO_MAX_ADC_V) {
g_adc_snapshot.vdc_zero_raw = 0U;
g_adc_snapshot.vdc_zero_adc_V = 0.0f;
}
g_adc_snapshot.zero_calibrated = 1U;
#endif
}
static float board_vdc_filter(float vdc_adc_V)
{
float alpha = AD_BOARD_ADC_VDC_FILTER_ALPHA;
if (alpha < 0.0f) {
alpha = 0.0f;
} else if (alpha > 1.0f) {
alpha = 1.0f;
}
if (g_vdc_filter_ready == 0U) {
g_vdc_adc_filtered_V = vdc_adc_V;
g_vdc_filter_ready = 1U;
} else {
g_vdc_adc_filtered_V += alpha * (vdc_adc_V - g_vdc_adc_filtered_V);
}
return g_vdc_adc_filtered_V;
}
static float board_sqrtf(float value)
{
float x;
uint8_t i;
if (value <= 0.0f) {
return 0.0f;
}
x = (value > 1.0f) ? value : 1.0f;
for (i = 0U; i < 8U; i++) {
x = 0.5f * (x + (value / x));
}
return x;
}
static float board_average_phase_current_rms(const AD_Measurements_t *meas)
{
float sum_A = 0.0f;
float count = 0.0f;
if (meas == 0) {
return 0.0f;
}
if (meas->ia_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
sum_A += meas->ia_rms_A;
count += 1.0f;
}
if (meas->ib_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
sum_A += meas->ib_rms_A;
count += 1.0f;
}
if (meas->ic_rms_A > AD_BOARD_TORQUE_MIN_CURRENT_RMS_A) {
sum_A += meas->ic_rms_A;
count += 1.0f;
}
return (count > 0.0f) ? (sum_A / count) : 0.0f;
}
static float board_phase_voltage_rms_estimate(const AD_Measurements_t *meas)
{
float sum_sq_V2;
if (meas == 0) {
return 0.0f;
}
sum_sq_V2 = (meas->va_V * meas->va_V) +
(meas->vb_V * meas->vb_V) +
(meas->vc_V * meas->vc_V);
return board_sqrtf(sum_sq_V2 / 3.0f);
}
static uint8_t board_rms_torque_power_estimate(const AD_Measurements_t *meas,
float *power_W)
{
float v_phase_rms_V;
float i_phase_rms_A;
float power_factor = AD_BOARD_TORQUE_RMS_POWER_FACTOR;
if ((meas == 0) || (power_W == 0)) {
return 0U;
}
if (power_factor < 0.0f) {
power_factor = 0.0f;
} else if (power_factor > 1.0f) {
power_factor = 1.0f;
}
if (power_factor <= 0.0f) {
return 0U;
}
v_phase_rms_V = board_phase_voltage_rms_estimate(meas);
i_phase_rms_A = board_average_phase_current_rms(meas);
if ((v_phase_rms_V <= 0.0f) ||
(i_phase_rms_A <= AD_BOARD_TORQUE_MIN_CURRENT_RMS_A)) {
return 0U;
}
*power_W = 3.0f * v_phase_rms_V * i_phase_rms_A * power_factor;
return (board_absf(*power_W) >= AD_BOARD_TORQUE_MIN_POWER_W) ? 1U : 0U;
}
static void board_update_current_rms(AD_Measurements_t *meas)
{
float alpha = AD_BOARD_ADC_CURRENT_RMS_FILTER_ALPHA;
float ia_sq;
float ib_sq;
float ic_sq;
if (meas == 0) {
return;
}
if (alpha < 0.0f) {
alpha = 0.0f;
} else if (alpha > 1.0f) {
alpha = 1.0f;
}
ia_sq = meas->ia_A * meas->ia_A;
ib_sq = meas->ib_A * meas->ib_A;
ic_sq = meas->ic_A * meas->ic_A;
if (g_current_rms_filter_ready == 0U) {
g_ia_rms_sq_A2 = ia_sq;
g_ib_rms_sq_A2 = ib_sq;
g_ic_rms_sq_A2 = ic_sq;
g_current_rms_filter_ready = 1U;
} else {
g_ia_rms_sq_A2 += alpha * (ia_sq - g_ia_rms_sq_A2);
g_ib_rms_sq_A2 += alpha * (ib_sq - g_ib_rms_sq_A2);
g_ic_rms_sq_A2 += alpha * (ic_sq - g_ic_rms_sq_A2);
}
meas->ia_rms_A = board_sqrtf(g_ia_rms_sq_A2);
meas->ib_rms_A = board_sqrtf(g_ib_rms_sq_A2);
meas->ic_rms_A = board_sqrtf(g_ic_rms_sq_A2);
}
static void board_update_phase_voltage_estimate(AD_Measurements_t *meas)
{
const AD_InverterState_t *inverter = AD_Inverter_GetState();
float duty_common;
if ((meas == 0) || (inverter == 0) || (meas->vdc_V <= 0.0f)) {
return;
}
duty_common = (inverter->duty_a + inverter->duty_b + inverter->duty_c) / 3.0f;
meas->va_V = (inverter->duty_a - duty_common) * meas->vdc_V;
meas->vb_V = (inverter->duty_b - duty_common) * meas->vdc_V;
meas->vc_V = (inverter->duty_c - duty_common) * meas->vdc_V;
}
static uint8_t board_mechanical_torque_estimate(const AD_Measurements_t *meas,
float *torque_Nm)
{
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
float omega_rad_s;
float alpha_rad_s2 = 0.0f;
float dt_s;
uint32_t dt_us;
uint8_t valid = 0U;
if ((meas == 0) || (torque_Nm == 0) || (params == 0)) {
return 0U;
}
omega_rad_s = board_rpm_to_rad_s(meas->speed_rpm);
if (g_torque_speed_ready != 0U) {
dt_us = meas->timestamp_us - g_torque_timestamp_us;
if (dt_us > 0UL) {
dt_s = (float)dt_us * 0.000001f;
alpha_rad_s2 = (omega_rad_s - g_torque_speed_rad_s) / dt_s;
valid = 1U;
}
}
g_torque_speed_rad_s = omega_rad_s;
g_torque_timestamp_us = meas->timestamp_us;
g_torque_speed_ready = 1U;
if (valid == 0U) {
return 0U;
}
if ((board_absf(omega_rad_s) < AD_BOARD_TORQUE_MIN_SPEED_RAD_S) &&
(board_absf(alpha_rad_s2) < AD_BOARD_TORQUE_MIN_ACCEL_RAD_S2)) {
return 0U;
}
*torque_Nm = 0.0f;
valid = 0U;
if (((params->valid_mask & AD_MOTOR_PARAM_VALID_J) != 0UL) &&
(params->J_kg_m2 > 0.0f)) {
*torque_Nm += params->J_kg_m2 * alpha_rad_s2;
valid = 1U;
}
if (((params->valid_mask & AD_MOTOR_PARAM_VALID_B) != 0UL) &&
(params->B_Nm_s > 0.0f)) {
*torque_Nm += params->B_Nm_s * omega_rad_s;
valid = 1U;
}
return valid;
}
static uint8_t board_electrical_torque_estimate(const AD_Measurements_t *meas,
float *torque_Nm)
{
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
float omega_rad_s;
float phase_current_sq_A2;
float power_W;
float copper_loss_W = 0.0f;
if ((meas == 0) || (torque_Nm == 0)) {
return 0U;
}
omega_rad_s = board_torque_omega_rad_s(meas, params);
if (board_absf(omega_rad_s) < AD_BOARD_TORQUE_MIN_SPEED_RAD_S) {
return 0U;
}
if (board_rms_torque_power_estimate(meas, &power_W) != 0U) {
if (omega_rad_s < 0.0f) {
power_W = -power_W;
}
} else {
power_W = (meas->va_V * meas->ia_A) +
(meas->vb_V * meas->ib_A) +
(meas->vc_V * meas->ic_A);
if ((params != 0) &&
((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL) &&
(params->Rs_ohm > 0.0f)) {
phase_current_sq_A2 = ((meas->ia_A * meas->ia_A) +
(meas->ib_A * meas->ib_A) +
(meas->ic_A * meas->ic_A)) / 3.0f;
copper_loss_W = 3.0f * phase_current_sq_A2 * params->Rs_ohm;
if (power_W > copper_loss_W) {
power_W -= copper_loss_W;
} else if (power_W < -copper_loss_W) {
power_W += copper_loss_W;
} else {
power_W = 0.0f;
}
}
if (board_absf(power_W) < AD_BOARD_TORQUE_MIN_POWER_W) {
return 0U;
}
}
*torque_Nm = power_W / omega_rad_s;
return 1U;
}
static void board_update_torque_estimate(AD_Measurements_t *meas)
{
float alpha = AD_BOARD_TORQUE_FILTER_ALPHA;
float torque_Nm = 0.0f;
uint8_t valid;
if (meas == 0) {
return;
}
board_update_phase_voltage_estimate(meas);
valid = board_mechanical_torque_estimate(meas, &torque_Nm);
if (valid == 0U) {
valid = board_electrical_torque_estimate(meas, &torque_Nm);
}
if (valid == 0U) {
g_torque_filter_ready = 0U;
g_torque_filtered_Nm = 0.0f;
meas->torque_Nm = 0.0f;
return;
}
torque_Nm = board_clamp_abs(torque_Nm, AD_BOARD_TORQUE_MAX_ABS_NM);
if (alpha < 0.0f) {
alpha = 0.0f;
} else if (alpha > 1.0f) {
alpha = 1.0f;
}
if (g_torque_filter_ready == 0U) {
g_torque_filtered_Nm = torque_Nm;
g_torque_filter_ready = 1U;
} else {
g_torque_filtered_Nm += alpha * (torque_Nm - g_torque_filtered_Nm);
}
meas->torque_Nm = g_torque_filtered_Nm;
}
static void board_update_slip_measurement(AD_Measurements_t *meas)
{
const AD_InverterState_t *inverter = AD_Inverter_GetState();
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
float pole_pairs;
float synchronous_rpm;
float slip_percent;
if ((meas == 0) ||
(inverter == 0) ||
(inverter->pwm_running == 0U) ||
(inverter->electrical_frequency_Hz == 0.0f)) {
return;
}
pole_pairs = board_get_pole_pairs(params);
if (pole_pairs <= 0.0f) {
return;
}
synchronous_rpm = (60.0f * inverter->electrical_frequency_Hz) / pole_pairs;
if (board_absf(synchronous_rpm) < AD_BOARD_SLIP_MIN_SYNC_RPM) {
return;
}
slip_percent = ((synchronous_rpm - meas->speed_rpm) * 100.0f) / synchronous_rpm;
meas->slip_percent = board_clamp_abs(slip_percent, AD_BOARD_SLIP_MAX_ABS_PERCENT);
}
static void board_adc_gpio_init(void)
{
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIOA->MODER |= (GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
GPIOA->PUPDR &= ~(GPIO_PUPDR_PUPD0 | GPIO_PUPDR_PUPD1);
GPIOC->MODER |= (GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
GPIOC->PUPDR &= ~(GPIO_PUPDR_PUPD0 | GPIO_PUPDR_PUPD1);
}
static void board_adc_init(void)
{
#if AD_BOARD_ADC_ENABLE != 0
uint32_t wait;
memset(&g_adc_snapshot, 0, sizeof(g_adc_snapshot));
g_adc_snapshot.phase_shunt_ohm = board_phase_shunt_resistance_ohm();
g_vdc_filter_ready = 0U;
g_vdc_adc_filtered_V = 0.0f;
g_current_rms_filter_ready = 0U;
g_ia_rms_sq_A2 = 0.0f;
g_ib_rms_sq_A2 = 0.0f;
g_ic_rms_sq_A2 = 0.0f;
g_mcu_temp_filter_ready = 0U;
g_mcu_temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
g_adc_timeout_count = 0U;
g_torque_speed_ready = 0U;
g_torque_filter_ready = 0U;
g_torque_timestamp_us = 0UL;
g_torque_speed_rad_s = 0.0f;
g_torque_filtered_Nm = 0.0f;
board_adc_gpio_init();
__HAL_RCC_ADC12_CLK_ENABLE();
if (board_adc_stop_regular() == 0U) {
g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
return;
}
if ((ADC1->CR & ADC_CR_ADEN) != 0UL) {
ADC1->CR |= ADC_CR_ADDIS;
if (wait_bits_clear(&ADC1->CR, ADC_CR_ADEN, AD_BOARD_ADC_TIMEOUT) == 0U) {
g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
return;
}
}
#if defined(ADC12_COMMON) && defined(ADC_CCR_CKMODE_0)
ADC12_COMMON->CCR &= ~ADC_CCR_CKMODE;
ADC12_COMMON->CCR |= ADC_CCR_CKMODE_1;
#endif
#if defined(ADC12_COMMON) && defined(ADC_CCR_VSENSESEL)
ADC12_COMMON->CCR |= ADC_CCR_VSENSESEL;
for (wait = 0UL; wait < 10000UL; wait++) {
__NOP();
}
#endif
#if defined(ADC_CR_DEEPPWD)
ADC1->CR &= ~ADC_CR_DEEPPWD;
#endif
ADC1->CR |= ADC_CR_ADVREGEN;
for (wait = 0UL; wait < 10000UL; wait++) {
__NOP();
}
ADC1->CR |= ADC_CR_ADCAL;
if (wait_bits_clear(&ADC1->CR, ADC_CR_ADCAL, AD_BOARD_ADC_TIMEOUT) == 0U) {
g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
return;
}
ADC1->CFGR = 0UL;
ADC1->SMPR1 = 0x3FFFFFFFUL;
ADC1->SMPR2 = 0x3FFFFFFFUL;
ADC1->SQR1 = 0UL;
ADC1->ISR = ADC_ISR_ADRDY | ADC_ISR_EOC | ADC_ISR_EOS;
ADC1->CR |= ADC_CR_ADEN;
if (wait_bits_set(&ADC1->ISR, ADC_ISR_ADRDY, AD_BOARD_ADC_TIMEOUT) == 0U) {
g_adc_snapshot.faults |= AD_BOARD_ADC_FAULT_INIT;
return;
}
g_adc_snapshot.initialized = 1U;
board_adc_zero_calibrate();
#else
memset(&g_adc_snapshot, 0, sizeof(g_adc_snapshot));
g_adc_snapshot.phase_shunt_ohm = board_phase_shunt_resistance_ohm();
g_current_rms_filter_ready = 0U;
g_ia_rms_sq_A2 = 0.0f;
g_ib_rms_sq_A2 = 0.0f;
g_ic_rms_sq_A2 = 0.0f;
g_mcu_temp_filter_ready = 0U;
g_mcu_temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
g_torque_speed_ready = 0U;
g_torque_filter_ready = 0U;
g_torque_timestamp_us = 0UL;
g_torque_speed_rad_s = 0.0f;
g_torque_filtered_Nm = 0.0f;
#endif
}
static void board_adc_update_measurements(AD_Measurements_t *meas)
{
#if AD_BOARD_ADC_ENABLE != 0
if (g_adc_snapshot.initialized == 0U) {
meas->status_flags |= AD_MEAS_STATUS_DRIVER_FAULT;
return;
}
g_adc_snapshot.ia_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IA);
g_adc_snapshot.ib_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IB);
g_adc_snapshot.ic_raw = board_adc_read_channel(AD_BOARD_ADC_CH_IC);
g_adc_snapshot.vdc_raw_avg = board_adc_read_channel_average(AD_BOARD_ADC_CH_VDC,
AD_BOARD_ADC_VDC_AVG_SAMPLES,
&g_adc_snapshot.vdc_raw);
g_adc_snapshot.mcu_temp_raw = board_adc_read_channel(AD_BOARD_ADC_CH_MCU_TEMP);
g_adc_snapshot.ia_adc_V = adc_raw_to_voltage(g_adc_snapshot.ia_raw);
g_adc_snapshot.ib_adc_V = adc_raw_to_voltage(g_adc_snapshot.ib_raw);
g_adc_snapshot.ic_adc_V = adc_raw_to_voltage(g_adc_snapshot.ic_raw);
g_adc_snapshot.vdc_adc_avg_V = adc_voltage_minus_offset(adc_raw_to_voltage(g_adc_snapshot.vdc_raw_avg),
g_adc_snapshot.vdc_zero_adc_V);
g_adc_snapshot.vdc_adc_V = board_vdc_filter(g_adc_snapshot.vdc_adc_avg_V);
g_adc_snapshot.mcu_temp_adc_V = adc_raw_to_voltage(g_adc_snapshot.mcu_temp_raw);
g_adc_snapshot.mcu_temperature_C =
board_mcu_temperature_filter(adc_mcu_temperature_from_raw(g_adc_snapshot.mcu_temp_raw));
meas->ia_A = adc_current_from_voltage(g_adc_snapshot.ia_adc_V,
g_adc_snapshot.ia_zero_adc_V);
meas->ib_A = adc_current_from_voltage(g_adc_snapshot.ib_adc_V,
g_adc_snapshot.ib_zero_adc_V);
meas->ic_A = adc_current_from_voltage(g_adc_snapshot.ic_adc_V,
g_adc_snapshot.ic_zero_adc_V);
meas->vdc_V = adc_vdc_from_voltage(g_adc_snapshot.vdc_adc_V);
meas->temperature_C = g_adc_snapshot.mcu_temperature_C;
if (g_adc_snapshot.faults != 0UL) {
meas->status_flags |= AD_MEAS_STATUS_DRIVER_FAULT;
}
#else
(void)meas;
#endif
}
static void board_apply_command(uint8_t enable, uint8_t mode, float pwm_duty_limit)
{
AD_Command_t command;
memset(&command, 0, sizeof(command));
command.enable = enable;
command.test_mode = mode;
command.pwm_polarity_flags =
(uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
AD_INVERTER_PWM_POLARITY_MASK);
command.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
command.motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
command.rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
command.current_limit_A = AD_BOARD_DEFAULT_CURRENT_LIMIT_A;
command.voltage_limit_V = AD_BOARD_DEFAULT_OVERVOLTAGE_V;
command.undervoltage_limit_V = AD_BOARD_DEFAULT_UNDERVOLTAGE_V;
command.speed_limit_rpm = AD_BOARD_DEFAULT_SPEED_LIMIT_RPM;
command.temperature_limit_C = AD_BOARD_DEFAULT_TEMPERATURE_C;
command.pwm_duty_limit = (pwm_duty_limit > 0.0f) ?
pwm_duty_limit :
AD_BOARD_DEFAULT_PWM_DUTY_LIMIT;
command.rotation_frequency_Hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
command.rotation_modulation = ((mode == AD_PARAM_ID_MODE_ROTATION_3HZ) &&
(pwm_duty_limit > 0.0f)) ?
pwm_duty_limit :
AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
SimulinkInterface_SetCommand(&command);
}
static void board_set_led(uint8_t on)
{
#if AD_BOARD_LED_ENABLE != 0
GPIO_PinState state = (on != 0U) ? GPIO_PIN_SET : GPIO_PIN_RESET;
HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, state);
#else
(void)on;
#endif
}
static void board_toggle_led(void)
{
g_led_state = (g_led_state == 0U) ? 1U : 0U;
board_set_led(g_led_state);
}
static void board_update_led(uint32_t now_ms)
{
uint32_t status = AD_ParamID_GetStatus();
if ((status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
if ((now_ms - g_last_led_tick_ms) >= AD_BOARD_FAULT_BLINK_MS) {
g_last_led_tick_ms = now_ms;
board_toggle_led();
}
return;
}
if ((status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) {
g_led_state = 1U;
board_set_led(1U);
return;
}
if ((now_ms - g_last_led_tick_ms) >= AD_BOARD_IDLE_BLINK_MS) {
g_last_led_tick_ms = now_ms;
board_toggle_led();
}
}
void AD_Board_Init(void)
{
g_button_toggle_request = 0U;
g_logging_enabled = 0U;
g_led_state = 0U;
g_last_button_tick_ms = 0UL;
g_last_slow_tick_ms = HAL_GetTick();
g_last_led_tick_ms = HAL_GetTick();
g_phase_shunt_resistance_ohm = AD_BOARD_ADC_DEFAULT_PHASE_SHUNT_OHM;
board_set_led(0U);
board_adc_init();
AD_Inverter_Init();
SimulinkInterface_Init();
board_apply_command(0U, AD_PARAM_ID_MODE_IDLE, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}
void AD_Board_ReadMeasurements(AD_Measurements_t *meas)
{
if (meas == 0) {
return;
}
memset(meas, 0, sizeof(*meas));
meas->timestamp_us = HAL_GetTick() * 1000UL;
meas->temperature_C = AD_BOARD_ADC_DEFAULT_MCU_TEMP_C;
board_adc_update_measurements(meas);
board_update_current_rms(meas);
board_update_torque_estimate(meas);
board_update_slip_measurement(meas);
}
const AD_BoardAdcSnapshot_t* AD_Board_GetAdcSnapshot(void)
{
return &g_adc_snapshot;
}
void AD_Board_ResetAdcFaults(void)
{
board_adc_init();
}
void AD_Board_SetPhaseShuntResistanceOhm(float resistance_ohm)
{
g_phase_shunt_resistance_ohm = board_normalize_phase_shunt_resistance(resistance_ohm);
g_adc_snapshot.phase_shunt_ohm = g_phase_shunt_resistance_ohm;
g_current_rms_filter_ready = 0U;
g_ia_rms_sq_A2 = 0.0f;
g_ib_rms_sq_A2 = 0.0f;
g_ic_rms_sq_A2 = 0.0f;
}
float AD_Board_GetPhaseShuntResistanceOhm(void)
{
return board_phase_shunt_resistance_ohm();
}
void AD_Board_StartDataLogging(void)
{
g_logging_enabled = 1U;
board_apply_command(1U, AD_PARAM_ID_MODE_DATA_LOGGING, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}
void AD_Board_StartParamTest(uint8_t mode)
{
AD_Board_StartParamTestWithDuty(mode, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
}
void AD_Board_StartParamTestWithDuty(uint8_t mode, float pwm_duty_limit)
{
g_logging_enabled = 1U;
board_apply_command(1U, mode, pwm_duty_limit);
}
void AD_Board_StopParamTest(void)
{
g_logging_enabled = 0U;
board_apply_command(0U, AD_PARAM_ID_MODE_IDLE, AD_BOARD_DEFAULT_PWM_DUTY_LIMIT);
AD_Inverter_Disable();
AD_ParamID_Stop();
}
void AD_Board_ToggleDataLogging(void)
{
if (g_logging_enabled == 0U) {
AD_Board_StartDataLogging();
} else {
AD_Board_StopParamTest();
}
}
void AD_Board_Loop(void)
{
AD_Measurements_t measurements;
uint32_t now_ms = HAL_GetTick();
if (g_button_toggle_request != 0U) {
g_button_toggle_request = 0U;
AD_Board_ToggleDataLogging();
}
AD_Board_ReadMeasurements(&measurements);
SimulinkInterface_SetMeasurements(&measurements);
SimulinkInterface_StepFast();
if ((now_ms - g_last_slow_tick_ms) >= AD_BOARD_SLOW_PERIOD_MS) {
g_last_slow_tick_ms = now_ms;
SimulinkInterface_StepSlow();
}
board_update_led(now_ms);
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
uint32_t now_ms;
if (GPIO_Pin != B1_Pin) {
return;
}
now_ms = HAL_GetTick();
if ((now_ms - g_last_button_tick_ms) >= AD_BOARD_BUTTON_DEBOUNCE_MS) {
g_last_button_tick_ms = now_ms;
g_button_toggle_request = 1U;
}
}

259
AD_Keil_Project/Core/Src/ad_can_telemetry.c Normal file
View File

@@ -0,0 +1,259 @@
#include "ad_can_telemetry.h"
#include <string.h>
#include "ad_project_config.h"
#include "main.h"
#include "simulink_interface.h"
#ifndef AD_CAN_TELEMETRY_ENABLE
#define AD_CAN_TELEMETRY_ENABLE 1
#endif
#ifndef AD_CAN_TELEMETRY_STD_ID_BASE
#define AD_CAN_TELEMETRY_STD_ID_BASE 0x5A0UL
#endif
#ifndef AD_CAN_TELEMETRY_NOMINAL_PRESCALER
#define AD_CAN_TELEMETRY_NOMINAL_PRESCALER 17UL
#endif
#ifndef AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1
#define AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1 15UL
#endif
#ifndef AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2
#define AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2 4UL
#endif
#define AD_CAN_TELEMETRY_FRAME_BYTES 8U
#define AD_CAN_TELEMETRY_MAX_FRAMES ((SIMULINK_TELEMETRY_MAX_BYTES + AD_CAN_TELEMETRY_FRAME_BYTES - 1U) / AD_CAN_TELEMETRY_FRAME_BYTES)
FDCAN_HandleTypeDef hfdcan1;
static AD_CAN_TelemetryState_t g_can_state;
static uint8_t g_tx_packet[SIMULINK_TELEMETRY_MAX_BYTES];
static uint16_t g_tx_size;
static uint8_t g_tx_frame_index;
static uint8_t g_tx_frame_count;
static void update_status_flags(void)
{
uint32_t flags = 0UL;
if (g_can_state.initialized != 0U) {
flags |= AD_CAN_TELEMETRY_STATUS_INITIALIZED;
}
if (g_can_state.started != 0U) {
flags |= AD_CAN_TELEMETRY_STATUS_STARTED;
}
if (g_can_state.tx_busy != 0U) {
flags |= AD_CAN_TELEMETRY_STATUS_TX_BUSY;
}
if ((g_can_state.status_flags & AD_CAN_TELEMETRY_STATUS_TX_DROPPED) != 0UL) {
flags |= AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
}
if ((g_can_state.status_flags & AD_CAN_TELEMETRY_STATUS_ERROR) != 0UL) {
flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
}
g_can_state.status_flags = flags;
}
static uint32_t can_dlc_from_len(uint8_t len)
{
switch (len) {
case 0U:
return FDCAN_DLC_BYTES_0;
case 1U:
return FDCAN_DLC_BYTES_1;
case 2U:
return FDCAN_DLC_BYTES_2;
case 3U:
return FDCAN_DLC_BYTES_3;
case 4U:
return FDCAN_DLC_BYTES_4;
case 5U:
return FDCAN_DLC_BYTES_5;
case 6U:
return FDCAN_DLC_BYTES_6;
case 7U:
return FDCAN_DLC_BYTES_7;
default:
return FDCAN_DLC_BYTES_8;
}
}
void AD_CAN_Telemetry_Init(void)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
memset(&g_can_state, 0, sizeof(g_can_state));
memset(g_tx_packet, 0, sizeof(g_tx_packet));
g_tx_size = 0U;
g_tx_frame_index = 0U;
g_tx_frame_count = 0U;
g_can_state.base_id = AD_CAN_TELEMETRY_STD_ID_BASE;
hfdcan1.Instance = FDCAN1;
hfdcan1.Init.ClockDivider = FDCAN_CLOCK_DIV1;
hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan1.Init.AutoRetransmission = ENABLE;
hfdcan1.Init.TransmitPause = DISABLE;
hfdcan1.Init.ProtocolException = DISABLE;
hfdcan1.Init.NominalPrescaler = AD_CAN_TELEMETRY_NOMINAL_PRESCALER;
hfdcan1.Init.NominalSyncJumpWidth = 1U;
hfdcan1.Init.NominalTimeSeg1 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1;
hfdcan1.Init.NominalTimeSeg2 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2;
hfdcan1.Init.DataPrescaler = AD_CAN_TELEMETRY_NOMINAL_PRESCALER;
hfdcan1.Init.DataSyncJumpWidth = 1U;
hfdcan1.Init.DataTimeSeg1 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG1;
hfdcan1.Init.DataTimeSeg2 = AD_CAN_TELEMETRY_NOMINAL_TIME_SEG2;
hfdcan1.Init.StdFiltersNbr = 0U;
hfdcan1.Init.ExtFiltersNbr = 0U;
hfdcan1.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
if (HAL_FDCAN_Init(&hfdcan1) != HAL_OK) {
g_can_state.error_count++;
g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
update_status_flags();
return;
}
g_can_state.initialized = 1U;
if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1,
FDCAN_REJECT,
FDCAN_REJECT,
FDCAN_REJECT_REMOTE,
FDCAN_REJECT_REMOTE) != HAL_OK) {
g_can_state.error_count++;
g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
}
if (HAL_FDCAN_Start(&hfdcan1) == HAL_OK) {
g_can_state.started = 1U;
} else {
g_can_state.error_count++;
g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
}
update_status_flags();
#else
memset(&g_can_state, 0, sizeof(g_can_state));
#endif
}
uint8_t AD_CAN_Telemetry_QueuePacket(const uint8_t *data, uint16_t size)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
if ((data == 0) ||
(size == 0U) ||
(size > SIMULINK_TELEMETRY_MAX_BYTES) ||
(g_can_state.started == 0U) ||
(g_can_state.tx_busy != 0U)) {
g_can_state.dropped_packets++;
g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
update_status_flags();
return 0U;
}
memcpy(g_tx_packet, data, size);
g_tx_size = size;
g_tx_frame_index = 0U;
g_tx_frame_count = (uint8_t)((size + AD_CAN_TELEMETRY_FRAME_BYTES - 1U) / AD_CAN_TELEMETRY_FRAME_BYTES);
if (g_tx_frame_count > AD_CAN_TELEMETRY_MAX_FRAMES) {
g_tx_frame_count = AD_CAN_TELEMETRY_MAX_FRAMES;
}
g_can_state.tx_busy = 1U;
g_can_state.last_size_bytes = size;
g_can_state.status_flags &= ~AD_CAN_TELEMETRY_STATUS_TX_DROPPED;
update_status_flags();
AD_CAN_Telemetry_Loop();
return 1U;
#else
(void)data;
(void)size;
return 0U;
#endif
}
void AD_CAN_Telemetry_Loop(void)
{
#if AD_CAN_TELEMETRY_ENABLE != 0
FDCAN_TxHeaderTypeDef header;
uint8_t frame[AD_CAN_TELEMETRY_FRAME_BYTES];
uint16_t offset;
uint16_t remaining;
uint8_t len;
if ((g_can_state.started == 0U) || (g_can_state.tx_busy == 0U)) {
update_status_flags();
return;
}
while ((g_tx_frame_index < g_tx_frame_count) &&
(HAL_FDCAN_GetTxFifoFreeLevel(&hfdcan1) > 0UL)) {
offset = (uint16_t)g_tx_frame_index * AD_CAN_TELEMETRY_FRAME_BYTES;
remaining = (uint16_t)(g_tx_size - offset);
len = (remaining > AD_CAN_TELEMETRY_FRAME_BYTES) ?
AD_CAN_TELEMETRY_FRAME_BYTES :
(uint8_t)remaining;
memset(frame, 0, sizeof(frame));
memcpy(frame, &g_tx_packet[offset], len);
memset(&header, 0, sizeof(header));
header.Identifier = (uint32_t)(AD_CAN_TELEMETRY_STD_ID_BASE + g_tx_frame_index);
header.IdType = FDCAN_STANDARD_ID;
header.TxFrameType = FDCAN_DATA_FRAME;
header.DataLength = can_dlc_from_len(len);
header.ErrorStateIndicator = FDCAN_ESI_ACTIVE;
header.BitRateSwitch = FDCAN_BRS_OFF;
header.FDFormat = FDCAN_CLASSIC_CAN;
header.TxEventFifoControl = FDCAN_NO_TX_EVENTS;
header.MessageMarker = g_tx_frame_index;
if (HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1, &header, frame) != HAL_OK) {
g_can_state.error_count++;
g_can_state.last_error = HAL_FDCAN_GetError(&hfdcan1);
g_can_state.status_flags |= AD_CAN_TELEMETRY_STATUS_ERROR;
update_status_flags();
return;
}
g_tx_frame_index++;
g_can_state.tx_frames++;
}
if (g_tx_frame_index >= g_tx_frame_count) {
g_can_state.tx_busy = 0U;
g_can_state.tx_packets++;
g_tx_size = 0U;
g_tx_frame_index = 0U;
g_tx_frame_count = 0U;
}
update_status_flags();
#endif
}
uint8_t AD_CAN_Telemetry_IsStarted(void)
{
return g_can_state.started;
}
uint8_t AD_CAN_Telemetry_IsTxBusy(void)
{
return g_can_state.tx_busy;
}
const AD_CAN_TelemetryState_t* AD_CAN_Telemetry_GetState(void)
{
update_status_flags();
return &g_can_state;
}

4
AD_Keil_Project/Core/Src/ad_debug.c Normal file
View File

@@ -0,0 +1,4 @@
#include "ad_debug.h"
/* Implementation is in ad_project.c so the project still links if Keil has
not reloaded this optional file in its source list yet. */

1704
AD_Keil_Project/Core/Src/ad_inverter.c Normal file
View File

@@ -0,0 +1,1704 @@
#include "ad_inverter.h"
#include "ad_parameter_identification.h"
#include "main.h"
extern TIM_HandleTypeDef htim1;
#ifndef AD_ID_DUTY_MAX
#define AD_ID_DUTY_MAX (0.20f)
#endif
#ifndef AD_ID_DUTY_MIN
#define AD_ID_DUTY_MIN (0.005f)
#endif
#ifndef AD_INVERTER_PWM_PERIOD_UP_TICKS
#define AD_INVERTER_PWM_PERIOD_UP_TICKS (8499UL)
#endif
#ifndef AD_INVERTER_PWM_PERIOD_CENTER_TICKS
#define AD_INVERTER_PWM_PERIOD_CENTER_TICKS (4249UL)
#endif
#define AD_ID_PI (3.14159265358979323846f)
#define AD_ID_TWO_PI (6.28318530717958647692f)
#define AD_ID_TWO_PI_OVER_THREE (2.09439510239319549231f)
#define AD_ID_INV_SQRT2 (0.70710678118654752440f)
#define AD_ID_RS_SETTLE_US (200000UL)
#define AD_ID_RS_MEASURE_US (800000UL)
#define AD_ID_LS_SETTLE_US (20000UL)
#define AD_ID_LS_PULSE_US (50000UL)
#define AD_ID_LOCKED_RR_SETTLE_US (300000UL)
#define AD_ID_LOCKED_RR_MEASURE_US (1000000UL)
#define AD_ID_MAG_SETTLE_US (1000000UL)
#define AD_ID_MAG_MEASURE_US (1500000UL)
#define AD_ID_INERTIA_ACCEL_US (2000000UL)
#define AD_ID_INERTIA_COAST_US (2500000UL)
#define AD_ID_MIN_CURRENT_A (0.05f)
#define AD_ID_MIN_AC_CURRENT_A (0.05f)
#define AD_ID_MIN_DELTA_CURRENT_A (0.05f)
#define AD_ID_MIN_IMPEDANCE_OHM (0.001f)
#define AD_ID_MIN_SPEED_RPM (20.0f)
#define AD_ID_MIN_SPEED_DELTA_RPM (10.0f)
#define AD_ID_MIN_ACCEL_RAD_S2 (0.1f)
#define AD_ID_LOCKED_RR_FREQ_HZ (5.0f)
#define AD_ID_NO_LOAD_FREQ_MAX_HZ (20.0f)
#define AD_ID_INERTIA_FREQ_MAX_HZ (20.0f)
#ifndef AD_ID_ROTATION_TEST_START_FREQ_HZ
#define AD_ID_ROTATION_TEST_START_FREQ_HZ (0.5f)
#endif
#ifndef AD_ID_ROTATION_TEST_ALIGN_US
#define AD_ID_ROTATION_TEST_ALIGN_US (500000UL)
#endif
#define AD_ID_DEFAULT_NOMINAL_FREQ_HZ (50.0f)
#define AD_ID_ROTATION_MIN_UPDATE_DT_S (0.000001f)
#define AD_ID_ROTATION_MAX_UPDATE_DT_S (0.010000f)
#define AD_ID_RESULT_RUNNING (0U)
#define AD_ID_RESULT_COMPLETE (1U)
#define AD_ID_RESULT_FAILED (2U)
#define AD_ID_STAGE_IDLE (0U)
#define AD_ID_STAGE_SETTLE (1U)
#define AD_ID_STAGE_MEASURE (2U)
#define AD_ID_STAGE_PULSE (3U)
#define AD_ID_STAGE_ACCEL (4U)
#define AD_ID_STAGE_COAST (5U)
#define AD_ID_KEY_AUTO_RS (100U)
#define AD_ID_KEY_AUTO_LS (101U)
#define AD_ID_KEY_AUTO_RR (102U)
#define AD_ID_KEY_AUTO_LM (103U)
#define AD_ID_KEY_AUTO_JB (104U)
#define AD_ID_KEY_LOCKED_LS (110U)
#define AD_ID_KEY_LOCKED_RR (111U)
static AD_InverterState_t g_inverter_state;
typedef struct
{
uint8_t key;
uint8_t stage;
uint8_t auto_stage;
uint32_t stage_start_us;
uint32_t sample_count;
float current_sum_A;
float current_sq_sum_A2;
float vdc_sum_V;
float current_start_A;
float vdc_start_V;
float angle_rad;
uint32_t pwm_timestamp_us;
float speed_start_rpm;
float speed_end_rpm;
float accel_torque_Nm;
float accel_alpha_rad_s2;
} AD_IdRuntime_t;
static AD_IdRuntime_t g_id_runtime;
typedef struct
{
uint8_t active;
uint8_t motor_control_type;
float target_duty;
float target_frequency_hz;
float current_duty;
float current_frequency_hz;
float duty_slew_per_s;
float frequency_slew_hz_per_s;
float angle_rad;
float update_dt_s;
uint16_t target_ramp_time_ms;
} AD_RotationPwmRuntime_t;
static volatile AD_RotationPwmRuntime_t g_rotation_pwm;
static void rotation_pwm_stop(void)
{
g_rotation_pwm.active = 0U;
g_inverter_state.electrical_frequency_Hz = 0.0f;
g_inverter_state.electrical_angle_rad = 0.0f;
}
static float slew_towards(float current, float target, float max_delta)
{
if (max_delta <= 0.0f) {
return target;
}
if (current < target) {
current += max_delta;
if (current > target) {
current = target;
}
} else if (current > target) {
current -= max_delta;
if (current < target) {
current = target;
}
}
return current;
}
static uint32_t inverter_get_tim1_clock_hz(void)
{
uint32_t clock_hz = HAL_RCC_GetPCLK2Freq();
#if defined(RCC_CFGR_PPRE2)
if ((RCC->CFGR & RCC_CFGR_PPRE2) != 0UL) {
clock_hz *= 2UL;
}
#endif
return (clock_hz != 0UL) ? clock_hz : SystemCoreClock;
}
static float inverter_get_pwm_update_dt_s(void)
{
uint32_t timer_clock_hz = inverter_get_tim1_clock_hz();
uint32_t prescaler = htim1.Instance->PSC + 1UL;
uint32_t period_ticks = __HAL_TIM_GET_AUTORELOAD(&htim1) + 1UL;
float dt_s;
if ((timer_clock_hz == 0UL) || (period_ticks == 0UL)) {
return AD_ID_ROTATION_MAX_UPDATE_DT_S;
}
dt_s = ((float)period_ticks * (float)prescaler) / (float)timer_clock_hz;
if (dt_s < AD_ID_ROTATION_MIN_UPDATE_DT_S) {
dt_s = AD_ID_ROTATION_MIN_UPDATE_DT_S;
} else if (dt_s > AD_ID_ROTATION_MAX_UPDATE_DT_S) {
dt_s = AD_ID_ROTATION_MAX_UPDATE_DT_S;
}
return dt_s;
}
static void inverter_enable_update_irq(void)
{
__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
}
static void inverter_disable_update_irq(void)
{
__HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_UPDATE);
}
static void inverter_clear_break_flags(void)
{
#if defined(TIM_FLAG_BREAK)
__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_BREAK);
#endif
#if defined(TIM_FLAG_BREAK2)
__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_BREAK2);
#endif
#if defined(TIM_FLAG_SYSTEM_BREAK)
__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_SYSTEM_BREAK);
#endif
}
static void inverter_write_pwm_polarity(uint16_t flags)
{
uint32_t main_mask = TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC3P;
uint32_t comp_mask = TIM_CCER_CC1NP | TIM_CCER_CC2NP | TIM_CCER_CC3NP;
uint32_t ccer = htim1.Instance->CCER;
ccer &= ~(main_mask | comp_mask);
if ((flags & AD_INVERTER_PWM_POLARITY_MAIN_INVERTED) != 0U) {
ccer |= main_mask;
}
if ((flags & AD_INVERTER_PWM_POLARITY_COMP_INVERTED) != 0U) {
ccer |= comp_mask;
}
htim1.Instance->CCER = ccer;
}
static void inverter_write_pwm_timing(uint16_t mode)
{
uint32_t cr1 = htim1.Instance->CR1;
uint32_t period;
cr1 &= ~(TIM_CR1_CMS | TIM_CR1_DIR);
if (mode == (uint16_t)AD_INVERTER_PWM_TIMING_CENTER) {
cr1 |= TIM_COUNTERMODE_CENTERALIGNED1;
period = AD_INVERTER_PWM_PERIOD_CENTER_TICKS;
} else {
period = AD_INVERTER_PWM_PERIOD_UP_TICKS;
}
htim1.Instance->CR1 = cr1;
__HAL_TIM_SET_AUTORELOAD(&htim1, period);
__HAL_TIM_SET_COUNTER(&htim1, 0UL);
}
static float clamp01(float value)
{
if (value < 0.0f) {
return 0.0f;
}
if (value > 1.0f) {
return 1.0f;
}
return value;
}
static uint32_t duty_to_compare(float duty)
{
uint32_t period = __HAL_TIM_GET_AUTORELOAD(&htim1);
float compare_f = clamp01(duty) * (float)period;
if (compare_f < 0.0f) {
return 0UL;
}
if (compare_f > (float)period) {
return period;
}
return (uint32_t)(compare_f + 0.5f);
}
static uint32_t duty_to_compare_inverted(float duty)
{
return duty_to_compare(1.0f - clamp01(duty));
}
static float absf_local(float value)
{
return (value < 0.0f) ? -value : value;
}
static float sqrtf_local(float value)
{
float x;
uint8_t i;
if (value <= 0.0f) {
return 0.0f;
}
x = (value > 1.0f) ? value : 1.0f;
for (i = 0U; i < 8U; i++) {
x = 0.5f * (x + (value / x));
}
return x;
}
static float wrap_angle(float angle_rad)
{
while (angle_rad >= AD_ID_TWO_PI) {
angle_rad -= AD_ID_TWO_PI;
}
while (angle_rad < 0.0f) {
angle_rad += AD_ID_TWO_PI;
}
return angle_rad;
}
static float cos_poly(float angle_rad)
{
float x2 = angle_rad * angle_rad;
float x4 = x2 * x2;
float x6 = x4 * x2;
return 1.0f - (0.5f * x2) + (0.0416666667f * x4) - (0.0013888889f * x6);
}
static float cos_approx(float angle_rad)
{
float x = wrap_angle(angle_rad);
float value;
if (x > AD_ID_PI) {
x -= AD_ID_TWO_PI;
}
if (x > (0.5f * AD_ID_PI)) {
value = -cos_poly(AD_ID_PI - x);
} else if (x < (-0.5f * AD_ID_PI)) {
value = -cos_poly(AD_ID_PI + x);
} else {
value = cos_poly(x);
}
if (value > 1.0f) {
return 1.0f;
}
if (value < -1.0f) {
return -1.0f;
}
return value;
}
static float rpm_to_rad_s(float rpm)
{
return rpm * (AD_ID_TWO_PI / 60.0f);
}
static float id_get_test_duty(void)
{
float duty = AD_ParamID_GetPwmDutyLimit();
g_inverter_state.requested_test_duty = duty;
if (duty < AD_ID_DUTY_MIN) {
duty = AD_ID_DUTY_MIN;
} else if (duty > AD_ID_DUTY_MAX) {
duty = AD_ID_DUTY_MAX;
}
g_inverter_state.applied_test_duty = duty;
return duty;
}
static float id_get_rotation_test_duty(void)
{
float duty = AD_ParamID_GetRotationModulation();
g_inverter_state.requested_test_duty = duty;
g_inverter_state.applied_test_duty = duty;
return duty;
}
static void rotation_pwm_apply_six_step(float duty, uint8_t sector)
{
float d = clamp01(duty);
float high = 0.5f + d;
float low = 0.5f - d;
float mid = 0.5f;
switch (sector % 6U) {
case 0U:
AD_Inverter_SetDuty(high, low, mid);
break;
case 1U:
AD_Inverter_SetDuty(high, mid, low);
break;
case 2U:
AD_Inverter_SetDuty(mid, high, low);
break;
case 3U:
AD_Inverter_SetDuty(low, high, mid);
break;
case 4U:
AD_Inverter_SetDuty(low, mid, high);
break;
default:
AD_Inverter_SetDuty(mid, low, high);
break;
}
}
static void rotation_pwm_service_update(void)
{
float duty;
float frequency_hz;
float angle_rad;
float dt_s;
uint8_t sector;
if (g_rotation_pwm.active == 0U) {
return;
}
if ((g_inverter_state.pwm_running == 0U) ||
(g_inverter_state.service_pwm_running != 0U)) {
rotation_pwm_stop();
return;
}
dt_s = g_rotation_pwm.update_dt_s;
if (dt_s < AD_ID_ROTATION_MIN_UPDATE_DT_S) {
dt_s = AD_ID_ROTATION_MIN_UPDATE_DT_S;
} else if (dt_s > AD_ID_ROTATION_MAX_UPDATE_DT_S) {
dt_s = AD_ID_ROTATION_MAX_UPDATE_DT_S;
}
duty = slew_towards(g_rotation_pwm.current_duty,
g_rotation_pwm.target_duty,
g_rotation_pwm.duty_slew_per_s * dt_s);
frequency_hz = slew_towards(g_rotation_pwm.current_frequency_hz,
g_rotation_pwm.target_frequency_hz,
g_rotation_pwm.frequency_slew_hz_per_s * dt_s);
if (frequency_hz < 0.1f) {
frequency_hz = 0.1f;
}
angle_rad = wrap_angle(g_rotation_pwm.angle_rad +
(AD_ID_TWO_PI * frequency_hz * dt_s));
g_rotation_pwm.current_duty = duty;
g_rotation_pwm.current_frequency_hz = frequency_hz;
g_rotation_pwm.angle_rad = angle_rad;
g_inverter_state.applied_test_duty = duty;
g_inverter_state.electrical_frequency_Hz = frequency_hz;
g_inverter_state.electrical_angle_rad = angle_rad;
if (g_rotation_pwm.motor_control_type ==
(uint8_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) {
sector = (uint8_t)(angle_rad * (6.0f / AD_ID_TWO_PI));
if (sector > 5U) {
sector = 5U;
}
rotation_pwm_apply_six_step(duty, sector);
return;
}
AD_Inverter_SetDuty(0.5f + (duty * cos_approx(angle_rad)),
0.5f + (duty * cos_approx(angle_rad -
AD_ID_TWO_PI_OVER_THREE)),
0.5f + (duty * cos_approx(angle_rad +
AD_ID_TWO_PI_OVER_THREE)));
}
static uint8_t id_configure_rotation_pwm(float duty,
float frequency_hz,
uint16_t motor_control_type)
{
uint32_t primask;
float start_frequency_hz = AD_ID_ROTATION_TEST_START_FREQ_HZ;
float update_dt_s = inverter_get_pwm_update_dt_s();
uint16_t ramp_time_ms = AD_ParamID_GetRotationRampTimeMs();
float ramp_time_s = (float)ramp_time_ms * 0.001f;
uint8_t reset_runtime;
uint8_t target_changed;
duty = clamp01(duty);
if (frequency_hz < 0.1f) {
frequency_hz = 0.1f;
}
if (start_frequency_hz > frequency_hz) {
start_frequency_hz = frequency_hz;
}
if (AD_Inverter_Enable() == 0U) {
rotation_pwm_stop();
return 0U;
}
primask = __get_PRIMASK();
__disable_irq();
reset_runtime = (uint8_t)((g_rotation_pwm.active == 0U) ||
(g_rotation_pwm.motor_control_type !=
(uint8_t)motor_control_type));
target_changed = (uint8_t)((reset_runtime != 0U) ||
(g_rotation_pwm.target_duty != duty) ||
(g_rotation_pwm.target_frequency_hz != frequency_hz) ||
(g_rotation_pwm.target_ramp_time_ms != ramp_time_ms));
if (reset_runtime != 0U) {
g_rotation_pwm.current_duty =
(motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) ?
duty :
0.0f;
g_rotation_pwm.current_frequency_hz = start_frequency_hz;
g_rotation_pwm.angle_rad = 0.0f;
}
if (ramp_time_s <= 0.0f) {
ramp_time_s = 0.001f;
}
if (target_changed != 0U) {
g_rotation_pwm.duty_slew_per_s =
absf_local(duty - g_rotation_pwm.current_duty) / ramp_time_s;
g_rotation_pwm.frequency_slew_hz_per_s =
absf_local(frequency_hz - g_rotation_pwm.current_frequency_hz) / ramp_time_s;
}
g_rotation_pwm.target_duty = duty;
g_rotation_pwm.target_frequency_hz = frequency_hz;
g_rotation_pwm.motor_control_type = (uint8_t)motor_control_type;
g_rotation_pwm.update_dt_s = update_dt_s;
g_rotation_pwm.target_ramp_time_ms = ramp_time_ms;
g_rotation_pwm.active = 1U;
g_inverter_state.electrical_frequency_Hz = g_rotation_pwm.current_frequency_hz;
g_inverter_state.electrical_angle_rad = g_rotation_pwm.angle_rad;
if (primask == 0UL) {
__enable_irq();
}
return 1U;
}
static float id_get_nominal_frequency_hz(void)
{
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
if ((params != 0) && (params->nominal_frequency_Hz > 1.0f)) {
return params->nominal_frequency_Hz;
}
return AD_ID_DEFAULT_NOMINAL_FREQ_HZ;
}
static float id_get_no_load_frequency_hz(void)
{
float frequency_hz = id_get_nominal_frequency_hz();
if (frequency_hz > AD_ID_NO_LOAD_FREQ_MAX_HZ) {
frequency_hz = AD_ID_NO_LOAD_FREQ_MAX_HZ;
}
if (frequency_hz < 1.0f) {
frequency_hz = 1.0f;
}
return frequency_hz;
}
static float id_get_inertia_frequency_hz(void)
{
float frequency_hz = id_get_nominal_frequency_hz() * 0.4f;
if (frequency_hz > AD_ID_INERTIA_FREQ_MAX_HZ) {
frequency_hz = AD_ID_INERTIA_FREQ_MAX_HZ;
}
if (frequency_hz < 1.0f) {
frequency_hz = 1.0f;
}
return frequency_hz;
}
static float id_phase_a_current(const AD_Measurements_t *meas)
{
return absf_local(meas->ia_A - (0.5f * (meas->ib_A + meas->ic_A)));
}
static float id_phase_current_sq(const AD_Measurements_t *meas)
{
return ((meas->ia_A * meas->ia_A) +
(meas->ib_A * meas->ib_A) +
(meas->ic_A * meas->ic_A)) / 3.0f;
}
static float id_phase_voltage_rms(float duty, float vdc_V)
{
return absf_local(duty) * vdc_V * AD_ID_INV_SQRT2;
}
static void id_accumulate_ac_sample(const AD_Measurements_t *meas)
{
g_id_runtime.current_sq_sum_A2 += id_phase_current_sq(meas);
g_id_runtime.vdc_sum_V += meas->vdc_V;
g_id_runtime.sample_count++;
}
static float id_average_ac_current_rms(void)
{
if (g_id_runtime.sample_count == 0UL) {
return 0.0f;
}
return sqrtf_local(g_id_runtime.current_sq_sum_A2 / (float)g_id_runtime.sample_count);
}
static float id_average_vdc(void)
{
if (g_id_runtime.sample_count == 0UL) {
return 0.0f;
}
return g_id_runtime.vdc_sum_V / (float)g_id_runtime.sample_count;
}
static uint32_t elapsed_us(uint32_t now_us, uint32_t start_us)
{
return now_us - start_us;
}
static void id_runtime_reset(uint8_t key, uint8_t stage, uint32_t now_us)
{
rotation_pwm_stop();
g_id_runtime.key = key;
g_id_runtime.stage = stage;
g_id_runtime.stage_start_us = now_us;
g_id_runtime.sample_count = 0UL;
g_id_runtime.current_sum_A = 0.0f;
g_id_runtime.current_sq_sum_A2 = 0.0f;
g_id_runtime.vdc_sum_V = 0.0f;
g_id_runtime.current_start_A = 0.0f;
g_id_runtime.vdc_start_V = 0.0f;
g_id_runtime.angle_rad = 0.0f;
g_id_runtime.pwm_timestamp_us = now_us;
g_id_runtime.speed_start_rpm = 0.0f;
g_id_runtime.speed_end_rpm = 0.0f;
g_id_runtime.accel_torque_Nm = 0.0f;
g_id_runtime.accel_alpha_rad_s2 = 0.0f;
g_inverter_state.id_stage = stage;
}
static uint8_t id_apply_dc_vector(float duty)
{
AD_InverterCommand_t command;
float d = clamp01(duty);
g_inverter_state.applied_test_duty = d;
g_inverter_state.electrical_frequency_Hz = 0.0f;
g_inverter_state.electrical_angle_rad = 0.0f;
command.enable = 1U;
command.duty_a = 0.5f + d;
command.duty_b = 0.5f - d;
command.duty_c = 0.5f - d;
return AD_Inverter_ApplyCommand(&command);
}
static uint8_t id_apply_rotating_vector(float duty,
float frequency_hz,
const AD_Measurements_t *meas)
{
AD_InverterCommand_t command;
float d = clamp01(duty);
float dt_s;
uint32_t dt_us;
if (frequency_hz < 0.1f) {
frequency_hz = 0.1f;
}
dt_us = elapsed_us(meas->timestamp_us, g_id_runtime.pwm_timestamp_us);
dt_s = (float)dt_us * 0.000001f;
if (dt_s > 0.1f) {
dt_s = 0.1f;
}
g_id_runtime.angle_rad = wrap_angle(g_id_runtime.angle_rad +
(AD_ID_TWO_PI * frequency_hz * dt_s));
g_id_runtime.pwm_timestamp_us = meas->timestamp_us;
g_inverter_state.applied_test_duty = d;
g_inverter_state.electrical_frequency_Hz = frequency_hz;
g_inverter_state.electrical_angle_rad = g_id_runtime.angle_rad;
command.enable = 1U;
command.duty_a = 0.5f + (d * cos_approx(g_id_runtime.angle_rad));
command.duty_b = 0.5f + (d * cos_approx(g_id_runtime.angle_rad -
AD_ID_TWO_PI_OVER_THREE));
command.duty_c = 0.5f + (d * cos_approx(g_id_runtime.angle_rad +
AD_ID_TWO_PI_OVER_THREE));
return AD_Inverter_ApplyCommand(&command);
}
static uint8_t id_apply_six_step_sector(float duty, uint8_t sector)
{
AD_InverterCommand_t command;
float d = clamp01(duty);
float high = 0.5f + d;
float low = 0.5f - d;
float mid = 0.5f;
command.enable = 1U;
g_inverter_state.electrical_frequency_Hz = 0.0f;
g_inverter_state.electrical_angle_rad = 0.0f;
switch (sector % 6U) {
case 0U:
command.duty_a = high;
command.duty_b = low;
command.duty_c = mid;
break;
case 1U:
command.duty_a = high;
command.duty_b = mid;
command.duty_c = low;
break;
case 2U:
command.duty_a = mid;
command.duty_b = high;
command.duty_c = low;
break;
case 3U:
command.duty_a = low;
command.duty_b = high;
command.duty_c = mid;
break;
case 4U:
command.duty_a = low;
command.duty_b = mid;
command.duty_c = high;
break;
default:
command.duty_a = mid;
command.duty_b = low;
command.duty_c = high;
break;
}
return AD_Inverter_ApplyCommand(&command);
}
static void id_store_rs(float rs_ohm)
{
AD_MotorParameters_t params = *AD_ParamID_GetParameters();
params.Rs_ohm = rs_ohm;
params.valid_mask |= AD_MOTOR_PARAM_VALID_RS;
AD_ParamID_SetParameters(&params);
}
static void id_update_inductance_model(AD_MotorParameters_t *params)
{
float lls_H;
float llr_H;
if (params == 0) {
return;
}
if (((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
((params->valid_mask & AD_MOTOR_PARAM_VALID_LM) != 0UL) &&
(params->Ll_H > 0.0f) &&
(params->Lm_H > 0.0f)) {
lls_H = 0.5f * params->Ll_H;
llr_H = 0.5f * params->Ll_H;
params->Ls_H = params->Lm_H + lls_H;
params->Lr_H = params->Lm_H + llr_H;
params->valid_mask |= (AD_MOTOR_PARAM_VALID_LS | AD_MOTOR_PARAM_VALID_LR);
} else if (((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
(params->Ll_H > 0.0f)) {
params->Ls_H = params->Ll_H;
params->Lr_H = params->Ll_H;
params->valid_mask |= (AD_MOTOR_PARAM_VALID_LS | AD_MOTOR_PARAM_VALID_LR);
}
}
static void id_store_ls(float ls_H)
{
AD_MotorParameters_t params = *AD_ParamID_GetParameters();
params.Ll_H = ls_H;
params.valid_mask |= AD_MOTOR_PARAM_VALID_LL;
id_update_inductance_model(&params);
AD_ParamID_SetParameters(&params);
}
static void id_store_rr(float rr_ohm)
{
AD_MotorParameters_t params = *AD_ParamID_GetParameters();
params.Rr_ohm = rr_ohm;
params.valid_mask |= AD_MOTOR_PARAM_VALID_RR;
AD_ParamID_SetParameters(&params);
}
static void id_store_lm(float lm_H)
{
AD_MotorParameters_t params = *AD_ParamID_GetParameters();
params.Lm_H = lm_H;
params.valid_mask |= AD_MOTOR_PARAM_VALID_LM;
id_update_inductance_model(&params);
AD_ParamID_SetParameters(&params);
}
static void id_store_inertia_friction(float j_kg_m2, float b_Nm_s)
{
AD_MotorParameters_t params = *AD_ParamID_GetParameters();
params.J_kg_m2 = j_kg_m2;
params.B_Nm_s = b_Nm_s;
params.valid_mask |= (AD_MOTOR_PARAM_VALID_J | AD_MOTOR_PARAM_VALID_B);
AD_ParamID_SetParameters(&params);
}
static uint8_t id_has_valid_parameters(void)
{
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
return (uint8_t)(((params != 0) && (params->valid_mask != 0UL)) ? 1U : 0U);
}
static uint8_t id_finish_auto_partial(uint32_t now_us)
{
if (id_has_valid_parameters() == 0U) {
return AD_ID_RESULT_FAILED;
}
AD_ParamID_MarkPartialComplete();
AD_ParamID_MarkStepFailed();
id_runtime_reset(AD_PARAM_ID_MODE_AUTO_IDENTIFICATION, AD_ID_STAGE_IDLE, now_us);
g_id_runtime.auto_stage = 0U;
return AD_ID_RESULT_COMPLETE;
}
static void set_service_debug_duty(uint8_t output, float duty)
{
g_inverter_state.duty_a = 0.0f;
g_inverter_state.duty_b = 0.0f;
g_inverter_state.duty_c = 0.0f;
g_inverter_state.electrical_frequency_Hz = 0.0f;
g_inverter_state.electrical_angle_rad = 0.0f;
switch (output) {
case AD_INVERTER_PWM_OUTPUT_UH:
case AD_INVERTER_PWM_OUTPUT_UL:
g_inverter_state.duty_a = duty;
break;
case AD_INVERTER_PWM_OUTPUT_VH:
case AD_INVERTER_PWM_OUTPUT_VL:
g_inverter_state.duty_b = duty;
break;
case AD_INVERTER_PWM_OUTPUT_WH:
case AD_INVERTER_PWM_OUTPUT_WL:
g_inverter_state.duty_c = duty;
break;
case AD_INVERTER_PWM_OUTPUT_ALL:
g_inverter_state.duty_a = 0.5f;
g_inverter_state.duty_b = 0.5f;
g_inverter_state.duty_c = 0.5f;
break;
default:
break;
}
}
static uint8_t id_step_rs(uint8_t key, const AD_Measurements_t *meas, float duty)
{
float i_avg_A;
float vdc_avg_V;
float v_phase_V;
float rs_ohm;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
}
if (id_apply_dc_vector(duty) == 0U) {
return AD_ID_RESULT_FAILED;
}
if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >= AD_ID_RS_SETTLE_US) {
id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
}
return AD_ID_RESULT_RUNNING;
}
g_id_runtime.current_sum_A += id_phase_a_current(meas);
g_id_runtime.vdc_sum_V += meas->vdc_V;
g_id_runtime.sample_count++;
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) < AD_ID_RS_MEASURE_US) {
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
if (g_id_runtime.sample_count == 0UL) {
return AD_ID_RESULT_FAILED;
}
i_avg_A = g_id_runtime.current_sum_A / (float)g_id_runtime.sample_count;
vdc_avg_V = g_id_runtime.vdc_sum_V / (float)g_id_runtime.sample_count;
if (i_avg_A < AD_ID_MIN_CURRENT_A) {
return AD_ID_RESULT_FAILED;
}
v_phase_V = (4.0f / 3.0f) * duty * vdc_avg_V;
rs_ohm = v_phase_V / i_avg_A;
id_store_rs(rs_ohm);
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
return AD_ID_RESULT_COMPLETE;
}
static uint8_t id_step_ls(uint8_t key, const AD_Measurements_t *meas, float duty)
{
float i_now_A;
float delta_i_A;
float v_phase_V;
float dt_s;
float ls_H;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
g_id_runtime.current_start_A = id_phase_a_current(meas);
g_id_runtime.vdc_start_V = meas->vdc_V;
}
if (id_apply_dc_vector(duty) == 0U) {
return AD_ID_RESULT_FAILED;
}
if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >= AD_ID_LS_SETTLE_US) {
id_runtime_reset(key, AD_ID_STAGE_PULSE, meas->timestamp_us);
g_id_runtime.current_start_A = id_phase_a_current(meas);
g_id_runtime.vdc_start_V = meas->vdc_V;
}
return AD_ID_RESULT_RUNNING;
}
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) < AD_ID_LS_PULSE_US) {
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
i_now_A = id_phase_a_current(meas);
delta_i_A = absf_local(i_now_A - g_id_runtime.current_start_A);
if (delta_i_A < AD_ID_MIN_DELTA_CURRENT_A) {
return AD_ID_RESULT_FAILED;
}
v_phase_V = (4.0f / 3.0f) * duty * g_id_runtime.vdc_start_V;
dt_s = (float)elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) * 0.000001f;
ls_H = (v_phase_V * dt_s) / delta_i_A;
id_store_ls(ls_H);
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
return AD_ID_RESULT_COMPLETE;
}
static uint8_t id_step_locked_rr(uint8_t key, const AD_Measurements_t *meas, float duty)
{
const AD_MotorParameters_t *params;
float i_rms_A;
float vdc_avg_V;
float v_phase_rms_V;
float z_abs_ohm;
float x_ohm;
float r_total_ohm;
float rr_ohm;
float ll_H;
float rs_ohm;
float omega_rad_s = AD_ID_TWO_PI * AD_ID_LOCKED_RR_FREQ_HZ;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
}
if (id_apply_rotating_vector(duty, AD_ID_LOCKED_RR_FREQ_HZ, meas) == 0U) {
return AD_ID_RESULT_FAILED;
}
if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >=
AD_ID_LOCKED_RR_SETTLE_US) {
id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
}
return AD_ID_RESULT_RUNNING;
}
id_accumulate_ac_sample(meas);
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
AD_ID_LOCKED_RR_MEASURE_US) {
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
i_rms_A = id_average_ac_current_rms();
vdc_avg_V = id_average_vdc();
if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
return AD_ID_RESULT_FAILED;
}
params = AD_ParamID_GetParameters();
if (params == 0) {
return AD_ID_RESULT_FAILED;
}
ll_H = ((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) ?
params->Ll_H :
params->Ls_H;
if (ll_H <= 0.0f) {
return AD_ID_RESULT_FAILED;
}
rs_ohm = ((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL) ?
params->Rs_ohm :
0.0f;
v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
z_abs_ohm = v_phase_rms_V / i_rms_A;
x_ohm = omega_rad_s * ll_H;
if ((z_abs_ohm <= AD_ID_MIN_IMPEDANCE_OHM) ||
((z_abs_ohm * z_abs_ohm) <= (x_ohm * x_ohm))) {
return AD_ID_RESULT_FAILED;
}
r_total_ohm = sqrtf_local((z_abs_ohm * z_abs_ohm) - (x_ohm * x_ohm));
rr_ohm = r_total_ohm - rs_ohm;
if (rr_ohm <= AD_ID_MIN_IMPEDANCE_OHM) {
return AD_ID_RESULT_FAILED;
}
id_store_rr(rr_ohm);
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
return AD_ID_RESULT_COMPLETE;
}
static uint8_t id_step_locked_rotor(uint8_t key, const AD_Measurements_t *meas, float duty)
{
uint8_t result;
if ((g_id_runtime.key != key) &&
(g_id_runtime.key != AD_ID_KEY_LOCKED_LS) &&
(g_id_runtime.key != AD_ID_KEY_LOCKED_RR)) {
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
g_id_runtime.auto_stage = 0U;
}
if (g_id_runtime.auto_stage == 0U) {
result = id_step_ls(AD_ID_KEY_LOCKED_LS, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = key;
g_id_runtime.auto_stage = 1U;
return AD_ID_RESULT_RUNNING;
}
return result;
}
result = id_step_locked_rr(AD_ID_KEY_LOCKED_RR, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = key;
g_id_runtime.auto_stage = 0U;
return AD_ID_RESULT_COMPLETE;
}
return result;
}
static uint8_t id_step_magnetizing(uint8_t key, const AD_Measurements_t *meas, float duty)
{
const AD_MotorParameters_t *params;
float frequency_hz = id_get_no_load_frequency_hz();
float i_rms_A;
float vdc_avg_V;
float v_phase_rms_V;
float l_total_H;
float lls_H = 0.0f;
float lm_H;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_SETTLE, meas->timestamp_us);
}
if (id_apply_rotating_vector(duty, frequency_hz, meas) == 0U) {
return AD_ID_RESULT_FAILED;
}
if (g_id_runtime.stage == AD_ID_STAGE_SETTLE) {
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) >=
AD_ID_MAG_SETTLE_US) {
id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
}
return AD_ID_RESULT_RUNNING;
}
id_accumulate_ac_sample(meas);
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
AD_ID_MAG_MEASURE_US) {
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
i_rms_A = id_average_ac_current_rms();
vdc_avg_V = id_average_vdc();
if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
return AD_ID_RESULT_FAILED;
}
params = AD_ParamID_GetParameters();
if ((params != 0) &&
((params->valid_mask & AD_MOTOR_PARAM_VALID_LL) != 0UL) &&
(params->Ll_H > 0.0f)) {
lls_H = 0.5f * params->Ll_H;
}
v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
l_total_H = v_phase_rms_V / ((AD_ID_TWO_PI * frequency_hz) * i_rms_A);
lm_H = l_total_H - lls_H;
if (lm_H <= 0.0f) {
return AD_ID_RESULT_FAILED;
}
id_store_lm(lm_H);
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
return AD_ID_RESULT_COMPLETE;
}
static uint8_t id_step_inertia_friction(uint8_t key, const AD_Measurements_t *meas, float duty)
{
const AD_MotorParameters_t *params;
float frequency_hz = id_get_inertia_frequency_hz();
float speed_now_rpm = absf_local(meas->speed_rpm);
float speed_delta_rpm;
float omega_start_rad_s;
float omega_end_rad_s;
float omega_avg_rad_s;
float accel_dt_s;
float coast_dt_s;
float i_rms_A;
float vdc_avg_V;
float v_phase_rms_V;
float apparent_power_W;
float copper_loss_W = 0.0f;
float mech_power_W;
float torque_Nm;
float accel_rad_s2;
float decel_rad_s2;
float j_kg_m2;
float b_Nm_s;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_ACCEL, meas->timestamp_us);
g_id_runtime.speed_start_rpm = speed_now_rpm;
}
if (g_id_runtime.stage == AD_ID_STAGE_ACCEL) {
if (id_apply_rotating_vector(duty, frequency_hz, meas) == 0U) {
return AD_ID_RESULT_FAILED;
}
id_accumulate_ac_sample(meas);
if (elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
AD_ID_INERTIA_ACCEL_US) {
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
g_id_runtime.speed_end_rpm = speed_now_rpm;
speed_delta_rpm = g_id_runtime.speed_end_rpm - g_id_runtime.speed_start_rpm;
if (speed_delta_rpm < AD_ID_MIN_SPEED_DELTA_RPM) {
return AD_ID_RESULT_FAILED;
}
accel_dt_s = (float)elapsed_us(meas->timestamp_us,
g_id_runtime.stage_start_us) * 0.000001f;
omega_start_rad_s = rpm_to_rad_s(g_id_runtime.speed_start_rpm);
omega_end_rad_s = rpm_to_rad_s(g_id_runtime.speed_end_rpm);
omega_avg_rad_s = 0.5f * (omega_start_rad_s + omega_end_rad_s);
accel_rad_s2 = (omega_end_rad_s - omega_start_rad_s) / accel_dt_s;
if ((omega_avg_rad_s <= 0.1f) || (accel_rad_s2 <= AD_ID_MIN_ACCEL_RAD_S2)) {
return AD_ID_RESULT_FAILED;
}
i_rms_A = id_average_ac_current_rms();
vdc_avg_V = id_average_vdc();
if ((i_rms_A < AD_ID_MIN_AC_CURRENT_A) || (vdc_avg_V <= 0.0f)) {
return AD_ID_RESULT_FAILED;
}
params = AD_ParamID_GetParameters();
if ((params != 0) &&
((params->valid_mask & AD_MOTOR_PARAM_VALID_RS) != 0UL)) {
copper_loss_W = 3.0f * i_rms_A * i_rms_A * params->Rs_ohm;
}
v_phase_rms_V = id_phase_voltage_rms(duty, vdc_avg_V);
apparent_power_W = 3.0f * v_phase_rms_V * i_rms_A;
mech_power_W = apparent_power_W - copper_loss_W;
if (mech_power_W <= 0.0f) {
return AD_ID_RESULT_FAILED;
}
torque_Nm = mech_power_W / omega_avg_rad_s;
g_id_runtime.accel_torque_Nm = torque_Nm;
g_id_runtime.accel_alpha_rad_s2 = accel_rad_s2;
id_runtime_reset(key, AD_ID_STAGE_COAST, meas->timestamp_us);
g_id_runtime.speed_start_rpm = speed_now_rpm;
g_id_runtime.accel_torque_Nm = torque_Nm;
g_id_runtime.accel_alpha_rad_s2 = accel_rad_s2;
return AD_ID_RESULT_RUNNING;
}
AD_Inverter_Disable();
if ((elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us) <
AD_ID_INERTIA_COAST_US) &&
(speed_now_rpm > AD_ID_MIN_SPEED_RPM)) {
return AD_ID_RESULT_RUNNING;
}
g_id_runtime.speed_end_rpm = speed_now_rpm;
omega_start_rad_s = rpm_to_rad_s(g_id_runtime.speed_start_rpm);
omega_end_rad_s = rpm_to_rad_s(g_id_runtime.speed_end_rpm);
if ((omega_start_rad_s <= rpm_to_rad_s(AD_ID_MIN_SPEED_RPM)) ||
(omega_end_rad_s >= omega_start_rad_s)) {
return AD_ID_RESULT_FAILED;
}
coast_dt_s = (float)elapsed_us(meas->timestamp_us,
g_id_runtime.stage_start_us) * 0.000001f;
omega_avg_rad_s = 0.5f * (omega_start_rad_s + omega_end_rad_s);
decel_rad_s2 = (omega_start_rad_s - omega_end_rad_s) / coast_dt_s;
if ((coast_dt_s <= 0.0f) ||
(omega_avg_rad_s <= 0.1f) ||
(decel_rad_s2 <= 0.0f) ||
(g_id_runtime.accel_torque_Nm <= 0.0f) ||
(g_id_runtime.accel_alpha_rad_s2 <= AD_ID_MIN_ACCEL_RAD_S2)) {
return AD_ID_RESULT_FAILED;
}
j_kg_m2 = g_id_runtime.accel_torque_Nm / g_id_runtime.accel_alpha_rad_s2;
b_Nm_s = (j_kg_m2 * decel_rad_s2) / omega_avg_rad_s;
if ((j_kg_m2 <= 0.0f) || (b_Nm_s <= 0.0f)) {
return AD_ID_RESULT_FAILED;
}
id_store_inertia_friction(j_kg_m2, b_Nm_s);
id_runtime_reset(key, AD_ID_STAGE_IDLE, meas->timestamp_us);
return AD_ID_RESULT_COMPLETE;
}
static uint8_t id_step_rotation_3hz(uint8_t key, const AD_Measurements_t *meas, float duty)
{
uint32_t ramp_us;
float target_frequency_hz;
uint16_t motor_control_type;
(void)duty;
if ((g_id_runtime.key != key) || (g_id_runtime.stage == AD_ID_STAGE_IDLE)) {
id_runtime_reset(key, AD_ID_STAGE_MEASURE, meas->timestamp_us);
}
duty = id_get_rotation_test_duty();
ramp_us = elapsed_us(meas->timestamp_us, g_id_runtime.stage_start_us);
motor_control_type = AD_ParamID_GetMotorControlType();
if ((motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) &&
(ramp_us < AD_ID_ROTATION_TEST_ALIGN_US)) {
rotation_pwm_stop();
g_id_runtime.pwm_timestamp_us = meas->timestamp_us;
if (id_apply_six_step_sector(duty, 0U) == 0U) {
return AD_ID_RESULT_FAILED;
}
return AD_ID_RESULT_RUNNING;
}
target_frequency_hz = AD_ParamID_GetRotationFrequencyHz();
if (id_configure_rotation_pwm(duty, target_frequency_hz, motor_control_type) == 0U) {
return AD_ID_RESULT_FAILED;
}
return AD_ID_RESULT_RUNNING;
}
static uint8_t id_step_auto(const AD_Measurements_t *meas, float duty)
{
uint8_t result;
if ((g_id_runtime.key != AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) &&
(g_id_runtime.key != AD_ID_KEY_AUTO_RS) &&
(g_id_runtime.key != AD_ID_KEY_AUTO_LS) &&
(g_id_runtime.key != AD_ID_KEY_AUTO_RR) &&
(g_id_runtime.key != AD_ID_KEY_AUTO_LM) &&
(g_id_runtime.key != AD_ID_KEY_AUTO_JB)) {
id_runtime_reset(AD_PARAM_ID_MODE_AUTO_IDENTIFICATION, AD_ID_STAGE_IDLE, meas->timestamp_us);
g_id_runtime.auto_stage = 0U;
}
if (g_id_runtime.auto_stage == 0U) {
result = id_step_rs(AD_ID_KEY_AUTO_RS, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 1U;
return AD_ID_RESULT_RUNNING;
}
return result;
}
if (g_id_runtime.auto_stage == 1U) {
result = id_step_ls(AD_ID_KEY_AUTO_LS, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 2U;
return AD_ID_RESULT_RUNNING;
}
return result;
}
if (g_id_runtime.auto_stage == 2U) {
if (AD_ParamID_IsLockedRotorAllowed() == 0U) {
AD_ParamID_MarkLockedRotorSkipped();
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 3U;
return AD_ID_RESULT_RUNNING;
}
result = id_step_locked_rr(AD_ID_KEY_AUTO_RR, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 3U;
return AD_ID_RESULT_RUNNING;
}
if (result == AD_ID_RESULT_FAILED) {
return id_finish_auto_partial(meas->timestamp_us);
}
return result;
}
if (g_id_runtime.auto_stage == 3U) {
result = id_step_magnetizing(AD_ID_KEY_AUTO_LM, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 4U;
return AD_ID_RESULT_RUNNING;
}
if (result == AD_ID_RESULT_FAILED) {
return id_finish_auto_partial(meas->timestamp_us);
}
return result;
}
result = id_step_inertia_friction(AD_ID_KEY_AUTO_JB, meas, duty);
if (result == AD_ID_RESULT_COMPLETE) {
g_id_runtime.key = AD_PARAM_ID_MODE_AUTO_IDENTIFICATION;
g_id_runtime.auto_stage = 0U;
return AD_ID_RESULT_COMPLETE;
}
if (result == AD_ID_RESULT_FAILED) {
return id_finish_auto_partial(meas->timestamp_us);
}
return result;
}
void AD_Inverter_SetDuty(float duty_a, float duty_b, float duty_c)
{
g_inverter_state.duty_a = clamp01(duty_a);
g_inverter_state.duty_b = clamp01(duty_b);
g_inverter_state.duty_c = clamp01(duty_c);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty_to_compare(g_inverter_state.duty_a));
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty_to_compare(g_inverter_state.duty_b));
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty_to_compare(g_inverter_state.duty_c));
}
void AD_Inverter_Disable(void)
{
rotation_pwm_stop();
inverter_disable_update_irq();
AD_Inverter_SetDuty(0.0f, 0.0f, 0.0f);
(void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_3);
(void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
(void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_2);
(void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
(void)HAL_TIMEx_PWMN_Stop(&htim1, TIM_CHANNEL_1);
(void)HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
g_inverter_state.pwm_running = 0U;
g_inverter_state.service_pwm_running = 0U;
g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
g_inverter_state.applied_test_duty = 0.0f;
}
void AD_Inverter_SetPwmPolarityFlags(uint16_t flags)
{
flags &= (uint16_t)AD_INVERTER_PWM_POLARITY_MASK;
if (flags == g_inverter_state.pwm_polarity_flags) {
return;
}
if (g_inverter_state.pwm_running != 0U) {
AD_Inverter_Disable();
}
g_inverter_state.pwm_polarity_flags = flags;
inverter_write_pwm_polarity(flags);
}
uint16_t AD_Inverter_GetPwmPolarityFlags(void)
{
return g_inverter_state.pwm_polarity_flags;
}
void AD_Inverter_SetPwmTimingMode(uint16_t mode)
{
mode = (mode == (uint16_t)AD_INVERTER_PWM_TIMING_UP) ?
(uint16_t)AD_INVERTER_PWM_TIMING_UP :
(uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
if (mode == g_inverter_state.pwm_timing_mode) {
return;
}
if (g_inverter_state.pwm_running != 0U) {
AD_Inverter_Disable();
}
g_inverter_state.pwm_timing_mode = mode;
inverter_write_pwm_timing(mode);
AD_Inverter_SetDuty(g_inverter_state.duty_a,
g_inverter_state.duty_b,
g_inverter_state.duty_c);
}
uint16_t AD_Inverter_GetPwmTimingMode(void)
{
return g_inverter_state.pwm_timing_mode;
}
void AD_Inverter_Init(void)
{
g_inverter_state.initialized = 1U;
g_inverter_state.outputs_allowed_by_build = (AD_INVERTER_ENABLE_OUTPUTS != 0) ? 1U : 0U;
g_inverter_state.pwm_running = 0U;
g_inverter_state.service_pwm_running = 0U;
g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
g_inverter_state.requested_test_duty = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
g_inverter_state.applied_test_duty = 0.0f;
g_inverter_state.electrical_frequency_Hz = 0.0f;
g_inverter_state.electrical_angle_rad = 0.0f;
g_inverter_state.pwm_polarity_flags =
(uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
AD_INVERTER_PWM_POLARITY_MASK);
g_inverter_state.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
g_inverter_state.id_stage = AD_ID_STAGE_IDLE;
inverter_write_pwm_polarity(g_inverter_state.pwm_polarity_flags);
inverter_write_pwm_timing(g_inverter_state.pwm_timing_mode);
id_runtime_reset(AD_ID_STAGE_IDLE, AD_ID_STAGE_IDLE, 0UL);
AD_Inverter_Disable();
}
uint8_t AD_Inverter_Enable(void)
{
if (g_inverter_state.initialized == 0U) {
return 0U;
}
if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
(AD_ParamID_IsPowerStageAllowed() == 0U)) {
AD_Inverter_Disable();
return 0U;
}
if ((g_inverter_state.pwm_running != 0U) &&
(g_inverter_state.service_pwm_running == 0U)) {
return 1U;
}
inverter_clear_break_flags();
if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
if (HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
if (HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
g_inverter_state.pwm_running = 1U;
g_inverter_state.service_pwm_running = 0U;
g_inverter_state.service_output = AD_INVERTER_PWM_OUTPUT_NONE;
inverter_enable_update_irq();
return 1U;
}
uint8_t AD_Inverter_StartPwmOutput(uint8_t output, float duty)
{
uint32_t channel;
uint8_t use_complementary = 0U;
uint8_t same_service;
uint32_t compare;
if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
(AD_ParamID_IsPowerStageAllowed() == 0U)) {
AD_Inverter_Disable();
return 0U;
}
duty = clamp01(duty);
g_inverter_state.requested_test_duty = duty;
g_inverter_state.applied_test_duty = duty;
same_service = (uint8_t)((g_inverter_state.pwm_running != 0U) &&
(g_inverter_state.service_pwm_running != 0U) &&
(g_inverter_state.service_output == output));
switch (output) {
case AD_INVERTER_PWM_OUTPUT_UH:
channel = TIM_CHANNEL_1;
break;
case AD_INVERTER_PWM_OUTPUT_UL:
channel = TIM_CHANNEL_1;
use_complementary = 1U;
break;
case AD_INVERTER_PWM_OUTPUT_VH:
channel = TIM_CHANNEL_2;
break;
case AD_INVERTER_PWM_OUTPUT_VL:
channel = TIM_CHANNEL_2;
use_complementary = 1U;
break;
case AD_INVERTER_PWM_OUTPUT_WH:
channel = TIM_CHANNEL_3;
break;
case AD_INVERTER_PWM_OUTPUT_WL:
channel = TIM_CHANNEL_3;
use_complementary = 1U;
break;
case AD_INVERTER_PWM_OUTPUT_ALL:
if (same_service == 0U) {
AD_Inverter_Disable();
}
AD_Inverter_SetDuty(0.5f, 0.5f, 0.5f);
if ((same_service == 0U) && (AD_Inverter_Enable() == 0U)) {
return 0U;
}
g_inverter_state.pwm_running = 1U;
g_inverter_state.service_pwm_running = 1U;
g_inverter_state.service_output = output;
set_service_debug_duty(output, duty);
return 1U;
default:
return 0U;
}
if (same_service == 0U) {
AD_Inverter_Disable();
}
compare = (use_complementary != 0U) ?
duty_to_compare_inverted(duty) :
duty_to_compare(duty);
__HAL_TIM_SET_COMPARE(&htim1, channel, compare);
if (same_service == 0U) {
inverter_clear_break_flags();
if (use_complementary != 0U) {
if (HAL_TIMEx_PWMN_Start(&htim1, channel) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
} else {
if (HAL_TIM_PWM_Start(&htim1, channel) != HAL_OK) {
AD_Inverter_Disable();
return 0U;
}
}
}
g_inverter_state.pwm_running = 1U;
g_inverter_state.service_pwm_running = 1U;
g_inverter_state.service_output = output;
set_service_debug_duty(output, duty);
return 1U;
}
uint8_t AD_Inverter_ApplyCommand(const AD_InverterCommand_t *command)
{
if ((command == 0) || (command->enable == 0U)) {
AD_Inverter_Disable();
return 1U;
}
if ((AD_INVERTER_ENABLE_OUTPUTS == 0) ||
(AD_ParamID_IsPowerStageAllowed() == 0U)) {
AD_Inverter_Disable();
return 0U;
}
if ((g_inverter_state.pwm_running != 0U) &&
(g_inverter_state.service_pwm_running == 0U)) {
AD_Inverter_SetDuty(command->duty_a, command->duty_b, command->duty_c);
return 1U;
}
if (g_inverter_state.service_pwm_running != 0U) {
AD_Inverter_Disable();
}
AD_Inverter_SetDuty(command->duty_a, command->duty_b, command->duty_c);
return AD_Inverter_Enable();
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if ((htim != 0) && (htim->Instance == TIM1)) {
rotation_pwm_service_update();
}
}
const AD_InverterState_t* AD_Inverter_GetState(void)
{
return &g_inverter_state;
}
void AD_ParamID_HardwareDisable(void)
{
AD_Inverter_Disable();
}
uint8_t AD_ParamID_HardwareStep(uint8_t mode, const AD_Measurements_t *meas)
{
uint8_t result;
float duty = id_get_test_duty();
switch (mode) {
case AD_PARAM_ID_MODE_PWM_TEST_UH:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_UH, duty);
case AD_PARAM_ID_MODE_PWM_TEST_UL:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_UL, duty);
case AD_PARAM_ID_MODE_PWM_TEST_VH:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_VH, duty);
case AD_PARAM_ID_MODE_PWM_TEST_VL:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_VL, duty);
case AD_PARAM_ID_MODE_PWM_TEST_WH:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_WH, duty);
case AD_PARAM_ID_MODE_PWM_TEST_WL:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_WL, duty);
case AD_PARAM_ID_MODE_PWM_TEST_ALL:
return AD_Inverter_StartPwmOutput(AD_INVERTER_PWM_OUTPUT_ALL, duty);
case AD_PARAM_ID_MODE_STATOR_RESISTANCE:
result = id_step_rs(mode, meas, duty);
break;
case AD_PARAM_ID_MODE_LOCKED_ROTOR_LEAKAGE:
result = id_step_locked_rotor(mode, meas, duty);
break;
case AD_PARAM_ID_MODE_AUTO_IDENTIFICATION:
result = id_step_auto(meas, duty);
break;
case AD_PARAM_ID_MODE_NO_LOAD_MAGNETIZING:
result = id_step_magnetizing(mode, meas, duty);
break;
case AD_PARAM_ID_MODE_INERTIA_FRICTION:
result = id_step_inertia_friction(mode, meas, duty);
break;
case AD_PARAM_ID_MODE_ROTATION_3HZ:
result = id_step_rotation_3hz(mode, meas, duty);
break;
default:
AD_Inverter_Disable();
return 0U;
}
if (result == AD_ID_RESULT_COMPLETE) {
AD_Inverter_Disable();
AD_ParamID_MarkComplete();
return 1U;
}
if (result == AD_ID_RESULT_RUNNING) {
return 1U;
}
AD_Inverter_Disable();
AD_ParamID_MarkStepFailed();
return 0U;
}

1042
AD_Keil_Project/Core/Src/ad_modbus.c Normal file
View File

@@ -0,0 +1,1042 @@
#include "ad_modbus.h"
#include <string.h>
#include "ad_binary_transport.h"
#include "ad_board.h"
#include "ad_inverter.h"
#include "ad_parameter_identification.h"
#include "ad_project_config.h"
#include "simulink_interface.h"
#include "stm32g4xx_hal_uart_ex.h"
#define AD_MODBUS_PROTOCOL_VERSION (8U)
#define AD_MODBUS_DEVICE_ID (0xAD01U)
#define AD_MODBUS_MAX_FRAME_BYTES (128U)
#define AD_MODBUS_MAX_READ_REGISTERS (60U)
#define AD_MODBUS_MAX_WRITE_REGISTERS ((AD_MODBUS_MAX_FRAME_BYTES - 9U) / 2U)
#define AD_MODBUS_HOLDING_40001_BASE (40001U)
#define AD_MODBUS_MAX_REGISTER_ADDRESS ((uint16_t)AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM)
#define AD_MODBUS_DEFAULT_CURRENT_LIMIT_A (10.0f)
#define AD_MODBUS_DEFAULT_OVERVOLTAGE_V (60.0f)
#define AD_MODBUS_DEFAULT_UNDERVOLTAGE_V (0.0f)
#define AD_MODBUS_DEFAULT_SPEED_LIMIT_RPM (1000.0f)
#define AD_MODBUS_DEFAULT_TEMPERATURE_C (85.0f)
volatile AD_ModbusRegisters_t g_ad_modbus_regs;
volatile AD_ModbusRegisters_t * const MB_REGS = &g_ad_modbus_regs;
static UART_HandleTypeDef *g_uart;
static AD_ModbusState_t g_modbus_state;
static uint8_t g_rx_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static uint8_t g_frame_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static uint8_t g_tx_buffer[AD_MODBUS_MAX_FRAME_BYTES];
static volatile uint16_t g_frame_size;
static uint8_t g_locked_rotor_allowed;
static AD_Command_t g_command_shadow;
static uint16_t crc16_modbus(const uint8_t *data, uint16_t size)
{
uint16_t crc = 0xFFFFU;
uint16_t i;
uint8_t bit;
for (i = 0U; i < size; i++) {
crc ^= data[i];
for (bit = 0U; bit < 8U; bit++) {
if ((crc & 0x0001U) != 0U) {
crc = (uint16_t)((crc >> 1) ^ 0xA001U);
} else {
crc >>= 1;
}
}
}
return crc;
}
static uint16_t read_be16(const uint8_t *data)
{
return (uint16_t)(((uint16_t)data[0] << 8) | data[1]);
}
static uint16_t normalize_register_address(uint16_t address)
{
uint32_t address32 = address;
if ((address32 >= AD_MODBUS_HOLDING_40001_BASE) &&
(address32 <= (AD_MODBUS_HOLDING_40001_BASE + AD_MODBUS_MAX_REGISTER_ADDRESS))) {
return (uint16_t)(address32 - AD_MODBUS_HOLDING_40001_BASE);
}
return address;
}
static void write_be16(uint8_t *data, uint16_t value)
{
data[0] = (uint8_t)(value >> 8);
data[1] = (uint8_t)(value & 0xFFU);
}
static uint16_t u32_lo(uint32_t value)
{
return (uint16_t)(value & 0xFFFFU);
}
static uint16_t u32_hi(uint32_t value)
{
return (uint16_t)((value >> 16) & 0xFFFFU);
}
static uint32_t float_to_u32_scaled(float value, float scale)
{
float scaled;
if (value <= 0.0f) {
return 0UL;
}
scaled = value * scale;
if (scaled > 4294967295.0f) {
return 0xFFFFFFFFUL;
}
return (uint32_t)(scaled + 0.5f);
}
static uint16_t float_to_u16_scaled(float value, float scale)
{
float scaled = value * scale;
if (scaled < -32768.0f) {
return 0x8000U;
}
if (scaled > 65535.0f) {
return 0xFFFFU;
}
if (scaled < 0.0f) {
return (uint16_t)((int16_t)(scaled - 0.5f));
}
return (uint16_t)(scaled + 0.5f);
}
static float reg_to_float(uint16_t value, float scale)
{
return (float)value / scale;
}
static float clamp_rotation_frequency(float frequency_hz)
{
if (frequency_hz <= 0.0f) {
return AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
}
if (frequency_hz < 0.1f) {
return 0.1f;
}
if (frequency_hz > AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ) {
return AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ;
}
return frequency_hz;
}
static uint8_t mode_is_valid(uint16_t mode)
{
return (uint8_t)((mode <= (uint16_t)AD_PARAM_ID_MODE_ROTATION_3HZ) ? 1U : 0U);
}
static void set_default_command(void)
{
memset(&g_command_shadow, 0, sizeof(g_command_shadow));
g_command_shadow.test_mode = AD_PARAM_ID_MODE_IDLE;
g_command_shadow.pwm_polarity_flags =
(uint16_t)(AD_INVERTER_DEFAULT_PWM_POLARITY_FLAGS &
AD_INVERTER_PWM_POLARITY_MASK);
g_command_shadow.pwm_timing_mode = (uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
g_command_shadow.motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
g_command_shadow.pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
g_command_shadow.rotation_frequency_Hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
g_command_shadow.rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
g_command_shadow.rotation_ramp_time_ms =
(uint16_t)AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
g_command_shadow.current_limit_A = AD_MODBUS_DEFAULT_CURRENT_LIMIT_A;
g_command_shadow.voltage_limit_V = AD_MODBUS_DEFAULT_OVERVOLTAGE_V;
g_command_shadow.undervoltage_limit_V = AD_MODBUS_DEFAULT_UNDERVOLTAGE_V;
g_command_shadow.speed_limit_rpm = AD_MODBUS_DEFAULT_SPEED_LIMIT_RPM;
g_command_shadow.temperature_limit_C = AD_MODBUS_DEFAULT_TEMPERATURE_C;
}
static void refresh_command_shadow(void)
{
const SimulinkInterface_OutputBus_t *output = SimulinkInterface_GetOutputBus();
if (output != 0) {
g_command_shadow = output->command;
}
}
static void apply_command_shadow(void)
{
SimulinkInterface_SetCommand(&g_command_shadow);
}
static uint8_t register_is_readable(uint16_t address)
{
return (uint8_t)(((address >= AD_MODBUS_REG_DEVICE_ID) &&
(address <= AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS)) ||
((address >= AD_MODBUS_REG_STATUS_MODE) &&
(address <= AD_MODBUS_REG_INVERTER_ID_STAGE)) ||
((address >= AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE) &&
(address <= AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS)) ||
((address >= AD_MODBUS_REG_MEAS_IA_0P001_A) &&
(address <= AD_MODBUS_REG_MEAS_SLIP_0P01_PERCENT)) ||
((address >= AD_MODBUS_REG_PARAM_VALID_MASK_LO) &&
(address <= AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM)));
}
static uint8_t register_is_writable(uint16_t address)
{
return (uint8_t)(((address >= AD_MODBUS_REG_CONTROL_ENABLE) &&
(address <= AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS)) ||
((address >= AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE) &&
(address <= AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS)) ||
(address == AD_MODBUS_REG_PARAM_POLE_PAIRS) ||
(address == AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM));
}
static uint16_t read_register_live(uint16_t address)
{
const AD_Measurements_t *meas = AD_ParamID_GetLastMeasurements();
const AD_PhaseCurrentPeaks_t *peaks = AD_ParamID_GetPhaseCurrentPeaks();
const AD_MotorParameters_t *params = AD_ParamID_GetParameters();
const AD_InverterState_t *inverter = AD_Inverter_GetState();
uint32_t value32;
uint32_t status;
uint32_t faults;
status = AD_ParamID_GetStatus();
faults = AD_ParamID_GetFaults();
switch ((AD_ModbusRegister_t)address) {
case AD_MODBUS_REG_DEVICE_ID:
return AD_MODBUS_DEVICE_ID;
case AD_MODBUS_REG_PROTOCOL_VERSION:
return AD_MODBUS_PROTOCOL_VERSION;
case AD_MODBUS_REG_CONTROL_ENABLE:
return g_command_shadow.enable;
case AD_MODBUS_REG_CONTROL_MODE:
return g_command_shadow.test_mode;
case AD_MODBUS_REG_CONTROL_RESET_FAULTS:
case AD_MODBUS_REG_CONTROL_STOP:
return 0U;
case AD_MODBUS_REG_CONTROL_LOCKED_ROTOR_ALLOWED:
return g_locked_rotor_allowed;
case AD_MODBUS_REG_CONTROL_PWM_DUTY_0P0001:
return float_to_u16_scaled(g_command_shadow.pwm_duty_limit, 10000.0f);
case AD_MODBUS_REG_LIMIT_CURRENT_0P01_A:
return float_to_u16_scaled(g_command_shadow.current_limit_A, 100.0f);
case AD_MODBUS_REG_LIMIT_OVERVOLTAGE_0P1_V:
return float_to_u16_scaled(g_command_shadow.voltage_limit_V, 10.0f);
case AD_MODBUS_REG_LIMIT_UNDERVOLTAGE_0P1_V:
return float_to_u16_scaled(g_command_shadow.undervoltage_limit_V, 10.0f);
case AD_MODBUS_REG_LIMIT_SPEED_RPM:
return float_to_u16_scaled(g_command_shadow.speed_limit_rpm, 1.0f);
case AD_MODBUS_REG_LIMIT_TEMPERATURE_0P1_C:
return float_to_u16_scaled(g_command_shadow.temperature_limit_C, 10.0f);
case AD_MODBUS_REG_CONTROL_ROTATION_FREQ_0P1_HZ:
return float_to_u16_scaled(g_command_shadow.rotation_frequency_Hz, 10.0f);
case AD_MODBUS_REG_CONTROL_ROTATION_MOD_0P0001:
return float_to_u16_scaled(g_command_shadow.rotation_modulation, 10000.0f);
case AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS:
return (uint16_t)(g_command_shadow.pwm_polarity_flags &
(uint16_t)AD_INVERTER_PWM_POLARITY_MASK);
case AD_MODBUS_REG_STATUS_MODE:
return AD_ParamID_GetMode();
case AD_MODBUS_REG_STATUS_FLAGS_LO:
return u32_lo(status);
case AD_MODBUS_REG_STATUS_FLAGS_HI:
return u32_hi(status);
case AD_MODBUS_REG_FAULT_FLAGS_LO:
return u32_lo(faults);
case AD_MODBUS_REG_FAULT_FLAGS_HI:
return u32_hi(faults);
case AD_MODBUS_REG_POWER_STAGE_ALLOWED:
return AD_ParamID_IsPowerStageAllowed();
case AD_MODBUS_REG_TEST_RUNNING:
return ((status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) ? 1U : 0U;
case AD_MODBUS_REG_INVERTER_PWM_RUNNING:
return (inverter != 0) ? inverter->pwm_running : 0U;
case AD_MODBUS_REG_INVERTER_SERVICE_OUTPUT:
return (inverter != 0) ? inverter->service_output : 0U;
case AD_MODBUS_REG_INVERTER_ID_STAGE:
return (inverter != 0) ? inverter->id_stage : 0U;
case AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE:
return g_command_shadow.pwm_timing_mode;
case AD_MODBUS_REG_CONTROL_MOTOR_CONTROL_TYPE:
return g_command_shadow.motor_control_type;
case AD_MODBUS_REG_CONTROL_HOST_PROTOCOL:
return AD_BinaryTransport_GetProtocol();
case AD_MODBUS_REG_CONTROL_BINARY_TRANSPORT:
return AD_BinaryTransport_GetTransport();
case AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS:
return g_command_shadow.rotation_ramp_time_ms;
case AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS:
return 0U;
case AD_MODBUS_REG_MEAS_IA_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ia_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IB_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ib_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IC_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ic_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_VDC_0P1_V:
return (meas != 0) ? float_to_u16_scaled(meas->vdc_V, 10.0f) : 0U;
case AD_MODBUS_REG_MEAS_TEMP_0P1_C:
return (meas != 0) ? float_to_u16_scaled(meas->temperature_C, 10.0f) : 0U;
case AD_MODBUS_REG_MEAS_STATUS_LO:
value32 = (meas != 0) ? meas->status_flags : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_MEAS_STATUS_HI:
value32 = (meas != 0) ? meas->status_flags : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_MEAS_SPEED_RPM:
return (meas != 0) ? float_to_u16_scaled(meas->speed_rpm, 1.0f) : 0U;
case AD_MODBUS_REG_MEAS_IA_RMS_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ia_rms_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IB_RMS_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ib_rms_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IC_RMS_0P001_A:
return (meas != 0) ? float_to_u16_scaled(meas->ic_rms_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_TORQUE_0P001_NM:
return (meas != 0) ? float_to_u16_scaled(meas->torque_Nm, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IA_PEAK_0P001_A:
return (peaks != 0) ? float_to_u16_scaled(peaks->ia_peak_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IB_PEAK_0P001_A:
return (peaks != 0) ? float_to_u16_scaled(peaks->ib_peak_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_IC_PEAK_0P001_A:
return (peaks != 0) ? float_to_u16_scaled(peaks->ic_peak_A, 1000.0f) : 0U;
case AD_MODBUS_REG_MEAS_SLIP_0P01_PERCENT:
return (meas != 0) ? float_to_u16_scaled(meas->slip_percent, 100.0f) : 0U;
case AD_MODBUS_REG_PARAM_VALID_MASK_LO:
value32 = (params != 0) ? params->valid_mask : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_VALID_MASK_HI:
value32 = (params != 0) ? params->valid_mask : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_RS_MOHM:
return (params != 0) ? float_to_u16_scaled(params->Rs_ohm, 1000.0f) : 0U;
case AD_MODBUS_REG_PARAM_LS_UH_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->Ls_H, 1000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_LS_UH_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->Ls_H, 1000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_LL_UH_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->Ll_H, 1000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_LL_UH_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->Ll_H, 1000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_RR_MOHM:
return (params != 0) ? float_to_u16_scaled(params->Rr_ohm, 1000.0f) : 0U;
case AD_MODBUS_REG_PARAM_LR_UH_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->Lr_H, 1000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_LR_UH_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->Lr_H, 1000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_LM_UH_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->Lm_H, 1000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_LM_UH_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->Lm_H, 1000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_J_NKGM2_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->J_kg_m2, 1000000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_J_NKGM2_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->J_kg_m2, 1000000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_B_NNMS_LO:
value32 = (params != 0) ? float_to_u32_scaled(params->B_Nm_s, 1000000000.0f) : 0UL;
return u32_lo(value32);
case AD_MODBUS_REG_PARAM_B_NNMS_HI:
value32 = (params != 0) ? float_to_u32_scaled(params->B_Nm_s, 1000000000.0f) : 0UL;
return u32_hi(value32);
case AD_MODBUS_REG_PARAM_POLE_PAIRS:
return AD_ParamID_GetPolePairs();
case AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM:
return float_to_u16_scaled(AD_Board_GetPhaseShuntResistanceOhm(), 1000.0f);
default:
return 0U;
}
}
void AD_Modbus_RefreshRegisters(void)
{
uint16_t address;
refresh_command_shadow();
for (address = 0U; address < AD_MODBUS_HOLDING_REGISTER_COUNT; address++) {
if (register_is_readable(address) != 0U) {
MB_REGS->holding_raw[address] = read_register_live(address);
} else {
MB_REGS->holding_raw[address] = 0U;
}
}
}
static uint16_t read_register(uint16_t address)
{
if (address < AD_MODBUS_HOLDING_REGISTER_COUNT) {
return MB_REGS->holding_raw[address];
}
return 0U;
}
typedef struct
{
uint8_t apply_command;
uint8_t reset_faults;
uint8_t stop_requested;
uint8_t locked_rotor_changed;
uint8_t pole_pairs_changed;
uint8_t phase_shunt_changed;
uint8_t host_protocol_changed;
uint8_t binary_transport_changed;
uint8_t reset_phase_current_peaks;
uint16_t host_protocol;
uint16_t binary_transport;
uint16_t pole_pairs;
uint16_t phase_shunt_mohm;
} AD_ModbusWriteEffects_t;
static void write_effects_clear(AD_ModbusWriteEffects_t *effects)
{
if (effects != 0) {
memset(effects, 0, sizeof(*effects));
}
}
static uint8_t stage_register_write(uint16_t address,
uint16_t value,
AD_Command_t *command,
uint8_t *locked_rotor_allowed,
uint16_t *pole_pairs,
AD_ModbusWriteEffects_t *effects)
{
if ((command == 0) || (locked_rotor_allowed == 0) ||
(pole_pairs == 0) || (effects == 0)) {
return 0U;
}
switch ((AD_ModbusRegister_t)address) {
case AD_MODBUS_REG_CONTROL_ENABLE:
command->enable = (value != 0U) ? 1U : 0U;
if (command->enable == 0U) {
command->test_mode = AD_PARAM_ID_MODE_IDLE;
effects->stop_requested = 1U;
}
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_MODE:
if (mode_is_valid(value) == 0U) {
return 0U;
}
command->test_mode = (uint8_t)value;
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_RESET_FAULTS:
if (value != 0U) {
effects->reset_faults = 1U;
}
break;
case AD_MODBUS_REG_CONTROL_STOP:
if (value != 0U) {
command->enable = 0U;
command->test_mode = AD_PARAM_ID_MODE_IDLE;
effects->apply_command = 1U;
effects->stop_requested = 1U;
}
break;
case AD_MODBUS_REG_CONTROL_LOCKED_ROTOR_ALLOWED:
*locked_rotor_allowed = (value != 0U) ? 1U : 0U;
effects->locked_rotor_changed = 1U;
break;
case AD_MODBUS_REG_CONTROL_PWM_DUTY_0P0001:
command->pwm_duty_limit = reg_to_float(value, 10000.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_LIMIT_CURRENT_0P01_A:
command->current_limit_A = reg_to_float(value, 100.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_LIMIT_OVERVOLTAGE_0P1_V:
command->voltage_limit_V = reg_to_float(value, 10.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_LIMIT_UNDERVOLTAGE_0P1_V:
command->undervoltage_limit_V = reg_to_float(value, 10.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_LIMIT_SPEED_RPM:
command->speed_limit_rpm = (float)value;
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_LIMIT_TEMPERATURE_0P1_C:
command->temperature_limit_C = reg_to_float(value, 10.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_ROTATION_FREQ_0P1_HZ:
command->rotation_frequency_Hz =
clamp_rotation_frequency(reg_to_float(value, 10.0f));
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_ROTATION_MOD_0P0001:
command->rotation_modulation = reg_to_float(value, 10000.0f);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_PWM_POLARITY_FLAGS:
command->pwm_polarity_flags =
(uint16_t)(value & (uint16_t)AD_INVERTER_PWM_POLARITY_MASK);
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_PWM_TIMING_MODE:
command->pwm_timing_mode =
(value == (uint16_t)AD_INVERTER_PWM_TIMING_UP) ?
(uint16_t)AD_INVERTER_PWM_TIMING_UP :
(uint16_t)AD_INVERTER_PWM_TIMING_CENTER;
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_MOTOR_CONTROL_TYPE:
command->motor_control_type =
(value == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) ?
(uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC :
(uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_HOST_PROTOCOL:
if (AD_BinaryTransport_IsProtocolValid(value) == 0U) {
return 0U;
}
effects->host_protocol = value;
effects->host_protocol_changed = 1U;
break;
case AD_MODBUS_REG_CONTROL_BINARY_TRANSPORT:
if (AD_BinaryTransport_IsTransportValid(value) == 0U) {
return 0U;
}
effects->binary_transport = value;
effects->binary_transport_changed = 1U;
break;
case AD_MODBUS_REG_CONTROL_ROTATION_RAMP_TIME_MS:
command->rotation_ramp_time_ms = value;
effects->apply_command = 1U;
break;
case AD_MODBUS_REG_CONTROL_RESET_PHASE_CURRENT_PEAKS:
if (value != 0U) {
effects->reset_phase_current_peaks = 1U;
}
break;
case AD_MODBUS_REG_PARAM_POLE_PAIRS:
if ((value != 0U) &&
(value > (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS)) {
return 0U;
}
*pole_pairs = value;
effects->pole_pairs = value;
effects->pole_pairs_changed = 1U;
break;
case AD_MODBUS_REG_PARAM_PHASE_SHUNT_MOHM:
effects->phase_shunt_mohm = value;
effects->phase_shunt_changed = 1U;
break;
default:
return 0U;
}
return 1U;
}
static void apply_write_effects(const AD_ModbusWriteEffects_t *effects)
{
if (effects == 0) {
return;
}
if (effects->reset_faults != 0U) {
AD_Board_ResetAdcFaults();
AD_ParamID_Reset();
}
if (effects->reset_phase_current_peaks != 0U) {
AD_ParamID_ResetPhaseCurrentPeaks();
}
if (effects->locked_rotor_changed != 0U) {
AD_ParamID_SetLockedRotorAllowed(g_locked_rotor_allowed);
}
if (effects->pole_pairs_changed != 0U) {
AD_ParamID_SetPolePairs(effects->pole_pairs);
}
if (effects->phase_shunt_changed != 0U) {
AD_Board_SetPhaseShuntResistanceOhm(reg_to_float(effects->phase_shunt_mohm, 1000.0f));
}
if (effects->apply_command != 0U) {
apply_command_shadow();
}
if (effects->binary_transport_changed != 0U) {
(void)AD_BinaryTransport_SetTransport(effects->binary_transport);
}
if (effects->host_protocol_changed != 0U) {
(void)AD_BinaryTransport_SetProtocol(effects->host_protocol);
}
if (effects->stop_requested != 0U) {
AD_ParamID_Stop();
}
}
static uint8_t write_register(uint16_t address, uint16_t value)
{
AD_Command_t staged_command;
AD_ModbusWriteEffects_t effects;
uint8_t staged_locked_rotor_allowed;
uint16_t staged_pole_pairs;
refresh_command_shadow();
staged_command = g_command_shadow;
staged_locked_rotor_allowed = g_locked_rotor_allowed;
staged_pole_pairs = AD_ParamID_GetPolePairs();
write_effects_clear(&effects);
if (stage_register_write(address,
value,
&staged_command,
&staged_locked_rotor_allowed,
&staged_pole_pairs,
&effects) == 0U) {
return 0U;
}
if (effects.stop_requested != 0U) {
staged_command.enable = 0U;
staged_command.test_mode = AD_PARAM_ID_MODE_IDLE;
}
g_command_shadow = staged_command;
g_locked_rotor_allowed = staged_locked_rotor_allowed;
apply_write_effects(&effects);
g_modbus_state.write_count++;
AD_Modbus_RefreshRegisters();
return 1U;
}
static void send_response(uint16_t size)
{
uint16_t crc;
if ((g_uart == 0) || (size > (AD_MODBUS_MAX_FRAME_BYTES - 2U))) {
return;
}
crc = crc16_modbus(g_tx_buffer, size);
g_tx_buffer[size++] = (uint8_t)(crc & 0xFFU);
g_tx_buffer[size++] = (uint8_t)(crc >> 8);
(void)HAL_UART_Transmit(g_uart, g_tx_buffer, size, 20U);
g_modbus_state.tx_frames++;
}
static void restart_uart_rx(void)
{
if (g_uart == 0) {
return;
}
if (HAL_UARTEx_ReceiveToIdle_IT(g_uart, g_rx_buffer, AD_MODBUS_MAX_FRAME_BYTES) == HAL_OK) {
g_modbus_state.status_flags |= AD_MODBUS_STATUS_UART_RX_ACTIVE;
} else {
g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_UART_RX_ACTIVE;
}
}
static void send_exception(uint8_t slave, uint8_t function, uint8_t exception)
{
g_tx_buffer[0] = slave;
g_tx_buffer[1] = function | 0x80U;
g_tx_buffer[2] = exception;
g_modbus_state.exception_errors++;
send_response(3U);
}
static uint8_t range_is_readable(uint16_t start, uint16_t quantity)
{
uint16_t i;
if ((quantity == 0U) || (quantity > AD_MODBUS_MAX_READ_REGISTERS)) {
return 0U;
}
for (i = 0U; i < quantity; i++) {
if (register_is_readable((uint16_t)(start + i)) == 0U) {
return 0U;
}
}
return 1U;
}
static uint8_t range_is_writable(uint16_t start, uint16_t quantity)
{
uint16_t i;
if (quantity == 0U) {
return 0U;
}
for (i = 0U; i < quantity; i++) {
if (register_is_writable((uint16_t)(start + i)) == 0U) {
return 0U;
}
}
return 1U;
}
static void process_read_holding(uint8_t slave, const uint8_t *frame, uint16_t size)
{
uint16_t start;
uint16_t quantity;
uint16_t i;
uint16_t value;
if (size != 8U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
return;
}
start = normalize_register_address(read_be16(&frame[2]));
quantity = read_be16(&frame[4]);
if (range_is_readable(start, quantity) == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
return;
}
AD_Modbus_RefreshRegisters();
g_tx_buffer[0] = slave;
g_tx_buffer[1] = frame[1];
g_tx_buffer[2] = (uint8_t)(quantity * 2U);
for (i = 0U; i < quantity; i++) {
value = read_register((uint16_t)(start + i));
write_be16(&g_tx_buffer[3U + (i * 2U)], value);
}
send_response((uint16_t)(3U + (quantity * 2U)));
}
static void process_write_single(uint8_t slave, const uint8_t *frame, uint16_t size, uint8_t broadcast)
{
uint16_t address;
uint16_t value;
if (size != 8U) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
}
return;
}
address = normalize_register_address(read_be16(&frame[2]));
value = read_be16(&frame[4]);
if (register_is_writable(address) == 0U) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
}
return;
}
if (write_register(address, value) == 0U) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
}
return;
}
if (broadcast == 0U) {
memcpy(g_tx_buffer, frame, 6U);
send_response(6U);
}
}
static void process_write_multiple(uint8_t slave, const uint8_t *frame, uint16_t size, uint8_t broadcast)
{
AD_Command_t staged_command;
AD_ModbusWriteEffects_t effects;
uint16_t start;
uint16_t quantity;
uint16_t i;
uint16_t byte_count;
uint16_t value;
uint8_t staged_locked_rotor_allowed;
uint16_t staged_pole_pairs;
if (size < 9U) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
}
return;
}
start = normalize_register_address(read_be16(&frame[2]));
quantity = read_be16(&frame[4]);
byte_count = frame[6];
if ((quantity == 0U) ||
(quantity > AD_MODBUS_MAX_WRITE_REGISTERS) ||
(byte_count != (quantity * 2U)) ||
(size != (uint16_t)(9U + byte_count)) ||
(range_is_writable(start, quantity) == 0U)) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
}
return;
}
refresh_command_shadow();
staged_command = g_command_shadow;
staged_locked_rotor_allowed = g_locked_rotor_allowed;
staged_pole_pairs = AD_ParamID_GetPolePairs();
write_effects_clear(&effects);
for (i = 0U; i < quantity; i++) {
value = read_be16(&frame[7U + (i * 2U)]);
if (stage_register_write((uint16_t)(start + i),
value,
&staged_command,
&staged_locked_rotor_allowed,
&staged_pole_pairs,
&effects) == 0U) {
if (broadcast == 0U) {
send_exception(slave, frame[1], AD_MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE);
}
return;
}
}
if (effects.stop_requested != 0U) {
staged_command.enable = 0U;
staged_command.test_mode = AD_PARAM_ID_MODE_IDLE;
}
g_command_shadow = staged_command;
g_locked_rotor_allowed = staged_locked_rotor_allowed;
apply_write_effects(&effects);
g_modbus_state.write_count = (uint32_t)(g_modbus_state.write_count + quantity);
AD_Modbus_RefreshRegisters();
if (broadcast == 0U) {
g_tx_buffer[0] = slave;
g_tx_buffer[1] = frame[1];
write_be16(&g_tx_buffer[2], read_be16(&frame[2]));
write_be16(&g_tx_buffer[4], quantity);
send_response(6U);
}
}
static void process_frame(const uint8_t *frame, uint16_t size)
{
uint8_t slave;
uint8_t function;
uint8_t broadcast;
uint16_t received_crc;
uint16_t computed_crc;
if (size < 4U) {
return;
}
slave = frame[0];
if ((slave != AD_MODBUS_SLAVE_ADDRESS) && (slave != 0U)) {
return;
}
received_crc = (uint16_t)frame[size - 2U] | ((uint16_t)frame[size - 1U] << 8);
computed_crc = crc16_modbus(frame, (uint16_t)(size - 2U));
if (received_crc != computed_crc) {
g_modbus_state.crc_errors++;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_LAST_FRAME_OK;
return;
}
function = frame[1];
broadcast = (slave == 0U) ? 1U : 0U;
g_modbus_state.rx_frames++;
g_modbus_state.last_function = function;
g_modbus_state.last_address = (size >= 4U) ? normalize_register_address(read_be16(&frame[2])) : 0U;
g_modbus_state.last_quantity = (size >= 6U) ? read_be16(&frame[4]) : 0U;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_OK;
g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_LAST_FRAME_ERROR;
switch ((AD_ModbusFunction_t)function) {
case AD_MODBUS_FUNCTION_READ_HOLDING_REGISTERS:
if (broadcast == 0U) {
process_read_holding(slave, frame, size);
}
break;
case AD_MODBUS_FUNCTION_WRITE_SINGLE_REGISTER:
process_write_single(slave, frame, size, broadcast);
break;
case AD_MODBUS_FUNCTION_WRITE_MULTIPLE_REGISTERS:
process_write_multiple(slave, frame, size, broadcast);
break;
default:
if (broadcast == 0U) {
send_exception(slave, function, AD_MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
}
break;
}
}
void AD_Modbus_Init(UART_HandleTypeDef *uart)
{
g_uart = uart;
memset(&g_modbus_state, 0, sizeof(g_modbus_state));
memset(g_rx_buffer, 0, sizeof(g_rx_buffer));
memset(g_frame_buffer, 0, sizeof(g_frame_buffer));
memset(g_tx_buffer, 0, sizeof(g_tx_buffer));
memset((void *)&g_ad_modbus_regs, 0, sizeof(g_ad_modbus_regs));
g_frame_size = 0U;
g_locked_rotor_allowed = 0U;
set_default_command();
g_modbus_state.initialized = (uart != 0) ? 1U : 0U;
g_modbus_state.slave_address = AD_MODBUS_SLAVE_ADDRESS;
g_modbus_state.baudrate = AD_MODBUS_UART_BAUDRATE;
if (g_modbus_state.initialized != 0U) {
g_modbus_state.status_flags = AD_MODBUS_STATUS_INITIALIZED;
restart_uart_rx();
}
AD_Modbus_RefreshRegisters();
}
void AD_Modbus_Loop(void)
{
#if AD_MODBUS_ENABLE != 0
uint8_t frame[AD_MODBUS_MAX_FRAME_BYTES];
uint16_t frame_size = 0U;
uint32_t primask;
AD_Modbus_RefreshRegisters();
if ((g_uart == 0) || (g_modbus_state.initialized == 0U)) {
return;
}
if (g_frame_size > 0U) {
primask = __get_PRIMASK();
__disable_irq();
frame_size = g_frame_size;
if (frame_size > AD_MODBUS_MAX_FRAME_BYTES) {
frame_size = AD_MODBUS_MAX_FRAME_BYTES;
}
memcpy(frame, g_frame_buffer, frame_size);
g_frame_size = 0U;
g_modbus_state.rx_count = 0U;
if (primask == 0UL) {
__enable_irq();
}
if (frame_size > 0U) {
process_frame(frame, frame_size);
AD_Modbus_RefreshRegisters();
g_modbus_state.status_flags &= ~AD_MODBUS_STATUS_FRAME_READY;
}
}
#endif
}
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
HAL_UART_RxEventTypeTypeDef event;
uint16_t size = Size;
if ((g_uart == 0) || (huart != g_uart)) {
return;
}
event = HAL_UARTEx_GetRxEventType(huart);
if ((event == HAL_UART_RXEVENT_TC) && (size >= AD_MODBUS_MAX_FRAME_BYTES)) {
g_modbus_state.rx_overflows++;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_RX_OVERFLOW;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
restart_uart_rx();
return;
}
if (size > AD_MODBUS_MAX_FRAME_BYTES) {
size = AD_MODBUS_MAX_FRAME_BYTES;
}
if (size > 0U) {
if (g_frame_size == 0U) {
memcpy(g_frame_buffer, g_rx_buffer, size);
g_frame_size = size;
g_modbus_state.rx_count = size;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_FRAME_READY;
} else {
g_modbus_state.rx_overflows++;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_RX_OVERFLOW;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
}
}
restart_uart_rx();
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
if ((g_uart == 0) || (huart != g_uart)) {
return;
}
g_modbus_state.uart_errors++;
g_modbus_state.status_flags |= AD_MODBUS_STATUS_LAST_FRAME_ERROR;
if (huart->RxState != HAL_UART_STATE_BUSY_RX) {
restart_uart_rx();
}
}
const AD_ModbusState_t* AD_Modbus_GetState(void)
{
return &g_modbus_state;
}
const volatile AD_ModbusRegisters_t* AD_Modbus_GetRegisters(void)
{
AD_Modbus_RefreshRegisters();
return MB_REGS;
}

602
AD_Keil_Project/Core/Src/ad_parameter_identification.c Normal file
View File

@@ -0,0 +1,602 @@
#include "ad_parameter_identification.h"
#include <string.h>
#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
#define AD_WEAK __attribute__((weak))
#elif defined(__CC_ARM)
#define AD_WEAK __weak
#elif defined(__GNUC__)
#define AD_WEAK __attribute__((weak))
#else
#define AD_WEAK
#endif
static AD_MotorParameters_t g_params;
static AD_Measurements_t g_last_meas;
static AD_PhaseCurrentPeaks_t g_phase_current_peaks;
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 float g_pwm_duty_limit;
static float g_rotation_frequency_hz;
static float g_rotation_modulation;
static uint16_t g_rotation_ramp_time_ms;
static uint16_t g_motor_control_type;
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) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_UH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_UL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_VH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_VL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_WH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_WL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_ALL) ||
(mode == AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) ||
(mode == AD_PARAM_ID_MODE_ROTATION_3HZ));
}
static uint8_t mode_uses_test_timeout(uint8_t mode)
{
return (uint8_t)((mode != AD_PARAM_ID_MODE_IDLE) &&
(mode != AD_PARAM_ID_MODE_DATA_LOGGING) &&
(mode != AD_PARAM_ID_MODE_ROTATION_3HZ));
}
static uint8_t mode_is_valid(uint8_t mode)
{
return (uint8_t)((mode == AD_PARAM_ID_MODE_IDLE) ||
(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) ||
(mode == AD_PARAM_ID_MODE_DATA_LOGGING) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_UH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_UL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_VH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_VL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_WH) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_WL) ||
(mode == AD_PARAM_ID_MODE_PWM_TEST_ALL) ||
(mode == AD_PARAM_ID_MODE_AUTO_IDENTIFICATION) ||
(mode == AD_PARAM_ID_MODE_ROTATION_3HZ));
}
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 update_phase_current_peaks(const AD_Measurements_t *meas)
{
float ia_abs;
float ib_abs;
float ic_abs;
if (meas == 0) {
return;
}
ia_abs = absf_local(meas->ia_A);
ib_abs = absf_local(meas->ib_A);
ic_abs = absf_local(meas->ic_A);
if (ia_abs > g_phase_current_peaks.ia_peak_A) {
g_phase_current_peaks.ia_peak_A = ia_abs;
}
if (ib_abs > g_phase_current_peaks.ib_peak_A) {
g_phase_current_peaks.ib_peak_A = ib_abs;
}
if (ic_abs > g_phase_current_peaks.ic_peak_A) {
g_phase_current_peaks.ic_peak_A = ic_abs;
}
}
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_status |= AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
} else {
g_status &= ~AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN;
}
}
static void enter_data_logging_blocked(void)
{
AD_ParamID_HardwareDisable();
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;
}
void AD_ParamID_Init(void)
{
memset(&g_params, 0, sizeof(g_params));
memset(&g_last_meas, 0, sizeof(g_last_meas));
memset(&g_phase_current_peaks, 0, sizeof(g_phase_current_peaks));
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;
g_pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
g_rotation_frequency_hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
g_rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
g_rotation_ramp_time_ms = AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
}
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 |
AD_PARAM_ID_STATUS_COMPLETE |
AD_PARAM_ID_STATUS_PARTIAL_COMPLETE |
AD_PARAM_ID_STATUS_STEP_FAILED |
AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED);
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 |
AD_PARAM_ID_STATUS_COMPLETE |
AD_PARAM_ID_STATUS_PARTIAL_COMPLETE |
AD_PARAM_ID_STATUS_STEP_FAILED |
AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED);
if (mode_is_valid(mode) == 0U) {
AD_ParamID_Stop();
return;
}
AD_ParamID_HardwareDisable();
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;
enter_data_logging_blocked();
return;
}
}
#if AD_PARAM_ID_ENABLE_POWER_TESTS == 0
if (mode_requires_power(mode) != 0U) {
enter_data_logging_blocked();
return;
}
#endif
if ((mode_requires_power(mode) != 0U) &&
((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL)) {
enter_data_logging_blocked();
return;
}
if ((mode_requires_power(mode) != 0U) && (g_software_enable == 0U)) {
enter_data_logging_blocked();
} 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)
{
if ((g_mode != AD_PARAM_ID_MODE_IDLE) ||
((g_status & (AD_PARAM_ID_STATUS_ACTIVE | AD_PARAM_ID_STATUS_POWER_STAGE_ARMED)) != 0UL)) {
AD_ParamID_HardwareDisable();
}
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_phase_current_peaks(meas);
g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
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) &&
(mode_uses_test_timeout(g_mode) != 0U)) {
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);
}
}
if ((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) != 0UL) {
return;
}
if (g_mode == AD_PARAM_ID_MODE_ROTATION_3HZ) {
if ((mode_requires_power(g_mode) == 0U) ||
(AD_ParamID_IsPowerStageAllowed() == 0U) ||
((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL) ||
(AD_ParamID_HardwareStep(g_mode, &g_last_meas) == 0U)) {
enter_data_logging_blocked();
}
}
}
void AD_ParamID_StepSlow(void)
{
if (g_mode == AD_PARAM_ID_MODE_IDLE) {
return;
}
if ((g_mode == AD_PARAM_ID_MODE_DATA_LOGGING) ||
(g_mode == AD_PARAM_ID_MODE_ROTATION_3HZ)) {
return;
}
if ((mode_requires_power(g_mode) == 0U) ||
(AD_ParamID_IsPowerStageAllowed() == 0U) ||
((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 0UL) ||
(AD_ParamID_HardwareStep(g_mode, &g_last_meas) == 0U)) {
enter_data_logging_blocked();
}
}
const AD_MotorParameters_t* AD_ParamID_GetParameters(void)
{
return &g_params;
}
const AD_Measurements_t* AD_ParamID_GetLastMeasurements(void)
{
return &g_last_meas;
}
const AD_PhaseCurrentPeaks_t* AD_ParamID_GetPhaseCurrentPeaks(void)
{
return &g_phase_current_peaks;
}
void AD_ParamID_ResetPhaseCurrentPeaks(void)
{
memset(&g_phase_current_peaks, 0, sizeof(g_phase_current_peaks));
}
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_SetParameters(const AD_MotorParameters_t *params)
{
if (params != 0) {
g_params = *params;
if (g_params.valid_mask != 0UL) {
g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
}
}
}
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)
{
uint8_t previous_enable = g_software_enable;
g_software_enable = (enable != 0U) ? 1U : 0U;
if (g_software_enable == 0U) {
g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
if ((previous_enable != 0U) ||
(g_mode != AD_PARAM_ID_MODE_IDLE) ||
((g_status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL)) {
AD_ParamID_HardwareDisable();
}
} else if (((g_status & AD_PARAM_ID_STATUS_FAULT_LATCHED) == 0UL) &&
((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) == 0UL) &&
(AD_PARAM_ID_ENABLE_POWER_TESTS != 0)) {
g_status |= AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
} else {
g_status &= ~AD_PARAM_ID_STATUS_POWER_STAGE_ARMED;
if (previous_enable != 0U) {
AD_ParamID_HardwareDisable();
}
}
}
void AD_ParamID_SetLockedRotorAllowed(uint8_t enable)
{
g_locked_rotor_allowed = (enable != 0U) ? 1U : 0U;
}
uint8_t AD_ParamID_IsLockedRotorAllowed(void)
{
return g_locked_rotor_allowed;
}
void AD_ParamID_SetPwmDutyLimit(float duty_limit)
{
if (duty_limit <= 0.0f) {
g_pwm_duty_limit = AD_PARAM_ID_DEFAULT_PWM_DUTY_LIMIT;
} else if (duty_limit > 0.45f) {
g_pwm_duty_limit = 0.45f;
} else {
g_pwm_duty_limit = duty_limit;
}
}
float AD_ParamID_GetPwmDutyLimit(void)
{
return g_pwm_duty_limit;
}
void AD_ParamID_SetRotationFrequencyHz(float frequency_hz)
{
if (frequency_hz <= 0.0f) {
g_rotation_frequency_hz = AD_PARAM_ID_DEFAULT_ROTATION_FREQUENCY_HZ;
} else if (frequency_hz < 0.1f) {
g_rotation_frequency_hz = 0.1f;
} else if (frequency_hz > AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ) {
g_rotation_frequency_hz = AD_PARAM_ID_MAX_ROTATION_FREQUENCY_HZ;
} else {
g_rotation_frequency_hz = frequency_hz;
}
}
float AD_ParamID_GetRotationFrequencyHz(void)
{
return g_rotation_frequency_hz;
}
void AD_ParamID_SetRotationModulation(float modulation)
{
if (modulation <= 0.0f) {
g_rotation_modulation = AD_PARAM_ID_DEFAULT_ROTATION_MODULATION;
} else if (modulation < 0.01f) {
g_rotation_modulation = 0.01f;
} else if (modulation > 0.45f) {
g_rotation_modulation = 0.45f;
} else {
g_rotation_modulation = modulation;
}
}
float AD_ParamID_GetRotationModulation(void)
{
return g_rotation_modulation;
}
void AD_ParamID_SetRotationRampTimeMs(uint16_t ramp_time_ms)
{
if (ramp_time_ms == 0U) {
g_rotation_ramp_time_ms = AD_PARAM_ID_DEFAULT_ROTATION_RAMP_TIME_MS;
} else if (ramp_time_ms < (uint16_t)AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS) {
g_rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_MIN_ROTATION_RAMP_TIME_MS;
} else if (ramp_time_ms > (uint16_t)AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS) {
g_rotation_ramp_time_ms = (uint16_t)AD_PARAM_ID_MAX_ROTATION_RAMP_TIME_MS;
} else {
g_rotation_ramp_time_ms = ramp_time_ms;
}
}
uint16_t AD_ParamID_GetRotationRampTimeMs(void)
{
return g_rotation_ramp_time_ms;
}
void AD_ParamID_SetMotorControlType(uint16_t motor_control_type)
{
if (motor_control_type == (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC) {
g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_BLDC;
} else {
g_motor_control_type = (uint16_t)AD_PARAM_ID_MOTOR_CONTROL_AD;
}
}
uint16_t AD_ParamID_GetMotorControlType(void)
{
return g_motor_control_type;
}
void AD_ParamID_SetPolePairs(uint16_t pole_pairs)
{
if (pole_pairs == 0U) {
g_params.pole_pairs = (float)AD_PARAM_ID_DEFAULT_POLE_PAIRS;
} else if (pole_pairs > (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS) {
g_params.pole_pairs = (float)AD_PARAM_ID_MAX_POLE_PAIRS;
} else {
g_params.pole_pairs = (float)pole_pairs;
}
g_params.valid_mask |= AD_MOTOR_PARAM_VALID_POLE_PAIRS;
g_status |= AD_PARAM_ID_STATUS_DATA_VALID;
}
uint16_t AD_ParamID_GetPolePairs(void)
{
if (g_params.pole_pairs <= 0.0f) {
return (uint16_t)AD_PARAM_ID_DEFAULT_POLE_PAIRS;
}
if (g_params.pole_pairs > (float)AD_PARAM_ID_MAX_POLE_PAIRS) {
return (uint16_t)AD_PARAM_ID_MAX_POLE_PAIRS;
}
return (uint16_t)(g_params.pole_pairs + 0.5f);
}
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) ||
((g_status & AD_PARAM_ID_STATUS_SAFETY_LIMITS_UNKNOWN) != 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);
}
void AD_ParamID_MarkComplete(void)
{
g_status |= AD_PARAM_ID_STATUS_COMPLETE;
AD_ParamID_Stop();
}
void AD_ParamID_MarkPartialComplete(void)
{
g_status |= AD_PARAM_ID_STATUS_PARTIAL_COMPLETE;
}
void AD_ParamID_MarkStepFailed(void)
{
g_status |= AD_PARAM_ID_STATUS_STEP_FAILED;
}
void AD_ParamID_MarkLockedRotorSkipped(void)
{
g_status |= AD_PARAM_ID_STATUS_LOCKED_ROTOR_SKIPPED |
AD_PARAM_ID_STATUS_PARTIAL_COMPLETE;
}
AD_WEAK uint8_t AD_ParamID_HardwareStep(uint8_t mode, const AD_Measurements_t *meas)
{
(void)mode;
(void)meas;
return 0U;
}
AD_WEAK void AD_ParamID_HardwareDisable(void)
{
}

142
AD_Keil_Project/Core/Src/ad_project.c Normal file
View File

@@ -0,0 +1,142 @@
#include "ad_project.h"
#include <string.h>
#include "ad_debug.h"
#include "ad_board.h"
#include "ad_binary_transport.h"
#include "ad_inverter.h"
#include "ad_modbus.h"
#include "ad_parameter_identification.h"
#include "main.h"
#ifndef AD_PROJECT_AUTO_START_MODE
#define AD_PROJECT_AUTO_START_MODE AD_PARAM_ID_MODE_IDLE
#endif
#ifndef AD_PROJECT_AUTO_START_LOCKED_ROTOR_ALLOWED
#define AD_PROJECT_AUTO_START_LOCKED_ROTOR_ALLOWED 0
#endif
static AD_ProjectState_t g_project_state;
volatile AD_DebugView_t g_ad_debug;
static void project_update_state(void)
{
const AD_InverterState_t *inverter = AD_Inverter_GetState();
if (inverter != 0) {
g_project_state.inverter_initialized = inverter->initialized;
g_project_state.outputs_allowed_by_build = inverter->outputs_allowed_by_build;
}
g_project_state.modbus_initialized = AD_Modbus_GetState()->initialized;
g_project_state.binary_transport_initialized = AD_BinaryTransport_GetState()->initialized;
g_project_state.param_id_status = AD_ParamID_GetStatus();
g_project_state.param_id_faults = AD_ParamID_GetFaults();
}
void AD_Project_Init(void)
{
memset(&g_project_state, 0, sizeof(g_project_state));
AD_Board_Init();
AD_BinaryTransport_Init();
AD_Modbus_Init(&huart2);
g_project_state.board_initialized = 1U;
g_project_state.simulink_initialized = 1U;
g_project_state.initialized = 1U;
#if AD_PROJECT_AUTO_START_LOCKED_ROTOR_ALLOWED != 0
AD_ParamID_SetLockedRotorAllowed(1U);
#endif
#if AD_PROJECT_AUTO_START_MODE != AD_PARAM_ID_MODE_IDLE
AD_Board_StartParamTest((uint8_t)AD_PROJECT_AUTO_START_MODE);
#endif
project_update_state();
AD_Debug_Init();
}
void AD_Project_Loop(void)
{
if (g_project_state.initialized == 0U) {
return;
}
AD_Board_Loop();
AD_BinaryTransport_Loop();
AD_Modbus_Loop();
project_update_state();
AD_Debug_Update();
}
void AD_Project_EmergencyStop(uint32_t fault_flags)
{
AD_Board_StopParamTest();
AD_ParamID_EmergencyStop(fault_flags);
project_update_state();
AD_Debug_Update();
}
const AD_ProjectState_t* AD_Project_GetState(void)
{
return &g_project_state;
}
void AD_Debug_Init(void)
{
memset((void *)&g_ad_debug, 0, sizeof(g_ad_debug));
AD_Debug_Update();
}
void AD_Debug_Update(void)
{
const AD_ProjectState_t *project = AD_Project_GetState();
const AD_InverterState_t *inverter = AD_Inverter_GetState();
const AD_BoardAdcSnapshot_t *adc = AD_Board_GetAdcSnapshot();
const AD_Measurements_t *measurements = AD_ParamID_GetLastMeasurements();
const AD_MotorParameters_t *parameters = AD_ParamID_GetParameters();
const AD_ModbusState_t *modbus = AD_Modbus_GetState();
const volatile AD_ModbusRegisters_t *modbus_regs = AD_Modbus_GetRegisters();
const AD_BinaryTransportState_t *binary_transport = AD_BinaryTransport_GetState();
const SimulinkInterface_OutputBus_t *output = SimulinkInterface_GetOutputBus();
uint32_t status = AD_ParamID_GetStatus();
if (project != 0) {
g_ad_debug.project = *project;
}
if (inverter != 0) {
g_ad_debug.inverter = *inverter;
}
if (adc != 0) {
g_ad_debug.adc = *adc;
}
if (measurements != 0) {
g_ad_debug.measurements = *measurements;
}
if (parameters != 0) {
g_ad_debug.motor_parameters = *parameters;
}
if (modbus != 0) {
g_ad_debug.modbus = *modbus;
}
if (modbus_regs != 0) {
g_ad_debug.modbus_regs = *modbus_regs;
}
if (binary_transport != 0) {
g_ad_debug.binary_transport = *binary_transport;
}
if (output != 0) {
g_ad_debug.command = output->command;
}
g_ad_debug.param_id_status = status;
g_ad_debug.param_id_faults = AD_ParamID_GetFaults();
g_ad_debug.param_id_mode = AD_ParamID_GetMode();
g_ad_debug.power_stage_allowed = AD_ParamID_IsPowerStageAllowed();
g_ad_debug.test_running = ((status & AD_PARAM_ID_STATUS_ACTIVE) != 0UL) ? 1U : 0U;
g_ad_debug.tick_ms = HAL_GetTick();
}

548
AD_Keil_Project/Core/Src/ad_usb_cdc.c Normal file
View File

@@ -0,0 +1,548 @@
#include "ad_usb_cdc.h"
#include <stddef.h>
#include <string.h>
#include "ad_project_config.h"
#include "main.h"
#include "simulink_interface.h"
#ifndef AD_USB_CDC_ENABLE
#define AD_USB_CDC_ENABLE 1
#endif
#define AD_USB_CDC_EP0_SIZE 64U
#define AD_USB_CDC_EP0_BUFFER_BYTES 128U
#define AD_USB_CDC_DATA_EP_SIZE 64U
#define AD_USB_CDC_CMD_EP_SIZE 8U
#define AD_USB_CDC_EP_IN 0x81U
#define AD_USB_CDC_EP_OUT 0x01U
#define AD_USB_CDC_EP_CMD 0x82U
#define USB_REQ_GET_STATUS 0x00U
#define USB_REQ_CLEAR_FEATURE 0x01U
#define USB_REQ_SET_FEATURE 0x03U
#define USB_REQ_SET_ADDRESS 0x05U
#define USB_REQ_GET_DESCRIPTOR 0x06U
#define USB_REQ_GET_CONFIGURATION 0x08U
#define USB_REQ_SET_CONFIGURATION 0x09U
#define USB_REQ_TYPE_MASK 0x60U
#define USB_REQ_TYPE_STANDARD 0x00U
#define USB_REQ_TYPE_CLASS 0x20U
#define USB_REQ_DIR_IN 0x80U
#define USB_DESC_TYPE_DEVICE 0x01U
#define USB_DESC_TYPE_CONFIGURATION 0x02U
#define USB_DESC_TYPE_STRING 0x03U
#define CDC_REQ_SET_LINE_CODING 0x20U
#define CDC_REQ_GET_LINE_CODING 0x21U
#define CDC_REQ_SET_CONTROL_LINE_STATE 0x22U
typedef struct
{
uint8_t bm_request_type;
uint8_t b_request;
uint16_t w_value;
uint16_t w_index;
uint16_t w_length;
} AD_USB_SetupPacket_t;
PCD_HandleTypeDef hpcd_USB_FS;
static AD_USB_CDC_State_t g_usb_state;
static uint8_t g_configuration;
static uint8_t g_pending_address;
static uint8_t g_address_pending;
static uint8_t g_ep0_out_request;
static uint16_t g_control_line_state;
static uint8_t g_line_coding[7] = { 0x00U, 0xC2U, 0x01U, 0x00U, 0x00U, 0x00U, 0x08U };
static uint8_t g_ep0_tx_buffer[AD_USB_CDC_EP0_BUFFER_BYTES];
static uint8_t g_string_descriptor[64];
static uint8_t g_rx_buffer[AD_USB_CDC_DATA_EP_SIZE];
static uint8_t g_tx_buffer[SIMULINK_TELEMETRY_MAX_BYTES];
static uint16_t g_tx_size;
static const uint8_t g_device_descriptor[] =
{
0x12U, USB_DESC_TYPE_DEVICE,
0x00U, 0x02U,
0x02U, 0x00U, 0x00U,
AD_USB_CDC_EP0_SIZE,
0x83U, 0x04U,
0x40U, 0x57U,
0x00U, 0x02U,
0x01U, 0x02U, 0x03U,
0x01U
};
static const uint8_t g_configuration_descriptor[] =
{
0x09U, USB_DESC_TYPE_CONFIGURATION, 0x43U, 0x00U, 0x02U, 0x01U, 0x00U, 0x80U, 0x32U,
0x09U, 0x04U, 0x00U, 0x00U, 0x01U, 0x02U, 0x02U, 0x01U, 0x00U,
0x05U, 0x24U, 0x00U, 0x10U, 0x01U,
0x05U, 0x24U, 0x01U, 0x00U, 0x01U,
0x04U, 0x24U, 0x02U, 0x02U,
0x05U, 0x24U, 0x06U, 0x00U, 0x01U,
0x07U, 0x05U, AD_USB_CDC_EP_CMD, 0x03U, AD_USB_CDC_CMD_EP_SIZE, 0x00U, 0xFFU,
0x09U, 0x04U, 0x01U, 0x00U, 0x02U, 0x0AU, 0x00U, 0x00U, 0x00U,
0x07U, 0x05U, AD_USB_CDC_EP_OUT, 0x02U, AD_USB_CDC_DATA_EP_SIZE, 0x00U, 0x00U,
0x07U, 0x05U, AD_USB_CDC_EP_IN, 0x02U, AD_USB_CDC_DATA_EP_SIZE, 0x00U, 0x00U
};
static void update_status_flags(void)
{
uint32_t flags = 0UL;
if (g_usb_state.initialized != 0U) {
flags |= AD_USB_CDC_STATUS_INITIALIZED;
}
if (g_usb_state.configured != 0U) {
flags |= AD_USB_CDC_STATUS_CONFIGURED;
}
if (g_usb_state.tx_busy != 0U) {
flags |= AD_USB_CDC_STATUS_TX_BUSY;
}
if ((g_usb_state.status_flags & AD_USB_CDC_STATUS_ENUM_ERROR) != 0UL) {
flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
}
if ((g_usb_state.status_flags & AD_USB_CDC_STATUS_TX_DROPPED) != 0UL) {
flags |= AD_USB_CDC_STATUS_TX_DROPPED;
}
g_usb_state.status_flags = flags;
}
static uint16_t min_u16(uint16_t a, uint16_t b)
{
return (a < b) ? a : b;
}
static void parse_setup(PCD_HandleTypeDef *hpcd, AD_USB_SetupPacket_t *setup)
{
const uint8_t *raw = (const uint8_t *)hpcd->Setup;
setup->bm_request_type = raw[0];
setup->b_request = raw[1];
setup->w_value = (uint16_t)raw[2] | ((uint16_t)raw[3] << 8);
setup->w_index = (uint16_t)raw[4] | ((uint16_t)raw[5] << 8);
setup->w_length = (uint16_t)raw[6] | ((uint16_t)raw[7] << 8);
}
static void ep0_stall(void)
{
(void)HAL_PCD_EP_SetStall(&hpcd_USB_FS, 0x80U);
(void)HAL_PCD_EP_SetStall(&hpcd_USB_FS, 0x00U);
g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
update_status_flags();
}
static void ep0_send(const uint8_t *data, uint16_t size, uint16_t requested)
{
uint16_t send_size = min_u16(size, requested);
if (send_size > sizeof(g_ep0_tx_buffer)) {
send_size = sizeof(g_ep0_tx_buffer);
}
if ((send_size > 0U) && (data != 0)) {
memcpy(g_ep0_tx_buffer, data, send_size);
}
(void)HAL_PCD_EP_Transmit(&hpcd_USB_FS, 0x80U, g_ep0_tx_buffer, send_size);
}
static void ep0_send_zlp(void)
{
(void)HAL_PCD_EP_Transmit(&hpcd_USB_FS, 0x80U, g_ep0_tx_buffer, 0U);
}
static uint16_t make_string_descriptor(const char *text)
{
uint16_t len = 0U;
uint16_t i;
while ((text[len] != '\0') && (len < 31U)) {
len++;
}
g_string_descriptor[0] = (uint8_t)(2U + (len * 2U));
g_string_descriptor[1] = USB_DESC_TYPE_STRING;
for (i = 0U; i < len; i++) {
g_string_descriptor[2U + (i * 2U)] = (uint8_t)text[i];
g_string_descriptor[3U + (i * 2U)] = 0U;
}
return (uint16_t)g_string_descriptor[0];
}
static void send_string_descriptor(uint8_t index, uint16_t requested)
{
uint16_t size;
if (index == 0U) {
static const uint8_t lang_id[] = { 0x04U, USB_DESC_TYPE_STRING, 0x09U, 0x04U };
ep0_send(lang_id, sizeof(lang_id), requested);
return;
}
switch (index) {
case 1U:
size = make_string_descriptor("SET");
break;
case 2U:
size = make_string_descriptor("AD Telemetry CDC");
break;
case 3U:
size = make_string_descriptor("AD0001");
break;
default:
ep0_stall();
return;
}
ep0_send(g_string_descriptor, size, requested);
}
static void open_cdc_endpoints(void)
{
(void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_CMD, AD_USB_CDC_CMD_EP_SIZE, EP_TYPE_INTR);
(void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, AD_USB_CDC_DATA_EP_SIZE, EP_TYPE_BULK);
(void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_IN, AD_USB_CDC_DATA_EP_SIZE, EP_TYPE_BULK);
(void)HAL_PCD_EP_Receive(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, g_rx_buffer, sizeof(g_rx_buffer));
}
static void close_cdc_endpoints(void)
{
(void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_CMD);
(void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_OUT);
(void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_IN);
}
static void handle_standard_request(const AD_USB_SetupPacket_t *setup)
{
uint8_t descriptor_type;
uint8_t descriptor_index;
uint8_t status[2] = { 0U, 0U };
switch (setup->b_request) {
case USB_REQ_GET_DESCRIPTOR:
descriptor_type = (uint8_t)(setup->w_value >> 8);
descriptor_index = (uint8_t)(setup->w_value & 0xFFU);
if (descriptor_type == USB_DESC_TYPE_DEVICE) {
ep0_send(g_device_descriptor, sizeof(g_device_descriptor), setup->w_length);
} else if (descriptor_type == USB_DESC_TYPE_CONFIGURATION) {
ep0_send(g_configuration_descriptor, sizeof(g_configuration_descriptor), setup->w_length);
} else if (descriptor_type == USB_DESC_TYPE_STRING) {
send_string_descriptor(descriptor_index, setup->w_length);
} else {
ep0_stall();
}
break;
case USB_REQ_SET_ADDRESS:
g_pending_address = (uint8_t)(setup->w_value & 0x7FU);
g_address_pending = 1U;
ep0_send_zlp();
break;
case USB_REQ_SET_CONFIGURATION:
g_configuration = (uint8_t)(setup->w_value & 0xFFU);
if (g_configuration != 0U) {
open_cdc_endpoints();
g_usb_state.configured = 1U;
} else {
close_cdc_endpoints();
g_usb_state.configured = 0U;
}
update_status_flags();
ep0_send_zlp();
break;
case USB_REQ_GET_CONFIGURATION:
ep0_send(&g_configuration, 1U, setup->w_length);
break;
case USB_REQ_GET_STATUS:
ep0_send(status, sizeof(status), setup->w_length);
break;
case USB_REQ_CLEAR_FEATURE:
case USB_REQ_SET_FEATURE:
ep0_send_zlp();
break;
default:
ep0_stall();
break;
}
}
static void handle_class_request(const AD_USB_SetupPacket_t *setup)
{
switch (setup->b_request) {
case CDC_REQ_SET_LINE_CODING:
if (((setup->bm_request_type & USB_REQ_DIR_IN) == 0U) && (setup->w_length == sizeof(g_line_coding))) {
g_ep0_out_request = CDC_REQ_SET_LINE_CODING;
(void)HAL_PCD_EP_Receive(&hpcd_USB_FS, 0x00U, g_line_coding, sizeof(g_line_coding));
} else {
ep0_stall();
}
break;
case CDC_REQ_GET_LINE_CODING:
ep0_send(g_line_coding, sizeof(g_line_coding), setup->w_length);
break;
case CDC_REQ_SET_CONTROL_LINE_STATE:
g_control_line_state = setup->w_value;
ep0_send_zlp();
break;
default:
ep0_stall();
break;
}
}
void AD_USB_CDC_Init(void)
{
#if AD_USB_CDC_ENABLE != 0
uint32_t tickstart;
memset(&g_usb_state, 0, sizeof(g_usb_state));
memset(&hpcd_USB_FS, 0, sizeof(hpcd_USB_FS));
memset(g_ep0_tx_buffer, 0, sizeof(g_ep0_tx_buffer));
memset(g_rx_buffer, 0, sizeof(g_rx_buffer));
memset(g_tx_buffer, 0, sizeof(g_tx_buffer));
g_configuration = 0U;
g_pending_address = 0U;
g_address_pending = 0U;
g_ep0_out_request = 0U;
g_control_line_state = 0U;
g_tx_size = 0U;
__HAL_RCC_HSI48_ENABLE();
tickstart = HAL_GetTick();
while (__HAL_RCC_GET_FLAG(RCC_FLAG_HSI48RDY) == RESET) {
if ((HAL_GetTick() - tickstart) > 10UL) {
g_usb_state.last_error = 1UL;
g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
update_status_flags();
return;
}
}
__HAL_RCC_USB_CONFIG(RCC_USBCLKSOURCE_HSI48);
hpcd_USB_FS.Instance = USB;
hpcd_USB_FS.Init.dev_endpoints = 8U;
hpcd_USB_FS.Init.speed = PCD_SPEED_FULL;
hpcd_USB_FS.Init.ep0_mps = PCD_EP0MPS_64;
hpcd_USB_FS.Init.phy_itface = PCD_PHY_EMBEDDED;
hpcd_USB_FS.Init.Sof_enable = DISABLE;
hpcd_USB_FS.Init.low_power_enable = DISABLE;
hpcd_USB_FS.Init.lpm_enable = DISABLE;
hpcd_USB_FS.Init.battery_charging_enable = DISABLE;
if (HAL_PCD_Init(&hpcd_USB_FS) != HAL_OK) {
g_usb_state.last_error = 2UL;
g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
update_status_flags();
return;
}
(void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, 0x00U, PCD_SNG_BUF, 0x18U);
(void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, 0x80U, PCD_SNG_BUF, 0x58U);
(void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, PCD_SNG_BUF, 0x98U);
(void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_IN, PCD_SNG_BUF, 0xD8U);
(void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_CMD, PCD_SNG_BUF, 0x118U);
if (HAL_PCD_Start(&hpcd_USB_FS) != HAL_OK) {
g_usb_state.last_error = 3UL;
g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
update_status_flags();
return;
}
(void)HAL_PCD_DevConnect(&hpcd_USB_FS);
g_usb_state.initialized = 1U;
update_status_flags();
#else
memset(&g_usb_state, 0, sizeof(g_usb_state));
#endif
}
void AD_USB_CDC_Loop(void)
{
update_status_flags();
}
uint8_t AD_USB_CDC_WritePacket(const uint8_t *data, uint16_t size)
{
#if AD_USB_CDC_ENABLE != 0
if ((data == 0) ||
(size == 0U) ||
(size > SIMULINK_TELEMETRY_MAX_BYTES) ||
(g_usb_state.configured == 0U) ||
(g_usb_state.tx_busy != 0U)) {
g_usb_state.dropped_packets++;
g_usb_state.status_flags |= AD_USB_CDC_STATUS_TX_DROPPED;
update_status_flags();
return 0U;
}
memcpy(g_tx_buffer, data, size);
g_tx_size = size;
g_usb_state.tx_busy = 1U;
g_usb_state.status_flags &= ~AD_USB_CDC_STATUS_TX_DROPPED;
update_status_flags();
if (HAL_PCD_EP_Transmit(&hpcd_USB_FS, AD_USB_CDC_EP_IN, g_tx_buffer, size) != HAL_OK) {
g_usb_state.tx_busy = 0U;
g_usb_state.dropped_packets++;
g_usb_state.status_flags |= AD_USB_CDC_STATUS_TX_DROPPED;
update_status_flags();
return 0U;
}
return 1U;
#else
(void)data;
(void)size;
return 0U;
#endif
}
uint8_t AD_USB_CDC_IsConfigured(void)
{
return g_usb_state.configured;
}
uint8_t AD_USB_CDC_IsTxBusy(void)
{
return g_usb_state.tx_busy;
}
const AD_USB_CDC_State_t* AD_USB_CDC_GetState(void)
{
update_status_flags();
return &g_usb_state;
}
void AD_USB_CDC_IRQHandler(void)
{
#if AD_USB_CDC_ENABLE != 0
HAL_PCD_IRQHandler(&hpcd_USB_FS);
#endif
}
void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
{
AD_USB_SetupPacket_t setup;
if (hpcd != &hpcd_USB_FS) {
return;
}
parse_setup(hpcd, &setup);
g_usb_state.setup_packets++;
if ((setup.bm_request_type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_STANDARD) {
handle_standard_request(&setup);
} else if ((setup.bm_request_type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_CLASS) {
handle_class_request(&setup);
} else {
ep0_stall();
}
}
void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
{
if (hpcd != &hpcd_USB_FS) {
return;
}
g_configuration = 0U;
g_pending_address = 0U;
g_address_pending = 0U;
g_ep0_out_request = 0U;
g_usb_state.configured = 0U;
g_usb_state.tx_busy = 0U;
g_tx_size = 0U;
(void)HAL_PCD_SetAddress(hpcd, 0U);
(void)HAL_PCD_EP_Open(hpcd, 0x00U, AD_USB_CDC_EP0_SIZE, EP_TYPE_CTRL);
(void)HAL_PCD_EP_Open(hpcd, 0x80U, AD_USB_CDC_EP0_SIZE, EP_TYPE_CTRL);
update_status_flags();
}
void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
{
if (hpcd != &hpcd_USB_FS) {
return;
}
if (epnum == 0U) {
if (g_address_pending != 0U) {
(void)HAL_PCD_SetAddress(hpcd, g_pending_address);
g_address_pending = 0U;
}
} else if (epnum == (AD_USB_CDC_EP_IN & 0x7FU)) {
g_usb_state.tx_busy = 0U;
g_usb_state.tx_packets++;
g_usb_state.tx_bytes += g_tx_size;
g_tx_size = 0U;
update_status_flags();
} else {
update_status_flags();
}
}
void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
{
uint32_t rx_count;
if (hpcd != &hpcd_USB_FS) {
return;
}
if (epnum == 0U) {
if (g_ep0_out_request == CDC_REQ_SET_LINE_CODING) {
g_ep0_out_request = 0U;
ep0_send_zlp();
}
return;
}
if (epnum == (AD_USB_CDC_EP_OUT & 0x7FU)) {
rx_count = HAL_PCD_EP_GetRxCount(hpcd, AD_USB_CDC_EP_OUT);
g_usb_state.rx_bytes += rx_count;
(void)HAL_PCD_EP_Receive(hpcd, AD_USB_CDC_EP_OUT, g_rx_buffer, sizeof(g_rx_buffer));
}
}
void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
{
(void)hpcd;
}
void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
{
(void)hpcd;
}
void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
{
(void)hpcd;
}
void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
{
if (hpcd == &hpcd_USB_FS) {
g_usb_state.configured = 0U;
g_usb_state.tx_busy = 0U;
update_status_flags();
}
}

349
AD_Keil_Project/Core/Src/main.c Normal file
View File

@@ -0,0 +1,349 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ad_project.h"
#include "ad_project_config.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#ifndef AD_TIM1_DEADTIME_TICKS
#define AD_TIM1_DEADTIME_TICKS 100U
#endif
#ifndef AD_TIM1_PWM_PERIOD_TICKS
#define AD_TIM1_PWM_PERIOD_TICKS 4249U
#endif
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim1;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_TIM1_Init(void);
static void MX_USART2_UART_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM1_Init();
MX_USART2_UART_Init();
/* USER CODE BEGIN 2 */
AD_Project_Init();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
AD_Project_Loop();
__NOP();
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV4;
RCC_OscInitStruct.PLL.PLLN = 85;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
{
/* USER CODE BEGIN TIM1_Init 0 */
/* USER CODE END TIM1_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
/* USER CODE BEGIN TIM1_Init 1 */
/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 0;
htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
htim1.Init.Period = AD_TIM1_PWM_PERIOD_TICKS;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
sBreakDeadTimeConfig.DeadTime = AD_TIM1_DEADTIME_TICKS;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_ENABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.BreakFilter = 0;
sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT;
sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
sBreakDeadTimeConfig.Break2Filter = 0;
sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT;
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_Init 2 */
HAL_TIM_MspPostInit(&htim1);
}
/**
* @brief USART2 Initialization Function
* @param None
* @retval None
*/
static void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = AD_MODBUS_UART_BAUDRATE;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1;
huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin : B1_Pin */
GPIO_InitStruct.Pin = B1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

196
AD_Keil_Project/Core/Src/simulink_interface.c Normal file
View File

@@ -0,0 +1,196 @@
#include "simulink_interface.h"
#include <string.h>
#include "ad_inverter.h"
static SimulinkInterface_InputBus_t g_input_bus;
static SimulinkInterface_OutputBus_t g_output_bus;
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;
uint32_t status;
uint8_t actual_mode;
uint8_t start_requested;
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 = command->undervoltage_limit_V;
limits.overtemperature_C = command->temperature_limit_C;
limits.speed_limit_rpm = command->speed_limit_rpm;
limits.timeout_us = AD_PARAM_ID_TIMEOUT_US_DEFAULT;
AD_ParamID_SetSafetyLimits(&limits);
AD_ParamID_SetPwmDutyLimit(command->pwm_duty_limit);
AD_ParamID_SetRotationFrequencyHz(command->rotation_frequency_Hz);
AD_ParamID_SetRotationModulation(command->rotation_modulation);
AD_ParamID_SetRotationRampTimeMs(command->rotation_ramp_time_ms);
AD_Inverter_SetPwmPolarityFlags(command->pwm_polarity_flags);
AD_Inverter_SetPwmTimingMode(command->pwm_timing_mode);
AD_ParamID_SetMotorControlType(command->motor_control_type);
if (command->enable == 0U) {
AD_ParamID_SetSoftwareEnable(0U);
AD_ParamID_Stop();
} else {
AD_ParamID_SetSoftwareEnable(1U);
status = AD_ParamID_GetStatus();
actual_mode = AD_ParamID_GetMode();
start_requested = (uint8_t)((command->test_mode != AD_PARAM_ID_MODE_IDLE) &&
((command->enable != g_last_enable) ||
(command->test_mode != g_last_test_mode) ||
(actual_mode != command->test_mode) ||
((status & AD_PARAM_ID_STATUS_ACTIVE) == 0UL)));
if ((status & AD_PARAM_ID_STATUS_FAULT_LATCHED) == 0UL) {
if (start_requested != 0U) {
AD_ParamID_Start(command->test_mode);
}
} else if (command->test_mode == AD_PARAM_ID_MODE_IDLE) {
AD_ParamID_Stop();
}
}
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);
}

358
AD_Keil_Project/Core/Src/stm32g4xx_hal_msp.c Normal file
View File

@@ -0,0 +1,358 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g4xx_hal_msp.c
* @brief This file provides code for the MSP Initialization
* and de-Initialization codes.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN Define */
/* USER CODE END Define */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN Macro */
/* USER CODE END Macro */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* External functions --------------------------------------------------------*/
/* USER CODE BEGIN ExternalFunctions */
/* USER CODE END ExternalFunctions */
/* USER CODE BEGIN 0 */
#ifndef GPIO_AF10_USB
#define GPIO_AF10_USB ((uint8_t)0x0A)
#endif
/* USER CODE END 0 */
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/**
* Initializes the Global MSP.
*/
void HAL_MspInit(void)
{
/* USER CODE BEGIN MspInit 0 */
/* USER CODE END MspInit 0 */
__HAL_RCC_SYSCFG_CLK_ENABLE();
__HAL_RCC_PWR_CLK_ENABLE();
/* System interrupt init*/
/** Disable the internal Pull-Up in Dead Battery pins of UCPD peripheral
*/
HAL_PWREx_DisableUCPDDeadBattery();
/* USER CODE BEGIN MspInit 1 */
/* USER CODE END MspInit 1 */
}
/**
* @brief FDCAN MSP Initialization
* This function configures the hardware resources used by FDCAN1.
* @param hfdcan: FDCAN handle pointer
* @retval None
*/
void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* hfdcan)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(hfdcan->Instance==FDCAN1)
{
/* USER CODE BEGIN FDCAN1_MspInit 0 */
/* USER CODE END FDCAN1_MspInit 0 */
__HAL_RCC_FDCAN_CONFIG(RCC_FDCANCLKSOURCE_PCLK1);
__HAL_RCC_FDCAN_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/**FDCAN1 GPIO Configuration
PB8 ------> FDCAN1_RX
PB9 ------> FDCAN1_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN FDCAN1_MspInit 1 */
/* USER CODE END FDCAN1_MspInit 1 */
}
}
/**
* @brief FDCAN MSP De-Initialization
* @param hfdcan: FDCAN handle pointer
* @retval None
*/
void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef* hfdcan)
{
if(hfdcan->Instance==FDCAN1)
{
/* USER CODE BEGIN FDCAN1_MspDeInit 0 */
/* USER CODE END FDCAN1_MspDeInit 0 */
__HAL_RCC_FDCAN_CLK_DISABLE();
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8|GPIO_PIN_9);
/* USER CODE BEGIN FDCAN1_MspDeInit 1 */
/* USER CODE END FDCAN1_MspDeInit 1 */
}
}
/**
* @brief PCD MSP Initialization
* This function configures the hardware resources used by USB FS.
* @param hpcd: PCD handle pointer
* @retval None
*/
void HAL_PCD_MspInit(PCD_HandleTypeDef* hpcd)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(hpcd->Instance==USB)
{
/* USER CODE BEGIN USB_MspInit 0 */
/* USER CODE END USB_MspInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_USB_CLK_ENABLE();
/**USB GPIO Configuration
PA11 ------> USB_DM
PA12 ------> USB_DP
*/
GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_12;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF10_USB;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
HAL_NVIC_SetPriority(USB_LP_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(USB_LP_IRQn);
/* USER CODE BEGIN USB_MspInit 1 */
/* USER CODE END USB_MspInit 1 */
}
}
/**
* @brief PCD MSP De-Initialization
* @param hpcd: PCD handle pointer
* @retval None
*/
void HAL_PCD_MspDeInit(PCD_HandleTypeDef* hpcd)
{
if(hpcd->Instance==USB)
{
/* USER CODE BEGIN USB_MspDeInit 0 */
/* USER CODE END USB_MspDeInit 0 */
__HAL_RCC_USB_CLK_DISABLE();
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_11|GPIO_PIN_12);
HAL_NVIC_DisableIRQ(USB_LP_IRQn);
/* USER CODE BEGIN USB_MspDeInit 1 */
/* USER CODE END USB_MspDeInit 1 */
}
}
/**
* @brief UART MSP Initialization
* This function configures the hardware resources used by USART2 VCP.
* @param huart: UART handle pointer
* @retval None
*/
void HAL_UART_MspInit(UART_HandleTypeDef* huart)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(huart->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspInit 0 */
/* USER CODE END USART2_MspInit 0 */
__HAL_RCC_USART2_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/**USART2 GPIO Configuration
PA2 ------> USART2_TX
PA3 ------> USART2_RX
*/
GPIO_InitStruct.Pin = VCP_TX_Pin|VCP_RX_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
HAL_NVIC_SetPriority(USART2_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(USART2_IRQn);
/* USER CODE BEGIN USART2_MspInit 1 */
/* USER CODE END USART2_MspInit 1 */
}
}
/**
* @brief UART MSP De-Initialization
* @param huart: UART handle pointer
* @retval None
*/
void HAL_UART_MspDeInit(UART_HandleTypeDef* huart)
{
if(huart->Instance==USART2)
{
/* USER CODE BEGIN USART2_MspDeInit 0 */
/* USER CODE END USART2_MspDeInit 0 */
__HAL_RCC_USART2_CLK_DISABLE();
HAL_GPIO_DeInit(GPIOA, VCP_TX_Pin|VCP_RX_Pin);
HAL_NVIC_DisableIRQ(USART2_IRQn);
/* USER CODE BEGIN USART2_MspDeInit 1 */
/* USER CODE END USART2_MspDeInit 1 */
}
}
/**
* @brief TIM_PWM MSP Initialization
* This function configures the hardware resources used in this example
* @param htim_pwm: TIM_PWM handle pointer
* @retval None
*/
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* htim_pwm)
{
if(htim_pwm->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspInit 0 */
/* USER CODE END TIM1_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_TIM1_CLK_ENABLE();
HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);
/* USER CODE BEGIN TIM1_MspInit 1 */
/* USER CODE END TIM1_MspInit 1 */
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(htim->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspPostInit 0 */
/* USER CODE END TIM1_MspPostInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/**TIM1 GPIO Configuration
PA6 ------> TIM1_BKIN
PA7 ------> TIM1_CH1N
PA8 ------> TIM1_CH1
PA9 ------> TIM1_CH2
PA10 ------> TIM1_CH3
PB0 ------> TIM1_CH2N
PB1 ------> TIM1_CH3N
*/
GPIO_InitStruct.Pin = INV_PWM_BKIN_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = INV_PWM_UL_Pin|INV_PWM_UH_Pin|INV_PWM_VH_Pin|INV_PWM_WH_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = INV_PWM_VL_Pin|INV_PWM_WL_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_TIM1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN TIM1_MspPostInit 1 */
/* USER CODE END TIM1_MspPostInit 1 */
}
}
/**
* @brief TIM_PWM MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param htim_pwm: TIM_PWM handle pointer
* @retval None
*/
void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* htim_pwm)
{
if(htim_pwm->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspDeInit 0 */
/* USER CODE END TIM1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM1_CLK_DISABLE();
HAL_NVIC_DisableIRQ(TIM1_UP_TIM16_IRQn);
/* USER CODE BEGIN TIM1_MspDeInit 1 */
/* USER CODE END TIM1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

276
AD_Keil_Project/Core/Src/stm32g4xx_it.c Normal file
View File

@@ -0,0 +1,276 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32g4xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ad_usb_cdc.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern TIM_HandleTypeDef htim1;
extern UART_HandleTypeDef huart2;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
* @brief This function handles Memory management fault.
*/
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
/* USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
* @brief This function handles Prefetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
/* USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
/* USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
* @brief This function handles System service call via SWI instruction.
*/
void SVC_Handler(void)
{
/* USER CODE BEGIN SVCall_IRQn 0 */
/* USER CODE END SVCall_IRQn 0 */
/* USER CODE BEGIN SVCall_IRQn 1 */
/* USER CODE END SVCall_IRQn 1 */
}
/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 */
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/**
* @brief This function handles Pendable request for system service.
*/
void PendSV_Handler(void)
{
/* USER CODE BEGIN PendSV_IRQn 0 */
/* USER CODE END PendSV_IRQn 0 */
/* USER CODE BEGIN PendSV_IRQn 1 */
/* USER CODE END PendSV_IRQn 1 */
}
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32G4xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32g4xx.s). */
/******************************************************************************/
/**
* @brief This function handles EXTI line[15:10] interrupts.
*/
void EXTI15_10_IRQHandler(void)
{
/* USER CODE BEGIN EXTI15_10_IRQn 0 */
/* USER CODE END EXTI15_10_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(B1_Pin);
/* USER CODE BEGIN EXTI15_10_IRQn 1 */
/* USER CODE END EXTI15_10_IRQn 1 */
}
/**
* @brief This function handles TIM1 update interrupt and TIM16 global interrupt.
*/
void TIM1_UP_TIM16_IRQHandler(void)
{
/* USER CODE BEGIN TIM1_UP_TIM16_IRQn 0 */
/* USER CODE END TIM1_UP_TIM16_IRQn 0 */
HAL_TIM_IRQHandler(&htim1);
/* USER CODE BEGIN TIM1_UP_TIM16_IRQn 1 */
/* USER CODE END TIM1_UP_TIM16_IRQn 1 */
}
/**
* @brief This function handles USART2 global interrupt.
*/
void USART2_IRQHandler(void)
{
/* USER CODE BEGIN USART2_IRQn 0 */
/* USER CODE END USART2_IRQn 0 */
HAL_UART_IRQHandler(&huart2);
/* USER CODE BEGIN USART2_IRQn 1 */
/* USER CODE END USART2_IRQn 1 */
}
/**
* @brief This function handles USB low priority interrupt.
*/
void USB_LP_IRQHandler(void)
{
/* USER CODE BEGIN USB_LP_IRQn 0 */
/* USER CODE END USB_LP_IRQn 0 */
AD_USB_CDC_IRQHandler();
/* USER CODE BEGIN USB_LP_IRQn 1 */
/* USER CODE END USB_LP_IRQn 1 */
}
/**
* @brief This function handles USB wakeup interrupt.
*/
void USBWakeUp_IRQHandler(void)
{
/* USER CODE BEGIN USBWakeUp_IRQn 0 */
/* USER CODE END USBWakeUp_IRQn 0 */
AD_USB_CDC_IRQHandler();
/* USER CODE BEGIN USBWakeUp_IRQn 1 */
/* USER CODE END USBWakeUp_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

285
AD_Keil_Project/Core/Src/system_stm32g4xx.c Normal file
View File

@@ -0,0 +1,285 @@
/**
******************************************************************************
* @file system_stm32g4xx.c
* @author MCD Application Team
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File
*
* This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32g4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* After each device reset the HSI (16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32g4xx.s" file, to
* configure the system clock before to branch to main program.
*
* This file configures the system clock as follows:
*=============================================================================
*-----------------------------------------------------------------------------
* System Clock source | HSI
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 16000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 16000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 1
*-----------------------------------------------------------------------------
* APB2 Prescaler | 1
*-----------------------------------------------------------------------------
* PLL_M | 1
*-----------------------------------------------------------------------------
* PLL_N | 16
*-----------------------------------------------------------------------------
* PLL_P | 7
*-----------------------------------------------------------------------------
* PLL_Q | 2
*-----------------------------------------------------------------------------
* PLL_R | 2
*-----------------------------------------------------------------------------
* Require 48MHz for RNG | Disabled
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* Copyright (c) 2019 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32g4xx_system
* @{
*/
/** @addtogroup STM32G4xx_System_Private_Includes
* @{
*/
#include "stm32g4xx.h"
#if !defined (HSE_VALUE)
#define HSE_VALUE 24000000U /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSI_VALUE)
#define HSI_VALUE 16000000U /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_Defines
* @{
*/
/************************* Miscellaneous Configuration ************************/
/* Note: Following vector table addresses must be defined in line with linker
configuration. */
/*!< Uncomment the following line if you need to relocate the vector table
anywhere in Flash or Sram, else the vector table is kept at the automatic
remap of boot address selected */
/* #define USER_VECT_TAB_ADDRESS */
#if defined(USER_VECT_TAB_ADDRESS)
/*!< Uncomment the following line if you need to relocate your vector Table
in Sram else user remap will be done in Flash. */
/* #define VECT_TAB_SRAM */
#if defined(VECT_TAB_SRAM)
#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
#else
#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_SRAM */
#endif /* USER_VECT_TAB_ADDRESS */
/******************************************************************************/
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_Variables
* @{
*/
/* The SystemCoreClock variable is updated in three ways:
1) by calling CMSIS function SystemCoreClockUpdate()
2) by calling HAL API function HAL_RCC_GetHCLKFreq()
3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
Note: If you use this function to configure the system clock; then there
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
*/
uint32_t SystemCoreClock = HSI_VALUE;
const uint8_t AHBPrescTable[16] = {0U, 0U, 0U, 0U, 0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U, 6U, 7U, 8U, 9U};
const uint8_t APBPrescTable[8] = {0U, 0U, 0U, 0U, 1U, 2U, 3U, 4U};
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_FunctionPrototypes
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G4xx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << (10*2))|(3UL << (11*2))); /* set CP10 and CP11 Full Access */
#endif
/* Configure the Vector Table location add offset address ------------------*/
#if defined(USER_VECT_TAB_ADDRESS)
SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#endif /* USER_VECT_TAB_ADDRESS */
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (**) HSI_VALUE is a constant defined in stm32g4xx_hal.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (***) HSE_VALUE is a constant defined in stm32g4xx_hal.h file (default value
* 24 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp, pllvco, pllr, pllsource, pllm;
/* Get SYSCLK source -------------------------------------------------------*/
switch (RCC->CFGR & RCC_CFGR_SWS)
{
case 0x04: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x08: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x0C: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
SYSCLK = PLL_VCO / PLLR
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> 4) + 1U ;
if (pllsource == 0x02UL) /* HSI used as PLL clock source */
{
pllvco = (HSI_VALUE / pllm);
}
else /* HSE used as PLL clock source */
{
pllvco = (HSE_VALUE / pllm);
}
pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 8);
pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> 25) + 1U) * 2U;
SystemCoreClock = pllvco/pllr;
break;
default:
break;
}
/* Compute HCLK clock frequency --------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/