mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-21 06:23:07 +08:00
273 lines
11 KiB
C++
273 lines
11 KiB
C++
/* Includes ------------------------------------------------------------------*/
|
|
|
|
// Because of broken cmsis_os.h, we need to include arm_math first,
|
|
// otherwise chip specific defines are ommited
|
|
#include <stm32f405xx.h>
|
|
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
|
|
#define ARM_MATH_CM4
|
|
#include <arm_math.h>
|
|
|
|
#include <low_level.h>
|
|
|
|
#include <cmsis_os.h>
|
|
#include <math.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
|
|
#include <adc.h>
|
|
#include <gpio.h>
|
|
#include <main.h>
|
|
#include <spi.h>
|
|
#include <tim.h>
|
|
#include <utils.h>
|
|
|
|
#include "odrive_main.hpp"
|
|
|
|
/* Private defines -----------------------------------------------------------*/
|
|
|
|
// #define DEBUG_PRINT
|
|
|
|
/* Private macros ------------------------------------------------------------*/
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
/* Global constant data ------------------------------------------------------*/
|
|
/* Global variables ----------------------------------------------------------*/
|
|
// 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;
|
|
|
|
// TODO: Migrate to C++, clearly we are actually doing object oriented code here...
|
|
|
|
float brake_resistance = 0.47f; // [ohm]
|
|
|
|
/* Private constant data -----------------------------------------------------*/
|
|
/* Private variables ---------------------------------------------------------*/
|
|
/* Function implementations --------------------------------------------------*/
|
|
|
|
void start_adc_pwm() {
|
|
// Enable ADC and interrupts
|
|
__HAL_ADC_ENABLE(&hadc1);
|
|
__HAL_ADC_ENABLE(&hadc2);
|
|
__HAL_ADC_ENABLE(&hadc3);
|
|
// Warp field stabilize.
|
|
osDelay(2);
|
|
__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
|
|
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
|
|
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
|
|
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC);
|
|
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC);
|
|
|
|
// Ensure that debug halting of the core doesn't leave the motor PWM running
|
|
__HAL_DBGMCU_FREEZE_TIM1();
|
|
__HAL_DBGMCU_FREEZE_TIM8();
|
|
|
|
start_pwm(&htim1);
|
|
start_pwm(&htim8);
|
|
// TODO: explain why this offset
|
|
sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128);
|
|
|
|
// Motor output starts in the disabled state
|
|
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
|
|
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8);
|
|
|
|
// Start brake resistor PWM in floating output configuration
|
|
htim2.Instance->CCR3 = 0;
|
|
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
|
|
}
|
|
|
|
void start_pwm(TIM_HandleTypeDef* htim) {
|
|
// Init PWM
|
|
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
|
|
htim->Instance->CCR1 = half_load;
|
|
htim->Instance->CCR2 = half_load;
|
|
htim->Instance->CCR3 = half_load;
|
|
|
|
// This hardware obfustication layer really is getting on my nerves
|
|
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
|
|
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1);
|
|
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
|
|
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2);
|
|
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
|
|
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3);
|
|
|
|
htim->Instance->CCR4 = 1;
|
|
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
|
|
}
|
|
|
|
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
|
|
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
|
|
// Store intial timer configs
|
|
uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
|
|
uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
|
|
uint16_t CR2_store = htim_a->Instance->CR2;
|
|
uint16_t SMCR_store = htim_b->Instance->SMCR;
|
|
// Turn off output
|
|
htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE);
|
|
htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE);
|
|
// Disable both timer counters
|
|
htim_a->Instance->CR1 &= ~TIM_CR1_CEN;
|
|
htim_b->Instance->CR1 &= ~TIM_CR1_CEN;
|
|
// Set first timer to send TRGO on counter enable
|
|
htim_a->Instance->CR2 &= ~TIM_CR2_MMS;
|
|
htim_a->Instance->CR2 |= TIM_TRGO_ENABLE;
|
|
// Set Trigger Source of second timer to the TRGO of the first timer
|
|
htim_b->Instance->SMCR &= ~TIM_SMCR_TS;
|
|
htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx;
|
|
// Set 2nd timer to start on trigger
|
|
htim_b->Instance->SMCR &= ~TIM_SMCR_SMS;
|
|
htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER;
|
|
// Dir bit is read only in center aligned mode, so we clear the mode for now
|
|
uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS;
|
|
uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS;
|
|
htim_a->Instance->CR1 &= ~TIM_CR1_CMS;
|
|
htim_b->Instance->CR1 &= ~TIM_CR1_CMS;
|
|
// Set both timers to up-counting state
|
|
htim_a->Instance->CR1 &= ~TIM_CR1_DIR;
|
|
htim_b->Instance->CR1 &= ~TIM_CR1_DIR;
|
|
// Restore center aligned mode
|
|
htim_a->Instance->CR1 |= CMS_store_a;
|
|
htim_b->Instance->CR1 |= CMS_store_b;
|
|
// set counter offset
|
|
htim_a->Instance->CNT = count_offset;
|
|
htim_b->Instance->CNT = 0;
|
|
// Start Timer a
|
|
htim_a->Instance->CR1 |= (TIM_CR1_CEN);
|
|
// Restore timer configs
|
|
htim_a->Instance->CR2 = CR2_store;
|
|
htim_b->Instance->SMCR = SMCR_store;
|
|
// restore output
|
|
htim_a->Instance->BDTR |= MOE_store_a;
|
|
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) {
|
|
// Disable all motors NOW!
|
|
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
|
axes[i]->motor_.disarm();
|
|
axes[i]->motor_.error_ = error;
|
|
}
|
|
}
|
|
|
|
//--------------------------------
|
|
// IRQ Callbacks
|
|
//--------------------------------
|
|
|
|
|
|
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
|
|
static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12);
|
|
// Only one conversion in sequence, so only rank1
|
|
uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
|
|
vbus_voltage = ADCValue * voltage_scale;
|
|
}
|
|
|
|
// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
|
|
// TODO: Document how the phasing is done, link to timing diagram
|
|
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
|
|
#define calib_tau 0.2f //@TOTO make more easily configurable
|
|
static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
|
|
|
|
// Ensure ADCs are expected ones to simplify the logic below
|
|
if (!(hadc == &hadc2 || hadc == &hadc3)) {
|
|
disable_all_pwms(Motor::ERROR_ADC_FAILED);
|
|
return;
|
|
};
|
|
|
|
// Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
|
|
// Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
|
|
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
|
|
// If we are counting down, we just sampled in SVM vector 7, with zero current
|
|
Axis& axis = injected ? *axes[0] : *axes[1];
|
|
Axis& other_axis = injected ? *axes[1] : *axes[0];
|
|
bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
|
|
|
|
bool current_meas_not_DC_CAL = !counting_down;
|
|
bool update_timings = false;
|
|
if (hadc == &hadc2) {
|
|
if (&axis == axes[1] && counting_down)
|
|
update_timings = true; // update timings of M0
|
|
else if (&axis == axes[0] && !counting_down)
|
|
update_timings = true; // update timings of M1
|
|
}
|
|
|
|
// 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 {
|
|
// 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();
|
|
}
|
|
}
|
|
|
|
// Check the timing of the sequencing
|
|
axis.motor_.log_timing();
|
|
|
|
uint32_t ADCValue;
|
|
if (injected) {
|
|
ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
|
|
} else {
|
|
ADCValue = HAL_ADC_GetValue(hadc);
|
|
}
|
|
float current = axis.motor_.phase_current_from_adcval(ADCValue);
|
|
|
|
if (current_meas_not_DC_CAL) {
|
|
// ADC2 and ADC3 record the phB and phC currents concurrently,
|
|
// and their interrupts should arrive on the same clock cycle.
|
|
// We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
|
|
// Therefore we store the value from ADC2 and signal the thread that the
|
|
// measurement is ready when we receive the ADC3 measurement
|
|
|
|
// return or continue
|
|
if (hadc == &hadc2) {
|
|
axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
|
|
return;
|
|
} else {
|
|
axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
|
|
}
|
|
// Trigger axis thread
|
|
axis.signal_current_meas();
|
|
} else {
|
|
// DC_CAL measurement
|
|
if (hadc == &hadc2) {
|
|
axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
|
|
} else {
|
|
axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
|
|
}
|
|
}
|
|
}
|
|
|
|
// @brief Sums up the Ibus contribution of each motor and updates the
|
|
// brake resistor PWM accordingly.
|
|
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;
|
|
}
|
|
float brake_current = -Ibus_sum;
|
|
// Clip negative values to 0.0f
|
|
if (brake_current < 0.0f) brake_current = 0.0f;
|
|
float brake_duty = brake_current * 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;
|
|
}
|