Skip to content

encoder_as5047: Add SPI loose magnet detection logic #892

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
fbg-jpg wants to merge 1 commit into
base: master
Choose a base branch
from
Open

encoder_as5047: Add SPI loose magnet detection logic #892

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions datatypes.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,6 +171,7 @@ typedef enum {
FAULT_CODE_PHASE_FILTER,
FAULT_CODE_ENCODER_FAULT,
FAULT_CODE_LV_OUTPUT_FAULT,
FAULT_CODE_ENCODER_LOOSE_MAGNET
} mc_fault_code;

typedef enum {
Expand Down
31 changes: 29 additions & 2 deletions encoder/encoder.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -636,7 +636,7 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto

if (is_foc_encoder) {
switch (m_conf->m_sensor_port_mode) {
case SENSOR_PORT_MODE_AS5047_SPI:
case SENSOR_PORT_MODE_AS5047_SPI:{
if (encoder_cfg_as504x.state.spi_error_rate > 0.05) {
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
}
Expand All @@ -653,8 +653,35 @@ void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_moto
mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false);
}
}
// Mechanical slip detection: Compare the FOC observer phase against the physical
// encoder phase. If the motor is spinning fast enough for a stable observer
// estimate (>110% of open-loop threshold) and the phase gap exceeds 15 degrees
// for more than 500ms, trigger a LOOSE_MAGNET fault.
float current_rpm = mc_interface_get_rpm();
float ol_erpm = m_conf->foc_openloop_rpm;
float stable_observer_threshold = ol_erpm * 1.1f;
static systime_t fault_start_time = 0;
if (fabsf(current_rpm) > stable_observer_threshold){
float obs = mcpwm_foc_get_phase_observer();
float enc = mcpwm_foc_get_phase_encoder();
float gap = fabsf(utils_angle_difference(obs, enc));
if (gap > 15.0f) {
if (fault_start_time == 0) {
fault_start_time = chVTGetSystemTime();
}
uint32_t elapsed_ms = ST2MS(chVTTimeElapsedSinceX(fault_start_time));
if (elapsed_ms > 500) {
fault_start_time = 0;
mc_interface_fault_stop(FAULT_CODE_ENCODER_LOOSE_MAGNET, is_second_motor, false);
}
} else {
fault_start_time = 0;
}
} else{
fault_start_time = 0;
}
break;

}
case SENSOR_PORT_MODE_MT6816_SPI_HW:
if (encoder_cfg_mt6816.state.encoder_no_magnet_error_rate > 0.05) {
mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
Expand Down
1 change: 1 addition & 0 deletions motor/mc_interface.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -500,6 +500,7 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
case FAULT_CODE_RESOLVER_DOS: return "FAULT_CODE_RESOLVER_DOS";
case FAULT_CODE_RESOLVER_LOS: return "FAULT_CODE_RESOLVER_LOS";
case FAULT_CODE_ENCODER_NO_MAGNET: return "FAULT_CODE_ENCODER_NO_MAGNET";
case FAULT_CODE_ENCODER_LOOSE_MAGNET: return "FAULT_CODE_ENCODER_LOOSE_MAGNET";
case FAULT_CODE_ENCODER_MAGNET_TOO_STRONG: return "FAULT_CODE_ENCODER_MAGNET_TOO_STRONG";
case FAULT_CODE_PHASE_FILTER: return "FAULT_CODE_PHASE_FILTER";
case FAULT_CODE_ENCODER_FAULT: return "FAULT_CODE_ENCODER_FAULT";
Expand Down