Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-23 09:03:40 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

check DRV fault

Browse Source
This commit is contained in:
Oskar Weigl
2017-12-15 19:51:51 -08:00
parent f301d3cac4
commit 5aec866788
2 changed files with 29 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/low_level.c
+26 -17
View File
@@ -320,7 +320,7 @@ void global_fault(int error) {
*(motors[i].axis_legacy.enable_control) = false;
}
// disable brake resistor
update_brake_current(0.0f);
set_brake_current(0.0f);
}
float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) {
@@ -1112,7 +1112,16 @@ bool spin_up_sensorless(Motor_t* motor) {
// TODO: check pll vel (abs ratio, 0.8)
}
void update_brake_current(float brake_current) {
void update_brake_current() {
float Ibus_sum = 0.0f;
for (int i = 0; i < num_motors; ++i) {
Ibus_sum += motors[i].current_control.Ibus;
}
// Note: set_brake_current will clip negative values to 0.0f
set_brake_current(-Ibus_sum);
}
void set_brake_current(float brake_current) {
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * brake_resistance / vbus_voltage;
@@ -1191,19 +1200,6 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
// Compute estimated bus current
ictrl->Ibus = mod_d * Id + mod_q * Iq;
// If this is last motor, update brake resistor duty
// if (motor == &motors[num_motors-1]) {
// Above check doesn't work if last motor is executing voltage control
// TODO trigger this update in control_motor_loop instead,
// and make voltage control a control mode in it.
float Ibus_sum = 0.0f;
for (int i = 0; i < num_motors; ++i) {
Ibus_sum += motors[i].current_control.Ibus;
}
// Note: function will clip negative values to 0.0f
update_brake_current(-Ibus_sum);
// }
// Inverse park transform
float mod_alpha = c * mod_d - s * mod_q;
float mod_beta = c * mod_q + s * mod_d;
@@ -1224,12 +1220,23 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) {
return true;
}
//Returns true if the fault line is asserted
bool check_DRV_fault(Motor_t* motor) {
//TODO: make this pin configurable per motor ch
GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin);
return (nFAULT_state == GPIO_PIN_RESET) ? true : false;
}
void control_motor_loop(Motor_t* motor) {
while (*(motor->axis_legacy.enable_control)) {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT;
break;
}
if (check_DRV_fault(motor)) {
motor->error = ERROR_DRV_FAULT;
break;
}
update_rotor(motor);
anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking
@@ -1304,9 +1311,11 @@ void control_motor_loop(Motor_t* motor) {
if (!FOC_current(motor, 0.0f, Iq)) {
break; // in case of error exit loop, motor->error has been set by FOC_current
}
update_brake_current();
}
//We are exiting control, reset Ibus, and update brake current
//TODO update brake current from all motors in 1 func
//TODO reset this motor Ibus, then call from here
motor->current_control.Ibus = 0.0f;
update_brake_current();
}
Firmware/MotorControl/low_level.h
+3 -1
View File
@@ -48,6 +48,7 @@ typedef enum {
ERROR_UNEXPECTED_STEP_SRC,
ERROR_POS_CTRL_DURING_SENSORLESS,
ERROR_SPIN_UP_TIMEOUT,
ERROR_DRV_FAULT,
} Error_t;
// Note: these should be sorted from lowest level of control to
@@ -232,7 +233,8 @@ bool using_sensorless(Motor_t* motor);
float get_rotor_phase(Motor_t* motor);
float get_pll_vel(Motor_t* motor);
bool spin_up_sensorless(Motor_t* motor);
void update_brake_current(float brake_current);
void update_brake_current();
void set_brake_current(float brake_current);
void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta);
void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta);
bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);