mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-20 03:52:08 +08:00
582 lines
22 KiB
C++
582 lines
22 KiB
C++
/* Includes ------------------------------------------------------------------*/
|
|
|
|
#include <board.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.hpp>
|
|
|
|
#include "odrive_main.h"
|
|
|
|
/* Private defines -----------------------------------------------------------*/
|
|
|
|
// #define DEBUG_PRINT
|
|
|
|
/* Private macros ------------------------------------------------------------*/
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
/* Global constant data ------------------------------------------------------*/
|
|
constexpr float adc_full_scale = static_cast<float>(1UL << 12UL);
|
|
constexpr float adc_ref_voltage = 3.3f;
|
|
/* 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;
|
|
float ibus_ = 0.0f; // exposed for monitoring only
|
|
bool task_timers_armed = false;
|
|
bool brake_resistor_armed = false;
|
|
bool brake_resistor_saturated = false;
|
|
/* Private constant data -----------------------------------------------------*/
|
|
/* CPU critical section helpers ----------------------------------------------*/
|
|
|
|
/* 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 Floats ALL phases immediately and disarms both motors and the brake resistor.
|
|
void low_level_fault(Motor::Error error) {
|
|
// Disable all motors NOW!
|
|
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
|
safety_critical_disarm_motor_pwm(axes[i].motor_);
|
|
axes[i].motor_.error_ |= error;
|
|
}
|
|
|
|
safety_critical_disarm_brake_resistor();
|
|
}
|
|
|
|
// @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) {
|
|
uint32_t mask = cpu_enter_critical();
|
|
if (brake_resistor_armed) {
|
|
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
|
|
}
|
|
cpu_exit_critical(mask);
|
|
}
|
|
|
|
// @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.
|
|
// @returns true if the motor was in a state other than disarmed before
|
|
bool safety_critical_disarm_motor_pwm(Motor& motor) {
|
|
uint32_t mask = cpu_enter_critical();
|
|
bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
|
|
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
|
|
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.timer_);
|
|
cpu_exit_critical(mask);
|
|
return was_armed;
|
|
}
|
|
|
|
// @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]) {
|
|
uint32_t mask = cpu_enter_critical();
|
|
if (!brake_resistor_armed) {
|
|
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
|
|
}
|
|
|
|
motor.timer_->Instance->CCR1 = timings[0];
|
|
motor.timer_->Instance->CCR2 = timings[1];
|
|
motor.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.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(mask);
|
|
}
|
|
|
|
// @brief Arms the brake resistor
|
|
void safety_critical_arm_brake_resistor() {
|
|
uint32_t mask = cpu_enter_critical();
|
|
brake_resistor_armed = true;
|
|
htim2.Instance->CCR3 = 0;
|
|
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
|
|
cpu_exit_critical(mask);
|
|
}
|
|
|
|
// @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() {
|
|
uint32_t mask = 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(mask);
|
|
}
|
|
|
|
// @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) {
|
|
if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS)
|
|
low_level_fault(Motor::ERROR_BRAKE_DEADTIME_VIOLATION);
|
|
uint32_t mask = 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(mask);
|
|
}
|
|
|
|
/* Function implementations --------------------------------------------------*/
|
|
|
|
void start_adc_pwm() {
|
|
// Disarm motors
|
|
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
|
safety_critical_disarm_motor_pwm(axes[i].motor_);
|
|
}
|
|
|
|
for (Motor& motor: motors) {
|
|
// Init PWM
|
|
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
|
|
motor.timer_->Instance->CCR1 = half_load;
|
|
motor.timer_->Instance->CCR2 = half_load;
|
|
motor.timer_->Instance->CCR3 = half_load;
|
|
|
|
// Enable PWM outputs (they are still masked by MOE though)
|
|
motor.timer_->Instance->CCER |= (TIM_CCx_ENABLE << TIM_CHANNEL_1);
|
|
motor.timer_->Instance->CCER |= (TIM_CCxN_ENABLE << TIM_CHANNEL_1);
|
|
motor.timer_->Instance->CCER |= (TIM_CCx_ENABLE << TIM_CHANNEL_2);
|
|
motor.timer_->Instance->CCER |= (TIM_CCxN_ENABLE << TIM_CHANNEL_2);
|
|
motor.timer_->Instance->CCER |= (TIM_CCx_ENABLE << TIM_CHANNEL_3);
|
|
motor.timer_->Instance->CCER |= (TIM_CCxN_ENABLE << TIM_CHANNEL_3);
|
|
}
|
|
|
|
// Enable ADC and interrupts
|
|
__HAL_ADC_ENABLE(&hadc1);
|
|
__HAL_ADC_ENABLE(&hadc2);
|
|
__HAL_ADC_ENABLE(&hadc3);
|
|
// Warp field stabilize.
|
|
osDelay(2);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_JEOC);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_JEOC);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_JEOC);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_EOC);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_EOC);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_OVR);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_OVR);
|
|
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_OVR);
|
|
__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);
|
|
|
|
|
|
for (Motor& motor: motors) {
|
|
// Enable the update interrupt (used to coherently sample GPIO)
|
|
__HAL_TIM_CLEAR_IT(motor.timer_, TIM_IT_UPDATE);
|
|
__HAL_TIM_ENABLE_IT(motor.timer_, TIM_IT_UPDATE);
|
|
}
|
|
|
|
|
|
// 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);
|
|
|
|
safety_critical_arm_brake_resistor();
|
|
}
|
|
|
|
// @brief ADC1 measurements are written to this buffer by DMA
|
|
uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = { 0 };
|
|
|
|
// @brief Starts the general purpose ADC on the ADC1 peripheral.
|
|
// The measured ADC voltages can be read with get_adc_voltage().
|
|
//
|
|
// ADC1 is set up to continuously sample all channels 0 to 15 in a
|
|
// round-robin fashion.
|
|
// DMA is used to copy the measured 12-bit values to adc_measurements_.
|
|
//
|
|
// The injected (high priority) channel of ADC1 is used to sample vbus_voltage.
|
|
// This conversion is triggered by TIM1 at the frequency of the motor control loop.
|
|
void start_general_purpose_adc() {
|
|
ADC_ChannelConfTypeDef sConfig;
|
|
|
|
// Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
|
|
hadc1.Instance = ADC1;
|
|
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
|
|
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
|
|
hadc1.Init.ScanConvMode = ENABLE;
|
|
hadc1.Init.ContinuousConvMode = ENABLE;
|
|
hadc1.Init.DiscontinuousConvMode = DISABLE;
|
|
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
|
|
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
|
|
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
|
|
hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT;
|
|
hadc1.Init.DMAContinuousRequests = ENABLE;
|
|
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
|
|
if (HAL_ADC_Init(&hadc1) != HAL_OK) {
|
|
odrv.misconfigured_ = true; // TODO: this is a bit of an abuse of this flag
|
|
return;
|
|
}
|
|
|
|
// Set up sampling sequence (channel 0 ... channel 15)
|
|
sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES;
|
|
for (uint32_t channel = 0; channel < ADC_CHANNEL_COUNT; ++channel) {
|
|
sConfig.Channel = channel << ADC_CR1_AWDCH_Pos;
|
|
sConfig.Rank = channel + 1; // rank numbering starts at 1
|
|
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) {
|
|
odrv.misconfigured_ = true; // TODO: this is a bit of an abuse of this flag
|
|
return;
|
|
}
|
|
}
|
|
|
|
HAL_ADC_Start_DMA(&hadc1, reinterpret_cast<uint32_t*>(adc_measurements_), ADC_CHANNEL_COUNT);
|
|
}
|
|
|
|
// @brief Returns the ADC voltage associated with the specified pin.
|
|
// This only works if the GPIO was not used for anything else since bootup, otherwise
|
|
// it must be put to analog mode first.
|
|
// Returns NaN if the pin has no associated ADC1 channel.
|
|
//
|
|
// On ODrive 3.3 and 3.4 the following pins can be used with this function:
|
|
// GPIO_1, GPIO_2, GPIO_3, GPIO_4 and some pins that are connected to
|
|
// on-board sensors (M0_TEMP, M1_TEMP, AUX_TEMP)
|
|
//
|
|
// The ADC values are sampled in background at ~30kHz without
|
|
// any CPU involvement.
|
|
//
|
|
// Details: each of the 16 conversion takes (15+26) ADC clock
|
|
// cycles and the ADC, so the update rate of the entire sequence is:
|
|
// 21000kHz / (15+26) / 16 = 32kHz
|
|
// The true frequency is slightly lower because of the injected vbus
|
|
// measurements
|
|
float get_adc_voltage(Stm32Gpio gpio) {
|
|
const uint16_t channel = channel_from_gpio(gpio);
|
|
return get_adc_voltage_channel(channel);
|
|
}
|
|
|
|
// @brief Given a GPIO_port and pin return the associated adc_channel.
|
|
// returns UINT16_MAX if there is no adc_channel;
|
|
uint16_t channel_from_gpio(Stm32Gpio gpio) {
|
|
uint32_t channel = UINT32_MAX;
|
|
if (gpio.port_ == GPIOA) {
|
|
if (gpio.pin_mask_ == GPIO_PIN_0)
|
|
channel = 0;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_1)
|
|
channel = 1;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_2)
|
|
channel = 2;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_3)
|
|
channel = 3;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_4)
|
|
channel = 4;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_5)
|
|
channel = 5;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_6)
|
|
channel = 6;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_7)
|
|
channel = 7;
|
|
} else if (gpio.port_ == GPIOB) {
|
|
if (gpio.pin_mask_ == GPIO_PIN_0)
|
|
channel = 8;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_1)
|
|
channel = 9;
|
|
} else if (gpio.port_ == GPIOC) {
|
|
if (gpio.pin_mask_ == GPIO_PIN_0)
|
|
channel = 10;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_1)
|
|
channel = 11;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_2)
|
|
channel = 12;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_3)
|
|
channel = 13;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_4)
|
|
channel = 14;
|
|
else if (gpio.pin_mask_ == GPIO_PIN_5)
|
|
channel = 15;
|
|
}
|
|
return channel;
|
|
}
|
|
|
|
// @brief Given an adc channel return the measured voltage.
|
|
// returns NaN if the channel is not valid.
|
|
float get_adc_voltage_channel(uint16_t channel)
|
|
{
|
|
if (channel < ADC_CHANNEL_COUNT)
|
|
return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
|
|
else
|
|
return 0.0f / 0.0f; // NaN
|
|
}
|
|
|
|
//--------------------------------
|
|
// IRQ Callbacks
|
|
//--------------------------------
|
|
|
|
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
|
|
constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
|
|
// 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) {
|
|
|
|
adc_timestamp = sample_TIM13();
|
|
#define calib_tau 0.2f //@TOTO make more easily configurable
|
|
constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
|
|
|
|
// Ensure ADCs are expected ones to simplify the logic below
|
|
if (!(hadc == &hadc2 || hadc == &hadc3)) {
|
|
low_level_fault(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];
|
|
int axis_num = injected ? 0 : 1;
|
|
Axis& other_axis = injected ? axes[1] : axes[0];
|
|
bool counting_down = axis.motor_.timer_->Instance->CR1 & TIM_CR1_DIR;
|
|
bool current_meas_not_DC_CAL = !counting_down;
|
|
|
|
// Check the timing of the sequencing
|
|
if (current_meas_not_DC_CAL) {
|
|
axis.motor_.log_timing(TIMING_LOG_ADC_CB_I);
|
|
}
|
|
else {
|
|
axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC);
|
|
}
|
|
|
|
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
|
|
|
|
// TODO: this is out of place here. However when moving it somewhere
|
|
// else we have to consider the timing requirements to prevent the SPI
|
|
// transfers of axis0 and axis1 from conflicting.
|
|
// Also see comment on sync_timers.
|
|
if((current_meas_not_DC_CAL && !axis_num) ||
|
|
(axis_num && !current_meas_not_DC_CAL)){
|
|
axis.encoder_.abs_spi_start_transaction();
|
|
}
|
|
}
|
|
|
|
// Load next timings for the motor that we're not currently sampling
|
|
if (update_timings) {
|
|
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
|
|
bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_);
|
|
if (was_armed) {
|
|
other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED;
|
|
}
|
|
} 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();
|
|
}
|
|
|
|
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 && axis_num == 0 && hadc == &hadc2){
|
|
if (task_timers_armed) {
|
|
TaskTimer::sample_next = true;
|
|
task_timers_armed = false;
|
|
axes[0].task_times_.adc_cb.startTime = adc_timestamp; // Start of ADC2
|
|
}
|
|
}
|
|
|
|
if(current_meas_not_DC_CAL && axis_num == 0 && hadc == &hadc3){
|
|
axes[0].task_times_.adc_cb.stopTimer(); // End of ADC3
|
|
}
|
|
|
|
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;
|
|
}
|
|
// Prepare hall readings
|
|
// TODO move this to inside encoder update function
|
|
axis.encoder_.decode_hall_samples();
|
|
// 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() {
|
|
axes[0].task_times_.brake_update.beginTimer();
|
|
axes[1].task_times_.brake_update.beginTimer();
|
|
float Ibus_sum = 0.0f;
|
|
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
|
if (axes[i].motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
|
|
Ibus_sum += axes[i].motor_.current_control_.Ibus;
|
|
}
|
|
}
|
|
|
|
// Don't start braking until -Ibus > regen_current_allowed
|
|
float brake_current = -Ibus_sum - odrv.config_.max_regen_current;
|
|
float brake_duty = brake_current * odrv.config_.brake_resistance / vbus_voltage;
|
|
|
|
if (odrv.config_.enable_dc_bus_overvoltage_ramp && (odrv.config_.brake_resistance > 0.0f) && (odrv.config_.dc_bus_overvoltage_ramp_start < odrv.config_.dc_bus_overvoltage_ramp_end)) {
|
|
brake_duty += std::fmax((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
|
|
}
|
|
|
|
if (std::isnan(brake_duty)) {
|
|
// Shuts off all motors AND brake resistor, sets error code on all motors.
|
|
low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
|
|
return;
|
|
}
|
|
|
|
if (brake_duty >= 0.95f) {
|
|
brake_resistor_saturated = true;
|
|
}
|
|
|
|
// Duty limit at 95% to allow bootstrap caps to charge
|
|
brake_duty = std::clamp(brake_duty, 0.0f, 0.95f);
|
|
|
|
// Special handling to avoid the case 0.0/0.0 == NaN.
|
|
Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / odrv.config_.brake_resistance) : 0.0f;
|
|
|
|
ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
|
|
|
|
if (Ibus_sum > odrv.config_.dc_max_positive_current) {
|
|
low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT);
|
|
return;
|
|
}
|
|
if (Ibus_sum < odrv.config_.dc_max_negative_current) {
|
|
low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT);
|
|
return;
|
|
}
|
|
|
|
int high_on = (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);
|
|
axes[0].task_times_.brake_update.stopTimer();
|
|
axes[1].task_times_.brake_update.stopTimer();
|
|
}
|
|
|
|
|
|
/* Analog speed control input */
|
|
|
|
static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio)
|
|
{
|
|
float fraction = get_adc_voltage(get_gpio(gpio)) / 3.3f;
|
|
float value = map->min + (fraction * (map->max - map->min));
|
|
fibre::set_endpoint_from_float(map->endpoint, value);
|
|
}
|
|
|
|
static void analog_polling_thread(void *)
|
|
{
|
|
while (true) {
|
|
for (int i = 0; i < GPIO_COUNT; i++) {
|
|
struct PWMMapping_t *map = &odrv.config_.analog_mappings[i];
|
|
|
|
if (fibre::is_endpoint_ref_valid(map->endpoint))
|
|
update_analog_endpoint(map, i);
|
|
}
|
|
osDelay(10);
|
|
}
|
|
}
|
|
|
|
void start_analog_thread() {
|
|
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 512 / sizeof(StackType_t));
|
|
osThreadCreate(osThread(thread_def), NULL);
|
|
}
|