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

Merge branch 'sam_refactoring' into sam_testing

Browse Source
This commit is contained in:
Samuel Sadok
2018-04-05 22:24:52 -07:00
parent 2d681f8b99 99c5649105
commit 8618549753
25 changed files with 372 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.vscode/c_cpp_properties.json
Vendored
+3
View File
@@ -26,6 +26,7 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -61,6 +62,7 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -103,6 +105,7 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch
+3 -11
View File
@@ -4,22 +4,14 @@ Date: Mon, 12 Mar 2018 23:49:32 -0700
Subject: [PATCH] expose correct serial number on USB
---
Firmware/Board/v3/Src/usbd_desc.c | 16 +++++++++-------
1 file changed, 9 insertions(+), 7 deletions(-)
Firmware/Board/v3/Src/usbd_desc.c | 15 +++++++++-------
1 file changed, 8 insertions(+), 7 deletions(-)
diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c
index b9c7bd0..94dc49b 100644
--- a/Firmware/Board/v3/Src/usbd_desc.c
+++ b/Firmware/Board/v3/Src/usbd_desc.c
@@ -51,6 +51,7 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
+#include "commands.h"
/* USER CODE BEGIN INCLUDE */
@@ -327,14 +328,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l
@@ -327,14 +327,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l
*/
uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{
Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch
+64
View File
@@ -0,0 +1,64 @@
From 510ead2b159e1d8116e5241066c54a7bf8b7bfbe Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 26 Mar 2018 15:29:44 -0700
Subject: [PATCH] FreeRTOS constness fixes
- make thread names const char *
- make thread argument non-const void*
---
.../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 6 +++---
Firmware/Board/v3/Src/freertos.c | 4 ++--
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
index 09cdf27..754be24 100644
--- a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
+++ b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
@@ -270,11 +270,11 @@ typedef enum {
/// Entry point of a thread.
/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
-typedef void (*os_pthread) (void const *argument);
+typedef void (*os_pthread) (void *argument);
/// Entry point of a timer call back function.
/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
-typedef void (*os_ptimer) (void const *argument);
+typedef void (*os_ptimer) (void *argument);
// >>> the following data type definitions may shall adapted towards a specific RTOS
@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t;
/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef struct os_thread_def {
- char *name; ///< Thread name
+ const char *name; ///< Thread name
os_pthread pthread; ///< start address of thread function
osPriority tpriority; ///< initial thread priority
uint32_t instances; ///< maximum number of instances of that thread function
diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c
index 6eaea82..b247994 100644
--- a/Firmware/Board/v3/Src/freertos.c
+++ b/Firmware/Board/v3/Src/freertos.c
@@ -75,7 +75,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE];
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
-void StartDefaultTask(void const * argument);
+void StartDefaultTask(void * argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
@@ -147,7 +147,7 @@ void MX_FREERTOS_Init(void) {
}
/* StartDefaultTask function */
-void StartDefaultTask(void const * argument)
+void StartDefaultTask(void * argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
--
2.16.2
Firmware/Board/v3/Inc/FreeRTOSConfig.h
+1 -1
View File
@@ -107,8 +107,8 @@
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
#define configCHECK_FOR_STACK_OVERFLOW 1
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
Vendored
+1 -1
View File
@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t;
/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef struct os_thread_def {
const char *name; ///< Thread name
const char *name; ///< Thread name
os_pthread pthread; ///< start address of thread function
osPriority tpriority; ///< initial thread priority
uint32_t instances; ///< maximum number of instances of that thread function
Firmware/Board/v3/Odrive.ioc
+4 -2
View File
@@ -102,9 +102,11 @@ Dma.UART4_TX.1.PeriphInc=DMA_PINC_DISABLE
Dma.UART4_TX.1.Priority=DMA_PRIORITY_LOW
Dma.UART4_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
FREERTOS.FootprintOK=true
FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark=1
FREERTOS.INCLUDE_vTaskDelayUntil=1
FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK
FREERTOS.Tasks01=defaultTask,-3,256,StartDefaultTask,Default
FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_uxTaskGetStackHighWaterMark
FREERTOS.Tasks01=defaultTask,0,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1
FREERTOS.configTOTAL_HEAP_SIZE=65536
File.Version=6
KeepUserPlacement=true
Firmware/Board/v3/Src/freertos.c
+10
View File
@@ -85,6 +85,16 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* USER CODE END FunctionPrototypes */
/* Hook prototypes */
void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName);
/* USER CODE BEGIN 4 */
__weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName)
{
/* Run time stack overflow checking is performed if
configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
called if a stack overflow is detected. */
}
/* USER CODE END 4 */
/* Init FreeRTOS */
Firmware/Board/v3/Src/stm32f4xx_it.c
+5 -1
View File
@@ -38,11 +38,15 @@
/* USER CODE BEGIN 0 */
#include "freertos_vars.h"
#include "low_level.h"
#include <stdbool.h>
typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected);
void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback);
// TODO: move somewhere else
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
Firmware/Board/v3/Src/usbd_desc.c
+1 -2
View File
@@ -51,10 +51,9 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
#include "communication.h"
/* USER CODE BEGIN INCLUDE */
#include "communication.h"
/* USER CODE END INCLUDE */
/* Private typedef -----------------------------------------------------------*/
Firmware/MotorControl/axis.cpp
+1 -3
View File
@@ -53,9 +53,7 @@ void Axis::signal_current_meas() {
// @brief Blocks until a current measurement is completed
// @returns True on success, false otherwise
bool Axis::wait_for_current_meas() {
if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal)
return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
return true;
return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal;
}
static void step_cb_wrapper(void* ctx) {
Firmware/MotorControl/axis.hpp
+8 -8
View File
@@ -106,7 +106,8 @@ public:
error_ = ERROR_MOTOR_FAILED;
break;
}
if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) {
if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) {
// motor got disarmed in something other than the idle loop
error_ = ERROR_CONTROL_LOOP_TIMEOUT;
break;
}
@@ -121,8 +122,12 @@ public:
++loop_counter_;
// Wait until the current measurement interrupt fires
if (!wait_for_current_meas()) { // error set by function call
motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases
if (!wait_for_current_meas()) {
// maybe the interrupt handler is dead, let's be
// safe and float the phases
safety_critical_disarm_motor_pwm(motor_);
update_brake_current();
error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT;
break;
}
}
@@ -148,10 +153,6 @@ public:
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler
// whenever there's no active control loop that
// sets the timings. The flag must be explicitly
// cleared by a call to motors.arm().
bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
@@ -162,7 +163,6 @@ public:
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_),
make_protocol_property("enable_step_dir", &enable_step_dir_),
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
Firmware/MotorControl/communication.cpp
+1
View File
@@ -202,6 +202,7 @@ static inline auto make_obj_tree() {
make_protocol_ro_property("fw_version_minor", &fw_version_minor),
make_protocol_ro_property("fw_version_revision", &fw_version_revision),
make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased),
make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_),
make_protocol_object("config",
make_protocol_property("brake_resistance", &board_config.brake_resistance),
// TODO: changing this currently requires a reboot - fix this
Firmware/MotorControl/controller.cpp
+1 -1
View File
@@ -96,7 +96,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)];
Iq += anticogging_.cogging_map[mod(static_cast<int>(pos_estimate), axis_->encoder_.config_.cpr)];
}
float v_err = vel_des - vel_estimate;
Firmware/MotorControl/encoder.cpp
+8 -4
View File
@@ -80,7 +80,8 @@ bool Encoder::run_index_search() {
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);
// continue until the index is found
@@ -115,7 +116,8 @@ bool Encoder::run_offset_calibration() {
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f);
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
@@ -131,7 +133,8 @@ bool Encoder::run_offset_calibration() {
float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
@@ -169,7 +172,8 @@ bool Encoder::run_offset_calibration() {
float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
Firmware/MotorControl/encoder.hpp
+1 -1
View File
@@ -16,7 +16,7 @@ struct EncoderConfig_t {
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once
// index search succeeds
float calib_range = 0.02;
float calib_range = 0.02f;
};
class Encoder {
Firmware/MotorControl/legacy_commands.h
+3 -1
View File
@@ -6,7 +6,9 @@ extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "low_level.h"
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
/* Exported types ------------------------------------------------------------*/
typedef enum {
Firmware/MotorControl/low_level.cpp
+189 -28
View File
@@ -7,8 +7,6 @@
#define ARM_MATH_CM4
#include <arm_math.h>
#include <low_level.h>
#include <cmsis_os.h>
#include <math.h>
#include <stdint.h>
@@ -34,9 +32,166 @@
// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
bool brake_resistor_armed_ = false;
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* CPU critical section helpers ----------------------------------------------*/
static inline uint8_t cpu_enter_critical() {
uint8_t status_register;
asm (
"MRS R0, PRIMASK\n\t"
"CPSID I\n\t"
"STRB R0, %[output]"
: [output] "=m" (status_register) :: "r0"
);
return status_register;
}
static inline void cpu_exit_critical(uint8_t status_register) {
asm (
"ldrb r0, %[input]\n\t"
"msr PRIMASK,r0;\n\t"
::[input] "m" (status_register) : "r0"
);
}
/* Safety critical functions -------------------------------------------------*/
/*
* This section contains all accesses to safety critical hardware registers.
* Specifically, these registers:
* Motor0 PWMs:
* Timer1.MOE (master output enabled)
* Timer1.CCR1 (counter compare register 1)
* Timer1.CCR2 (counter compare register 2)
* Timer1.CCR3 (counter compare register 3)
* Motor1 PWMs:
* Timer8.MOE (master output enabled)
* Timer8.CCR1 (counter compare register 1)
* Timer8.CCR2 (counter compare register 2)
* Timer8.CCR3 (counter compare register 3)
* Brake resistor PWM:
* Timer2.CCR3 (counter compare register 3)
* Timer2.CCR4 (counter compare register 4)
*
* The following assumptions are made:
* - The hardware operates as described in the datasheet:
* http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
* This assumption also requires for instance that there are no radiation
* caused hardware errors.
* - After startup, all variables used in this section are exclusively modified
* by the code in this section (this excludes function parameters)
* This assumption also requires that there is no memory corruption.
* - This code is compiled by a C standard compliant compiler.
*
* Furthermore:
* - Between calls to safety_critical_arm_motor_pwm and
* safety_critical_disarm_motor_pwm the motor's Ibus current is
* set to the correct value and update_brake_resistor is called
* at a high rate.
*/
// @brief Kicks off the arming process of the motor.
// All calls to this function must clearly originate
// from user input.
void safety_critical_arm_motor_pwm(Motor& motor) {
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
}
cpu_exit_critical(sr);
}
// @brief Disarms the motor PWM.
// After calling this function, it is guaranteed that all three
// motor phases are floating and will not be enabled again until
// safety_critical_arm_motor_phases is called.
void safety_critical_disarm_motor_pwm(Motor& motor) {
uint8_t sr = cpu_enter_critical();
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer);
cpu_exit_critical(sr);
}
// @brief Updates the phase timings unless the motor is disarmed.
//
// If this is called at a rate higher than the motor's timer period,
// the actual PMW timings on the pins can be undefined for up to one
// timer period.
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) {
uint8_t sr = cpu_enter_critical();
if (!brake_resistor_armed_) {
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
}
motor.hw_config_.timer->Instance->CCR1 = timings[0];
motor.hw_config_.timer->Instance->CCR2 = timings[1];
motor.hw_config_.timer->Instance->CCR3 = timings[2];
if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) {
// timings were just loaded into the timer registers
// the timer register are buffered, so they won't have an effect
// on the output just yet so we need to wait until the next
// interrupt before we actually enable the output
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE;
} else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) {
// now we waited long enough. Enter armed state and
// enable the actual PWM outputs.
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
__HAL_TIM_MOE_ENABLE(motor.hw_config_.timer); // enable pwm outputs
} else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) {
// nothing to do, PWM is running, all good
} else {
// unknown state oh no
safety_critical_disarm_motor_pwm(motor);
}
cpu_exit_critical(sr);
}
// @brief Arms the brake resistor
void safety_critical_arm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
cpu_exit_critical(sr);
}
// @brief Disarms the brake resistor and by extension
// all motor PWM outputs.
// After calling this, the brake resistor can only be armed again
// by calling safety_critical_arm_brake_resistor().
void safety_critical_disarm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
}
cpu_exit_critical(sr);
}
// @brief Updates the brake resistor PWM timings unless
// the brake resistor is disarmed.
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) {
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
htim2.Instance->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
}
cpu_exit_critical(sr);
}
/* Function implementations --------------------------------------------------*/
void start_adc_pwm() {
@@ -70,6 +225,12 @@ void start_adc_pwm() {
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
// Disarm motors and arm brake resistor
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
}
safety_critical_arm_brake_resistor();
}
void start_pwm(TIM_HandleTypeDef* htim) {
@@ -137,13 +298,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
htim_b->Instance->BDTR |= MOE_store_b;
}
// @brief Floats ALL phases immediately and sets the brake current to 0.
void disable_all_pwms(Motor::Error_t error) {
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error_t error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->motor_.disarm();
safety_critical_disarm_motor_pwm(axes[i]->motor_);
axes[i]->motor_.error_ = error;
}
safety_critical_disarm_brake_resistor();
}
//--------------------------------
@@ -166,7 +329,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// Ensure ADCs are expected ones to simplify the logic below
if (!(hadc == &hadc2 || hadc == &hadc3)) {
disable_all_pwms(Motor::ERROR_ADC_FAILED);
low_level_fault(Motor::ERROR_ADC_FAILED);
return;
};
@@ -189,18 +352,17 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// Load next timings for the motor that we're not currently sampling
if (update_timings) {
if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) {
other_axis.motor_.next_timings_valid_ = false;
other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0];
other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1];
other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2];
__HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs
update_brake_current();
} else {
if (!other_axis.motor_.next_timings_valid_) {
// the motor control loop failed to update the timings in time
// we must assume that it died and therefore float all phases
other_axis.motor_.disarm();
safety_critical_disarm_motor_pwm(other_axis.motor_);
} else {
other_axis.motor_.next_timings_valid_ = false;
safety_critical_apply_motor_pwm_timings(
other_axis.motor_, other_axis.motor_.next_timings_
);
}
update_brake_current();
}
// Check the timing of the sequencing
@@ -248,7 +410,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
void update_brake_current() {
float Ibus_sum = 0.0f;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
}
float brake_current = -Ibus_sum;
// Clip negative values to 0.0f
@@ -256,16 +420,13 @@ void update_brake_current() {
float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage;
// Duty limit at 90% to allow bootstrap caps to charge
if (brake_duty > 0.9f) brake_duty = 0.9f;
int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty);
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
htim2.Instance->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
// If brake_duty is NaN, this expression will also evaluate to true
if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) {
int high_on = static_cast<int>(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
} else {
safety_critical_disarm_brake_resistor();
}
}
Firmware/MotorControl/low_level.h
+13 -1
View File
@@ -2,6 +2,10 @@
#ifndef __LOW_LEVEL_H
#define __LOW_LEVEL_H
#ifndef __ODRIVE_MAIN_HPP
#error "This file should not be included directly. Include odrive_main.hpp instead."
#endif
#ifdef __cplusplus
extern "C" {
#endif
@@ -17,10 +21,18 @@ extern "C" {
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
//Note: to control without feed forward, set feed forward terms to 0.0f.
void safety_critical_arm_motor_pwm(Motor& motor);
void safety_critical_disarm_motor_pwm(Motor& motor);
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]);
void safety_critical_arm_brake_resistor();
void safety_critical_disarm_brake_resistor();
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on);
// called from STM platform code
extern "C" {
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
}
// Initalisation
void start_adc_pwm();
Firmware/MotorControl/motor.cpp
+22 -27
View File
@@ -39,25 +39,12 @@ bool Motor::arm() {
// that we have exactly one full interrupt period until the third trigger. This gives
// the control loop the correct time quota to set up modulation timings.
if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas()))
return false;
return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
next_timings_valid_ = false;
axis_->missed_control_deadline_ = false;
safety_critical_arm_motor_pwm(*this);
return true;
}
// @brief Floats the phases of this motor immediately and updates
// the brake current accordingly.
void Motor::disarm() {
// disable pwm
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer);
// set this motor's contribution to 0
current_control_.Ibus = 0.0f;
update_brake_current();
// ensure the PWM is not re-enabled without the state machine explicitly
// calling motor.arm()
axis_->missed_control_deadline_ = true;
}
// @brief Tune the current controller based on phase resistance and inductance
// This should be invoked whenever one of these values changes.
// TODO: allow update on user-request or update automatically via hooks
@@ -167,7 +154,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) {
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
@@ -178,7 +165,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
// Test voltage along phase A
enqueue_voltage_timings(test_voltage, 0.0f);
if (!enqueue_voltage_timings(test_voltage, 0.0f))
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_R);
return ++i < num_test_cycles;
@@ -187,7 +175,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
return false;
//// De-energize motor
//enqueue_voltage_timings(motor, 0.0f, 0.0f);
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
// return false; // error set inside enqueue_voltage_timings
float R = test_voltage / test_current;
config_.phase_resistance = R;
@@ -205,7 +194,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
Ialphas[i] += -current_meas_.phB - current_meas_.phC;
// Test voltage along phase A
enqueue_voltage_timings(test_voltages[i], 0.0f);
if (!enqueue_voltage_timings(test_voltages[i], 0.0f))
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_L);
return ++t < (num_cycles << 1);
@@ -214,7 +204,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
return false;
//// De-energize motor
//enqueue_voltage_timings(motor, 0.0f, 0.0f);
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
// return false; // error set inside enqueue_voltage_timings
float v_L = 0.5f * (voltage_high - voltage_low);
// Note: A more correct formula would also take into account that there is a finite timestep.
@@ -251,21 +242,25 @@ bool Motor::run_calibration() {
return true;
}
void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
float tA, tB, tC;
SVM(mod_alpha, mod_beta, &tA, &tB, &tC);
if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
return error_ = ERROR_NUMERICAL, false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_valid_ = true;
return true;
}
void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float mod_alpha = vfactor * v_alpha;
float mod_beta = vfactor * v_beta;
enqueue_modulation_timings(mod_alpha, mod_beta);
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false;
log_timing(TIMING_LOG_FOC_VOLTAGE);
return true;
}
// TODO: This doesn't update brake current
@@ -275,8 +270,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
float s = arm_sin_f32(phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
enqueue_voltage_timings(v_alpha, v_beta);
return true;
return enqueue_voltage_timings(v_alpha, v_beta);
}
bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
@@ -336,7 +330,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
ictrl->final_v_beta = mod_to_V * mod_beta;
// Apply SVM
enqueue_modulation_timings(mod_alpha, mod_beta);
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false; // error set inside enqueue_modulation_timings
log_timing(TIMING_LOG_FOC_CURRENT);
return true;
Firmware/MotorControl/motor.hpp
+15 -2
View File
@@ -60,6 +60,8 @@ public:
ERROR_ADC_FAILED,
ERROR_DRV_FAULT,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE,
ERROR_BRAKE_CURRENT_OUT_OF_RANGE,
ERROR_NUMERICAL
};
enum TimingLog_t {
@@ -75,6 +77,13 @@ public:
TIMING_LOG_NUM_SLOTS
};
enum ArmedState_t {
ARMED_STATE_DISARMED,
ARMED_STATE_WAITING_FOR_TIMINGS,
ARMED_STATE_WAITING_FOR_UPDATE,
ARMED_STATE_ARMED,
};
Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config);
@@ -94,8 +103,8 @@ public:
bool measure_phase_resistance(float test_current, float max_voltage);
bool measure_phase_inductance(float voltage_low, float voltage_high);
bool run_calibration();
void enqueue_modulation_timings(float mod_alpha, float mod_beta);
void enqueue_voltage_timings(float v_alpha, float v_beta);
bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
bool enqueue_voltage_timings(float v_alpha, float v_beta);
bool FOC_voltage(float v_d, float v_q, float phase);
bool FOC_current(float Id_des, float Iq_des, float phase);
bool update(float current_setpoint, float phase);
@@ -120,6 +129,9 @@ public:
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
// Do not write to this variable directly!
// It is for exclusive use by the safety_critical_... functions.
ArmedState_t armed_state_ = ARMED_STATE_DISARMED;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
@@ -144,6 +156,7 @@ public:
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("armed_state", &armed_state_),
make_protocol_ro_property("is_calibrated", &is_calibrated_),
make_protocol_ro_property("current_meas_phB", &current_meas_.phB),
make_protocol_ro_property("current_meas_phC", &current_meas_.phC),
Firmware/MotorControl/odrive_main.hpp
+2
View File
@@ -26,6 +26,7 @@ struct BoardConfig_t {
};
class Axis;
class Motor;
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
@@ -33,6 +34,7 @@ class Axis;
static const float current_meas_period = CURRENT_MEAS_PERIOD;
static const int current_meas_hz = CURRENT_MEAS_HZ;
extern float vbus_voltage;
extern bool brake_resistor_armed_;
extern const float elec_rad_per_enc;
extern BoardConfig_t board_config;
Firmware/MotorControl/protocol.cpp
+1 -1
View File
@@ -1,7 +1,7 @@
/* Includes ------------------------------------------------------------------*/
#include "low_level.h"
//#include "low_level.h"
#include "protocol.hpp"
#include <memory>
Firmware/MotorControl/utils.c
+6 -10
View File
@@ -120,16 +120,12 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
}
}
int retval = 0;
if (
*tA < 0.0f
|| *tA > 1.0f
|| *tB < 0.0f
|| *tB > 1.0f
|| *tC < 0.0f
|| *tC > 1.0f
) retval = -1;
return retval;
// if any of the results becomes NaN, result_valid will evaluate to false
int result_valid =
*tA >= 0.0f && *tA <= 1.0f
&& *tB >= 0.0f && *tB <= 1.0f
&& *tC >= 0.0f && *tC <= 1.0f;
return result_valid ? 0 : -1;
}
//beware of inserting large angles!
Firmware/Tupfile.lua
+6 -1
View File
@@ -63,6 +63,11 @@ if tup.getconfig("STEP_DIR") == "y" then
end
end
-- Compiler settings
if tup.getconfig("STRICT") == "true" then
FLAGS += '-Werror'
end
-- C-specific flags
FLAGS += '-D__weak="__attribute__((weak))"'
@@ -74,7 +79,7 @@ FLAGS += '-mthumb'
FLAGS += '-mcpu=cortex-m4'
FLAGS += '-mfpu=fpv4-sp-d16'
FLAGS += '-mfloat-abi=hard'
FLAGS += { '-Wall', '-fdata-sections', '-ffunction-sections'}
FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
-- debug build
FLAGS += '-g -gdwarf-2'
Firmware/build.sh
+3
View File
@@ -8,6 +8,9 @@ set -euo pipefail
THIS_DIR="$(dirname "$0")"
cd "$THIS_DIR"
# Treat warnings as errors
export CONFIG_STRICT=true
# Write all environment variables that start with "CONFIG_" to tup.config
rm -rdf build
mkdir -p build