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

Add microsecond delay functions.

Browse Source 
Add DRV register/fault status reporting.
This commit is contained in:
Quincy Jones
2018-02-21 13:41:42 -06:00
parent f6cb9585ba
commit ef0f8dbf85
8 changed files with 98 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/Drivers/DRV8301/drv8301.c
+14 -9
View File
@@ -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;
}
Firmware/Drivers/DRV8301/drv8301.h
+5
View File
@@ -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;
Firmware/MotorControl/commands.cpp
+14
View File
@@ -131,6 +131,13 @@ const Endpoint endpoints[] = {
Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured),
Endpoint::make_property("Ibus", const_cast<const float*>(&motors[0].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("gate_driver"),
Endpoint::make_property("drv_error", reinterpret_cast<int32_t*>(&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<const float*>(&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<const float*>(&motors[1].current_control.Ibus)),
Endpoint::close_tree(),
Endpoint::make_object("gate_driver"),
Endpoint::make_property("drv_error", reinterpret_cast<int32_t*>(&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<const float*>(&motors[1].encoder.phase)),
Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos),
Firmware/MotorControl/low_level.c
+11 -1
View File
@@ -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);
Firmware/MotorControl/low_level.h
+1
View File
@@ -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;
Firmware/MotorControl/utils.c
+21
View File
@@ -2,6 +2,7 @@
#include <utils.h>
#include <math.h>
#include <cmsis_os.h>
#include <stm32f4xx_hal.h>
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");
}
}
Firmware/MotorControl/utils.h
+4
View File
@@ -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
tools/drv_status.py
+28
View File
@@ -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') + ")")