From bb9d4f5225562c0df3a7a9dc6a98a3a973c39a2a Mon Sep 17 00:00:00 2001 From: Nick Knudson Date: Wed, 1 Mar 2017 21:44:06 -0800 Subject: [PATCH 01/12] added travis-ci --- .travis.yml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 00000000..ba888a75 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,23 @@ +# adapted from https://github.com/andysworkshop/stm32plus/blob/master/.travis.yml + +language: c +sudo: false + +addons: + apt: + packages: + libc6-i386 + +cache: + directories: + - $HOME/dl + +install: + - export GCC_DIR=$HOME/dl/gcc-arm-none-eabi-5_2-2015q4 + - export GCC_ARCHIVE=$HOME/dl/gcc-arm-none-eabi-5_2-2015q4-20151219-linux.tar.bz2 + - export GCC_URL=https://launchpad.net/gcc-arm-embedded/5.0/5-2015-q4-major/+download/gcc-arm-none-eabi-5_2-2015q4-20151219-linux.tar.bz2 + - if [ ! -e $GCC_DIR/bin/arm-none-eabi-gcc ]; then wget $GCC_URL -O $GCC_ARCHIVE; tar xfj $GCC_ARCHIVE -C $HOME/dl; fi + - export PATH=$PATH:$GCC_DIR/bin + +script: + - make clean && make From 72efd17f31e370721f5bfe0de28b2d08f1bc9e0f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 19 Feb 2018 00:32:01 -0800 Subject: [PATCH 02/12] implement dc bus brownout check --- Firmware/MotorControl/axis.cpp | 4 +- Firmware/MotorControl/low_level.c | 69 +++++++++++++++++++++++++++---- Firmware/MotorControl/low_level.h | 4 ++ 3 files changed, 66 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index aaac53b5..bd000916 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -62,8 +62,8 @@ void Axis::StateMachineLoop() { for (;;) { // Keep rotor estimation up to date while idling osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - update_rotor(legacy_motor_ref_); - + loop_updates(legacy_motor_ref_); + if (do_calibration_) { do_calibration_ = false; diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c49b95ea..3819fa43 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -72,6 +72,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] + .dc_bus_brownout_trip_level = 8.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -177,6 +178,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] + .dc_bus_brownout_trip_level = 8.0f, // [V] .phase_inductance = 0.0f, // to be set by measure_phase_inductance .phase_resistance = 0.0f, // to be set by measure_phase_resistance .motor_thread = 0, @@ -553,6 +555,7 @@ void step_cb(uint16_t GPIO_Pin) { } // Triggered when an encoder passes over the "Index" pin +// TODO: only arm index edge interrupt when we know encoder has powered up void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { Motor_t* motor = &motors[motor_index]; if (!motor->encoder.index_found) { @@ -685,6 +688,9 @@ bool measure_phase_resistance(Motor_t* motor, float test_current, float max_volt motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; + float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage) test_voltage = max_voltage; @@ -724,6 +730,9 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; + Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; // Test voltage along phase A @@ -774,6 +783,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; queue_voltage_timings(motor, voltage_magnitude, 0.0f); } // scan forwards @@ -783,6 +794,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; float v_alpha = voltage_magnitude * arm_cos_f32(ph); float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); @@ -808,6 +821,8 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; float v_alpha = voltage_magnitude * arm_cos_f32(ph); float v_beta = voltage_magnitude * arm_sin_f32(ph); queue_voltage_timings(motor, v_alpha, v_beta); @@ -911,6 +926,8 @@ bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { for (;;) { for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); + if (!do_checks(motor)) + return false; if (motor->encoder.index_found) return true; @@ -1101,8 +1118,13 @@ bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { motor->error = ERROR_SPIN_UP_TIMEOUT; return false; } + + if (!do_checks(motor)) + return false; // run estimator - update_rotor(motor); + if (!loop_updates(motor)) + return false; + // override the phase during spinup motor->sensorless.phase = phase; // run current control (with the phase override) @@ -1191,6 +1213,8 @@ void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { queue_modulation_timings(motor, mod_alpha, mod_beta); } +// TODO: This doesn't update brake current +// We should probably make FOC Current call FOC Voltage to avoid duplication. bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { float phase = get_rotor_phase(motor); float c = arm_cos_f32(phase); @@ -1273,14 +1297,40 @@ bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { motor->error = ERROR_FOC_TIMING; return false; } + + update_brake_current(); return true; } -//Returns true if the fault line is asserted +//Returns true if everything is OK (no fault) 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; + return (nFAULT_state == GPIO_PIN_RESET) ? false : true; +} + +//Returns true if everything is OK (no fault) +bool check_PSU_brownout(Motor_t* motor) { + if(vbus_voltage < motor->dc_bus_brownout_trip_level) { + motor->error = ERROR_DC_BUS_BROWNOUT; + return false; + } + return true; +} + +// Returns true if everything is ok. Sets motor->error and returns false otherwise. +bool do_checks(Motor_t* motor) { + // Checks + if (!check_DRV_fault(motor)) { + motor->error = ERROR_DRV_FAULT; + return false; + } + return true; +} + +bool loop_updates(Motor_t* motor) { + update_rotor(motor); + return true; } void control_motor_loop(Motor_t* motor) { @@ -1289,12 +1339,14 @@ void control_motor_loop(Motor_t* motor) { motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; break; } - if (check_DRV_fault(motor)) { - motor->error = ERROR_DRV_FAULT; + + if (!do_checks(motor)) break; - } - update_rotor(motor); - anti_cogging_calibration(motor); // Only runs if anticogging.calib_anticogging is true; non-blocking + if (!loop_updates(motor)) + break; + + // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(motor); // Position control // TODO Decide if we want to use encoder or pll position here @@ -1377,7 +1429,6 @@ void control_motor_loop(Motor_t* motor) { break; } - update_brake_current(); ++(motor->loop_counter); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 9045b1d9..e8f4bcc5 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -50,6 +50,7 @@ typedef enum { ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_DC_BUS_BROWNOUT, } Error_t; // Note: these should be sorted from lowest level of control to @@ -147,6 +148,7 @@ typedef struct { float current_setpoint; float calibration_current; float resistance_calib_max_voltage; + float dc_bus_brownout_trip_level; float phase_inductance; float phase_resistance; osThreadId motor_thread; @@ -243,6 +245,8 @@ bool anti_cogging_calibration(Motor_t* motor); // Test functions void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude); // Main motor control +bool do_checks(Motor_t* motor); +bool loop_updates(Motor_t* motor); void update_rotor(Motor_t* motor); bool using_encoder(Motor_t* motor); bool using_sensorless(Motor_t* motor); From 06ab4d7bae930c46dd98c75c55ca6a136f5a61f1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 19 Feb 2018 19:42:47 -0500 Subject: [PATCH 03/12] Add Encoder CPR range checking --- Firmware/MotorControl/low_level.c | 17 ++++++++++++++--- Firmware/MotorControl/low_level.h | 2 ++ 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 96826b0d..d0a7ff5f 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -129,6 +129,7 @@ Motor_t motors[] = { .encoder_offset = 0, .encoder_state = 0, .motor_dir = 1, // 1 or -1 + .encoder_calib_range = 0.02, .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -231,6 +232,7 @@ Motor_t motors[] = { .encoder_offset = 0, .encoder_state = 0, .motor_dir = 1, // 1 or -1 + .encoder_calib_range = 0.02, .phase = 0.0f, // [rad] .pll_pos = 0.0f, // [rad] .pll_vel = 0.0f, // [rad/s] @@ -765,9 +767,6 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float scan_range = 4.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - // go to motor zero phase for start_lock_duration to get ready to scan for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { @@ -776,6 +775,10 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } queue_voltage_timings(motor, voltage_magnitude, 0.0f); } + + int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; + int32_t encvaluesum = 0; + // scan forwards for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { @@ -789,6 +792,14 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; } + + float estEnc = scan_range / elec_rad_per_enc; + float adjustedCount = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT)-init_enc_val; + if(fabsf(adjustedCount-estEnc)/estEnc > motor->encoder.encoder_calib_range) + { + motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; + return false; + } // check direction if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { // motor same dir as encoder diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 9045b1d9..deb89f3c 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -50,6 +50,7 @@ typedef enum { ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_ENCODER_CPR_OUT_OF_RANGE } Error_t; // Note: these should be sorted from lowest level of control to @@ -119,6 +120,7 @@ typedef struct { int32_t encoder_offset; int32_t encoder_state; int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. + float encoder_calib_range; float phase; float pll_pos; float pll_vel; From 4d0366f8dff70e8b356c7a0b3f539407685531f1 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Mon, 19 Feb 2018 20:01:45 -0500 Subject: [PATCH 04/12] Change the calibration distance & number of steps for accuracy --- Firmware/MotorControl/low_level.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d0a7ff5f..afc63053 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -762,9 +762,9 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // TODO: add check_timing bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; - static const int num_steps = 1024; + static const int num_steps = 1024*4; static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 4.0f * M_PI; + static const float scan_range = 16.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) // go to motor zero phase for start_lock_duration to get ready to scan From ef0f8dbf85c1a4d4ec69b8e2ba0543d5bc979ae1 Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Wed, 21 Feb 2018 13:41:42 -0600 Subject: [PATCH 05/12] Add microsecond delay functions. Add DRV register/fault status reporting. --- Firmware/Drivers/DRV8301/drv8301.c | 23 ++++++++++++++--------- Firmware/Drivers/DRV8301/drv8301.h | 5 +++++ Firmware/MotorControl/commands.cpp | 14 ++++++++++++++ Firmware/MotorControl/low_level.c | 12 +++++++++++- Firmware/MotorControl/low_level.h | 1 + Firmware/MotorControl/utils.c | 21 +++++++++++++++++++++ Firmware/MotorControl/utils.h | 4 ++++ tools/drv_status.py | 28 ++++++++++++++++++++++++++++ 8 files changed, 98 insertions(+), 10 deletions(-) create mode 100644 tools/drv_status.py diff --git a/Firmware/Drivers/DRV8301/drv8301.c b/Firmware/Drivers/DRV8301/drv8301.c index db3a8081..765d7d49 100644 --- a/Firmware/Drivers/DRV8301/drv8301.c +++ b/Firmware/Drivers/DRV8301/drv8301.c @@ -45,6 +45,8 @@ // drivers #include "drv8301.h" +#include "utils.h" + // ************************************************************************** // the defines @@ -362,7 +364,7 @@ uint16_t DRV8301_readSpi(DRV8301_Handle handle, const DRV8301_RegName_e regName) // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_RESET); - osDelay(1); + delay_us(1); // Do blocking read uint16_t zerobuff = 0; @@ -374,17 +376,17 @@ uint16_t DRV8301_readSpi(DRV8301_Handle handle, const DRV8301_RegName_e regName) // but for some reason you actually need to pulse it. // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_SET); - osDelay(1); + delay_us(1); // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_RESET); - osDelay(1); + delay_us(1); HAL_SPI_TransmitReceive(handle->spiHandle, (uint8_t*)(&zerobuff), (uint8_t*)(&recbuff), 1, 1000); - osDelay(1); + delay_us(1); // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_SET); - osDelay(1); + delay_us(1); assert(recbuff != 0xbeef); @@ -590,16 +592,16 @@ void DRV8301_writeSpi(DRV8301_Handle handle, const DRV8301_RegName_e regName,con { // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_RESET); - osDelay(1); + delay_us(5); // Do blocking write uint16_t controlword = (uint16_t)DRV8301_buildCtrlWord(DRV8301_CtrlMode_Write, regName, data); HAL_SPI_Transmit(handle->spiHandle, (uint8_t*)(&controlword), 1, 1000); - osDelay(1); + delay_us(5); // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_SET); - osDelay(1); + delay_us(5); return; } // end of DRV8301_writeSpi() function @@ -659,12 +661,14 @@ void DRV8301_readData(DRV8301_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars) Spi_8301_Vars->Stat_Reg_1.FETLB_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETLB_OC_BITS); Spi_8301_Vars->Stat_Reg_1.FETHC_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETHC_OC_BITS); Spi_8301_Vars->Stat_Reg_1.FETLC_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETLC_OC_BITS); + Spi_8301_Vars->Stat_Reg_1_Value = drvDataNew; // Update Status Register 2 drvRegName = DRV8301_RegName_Status_2; drvDataNew = DRV8301_readSpi(handle,drvRegName); Spi_8301_Vars->Stat_Reg_2.GVDD_OV = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS2_GVDD_OV_BITS); Spi_8301_Vars->Stat_Reg_2.DeviceID = (uint16_t)(drvDataNew & (uint16_t)DRV8301_STATUS2_ID_BITS); + Spi_8301_Vars->Stat_Reg_2_Value = drvDataNew; // Update Control Register 1 drvRegName = DRV8301_RegName_Control_1; @@ -674,6 +678,7 @@ void DRV8301_readData(DRV8301_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars) Spi_8301_Vars->Ctrl_Reg_1.PWM_MODE = (DRV8301_PwmMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_PWM_MODE_BITS); Spi_8301_Vars->Ctrl_Reg_1.OC_MODE = (DRV8301_OcMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_OC_MODE_BITS); Spi_8301_Vars->Ctrl_Reg_1.OC_ADJ_SET = (DRV8301_VdsLevel_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_OC_ADJ_SET_BITS); + Spi_8301_Vars->Ctrl_Reg_1_Value = drvDataNew; // Update Control Register 2 drvRegName = DRV8301_RegName_Control_2; @@ -682,7 +687,7 @@ void DRV8301_readData(DRV8301_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars) Spi_8301_Vars->Ctrl_Reg_2.GAIN = (DRV8301_ShuntAmpGain_e)(drvDataNew & (uint16_t)DRV8301_CTRL2_GAIN_BITS); Spi_8301_Vars->Ctrl_Reg_2.DC_CAL_CH1p2 = (DRV8301_DcCalMode_e)(drvDataNew & (uint16_t)(DRV8301_CTRL2_DC_CAL_1_BITS | DRV8301_CTRL2_DC_CAL_2_BITS)); Spi_8301_Vars->Ctrl_Reg_2.OC_TOFF = (DRV8301_OcOffTimeMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL2_OC_TOFF_BITS); - + Spi_8301_Vars->Ctrl_Reg_2_Value = drvDataNew; Spi_8301_Vars->RcvCmd = false; } diff --git a/Firmware/Drivers/DRV8301/drv8301.h b/Firmware/Drivers/DRV8301/drv8301.h index bdf9fa50..e3c3e3f3 100644 --- a/Firmware/Drivers/DRV8301/drv8301.h +++ b/Firmware/Drivers/DRV8301/drv8301.h @@ -412,8 +412,13 @@ typedef struct _DRV_SPI_8301_Vars_t_ DRV_SPI_8301_Stat2_t_ Stat_Reg_2; DRV_SPI_8301_CTRL1_t_ Ctrl_Reg_1; DRV_SPI_8301_CTRL2_t_ Ctrl_Reg_2; + uint16_t Stat_Reg_1_Value; + uint16_t Stat_Reg_2_Value; + uint16_t Ctrl_Reg_1_Value; + uint16_t Ctrl_Reg_2_Value; bool SndCmd; bool RcvCmd; + }DRV_SPI_8301_Vars_t; diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 35e5651d..7c84e00c 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -131,6 +131,13 @@ const Endpoint endpoints[] = { Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), Endpoint::close_tree(), + Endpoint::make_object("gate_driver"), + Endpoint::make_property("drv_error", reinterpret_cast(&motors[0].drv_fault)), + Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)), + Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)), + Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)), + Endpoint::make_property("ctrl_reg_2", (&motors[0].gate_driver_regs.Ctrl_Reg_2_Value)), + Endpoint::close_tree(), Endpoint::make_object("encoder"), Endpoint::make_property("phase", const_cast(&motors[0].encoder.phase)), Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos), @@ -188,6 +195,13 @@ const Endpoint endpoints[] = { Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), Endpoint::close_tree(), + Endpoint::make_object("gate_driver"), + Endpoint::make_property("drv_error", reinterpret_cast(&motors[1].drv_fault)), + Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)), + Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)), + Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)), + Endpoint::make_property("ctrl_reg_2", (&motors[1].gate_driver_regs.Ctrl_Reg_2_Value)), + Endpoint::close_tree(), Endpoint::make_object("encoder"), Endpoint::make_property("phase", const_cast(&motors[1].encoder.phase)), Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos), diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index c49b95ea..d1e1dd66 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (2048 * 4) // Default resolution of CUI-AMT102 encoder +#define ENCODER_CPR (1000 * 4) // Default resolution of CUI-AMT102 encoder #define POLE_PAIRS 7 // This value is correct for N5065 motors and Turnigy SK3 series. const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); @@ -59,6 +59,7 @@ Motor_t motors[] = { .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, + .drv_fault = DRV8301_FaultType_NoFault, .pos_setpoint = 0.0f, .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, @@ -167,6 +168,7 @@ Motor_t motors[] = { .enable_step_dir = false, //auto enabled after calibration .counts_per_step = 2.0f, .error = ERROR_NO_ERROR, + .drv_fault = DRV8301_FaultType_NoFault, .pos_setpoint = 0.0f, .pos_gain = 20.0f, // [(counts/s) / counts] .vel_setpoint = 0.0f, @@ -1291,6 +1293,14 @@ void control_motor_loop(Motor_t* motor) { } if (check_DRV_fault(motor)) { motor->error = ERROR_DRV_FAULT; + + // Update DRV Fault Code + motor->drv_fault = DRV8301_getFaultType(&motor->gate_driver); + + // Update/Cache all SPI device registers + DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; + local_regs->RcvCmd = true; + DRV8301_readData(&motor->gate_driver, local_regs); break; } update_rotor(motor); diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 9045b1d9..337c933c 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -137,6 +137,7 @@ typedef struct { bool enable_step_dir; float counts_per_step; Error_t error; + DRV8301_FaultType_e drv_fault; float pos_setpoint; float pos_gain; float vel_setpoint; diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index b6fbd0d5..032a996a 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -2,6 +2,7 @@ #include #include #include +#include static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; @@ -183,3 +184,23 @@ uint32_t timeout_to_deadline(uint32_t timeout_ms) { uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency); return now_ms + timeout_ms; } + +// @brief: Returns number of microseconds since system startup +uint32_t micros(void) { + uint32_t usTicks = HAL_RCC_GetSysClockFreq() / 1000000; + register uint32_t ms, cycle_cnt; + do { + ms = HAL_GetTick(); + cycle_cnt = SysTick->VAL; + } while (ms != HAL_GetTick()); + return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; +} + +// @brief: Busy wait delay for given amount of microseconds (us) +void delay_us(uint32_t us) +{ + uint32_t start = micros(); + while (micros() - start < (uint32_t) us) { + __ASM("nop"); + } +} \ No newline at end of file diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 92436b74..004d00cf 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -91,6 +91,10 @@ int mod(int dividend, int divisor); uint32_t deadline_to_timeout(uint32_t deadline_ms); uint32_t timeout_to_deadline(uint32_t timeout_ms); +uint32_t micros(void); + +void delay_us(uint32_t us); + #ifdef __cplusplus } #endif diff --git a/tools/drv_status.py b/tools/drv_status.py new file mode 100644 index 00000000..5f16575f --- /dev/null +++ b/tools/drv_status.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +""" +Example usage of the ODrive python library to monitor and control ODrive devices +""" + +from __future__ import print_function + +import odrive.core +import time +import math + +# Find a connected ODrive (this will block until you connect one) +my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print) + +# Print DRV device regs for Motor 0 +fault = my_drive.motor0.gate_driver.drv_error +status_reg_1 = my_drive.motor0.gate_driver.status_reg_1 +status_reg_2 = my_drive.motor0.gate_driver.status_reg_2 +ctrl_reg_1 = my_drive.motor0.gate_driver.ctrl_reg_1 +ctrl_reg_2 = my_drive.motor0.gate_driver.ctrl_reg_2 + +print("DRV Fault Code: " + str(fault)) +print("Status Reg 1: " + str(status_reg_1) + " (" + format(status_reg_1, '#010b') + ")") +print("Status Reg 2: " + str(status_reg_2) + " (" + format(status_reg_2, '#010b') + ")") +print("Control Reg 1: " + str(ctrl_reg_1) + " (" + format(ctrl_reg_1, '#010b') + ")") +print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#010b') + ")") + + From aee5398ba2e5f0188d99cfce880f36dafb3de951 Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Wed, 21 Feb 2018 13:46:47 -0600 Subject: [PATCH 06/12] Revert CPR back to default. Fix delay block times --- Firmware/Drivers/DRV8301/drv8301.c | 6 +++--- Firmware/MotorControl/low_level.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/Drivers/DRV8301/drv8301.c b/Firmware/Drivers/DRV8301/drv8301.c index 765d7d49..0e90a652 100644 --- a/Firmware/Drivers/DRV8301/drv8301.c +++ b/Firmware/Drivers/DRV8301/drv8301.c @@ -592,16 +592,16 @@ void DRV8301_writeSpi(DRV8301_Handle handle, const DRV8301_RegName_e regName,con { // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_RESET); - delay_us(5); + delay_us(1); // Do blocking write uint16_t controlword = (uint16_t)DRV8301_buildCtrlWord(DRV8301_CtrlMode_Write, regName, data); HAL_SPI_Transmit(handle->spiHandle, (uint8_t*)(&controlword), 1, 1000); - delay_us(5); + delay_us(1); // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_SET); - delay_us(5); + delay_us(1); return; } // end of DRV8301_writeSpi() function diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index d1e1dd66..280bc6bf 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -34,7 +34,7 @@ float vbus_voltage = 12.0f; // TODO stick parameter into struct -#define ENCODER_CPR (1000 * 4) // Default resolution of CUI-AMT102 encoder +#define ENCODER_CPR (2048 * 4) // Default resolution of CUI-AMT102 encoder #define POLE_PAIRS 7 // This value is correct for N5065 motors and Turnigy SK3 series. const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR); From f75799fd3ceb162ab2f4580ce57ec253a5791f7e Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Wed, 21 Feb 2018 14:23:12 -0600 Subject: [PATCH 07/12] Fix a naming bug on drv_error -> drv_fault --- Firmware/MotorControl/commands.cpp | 4 ++-- Firmware/MotorControl/low_level.h | 2 +- tools/drv_status.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 7c84e00c..1e9d0117 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -132,7 +132,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_error", reinterpret_cast(&motors[0].drv_fault)), + Endpoint::make_property("drv_fault", reinterpret_cast(&motors[0].drv_fault)), Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)), Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)), Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)), @@ -196,7 +196,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), Endpoint::close_tree(), Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_error", reinterpret_cast(&motors[1].drv_fault)), + Endpoint::make_property("drv_fault", reinterpret_cast(&motors[1].drv_fault)), Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)), Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)), Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)), diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 337c933c..c0345a0f 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -137,7 +137,6 @@ typedef struct { bool enable_step_dir; float counts_per_step; Error_t error; - DRV8301_FaultType_e drv_fault; float pos_setpoint; float pos_gain; float vel_setpoint; @@ -187,6 +186,7 @@ typedef struct { float current_setpoint; } set_current_setpoint_args; Anticogging_t anticogging; + DRV8301_FaultType_e drv_fault; } Motor_t; typedef struct{ diff --git a/tools/drv_status.py b/tools/drv_status.py index 5f16575f..fda6c292 100644 --- a/tools/drv_status.py +++ b/tools/drv_status.py @@ -13,7 +13,7 @@ import math my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, printer=print) # Print DRV device regs for Motor 0 -fault = my_drive.motor0.gate_driver.drv_error +fault = my_drive.motor0.gate_driver.drv_fault status_reg_1 = my_drive.motor0.gate_driver.status_reg_1 status_reg_2 = my_drive.motor0.gate_driver.status_reg_2 ctrl_reg_1 = my_drive.motor0.gate_driver.ctrl_reg_1 From e87b2d4a410cd39710afe5e03ae3819ee99a0445 Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Wed, 21 Feb 2018 17:46:43 -0600 Subject: [PATCH 08/12] Update Changelog.md --- Firmware/CHANGELOG.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index cd09a09d..4d6c5602 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,3 +1,10 @@ +## [0.3.4] - 2018-02-21 + +### Added +* Reporting of DRV status/control registers and fault codes +* DRV status read script +* Microsecond delay function + ## [0.3.3] - 2018-02-12 ### Added From 95d1d3ff0fa4b0edc3e24e8aaafb17aeaaa1855c Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Wed, 21 Feb 2018 18:46:51 -0500 Subject: [PATCH 09/12] Fix fabsf bracket, make variable names more descriptive --- Firmware/MotorControl/low_level.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index afc63053..f3624eef 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -793,9 +793,9 @@ bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; } - float estEnc = scan_range / elec_rad_per_enc; - float adjustedCount = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT)-init_enc_val; - if(fabsf(adjustedCount-estEnc)/estEnc > motor->encoder.encoder_calib_range) + float expected_encoder_delta = scan_range / elec_rad_per_enc; + float actual_encoder_delta_abs = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > motor->encoder.encoder_calib_range) { motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; return false; From 430e4088754e76843da96579060aee9d8c762fe3 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 23 Feb 2018 12:48:27 -0800 Subject: [PATCH 10/12] default calib speed a bit faster --- Firmware/MotorControl/low_level.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index f3624eef..40ac40ad 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -762,7 +762,7 @@ bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_h // TODO: add check_timing bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { static const float start_lock_duration = 1.0f; - static const int num_steps = 1024*4; + static const int num_steps = 1024*2; static const float dt_step = 1.0f / 500.0f; static const float scan_range = 16.0f * M_PI; const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) From dea9df65badb736d339717e7bf4477dfd21b14d4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 23 Feb 2018 12:57:58 -0800 Subject: [PATCH 11/12] update CHANGELOG.md --- Firmware/CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 2247daf8..9c2fb38d 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -2,6 +2,7 @@ Please add a note of your changes below this heading if you make a PR ### Added +* Reporting error if your encoder CPR is incorrect * Reporting of DRV status/control registers and fault codes * DRV status read script * Microsecond delay function From 4801c4a8b5b197d07db690d001aee61104c28ea7 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 23 Feb 2018 13:05:49 -0800 Subject: [PATCH 12/12] add psu brownout check to do_checks --- Firmware/MotorControl/low_level.c | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c index 263fdf79..88014cd9 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -1324,16 +1324,13 @@ bool check_DRV_fault(Motor_t* motor) { //Returns true if everything is OK (no fault) bool check_PSU_brownout(Motor_t* motor) { - if(vbus_voltage < motor->dc_bus_brownout_trip_level) { - motor->error = ERROR_DC_BUS_BROWNOUT; + if(vbus_voltage < motor->dc_bus_brownout_trip_level) return false; - } return true; } // Returns true if everything is ok. Sets motor->error and returns false otherwise. bool do_checks(Motor_t* motor) { - // Checks if (!check_DRV_fault(motor)) { motor->error = ERROR_DRV_FAULT; // Update DRV Fault Code @@ -1344,6 +1341,10 @@ bool do_checks(Motor_t* motor) { DRV8301_readData(&motor->gate_driver, local_regs); return false; } + if (!check_PSU_brownout(motor)) { + motor->error = ERROR_DC_BUS_BROWNOUT; + return false; + } return true; }