From ef0f8dbf85c1a4d4ec69b8e2ba0543d5bc979ae1 Mon Sep 17 00:00:00 2001 From: Quincy Jones Date: Wed, 21 Feb 2018 13:41:42 -0600 Subject: [PATCH] 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') + ")") + +