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 diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index c18f9960..9c2fb38d 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -1,6 +1,12 @@ ## UNRELEASED 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 + ## [0.3.4] - 2018-02-13 ### Fixed diff --git a/Firmware/Drivers/DRV8301/drv8301.c b/Firmware/Drivers/DRV8301/drv8301.c index db3a8081..0e90a652 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(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); - osDelay(1); + delay_us(1); // Actuate chipselect HAL_GPIO_WritePin(handle->nCSgpioHandle, handle->nCSgpioNumber, GPIO_PIN_SET); - osDelay(1); + delay_us(1); 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/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/commands.cpp b/Firmware/MotorControl/commands.cpp index 3ffc70b8..87d46873 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -142,6 +142,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_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)), + 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), @@ -199,6 +206,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_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)), + 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 96826b0d..88014cd9 100644 --- a/Firmware/MotorControl/low_level.c +++ b/Firmware/MotorControl/low_level.c @@ -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, @@ -72,6 +73,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + .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, @@ -129,6 +131,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] @@ -167,6 +170,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, @@ -177,6 +181,7 @@ Motor_t motors[] = { .current_setpoint = 0.0f, // [A] .calibration_current = 10.0f, // [A] .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + .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, @@ -231,6 +236,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] @@ -553,6 +559,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 +692,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 +734,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 @@ -760,22 +773,25 @@ 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*2; 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++ ?) - 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) { motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; return false; } + if (!do_checks(motor)) + return false; 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) { @@ -783,12 +799,22 @@ 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); } encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; } + + 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; + } // check direction if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { // motor same dir as encoder @@ -808,6 +834,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 +939,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 +1131,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 +1226,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 +1310,47 @@ 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) + return false; + return true; +} + +// Returns true if everything is ok. Sets motor->error and returns false otherwise. +bool do_checks(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); + return false; + } + if (!check_PSU_brownout(motor)) { + motor->error = ERROR_DC_BUS_BROWNOUT; + return false; + } + return true; +} + +bool loop_updates(Motor_t* motor) { + update_rotor(motor); + return true; } void control_motor_loop(Motor_t* motor) { @@ -1289,12 +1359,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 +1449,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..baad486e 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -50,6 +50,8 @@ typedef enum { ERROR_SPIN_UP_TIMEOUT, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_ENCODER_CPR_OUT_OF_RANGE, + ERROR_DC_BUS_BROWNOUT, } Error_t; // Note: these should be sorted from lowest level of control to @@ -119,6 +121,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; @@ -147,6 +150,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; @@ -186,6 +190,7 @@ typedef struct { float current_setpoint; } set_current_setpoint_args; Anticogging_t anticogging; + DRV8301_FaultType_e drv_fault; } Motor_t; typedef struct{ @@ -243,6 +248,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); 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..fda6c292 --- /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_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 +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') + ")") + +