*
This commit is contained in:
@@ -0,0 +1,45 @@
|
||||
|
||||
#include "acim_estimator.hpp"
|
||||
#include <board.h>
|
||||
|
||||
void AcimEstimator::update(uint32_t timestamp) {
|
||||
std::optional<float> rotor_phase = rotor_phase_src_.present();
|
||||
std::optional<float> rotor_phase_vel = rotor_phase_vel_src_.present();
|
||||
std::optional<float2D> idq = idq_src_.present();
|
||||
|
||||
if (!rotor_phase.has_value() || !rotor_phase_vel.has_value() || !idq.has_value()) {
|
||||
active_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
auto [id, iq] = *idq;
|
||||
|
||||
float dt = (float)(timestamp - last_timestamp_) / (float)TIM_1_8_CLOCK_HZ;
|
||||
last_timestamp_ = timestamp;
|
||||
|
||||
if (!active_) {
|
||||
// Skip first iteration and use it to reset state
|
||||
rotor_flux_ = 0.0f;
|
||||
phase_offset_ = 0.0f;
|
||||
active_ = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later
|
||||
// However the rotor time constant is (usually) so slow that it doesn't matter
|
||||
// So we elect to write it as if the effect is immediate, to have cleaner code
|
||||
|
||||
// acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
|
||||
float dflux_by_dt = config_.slip_velocity * (id - rotor_flux_);
|
||||
rotor_flux_ += dflux_by_dt * dt;
|
||||
float slip_velocity = config_.slip_velocity * (iq / rotor_flux_);
|
||||
// Check for issues with small denominator.
|
||||
if (is_nan(slip_velocity) || (std::abs(slip_velocity) > 0.1f / dt)) {
|
||||
slip_velocity = 0.0f;
|
||||
}
|
||||
slip_vel_ = slip_velocity; // reporting only
|
||||
|
||||
stator_phase_vel_ = *rotor_phase_vel + slip_velocity;
|
||||
phase_offset_ = wrap_pm_pi(phase_offset_ + slip_velocity * dt);
|
||||
stator_phase_ = wrap_pm_pi(*rotor_phase + phase_offset_);
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#ifndef __ACIM_ESTIMATOR_HPP
|
||||
#define __ACIM_ESTIMATOR_HPP
|
||||
|
||||
#include <component.hpp>
|
||||
#include <cmath>
|
||||
#include <autogen/interfaces.hpp>
|
||||
|
||||
class AcimEstimator : public ComponentBase {
|
||||
public:
|
||||
struct Config_t {
|
||||
float slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
|
||||
};
|
||||
|
||||
void update(uint32_t timestamp) final;
|
||||
|
||||
// Config
|
||||
Config_t config_;
|
||||
|
||||
// Inputs
|
||||
InputPort<float> rotor_phase_src_;
|
||||
InputPort<float> rotor_phase_vel_src_;
|
||||
InputPort<float2D> idq_src_;
|
||||
|
||||
// State variables
|
||||
bool active_ = false;
|
||||
uint32_t last_timestamp_ = 0;
|
||||
float rotor_flux_ = 0.0f; // [A]
|
||||
float phase_offset_ = 0.0f; // [A]
|
||||
|
||||
// Outputs
|
||||
OutputPort<float> slip_vel_ = 0.0f; // [rad/s electrical]
|
||||
OutputPort<float> stator_phase_vel_ = 0.0f; // [rad/s] rotor flux angular velocity estimate
|
||||
OutputPort<float> stator_phase_ = 0.0f; // [rad] rotor flux phase angle estimate
|
||||
};
|
||||
|
||||
#endif // __ACIM_ESTIMATOR_HPP
|
||||
@@ -0,0 +1,123 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_cos_f32.c
|
||||
* Description: Fast cosine calculation for floating-point values
|
||||
*
|
||||
* $Date: 27. January 2017
|
||||
* $Revision: V.1.5.1
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <board.h>
|
||||
#include "arm_math.h"
|
||||
#include "arm_common_tables.h"
|
||||
|
||||
/**
|
||||
* @ingroup groupFastMath
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup cos Cosine
|
||||
*
|
||||
* Computes the trigonometric cosine function using a combination of table lookup
|
||||
* and linear interpolation. There are separate functions for
|
||||
* Q15, Q31, and floating-point data types.
|
||||
* The input to the floating-point version is in radians and in the range [0 2*pi) while the
|
||||
* fixed-point Q15 and Q31 have a scaled input with the range
|
||||
* [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a
|
||||
* value of 2*pi wraps around to 0.
|
||||
*
|
||||
* The implementation is based on table lookup using 256 values together with linear interpolation.
|
||||
* The steps used are:
|
||||
* -# Calculation of the nearest integer table index
|
||||
* -# Compute the fractional portion (fract) of the table index.
|
||||
* -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
|
||||
*
|
||||
* where
|
||||
* <pre>
|
||||
* b=Table[index+0];
|
||||
* c=Table[index+1];
|
||||
* </pre>
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup cos
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Fast approximation to the trigonometric cosine function for floating-point data.
|
||||
* @param[in] x input value in radians.
|
||||
* @return cos(x).
|
||||
*/
|
||||
|
||||
float32_t our_arm_cos_f32(
|
||||
float32_t x)
|
||||
{
|
||||
float32_t cosVal, fract, in; /* Temporary variables for input, output */
|
||||
uint16_t index; /* Index variable */
|
||||
float32_t a, b; /* Two nearest output values */
|
||||
int32_t n;
|
||||
float32_t findex;
|
||||
|
||||
/* input x is in radians */
|
||||
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */
|
||||
in = x * 0.159154943092f + 0.25f;
|
||||
|
||||
/* Calculation of floor value of input */
|
||||
n = (int32_t) in;
|
||||
|
||||
/* Make negative values towards -infinity */
|
||||
if (in < 0.0f)
|
||||
{
|
||||
n--;
|
||||
}
|
||||
|
||||
/* Map input value to [0 1] */
|
||||
in = in - (float32_t) n;
|
||||
|
||||
/* Calculation of index of the table */
|
||||
findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
|
||||
index = (uint16_t)findex;
|
||||
|
||||
/* when "in" is exactly 1, we need to rotate the index down to 0 */
|
||||
if (index >= FAST_MATH_TABLE_SIZE) {
|
||||
index = 0;
|
||||
findex -= (float32_t)FAST_MATH_TABLE_SIZE;
|
||||
}
|
||||
|
||||
/* fractional value calculation */
|
||||
fract = findex - (float32_t) index;
|
||||
|
||||
/* Read two nearest values of input value from the cos table */
|
||||
a = sinTable_f32[index];
|
||||
b = sinTable_f32[index+1];
|
||||
|
||||
/* Linear interpolation process */
|
||||
cosVal = (1.0f-fract)*a + fract*b;
|
||||
|
||||
/* Return the output value */
|
||||
return (cosVal);
|
||||
}
|
||||
|
||||
/**
|
||||
* @} end of cos group
|
||||
*/
|
||||
@@ -0,0 +1,123 @@
|
||||
/* ----------------------------------------------------------------------
|
||||
* Project: CMSIS DSP Library
|
||||
* Title: arm_sin_f32.c
|
||||
* Description: Fast sine calculation for floating-point values
|
||||
*
|
||||
* $Date: 27. January 2017
|
||||
* $Revision: V.1.5.1
|
||||
*
|
||||
* Target Processor: Cortex-M cores
|
||||
* -------------------------------------------------------------------- */
|
||||
/*
|
||||
* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the License); you may
|
||||
* not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an AS IS BASIS, WITHOUT
|
||||
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include <board.h>
|
||||
#include "arm_math.h"
|
||||
#include "arm_common_tables.h"
|
||||
|
||||
/**
|
||||
* @ingroup groupFastMath
|
||||
*/
|
||||
|
||||
/**
|
||||
* @defgroup sin Sine
|
||||
*
|
||||
* Computes the trigonometric sine function using a combination of table lookup
|
||||
* and linear interpolation. There are separate functions for
|
||||
* Q15, Q31, and floating-point data types.
|
||||
* The input to the floating-point version is in radians and in the range [0 2*pi) while the
|
||||
* fixed-point Q15 and Q31 have a scaled input with the range
|
||||
* [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a
|
||||
* value of 2*pi wraps around to 0.
|
||||
*
|
||||
* The implementation is based on table lookup using 256 values together with linear interpolation.
|
||||
* The steps used are:
|
||||
* -# Calculation of the nearest integer table index
|
||||
* -# Compute the fractional portion (fract) of the table index.
|
||||
* -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
|
||||
*
|
||||
* where
|
||||
* <pre>
|
||||
* b=Table[index+0];
|
||||
* c=Table[index+1];
|
||||
* </pre>
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup sin
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Fast approximation to the trigonometric sine function for floating-point data.
|
||||
* @param[in] x input value in radians.
|
||||
* @return sin(x).
|
||||
*/
|
||||
|
||||
float32_t our_arm_sin_f32(
|
||||
float32_t x)
|
||||
{
|
||||
float32_t sinVal, fract, in; /* Temporary variables for input, output */
|
||||
uint16_t index; /* Index variable */
|
||||
float32_t a, b; /* Two nearest output values */
|
||||
int32_t n;
|
||||
float32_t findex;
|
||||
|
||||
/* input x is in radians */
|
||||
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
|
||||
in = x * 0.159154943092f;
|
||||
|
||||
/* Calculation of floor value of input */
|
||||
n = (int32_t) in;
|
||||
|
||||
/* Make negative values towards -infinity */
|
||||
if (x < 0.0f)
|
||||
{
|
||||
n--;
|
||||
}
|
||||
|
||||
/* Map input value to [0 1] */
|
||||
in = in - (float32_t) n;
|
||||
|
||||
/* Calculation of index of the table */
|
||||
findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
|
||||
index = (uint16_t)findex;
|
||||
|
||||
/* when "in" is exactly 1, we need to rotate the index down to 0 */
|
||||
if (index >= FAST_MATH_TABLE_SIZE) {
|
||||
index = 0;
|
||||
findex -= (float32_t)FAST_MATH_TABLE_SIZE;
|
||||
}
|
||||
|
||||
/* fractional value calculation */
|
||||
fract = findex - (float32_t) index;
|
||||
|
||||
/* Read two nearest values of input value from the sin table */
|
||||
a = sinTable_f32[index];
|
||||
b = sinTable_f32[index+1];
|
||||
|
||||
/* Linear interpolation process */
|
||||
sinVal = (1.0f-fract)*a + fract*b;
|
||||
|
||||
/* Return the output value */
|
||||
return (sinVal);
|
||||
}
|
||||
|
||||
/**
|
||||
* @} end of sin group
|
||||
*/
|
||||
@@ -0,0 +1,602 @@
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <functional>
|
||||
#include "gpio.h"
|
||||
|
||||
#include "odrive_main.h"
|
||||
#include "utils.hpp"
|
||||
#include "communication/interface_can.hpp"
|
||||
|
||||
Axis::Axis(int axis_num,
|
||||
uint16_t default_step_gpio_pin,
|
||||
uint16_t default_dir_gpio_pin,
|
||||
osPriority thread_priority,
|
||||
Encoder& encoder,
|
||||
SensorlessEstimator& sensorless_estimator,
|
||||
Controller& controller,
|
||||
Motor& motor,
|
||||
TrapezoidalTrajectory& trap,
|
||||
Endstop& min_endstop,
|
||||
Endstop& max_endstop,
|
||||
MechanicalBrake& mechanical_brake)
|
||||
: axis_num_(axis_num),
|
||||
default_step_gpio_pin_(default_step_gpio_pin),
|
||||
default_dir_gpio_pin_(default_dir_gpio_pin),
|
||||
thread_priority_(thread_priority),
|
||||
encoder_(encoder),
|
||||
sensorless_estimator_(sensorless_estimator),
|
||||
controller_(controller),
|
||||
motor_(motor),
|
||||
trap_traj_(trap),
|
||||
min_endstop_(min_endstop),
|
||||
max_endstop_(max_endstop),
|
||||
mechanical_brake_(mechanical_brake)
|
||||
{
|
||||
encoder_.axis_ = this;
|
||||
sensorless_estimator_.axis_ = this;
|
||||
controller_.axis_ = this;
|
||||
motor_.axis_ = this;
|
||||
trap_traj_.axis_ = this;
|
||||
min_endstop_.axis_ = this;
|
||||
max_endstop_.axis_ = this;
|
||||
mechanical_brake_.axis_ = this;
|
||||
}
|
||||
|
||||
Axis::LockinConfig_t Axis::default_calibration() {
|
||||
Axis::LockinConfig_t config;
|
||||
config.current = 10.0f; // [A]
|
||||
config.ramp_time = 0.4f; // [s]
|
||||
config.ramp_distance = 1 * M_PI; // [rad]
|
||||
config.accel = 20.0f; // [rad/s^2]
|
||||
config.vel = 40.0f; // [rad/s]
|
||||
config.finish_distance = 100.0f * 2.0f * M_PI; // [rad]
|
||||
config.finish_on_vel = false;
|
||||
config.finish_on_distance = true;
|
||||
config.finish_on_enc_idx = true;
|
||||
return config;
|
||||
}
|
||||
|
||||
Axis::LockinConfig_t Axis::default_sensorless() {
|
||||
Axis::LockinConfig_t config;
|
||||
config.current = 10.0f; // [A]
|
||||
config.ramp_time = 0.4f; // [s]
|
||||
config.ramp_distance = 1 * M_PI; // [rad]
|
||||
config.accel = 200.0f; // [rad/s^2]
|
||||
config.vel = 400.0f; // [rad/s]
|
||||
config.finish_distance = 100.0f; // [rad]
|
||||
config.finish_on_vel = true;
|
||||
config.finish_on_distance = false;
|
||||
config.finish_on_enc_idx = false;
|
||||
return config;
|
||||
}
|
||||
|
||||
static void step_cb_wrapper(void* ctx) {
|
||||
reinterpret_cast<Axis*>(ctx)->step_cb();
|
||||
}
|
||||
|
||||
bool Axis::apply_config() {
|
||||
config_.parent = this;
|
||||
decode_step_dir_pins();
|
||||
watchdog_feed();
|
||||
return true;
|
||||
}
|
||||
|
||||
void Axis::clear_config() {
|
||||
config_ = {};
|
||||
config_.step_gpio_pin = default_step_gpio_pin_;
|
||||
config_.dir_gpio_pin = default_dir_gpio_pin_;
|
||||
config_.can.node_id = axis_num_;
|
||||
}
|
||||
|
||||
static void run_state_machine_loop_wrapper(void* ctx) {
|
||||
reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
|
||||
reinterpret_cast<Axis*>(ctx)->thread_id_valid_ = false;
|
||||
}
|
||||
|
||||
// @brief Starts run_state_machine_loop in a new thread
|
||||
void Axis::start_thread() {
|
||||
osThreadDef(thread_def, run_state_machine_loop_wrapper, thread_priority_, 0, stack_size_ / sizeof(StackType_t));
|
||||
thread_id_ = osThreadCreate(osThread(thread_def), this);
|
||||
thread_id_valid_ = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Blocks until at least one complete control loop has been executed.
|
||||
*/
|
||||
bool Axis::wait_for_control_iteration() {
|
||||
osSignalWait(0x0001, osWaitForever); // this might return instantly
|
||||
osSignalWait(0x0001, osWaitForever); // this might be triggered at the
|
||||
// end of a control loop iteration
|
||||
// which was started before we entered
|
||||
// this function
|
||||
osSignalWait(0x0001, osWaitForever);
|
||||
return true;
|
||||
}
|
||||
|
||||
// step/direction interface
|
||||
void Axis::step_cb() {
|
||||
if (step_dir_active_) {
|
||||
dir_gpio_.read() ? ++steps_ : --steps_;
|
||||
controller_.input_pos_updated();
|
||||
}
|
||||
}
|
||||
|
||||
void Axis::decode_step_dir_pins() {
|
||||
step_gpio_ = get_gpio(config_.step_gpio_pin);
|
||||
dir_gpio_ = get_gpio(config_.dir_gpio_pin);
|
||||
}
|
||||
|
||||
// @brief (de)activates step/dir input
|
||||
void Axis::set_step_dir_active(bool active) {
|
||||
if (active) {
|
||||
// Subscribe to rising edges of the step GPIO
|
||||
if (!step_gpio_.subscribe(true, false, step_cb_wrapper, this)) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
|
||||
step_dir_active_ = true;
|
||||
} else {
|
||||
step_dir_active_ = false;
|
||||
|
||||
// Unsubscribe from step GPIO
|
||||
// TODO: if we change the GPIO while the subscription is active and then
|
||||
// unsubscribe then the unsubscribe is for the wrong pin.
|
||||
step_gpio_.unsubscribe();
|
||||
}
|
||||
}
|
||||
|
||||
// @brief Do axis level checks and call subcomponent do_checks
|
||||
// Returns true if everything is ok.
|
||||
bool Axis::do_checks(uint32_t timestamp) {
|
||||
// Sub-components should use set_error which will propegate to this error_
|
||||
motor_.effective_current_lim();
|
||||
motor_.do_checks(timestamp);
|
||||
|
||||
// Check for endstop presses
|
||||
if (min_endstop_.config_.enabled && min_endstop_.rose() && !(current_state_ == AXIS_STATE_HOMING)) {
|
||||
error_ |= ERROR_MIN_ENDSTOP_PRESSED;
|
||||
} else if (max_endstop_.config_.enabled && max_endstop_.rose() && !(current_state_ == AXIS_STATE_HOMING)) {
|
||||
error_ |= ERROR_MAX_ENDSTOP_PRESSED;
|
||||
}
|
||||
|
||||
return check_for_errors();
|
||||
}
|
||||
|
||||
// @brief Feed the watchdog to prevent watchdog timeouts.
|
||||
void Axis::watchdog_feed() {
|
||||
watchdog_current_value_ = get_watchdog_reset();
|
||||
}
|
||||
|
||||
// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
|
||||
bool Axis::watchdog_check() {
|
||||
if (!config_.enable_watchdog) return true;
|
||||
|
||||
// explicit check here to ensure that we don't underflow back to UINT32_MAX
|
||||
if (watchdog_current_value_ > 0) {
|
||||
watchdog_current_value_--;
|
||||
return true;
|
||||
} else {
|
||||
error_ |= ERROR_WATCHDOG_TIMER_EXPIRED;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed,
|
||||
std::function<bool(bool)> loop_cb) {
|
||||
CRITICAL_SECTION() {
|
||||
// Reset state variables
|
||||
open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f};
|
||||
open_loop_controller_.Vdq_setpoint_ = {0.0f, 0.0f};
|
||||
open_loop_controller_.phase_ = 0.0f;
|
||||
open_loop_controller_.phase_vel_ = 0.0f;
|
||||
|
||||
open_loop_controller_.max_current_ramp_ = lockin_config.current / lockin_config.ramp_time;
|
||||
open_loop_controller_.max_voltage_ramp_ = lockin_config.current / lockin_config.ramp_time;
|
||||
open_loop_controller_.max_phase_vel_ramp_ = lockin_config.accel;
|
||||
open_loop_controller_.target_current_ = motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? lockin_config.current : 0.0f;
|
||||
open_loop_controller_.target_voltage_ = motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : lockin_config.current;
|
||||
open_loop_controller_.target_vel_ = lockin_config.vel;
|
||||
open_loop_controller_.total_distance_ = 0.0f;
|
||||
|
||||
motor_.current_control_.enable_current_control_src_ = motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL;
|
||||
motor_.current_control_.Idq_setpoint_src_.connect_to(&open_loop_controller_.Idq_setpoint_);
|
||||
motor_.current_control_.Vdq_setpoint_src_.connect_to(&open_loop_controller_.Vdq_setpoint_);
|
||||
|
||||
motor_.current_control_.phase_src_.connect_to(&open_loop_controller_.phase_);
|
||||
acim_estimator_.rotor_phase_src_.connect_to(&open_loop_controller_.phase_);
|
||||
|
||||
motor_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
|
||||
motor_.current_control_.phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
|
||||
acim_estimator_.rotor_phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_);
|
||||
}
|
||||
wait_for_control_iteration();
|
||||
|
||||
motor_.arm(&motor_.current_control_);
|
||||
|
||||
bool subscribed_to_idx_once = false;
|
||||
bool success = false;
|
||||
float dir = lockin_config.vel >= 0.0f ? 1.0f : -1.0f;
|
||||
|
||||
while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_) {
|
||||
bool reached_target_vel = std::abs(open_loop_controller_.phase_vel_.any().value_or(0.0f) - lockin_config.vel) <= std::numeric_limits<float>::epsilon();
|
||||
bool reached_target_dist = open_loop_controller_.total_distance_.any().value_or(0.0f) * dir >= lockin_config.finish_distance * dir;
|
||||
|
||||
// Check if terminal condition is reached
|
||||
bool terminal_condition = (reached_target_vel && lockin_config.finish_on_vel)
|
||||
|| (reached_target_dist && lockin_config.finish_on_distance)
|
||||
|| (encoder_.index_found_ && lockin_config.finish_on_enc_idx);
|
||||
if (terminal_condition) {
|
||||
success = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// Activate index pin as soon as target velocity was reached. This is
|
||||
// to avoid hitting the index from the wrong direction.
|
||||
if (reached_target_vel && !encoder_.index_found_ && !subscribed_to_idx_once) {
|
||||
encoder_.set_idx_subscribe(true);
|
||||
subscribed_to_idx_once = true;
|
||||
}
|
||||
|
||||
if (loop_cb)
|
||||
if (!loop_cb(reached_target_vel))
|
||||
break;
|
||||
|
||||
// TODO: use new sync function instead
|
||||
asm volatile ("" ::: "memory");
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
if (!success || !remain_armed) {
|
||||
motor_.disarm();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
bool Axis::start_closed_loop_control() {
|
||||
bool sensorless_mode = config_.enable_sensorless_mode;
|
||||
|
||||
if (sensorless_mode) {
|
||||
// TODO: restart if desired
|
||||
if (!run_lockin_spin(config_.sensorless_ramp, true)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Hook up the data paths between the components
|
||||
CRITICAL_SECTION() {
|
||||
if (sensorless_mode) {
|
||||
controller_.pos_estimate_linear_src_.disconnect();
|
||||
controller_.pos_estimate_circular_src_.disconnect();
|
||||
controller_.pos_wrap_src_.disconnect();
|
||||
controller_.vel_estimate_src_.connect_to(&sensorless_estimator_.vel_estimate_);
|
||||
} else if (controller_.config_.load_encoder_axis < AXIS_COUNT) {
|
||||
Axis* ax = &axes[controller_.config_.load_encoder_axis];
|
||||
controller_.pos_estimate_circular_src_.connect_to(&ax->encoder_.pos_circular_);
|
||||
controller_.pos_wrap_src_.connect_to(&controller_.config_.circular_setpoint_range);
|
||||
controller_.pos_estimate_linear_src_.connect_to(&ax->encoder_.pos_estimate_);
|
||||
controller_.vel_estimate_src_.connect_to(&ax->encoder_.vel_estimate_);
|
||||
} else {
|
||||
controller_.pos_estimate_circular_src_.disconnect();
|
||||
controller_.pos_estimate_linear_src_.disconnect();
|
||||
controller_.pos_wrap_src_.disconnect();
|
||||
controller_.vel_estimate_src_.disconnect();
|
||||
controller_.set_error(Controller::ERROR_INVALID_LOAD_ENCODER);
|
||||
return false;
|
||||
}
|
||||
|
||||
// To avoid any transient on startup, we intialize the setpoint to be the current position
|
||||
controller_.control_mode_updated();
|
||||
controller_.input_pos_updated();
|
||||
|
||||
// Avoid integrator windup issues
|
||||
controller_.vel_integrator_torque_ = 0.0f;
|
||||
|
||||
motor_.torque_setpoint_src_.connect_to(&controller_.torque_output_);
|
||||
motor_.direction_ = sensorless_mode ? 1.0f : encoder_.config_.direction;
|
||||
|
||||
motor_.current_control_.enable_current_control_src_ = motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL;
|
||||
motor_.current_control_.Idq_setpoint_src_.connect_to(&motor_.Idq_setpoint_);
|
||||
motor_.current_control_.Vdq_setpoint_src_.connect_to(&motor_.Vdq_setpoint_);
|
||||
|
||||
bool is_acim = motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM;
|
||||
// phase
|
||||
OutputPort<float>* phase_src = sensorless_mode ? &sensorless_estimator_.phase_ : &encoder_.phase_;
|
||||
acim_estimator_.rotor_phase_src_.connect_to(phase_src);
|
||||
OutputPort<float>* stator_phase_src = is_acim ? &acim_estimator_.stator_phase_ : phase_src;
|
||||
motor_.current_control_.phase_src_.connect_to(stator_phase_src);
|
||||
// phase vel
|
||||
OutputPort<float>* phase_vel_src = sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_;
|
||||
acim_estimator_.rotor_phase_vel_src_.connect_to(phase_vel_src);
|
||||
OutputPort<float>* stator_phase_vel_src = is_acim ? &acim_estimator_.stator_phase_vel_ : phase_vel_src;
|
||||
motor_.phase_vel_src_.connect_to(stator_phase_vel_src);
|
||||
motor_.current_control_.phase_vel_src_.connect_to(stator_phase_vel_src);
|
||||
|
||||
if (sensorless_mode) {
|
||||
// Make the final velocity of the loĉk-in spin the setpoint of the
|
||||
// closed loop controller to allow for smooth transition.
|
||||
float vel = config_.sensorless_ramp.vel / (2.0f * M_PI * motor_.config_.pole_pairs);
|
||||
controller_.input_vel_ = vel;
|
||||
controller_.vel_setpoint_ = vel;
|
||||
}
|
||||
}
|
||||
|
||||
// In sensorless mode the motor is already armed.
|
||||
if (!motor_.is_armed_) {
|
||||
wait_for_control_iteration();
|
||||
motor_.arm(&motor_.current_control_);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Axis::stop_closed_loop_control() {
|
||||
motor_.disarm();
|
||||
return check_for_errors();
|
||||
}
|
||||
|
||||
bool Axis::run_closed_loop_control_loop() {
|
||||
start_closed_loop_control();
|
||||
set_step_dir_active(config_.enable_step_dir);
|
||||
|
||||
while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_) {
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
|
||||
stop_closed_loop_control();
|
||||
|
||||
return check_for_errors();
|
||||
}
|
||||
|
||||
|
||||
// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
|
||||
// When pressed, set the linear count to the offset (default 0), and then go to position 0
|
||||
bool Axis::run_homing() {
|
||||
// TODO: theoretically this check should be inside the update loop,
|
||||
// otherwise someone could disable the endstop while homing is in progress.
|
||||
if (!min_endstop_.config_.enabled) {
|
||||
return error_ |= ERROR_HOMING_WITHOUT_ENDSTOP, false;
|
||||
}
|
||||
|
||||
controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
|
||||
controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP;
|
||||
|
||||
controller_.input_pos_ = 0.0f;
|
||||
controller_.input_pos_updated();
|
||||
controller_.input_vel_ = -controller_.config_.homing_speed;
|
||||
controller_.input_torque_ = 0.0f;
|
||||
|
||||
homing_.is_homed = false;
|
||||
|
||||
error_ &= ~ERROR_MIN_ENDSTOP_PRESSED;
|
||||
|
||||
bool done = false;
|
||||
|
||||
start_closed_loop_control();
|
||||
|
||||
// Driving toward the endstop
|
||||
while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_ && !(done = min_endstop_.get_state())) {
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
stop_closed_loop_control();
|
||||
|
||||
controller_.input_vel_ = 0.0f;
|
||||
|
||||
if (!done) {
|
||||
return false;
|
||||
}
|
||||
|
||||
error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop
|
||||
|
||||
std::optional<float> pos_estimate_local = encoder_.pos_estimate_.any();
|
||||
if (pos_estimate_local == std::nullopt || !pos_estimate_local.has_value()){
|
||||
return error_ |= ERROR_UNKNOWN_POSITION, false;
|
||||
}
|
||||
|
||||
controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
|
||||
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
|
||||
|
||||
// Initialize closed loop control, and then set the desired location.
|
||||
start_closed_loop_control();
|
||||
|
||||
controller_.input_pos_ = pos_estimate_local.value() + min_endstop_.config_.offset;
|
||||
controller_.pos_setpoint_ = pos_estimate_local.value();
|
||||
controller_.vel_setpoint_ = 0.0f;
|
||||
controller_.input_pos_updated();
|
||||
|
||||
// Synchronization issue. Ensure trajectory_done is false prior to the while loop, so that
|
||||
// the controller has time to run move_to_pos() on the next update()
|
||||
controller_.trajectory_done_ = false;
|
||||
|
||||
while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_ && !(done = controller_.trajectory_done_)) {
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
stop_closed_loop_control();
|
||||
|
||||
if (!done) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set the current position to 0, the target to zero, and make sure we're path planning from 0 to 0
|
||||
encoder_.set_linear_count(0);
|
||||
const auto load_encoder_axis = controller_.config_.load_encoder_axis;
|
||||
if(load_encoder_axis != axis_num_ && load_encoder_axis < AXIS_COUNT) {
|
||||
axes[load_encoder_axis].encoder_.set_linear_count(0);
|
||||
}
|
||||
controller_.input_pos_ = 0.0f;
|
||||
controller_.pos_setpoint_ = 0.0f;
|
||||
controller_.vel_setpoint_ = 0.0f;
|
||||
controller_.input_pos_updated();
|
||||
|
||||
// Force encoder estimate to update
|
||||
osDelay(1);
|
||||
|
||||
homing_.is_homed = true;
|
||||
|
||||
return check_for_errors();
|
||||
}
|
||||
|
||||
bool Axis::run_idle_loop() {
|
||||
last_drv_fault_ = motor_.gate_driver_.get_error();
|
||||
mechanical_brake_.engage();
|
||||
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
|
||||
while (requested_state_ == AXIS_STATE_UNDEFINED) {
|
||||
motor_.setup();
|
||||
osDelay(1);
|
||||
}
|
||||
return check_for_errors();
|
||||
}
|
||||
|
||||
// Infinite loop that does calibration and enters main control loop as appropriate
|
||||
void Axis::run_state_machine_loop() {
|
||||
for (;;) {
|
||||
// Load the task chain if a specific request is pending
|
||||
if (requested_state_ != AXIS_STATE_UNDEFINED) {
|
||||
size_t pos = 0;
|
||||
if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
|
||||
if (config_.startup_motor_calibration)
|
||||
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
|
||||
if (config_.startup_encoder_index_search && encoder_.config_.use_index)
|
||||
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
|
||||
if (config_.startup_encoder_offset_calibration)
|
||||
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
|
||||
if (config_.startup_homing)
|
||||
task_chain_[pos++] = AXIS_STATE_HOMING;
|
||||
if (config_.startup_closed_loop_control)
|
||||
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
|
||||
task_chain_[pos++] = AXIS_STATE_IDLE;
|
||||
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
|
||||
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
|
||||
if (encoder_.config_.mode == ODriveIntf::EncoderIntf::MODE_HALL)
|
||||
task_chain_[pos++] = AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION;
|
||||
if (encoder_.config_.use_index)
|
||||
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
|
||||
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
|
||||
task_chain_[pos++] = AXIS_STATE_IDLE;
|
||||
} else if (requested_state_ != AXIS_STATE_UNDEFINED) {
|
||||
task_chain_[pos++] = requested_state_;
|
||||
task_chain_[pos++] = AXIS_STATE_IDLE;
|
||||
}
|
||||
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
|
||||
requested_state_ = AXIS_STATE_UNDEFINED;
|
||||
// Auto-clear any invalid state error
|
||||
error_ &= ~ERROR_INVALID_STATE;
|
||||
}
|
||||
|
||||
// Note that current_state is a reference to task_chain_[0]
|
||||
|
||||
// Run the specified state
|
||||
// Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
|
||||
bool status;
|
||||
switch (current_state_) {
|
||||
case AXIS_STATE_MOTOR_CALIBRATION: {
|
||||
// These error checks are a hacky way to force legacy behavior
|
||||
// when an error is raised. TODO: remove this when we overhaul
|
||||
// the error architecture
|
||||
// (https://github.com/madcowswe/ODrive/issues/526).
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
status = motor_.run_calibration();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
if (!motor_.is_calibrated_)
|
||||
goto invalid_state_label;
|
||||
|
||||
status = encoder_.run_index_search();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_ENCODER_DIR_FIND: {
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
if (!motor_.is_calibrated_)
|
||||
goto invalid_state_label;
|
||||
|
||||
status = encoder_.run_direction_find();
|
||||
// Help facilitate encoder.is_ready without reboot
|
||||
if (status)
|
||||
encoder_.apply_config(motor_.config_.motor_type);
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION: {
|
||||
if (!motor_.is_calibrated_)
|
||||
goto invalid_state_label;
|
||||
|
||||
status = encoder_.run_hall_polarity_calibration();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION: {
|
||||
if (!motor_.is_calibrated_)
|
||||
goto invalid_state_label;
|
||||
|
||||
if (!encoder_.config_.hall_polarity_calibrated) {
|
||||
encoder_.set_error(ODriveIntf::EncoderIntf::ERROR_HALL_NOT_CALIBRATED_YET);
|
||||
goto invalid_state_label;
|
||||
}
|
||||
|
||||
status = encoder_.run_hall_phase_calibration();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_HOMING: {
|
||||
Controller::ControlMode stored_control_mode = controller_.config_.control_mode;
|
||||
Controller::InputMode stored_input_mode = controller_.config_.input_mode;
|
||||
|
||||
status = run_homing();
|
||||
|
||||
controller_.config_.control_mode = stored_control_mode;
|
||||
controller_.config_.input_mode = stored_input_mode;
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
if (!motor_.is_calibrated_)
|
||||
goto invalid_state_label;
|
||||
status = encoder_.run_offset_calibration();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_LOCKIN_SPIN: {
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
if (!motor_.is_calibrated_ || encoder_.config_.direction==0)
|
||||
goto invalid_state_label;
|
||||
status = run_lockin_spin(config_.general_lockin, false);
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_CLOSED_LOOP_CONTROL: {
|
||||
//if (odrv.any_error())
|
||||
// goto invalid_state_label;
|
||||
if (!motor_.is_calibrated_ || (encoder_.config_.direction==0 && !config_.enable_sensorless_mode))
|
||||
goto invalid_state_label;
|
||||
watchdog_feed();
|
||||
status = run_closed_loop_control_loop();
|
||||
} break;
|
||||
|
||||
case AXIS_STATE_IDLE: {
|
||||
run_idle_loop();
|
||||
status = true;
|
||||
} break;
|
||||
|
||||
default:
|
||||
invalid_state_label:
|
||||
error_ |= ERROR_INVALID_STATE;
|
||||
status = false; // this will set the state to idle
|
||||
break;
|
||||
}
|
||||
|
||||
// If the state failed, go to idle, else advance task chain
|
||||
if (!status) {
|
||||
std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
|
||||
current_state_ = AXIS_STATE_IDLE;
|
||||
} else {
|
||||
std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
|
||||
task_chain_.back() = AXIS_STATE_UNDEFINED;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,214 @@
|
||||
#ifndef __AXIS_HPP
|
||||
#define __AXIS_HPP
|
||||
|
||||
class Axis;
|
||||
|
||||
#include "encoder.hpp"
|
||||
#include "acim_estimator.hpp"
|
||||
#include "sensorless_estimator.hpp"
|
||||
#include "controller.hpp"
|
||||
#include "open_loop_controller.hpp"
|
||||
#include "trapTraj.hpp"
|
||||
#include "endstop.hpp"
|
||||
#include "mechanical_brake.hpp"
|
||||
#include "low_level.h"
|
||||
#include "utils.hpp"
|
||||
#include "task_timer.hpp"
|
||||
|
||||
#include <array>
|
||||
|
||||
class Axis : public ODriveIntf::AxisIntf {
|
||||
public:
|
||||
struct LockinConfig_t {
|
||||
float current = 10.0f; // [A]
|
||||
float ramp_time = 0.4f; // [s]
|
||||
float ramp_distance = 1 * M_PI; // [rad]
|
||||
float accel = 20.0f; // [rad/s^2]
|
||||
float vel = 40.0f; // [rad/s]
|
||||
float finish_distance = 100.0f; // [rad]
|
||||
bool finish_on_vel = false;
|
||||
bool finish_on_distance = false;
|
||||
bool finish_on_enc_idx = false;
|
||||
};
|
||||
|
||||
struct TaskTimes {
|
||||
TaskTimer thermistor_update;
|
||||
TaskTimer encoder_update;
|
||||
TaskTimer sensorless_estimator_update;
|
||||
TaskTimer endstop_update;
|
||||
TaskTimer can_heartbeat;
|
||||
TaskTimer controller_update;
|
||||
TaskTimer open_loop_controller_update;
|
||||
TaskTimer acim_estimator_update;
|
||||
TaskTimer motor_update;
|
||||
TaskTimer current_controller_update;
|
||||
TaskTimer dc_calib;
|
||||
TaskTimer current_sense;
|
||||
TaskTimer pwm_update;
|
||||
};
|
||||
|
||||
static LockinConfig_t default_calibration();
|
||||
static LockinConfig_t default_sensorless();
|
||||
static LockinConfig_t default_lockin();
|
||||
|
||||
struct CANConfig_t {
|
||||
uint32_t node_id = 0;
|
||||
bool is_extended = false;
|
||||
uint32_t heartbeat_rate_ms = 100;
|
||||
uint32_t encoder_rate_ms = 10;
|
||||
uint32_t motor_error_rate_ms = 0;
|
||||
uint32_t encoder_error_rate_ms = 0;
|
||||
uint32_t controller_error_rate_ms = 0;
|
||||
uint32_t sensorless_error_rate_ms = 0;
|
||||
uint32_t encoder_count_rate_ms = 0;
|
||||
uint32_t iq_rate_ms = 0;
|
||||
uint32_t sensorless_rate_ms = 0;
|
||||
uint32_t bus_vi_rate_ms = 0;
|
||||
};
|
||||
|
||||
struct Config_t {
|
||||
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
|
||||
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
|
||||
// this only has an effect if encoder.config.use_index is also true
|
||||
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
|
||||
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
|
||||
bool startup_homing = false; //<! enable homing after calibration/startup
|
||||
|
||||
bool enable_step_dir = false; //<! enable step/dir input after calibration
|
||||
// For M0 this has no effect if enable_uart is true
|
||||
bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
|
||||
//<! This is ignored if enable_step_dir is false.
|
||||
//<! This setting only takes effect on a state transition
|
||||
//<! into idle or out of closed loop control.
|
||||
|
||||
bool enable_sensorless_mode = false;
|
||||
|
||||
float watchdog_timeout = 0.0f; // [s]
|
||||
bool enable_watchdog = false;
|
||||
|
||||
// Defaults loaded from hw_config in load_configuration in main.cpp
|
||||
uint16_t step_gpio_pin = 0;
|
||||
uint16_t dir_gpio_pin = 0;
|
||||
|
||||
LockinConfig_t calibration_lockin = default_calibration();
|
||||
LockinConfig_t sensorless_ramp = default_sensorless();
|
||||
LockinConfig_t general_lockin;
|
||||
|
||||
CANConfig_t can;
|
||||
|
||||
// custom setters
|
||||
Axis* parent = nullptr;
|
||||
void set_step_gpio_pin(uint16_t value) { step_gpio_pin = value; parent->decode_step_dir_pins(); }
|
||||
void set_dir_gpio_pin(uint16_t value) { dir_gpio_pin = value; parent->decode_step_dir_pins(); }
|
||||
};
|
||||
|
||||
struct Homing_t {
|
||||
bool is_homed = false;
|
||||
};
|
||||
|
||||
struct CAN_t {
|
||||
uint32_t last_heartbeat = 0;
|
||||
uint32_t last_encoder = 0;
|
||||
uint32_t last_motor_error = 0;
|
||||
uint32_t last_encoder_error = 0;
|
||||
uint32_t last_controller_error = 0;
|
||||
uint32_t last_sensorless_error = 0;
|
||||
uint32_t last_encoder_count = 0;
|
||||
uint32_t last_iq = 0;
|
||||
uint32_t last_sensorless = 0;
|
||||
uint32_t last_bus_vi = 0;
|
||||
};
|
||||
|
||||
Axis(int axis_num,
|
||||
uint16_t default_step_gpio_pin,
|
||||
uint16_t default_dir_gpio_pin,
|
||||
osPriority thread_priority,
|
||||
Encoder& encoder,
|
||||
SensorlessEstimator& sensorless_estimator,
|
||||
Controller& controller,
|
||||
Motor& motor,
|
||||
TrapezoidalTrajectory& trap,
|
||||
Endstop& min_endstop,
|
||||
Endstop& max_endstop,
|
||||
MechanicalBrake& mechanical_brake);
|
||||
|
||||
bool apply_config();
|
||||
void clear_config();
|
||||
|
||||
void start_thread();
|
||||
bool wait_for_control_iteration();
|
||||
|
||||
void step_cb();
|
||||
void set_step_dir_active(bool enable);
|
||||
void decode_step_dir_pins();
|
||||
|
||||
bool do_checks(uint32_t timestamp);
|
||||
|
||||
void watchdog_feed();
|
||||
bool watchdog_check();
|
||||
|
||||
// True if there are no errors
|
||||
bool inline check_for_errors() {
|
||||
return error_ == ERROR_NONE;
|
||||
}
|
||||
|
||||
bool start_closed_loop_control();
|
||||
bool stop_closed_loop_control();
|
||||
bool run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed,
|
||||
std::function<bool(bool)> loop_cb = {} );
|
||||
bool run_closed_loop_control_loop();
|
||||
bool run_homing();
|
||||
bool run_idle_loop();
|
||||
|
||||
constexpr uint32_t get_watchdog_reset() {
|
||||
return static_cast<uint32_t>(std::clamp<float>(config_.watchdog_timeout, 0, UINT32_MAX / (current_meas_hz + 1)) * current_meas_hz);
|
||||
}
|
||||
|
||||
void run_state_machine_loop();
|
||||
|
||||
// hardware config
|
||||
int axis_num_;
|
||||
uint16_t default_step_gpio_pin_;
|
||||
uint16_t default_dir_gpio_pin_;
|
||||
osPriority thread_priority_;
|
||||
Config_t config_;
|
||||
|
||||
Encoder& encoder_;
|
||||
AcimEstimator acim_estimator_;
|
||||
SensorlessEstimator& sensorless_estimator_;
|
||||
Controller& controller_;
|
||||
OpenLoopController open_loop_controller_;
|
||||
Motor& motor_;
|
||||
TrapezoidalTrajectory& trap_traj_;
|
||||
Endstop& min_endstop_;
|
||||
Endstop& max_endstop_;
|
||||
MechanicalBrake& mechanical_brake_;
|
||||
TaskTimes task_times_;
|
||||
|
||||
osThreadId thread_id_ = 0;
|
||||
const uint32_t stack_size_ = 2048; // Bytes
|
||||
volatile bool thread_id_valid_ = false;
|
||||
|
||||
// variables exposed on protocol
|
||||
Error error_ = ERROR_NONE;
|
||||
bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir
|
||||
int64_t steps_ = 0; // Steps counted at interface
|
||||
uint32_t last_drv_fault_ = 0;
|
||||
|
||||
// updated from config in constructor, and on protocol hook
|
||||
Stm32Gpio step_gpio_;
|
||||
Stm32Gpio dir_gpio_;
|
||||
|
||||
AxisState requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
|
||||
std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
|
||||
AxisState& current_state_ = task_chain_.front();
|
||||
Homing_t homing_;
|
||||
CAN_t can_;
|
||||
|
||||
|
||||
// watchdog
|
||||
uint32_t watchdog_current_value_= 0;
|
||||
};
|
||||
|
||||
|
||||
#endif /* __AXIS_HPP */
|
||||
@@ -0,0 +1,182 @@
|
||||
#ifndef __COMPONENT_HPP
|
||||
#define __COMPONENT_HPP
|
||||
|
||||
#include <stdint.h>
|
||||
#include <optional>
|
||||
#include <variant>
|
||||
|
||||
class ComponentBase {
|
||||
public:
|
||||
/**
|
||||
* @brief Shall run the update action of this component.
|
||||
*
|
||||
* This function gets called in a low priority interrupt context and is
|
||||
* allowed to call CMSIS functions.
|
||||
*
|
||||
* @param timestamp: The timestamp (in HCLK ticks) for which this update
|
||||
* is run.
|
||||
*/
|
||||
virtual void update(uint32_t timestamp) = 0;
|
||||
};
|
||||
|
||||
|
||||
template<typename T>
|
||||
class InputPort;
|
||||
|
||||
/**
|
||||
* @brief An output port stores a value for consumption by a connecting input
|
||||
* port.
|
||||
*
|
||||
* Output ports are supposed to be reset at the beginning of a control loop
|
||||
* iteration. This ensures that connecting input ports don't use an outdated
|
||||
* value and, more importantly, ensures proper handling if the producer of the
|
||||
* value is incapable of producing the value for any reason.
|
||||
*
|
||||
* Member functions of this class are not thread-safe unless noted otherwise.
|
||||
*/
|
||||
template<typename T>
|
||||
class OutputPort {
|
||||
public:
|
||||
/**
|
||||
* @brief Initializes the output port with the specified value.
|
||||
*
|
||||
* An initialization value is required for any() to work properly.
|
||||
* present() and previous() cannot be used to fetch the
|
||||
* initialization value.
|
||||
*/
|
||||
OutputPort(T val) : content_(val) {}
|
||||
|
||||
/**
|
||||
* @brief Updates the underlying value of this output port.
|
||||
*/
|
||||
void operator=(T value) {
|
||||
content_ = value;
|
||||
age_ = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Marks the contained value as outdated. The value is not actually
|
||||
* deleted and can still be accessed through some of the member functions
|
||||
* of this class.
|
||||
*/
|
||||
void reset() {
|
||||
// This will eventually overflow to 0 so present() could
|
||||
// theoretically return a very old value however it is very likely that
|
||||
// the motor will be long disarmed by then.
|
||||
age_++;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the value from this control loop iteration or std::nullopt
|
||||
* if the value was not yet set during this control loop iteration.
|
||||
*/
|
||||
std::optional<T> present() {
|
||||
if (age_ == 0) {
|
||||
return content_;
|
||||
} else {
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the value from exactly the previous control loop iteration.
|
||||
*
|
||||
* If during the last iteration no value was set or the value was already
|
||||
* overwritten during this control loop iteration then this function returns
|
||||
* std::nullopt.
|
||||
*/
|
||||
std::optional<T> previous() {
|
||||
if (age_ == 1) {
|
||||
return content_;
|
||||
} else {
|
||||
return std::nullopt;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns the value contained in this output port with disregard of
|
||||
* when the value was set.
|
||||
*
|
||||
* This function is thread-safe if load/store operations of T are atomic.
|
||||
*/
|
||||
std::optional<T> any() {
|
||||
return content_;
|
||||
}
|
||||
|
||||
private:
|
||||
uint32_t age_ = 2; // Age in number of control loop iterations
|
||||
T content_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief An input port provides a value from the source to which it's configured.
|
||||
*
|
||||
* The source can be one of:
|
||||
* - an internally stored value
|
||||
* - an externally stored value (referenced by a pointer)
|
||||
* - an external OutputPort (referenced by a pointer)
|
||||
* - none (all queries will return std::nullopt)
|
||||
*
|
||||
* Member functions of this class are not thread-safe unless otherwise noted.
|
||||
*/
|
||||
template<typename T>
|
||||
class InputPort {
|
||||
public:
|
||||
void connect_to(OutputPort<T>* input_port) {
|
||||
content_ = input_port;
|
||||
}
|
||||
|
||||
void connect_to(T* input_ptr) {
|
||||
content_ = input_ptr;
|
||||
}
|
||||
|
||||
void disconnect() {
|
||||
content_ = (OutputPort<T>*)nullptr;
|
||||
}
|
||||
|
||||
std::optional<T> present() {
|
||||
if (content_.index() == 2) {
|
||||
OutputPort<T>* ptr = std::get<2>(content_);
|
||||
return ptr ? ptr->present() : std::nullopt;
|
||||
} else if (content_.index() == 1) {
|
||||
T* ptr = std::get<1>(content_);
|
||||
return ptr ? std::make_optional(*ptr) : std::nullopt;
|
||||
} else {
|
||||
return std::get<0>(content_);
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: probably it makes sense to let the application define that it's
|
||||
// ok for this input port to fetch the value from the last iteration.
|
||||
// This would provide a general way to resolve same-iteration data path cycles.
|
||||
|
||||
//std::optional<T> previous() {
|
||||
// if (content_.index() == 2) {
|
||||
// OutputPort<T>* ptr = std::get<2>(content_);
|
||||
// return ptr ? ptr->previous() : std::nullopt;
|
||||
// } else if (content_.index() == 1) {
|
||||
// T* ptr = std::get<1>(content_);
|
||||
// return ptr ? std::make_optional(*ptr) : std::nullopt;
|
||||
// } else {
|
||||
// return std::get<0>(content_);
|
||||
// }
|
||||
//}
|
||||
|
||||
std::optional<T> any() {
|
||||
if (content_.index() == 2) {
|
||||
OutputPort<T>* ptr = std::get<2>(content_);
|
||||
return ptr ? ptr->any() : std::nullopt;
|
||||
} else if (content_.index() == 1) {
|
||||
T* ptr = std::get<1>(content_);
|
||||
return ptr ? std::make_optional(*ptr) : std::nullopt;
|
||||
} else {
|
||||
return std::get<0>(content_);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
std::variant<T, T*, OutputPort<T>*> content_;
|
||||
};
|
||||
|
||||
|
||||
#endif // __COMPONENT_HPP
|
||||
@@ -0,0 +1,451 @@
|
||||
|
||||
#include "odrive_main.h"
|
||||
#include <algorithm>
|
||||
#include <numeric>
|
||||
|
||||
bool Controller::apply_config() {
|
||||
config_.parent = this;
|
||||
update_filter_gains();
|
||||
return true;
|
||||
}
|
||||
|
||||
void Controller::reset() {
|
||||
// pos_setpoint is initialized in start_closed_loop_control
|
||||
vel_setpoint_ = 0.0f;
|
||||
vel_integrator_torque_ = 0.0f;
|
||||
torque_setpoint_ = 0.0f;
|
||||
mechanical_power_ = 0.0f;
|
||||
electrical_power_ = 0.0f;
|
||||
}
|
||||
|
||||
void Controller::set_error(Error error) {
|
||||
error_ |= error;
|
||||
last_error_time_ = odrv.n_evt_control_loop_ * current_meas_period;
|
||||
}
|
||||
|
||||
//--------------------------------
|
||||
// Command Handling
|
||||
//--------------------------------
|
||||
|
||||
|
||||
void Controller::move_to_pos(float goal_point) {
|
||||
axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
|
||||
axis_->trap_traj_.config_.vel_limit,
|
||||
axis_->trap_traj_.config_.accel_limit,
|
||||
axis_->trap_traj_.config_.decel_limit);
|
||||
axis_->trap_traj_.t_ = 0.0f;
|
||||
trajectory_done_ = false;
|
||||
}
|
||||
|
||||
void Controller::move_incremental(float displacement, bool from_input_pos = true){
|
||||
if(from_input_pos){
|
||||
input_pos_ += displacement;
|
||||
} else{
|
||||
input_pos_ = pos_setpoint_ + displacement;
|
||||
}
|
||||
|
||||
input_pos_updated();
|
||||
}
|
||||
|
||||
void Controller::start_anticogging_calibration() {
|
||||
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
|
||||
if (axis_->error_ == Axis::ERROR_NONE) {
|
||||
config_.anticogging.calib_anticogging = true;
|
||||
}
|
||||
}
|
||||
|
||||
float Controller::remove_anticogging_bias()
|
||||
{
|
||||
auto& cogmap = config_.anticogging.cogging_map;
|
||||
|
||||
auto sum = std::accumulate(std::begin(cogmap), std::end(cogmap), 0.0f);
|
||||
auto average = sum / std::size(cogmap);
|
||||
|
||||
for(auto& val : cogmap) {
|
||||
val -= average;
|
||||
}
|
||||
|
||||
return average;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* This anti-cogging implementation iterates through each encoder position,
|
||||
* waits for zero velocity & position error,
|
||||
* then samples the current required to maintain that position.
|
||||
*
|
||||
* This holding current is added as a feedforward term in the control loop.
|
||||
*/
|
||||
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
|
||||
float pos_err = input_pos_ - pos_estimate;
|
||||
if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold / (float)axis_->encoder_.config_.cpr &&
|
||||
std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold / (float)axis_->encoder_.config_.cpr) {
|
||||
config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;
|
||||
}
|
||||
if (config_.anticogging.index < 3600) {
|
||||
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
|
||||
input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
|
||||
input_vel_ = 0.0f;
|
||||
input_torque_ = 0.0f;
|
||||
input_pos_updated();
|
||||
return false;
|
||||
} else {
|
||||
config_.anticogging.index = 0;
|
||||
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
|
||||
input_pos_ = 0.0f; // Send the motor home
|
||||
input_vel_ = 0.0f;
|
||||
input_torque_ = 0.0f;
|
||||
input_pos_updated();
|
||||
anticogging_valid_ = true;
|
||||
config_.anticogging.calib_anticogging = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void Controller::set_input_pos_and_steps(float const pos) {
|
||||
input_pos_ = pos;
|
||||
if (config_.circular_setpoints) {
|
||||
float const range = config_.circular_setpoint_range;
|
||||
axis_->steps_ = (int64_t)(fmodf_pos(pos, range) / range * config_.steps_per_circular_range);
|
||||
} else {
|
||||
axis_->steps_ = (int64_t)(pos * config_.steps_per_circular_range);
|
||||
}
|
||||
}
|
||||
|
||||
bool Controller::control_mode_updated() {
|
||||
if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
|
||||
std::optional<float> estimate = (config_.circular_setpoints ?
|
||||
pos_estimate_circular_src_ :
|
||||
pos_estimate_linear_src_).any();
|
||||
if (!estimate.has_value()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
pos_setpoint_ = *estimate;
|
||||
set_input_pos_and_steps(*estimate);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void Controller::update_filter_gains() {
|
||||
float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz);
|
||||
input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time
|
||||
input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
|
||||
}
|
||||
|
||||
static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float torque) {
|
||||
float Tmax = (vel_limit - vel_estimate) * vel_gain;
|
||||
float Tmin = (-vel_limit - vel_estimate) * vel_gain;
|
||||
return std::clamp(torque, Tmin, Tmax);
|
||||
}
|
||||
|
||||
bool Controller::update() {
|
||||
std::optional<float> pos_estimate_linear = pos_estimate_linear_src_.present();
|
||||
std::optional<float> pos_estimate_circular = pos_estimate_circular_src_.present();
|
||||
std::optional<float> pos_wrap = pos_wrap_src_.present();
|
||||
std::optional<float> vel_estimate = vel_estimate_src_.present();
|
||||
|
||||
std::optional<float> anticogging_pos_estimate = axis_->encoder_.pos_estimate_.present();
|
||||
std::optional<float> anticogging_vel_estimate = axis_->encoder_.vel_estimate_.present();
|
||||
|
||||
if (axis_->step_dir_active_) {
|
||||
if (config_.circular_setpoints) {
|
||||
if (!pos_wrap.has_value()) {
|
||||
set_error(ERROR_INVALID_CIRCULAR_RANGE);
|
||||
return false;
|
||||
}
|
||||
input_pos_ = (float)(axis_->steps_ % config_.steps_per_circular_range) * (*pos_wrap / (float)(config_.steps_per_circular_range));
|
||||
} else {
|
||||
input_pos_ = (float)(axis_->steps_) / (float)(config_.steps_per_circular_range);
|
||||
}
|
||||
}
|
||||
|
||||
if (config_.anticogging.calib_anticogging) {
|
||||
if (!anticogging_pos_estimate.has_value() || !anticogging_vel_estimate.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
// non-blocking
|
||||
anticogging_calibration(*anticogging_pos_estimate, *anticogging_vel_estimate);
|
||||
}
|
||||
|
||||
// TODO also enable circular deltas for 2nd order filter, etc.
|
||||
if (config_.circular_setpoints) {
|
||||
if (!pos_wrap.has_value()) {
|
||||
set_error(ERROR_INVALID_CIRCULAR_RANGE);
|
||||
return false;
|
||||
}
|
||||
input_pos_ = fmodf_pos(input_pos_, *pos_wrap);
|
||||
}
|
||||
|
||||
// Update inputs
|
||||
switch (config_.input_mode) {
|
||||
case INPUT_MODE_INACTIVE: {
|
||||
// do nothing
|
||||
} break;
|
||||
case INPUT_MODE_PASSTHROUGH: {
|
||||
pos_setpoint_ = input_pos_;
|
||||
vel_setpoint_ = input_vel_;
|
||||
torque_setpoint_ = input_torque_;
|
||||
} break;
|
||||
case INPUT_MODE_VEL_RAMP: {
|
||||
float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
|
||||
float full_step = input_vel_ - vel_setpoint_;
|
||||
float step = std::clamp(full_step, -max_step_size, max_step_size);
|
||||
|
||||
vel_setpoint_ += step;
|
||||
torque_setpoint_ = (step / current_meas_period) * config_.inertia;
|
||||
} break;
|
||||
case INPUT_MODE_TORQUE_RAMP: {
|
||||
float max_step_size = std::abs(current_meas_period * config_.torque_ramp_rate);
|
||||
float full_step = input_torque_ - torque_setpoint_;
|
||||
float step = std::clamp(full_step, -max_step_size, max_step_size);
|
||||
|
||||
torque_setpoint_ += step;
|
||||
} break;
|
||||
case INPUT_MODE_POS_FILTER: {
|
||||
// 2nd order pos tracking filter
|
||||
float delta_pos = input_pos_ - pos_setpoint_; // Pos error
|
||||
if (config_.circular_setpoints) {
|
||||
if (!pos_wrap.has_value()) {
|
||||
set_error(ERROR_INVALID_CIRCULAR_RANGE);
|
||||
return false;
|
||||
}
|
||||
delta_pos = wrap_pm(delta_pos, *pos_wrap);
|
||||
}
|
||||
float delta_vel = input_vel_ - vel_setpoint_; // Vel error
|
||||
float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback
|
||||
torque_setpoint_ = accel * config_.inertia; // Accel
|
||||
vel_setpoint_ += current_meas_period * accel; // delta vel
|
||||
pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
|
||||
} break;
|
||||
case INPUT_MODE_MIRROR: {
|
||||
if (config_.axis_to_mirror < AXIS_COUNT) {
|
||||
std::optional<float> other_pos = axes[config_.axis_to_mirror].encoder_.pos_estimate_.present();
|
||||
std::optional<float> other_vel = axes[config_.axis_to_mirror].encoder_.vel_estimate_.present();
|
||||
std::optional<float> other_torque = axes[config_.axis_to_mirror].controller_.torque_output_.present();
|
||||
|
||||
if (!other_pos.has_value() || !other_vel.has_value() || !other_torque.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
|
||||
pos_setpoint_ = *other_pos * config_.mirror_ratio;
|
||||
vel_setpoint_ = *other_vel * config_.mirror_ratio;
|
||||
torque_setpoint_ = *other_torque * config_.torque_mirror_ratio;
|
||||
} else {
|
||||
set_error(ERROR_INVALID_MIRROR_AXIS);
|
||||
return false;
|
||||
}
|
||||
} break;
|
||||
// case INPUT_MODE_MIX_CHANNELS: {
|
||||
// // NOT YET IMPLEMENTED
|
||||
// } break;
|
||||
case INPUT_MODE_TRAP_TRAJ: {
|
||||
if(input_pos_updated_){
|
||||
move_to_pos(input_pos_);
|
||||
input_pos_updated_ = false;
|
||||
}
|
||||
// Avoid updating uninitialized trajectory
|
||||
if (trajectory_done_)
|
||||
break;
|
||||
|
||||
if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {
|
||||
// Drop into position control mode when done to avoid problems on loop counter delta overflow
|
||||
config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
|
||||
pos_setpoint_ = axis_->trap_traj_.Xf_;
|
||||
vel_setpoint_ = 0.0f;
|
||||
torque_setpoint_ = 0.0f;
|
||||
trajectory_done_ = true;
|
||||
} else {
|
||||
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_);
|
||||
pos_setpoint_ = traj_step.Y;
|
||||
vel_setpoint_ = traj_step.Yd;
|
||||
torque_setpoint_ = traj_step.Ydd * config_.inertia;
|
||||
axis_->trap_traj_.t_ += current_meas_period;
|
||||
}
|
||||
anticogging_pos_estimate = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
|
||||
} break;
|
||||
case INPUT_MODE_TUNING: {
|
||||
autotuning_phase_ = wrap_pm_pi(autotuning_phase_ + (2.0f * M_PI * autotuning_.frequency * current_meas_period));
|
||||
float c = our_arm_cos_f32(autotuning_phase_);
|
||||
float s = our_arm_sin_f32(autotuning_phase_);
|
||||
pos_setpoint_ = input_pos_ + autotuning_.pos_amplitude * s; // + pos_amp_c * c
|
||||
vel_setpoint_ = input_vel_ + autotuning_.vel_amplitude * c;
|
||||
torque_setpoint_ = input_torque_ + autotuning_.torque_amplitude * -s;
|
||||
} break;
|
||||
default: {
|
||||
set_error(ERROR_INVALID_INPUT_MODE);
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Never command a setpoint beyond its limit
|
||||
if(config_.enable_vel_limit) {
|
||||
vel_setpoint_ = std::clamp(vel_setpoint_, -config_.vel_limit, config_.vel_limit);
|
||||
}
|
||||
const float Tlim = axis_->motor_.max_available_torque();
|
||||
torque_setpoint_ = std::clamp(torque_setpoint_, -Tlim, Tlim);
|
||||
|
||||
// Position control
|
||||
// TODO Decide if we want to use encoder or pll position here
|
||||
float gain_scheduling_multiplier = 1.0f;
|
||||
float vel_des = vel_setpoint_;
|
||||
if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
|
||||
float pos_err;
|
||||
|
||||
if (config_.circular_setpoints) {
|
||||
if (!pos_estimate_circular.has_value() || !pos_wrap.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
// Keep pos setpoint from drifting
|
||||
pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap);
|
||||
// Circular delta
|
||||
pos_err = pos_setpoint_ - *pos_estimate_circular;
|
||||
pos_err = wrap_pm(pos_err, *pos_wrap);
|
||||
} else {
|
||||
if (!pos_estimate_linear.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
pos_err = pos_setpoint_ - *pos_estimate_linear;
|
||||
}
|
||||
|
||||
vel_des += config_.pos_gain * pos_err;
|
||||
// V-shaped gain shedule based on position error
|
||||
float abs_pos_err = std::abs(pos_err);
|
||||
if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) {
|
||||
gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width;
|
||||
}
|
||||
}
|
||||
|
||||
// Velocity limiting
|
||||
float vel_lim = config_.vel_limit;
|
||||
if (config_.enable_vel_limit) {
|
||||
vel_des = std::clamp(vel_des, -vel_lim, vel_lim);
|
||||
}
|
||||
|
||||
// Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
|
||||
if (config_.enable_overspeed_error) { // 0.0f to disable
|
||||
if (!vel_estimate.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
if (std::abs(*vel_estimate) > config_.vel_limit_tolerance * vel_lim) {
|
||||
set_error(ERROR_OVERSPEED);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Change to controller working in torque units
|
||||
// Torque per amp gain scheduling (ACIM)
|
||||
float vel_gain = config_.vel_gain;
|
||||
float vel_integrator_gain = config_.vel_integrator_gain;
|
||||
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
|
||||
float effective_flux = axis_->acim_estimator_.rotor_flux_;
|
||||
float minflux = axis_->motor_.config_.acim_gain_min_flux;
|
||||
if (std::abs(effective_flux) < minflux)
|
||||
effective_flux = std::copysignf(minflux, effective_flux);
|
||||
vel_gain /= effective_flux;
|
||||
vel_integrator_gain /= effective_flux;
|
||||
// TODO: also scale the integral value which is also changing units.
|
||||
// (or again just do control in torque units)
|
||||
}
|
||||
|
||||
// Velocity control
|
||||
float torque = torque_setpoint_;
|
||||
|
||||
// Anti-cogging is enabled after calibration
|
||||
// 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_valid_ && config_.anticogging.anticogging_enabled) {
|
||||
if (!anticogging_pos_estimate.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
float anticogging_pos = *anticogging_pos_estimate / axis_->encoder_.getCoggingRatio();
|
||||
torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
|
||||
}
|
||||
|
||||
float v_err = 0.0f;
|
||||
if (config_.control_mode >= CONTROL_MODE_VELOCITY_CONTROL) {
|
||||
if (!vel_estimate.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
|
||||
v_err = vel_des - *vel_estimate;
|
||||
torque += (vel_gain * gain_scheduling_multiplier) * v_err;
|
||||
|
||||
// Velocity integral action before limiting
|
||||
torque += vel_integrator_torque_;
|
||||
}
|
||||
|
||||
// Velocity limiting in current mode
|
||||
if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_torque_mode_vel_limit) {
|
||||
if (!vel_estimate.has_value()) {
|
||||
set_error(ERROR_INVALID_ESTIMATE);
|
||||
return false;
|
||||
}
|
||||
torque = limitVel(config_.vel_limit, *vel_estimate, vel_gain, torque);
|
||||
}
|
||||
|
||||
// Torque limiting
|
||||
bool limited = false;
|
||||
if (torque > Tlim) {
|
||||
limited = true;
|
||||
torque = Tlim;
|
||||
}
|
||||
if (torque < -Tlim) {
|
||||
limited = true;
|
||||
torque = -Tlim;
|
||||
}
|
||||
|
||||
// Velocity integrator (behaviour dependent on limiting)
|
||||
if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL) {
|
||||
// reset integral if not in use
|
||||
vel_integrator_torque_ = 0.0f;
|
||||
} else {
|
||||
if (limited) {
|
||||
// TODO make decayfactor configurable
|
||||
vel_integrator_torque_ *= 0.99f;
|
||||
} else {
|
||||
vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
|
||||
}
|
||||
// integrator limiting to prevent windup
|
||||
vel_integrator_torque_ = std::clamp(vel_integrator_torque_, -config_.vel_integrator_limit, config_.vel_integrator_limit);
|
||||
}
|
||||
|
||||
float ideal_electrical_power = 0.0f;
|
||||
if (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL) {
|
||||
ideal_electrical_power = axis_->motor_.current_control_.power_ - \
|
||||
SQ(axis_->motor_.current_control_.Iq_measured_) * 1.5f * axis_->motor_.config_.phase_resistance - \
|
||||
SQ(axis_->motor_.current_control_.Id_measured_) * 1.5f * axis_->motor_.config_.phase_resistance;
|
||||
}
|
||||
else {
|
||||
ideal_electrical_power = axis_->motor_.current_control_.power_;
|
||||
}
|
||||
mechanical_power_ += config_.mechanical_power_bandwidth * current_meas_period * (torque * *vel_estimate * M_PI * 2.0f - mechanical_power_);
|
||||
electrical_power_ += config_.electrical_power_bandwidth * current_meas_period * (ideal_electrical_power - electrical_power_);
|
||||
|
||||
// Spinout check
|
||||
// If mechanical power is negative (braking) and measured power is positive, something is wrong
|
||||
// This indicates that the controller is trying to stop, but torque is being produced.
|
||||
// Usually caused by an incorrect encoder offset
|
||||
if (mechanical_power_ < config_.spinout_mechanical_power_threshold && electrical_power_ > config_.spinout_electrical_power_threshold) {
|
||||
set_error(ERROR_SPINOUT_DETECTED);
|
||||
return false;
|
||||
}
|
||||
|
||||
torque_output_ = torque;
|
||||
|
||||
// TODO: this is inconsistent with the other errors which are sticky.
|
||||
// However if we make ERROR_INVALID_ESTIMATE sticky then it will be
|
||||
// confusing that a normal sequence of motor calibration + encoder
|
||||
// calibration would leave the controller in an error state.
|
||||
error_ &= ~ERROR_INVALID_ESTIMATE;
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,136 @@
|
||||
#ifndef __CONTROLLER_HPP
|
||||
#define __CONTROLLER_HPP
|
||||
|
||||
class Controller : public ODriveIntf::ControllerIntf {
|
||||
public:
|
||||
struct Anticogging_t {
|
||||
uint32_t index = 0;
|
||||
float cogging_map[3600];
|
||||
bool pre_calibrated = false;
|
||||
bool calib_anticogging = false;
|
||||
float calib_pos_threshold = 1.0f;
|
||||
float calib_vel_threshold = 1.0f;
|
||||
float cogging_ratio = 1.0f;
|
||||
bool anticogging_enabled = true;
|
||||
};
|
||||
|
||||
struct Autotuning_t {
|
||||
float frequency = 0.0f;
|
||||
float pos_amplitude = 0.0f;
|
||||
float vel_amplitude = 0.0f;
|
||||
float torque_amplitude = 0.0f;
|
||||
};
|
||||
|
||||
struct Config_t {
|
||||
ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL; //see: ControlMode_t
|
||||
InputMode input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
|
||||
float pos_gain = 20.0f; // [(turn/s) / turn]
|
||||
float vel_gain = 1.0f / 6.0f; // [Nm/(turn/s)]
|
||||
// float vel_gain = 0.2f / 200.0f, // [Nm/(rad/s)] <sensorless example>
|
||||
float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)]
|
||||
float vel_limit = 2.0f; // [turn/s] Infinity to disable.
|
||||
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
|
||||
float vel_integrator_limit = INFINITY; // Vel. integrator clamping value. Infinity to disable.
|
||||
float vel_ramp_rate = 1.0f; // [(turn/s) / s]
|
||||
float torque_ramp_rate = 0.01f; // Nm / sec
|
||||
bool circular_setpoints = false;
|
||||
float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn]
|
||||
uint32_t steps_per_circular_range = 1024;
|
||||
float inertia = 0.0f; // [Nm/(turn/s^2)]
|
||||
float input_filter_bandwidth = 2.0f; // [1/s]
|
||||
float homing_speed = 0.25f; // [turn/s]
|
||||
Anticogging_t anticogging;
|
||||
float gain_scheduling_width = 10.0f;
|
||||
bool enable_gain_scheduling = false;
|
||||
bool enable_vel_limit = true;
|
||||
bool enable_overspeed_error = true;
|
||||
bool enable_torque_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
|
||||
uint8_t axis_to_mirror = -1;
|
||||
float mirror_ratio = 1.0f;
|
||||
float torque_mirror_ratio = 0.0f;
|
||||
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration(). Set to -1 to select sensorless estimator.
|
||||
float mechanical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for mechanical power for spinout detction
|
||||
float electrical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for electrical power for spinout detection
|
||||
float spinout_electrical_power_threshold = 10.0f; // [W] electrical power threshold for spinout detection
|
||||
float spinout_mechanical_power_threshold = -10.0f; // [W] mechanical power threshold for spinout detection
|
||||
|
||||
// custom setters
|
||||
Controller* parent;
|
||||
void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); }
|
||||
void set_steps_per_circular_range(uint32_t value) { steps_per_circular_range = value > 0 ? value : steps_per_circular_range; }
|
||||
void set_control_mode(ControlMode value) { control_mode = value; parent->control_mode_updated(); }
|
||||
};
|
||||
|
||||
|
||||
bool apply_config();
|
||||
|
||||
void reset();
|
||||
void set_error(Error error);
|
||||
|
||||
constexpr void input_pos_updated() {
|
||||
input_pos_updated_ = true;
|
||||
}
|
||||
bool control_mode_updated();
|
||||
void set_input_pos_and_steps(float pos);
|
||||
|
||||
bool select_encoder(size_t encoder_num);
|
||||
|
||||
// Trajectory-Planned control
|
||||
void move_to_pos(float goal_point);
|
||||
void move_incremental(float displacement, bool from_goal_point);
|
||||
|
||||
// TODO: make this more similar to other calibration loops
|
||||
void start_anticogging_calibration();
|
||||
float remove_anticogging_bias();
|
||||
bool anticogging_calibration(float pos_estimate, float vel_estimate);
|
||||
|
||||
float get_anticogging_value(uint32_t index) {
|
||||
return (index < 3600) ? config_.anticogging.cogging_map[index] : 0.0f;
|
||||
}
|
||||
|
||||
void update_filter_gains();
|
||||
bool update();
|
||||
|
||||
Config_t config_;
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
|
||||
Error error_ = ERROR_NONE;
|
||||
float last_error_time_ = 0.0f;
|
||||
|
||||
// Inputs
|
||||
InputPort<float> pos_estimate_linear_src_;
|
||||
InputPort<float> pos_estimate_circular_src_;
|
||||
InputPort<float> vel_estimate_src_;
|
||||
InputPort<float> pos_wrap_src_;
|
||||
|
||||
float pos_setpoint_ = 0.0f; // [turns]
|
||||
float vel_setpoint_ = 0.0f; // [turn/s]
|
||||
// float vel_setpoint = 800.0f; <sensorless example>
|
||||
float vel_integrator_torque_ = 0.0f; // [Nm]
|
||||
float torque_setpoint_ = 0.0f; // [Nm]
|
||||
|
||||
float input_pos_ = 0.0f; // [turns]
|
||||
float input_vel_ = 0.0f; // [turn/s]
|
||||
float input_torque_ = 0.0f; // [Nm]
|
||||
float input_filter_kp_ = 0.0f;
|
||||
float input_filter_ki_ = 0.0f;
|
||||
|
||||
Autotuning_t autotuning_;
|
||||
float autotuning_phase_ = 0.0f;
|
||||
|
||||
bool input_pos_updated_ = false;
|
||||
|
||||
bool trajectory_done_ = true;
|
||||
|
||||
bool anticogging_valid_ = false;
|
||||
float mechanical_power_ = 0.0f; // [W]
|
||||
float electrical_power_ = 0.0f; // [W]
|
||||
|
||||
// Outputs
|
||||
OutputPort<float> torque_output_ = 0.0f;
|
||||
|
||||
// custom setters
|
||||
void set_input_pos(float value) { set_input_pos_and_steps(value); input_pos_updated(); }
|
||||
};
|
||||
|
||||
#endif // __CONTROLLER_HPP
|
||||
@@ -0,0 +1,10 @@
|
||||
#ifndef __CURRENT_LIMITER_HPP
|
||||
#define __CURRENT_LIMITER_HPP
|
||||
|
||||
class CurrentLimiter {
|
||||
public:
|
||||
virtual ~CurrentLimiter() = default;
|
||||
virtual float get_current_limit(float base_current_lim) const = 0;
|
||||
};
|
||||
|
||||
#endif // __CURRENT_LIMITER_HPP
|
||||
@@ -0,0 +1,842 @@
|
||||
|
||||
#include "odrive_main.h"
|
||||
#include <Drivers/STM32/stm32_system.h>
|
||||
#include <bitset>
|
||||
|
||||
Encoder::Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
|
||||
Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
|
||||
Stm32SpiArbiter* spi_arbiter) :
|
||||
timer_(timer), index_gpio_(index_gpio),
|
||||
hallA_gpio_(hallA_gpio), hallB_gpio_(hallB_gpio), hallC_gpio_(hallC_gpio),
|
||||
spi_arbiter_(spi_arbiter)
|
||||
{
|
||||
}
|
||||
|
||||
static void enc_index_cb_wrapper(void* ctx) {
|
||||
reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
|
||||
}
|
||||
|
||||
bool Encoder::apply_config(ODriveIntf::MotorIntf::MotorType motor_type) {
|
||||
config_.parent = this;
|
||||
|
||||
update_pll_gains();
|
||||
|
||||
if (config_.pre_calibrated) {
|
||||
if (config_.mode == Encoder::MODE_HALL && config_.hall_polarity_calibrated)
|
||||
is_ready_ = true;
|
||||
if (config_.mode == Encoder::MODE_SINCOS)
|
||||
is_ready_ = true;
|
||||
if (motor_type == Motor::MOTOR_TYPE_ACIM)
|
||||
is_ready_ = true;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Encoder::setup() {
|
||||
HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
|
||||
set_idx_subscribe();
|
||||
|
||||
mode_ = config_.mode;
|
||||
|
||||
spi_task_.config = {
|
||||
.Mode = SPI_MODE_MASTER,
|
||||
.Direction = SPI_DIRECTION_2LINES,
|
||||
.DataSize = SPI_DATASIZE_16BIT,
|
||||
.CLKPolarity = (mode_ == MODE_SPI_ABS_AEAT || mode_ == MODE_SPI_ABS_MA732) ? SPI_POLARITY_HIGH : SPI_POLARITY_LOW,
|
||||
.CLKPhase = SPI_PHASE_2EDGE,
|
||||
.NSS = SPI_NSS_SOFT,
|
||||
.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16,
|
||||
.FirstBit = SPI_FIRSTBIT_MSB,
|
||||
.TIMode = SPI_TIMODE_DISABLE,
|
||||
.CRCCalculation = SPI_CRCCALCULATION_DISABLE,
|
||||
.CRCPolynomial = 10,
|
||||
};
|
||||
|
||||
if (mode_ == MODE_SPI_ABS_MA732) {
|
||||
abs_spi_dma_tx_[0] = 0x0000;
|
||||
}
|
||||
|
||||
if(mode_ & MODE_FLAG_ABS){
|
||||
abs_spi_cs_pin_init();
|
||||
|
||||
if (axis_->controller_.config_.anticogging.pre_calibrated) {
|
||||
axis_->controller_.anticogging_valid_ = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::set_error(Error error) {
|
||||
vel_estimate_valid_ = false;
|
||||
pos_estimate_valid_ = false;
|
||||
error_ |= error;
|
||||
axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
|
||||
}
|
||||
|
||||
bool Encoder::do_checks(){
|
||||
return error_ == ERROR_NONE;
|
||||
}
|
||||
|
||||
//--------------------
|
||||
// Hardware Dependent
|
||||
//--------------------
|
||||
|
||||
// Triggered when an encoder passes over the "Index" pin
|
||||
// TODO: only arm index edge interrupt when we know encoder has powered up
|
||||
// (maybe by attaching the interrupt on start search, synergistic with following)
|
||||
void Encoder::enc_index_cb() {
|
||||
if (config_.use_index) {
|
||||
set_circular_count(0, false);
|
||||
if (config_.use_index_offset)
|
||||
set_linear_count((int32_t)(config_.index_offset * config_.cpr));
|
||||
if (config_.pre_calibrated) {
|
||||
is_ready_ = true;
|
||||
if(axis_->controller_.config_.anticogging.pre_calibrated){
|
||||
axis_->controller_.anticogging_valid_ = true;
|
||||
}
|
||||
} else {
|
||||
// We can't use the update_offset facility in set_circular_count because
|
||||
// we also set the linear count before there is a chance to update. Therefore:
|
||||
// Invalidate offset calibration that may have happened before idx search
|
||||
is_ready_ = false;
|
||||
}
|
||||
index_found_ = true;
|
||||
}
|
||||
|
||||
// Disable interrupt
|
||||
index_gpio_.unsubscribe();
|
||||
}
|
||||
|
||||
void Encoder::set_idx_subscribe(bool override_enable) {
|
||||
if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
|
||||
if (!index_gpio_.subscribe(true, false, enc_index_cb_wrapper, this)) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} else if (!config_.use_index || config_.find_idx_on_lockin_only) {
|
||||
index_gpio_.unsubscribe();
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::update_pll_gains() {
|
||||
pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time
|
||||
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
|
||||
|
||||
// Check that we don't get problems with discrete time approximation
|
||||
if (!(current_meas_period * pll_kp_ < 1.0f)) {
|
||||
set_error(ERROR_UNSTABLE_GAIN);
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::check_pre_calibrated() {
|
||||
// TODO: restoring config from python backup is fragile here (ACIM motor type must be set first)
|
||||
if (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_ACIM) {
|
||||
if (!is_ready_)
|
||||
config_.pre_calibrated = false;
|
||||
if (mode_ == MODE_INCREMENTAL && !index_found_)
|
||||
config_.pre_calibrated = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Function that sets the current encoder count to a desired 32-bit value.
|
||||
void Encoder::set_linear_count(int32_t count) {
|
||||
// Disable interrupts to make a critical section to avoid race condition
|
||||
uint32_t prim = cpu_enter_critical();
|
||||
|
||||
// Update states
|
||||
shadow_count_ = count;
|
||||
pos_estimate_counts_ = (float)count;
|
||||
tim_cnt_sample_ = count;
|
||||
|
||||
//Write hardware last
|
||||
timer_->Instance->CNT = count;
|
||||
|
||||
cpu_exit_critical(prim);
|
||||
}
|
||||
|
||||
// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
|
||||
// Note that this will get mod'ed down to [0, cpr)
|
||||
void Encoder::set_circular_count(int32_t count, bool update_offset) {
|
||||
// Disable interrupts to make a critical section to avoid race condition
|
||||
uint32_t prim = cpu_enter_critical();
|
||||
|
||||
if (update_offset) {
|
||||
config_.phase_offset += count - count_in_cpr_;
|
||||
config_.phase_offset = mod(config_.phase_offset, config_.cpr);
|
||||
}
|
||||
|
||||
// Update states
|
||||
count_in_cpr_ = mod(count, config_.cpr);
|
||||
pos_cpr_counts_ = (float)count_in_cpr_;
|
||||
|
||||
cpu_exit_critical(prim);
|
||||
}
|
||||
|
||||
bool Encoder::run_index_search() {
|
||||
config_.use_index = true;
|
||||
index_found_ = false;
|
||||
set_idx_subscribe();
|
||||
|
||||
bool success = axis_->run_lockin_spin(axis_->config_.calibration_lockin, false);
|
||||
return success;
|
||||
}
|
||||
|
||||
bool Encoder::run_direction_find() {
|
||||
int32_t init_enc_val = shadow_count_;
|
||||
|
||||
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
|
||||
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
|
||||
lockin_config.finish_on_distance = true;
|
||||
lockin_config.finish_on_enc_idx = false;
|
||||
lockin_config.finish_on_vel = false;
|
||||
bool success = axis_->run_lockin_spin(lockin_config, false);
|
||||
|
||||
if (success) {
|
||||
// Check response and direction
|
||||
if (shadow_count_ > init_enc_val + 8) {
|
||||
// motor same dir as encoder
|
||||
config_.direction = 1;
|
||||
} else if (shadow_count_ < init_enc_val - 8) {
|
||||
// motor opposite dir as encoder
|
||||
config_.direction = -1;
|
||||
} else {
|
||||
config_.direction = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
bool Encoder::run_hall_polarity_calibration() {
|
||||
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
|
||||
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
|
||||
lockin_config.finish_on_distance = true;
|
||||
lockin_config.finish_on_enc_idx = false;
|
||||
lockin_config.finish_on_vel = false;
|
||||
|
||||
auto loop_cb = [this](bool const_vel) {
|
||||
if (const_vel)
|
||||
sample_hall_states_ = true;
|
||||
// No need to cancel early
|
||||
return true;
|
||||
};
|
||||
|
||||
config_.hall_polarity_calibrated = false;
|
||||
states_seen_count_.fill(0);
|
||||
bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb);
|
||||
sample_hall_states_ = false;
|
||||
|
||||
if (success) {
|
||||
std::bitset<8> state_seen;
|
||||
std::bitset<8> state_confirmed;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (states_seen_count_[i] > 0)
|
||||
state_seen[i] = true;
|
||||
if (states_seen_count_[i] > 50)
|
||||
state_confirmed[i] = true;
|
||||
}
|
||||
if (!(state_seen == state_confirmed)) {
|
||||
set_error(ERROR_ILLEGAL_HALL_STATE);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Hall effect sensors can be arranged at 60 or 120 electrical degrees.
|
||||
// Out of 8 possible states, 120 and 60 deg arrangements each miss 2 states.
|
||||
// ODrive assumes 120 deg separation - if a 60 deg setup is used, it can
|
||||
// be converted to 120 deg states by flipping the polarity of one sensor.
|
||||
uint8_t states = state_seen.to_ulong();
|
||||
uint8_t hall_polarity = 0;
|
||||
auto flip_detect = [](uint8_t states, unsigned int idx)->bool {
|
||||
return (~states & 0xFF) == (1<<(0+idx) | 1<<(7-idx));
|
||||
};
|
||||
if (flip_detect(states, 0)) {
|
||||
hall_polarity = 0b000;
|
||||
} else if (flip_detect(states, 1)) {
|
||||
hall_polarity = 0b001;
|
||||
} else if (flip_detect(states, 2)) {
|
||||
hall_polarity = 0b010;
|
||||
} else if (flip_detect(states, 3)) {
|
||||
hall_polarity = 0b100;
|
||||
} else {
|
||||
set_error(ERROR_ILLEGAL_HALL_STATE);
|
||||
return false;
|
||||
}
|
||||
config_.hall_polarity = hall_polarity;
|
||||
config_.hall_polarity_calibrated = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool Encoder::run_hall_phase_calibration() {
|
||||
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
|
||||
lockin_config.finish_distance = lockin_config.vel * 30.0f; // run for 30 seconds
|
||||
lockin_config.finish_on_distance = true;
|
||||
lockin_config.finish_on_enc_idx = false;
|
||||
lockin_config.finish_on_vel = false;
|
||||
|
||||
auto loop_cb = [this](bool const_vel) {
|
||||
if (const_vel)
|
||||
sample_hall_phase_ = true;
|
||||
// No need to cancel early
|
||||
return true;
|
||||
};
|
||||
|
||||
// TODO: There is a race condition here with the execution in Encoder::update.
|
||||
// We should evaluate making thread execution synchronous with the control loops
|
||||
// at least optionally.
|
||||
// Perhaps the new loop_sync feature will give a loose timing guarantee that may be sufficient
|
||||
calibrate_hall_phase_ = true;
|
||||
config_.hall_edge_phcnt.fill(0.0f);
|
||||
hall_phase_calib_seen_count_.fill(0);
|
||||
bool success = axis_->run_lockin_spin(lockin_config, false, loop_cb);
|
||||
if (error_ & ERROR_ILLEGAL_HALL_STATE)
|
||||
success = false;
|
||||
|
||||
if (success) {
|
||||
// Check deltas to dicern rotation direction
|
||||
float delta_phase = 0.0f;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
int next_i = (i == 5) ? 0 : i+1;
|
||||
delta_phase += wrap_pm_pi(config_.hall_edge_phcnt[next_i] - config_.hall_edge_phcnt[i]);
|
||||
}
|
||||
// Correct reverse rotation
|
||||
if (delta_phase < 0.0f) {
|
||||
config_.direction = -1;
|
||||
for (int i = 0; i < 6; i++)
|
||||
config_.hall_edge_phcnt[i] = wrap_pm_pi(-config_.hall_edge_phcnt[i]);
|
||||
} else {
|
||||
config_.direction = 1;
|
||||
}
|
||||
// Normalize edge timing to 1st edge in sequence, and change units to counts
|
||||
float offset = config_.hall_edge_phcnt[0];
|
||||
for (int i = 0; i < 6; i++) {
|
||||
float& phcnt = config_.hall_edge_phcnt[i];
|
||||
phcnt = fmodf_pos((6.0f / (2.0f * M_PI)) * (phcnt - offset), 6.0f);
|
||||
}
|
||||
} else {
|
||||
config_.hall_edge_phcnt = hall_edge_defaults;
|
||||
}
|
||||
|
||||
calibrate_hall_phase_ = false;
|
||||
return success;
|
||||
}
|
||||
|
||||
// @brief Turns the motor in one direction for a bit and then in the other
|
||||
// direction in order to find the offset between the electrical phase 0
|
||||
// and the encoder state 0.
|
||||
bool Encoder::run_offset_calibration() {
|
||||
const float start_lock_duration = 1.0f;
|
||||
|
||||
// Require index found if enabled
|
||||
if (config_.use_index && !index_found_) {
|
||||
set_error(ERROR_INDEX_NOT_FOUND_YET);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (config_.mode == MODE_HALL && !config_.hall_polarity_calibrated) {
|
||||
set_error(ERROR_HALL_NOT_CALIBRATED_YET);
|
||||
return false;
|
||||
}
|
||||
|
||||
// We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_
|
||||
// Therefore we have to sync them for calibration
|
||||
shadow_count_ = count_in_cpr_;
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
// Reset state variables
|
||||
axis_->open_loop_controller_.Idq_setpoint_ = {0.0f, 0.0f};
|
||||
axis_->open_loop_controller_.Vdq_setpoint_ = {0.0f, 0.0f};
|
||||
axis_->open_loop_controller_.phase_ = 0.0f;
|
||||
axis_->open_loop_controller_.phase_vel_ = 0.0f;
|
||||
|
||||
float max_current_ramp = axis_->motor_.config_.calibration_current / start_lock_duration * 2.0f;
|
||||
axis_->open_loop_controller_.max_current_ramp_ = max_current_ramp;
|
||||
axis_->open_loop_controller_.max_voltage_ramp_ = max_current_ramp;
|
||||
axis_->open_loop_controller_.max_phase_vel_ramp_ = INFINITY;
|
||||
axis_->open_loop_controller_.target_current_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? axis_->motor_.config_.calibration_current : 0.0f;
|
||||
axis_->open_loop_controller_.target_voltage_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : axis_->motor_.config_.calibration_current;
|
||||
axis_->open_loop_controller_.target_vel_ = 0.0f;
|
||||
axis_->open_loop_controller_.total_distance_ = 0.0f;
|
||||
axis_->open_loop_controller_.phase_ = axis_->open_loop_controller_.initial_phase_ = wrap_pm_pi(0 - config_.calib_scan_distance / 2.0f);
|
||||
|
||||
axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL);
|
||||
axis_->motor_.current_control_.Idq_setpoint_src_.connect_to(&axis_->open_loop_controller_.Idq_setpoint_);
|
||||
axis_->motor_.current_control_.Vdq_setpoint_src_.connect_to(&axis_->open_loop_controller_.Vdq_setpoint_);
|
||||
|
||||
axis_->motor_.current_control_.phase_src_.connect_to(&axis_->open_loop_controller_.phase_);
|
||||
axis_->acim_estimator_.rotor_phase_src_.connect_to(&axis_->open_loop_controller_.phase_);
|
||||
|
||||
axis_->motor_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
|
||||
axis_->motor_.current_control_.phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
|
||||
axis_->acim_estimator_.rotor_phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_);
|
||||
}
|
||||
axis_->wait_for_control_iteration();
|
||||
|
||||
axis_->motor_.arm(&axis_->motor_.current_control_);
|
||||
|
||||
// go to start position of forward scan for start_lock_duration to get ready to scan
|
||||
for (size_t i = 0; i < (size_t)(start_lock_duration * 1000.0f); ++i) {
|
||||
if (!axis_->motor_.is_armed_) {
|
||||
return false; // TODO: return "disarmed" error code
|
||||
}
|
||||
if (axis_->requested_state_ != Axis::AXIS_STATE_UNDEFINED) {
|
||||
axis_->motor_.disarm();
|
||||
return false; // TODO: return "aborted" error code
|
||||
}
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
|
||||
int32_t init_enc_val = shadow_count_;
|
||||
uint32_t num_steps = 0;
|
||||
int64_t encvaluesum = 0;
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
axis_->open_loop_controller_.target_vel_ = config_.calib_scan_omega;
|
||||
axis_->open_loop_controller_.total_distance_ = 0.0f;
|
||||
}
|
||||
|
||||
// scan forward
|
||||
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
|
||||
bool reached_target_dist = axis_->open_loop_controller_.total_distance_.any().value_or(-INFINITY) >= config_.calib_scan_distance;
|
||||
if (reached_target_dist) {
|
||||
break;
|
||||
}
|
||||
encvaluesum += shadow_count_;
|
||||
num_steps++;
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
// Check response and direction
|
||||
if (shadow_count_ > init_enc_val + 8) {
|
||||
// motor same dir as encoder
|
||||
config_.direction = 1;
|
||||
} else if (shadow_count_ < init_enc_val - 8) {
|
||||
// motor opposite dir as encoder
|
||||
config_.direction = -1;
|
||||
} else {
|
||||
// Encoder response error
|
||||
set_error(ERROR_NO_RESPONSE);
|
||||
axis_->motor_.disarm();
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check CPR
|
||||
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
|
||||
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
|
||||
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
|
||||
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
|
||||
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
|
||||
axis_->motor_.disarm();
|
||||
return false;
|
||||
}
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
axis_->open_loop_controller_.target_vel_ = -config_.calib_scan_omega;
|
||||
}
|
||||
|
||||
// scan backwards
|
||||
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
|
||||
bool reached_target_dist = axis_->open_loop_controller_.total_distance_.any().value_or(INFINITY) <= 0.0f;
|
||||
if (reached_target_dist) {
|
||||
break;
|
||||
}
|
||||
encvaluesum += shadow_count_;
|
||||
num_steps++;
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
// Motor disarmed because of an error
|
||||
if (!axis_->motor_.is_armed_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
axis_->motor_.disarm();
|
||||
|
||||
config_.phase_offset = encvaluesum / num_steps;
|
||||
int32_t residual = encvaluesum - ((int64_t)config_.phase_offset * (int64_t)num_steps);
|
||||
config_.phase_offset_float = (float)residual / (float)num_steps + 0.5f; // add 0.5 to center-align state to phase
|
||||
|
||||
is_ready_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
|
||||
switch (hall_state) {
|
||||
case 0b001: *hall_cnt = 0; return true;
|
||||
case 0b011: *hall_cnt = 1; return true;
|
||||
case 0b010: *hall_cnt = 2; return true;
|
||||
case 0b110: *hall_cnt = 3; return true;
|
||||
case 0b100: *hall_cnt = 4; return true;
|
||||
case 0b101: *hall_cnt = 5; return true;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
void Encoder::sample_now() {
|
||||
switch (mode_) {
|
||||
case MODE_INCREMENTAL: {
|
||||
tim_cnt_sample_ = (int16_t)timer_->Instance->CNT;
|
||||
} break;
|
||||
|
||||
case MODE_HALL: {
|
||||
// do nothing: samples already captured in general GPIO capture
|
||||
} break;
|
||||
|
||||
case MODE_SINCOS: {
|
||||
sincos_sample_s_ = get_adc_relative_voltage(get_gpio(config_.sincos_gpio_pin_sin)) - 0.5f;
|
||||
sincos_sample_c_ = get_adc_relative_voltage(get_gpio(config_.sincos_gpio_pin_cos)) - 0.5f;
|
||||
} break;
|
||||
|
||||
case MODE_SPI_ABS_AMS:
|
||||
case MODE_SPI_ABS_CUI:
|
||||
case MODE_SPI_ABS_AEAT:
|
||||
case MODE_SPI_ABS_RLS:
|
||||
case MODE_SPI_ABS_MA732:
|
||||
{
|
||||
abs_spi_start_transaction();
|
||||
// Do nothing
|
||||
} break;
|
||||
|
||||
default: {
|
||||
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
||||
} break;
|
||||
}
|
||||
|
||||
// Sample all GPIO digital input data registers, used for HALL sensors for example.
|
||||
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
|
||||
port_samples_[i] = ports_to_sample[i]->IDR;
|
||||
}
|
||||
}
|
||||
|
||||
bool Encoder::read_sampled_gpio(Stm32Gpio gpio) {
|
||||
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
|
||||
if (ports_to_sample[i] == gpio.port_) {
|
||||
return port_samples_[i] & gpio.pin_mask_;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void Encoder::decode_hall_samples() {
|
||||
hall_state_ = (read_sampled_gpio(hallA_gpio_) ? 1 : 0)
|
||||
| (read_sampled_gpio(hallB_gpio_) ? 2 : 0)
|
||||
| (read_sampled_gpio(hallC_gpio_) ? 4 : 0);
|
||||
}
|
||||
|
||||
bool Encoder::abs_spi_start_transaction() {
|
||||
if (mode_ & MODE_FLAG_ABS){
|
||||
if (Stm32SpiArbiter::acquire_task(&spi_task_)) {
|
||||
spi_task_.ncs_gpio = abs_spi_cs_gpio_;
|
||||
spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_;
|
||||
spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_;
|
||||
spi_task_.length = 1;
|
||||
spi_task_.on_complete = [](void* ctx, bool success) { ((Encoder*)ctx)->abs_spi_cb(success); };
|
||||
spi_task_.on_complete_ctx = this;
|
||||
spi_task_.next = nullptr;
|
||||
|
||||
spi_arbiter_->transfer_async(&spi_task_);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t ams_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
v ^= v >> 1;
|
||||
return v & 1;
|
||||
}
|
||||
|
||||
uint8_t cui_parity(uint16_t v) {
|
||||
v ^= v >> 8;
|
||||
v ^= v >> 4;
|
||||
v ^= v >> 2;
|
||||
return ~v & 3;
|
||||
}
|
||||
|
||||
void Encoder::abs_spi_cb(bool success) {
|
||||
uint16_t pos;
|
||||
|
||||
if (!success) {
|
||||
goto done;
|
||||
}
|
||||
|
||||
switch (mode_) {
|
||||
case MODE_SPI_ABS_AMS: {
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
// check if parity is correct (even) and error flag clear
|
||||
if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
|
||||
goto done;
|
||||
}
|
||||
pos = rawVal & 0x3fff;
|
||||
} break;
|
||||
|
||||
case MODE_SPI_ABS_CUI: {
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
// check if parity is correct
|
||||
if (cui_parity(rawVal)) {
|
||||
goto done;
|
||||
}
|
||||
pos = rawVal & 0x3fff;
|
||||
} break;
|
||||
|
||||
case MODE_SPI_ABS_RLS: {
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
pos = (rawVal >> 2) & 0x3fff;
|
||||
} break;
|
||||
|
||||
case MODE_SPI_ABS_MA732: {
|
||||
uint16_t rawVal = abs_spi_dma_rx_[0];
|
||||
pos = (rawVal >> 2) & 0x3fff;
|
||||
} break;
|
||||
|
||||
default: {
|
||||
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
||||
goto done;
|
||||
} break;
|
||||
}
|
||||
|
||||
pos_abs_ = pos;
|
||||
abs_spi_pos_updated_ = true;
|
||||
if (config_.pre_calibrated) {
|
||||
is_ready_ = true;
|
||||
}
|
||||
|
||||
done:
|
||||
Stm32SpiArbiter::release_task(&spi_task_);
|
||||
}
|
||||
|
||||
void Encoder::abs_spi_cs_pin_init(){
|
||||
// Decode and init cs pin
|
||||
#if HW_VERSION_MAJOR == 4
|
||||
if (mode_ == MODE_SPI_ABS_MA732)
|
||||
abs_spi_cs_gpio_ = {GPIOA, GPIO_PIN_15};
|
||||
else
|
||||
#else
|
||||
abs_spi_cs_gpio_ = get_gpio(config_.abs_spi_cs_gpio_pin);
|
||||
#endif
|
||||
abs_spi_cs_gpio_.config(GPIO_MODE_OUTPUT_PP, GPIO_PULLUP);
|
||||
|
||||
// Write pin high
|
||||
abs_spi_cs_gpio_.write(true);
|
||||
}
|
||||
|
||||
// Note that this may return counts +1 or -1 without any wrapping
|
||||
int32_t Encoder::hall_model(float internal_pos) {
|
||||
int32_t base_cnt = (int32_t)std::floor(internal_pos);
|
||||
|
||||
float pos_in_range = fmodf_pos(internal_pos, 6.0f);
|
||||
int pos_idx = (int)pos_in_range;
|
||||
if (pos_idx == 6) pos_idx = 5; // in case of rounding error
|
||||
int next_i = (pos_idx == 5) ? 0 : pos_idx+1;
|
||||
|
||||
float below_edge = config_.hall_edge_phcnt[pos_idx];
|
||||
float above_edge = config_.hall_edge_phcnt[next_i];
|
||||
|
||||
// if we are blow the "below" edge, we are the count under
|
||||
if (wrap_pm(pos_in_range - below_edge, 6.0f) < 0.0f)
|
||||
return base_cnt - 1;
|
||||
// if we are above the "above" edge, we are the count over
|
||||
else if (wrap_pm(pos_in_range - above_edge, 6.0f) > 0.0f)
|
||||
return base_cnt + 1;
|
||||
// otherwise we are in the nominal count (or completely lost)
|
||||
return base_cnt;
|
||||
}
|
||||
|
||||
bool Encoder::update() {
|
||||
// update internal encoder state.
|
||||
int32_t delta_enc = 0;
|
||||
int32_t pos_abs_latched = pos_abs_; //LATCH
|
||||
|
||||
switch (mode_) {
|
||||
case MODE_INCREMENTAL: {
|
||||
//TODO: use count_in_cpr_ instead as shadow_count_ can overflow
|
||||
//or use 64 bit
|
||||
int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
|
||||
delta_enc = (int32_t)delta_enc_16; //sign extend
|
||||
} break;
|
||||
|
||||
case MODE_HALL: {
|
||||
decode_hall_samples();
|
||||
if (sample_hall_states_) {
|
||||
states_seen_count_[hall_state_]++;
|
||||
}
|
||||
if (config_.hall_polarity_calibrated) {
|
||||
int32_t hall_cnt;
|
||||
if (decode_hall((hall_state_ ^ config_.hall_polarity), &hall_cnt)) {
|
||||
if (calibrate_hall_phase_) {
|
||||
if (sample_hall_phase_ && last_hall_cnt_.has_value()) {
|
||||
int mod_hall_cnt = mod(hall_cnt - last_hall_cnt_.value(), 6);
|
||||
size_t edge_idx;
|
||||
if (mod_hall_cnt == 0) { goto skip; } // no count - do nothing
|
||||
else if (mod_hall_cnt == 1) { // counted up
|
||||
edge_idx = hall_cnt;
|
||||
} else if (mod_hall_cnt == 5) { // counted down
|
||||
edge_idx = last_hall_cnt_.value();
|
||||
} else {
|
||||
set_error(ERROR_ILLEGAL_HALL_STATE);
|
||||
return false;
|
||||
}
|
||||
|
||||
auto maybe_phase = axis_->open_loop_controller_.phase_.any();
|
||||
if (maybe_phase) {
|
||||
float phase = maybe_phase.value();
|
||||
// Early increment to get the right divisor in recursive average
|
||||
hall_phase_calib_seen_count_[edge_idx]++;
|
||||
float& edge_phase = config_.hall_edge_phcnt[edge_idx];
|
||||
if (hall_phase_calib_seen_count_[edge_idx] == 1)
|
||||
edge_phase = phase;
|
||||
else {
|
||||
// circularly wrapped recursive average
|
||||
edge_phase += (phase - edge_phase) / hall_phase_calib_seen_count_[edge_idx];
|
||||
edge_phase = wrap_pm_pi(edge_phase);
|
||||
}
|
||||
}
|
||||
}
|
||||
skip:
|
||||
last_hall_cnt_ = hall_cnt;
|
||||
|
||||
return true; // Skip all velocity and phase estimation
|
||||
}
|
||||
|
||||
delta_enc = hall_cnt - count_in_cpr_;
|
||||
delta_enc = mod(delta_enc, 6);
|
||||
if (delta_enc > 3)
|
||||
delta_enc -= 6;
|
||||
} else {
|
||||
if (!config_.ignore_illegal_hall_state) {
|
||||
set_error(ERROR_ILLEGAL_HALL_STATE);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
} break;
|
||||
|
||||
case MODE_SINCOS: {
|
||||
float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
|
||||
int fake_count = (int)(1000.0f * phase);
|
||||
//CPR = 6283 = 2pi * 1k
|
||||
|
||||
delta_enc = fake_count - count_in_cpr_;
|
||||
delta_enc = mod(delta_enc, 6283);
|
||||
if (delta_enc > 6283/2)
|
||||
delta_enc -= 6283;
|
||||
} break;
|
||||
|
||||
case MODE_SPI_ABS_RLS:
|
||||
case MODE_SPI_ABS_AMS:
|
||||
case MODE_SPI_ABS_CUI:
|
||||
case MODE_SPI_ABS_AEAT:
|
||||
case MODE_SPI_ABS_MA732: {
|
||||
if (abs_spi_pos_updated_ == false) {
|
||||
// Low pass filter the error
|
||||
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
|
||||
if (spi_error_rate_ > 0.05f) {
|
||||
set_error(ERROR_ABS_SPI_COM_FAIL);
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
// Low pass filter the error
|
||||
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
|
||||
}
|
||||
|
||||
abs_spi_pos_updated_ = false;
|
||||
delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
|
||||
delta_enc = mod(delta_enc, config_.cpr);
|
||||
if (delta_enc > config_.cpr/2) {
|
||||
delta_enc -= config_.cpr;
|
||||
}
|
||||
|
||||
}break;
|
||||
default: {
|
||||
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
|
||||
return false;
|
||||
} break;
|
||||
}
|
||||
|
||||
shadow_count_ += delta_enc;
|
||||
count_in_cpr_ += delta_enc;
|
||||
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
|
||||
|
||||
if(mode_ & MODE_FLAG_ABS)
|
||||
count_in_cpr_ = pos_abs_latched;
|
||||
|
||||
// Memory for pos_circular
|
||||
float pos_cpr_counts_last = pos_cpr_counts_;
|
||||
|
||||
//// run pll (for now pll is in units of encoder counts)
|
||||
// Predict current pos
|
||||
pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
|
||||
pos_cpr_counts_ += current_meas_period * vel_estimate_counts_;
|
||||
// Encoder model
|
||||
auto encoder_model = [this](float internal_pos)->int32_t {
|
||||
if (config_.mode == MODE_HALL)
|
||||
return hall_model(internal_pos);
|
||||
else
|
||||
return (int32_t)std::floor(internal_pos);
|
||||
};
|
||||
// discrete phase detector
|
||||
float delta_pos_counts = (float)(shadow_count_ - encoder_model(pos_estimate_counts_));
|
||||
float delta_pos_cpr_counts = (float)(count_in_cpr_ - encoder_model(pos_cpr_counts_));
|
||||
delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, (float)(config_.cpr));
|
||||
delta_pos_cpr_counts_ += 0.1f * (delta_pos_cpr_counts - delta_pos_cpr_counts_); // for debug
|
||||
// pll feedback
|
||||
pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
|
||||
pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
|
||||
pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
|
||||
vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
|
||||
bool snap_to_zero_vel = false;
|
||||
if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
|
||||
vel_estimate_counts_ = 0.0f; //align delta-sigma on zero to prevent jitter
|
||||
snap_to_zero_vel = true;
|
||||
}
|
||||
|
||||
// Outputs from Encoder for Controller
|
||||
pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
|
||||
vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
|
||||
|
||||
// TODO: we should strictly require that this value is from the previous iteration
|
||||
// to avoid spinout scenarios. However that requires a proper way to reset
|
||||
// the encoder from error states.
|
||||
float pos_circular = pos_circular_.any().value_or(0.0f);
|
||||
pos_circular += wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 1.0f);
|
||||
pos_circular = fmodf_pos(pos_circular, axis_->controller_.config_.circular_setpoint_range);
|
||||
pos_circular_ = pos_circular;
|
||||
|
||||
//// run encoder count interpolation
|
||||
int32_t corrected_enc = count_in_cpr_ - config_.phase_offset;
|
||||
// if we are stopped, make sure we don't randomly drift
|
||||
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
|
||||
interpolation_ = 0.5f;
|
||||
// reset interpolation if encoder edge comes
|
||||
// TODO: This isn't correct. At high velocities the first phase in this count may very well not be at the edge.
|
||||
} else if (delta_enc > 0) {
|
||||
interpolation_ = 0.0f;
|
||||
} else if (delta_enc < 0) {
|
||||
interpolation_ = 1.0f;
|
||||
} else {
|
||||
// Interpolate (predict) between encoder counts using vel_estimate,
|
||||
interpolation_ += current_meas_period * vel_estimate_counts_;
|
||||
// don't allow interpolation indicated position outside of [enc, enc+1)
|
||||
if (interpolation_ > 1.0f) interpolation_ = 1.0f;
|
||||
if (interpolation_ < 0.0f) interpolation_ = 0.0f;
|
||||
}
|
||||
float interpolated_enc = corrected_enc + interpolation_;
|
||||
|
||||
//// compute electrical phase
|
||||
//TODO avoid recomputing elec_rad_per_enc every time
|
||||
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
|
||||
float ph = elec_rad_per_enc * (interpolated_enc - config_.phase_offset_float);
|
||||
|
||||
if (is_ready_) {
|
||||
phase_ = wrap_pm_pi(ph) * config_.direction;
|
||||
phase_vel_ = (2*M_PI) * *vel_estimate_.present() * axis_->motor_.config_.pole_pairs * config_.direction;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
#ifndef __ENCODER_HPP
|
||||
#define __ENCODER_HPP
|
||||
|
||||
class Encoder;
|
||||
|
||||
#include <board.h> // needed for arm_math.h
|
||||
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
|
||||
#include "utils.hpp"
|
||||
#include <autogen/interfaces.hpp>
|
||||
#include "component.hpp"
|
||||
|
||||
|
||||
class Encoder : public ODriveIntf::EncoderIntf {
|
||||
public:
|
||||
static constexpr uint32_t MODE_FLAG_ABS = 0x100;
|
||||
static constexpr std::array<float, 6> hall_edge_defaults =
|
||||
{0.0f, 1.0f, 2.0f, 3.0f, 4.0f, 5.0f};
|
||||
|
||||
struct Config_t {
|
||||
Mode mode = MODE_INCREMENTAL;
|
||||
float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
|
||||
float calib_scan_distance = 16.0f * M_PI; // rad electrical
|
||||
float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
|
||||
float bandwidth = 1000.0f;
|
||||
int32_t phase_offset = 0; // Offset between encoder count and rotor electrical phase
|
||||
float phase_offset_float = 0.0f; // Sub-count phase alignment offset
|
||||
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
|
||||
float index_offset = 0.0f;
|
||||
bool use_index = false;
|
||||
bool pre_calibrated = false; // If true, this means the offset stored in
|
||||
// configuration is valid and does not need
|
||||
// be determined by run_offset_calibration.
|
||||
// In this case the encoder will enter ready
|
||||
// state as soon as the index is found.
|
||||
int32_t direction = 0; // direction with respect to motor
|
||||
bool use_index_offset = true;
|
||||
bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
|
||||
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
|
||||
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
|
||||
uint8_t hall_polarity = 0;
|
||||
bool hall_polarity_calibrated = false;
|
||||
std::array<float, 6> hall_edge_phcnt = hall_edge_defaults;
|
||||
uint16_t abs_spi_cs_gpio_pin = 1;
|
||||
uint16_t sincos_gpio_pin_sin = 3;
|
||||
uint16_t sincos_gpio_pin_cos = 4;
|
||||
|
||||
|
||||
// custom setters
|
||||
Encoder* parent = nullptr;
|
||||
void set_use_index(bool value) { use_index = value; parent->set_idx_subscribe(); }
|
||||
void set_find_idx_on_lockin_only(bool value) { find_idx_on_lockin_only = value; parent->set_idx_subscribe(); }
|
||||
void set_abs_spi_cs_gpio_pin(uint16_t value) { abs_spi_cs_gpio_pin = value; parent->abs_spi_cs_pin_init(); }
|
||||
void set_pre_calibrated(bool value) { pre_calibrated = value; parent->check_pre_calibrated(); }
|
||||
void set_bandwidth(float value) { bandwidth = value; parent->update_pll_gains(); }
|
||||
};
|
||||
|
||||
Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
|
||||
Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
|
||||
Stm32SpiArbiter* spi_arbiter);
|
||||
|
||||
bool apply_config(ODriveIntf::MotorIntf::MotorType motor_type);
|
||||
void setup();
|
||||
void set_error(Error error);
|
||||
bool do_checks();
|
||||
|
||||
void enc_index_cb();
|
||||
void set_idx_subscribe(bool override_enable = false);
|
||||
void update_pll_gains();
|
||||
void check_pre_calibrated();
|
||||
|
||||
void set_linear_count(int32_t count);
|
||||
void set_circular_count(int32_t count, bool update_offset);
|
||||
bool calib_enc_offset(float voltage_magnitude);
|
||||
|
||||
bool run_index_search();
|
||||
bool run_direction_find();
|
||||
bool run_hall_polarity_calibration();
|
||||
bool run_hall_phase_calibration();
|
||||
bool run_offset_calibration();
|
||||
void sample_now();
|
||||
bool read_sampled_gpio(Stm32Gpio gpio);
|
||||
void decode_hall_samples();
|
||||
int32_t hall_model(float internal_pos);
|
||||
bool update();
|
||||
|
||||
TIM_HandleTypeDef* timer_;
|
||||
Stm32Gpio index_gpio_;
|
||||
Stm32Gpio hallA_gpio_;
|
||||
Stm32Gpio hallB_gpio_;
|
||||
Stm32Gpio hallC_gpio_;
|
||||
Stm32SpiArbiter* spi_arbiter_;
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
|
||||
Config_t config_;
|
||||
|
||||
Error error_ = ERROR_NONE;
|
||||
bool index_found_ = false;
|
||||
bool is_ready_ = false;
|
||||
int32_t shadow_count_ = 0;
|
||||
int32_t count_in_cpr_ = 0;
|
||||
float interpolation_ = 0.0f;
|
||||
OutputPort<float> phase_ = 0.0f; // [rad]
|
||||
OutputPort<float> phase_vel_ = 0.0f; // [rad/s]
|
||||
float pos_estimate_counts_ = 0.0f; // [count]
|
||||
float pos_cpr_counts_ = 0.0f; // [count]
|
||||
float delta_pos_cpr_counts_ = 0.0f; // [count] phase detector result for debug
|
||||
float vel_estimate_counts_ = 0.0f; // [count/s]
|
||||
float pll_kp_ = 0.0f; // [count/s / count]
|
||||
float pll_ki_ = 0.0f; // [(count/s^2) / count]
|
||||
float calib_scan_response_ = 0.0f; // debug report from offset calib
|
||||
int32_t pos_abs_ = 0;
|
||||
float spi_error_rate_ = 0.0f;
|
||||
|
||||
OutputPort<float> pos_estimate_ = 0.0f; // [turn]
|
||||
OutputPort<float> vel_estimate_ = 0.0f; // [turn/s]
|
||||
OutputPort<float> pos_circular_ = 0.0f; // [turn]
|
||||
|
||||
bool pos_estimate_valid_ = false;
|
||||
bool vel_estimate_valid_ = false;
|
||||
|
||||
int16_t tim_cnt_sample_ = 0; //
|
||||
static const constexpr GPIO_TypeDef* ports_to_sample[] = { GPIOA, GPIOB, GPIOC };
|
||||
uint16_t port_samples_[sizeof(ports_to_sample) / sizeof(ports_to_sample[0])];
|
||||
// Updated by low_level pwm_adc_cb
|
||||
uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
|
||||
std::optional<uint8_t> last_hall_cnt_ = std::nullopt; // Used to find hall edges for calibration
|
||||
bool calibrate_hall_phase_ = false;
|
||||
bool sample_hall_states_ = false;
|
||||
bool sample_hall_phase_ = false;
|
||||
std::array<int, 8> states_seen_count_; // for hall polarity calibration
|
||||
std::array<int, 6> hall_phase_calib_seen_count_;
|
||||
|
||||
float sincos_sample_s_ = 0.0f;
|
||||
float sincos_sample_c_ = 0.0f;
|
||||
|
||||
bool abs_spi_start_transaction();
|
||||
void abs_spi_cb(bool success);
|
||||
void abs_spi_cs_pin_init();
|
||||
bool abs_spi_pos_updated_ = false;
|
||||
Mode mode_ = MODE_INCREMENTAL;
|
||||
Stm32Gpio abs_spi_cs_gpio_;
|
||||
uint32_t abs_spi_cr1;
|
||||
uint32_t abs_spi_cr2;
|
||||
uint16_t abs_spi_dma_tx_[1] = {0xFFFF};
|
||||
uint16_t abs_spi_dma_rx_[1];
|
||||
Stm32SpiArbiter::SpiTask spi_task_;
|
||||
|
||||
constexpr float getCoggingRatio(){
|
||||
return 1.0f / 3600.0f;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // __ENCODER_HPP
|
||||
@@ -0,0 +1,32 @@
|
||||
#include <odrive_main.h>
|
||||
|
||||
|
||||
void Endstop::update() {
|
||||
debounceTimer_.update();
|
||||
last_state_ = endstop_state_;
|
||||
if (config_.enabled) {
|
||||
bool last_pin_state = pin_state_;
|
||||
|
||||
pin_state_ = get_gpio(config_.gpio_num).read();
|
||||
|
||||
// If the pin state has changed, reset the timer
|
||||
if (pin_state_ != last_pin_state)
|
||||
debounceTimer_.reset();
|
||||
|
||||
if (debounceTimer_.expired())
|
||||
endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state
|
||||
} else {
|
||||
endstop_state_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Endstop::apply_config() {
|
||||
debounceTimer_.reset();
|
||||
if (config_.enabled) {
|
||||
debounceTimer_.start();
|
||||
} else {
|
||||
debounceTimer_.stop();
|
||||
}
|
||||
debounceTimer_.setIncrement(config_.debounce_ms * 0.001f);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
#ifndef __ENDSTOP_HPP
|
||||
#define __ENDSTOP_HPP
|
||||
|
||||
#include "timer.hpp"
|
||||
class Endstop {
|
||||
public:
|
||||
struct Config_t {
|
||||
float offset = 0;
|
||||
uint32_t debounce_ms = 50;
|
||||
uint16_t gpio_num = 0;
|
||||
bool enabled = false;
|
||||
bool is_active_high = false;
|
||||
|
||||
// custom setters
|
||||
Endstop* parent = nullptr;
|
||||
void set_gpio_num(uint16_t value) { gpio_num = value; parent->apply_config(); }
|
||||
void set_enabled(uint32_t value) { enabled = value; parent->apply_config(); }
|
||||
void set_debounce_ms(uint32_t value) { debounce_ms = value; parent->apply_config(); }
|
||||
};
|
||||
|
||||
|
||||
Endstop::Config_t config_;
|
||||
Axis* axis_ = nullptr;
|
||||
|
||||
bool apply_config();
|
||||
|
||||
void update();
|
||||
constexpr bool get_state(){
|
||||
return endstop_state_;
|
||||
}
|
||||
|
||||
constexpr bool rose(){
|
||||
return (endstop_state_ != last_state_) && endstop_state_;
|
||||
}
|
||||
|
||||
constexpr bool fell(){
|
||||
return (endstop_state_ != last_state_) && !endstop_state_;
|
||||
}
|
||||
|
||||
bool endstop_state_ = false;
|
||||
|
||||
private:
|
||||
bool last_state_ = false;
|
||||
bool pin_state_ = false;
|
||||
float pos_when_pressed_ = 0.0f;
|
||||
Timer<float> debounceTimer_;
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,37 @@
|
||||
[
|
||||
{
|
||||
"name": "",
|
||||
"id": 0,
|
||||
"type": "json"
|
||||
},
|
||||
{
|
||||
"name": "subscriptions",
|
||||
"id": 1,
|
||||
"type": "int32[]"
|
||||
},
|
||||
{
|
||||
"name": "motor0",
|
||||
"id": 2,
|
||||
"type": "tree",
|
||||
"content": [
|
||||
{
|
||||
"name": "pos_setpoint",
|
||||
"id": 3,
|
||||
"type": "float",
|
||||
"access": "rw"
|
||||
},
|
||||
{
|
||||
"name": "pos_gain",
|
||||
"id": 4,
|
||||
"type": "float",
|
||||
"access": "rw"
|
||||
},
|
||||
{
|
||||
"name": "vel_setpoint",
|
||||
"id": 5,
|
||||
"type": "float",
|
||||
"access": "rw"
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
@@ -0,0 +1,193 @@
|
||||
|
||||
#include "foc.hpp"
|
||||
#include <board.h>
|
||||
|
||||
Motor::Error AlphaBetaFrameController::on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<std::array<float, 3>> currents,
|
||||
uint32_t input_timestamp) {
|
||||
|
||||
std::optional<float2D> Ialpha_beta;
|
||||
|
||||
if (currents.has_value()) {
|
||||
// Clarke transform
|
||||
Ialpha_beta = {
|
||||
(*currents)[0],
|
||||
one_by_sqrt3 * ((*currents)[1] - (*currents)[2])
|
||||
};
|
||||
}
|
||||
|
||||
return on_measurement(vbus_voltage, Ialpha_beta, input_timestamp);
|
||||
}
|
||||
|
||||
Motor::Error AlphaBetaFrameController::get_output(
|
||||
uint32_t output_timestamp, float (&pwm_timings)[3],
|
||||
std::optional<float>* ibus) {
|
||||
std::optional<float2D> mod_alpha_beta;
|
||||
Motor::Error status = get_alpha_beta_output(output_timestamp, &mod_alpha_beta, ibus);
|
||||
|
||||
if (status != Motor::ERROR_NONE) {
|
||||
return status;
|
||||
} else if (!mod_alpha_beta.has_value() || is_nan(mod_alpha_beta->first) || is_nan(mod_alpha_beta->second)) {
|
||||
return Motor::ERROR_MODULATION_IS_NAN;
|
||||
}
|
||||
|
||||
auto [tA, tB, tC, success] = SVM(mod_alpha_beta->first, mod_alpha_beta->second);
|
||||
if (!success) {
|
||||
return Motor::ERROR_MODULATION_MAGNITUDE;
|
||||
}
|
||||
|
||||
pwm_timings[0] = tA;
|
||||
pwm_timings[1] = tB;
|
||||
pwm_timings[2] = tC;
|
||||
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
|
||||
void FieldOrientedController::reset() {
|
||||
v_current_control_integral_d_ = 0.0f;
|
||||
v_current_control_integral_q_ = 0.0f;
|
||||
vbus_voltage_measured_ = std::nullopt;
|
||||
Ialpha_beta_measured_ = std::nullopt;
|
||||
power_ = 0.0f;
|
||||
}
|
||||
|
||||
Motor::Error FieldOrientedController::on_measurement(
|
||||
std::optional<float> vbus_voltage, std::optional<float2D> Ialpha_beta,
|
||||
uint32_t input_timestamp) {
|
||||
// Store the measurements for later processing.
|
||||
i_timestamp_ = input_timestamp;
|
||||
vbus_voltage_measured_ = vbus_voltage;
|
||||
Ialpha_beta_measured_ = Ialpha_beta;
|
||||
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
|
||||
ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(
|
||||
uint32_t output_timestamp, std::optional<float2D>* mod_alpha_beta,
|
||||
std::optional<float>* ibus) {
|
||||
|
||||
if (!vbus_voltage_measured_.has_value() || !Ialpha_beta_measured_.has_value()) {
|
||||
// FOC didn't receive a current measurement yet.
|
||||
return Motor::ERROR_CONTROLLER_INITIALIZING;
|
||||
} else if (abs((int32_t)(i_timestamp_ - ctrl_timestamp_)) > MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA) {
|
||||
// Data from control loop and current measurement are too far apart.
|
||||
return Motor::ERROR_BAD_TIMING;
|
||||
}
|
||||
|
||||
// TODO: improve efficiency in case PWM updates are requested at a higher
|
||||
// rate than current sensor updates. In this case we can reuse mod_d and
|
||||
// mod_q from a previous iteration.
|
||||
|
||||
if (!Vdq_setpoint_.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_VOLTAGE_COMMAND;
|
||||
} else if (!phase_.has_value() || !phase_vel_.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_PHASE_ESTIMATE;
|
||||
} else if (!vbus_voltage_measured_.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
|
||||
}
|
||||
|
||||
auto [Vd, Vq] = *Vdq_setpoint_;
|
||||
float phase = *phase_;
|
||||
float phase_vel = *phase_vel_;
|
||||
float vbus_voltage = *vbus_voltage_measured_;
|
||||
|
||||
std::optional<float2D> Idq;
|
||||
|
||||
// Park transform
|
||||
if (Ialpha_beta_measured_.has_value()) {
|
||||
auto [Ialpha, Ibeta] = *Ialpha_beta_measured_;
|
||||
float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
|
||||
float c_I = our_arm_cos_f32(I_phase);
|
||||
float s_I = our_arm_sin_f32(I_phase);
|
||||
Idq = {
|
||||
c_I * Ialpha + s_I * Ibeta,
|
||||
c_I * Ibeta - s_I * Ialpha
|
||||
};
|
||||
Id_measured_ += I_measured_report_filter_k_ * (Idq->first - Id_measured_);
|
||||
Iq_measured_ += I_measured_report_filter_k_ * (Idq->second - Iq_measured_);
|
||||
} else {
|
||||
Id_measured_ = 0.0f;
|
||||
Iq_measured_ = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
|
||||
float V_to_mod = 1.0f / mod_to_V;
|
||||
float mod_d;
|
||||
float mod_q;
|
||||
|
||||
if (enable_current_control_) {
|
||||
// Current control mode
|
||||
|
||||
if (!pi_gains_.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_GAINS;
|
||||
} else if (!Idq.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT;
|
||||
} else if (!Idq_setpoint_.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_CURRENT_COMMAND;
|
||||
}
|
||||
|
||||
auto [p_gain, i_gain] = *pi_gains_;
|
||||
auto [Id, Iq] = *Idq;
|
||||
auto [Id_setpoint, Iq_setpoint] = *Idq_setpoint_;
|
||||
|
||||
float Ierr_d = Id_setpoint - Id;
|
||||
float Ierr_q = Iq_setpoint - Iq;
|
||||
|
||||
// Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode)
|
||||
mod_d = V_to_mod * (Vd + v_current_control_integral_d_ + Ierr_d * p_gain);
|
||||
mod_q = V_to_mod * (Vq + v_current_control_integral_q_ + Ierr_q * p_gain);
|
||||
|
||||
// Vector modulation saturation, lock integrator if saturated
|
||||
// TODO make maximum modulation configurable
|
||||
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / std::sqrt(mod_d * mod_d + mod_q * mod_q);
|
||||
if (mod_scalefactor < 1.0f) {
|
||||
mod_d *= mod_scalefactor;
|
||||
mod_q *= mod_scalefactor;
|
||||
// TODO make decayfactor configurable
|
||||
v_current_control_integral_d_ *= 0.99f;
|
||||
v_current_control_integral_q_ *= 0.99f;
|
||||
} else {
|
||||
v_current_control_integral_d_ += Ierr_d * (i_gain * current_meas_period);
|
||||
v_current_control_integral_q_ += Ierr_q * (i_gain * current_meas_period);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Voltage control mode
|
||||
mod_d = V_to_mod * Vd;
|
||||
mod_q = V_to_mod * Vq;
|
||||
}
|
||||
|
||||
// Inverse park transform
|
||||
float pwm_phase = phase + phase_vel * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
|
||||
float c_p = our_arm_cos_f32(pwm_phase);
|
||||
float s_p = our_arm_sin_f32(pwm_phase);
|
||||
float mod_alpha = c_p * mod_d - s_p * mod_q;
|
||||
float mod_beta = c_p * mod_q + s_p * mod_d;
|
||||
|
||||
// Report final applied voltage in stationary frame (for sensorless estimator)
|
||||
final_v_alpha_ = mod_to_V * mod_alpha;
|
||||
final_v_beta_ = mod_to_V * mod_beta;
|
||||
|
||||
*mod_alpha_beta = {mod_alpha, mod_beta};
|
||||
|
||||
if (Idq.has_value()) {
|
||||
auto [Id, Iq] = *Idq;
|
||||
*ibus = mod_d * Id + mod_q * Iq;
|
||||
power_ = vbus_voltage * (*ibus).value();
|
||||
}
|
||||
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
|
||||
void FieldOrientedController::update(uint32_t timestamp) {
|
||||
CRITICAL_SECTION() {
|
||||
ctrl_timestamp_ = timestamp;
|
||||
enable_current_control_ = enable_current_control_src_;
|
||||
Idq_setpoint_ = Idq_setpoint_src_.present();
|
||||
Vdq_setpoint_ = Vdq_setpoint_src_.present();
|
||||
phase_ = phase_src_.present();
|
||||
phase_vel_ = phase_vel_src_.present();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
#ifndef __FOC_HPP
|
||||
#define __FOC_HPP
|
||||
|
||||
#include "phase_control_law.hpp"
|
||||
#include "component.hpp"
|
||||
|
||||
/**
|
||||
* @brief Field oriented controller.
|
||||
*
|
||||
* This controller can run in either current control mode or voltage control
|
||||
* mode.
|
||||
*/
|
||||
class FieldOrientedController : public AlphaBetaFrameController, public ComponentBase {
|
||||
public:
|
||||
void update(uint32_t timestamp) final;
|
||||
|
||||
void reset() final;
|
||||
|
||||
ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<float2D> Ialpha_beta,
|
||||
uint32_t input_timestamp) final;
|
||||
|
||||
ODriveIntf::MotorIntf::Error get_alpha_beta_output(
|
||||
uint32_t output_timestamp,
|
||||
std::optional<float2D>* mod_alpha_beta,
|
||||
std::optional<float>* ibus) final;
|
||||
|
||||
// Config - these values are set while this controller is inactive
|
||||
std::optional<float2D> pi_gains_; // [V/A, V/As] should be auto set after resistance and inductance measurement
|
||||
float I_measured_report_filter_k_ = 1.0f;
|
||||
|
||||
// Inputs
|
||||
bool enable_current_control_src_ = false;
|
||||
InputPort<float2D> Idq_setpoint_src_;
|
||||
InputPort<float2D> Vdq_setpoint_src_;
|
||||
InputPort<float> phase_src_;
|
||||
InputPort<float> phase_vel_src_;
|
||||
|
||||
// These values are set atomically by the update() function and read by the
|
||||
// calculate() function in an interrupt context.
|
||||
uint32_t ctrl_timestamp_; // [HCLK ticks]
|
||||
bool enable_current_control_ = false; // true: FOC runs in current control mode using I{dq}_setpoint, false: FOC runs in voltage control mode using V{dq}_setpoint
|
||||
std::optional<float2D> Idq_setpoint_; // [A] only used if enable_current_control_ == true
|
||||
std::optional<float2D> Vdq_setpoint_; // [V] feed-forward voltage term (or standalone setpoint if enable_current_control_ == false)
|
||||
std::optional<float> phase_; // [rad]
|
||||
std::optional<float> phase_vel_; // [rad/s]
|
||||
|
||||
// These values (or some of them) are updated inside on_measurement() and get_alpha_beta_output()
|
||||
uint32_t i_timestamp_;
|
||||
std::optional<float> vbus_voltage_measured_; // [V]
|
||||
std::optional<float2D> Ialpha_beta_measured_; // [A, A]
|
||||
float Id_measured_; // [A]
|
||||
float Iq_measured_; // [A]
|
||||
float v_current_control_integral_d_ = 0.0f; // [V]
|
||||
float v_current_control_integral_q_ = 0.0f; // [V]
|
||||
//float mod_to_V_ = 0.0f;
|
||||
//float mod_d_ = 0.0f;
|
||||
//float mod_q_ = 0.0f;
|
||||
//float ibus_ = 0.0f;
|
||||
float final_v_alpha_ = 0.0f; // [V]
|
||||
float final_v_beta_ = 0.0f; // [V]
|
||||
float power_ = 0.0f; // [W] dot product of Vdq and Idq
|
||||
};
|
||||
|
||||
#endif // __FOC_HPP
|
||||
@@ -0,0 +1,411 @@
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
#include <board.h>
|
||||
|
||||
#include <cmsis_os.h>
|
||||
#include <cmath>
|
||||
#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;
|
||||
const uint32_t stack_size_analog_thread = 1024; // Bytes
|
||||
/* 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 brake_resistor_armed = false;
|
||||
bool brake_resistor_saturated = false;
|
||||
float brake_resistor_current = 0.0f;
|
||||
osThreadId analog_thread = 0;
|
||||
/* 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 Arms the brake resistor
|
||||
void safety_critical_arm_brake_resistor() {
|
||||
CRITICAL_SECTION() {
|
||||
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
||||
axes[i].motor_.I_bus_ = 0.0f;
|
||||
}
|
||||
brake_resistor_armed = true;
|
||||
#if HW_VERSION_MAJOR == 3
|
||||
htim2.Instance->CCR3 = 0;
|
||||
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
// @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() {
|
||||
bool brake_resistor_was_armed = brake_resistor_armed;
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
brake_resistor_armed = false;
|
||||
#if HW_VERSION_MAJOR == 3
|
||||
htim2.Instance->CCR3 = 0;
|
||||
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
// Check necessary to prevent infinite recursion
|
||||
if (brake_resistor_was_armed) {
|
||||
for (auto& axis: axes) {
|
||||
axis.motor_.disarm();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// @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) {
|
||||
odrv.disarm_with_error(ODrive::ERROR_BRAKE_DEADTIME_VIOLATION);
|
||||
}
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
if (brake_resistor_armed) {
|
||||
#if HW_VERSION_MAJOR == 3
|
||||
// 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;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Function implementations --------------------------------------------------*/
|
||||
|
||||
void start_adc_pwm() {
|
||||
// Disarm motors
|
||||
for (auto& axis: axes) {
|
||||
axis.motor_.disarm();
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
|
||||
start_timers();
|
||||
|
||||
|
||||
// Start brake resistor PWM in floating output configuration
|
||||
#if HW_VERSION_MAJOR == 3
|
||||
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);
|
||||
#endif
|
||||
|
||||
if (odrv.config_.enable_brake_resistor) {
|
||||
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 -1.0f 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) {
|
||||
return get_adc_relative_voltage(gpio) * adc_ref_voltage;
|
||||
}
|
||||
|
||||
float get_adc_relative_voltage(Stm32Gpio gpio) {
|
||||
const uint16_t channel = channel_from_gpio(gpio);
|
||||
return get_adc_relative_voltage_ch(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 voltage as a ratio of adc_ref_voltage
|
||||
// returns -1.0f if the channel is not valid.
|
||||
float get_adc_relative_voltage_ch(uint16_t channel) {
|
||||
if (channel < ADC_CHANNEL_COUNT)
|
||||
return (float)adc_measurements_[channel] / adc_full_scale;
|
||||
else
|
||||
return -1.0f;
|
||||
}
|
||||
|
||||
//--------------------------------
|
||||
// IRQ Callbacks
|
||||
//--------------------------------
|
||||
|
||||
void vbus_sense_adc_cb(uint32_t adc_value) {
|
||||
constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
|
||||
vbus_voltage = adc_value * voltage_scale;
|
||||
}
|
||||
|
||||
// @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) {
|
||||
if (axes[i].motor_.is_armed_) {
|
||||
Ibus_sum += axes[i].motor_.I_bus_;
|
||||
}
|
||||
}
|
||||
|
||||
float brake_duty = 0.0f;
|
||||
float brake_current = 0.0f;
|
||||
if (odrv.config_.enable_brake_resistor) {
|
||||
if (!(odrv.config_.brake_resistance > 0.0f)) {
|
||||
odrv.disarm_with_error(ODrive::ERROR_INVALID_BRAKE_RESISTANCE);
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't start braking until -Ibus > regen_current_allowed
|
||||
brake_current = -Ibus_sum - odrv.config_.max_regen_current;
|
||||
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::max((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 (is_nan(brake_duty)) {
|
||||
// Shuts off all motors AND brake resistor, sets error code on all motors.
|
||||
odrv.disarm_with_error(ODrive::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);
|
||||
|
||||
// This cannot result in NaN (safe for race conditions) because we check
|
||||
// brake_resistance != 0 further up.
|
||||
brake_current = brake_duty * vbus_voltage / odrv.config_.brake_resistance;
|
||||
Ibus_sum += brake_duty * vbus_voltage / odrv.config_.brake_resistance;
|
||||
} else {
|
||||
brake_duty = 0;
|
||||
}
|
||||
|
||||
brake_resistor_current = brake_current;
|
||||
ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
|
||||
|
||||
if (Ibus_sum > odrv.config_.dc_max_positive_current) {
|
||||
odrv.disarm_with_error(ODrive::ERROR_DC_BUS_OVER_CURRENT);
|
||||
return;
|
||||
}
|
||||
if (Ibus_sum < odrv.config_.dc_max_negative_current) {
|
||||
odrv.disarm_with_error(ODrive::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);
|
||||
}
|
||||
|
||||
|
||||
/* 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(analog_thread_def, analog_polling_thread, osPriorityLow, 0, stack_size_analog_thread / sizeof(StackType_t));
|
||||
analog_thread = osThreadCreate(osThread(analog_thread_def), NULL);
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||
#ifndef __LOW_LEVEL_H
|
||||
#define __LOW_LEVEL_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include <cmsis_os.h>
|
||||
#include <stdbool.h>
|
||||
#include <adc.h>
|
||||
|
||||
/* Exported types ------------------------------------------------------------*/
|
||||
/* Exported constants --------------------------------------------------------*/
|
||||
#define ADC_CHANNEL_COUNT 16
|
||||
extern const float adc_full_scale;
|
||||
extern const float adc_ref_voltage;
|
||||
/* Exported variables --------------------------------------------------------*/
|
||||
extern float vbus_voltage;
|
||||
extern float ibus_;
|
||||
extern bool brake_resistor_armed;
|
||||
extern bool brake_resistor_saturated;
|
||||
extern float brake_resistor_current;
|
||||
extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT];
|
||||
extern osThreadId analog_thread;
|
||||
extern const uint32_t stack_size_analog_thread;
|
||||
/* Exported macro ------------------------------------------------------------*/
|
||||
/* Exported functions --------------------------------------------------------*/
|
||||
|
||||
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 vbus_sense_adc_cb(uint32_t adc_value);
|
||||
void pwm_in_cb(TIM_HandleTypeDef *htim);
|
||||
}
|
||||
|
||||
// Initalisation
|
||||
void start_adc_pwm();
|
||||
void start_pwm(TIM_HandleTypeDef* htim);
|
||||
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
|
||||
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
|
||||
TIM_HandleTypeDef* htim_refbase = nullptr);
|
||||
void start_general_purpose_adc();
|
||||
void pwm_in_init();
|
||||
void start_analog_thread();
|
||||
|
||||
// ADC getters
|
||||
uint16_t channel_from_gpio(Stm32Gpio gpio);
|
||||
float get_adc_voltage(Stm32Gpio gpio);
|
||||
float get_adc_relative_voltage(Stm32Gpio gpio);
|
||||
float get_adc_relative_voltage_ch(uint16_t channel);
|
||||
|
||||
void update_brake_current();
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif //__LOW_LEVEL_H
|
||||
@@ -0,0 +1,846 @@
|
||||
|
||||
#define __MAIN_CPP__
|
||||
#include "odrive_main.h"
|
||||
#include "nvm_config.hpp"
|
||||
|
||||
#include "usart.h"
|
||||
#include "freertos_vars.h"
|
||||
#include "usb_device.h"
|
||||
#include <communication/interface_usb.h>
|
||||
#include <communication/interface_uart.h>
|
||||
#include <communication/interface_i2c.h>
|
||||
#include <communication/interface_can.hpp>
|
||||
|
||||
osSemaphoreId sem_usb_irq;
|
||||
osMessageQId uart_event_queue;
|
||||
osMessageQId usb_event_queue;
|
||||
osSemaphoreId sem_can;
|
||||
|
||||
#if defined(STM32F405xx)
|
||||
// Place FreeRTOS heap in core coupled memory for better performance
|
||||
__attribute__((section(".ccmram")))
|
||||
#endif
|
||||
uint8_t ucHeap[configTOTAL_HEAP_SIZE];
|
||||
|
||||
uint32_t _reboot_cookie __attribute__ ((section (".noinit")));
|
||||
extern char _estack; // provided by the linker script
|
||||
|
||||
|
||||
ODrive odrv{};
|
||||
|
||||
|
||||
ConfigManager config_manager;
|
||||
|
||||
class StatusLedController {
|
||||
public:
|
||||
void update();
|
||||
};
|
||||
|
||||
StatusLedController status_led_controller;
|
||||
|
||||
void StatusLedController::update() {
|
||||
#if HW_VERSION_MAJOR == 4
|
||||
uint32_t t = HAL_GetTick();
|
||||
|
||||
bool is_booting = std::any_of(axes.begin(), axes.end(), [](Axis& axis){
|
||||
return axis.current_state_ == Axis::AXIS_STATE_UNDEFINED;
|
||||
});
|
||||
|
||||
if (is_booting) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool is_armed = std::any_of(axes.begin(), axes.end(), [](Axis& axis){
|
||||
return axis.motor_.is_armed_;
|
||||
});
|
||||
bool any_error = odrv.any_error();
|
||||
|
||||
if (is_armed) {
|
||||
// Fast green pulsating
|
||||
const uint32_t period_ms = 256;
|
||||
const uint8_t min_brightness = 0;
|
||||
const uint8_t max_brightness = 255;
|
||||
uint32_t brightness = std::abs((int32_t)(t % period_ms) - (int32_t)(period_ms / 2)) * (max_brightness - min_brightness) / (period_ms / 2) + min_brightness;
|
||||
brightness = (brightness * brightness) >> 8; // eye response very roughly sqrt
|
||||
status_led.set_color(rgb_t{(uint8_t)(any_error ? brightness / 2 : 0), (uint8_t)brightness, 0});
|
||||
} else if (any_error) {
|
||||
// Red pulsating
|
||||
const uint32_t period_ms = 1024;
|
||||
const uint8_t min_brightness = 0;
|
||||
const uint8_t max_brightness = 255;
|
||||
uint32_t brightness = std::abs((int32_t)(t % period_ms) - (int32_t)(period_ms / 2)) * (max_brightness - min_brightness) / (period_ms / 2) + min_brightness;
|
||||
brightness = (brightness * brightness) >> 8; // eye response very roughly sqrt
|
||||
status_led.set_color(rgb_t{(uint8_t)brightness, 0, 0});
|
||||
} else {
|
||||
// Slow blue pulsating
|
||||
const uint32_t period_ms = 4096;
|
||||
const uint8_t min_brightness = 50;
|
||||
const uint8_t max_brightness = 160;
|
||||
uint32_t brightness = std::abs((int32_t)(t % period_ms) - (int32_t)(period_ms / 2)) * (max_brightness - min_brightness) / (period_ms / 2) + min_brightness;
|
||||
brightness = (brightness * brightness) >> 8; // eye response very roughly sqrt
|
||||
status_led.set_color(rgb_t{0, 0, (uint8_t)brightness});
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
static bool config_read_all() {
|
||||
bool success = board_read_config() &&
|
||||
config_manager.read(&odrv.config_) &&
|
||||
config_manager.read(&odrv.can_.config_);
|
||||
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
|
||||
success = config_manager.read(&encoders[i].config_) &&
|
||||
config_manager.read(&axes[i].sensorless_estimator_.config_) &&
|
||||
config_manager.read(&axes[i].controller_.config_) &&
|
||||
config_manager.read(&axes[i].trap_traj_.config_) &&
|
||||
config_manager.read(&axes[i].min_endstop_.config_) &&
|
||||
config_manager.read(&axes[i].max_endstop_.config_) &&
|
||||
config_manager.read(&axes[i].mechanical_brake_.config_) &&
|
||||
config_manager.read(&motors[i].config_) &&
|
||||
config_manager.read(&motors[i].fet_thermistor_.config_) &&
|
||||
config_manager.read(&motors[i].motor_thermistor_.config_) &&
|
||||
config_manager.read(&axes[i].config_);
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
static bool config_write_all() {
|
||||
bool success = board_write_config() &&
|
||||
config_manager.write(&odrv.config_) &&
|
||||
config_manager.write(&odrv.can_.config_);
|
||||
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
|
||||
success = config_manager.write(&encoders[i].config_) &&
|
||||
config_manager.write(&axes[i].sensorless_estimator_.config_) &&
|
||||
config_manager.write(&axes[i].controller_.config_) &&
|
||||
config_manager.write(&axes[i].trap_traj_.config_) &&
|
||||
config_manager.write(&axes[i].min_endstop_.config_) &&
|
||||
config_manager.write(&axes[i].max_endstop_.config_) &&
|
||||
config_manager.write(&axes[i].mechanical_brake_.config_) &&
|
||||
config_manager.write(&motors[i].config_) &&
|
||||
config_manager.write(&motors[i].fet_thermistor_.config_) &&
|
||||
config_manager.write(&motors[i].motor_thermistor_.config_) &&
|
||||
config_manager.write(&axes[i].config_);
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
static void config_clear_all() {
|
||||
odrv.config_ = {};
|
||||
odrv.can_.config_ = {};
|
||||
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
||||
encoders[i].config_ = {};
|
||||
axes[i].sensorless_estimator_.config_ = {};
|
||||
axes[i].controller_.config_ = {};
|
||||
axes[i].controller_.config_.load_encoder_axis = i;
|
||||
axes[i].trap_traj_.config_ = {};
|
||||
axes[i].min_endstop_.config_ = {};
|
||||
axes[i].max_endstop_.config_ = {};
|
||||
axes[i].mechanical_brake_.config_ = {};
|
||||
motors[i].config_ = {};
|
||||
motors[i].fet_thermistor_.config_ = {};
|
||||
motors[i].motor_thermistor_.config_ = {};
|
||||
axes[i].clear_config();
|
||||
}
|
||||
}
|
||||
|
||||
static bool config_apply_all() {
|
||||
bool success = odrv.can_.apply_config();
|
||||
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
|
||||
success = encoders[i].apply_config(motors[i].config_.motor_type)
|
||||
&& axes[i].controller_.apply_config()
|
||||
&& axes[i].min_endstop_.apply_config()
|
||||
&& axes[i].max_endstop_.apply_config()
|
||||
&& motors[i].apply_config()
|
||||
&& motors[i].motor_thermistor_.apply_config()
|
||||
&& axes[i].apply_config();
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
bool ODrive::save_configuration(void) {
|
||||
bool success;
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
bool any_armed = std::any_of(axes.begin(), axes.end(),
|
||||
[](auto& axis){ return axis.motor_.is_armed_; });
|
||||
if (any_armed) {
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t config_size = 0;
|
||||
success = config_manager.prepare_store()
|
||||
&& config_write_all()
|
||||
&& config_manager.start_store(&config_size)
|
||||
&& config_write_all()
|
||||
&& config_manager.finish_store();
|
||||
|
||||
// FIXME: during save_configuration we might miss some interrupts
|
||||
// because the CPU gets halted during a flash erase. Missing events
|
||||
// (encoder updates, step/dir steps) is not good so to be sure we just
|
||||
// reboot.
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void ODrive::erase_configuration(void) {
|
||||
NVM_erase();
|
||||
|
||||
// FIXME: this reboot is a workaround because we don't want the next save_configuration
|
||||
// to write back the old configuration from RAM to NVM. The proper action would
|
||||
// be to reset the values in RAM to default. However right now that's not
|
||||
// practical because several startup actions depend on the config. The
|
||||
// other problem is that the stack overflows if we reset to default here.
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
|
||||
void ODrive::enter_dfu_mode() {
|
||||
if ((hw_version_major_ == 3) && (hw_version_minor_ >= 5)) {
|
||||
__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
|
||||
_reboot_cookie = 0xDEADBEEF;
|
||||
NVIC_SystemReset();
|
||||
} else {
|
||||
/*
|
||||
* DFU mode is only allowed on board version >= 3.5 because it can burn
|
||||
* the brake resistor FETs on older boards.
|
||||
* If you really want to use it on an older board, add 3.3k pull-down resistors
|
||||
* to the AUX_L and AUX_H signals and _only then_ uncomment these lines.
|
||||
*/
|
||||
//__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
|
||||
//_reboot_cookie = 0xDEADFE75;
|
||||
//NVIC_SystemReset();
|
||||
}
|
||||
}
|
||||
|
||||
bool ODrive::any_error() {
|
||||
return error_ != ODrive::ERROR_NONE
|
||||
|| std::any_of(axes.begin(), axes.end(), [](Axis& axis){
|
||||
return axis.error_ != Axis::ERROR_NONE
|
||||
|| axis.motor_.error_ != Motor::ERROR_NONE
|
||||
|| axis.sensorless_estimator_.error_ != SensorlessEstimator::ERROR_NONE
|
||||
|| axis.encoder_.error_ != Encoder::ERROR_NONE
|
||||
|| axis.controller_.error_ != Controller::ERROR_NONE;
|
||||
});
|
||||
}
|
||||
|
||||
uint64_t ODrive::get_drv_fault() {
|
||||
#if AXIS_COUNT == 1
|
||||
return motors[0].gate_driver_.get_error();
|
||||
#elif AXIS_COUNT == 2
|
||||
return (uint64_t)motors[0].gate_driver_.get_error() | ((uint64_t)motors[1].gate_driver_.get_error() << 32ULL);
|
||||
#else
|
||||
#error "not supported"
|
||||
#endif
|
||||
}
|
||||
|
||||
void ODrive::clear_errors() {
|
||||
for (auto& axis: axes) {
|
||||
axis.motor_.error_ = Motor::ERROR_NONE;
|
||||
axis.controller_.error_ = Controller::ERROR_NONE;
|
||||
axis.sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
|
||||
axis.encoder_.error_ = Encoder::ERROR_NONE;
|
||||
axis.encoder_.spi_error_rate_ = 0.0f;
|
||||
axis.error_ = Axis::ERROR_NONE;
|
||||
}
|
||||
error_ = ERROR_NONE;
|
||||
if (odrv.config_.enable_brake_resistor) {
|
||||
safety_critical_arm_brake_resistor();
|
||||
}
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
|
||||
for(auto& axis: axes){
|
||||
axis.motor_.disarm();
|
||||
}
|
||||
safety_critical_disarm_brake_resistor();
|
||||
for (;;); // TODO: safe action
|
||||
}
|
||||
|
||||
void vApplicationIdleHook(void) {
|
||||
if (odrv.system_stats_.fully_booted) {
|
||||
odrv.system_stats_.uptime = xTaskGetTickCount();
|
||||
odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
|
||||
|
||||
uint32_t min_stack_space[AXIS_COUNT];
|
||||
std::transform(axes.begin(), axes.end(), std::begin(min_stack_space), [](auto& axis) { return uxTaskGetStackHighWaterMark(axis.thread_id_) * sizeof(StackType_t); });
|
||||
odrv.system_stats_.max_stack_usage_axis = axes[0].stack_size_ - *std::min_element(std::begin(min_stack_space), std::end(min_stack_space));
|
||||
odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
|
||||
odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
|
||||
odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
|
||||
odrv.system_stats_.max_stack_usage_can = odrv.can_.stack_size_ - uxTaskGetStackHighWaterMark(odrv.can_.thread_id_) * sizeof(StackType_t);
|
||||
odrv.system_stats_.max_stack_usage_analog = stack_size_analog_thread - uxTaskGetStackHighWaterMark(analog_thread) * sizeof(StackType_t);
|
||||
|
||||
odrv.system_stats_.stack_size_axis = axes[0].stack_size_;
|
||||
odrv.system_stats_.stack_size_usb = stack_size_usb_thread;
|
||||
odrv.system_stats_.stack_size_uart = stack_size_uart_thread;
|
||||
odrv.system_stats_.stack_size_startup = stack_size_default_task;
|
||||
odrv.system_stats_.stack_size_can = odrv.can_.stack_size_;
|
||||
odrv.system_stats_.stack_size_analog = stack_size_analog_thread;
|
||||
|
||||
odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_);
|
||||
odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread);
|
||||
odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread);
|
||||
odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle);
|
||||
odrv.system_stats_.prio_can = osThreadGetPriority(odrv.can_.thread_id_);
|
||||
odrv.system_stats_.prio_analog = osThreadGetPriority(analog_thread);
|
||||
|
||||
status_led_controller.update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Runs system-level checks that need to be as real-time as possible.
|
||||
*
|
||||
* This function is called after every current measurement of every motor.
|
||||
* It should finish as quickly as possible.
|
||||
*/
|
||||
void ODrive::do_fast_checks() {
|
||||
if (!(vbus_voltage >= config_.dc_bus_undervoltage_trip_level))
|
||||
disarm_with_error(ERROR_DC_BUS_UNDER_VOLTAGE);
|
||||
if (!(vbus_voltage <= config_.dc_bus_overvoltage_trip_level))
|
||||
disarm_with_error(ERROR_DC_BUS_OVER_VOLTAGE);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Floats all power phases on the system (all motors and brake resistors).
|
||||
*
|
||||
* This should be called if a system level exception ocurred that makes it
|
||||
* unsafe to run power through the system in general.
|
||||
*/
|
||||
void ODrive::disarm_with_error(Error error) {
|
||||
CRITICAL_SECTION() {
|
||||
for (auto& axis: axes) {
|
||||
axis.motor_.disarm_with_error(Motor::ERROR_SYSTEM_LEVEL);
|
||||
}
|
||||
safety_critical_disarm_brake_resistor();
|
||||
error_ |= error;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Runs the periodic sampling tasks
|
||||
*
|
||||
* All components that need to sample real-world data should do it in this
|
||||
* function as it runs on a high interrupt priority and provides lowest possible
|
||||
* timing jitter.
|
||||
*
|
||||
* All function called from this function should adhere to the following rules:
|
||||
* - Try to use the same number of CPU cycles in every iteration.
|
||||
* (reason: Tasks that run later in the function still want lowest possible timing jitter)
|
||||
* - Use as few cycles as possible.
|
||||
* (reason: The interrupt blocks other important interrupts (TODO: which ones?))
|
||||
* - Not call any FreeRTOS functions.
|
||||
* (reason: The interrupt priority is higher than the max allowed priority for syscalls)
|
||||
*
|
||||
* Time consuming and undeterministic logic/arithmetic should live on
|
||||
* control_loop_cb() instead.
|
||||
*/
|
||||
void ODrive::sampling_cb() {
|
||||
n_evt_sampling_++;
|
||||
|
||||
MEASURE_TIME(task_times_.sampling) {
|
||||
for (auto& axis: axes) {
|
||||
axis.encoder_.sample_now();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Runs the periodic control loop.
|
||||
*
|
||||
* This function is executed in a low priority interrupt context and is allowed
|
||||
* to call CMSIS functions.
|
||||
*
|
||||
* Yet it runs at a higher priority than communication workloads.
|
||||
*
|
||||
* @param update_cnt: The true count of update events (wrapping around at 16
|
||||
* bits). This is used for timestamp calculation in the face of
|
||||
* potentially missed timer update interrupts. Therefore this counter
|
||||
* must not rely on any interrupts.
|
||||
*/
|
||||
void ODrive::control_loop_cb(uint32_t timestamp) {
|
||||
last_update_timestamp_ = timestamp;
|
||||
n_evt_control_loop_++;
|
||||
|
||||
// TODO: use a configurable component list for most of the following things
|
||||
|
||||
MEASURE_TIME(task_times_.control_loop_misc) {
|
||||
// Reset all output ports so that we are certain about the freshness of
|
||||
// all values that we use.
|
||||
// If we forget to reset a value here the worst that can happen is that
|
||||
// this safety check doesn't work.
|
||||
// TODO: maybe we should add a check to output ports that prevents
|
||||
// double-setting the value.
|
||||
for (auto& axis: axes) {
|
||||
axis.acim_estimator_.slip_vel_.reset();
|
||||
axis.acim_estimator_.stator_phase_vel_.reset();
|
||||
axis.acim_estimator_.stator_phase_.reset();
|
||||
axis.controller_.torque_output_.reset();
|
||||
axis.encoder_.phase_.reset();
|
||||
axis.encoder_.phase_vel_.reset();
|
||||
axis.encoder_.pos_estimate_.reset();
|
||||
axis.encoder_.vel_estimate_.reset();
|
||||
axis.encoder_.pos_circular_.reset();
|
||||
axis.motor_.Vdq_setpoint_.reset();
|
||||
axis.motor_.Idq_setpoint_.reset();
|
||||
axis.open_loop_controller_.Idq_setpoint_.reset();
|
||||
axis.open_loop_controller_.Vdq_setpoint_.reset();
|
||||
axis.open_loop_controller_.phase_.reset();
|
||||
axis.open_loop_controller_.phase_vel_.reset();
|
||||
axis.open_loop_controller_.total_distance_.reset();
|
||||
axis.sensorless_estimator_.phase_.reset();
|
||||
axis.sensorless_estimator_.phase_vel_.reset();
|
||||
axis.sensorless_estimator_.vel_estimate_.reset();
|
||||
}
|
||||
|
||||
uart_poll();
|
||||
odrv.oscilloscope_.update();
|
||||
}
|
||||
|
||||
for (auto& axis : axes) {
|
||||
MEASURE_TIME(axis.task_times_.endstop_update) {
|
||||
axis.min_endstop_.update();
|
||||
axis.max_endstop_.update();
|
||||
}
|
||||
}
|
||||
|
||||
MEASURE_TIME(task_times_.control_loop_checks) {
|
||||
for (auto& axis: axes) {
|
||||
// look for errors at axis level and also all subcomponents
|
||||
bool checks_ok = axis.do_checks(timestamp);
|
||||
|
||||
// make sure the watchdog is being fed.
|
||||
bool watchdog_ok = axis.watchdog_check();
|
||||
|
||||
if (!checks_ok || !watchdog_ok) {
|
||||
axis.motor_.disarm();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (auto& axis: axes) {
|
||||
// Sub-components should use set_error which will propegate to this error_
|
||||
MEASURE_TIME(axis.task_times_.thermistor_update) {
|
||||
axis.motor_.fet_thermistor_.update();
|
||||
axis.motor_.motor_thermistor_.update();
|
||||
}
|
||||
|
||||
MEASURE_TIME(axis.task_times_.encoder_update)
|
||||
axis.encoder_.update();
|
||||
}
|
||||
|
||||
// Controller of either axis might use the encoder estimate of the other
|
||||
// axis so we process both encoders before we continue.
|
||||
|
||||
for (auto& axis: axes) {
|
||||
MEASURE_TIME(axis.task_times_.sensorless_estimator_update)
|
||||
axis.sensorless_estimator_.update();
|
||||
|
||||
MEASURE_TIME(axis.task_times_.controller_update) {
|
||||
if (!axis.controller_.update()) { // uses position and velocity from encoder
|
||||
axis.error_ |= Axis::ERROR_CONTROLLER_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
MEASURE_TIME(axis.task_times_.open_loop_controller_update)
|
||||
axis.open_loop_controller_.update(timestamp);
|
||||
|
||||
MEASURE_TIME(axis.task_times_.motor_update)
|
||||
axis.motor_.update(timestamp); // uses torque from controller and phase_vel from encoder
|
||||
|
||||
MEASURE_TIME(axis.task_times_.current_controller_update)
|
||||
axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or acim_estimator_
|
||||
}
|
||||
|
||||
// Tell the axis threads that the control loop has finished
|
||||
for (auto& axis: axes) {
|
||||
if (axis.thread_id_) {
|
||||
osSignalSet(axis.thread_id_, 0x0001);
|
||||
}
|
||||
}
|
||||
|
||||
get_gpio(odrv.config_.error_gpio_pin).write(odrv.any_error());
|
||||
}
|
||||
|
||||
|
||||
/** @brief For diagnostics only */
|
||||
uint32_t ODrive::get_interrupt_status(int32_t irqn) {
|
||||
if ((irqn < -14) || (irqn >= 240)) {
|
||||
return 0xffffffff;
|
||||
}
|
||||
|
||||
uint8_t priority = (irqn < -12)
|
||||
? 0 // hard fault and NMI always have maximum priority
|
||||
: NVIC_GetPriority((IRQn_Type)irqn);
|
||||
uint32_t counter = GET_IRQ_COUNTER((IRQn_Type)irqn);
|
||||
bool is_enabled = (irqn < 0)
|
||||
? true // processor interrupt vectors are always enabled
|
||||
: NVIC->ISER[(((uint32_t)(int32_t)irqn) >> 5UL)] & (uint32_t)(1UL << (((uint32_t)(int32_t)irqn) & 0x1FUL));
|
||||
|
||||
return priority | ((counter & 0x7ffffff) << 8) | (is_enabled ? 0x80000000 : 0);
|
||||
}
|
||||
|
||||
/** @brief For diagnostics only */
|
||||
uint32_t ODrive::get_dma_status(uint8_t stream_num) {
|
||||
DMA_Stream_TypeDef* streams[] = {
|
||||
DMA1_Stream0, DMA1_Stream1, DMA1_Stream2, DMA1_Stream3, DMA1_Stream4, DMA1_Stream5, DMA1_Stream6, DMA1_Stream7,
|
||||
DMA2_Stream0, DMA2_Stream1, DMA2_Stream2, DMA2_Stream3, DMA2_Stream4, DMA2_Stream5, DMA2_Stream6, DMA2_Stream7
|
||||
};
|
||||
if (stream_num >= 16) {
|
||||
return 0xffffffff;
|
||||
}
|
||||
DMA_Stream_TypeDef* stream = streams[stream_num];
|
||||
bool is_reset = (stream->CR == 0x00000000)
|
||||
&& (stream->NDTR == 0x00000000)
|
||||
&& (stream->PAR == 0x00000000)
|
||||
&& (stream->M0AR == 0x00000000)
|
||||
&& (stream->M1AR == 0x00000000)
|
||||
&& (stream->FCR == 0x00000021);
|
||||
uint8_t channel = ((stream->CR & DMA_SxCR_CHSEL_Msk) >> DMA_SxCR_CHSEL_Pos);
|
||||
uint8_t priority = ((stream->CR & DMA_SxCR_PL_Msk) >> DMA_SxCR_PL_Pos);
|
||||
return (is_reset ? 0 : 0x80000000) | ((channel & 0x7) << 2) | (priority & 0x3);
|
||||
}
|
||||
|
||||
uint32_t ODrive::get_gpio_states() {
|
||||
// TODO: get values that were sampled synchronously with the control loop
|
||||
uint32_t val = 0;
|
||||
for (size_t i = 0; i < GPIO_COUNT; ++i) {
|
||||
val |= ((gpios[i].read() ? 1UL : 0UL) << i);
|
||||
}
|
||||
return val;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main thread started from main().
|
||||
*/
|
||||
static void rtos_main(void*) {
|
||||
// Init USB device
|
||||
MX_USB_DEVICE_Init();
|
||||
|
||||
|
||||
// Start ADC for temperature measurements and user measurements
|
||||
start_general_purpose_adc();
|
||||
|
||||
//osDelay(100);
|
||||
// Init communications (this requires the axis objects to be constructed)
|
||||
init_communication();
|
||||
|
||||
// Start pwm-in compare modules
|
||||
// must happen after communication is initialized
|
||||
pwm0_input.init();
|
||||
|
||||
// Set up the CS pins for absolute encoders (TODO: move to GPIO init switch statement)
|
||||
for(auto& axis : axes){
|
||||
if(axis.encoder_.config_.mode & Encoder::MODE_FLAG_ABS){
|
||||
axis.encoder_.abs_spi_cs_pin_init();
|
||||
}
|
||||
}
|
||||
|
||||
// Try to initialized gate drivers for fault-free startup.
|
||||
// If this does not succeed, a fault will be raised and the idle loop will
|
||||
// periodically attempt to reinit the gate driver.
|
||||
for(auto& axis: axes){
|
||||
axis.motor_.setup();
|
||||
}
|
||||
|
||||
for(auto& axis: axes){
|
||||
axis.encoder_.setup();
|
||||
}
|
||||
|
||||
for(auto& axis: axes){
|
||||
axis.acim_estimator_.idq_src_.connect_to(&axis.motor_.Idq_setpoint_);
|
||||
}
|
||||
|
||||
// Start PWM and enable adc interrupts/callbacks
|
||||
start_adc_pwm();
|
||||
start_analog_thread();
|
||||
|
||||
// Wait for up to 2s for motor to become ready to allow for error-free
|
||||
// startup. This delay gives the current sensor calibration time to
|
||||
// converge. If the DRV chip is unpowered, the motor will not become ready
|
||||
// but we still enter idle state.
|
||||
for (size_t i = 0; i < 2000; ++i) {
|
||||
bool motors_ready = std::all_of(axes.begin(), axes.end(), [](auto& axis) {
|
||||
return axis.motor_.current_meas_.has_value();
|
||||
});
|
||||
if (motors_ready) {
|
||||
break;
|
||||
}
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
for (auto& axis: axes) {
|
||||
axis.sensorless_estimator_.error_ &= ~SensorlessEstimator::ERROR_UNKNOWN_CURRENT_MEASUREMENT;
|
||||
}
|
||||
|
||||
// Start state machine threads. Each thread will go through various calibration
|
||||
// procedures and then run the actual controller loops.
|
||||
// TODO: generalize for AXIS_COUNT != 2
|
||||
for (size_t i = 0; i < AXIS_COUNT; ++i) {
|
||||
axes[i].start_thread();
|
||||
}
|
||||
|
||||
odrv.system_stats_.fully_booted = true;
|
||||
|
||||
// Main thread finished starting everything and can delete itself now (yes this is legal).
|
||||
vTaskDelete(defaultTaskHandle);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Carries out early startup tasks that need to run before any static
|
||||
* initializers.
|
||||
* This function gets called from the startup assembly code.
|
||||
*/
|
||||
extern "C" void early_start_checks(void) {
|
||||
if(_reboot_cookie == 0xDEADFE75) {
|
||||
/* The STM DFU bootloader enables internal pull-up resistors on PB10 (AUX_H)
|
||||
* and PB11 (AUX_L), thereby causing shoot-through on the brake resistor
|
||||
* FETs and obliterating them unless external 3.3k pull-down resistors are
|
||||
* present. Pull-downs are only present on ODrive 3.5 or newer.
|
||||
* On older boards we disable DFU by default but if the user insists
|
||||
* there's only one thing left that might save it: time.
|
||||
* The brake resistor gate driver needs a certain 10V supply (GVDD) to
|
||||
* make it work. This voltage is supplied by the motor gate drivers which get
|
||||
* disabled at system reset. So over time GVDD voltage _should_ below
|
||||
* dangerous levels. This is completely handwavy and should not be relied on
|
||||
* so you are on your own on if you ignore this warning.
|
||||
*
|
||||
* This loop takes 5 cycles per iteration and at this point the system runs
|
||||
* on the internal 16MHz RC oscillator so the delay is about 2 seconds.
|
||||
*/
|
||||
for (size_t i = 0; i < (16000000UL / 5UL * 2UL); ++i) {
|
||||
__NOP();
|
||||
}
|
||||
_reboot_cookie = 0xDEADBEEF;
|
||||
}
|
||||
|
||||
/* We could jump to the bootloader directly on demand without rebooting
|
||||
but that requires us to reset several peripherals and interrupts for it
|
||||
to function correctly. Therefore it's easier to just reset the entire chip. */
|
||||
if(_reboot_cookie == 0xDEADBEEF) {
|
||||
_reboot_cookie = 0xCAFEFEED; //Reset bootloader trigger
|
||||
__set_MSP((uintptr_t)&_estack);
|
||||
// http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf
|
||||
void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004));
|
||||
builtin_bootloader();
|
||||
}
|
||||
|
||||
/* The bootloader might fail to properly clean up after itself,
|
||||
so if we're not sure that the system is in a clean state we
|
||||
just reset it again */
|
||||
if(_reboot_cookie != 42) {
|
||||
_reboot_cookie = 42;
|
||||
NVIC_SystemReset();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main entry point called from assembly startup code.
|
||||
*/
|
||||
extern "C" int main(void) {
|
||||
// This procedure of building a USB serial number should be identical
|
||||
// to the way the STM's built-in USB bootloader does it. This means
|
||||
// that the device will have the same serial number in normal and DFU mode.
|
||||
uint32_t uuid0 = *(uint32_t *)(UID_BASE + 0);
|
||||
uint32_t uuid1 = *(uint32_t *)(UID_BASE + 4);
|
||||
uint32_t uuid2 = *(uint32_t *)(UID_BASE + 8);
|
||||
uint32_t uuid_mixed_part = uuid0 + uuid2;
|
||||
serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16);
|
||||
|
||||
uint64_t val = serial_number;
|
||||
for (size_t i = 0; i < 12; ++i) {
|
||||
serial_number_str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf];
|
||||
val <<= 4;
|
||||
}
|
||||
serial_number_str[12] = 0;
|
||||
|
||||
// Init low level system functions (clocks, flash interface)
|
||||
system_init();
|
||||
|
||||
// Load configuration from NVM. This needs to happen after system_init()
|
||||
// since the flash interface must be initialized and before board_init()
|
||||
// since board initialization can depend on the config.
|
||||
size_t config_size = 0;
|
||||
bool success = config_manager.start_load()
|
||||
&& config_read_all()
|
||||
&& config_manager.finish_load(&config_size)
|
||||
&& config_apply_all();
|
||||
if (success) {
|
||||
odrv.user_config_loaded_ = config_size;
|
||||
} else {
|
||||
config_clear_all();
|
||||
config_apply_all();
|
||||
}
|
||||
|
||||
odrv.misconfigured_ = odrv.misconfigured_
|
||||
|| (odrv.config_.enable_uart_a && !uart_a)
|
||||
|| (odrv.config_.enable_uart_b && !uart_b)
|
||||
|| (odrv.config_.enable_uart_c && !uart_c);
|
||||
|
||||
// Init board-specific peripherals
|
||||
if (!board_init()) {
|
||||
for (;;); // TODO: handle properly
|
||||
}
|
||||
|
||||
// Init GPIOs according to their configured mode
|
||||
for (size_t i = 0; i < GPIO_COUNT; ++i) {
|
||||
// Skip unavailable GPIOs
|
||||
if (!get_gpio(i)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
ODriveIntf::GpioMode mode = odrv.config_.gpio_modes[i];
|
||||
|
||||
GPIO_InitTypeDef GPIO_InitStruct;
|
||||
GPIO_InitStruct.Pin = get_gpio(i).pin_mask_;
|
||||
|
||||
// Set Alternate Function setting for this GPIO mode
|
||||
if (mode == ODriveIntf::GPIO_MODE_DIGITAL ||
|
||||
mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_UP ||
|
||||
mode == ODriveIntf::GPIO_MODE_DIGITAL_PULL_DOWN ||
|
||||
mode == ODriveIntf::GPIO_MODE_MECH_BRAKE ||
|
||||
mode == ODriveIntf::GPIO_MODE_STATUS ||
|
||||
mode == ODriveIntf::GPIO_MODE_ANALOG_IN) {
|
||||
GPIO_InitStruct.Alternate = 0;
|
||||
} else {
|
||||
auto it = std::find_if(
|
||||
alternate_functions[i].begin(), alternate_functions[i].end(),
|
||||
[mode](auto a) { return a.mode == mode; });
|
||||
|
||||
if (it == alternate_functions[i].end()) {
|
||||
odrv.misconfigured_ = true; // this GPIO doesn't support the selected mode
|
||||
continue;
|
||||
}
|
||||
GPIO_InitStruct.Alternate = it->alternate_function;
|
||||
}
|
||||
|
||||
switch (mode) {
|
||||
case ODriveIntf::GPIO_MODE_DIGITAL: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_DIGITAL_PULL_UP: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_DIGITAL_PULL_DOWN: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_ANALOG_IN: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_UART_A: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = (i == 0) ? GPIO_PULLDOWN : GPIO_PULLUP; // this is probably swapped but imitates old behavior
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
if (!odrv.config_.enable_uart_a) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_UART_B: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = (i == 0) ? GPIO_PULLDOWN : GPIO_PULLUP; // this is probably swapped but imitates old behavior
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
if (!odrv.config_.enable_uart_b) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_UART_C: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = (i == 0) ? GPIO_PULLDOWN : GPIO_PULLUP; // this is probably swapped but imitates old behavior
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
if (!odrv.config_.enable_uart_c) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_CAN_A: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
if (!odrv.config_.enable_can_a) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_I2C_A: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLUP;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
|
||||
if (!odrv.config_.enable_i2c_a) {
|
||||
odrv.misconfigured_ = true;
|
||||
}
|
||||
} break;
|
||||
//case ODriveIntf::GPIO_MODE_SPI_A: { // TODO
|
||||
//} break;
|
||||
case ODriveIntf::GPIO_MODE_PWM: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_ENC0: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_ENC1: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_ENC2: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_MECH_BRAKE: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
case ODriveIntf::GPIO_MODE_STATUS: {
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
} break;
|
||||
default: {
|
||||
odrv.misconfigured_ = true;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
HAL_GPIO_Init(get_gpio(i).port_, &GPIO_InitStruct);
|
||||
}
|
||||
|
||||
// Init usb irq binary semaphore, and start with no tokens by removing the starting one.
|
||||
osSemaphoreDef(sem_usb_irq);
|
||||
sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1);
|
||||
osSemaphoreWait(sem_usb_irq, 0);
|
||||
|
||||
// Create an event queue for UART
|
||||
osMessageQDef(uart_event_queue, 4, uint32_t);
|
||||
uart_event_queue = osMessageCreate(osMessageQ(uart_event_queue), NULL);
|
||||
|
||||
// Create an event queue for USB
|
||||
osMessageQDef(usb_event_queue, 7, uint32_t);
|
||||
usb_event_queue = osMessageCreate(osMessageQ(usb_event_queue), NULL);
|
||||
|
||||
osSemaphoreDef(sem_can);
|
||||
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
|
||||
osSemaphoreWait(sem_can, 0);
|
||||
|
||||
// Create main thread
|
||||
osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t));
|
||||
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
|
||||
|
||||
// Start scheduler
|
||||
osKernelStart();
|
||||
|
||||
for (;;);
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#include <odrive_main.h>
|
||||
|
||||
void MechanicalBrake::engage() {
|
||||
if (odrv.config_.gpio_modes[config_.gpio_num] == ODriveIntf::GPIO_MODE_MECH_BRAKE){
|
||||
get_gpio(config_.gpio_num).write(config_.is_active_low ? 0 : 1);
|
||||
}
|
||||
}
|
||||
|
||||
void MechanicalBrake::release() {
|
||||
if (odrv.config_.gpio_modes[config_.gpio_num] == ODriveIntf::GPIO_MODE_MECH_BRAKE){
|
||||
get_gpio(config_.gpio_num).write(config_.is_active_low ? 1 : 0);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
#ifndef __MECHANICAL_BRAKE_HPP
|
||||
#define __MECHANICAL_BRAKE_HPP
|
||||
|
||||
#include <autogen/interfaces.hpp>
|
||||
|
||||
class MechanicalBrake : public ODriveIntf::MechanicalBrakeIntf {
|
||||
public:
|
||||
struct Config_t {
|
||||
uint16_t gpio_num = 0;
|
||||
bool is_active_low = true;
|
||||
|
||||
// custom setters
|
||||
MechanicalBrake* parent = nullptr;
|
||||
void set_gpio_num(uint16_t value) { gpio_num = value; }
|
||||
};
|
||||
|
||||
MechanicalBrake() {}
|
||||
|
||||
MechanicalBrake::Config_t config_;
|
||||
Axis* axis_ = nullptr;
|
||||
|
||||
void release();
|
||||
void engage();
|
||||
};
|
||||
#endif // __MECHANICAL_BRAKE_HPP
|
||||
@@ -0,0 +1,733 @@
|
||||
|
||||
#include "motor.hpp"
|
||||
#include "axis.hpp"
|
||||
#include "low_level.h"
|
||||
#include "odrive_main.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
static constexpr auto CURRENT_ADC_LOWER_BOUND = (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MIN_VOLT / 3.3f);
|
||||
static constexpr auto CURRENT_ADC_UPPER_BOUND = (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MAX_VOLT / 3.3f);
|
||||
|
||||
/**
|
||||
* @brief This control law adjusts the output voltage such that a predefined
|
||||
* current is tracked. A hardcoded integrator gain is used for this.
|
||||
*
|
||||
* TODO: this might as well be implemented using the FieldOrientedController.
|
||||
*/
|
||||
struct ResistanceMeasurementControlLaw : AlphaBetaFrameController {
|
||||
void reset() final {
|
||||
test_voltage_ = 0.0f;
|
||||
test_mod_ = std::nullopt;
|
||||
}
|
||||
|
||||
ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<float2D> Ialpha_beta,
|
||||
uint32_t input_timestamp) final {
|
||||
|
||||
if (Ialpha_beta.has_value()) {
|
||||
actual_current_ = Ialpha_beta->first;
|
||||
test_voltage_ += (kI * current_meas_period) * (target_current_ - actual_current_);
|
||||
I_beta_ += (kIBetaFilt * current_meas_period) * (Ialpha_beta->second - I_beta_);
|
||||
} else {
|
||||
actual_current_ = 0.0f;
|
||||
test_voltage_ = 0.0f;
|
||||
}
|
||||
|
||||
if (std::abs(test_voltage_) > max_voltage_) {
|
||||
test_voltage_ = NAN;
|
||||
return Motor::ERROR_PHASE_RESISTANCE_OUT_OF_RANGE;
|
||||
} else if (!vbus_voltage.has_value()) {
|
||||
return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
|
||||
} else {
|
||||
float vfactor = 1.0f / ((2.0f / 3.0f) * *vbus_voltage);
|
||||
test_mod_ = test_voltage_ * vfactor;
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
ODriveIntf::MotorIntf::Error get_alpha_beta_output(
|
||||
uint32_t output_timestamp,
|
||||
std::optional<float2D>* mod_alpha_beta,
|
||||
std::optional<float>* ibus) final {
|
||||
if (!test_mod_.has_value()) {
|
||||
return Motor::ERROR_CONTROLLER_INITIALIZING;
|
||||
} else {
|
||||
*mod_alpha_beta = {*test_mod_, 0.0f};
|
||||
*ibus = *test_mod_ * actual_current_;
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
float get_resistance() {
|
||||
return test_voltage_ / target_current_;
|
||||
}
|
||||
|
||||
float get_Ibeta() {
|
||||
return I_beta_;
|
||||
}
|
||||
|
||||
const float kI = 1.0f; // [(V/s)/A]
|
||||
const float kIBetaFilt = 80.0f;
|
||||
float max_voltage_ = 0.0f;
|
||||
float actual_current_ = 0.0f;
|
||||
float target_current_ = 0.0f;
|
||||
float test_voltage_ = 0.0f;
|
||||
float I_beta_ = 0.0f; // [A] low pass filtered Ibeta response
|
||||
std::optional<float> test_mod_ = NAN;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This control law toggles rapidly between positive and negative output
|
||||
* voltage. By measuring how large the current ripples are, the phase inductance
|
||||
* can be determined.
|
||||
*
|
||||
* TODO: this method assumes a certain synchronization between current measurement and output application
|
||||
*/
|
||||
struct InductanceMeasurementControlLaw : AlphaBetaFrameController {
|
||||
void reset() final {
|
||||
attached_ = false;
|
||||
}
|
||||
|
||||
ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<float2D> Ialpha_beta,
|
||||
uint32_t input_timestamp) final
|
||||
{
|
||||
if (!Ialpha_beta.has_value()) {
|
||||
return {Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT};
|
||||
}
|
||||
|
||||
float Ialpha = Ialpha_beta->first;
|
||||
|
||||
if (attached_) {
|
||||
float sign = test_voltage_ >= 0.0f ? 1.0f : -1.0f;
|
||||
deltaI_ += -sign * (Ialpha - last_Ialpha_);
|
||||
} else {
|
||||
start_timestamp_ = input_timestamp;
|
||||
attached_ = true;
|
||||
}
|
||||
|
||||
last_Ialpha_ = Ialpha;
|
||||
last_input_timestamp_ = input_timestamp;
|
||||
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
|
||||
ODriveIntf::MotorIntf::Error get_alpha_beta_output(
|
||||
uint32_t output_timestamp, std::optional<float2D>* mod_alpha_beta,
|
||||
std::optional<float>* ibus) final
|
||||
{
|
||||
test_voltage_ *= -1.0f;
|
||||
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
|
||||
*mod_alpha_beta = {test_voltage_ * vfactor, 0.0f};
|
||||
*ibus = 0.0f;
|
||||
return Motor::ERROR_NONE;
|
||||
}
|
||||
|
||||
float get_inductance() {
|
||||
// Note: A more correct formula would also take into account that there is a finite timestep.
|
||||
// However, the discretisation in the current control loop inverts the same discrepancy
|
||||
float dt = (float)(last_input_timestamp_ - start_timestamp_) / (float)TIM_1_8_CLOCK_HZ; // at 216MHz this overflows after 19 seconds
|
||||
return std::abs(test_voltage_) / (deltaI_ / dt);
|
||||
}
|
||||
|
||||
// Config
|
||||
float test_voltage_ = 0.0f;
|
||||
|
||||
// State
|
||||
bool attached_ = false;
|
||||
float sign_ = 0;
|
||||
|
||||
// Outputs
|
||||
uint32_t start_timestamp_ = 0;
|
||||
float last_Ialpha_ = NAN;
|
||||
uint32_t last_input_timestamp_ = 0;
|
||||
float deltaI_ = 0.0f;
|
||||
};
|
||||
|
||||
|
||||
Motor::Motor(TIM_HandleTypeDef* timer,
|
||||
uint8_t current_sensor_mask,
|
||||
float shunt_conductance,
|
||||
TGateDriver& gate_driver,
|
||||
TOpAmp& opamp,
|
||||
OnboardThermistorCurrentLimiter& fet_thermistor,
|
||||
OffboardThermistorCurrentLimiter& motor_thermistor) :
|
||||
timer_(timer),
|
||||
current_sensor_mask_(current_sensor_mask),
|
||||
shunt_conductance_(shunt_conductance),
|
||||
gate_driver_(gate_driver),
|
||||
opamp_(opamp),
|
||||
fet_thermistor_(fet_thermistor),
|
||||
motor_thermistor_(motor_thermistor) {
|
||||
apply_config();
|
||||
fet_thermistor_.motor_ = this;
|
||||
motor_thermistor_.motor_ = this;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Arms the PWM outputs that belong to this motor.
|
||||
*
|
||||
* Note that this does not activate the PWM outputs immediately, it just sets
|
||||
* a flag so they will be enabled later.
|
||||
*
|
||||
* The sequence goes like this:
|
||||
* - Motor::arm() sets the is_armed_ flag.
|
||||
* - On the next timer update event Motor::timer_update_cb() gets called in an
|
||||
* interrupt context
|
||||
* - Motor::timer_update_cb() runs specified control law to determine PWM values
|
||||
* - Motor::timer_update_cb() calls Motor::apply_pwm_timings()
|
||||
* - Motor::apply_pwm_timings() sets the output compare registers and the AOE
|
||||
* (automatic output enable) bit.
|
||||
* - On the next update event the timer latches the configured values into the
|
||||
* active shadow register and enables the outputs at the same time.
|
||||
*
|
||||
* The sequence can be aborted at any time by calling Motor::disarm().
|
||||
*
|
||||
* @param control_law: An control law that is called at the frequency of current
|
||||
* measurements. The function must return as quickly as possible
|
||||
* such that the resulting PWM timings are available before the next
|
||||
* timer update event.
|
||||
* @returns: True on success, false otherwise
|
||||
*/
|
||||
bool Motor::arm(PhaseControlLaw<3>* control_law) {
|
||||
axis_->mechanical_brake_.release();
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
control_law_ = control_law;
|
||||
|
||||
// Reset controller states, integrators, setpoints, etc.
|
||||
axis_->controller_.reset();
|
||||
axis_->acim_estimator_.rotor_flux_ = 0.0f;
|
||||
if (control_law_) {
|
||||
control_law_->reset();
|
||||
}
|
||||
|
||||
if (!odrv.config_.enable_brake_resistor || brake_resistor_armed) {
|
||||
armed_state_ = 1;
|
||||
is_armed_ = true;
|
||||
} else {
|
||||
error_ |= Motor::ERROR_BRAKE_RESISTOR_DISARMED;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Updates the phase PWM timings unless the motor is disarmed.
|
||||
*
|
||||
* If the motor is armed, the PWM timings come into effect at the next update
|
||||
* event (and are enabled if they weren't already), unless the motor is disarmed
|
||||
* prior to that.
|
||||
*
|
||||
* @param tentative: If true, the update is not counted as "refresh".
|
||||
*/
|
||||
void Motor::apply_pwm_timings(uint16_t timings[3], bool tentative) {
|
||||
CRITICAL_SECTION() {
|
||||
if (odrv.config_.enable_brake_resistor && !brake_resistor_armed) {
|
||||
disarm_with_error(ERROR_BRAKE_RESISTOR_DISARMED);
|
||||
}
|
||||
|
||||
TIM_HandleTypeDef* htim = timer_;
|
||||
TIM_TypeDef* tim = htim->Instance;
|
||||
tim->CCR1 = timings[0];
|
||||
tim->CCR2 = timings[1];
|
||||
tim->CCR3 = timings[2];
|
||||
|
||||
if (!tentative) {
|
||||
if (is_armed_) {
|
||||
// Set the Automatic Output Enable so that the Master Output Enable
|
||||
// bit will be automatically enabled on the next update event.
|
||||
tim->BDTR |= TIM_BDTR_AOE;
|
||||
}
|
||||
}
|
||||
|
||||
// If a timer update event occurred just now while we were updating the
|
||||
// timings, we can't be sure what values the shadow registers now contain,
|
||||
// so we must disarm the motor.
|
||||
// (this also protects against the case where the update interrupt has too
|
||||
// low priority, but that should not happen)
|
||||
//if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_UPDATE)) {
|
||||
// disarm_with_error(ERROR_CONTROL_DEADLINE_MISSED);
|
||||
//}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Disarms the motor PWM.
|
||||
*
|
||||
* After this function returns, it is guaranteed that all three
|
||||
* motor phases are floating and will not be enabled again until
|
||||
* arm() is called.
|
||||
*/
|
||||
bool Motor::disarm(bool* p_was_armed) {
|
||||
bool was_armed;
|
||||
|
||||
CRITICAL_SECTION() {
|
||||
was_armed = is_armed_;
|
||||
if (is_armed_) {
|
||||
gate_driver_.set_enabled(false);
|
||||
}
|
||||
is_armed_ = false;
|
||||
armed_state_ = 0;
|
||||
TIM_HandleTypeDef* timer = timer_;
|
||||
timer->Instance->BDTR &= ~TIM_BDTR_AOE; // prevent the PWMs from automatically enabling at the next update
|
||||
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(timer);
|
||||
control_law_ = nullptr;
|
||||
}
|
||||
|
||||
// Check necessary to prevent infinite recursion
|
||||
if (was_armed) {
|
||||
update_brake_current();
|
||||
}
|
||||
|
||||
if (p_was_armed) {
|
||||
*p_was_armed = was_armed;
|
||||
}
|
||||
|
||||
return 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
|
||||
void Motor::update_current_controller_gains() {
|
||||
// Calculate current control gains
|
||||
float p_gain = config_.current_control_bandwidth * config_.phase_inductance;
|
||||
float plant_pole = config_.phase_resistance / config_.phase_inductance;
|
||||
current_control_.pi_gains_ = {p_gain, plant_pole * p_gain};
|
||||
}
|
||||
|
||||
bool Motor::apply_config() {
|
||||
config_.parent = this;
|
||||
is_calibrated_ = config_.pre_calibrated;
|
||||
update_current_controller_gains();
|
||||
return true;
|
||||
}
|
||||
|
||||
// @brief Set up the gate drivers
|
||||
bool Motor::setup() {
|
||||
fet_thermistor_.update();
|
||||
motor_thermistor_.update();
|
||||
|
||||
// Solve for exact gain, then snap down to have equal or larger range as requested
|
||||
// or largest possible range otherwise
|
||||
constexpr float kMargin = 0.90f;
|
||||
constexpr float max_output_swing = 1.35f; // [V] out of amplifier
|
||||
float max_unity_gain_current = kMargin * max_output_swing * shunt_conductance_; // [A]
|
||||
float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]
|
||||
|
||||
float actual_gain;
|
||||
if (!gate_driver_.config(requested_gain, &actual_gain))
|
||||
return false;
|
||||
|
||||
// Values for current controller
|
||||
phase_current_rev_gain_ = 1.0f / actual_gain;
|
||||
// Clip all current control to actual usable range
|
||||
max_allowed_current_ = max_unity_gain_current * phase_current_rev_gain_;
|
||||
|
||||
max_dc_calib_ = 0.1f * max_allowed_current_;
|
||||
|
||||
if (!gate_driver_.init())
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Motor::disarm_with_error(Motor::Error error){
|
||||
error_ |= error;
|
||||
axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
|
||||
last_error_time_ = odrv.n_evt_control_loop_ * current_meas_period;
|
||||
disarm();
|
||||
}
|
||||
|
||||
bool Motor::do_checks(uint32_t timestamp) {
|
||||
gate_driver_.do_checks();
|
||||
|
||||
if (!gate_driver_.is_ready()) {
|
||||
disarm_with_error(ERROR_DRV_FAULT);
|
||||
return false;
|
||||
}
|
||||
if (!motor_thermistor_.do_checks()) {
|
||||
disarm_with_error(ERROR_MOTOR_THERMISTOR_OVER_TEMP);
|
||||
return false;
|
||||
}
|
||||
if (!fet_thermistor_.do_checks()) {
|
||||
disarm_with_error(ERROR_FET_THERMISTOR_OVER_TEMP);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
float Motor::effective_current_lim() {
|
||||
// Configured limit
|
||||
float current_lim = config_.current_lim;
|
||||
// Hardware limit
|
||||
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
|
||||
current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control
|
||||
} else {
|
||||
current_lim = std::min(current_lim, axis_->motor_.max_allowed_current_);
|
||||
}
|
||||
|
||||
// Apply thermistor current limiters
|
||||
current_lim = std::min(current_lim, motor_thermistor_.get_current_limit(config_.current_lim));
|
||||
current_lim = std::min(current_lim, fet_thermistor_.get_current_limit(config_.current_lim));
|
||||
effective_current_lim_ = current_lim;
|
||||
|
||||
return effective_current_lim_;
|
||||
}
|
||||
|
||||
//return the maximum available torque for the motor.
|
||||
//Note - for ACIM motors, available torque is allowed to be 0.
|
||||
float Motor::max_available_torque() {
|
||||
if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
|
||||
float max_torque = effective_current_lim_ * config_.torque_constant * axis_->acim_estimator_.rotor_flux_;
|
||||
max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
|
||||
return max_torque;
|
||||
} else {
|
||||
float max_torque = effective_current_lim_ * config_.torque_constant;
|
||||
max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
|
||||
return max_torque;
|
||||
}
|
||||
}
|
||||
|
||||
std::optional<float> Motor::phase_current_from_adcval(uint32_t ADCValue) {
|
||||
// Make sure the measurements don't come too close to the current sensor's hardware limitations
|
||||
if (ADCValue < CURRENT_ADC_LOWER_BOUND || ADCValue > CURRENT_ADC_UPPER_BOUND) {
|
||||
error_ |= ERROR_CURRENT_SENSE_SATURATION;
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
int adcval_bal = (int)ADCValue - (1 << 11);
|
||||
float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
|
||||
float shunt_volt = amp_out_volt * phase_current_rev_gain_;
|
||||
float current = shunt_volt * shunt_conductance_;
|
||||
return current;
|
||||
}
|
||||
|
||||
//--------------------------------
|
||||
// Measurement and calibration
|
||||
//--------------------------------
|
||||
|
||||
// TODO check Ibeta balance to verify good motor connection
|
||||
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
|
||||
ResistanceMeasurementControlLaw control_law;
|
||||
control_law.target_current_ = test_current;
|
||||
control_law.max_voltage_ = max_voltage;
|
||||
|
||||
arm(&control_law);
|
||||
|
||||
for (size_t i = 0; i < 3000; ++i) {
|
||||
if (!((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_)) {
|
||||
break;
|
||||
}
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
bool success = is_armed_;
|
||||
|
||||
//// De-energize motor
|
||||
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
|
||||
// return false; // error set inside enqueue_voltage_timings
|
||||
|
||||
disarm();
|
||||
|
||||
config_.phase_resistance = control_law.get_resistance();
|
||||
if (is_nan(config_.phase_resistance)) {
|
||||
// TODO: the motor is already disarmed at this stage. This is an error
|
||||
// that only pretains to the measurement and its result so it should
|
||||
// just be a return value of this function.
|
||||
disarm_with_error(ERROR_PHASE_RESISTANCE_OUT_OF_RANGE);
|
||||
success = false;
|
||||
}
|
||||
|
||||
float I_beta = control_law.get_Ibeta();
|
||||
if (is_nan(I_beta) || (abs(I_beta) / test_current) > 0.2f) {
|
||||
disarm_with_error(ERROR_UNBALANCED_PHASES);
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
bool Motor::measure_phase_inductance(float test_voltage) {
|
||||
InductanceMeasurementControlLaw control_law;
|
||||
control_law.test_voltage_ = test_voltage;
|
||||
|
||||
arm(&control_law);
|
||||
|
||||
for (size_t i = 0; i < 1250; ++i) {
|
||||
if (!((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_)) {
|
||||
break;
|
||||
}
|
||||
osDelay(1);
|
||||
}
|
||||
|
||||
bool success = is_armed_;
|
||||
|
||||
//// De-energize motor
|
||||
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
|
||||
// return false; // error set inside enqueue_voltage_timings
|
||||
|
||||
disarm();
|
||||
|
||||
config_.phase_inductance = control_law.get_inductance();
|
||||
|
||||
// TODO arbitrary values set for now
|
||||
if (!(config_.phase_inductance >= 2e-6f && config_.phase_inductance <= 4000e-6f)) {
|
||||
error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE;
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
// TODO: motor calibration should only be a utility function that's called from
|
||||
// the UI on explicit user request. It should take its parameters as input
|
||||
// arguments and return the measured results without modifying any config values.
|
||||
bool Motor::run_calibration() {
|
||||
float R_calib_max_voltage = config_.resistance_calib_max_voltage;
|
||||
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT
|
||||
|| config_.motor_type == MOTOR_TYPE_ACIM) {
|
||||
if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
|
||||
return false;
|
||||
if (!measure_phase_inductance(R_calib_max_voltage))
|
||||
return false;
|
||||
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
|
||||
// no calibration needed
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
update_current_controller_gains();
|
||||
|
||||
is_calibrated_ = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
void Motor::update(uint32_t timestamp) {
|
||||
// Load torque setpoint, convert to motor direction
|
||||
std::optional<float> maybe_torque = torque_setpoint_src_.present();
|
||||
if (!maybe_torque.has_value()) {
|
||||
error_ |= ERROR_UNKNOWN_TORQUE;
|
||||
return;
|
||||
}
|
||||
float torque = direction_ * *maybe_torque;
|
||||
|
||||
// Load setpoints from previous iteration.
|
||||
auto [id, iq] = Idq_setpoint_.previous()
|
||||
.value_or(float2D{0.0f, 0.0f});
|
||||
// Load effective current limit
|
||||
float ilim = axis_->motor_.effective_current_lim_;
|
||||
|
||||
// Autoflux tracks old Iq (that may be 2-norm clamped last cycle) to make sure we are chasing a feasable current.
|
||||
if ((axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) && config_.acim_autoflux_enable) {
|
||||
float abs_iq = std::abs(iq);
|
||||
float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
|
||||
id += gain * (abs_iq - id) * current_meas_period;
|
||||
id = std::clamp(id, config_.acim_autoflux_min_Id, 0.9f * ilim); // 10% space reserved for Iq
|
||||
} else {
|
||||
id = std::clamp(id, -ilim*0.99f, ilim*0.99f); // 1% space reserved for Iq to avoid numerical issues
|
||||
}
|
||||
|
||||
// Convert requested torque to current
|
||||
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
|
||||
iq = torque / (axis_->motor_.config_.torque_constant * std::max(axis_->acim_estimator_.rotor_flux_, config_.acim_gain_min_flux));
|
||||
} else {
|
||||
iq = torque / axis_->motor_.config_.torque_constant;
|
||||
}
|
||||
|
||||
// 2-norm clamping where Id takes priority
|
||||
float iq_lim_sqr = SQ(ilim) - SQ(id);
|
||||
float Iq_lim = (iq_lim_sqr <= 0.0f) ? 0.0f : sqrt(iq_lim_sqr);
|
||||
iq = std::clamp(iq, -Iq_lim, Iq_lim);
|
||||
|
||||
if (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL) {
|
||||
Idq_setpoint_ = {id, iq};
|
||||
}
|
||||
|
||||
// This update call is in bit a weird position because it depends on the
|
||||
// Id,q setpoint but outputs the phase velocity that we depend on later
|
||||
// in this function.
|
||||
// A cleaner fix would be to take the feedforward calculation out of here
|
||||
// and turn it into a separate component.
|
||||
MEASURE_TIME(axis_->task_times_.acim_estimator_update)
|
||||
axis_->acim_estimator_.update(timestamp);
|
||||
|
||||
float vd = 0.0f;
|
||||
float vq = 0.0f;
|
||||
|
||||
std::optional<float> phase_vel = phase_vel_src_.present();
|
||||
|
||||
if (config_.R_wL_FF_enable) {
|
||||
if (!phase_vel.has_value()) {
|
||||
error_ |= ERROR_UNKNOWN_PHASE_VEL;
|
||||
return;
|
||||
}
|
||||
|
||||
vd -= *phase_vel * config_.phase_inductance * iq;
|
||||
vq += *phase_vel * config_.phase_inductance * id;
|
||||
vd += config_.phase_resistance * id;
|
||||
vq += config_.phase_resistance * iq;
|
||||
}
|
||||
|
||||
if (config_.bEMF_FF_enable) {
|
||||
if (!phase_vel.has_value()) {
|
||||
error_ |= ERROR_UNKNOWN_PHASE_VEL;
|
||||
return;
|
||||
}
|
||||
|
||||
vq += *phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs);
|
||||
}
|
||||
|
||||
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
|
||||
// reinterpret current as voltage
|
||||
Vdq_setpoint_ = {vd + id, vq + iq};
|
||||
} else {
|
||||
Vdq_setpoint_ = {vd, vq};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Called when the underlying hardware timer triggers an update event.
|
||||
*/
|
||||
void Motor::current_meas_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current) {
|
||||
// TODO: this is platform specific
|
||||
//const float current_meas_period = static_cast<float>(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ;
|
||||
TaskTimerContext tmr{axis_->task_times_.current_sense};
|
||||
|
||||
n_evt_current_measurement_++;
|
||||
|
||||
bool dc_calib_valid = (dc_calib_running_since_ >= config_.dc_calib_tau * 7.5f)
|
||||
&& (abs(DC_calib_.phA) < max_dc_calib_)
|
||||
&& (abs(DC_calib_.phB) < max_dc_calib_)
|
||||
&& (abs(DC_calib_.phC) < max_dc_calib_);
|
||||
|
||||
if (armed_state_ == 1 || armed_state_ == 2) {
|
||||
current_meas_ = {0.0f, 0.0f, 0.0f};
|
||||
armed_state_ += 1;
|
||||
} else if (current.has_value() && dc_calib_valid) {
|
||||
current_meas_ = {
|
||||
current->phA - DC_calib_.phA,
|
||||
current->phB - DC_calib_.phB,
|
||||
current->phC - DC_calib_.phC
|
||||
};
|
||||
} else {
|
||||
current_meas_ = std::nullopt;
|
||||
}
|
||||
|
||||
// Run system-level checks (e.g. overvoltage/undervoltage condition)
|
||||
// The motor might be disarmed in this function. In this case the
|
||||
// handler will continue to run until the end but it won't have an
|
||||
// effect on the PWM.
|
||||
odrv.do_fast_checks();
|
||||
|
||||
if (current_meas_.has_value()) {
|
||||
// Check for violation of current limit
|
||||
// If Ia + Ib + Ic == 0 holds then we have:
|
||||
// Inorm^2 = Id^2 + Iq^2 = Ialpha^2 + Ibeta^2 = 2/3 * (Ia^2 + Ib^2 + Ic^2)
|
||||
float Itrip = effective_current_lim_ + config_.current_lim_margin;
|
||||
float Inorm_sq = 2.0f / 3.0f * (SQ(current_meas_->phA)
|
||||
+ SQ(current_meas_->phB)
|
||||
+ SQ(current_meas_->phC));
|
||||
|
||||
// Hack: we disable the current check during motor calibration because
|
||||
// it tends to briefly overshoot when the motor moves to align flux with I_alpha
|
||||
if (Inorm_sq > SQ(Itrip)) {
|
||||
disarm_with_error(ERROR_CURRENT_LIMIT_VIOLATION);
|
||||
}
|
||||
} else if (is_armed_) {
|
||||
// Since we can't check current limits, be safe for now and disarm.
|
||||
// Theoretically we could continue to operate if there is no active
|
||||
// current limit.
|
||||
disarm_with_error(ERROR_UNKNOWN_CURRENT_MEASUREMENT);
|
||||
}
|
||||
|
||||
if (control_law_) {
|
||||
Error err = control_law_->on_measurement(vbus_voltage,
|
||||
current_meas_.has_value() ?
|
||||
std::make_optional(std::array<float, 3>{current_meas_->phA, current_meas_->phB, current_meas_->phC})
|
||||
: std::nullopt,
|
||||
timestamp);
|
||||
if (err != ERROR_NONE) {
|
||||
disarm_with_error(err);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Called when the underlying hardware timer triggers an update event.
|
||||
*/
|
||||
void Motor::dc_calib_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current) {
|
||||
const float dc_calib_period = static_cast<float>(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ;
|
||||
TaskTimerContext tmr{axis_->task_times_.dc_calib};
|
||||
|
||||
if (current.has_value()) {
|
||||
const float calib_filter_k = std::min(dc_calib_period / config_.dc_calib_tau, 1.0f);
|
||||
DC_calib_.phA += (current->phA - DC_calib_.phA) * calib_filter_k;
|
||||
DC_calib_.phB += (current->phB - DC_calib_.phB) * calib_filter_k;
|
||||
DC_calib_.phC += (current->phC - DC_calib_.phC) * calib_filter_k;
|
||||
dc_calib_running_since_ += dc_calib_period;
|
||||
} else {
|
||||
DC_calib_.phA = 0.0f;
|
||||
DC_calib_.phB = 0.0f;
|
||||
DC_calib_.phC = 0.0f;
|
||||
dc_calib_running_since_ = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Motor::pwm_update_cb(uint32_t output_timestamp) {
|
||||
TaskTimerContext tmr{axis_->task_times_.pwm_update};
|
||||
n_evt_pwm_update_++;
|
||||
|
||||
Error control_law_status = ERROR_CONTROLLER_FAILED;
|
||||
float pwm_timings[3] = {NAN, NAN, NAN};
|
||||
std::optional<float> i_bus;
|
||||
|
||||
if (control_law_) {
|
||||
control_law_status = control_law_->get_output(
|
||||
output_timestamp, pwm_timings, &i_bus);
|
||||
}
|
||||
|
||||
// Apply control law to calculate PWM duty cycles
|
||||
if (is_armed_ && control_law_status == ERROR_NONE) {
|
||||
uint16_t next_timings[] = {
|
||||
(uint16_t)(pwm_timings[0] * (float)TIM_1_8_PERIOD_CLOCKS),
|
||||
(uint16_t)(pwm_timings[1] * (float)TIM_1_8_PERIOD_CLOCKS),
|
||||
(uint16_t)(pwm_timings[2] * (float)TIM_1_8_PERIOD_CLOCKS)
|
||||
};
|
||||
apply_pwm_timings(next_timings, false);
|
||||
} else if (is_armed_) {
|
||||
if (!(timer_->Instance->BDTR & TIM_BDTR_MOE) && (control_law_status == ERROR_CONTROLLER_INITIALIZING)) {
|
||||
// If the PWM output is armed in software but not yet in
|
||||
// hardware we tolerate the "initializing" error.
|
||||
i_bus = 0.0f;
|
||||
} else {
|
||||
disarm_with_error(control_law_status);
|
||||
}
|
||||
}
|
||||
|
||||
if (!is_armed_) {
|
||||
// If something above failed, reset I_bus to 0A.
|
||||
i_bus = 0.0f;
|
||||
} else if (is_armed_ && !i_bus.has_value()) {
|
||||
// If the motor is armed then i_bus must be known
|
||||
disarm_with_error(ERROR_UNKNOWN_CURRENT_MEASUREMENT);
|
||||
i_bus = 0.0f;
|
||||
}
|
||||
|
||||
I_bus_ = *i_bus;
|
||||
|
||||
if (*i_bus < config_.I_bus_hard_min || *i_bus > config_.I_bus_hard_max) {
|
||||
disarm_with_error(ERROR_I_BUS_OUT_OF_RANGE);
|
||||
}
|
||||
|
||||
update_brake_current();
|
||||
}
|
||||
@@ -0,0 +1,139 @@
|
||||
#ifndef __MOTOR_HPP
|
||||
#define __MOTOR_HPP
|
||||
|
||||
class Axis; // declared in axis.hpp
|
||||
class Motor;
|
||||
|
||||
#include <board.h>
|
||||
#include <autogen/interfaces.hpp>
|
||||
#include "foc.hpp"
|
||||
|
||||
class Motor : public ODriveIntf::MotorIntf {
|
||||
public:
|
||||
|
||||
// NOTE: for gimbal motors, all units of Nm are instead V.
|
||||
// example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)]
|
||||
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
|
||||
struct Config_t {
|
||||
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
|
||||
int32_t pole_pairs = 7;
|
||||
float calibration_current = 10.0f; // [A]
|
||||
float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
|
||||
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
|
||||
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
|
||||
float torque_constant = 0.04f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor
|
||||
MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT;
|
||||
// Read out max_allowed_current to see max supported value for current_lim.
|
||||
// float current_lim = 70.0f; //[A]
|
||||
float current_lim = 10.0f; //[A]
|
||||
float current_lim_margin = 8.0f; // Maximum violation of current_lim
|
||||
float torque_lim = std::numeric_limits<float>::infinity(); //[Nm].
|
||||
// Value used to compute shunt amplifier gains
|
||||
float requested_current_range = 60.0f; // [A]
|
||||
float current_control_bandwidth = 1000.0f; // [rad/s]
|
||||
float inverter_temp_limit_lower = 100;
|
||||
float inverter_temp_limit_upper = 120;
|
||||
|
||||
float acim_gain_min_flux = 10; // [A]
|
||||
float acim_autoflux_min_Id = 10; // [A]
|
||||
bool acim_autoflux_enable = false;
|
||||
float acim_autoflux_attack_gain = 10.0f;
|
||||
float acim_autoflux_decay_gain = 1.0f;
|
||||
|
||||
bool R_wL_FF_enable = false; // Enable feedforwards for R*I and w*L*I terms
|
||||
bool bEMF_FF_enable = false; // Enable feedforward for bEMF
|
||||
|
||||
float I_bus_hard_min = -INFINITY;
|
||||
float I_bus_hard_max = INFINITY;
|
||||
float I_leak_max = 0.1f;
|
||||
|
||||
float dc_calib_tau = 0.2f;
|
||||
|
||||
// custom property setters
|
||||
Motor* parent = nullptr;
|
||||
void set_pre_calibrated(bool value) {
|
||||
pre_calibrated = value;
|
||||
parent->is_calibrated_ = parent->is_calibrated_ || parent->config_.pre_calibrated;
|
||||
}
|
||||
void set_phase_inductance(float value) { phase_inductance = value; parent->update_current_controller_gains(); }
|
||||
void set_phase_resistance(float value) { phase_resistance = value; parent->update_current_controller_gains(); }
|
||||
void set_current_control_bandwidth(float value) { current_control_bandwidth = value; parent->update_current_controller_gains(); }
|
||||
};
|
||||
|
||||
Motor(TIM_HandleTypeDef* timer,
|
||||
uint8_t current_sensor_mask,
|
||||
float shunt_conductance,
|
||||
TGateDriver& gate_driver,
|
||||
TOpAmp& opamp,
|
||||
OnboardThermistorCurrentLimiter& fet_thermistor,
|
||||
OffboardThermistorCurrentLimiter& motor_thermistor);
|
||||
|
||||
bool arm(PhaseControlLaw<3>* control_law);
|
||||
void apply_pwm_timings(uint16_t timings[3], bool tentative);
|
||||
bool disarm(bool* was_armed = nullptr);
|
||||
bool apply_config();
|
||||
bool setup();
|
||||
|
||||
void update_current_controller_gains();
|
||||
void disarm_with_error(Error error);
|
||||
bool do_checks(uint32_t timestamp);
|
||||
float effective_current_lim();
|
||||
float max_available_torque();
|
||||
std::optional<float> phase_current_from_adcval(uint32_t ADCValue);
|
||||
bool measure_phase_resistance(float test_current, float max_voltage);
|
||||
bool measure_phase_inductance(float test_voltage);
|
||||
bool run_calibration();
|
||||
void update(uint32_t timestamp);
|
||||
|
||||
// These functions are called as appropriate from the board.cpp file.
|
||||
void current_meas_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current);
|
||||
void dc_calib_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current);
|
||||
void pwm_update_cb(uint32_t output_timestamp);
|
||||
|
||||
// hardware config
|
||||
TIM_HandleTypeDef* const timer_;
|
||||
const uint8_t current_sensor_mask_;
|
||||
const float shunt_conductance_;
|
||||
TGateDriver& gate_driver_;
|
||||
TOpAmp& opamp_;
|
||||
OnboardThermistorCurrentLimiter& fet_thermistor_;
|
||||
OffboardThermistorCurrentLimiter& motor_thermistor_;
|
||||
|
||||
Config_t config_;
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
|
||||
//private:
|
||||
|
||||
uint32_t n_evt_current_measurement_ = 0;
|
||||
uint32_t n_evt_pwm_update_ = 0;
|
||||
|
||||
// variables exposed on protocol
|
||||
Error error_ = ERROR_NONE;
|
||||
float last_error_time_ = 0.0f;
|
||||
// Do not write to this variable directly!
|
||||
// It is for exclusive use by the safety_critical_... functions.
|
||||
bool is_armed_ = false;
|
||||
uint8_t armed_state_ = 0;
|
||||
bool is_calibrated_ = false; // Set in apply_config()
|
||||
std::optional<Iph_ABC_t> current_meas_;
|
||||
Iph_ABC_t DC_calib_ = {0.0f, 0.0f, 0.0f};
|
||||
float dc_calib_running_since_ = 0.0f; // current sensor calibration needs some time to settle
|
||||
float I_bus_ = 0.0f; // this motors contribution to the bus current
|
||||
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
|
||||
FieldOrientedController current_control_;
|
||||
float effective_current_lim_ = 10.0f; // [A]
|
||||
float max_allowed_current_ = 0.0f; // [A] set in setup()
|
||||
float max_dc_calib_ = 0.0f; // [A] set in setup()
|
||||
|
||||
InputPort<float> torque_setpoint_src_; // Usually points to the Controller object's output
|
||||
InputPort<float> phase_vel_src_; // Usually points to the Encoder object's output
|
||||
|
||||
float direction_ = 0.0f; // if -1 then positive torque is converted to negative Iq
|
||||
OutputPort<float2D> Vdq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC
|
||||
OutputPort<float2D> Idq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC
|
||||
|
||||
PhaseControlLaw<3>* control_law_;
|
||||
};
|
||||
|
||||
|
||||
#endif // __MOTOR_HPP
|
||||
@@ -0,0 +1,192 @@
|
||||
/*
|
||||
* Convenience functions to load and store multiple objects from and to NVM.
|
||||
*
|
||||
* The NVM stores consecutive one-to-one copies of arbitrary objects.
|
||||
* The types of these objects are passed as template arguments to Config<Ts...>.
|
||||
*/
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <Drivers/STM32/stm32_nvm.h>
|
||||
#include <fibre/../../crc.hpp>
|
||||
|
||||
|
||||
/* Private defines -----------------------------------------------------------*/
|
||||
#define CONFIG_CRC16_INIT 0xabcd
|
||||
#define CONFIG_CRC16_POLYNOMIAL 0x3d65
|
||||
|
||||
/* Private macros ------------------------------------------------------------*/
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
/* Global constant data ------------------------------------------------------*/
|
||||
/* Global variables ----------------------------------------------------------*/
|
||||
/* Private constant data -----------------------------------------------------*/
|
||||
|
||||
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
|
||||
// the config structs without changing its total length, make sure to increment this number:
|
||||
static constexpr uint16_t config_version = 0x0001;
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/* Private function prototypes -----------------------------------------------*/
|
||||
/* Function implementations --------------------------------------------------*/
|
||||
|
||||
/**
|
||||
* @brief Manages configuration load and store operations from and to NVM
|
||||
*
|
||||
* Usage:
|
||||
* 1. start_load()
|
||||
* 2. read() (as often needed)
|
||||
* 3. finish_load() (to see if all reads were successful and the CRC in the end is valid)
|
||||
*
|
||||
* 1. prepare_store()
|
||||
* 2. write() (as often as needed)
|
||||
* 3. start_store()
|
||||
* 4. write() (same sequence as before)
|
||||
* 5. finish_store()
|
||||
*
|
||||
* The two store passes are required in order to measure the size on the first
|
||||
* pass. If the size increases between the first and second pass, finish_store()
|
||||
* will return an error.
|
||||
*/
|
||||
class ConfigManager {
|
||||
public:
|
||||
/**
|
||||
* @brief Starts a load operation. This can be called at any time, even half
|
||||
* way through a previous load operation.
|
||||
*/
|
||||
bool start_load() {
|
||||
if (NVM_init() != 0) {
|
||||
return (load_state = kLoadStateFailed), false;
|
||||
}
|
||||
load_offset = 0;
|
||||
load_crc16 = CONFIG_CRC16_INIT ^ config_version;
|
||||
load_state = kLoadStateInProgress;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Loads the next chunk from NVM.
|
||||
* Note that this may return true even if invalid data was read. The user
|
||||
* will know the final verdict by the return value of finish_load().
|
||||
*/
|
||||
template<typename T>
|
||||
bool read(T* val) {
|
||||
if (load_state != 1) {
|
||||
return (load_state = kLoadStateFailed), false;
|
||||
}
|
||||
size_t size = sizeof(T);
|
||||
if (NVM_read(load_offset, (uint8_t *)val, size) != 0)
|
||||
return (load_state = kLoadStateFailed), false;
|
||||
load_crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(load_crc16, (uint8_t *)val, size);
|
||||
load_offset += size;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks the final state of the load operation.
|
||||
* If this function returns false, it is possible that previous read()
|
||||
* operations actually returned garbage.
|
||||
*/
|
||||
bool finish_load(size_t* occupied_size) {
|
||||
if (occupied_size) {
|
||||
*occupied_size = load_offset + 2;
|
||||
}
|
||||
|
||||
uint16_t crc16_calculated = load_crc16;
|
||||
uint16_t crc16_loaded;
|
||||
if (!read(&crc16_loaded)) {
|
||||
return (load_state = kLoadStateFailed), false;
|
||||
}
|
||||
bool result = (load_state == 1) && (crc16_loaded == crc16_calculated);
|
||||
load_state = kLoadStateIdle;
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Starts preparation of a new store operation.
|
||||
*/
|
||||
bool prepare_store() {
|
||||
if (store_state != kStoreStateIdle) {
|
||||
// it might be possible to restart the store process from other states but let's be safe
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
store_offset = 0;
|
||||
store_crc16 = CONFIG_CRC16_INIT ^ config_version;
|
||||
store_state = kStoreStatePreparing;
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
bool write(T* val) {
|
||||
if (store_state == kStoreStateInProgress) {
|
||||
if (NVM_write(store_offset, (uint8_t*)val, sizeof(T)) != 0) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
} else if (store_state != kStoreStatePreparing) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
store_crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(store_crc16, (uint8_t *)val, sizeof(T));
|
||||
store_offset += sizeof(T);
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Finishes the prepare pass and starts the actual store pass.
|
||||
*/
|
||||
bool start_store(size_t* occupied_size) {
|
||||
if (occupied_size) {
|
||||
*occupied_size = store_offset + 2;
|
||||
}
|
||||
|
||||
if (store_state != kStoreStatePreparing) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
store_offset += 2; // account for CRC16
|
||||
if (store_offset > NVM_get_max_write_length()) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
if (NVM_start_write(store_offset) != 0) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
store_offset = 0;
|
||||
store_crc16 = CONFIG_CRC16_INIT ^ config_version;
|
||||
store_state = kStoreStateInProgress;
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Commits the store operation.
|
||||
* If this function succeeds, the new configuration was successfully saved.
|
||||
* If this function fails, the old configuration was not touched.
|
||||
*/
|
||||
bool finish_store() {
|
||||
uint16_t crc16 = store_crc16;
|
||||
if (!write(&crc16)) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
if (NVM_commit() != 0) {
|
||||
return (store_state = kStoreStateFailed), false;
|
||||
}
|
||||
store_state = kStoreStateIdle;
|
||||
return true;
|
||||
}
|
||||
|
||||
enum {
|
||||
kLoadStateIdle = 0,
|
||||
kLoadStateInProgress = 1,
|
||||
kLoadStateFailed = 2
|
||||
} load_state = kLoadStateIdle;
|
||||
size_t load_offset;
|
||||
size_t load_crc16;
|
||||
|
||||
enum {
|
||||
kStoreStateIdle = 0,
|
||||
kStoreStatePreparing = 1,
|
||||
kStoreStateInProgress = 2,
|
||||
kStoreStateFailed = 3
|
||||
} store_state = kStoreStateIdle;
|
||||
size_t store_offset;
|
||||
size_t store_crc16;
|
||||
};
|
||||
@@ -0,0 +1,253 @@
|
||||
#ifndef __ODRIVE_MAIN_H
|
||||
#define __ODRIVE_MAIN_H
|
||||
|
||||
// Hardware configuration
|
||||
#include <board.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
#include <communication/interface_usb.h>
|
||||
#include <communication/interface_i2c.h>
|
||||
#include <communication/interface_uart.h>
|
||||
#include <task_timer.hpp>
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// OS includes
|
||||
#include <cmsis_os.h>
|
||||
|
||||
// extern const float elec_rad_per_enc;
|
||||
extern uint32_t _reboot_cookie;
|
||||
|
||||
extern uint64_t serial_number;
|
||||
extern char serial_number_str[13];
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
bool fully_booted;
|
||||
uint32_t uptime; // [ms]
|
||||
uint32_t min_heap_space; // FreeRTOS heap [Bytes]
|
||||
uint32_t max_stack_usage_axis; // minimum remaining space since startup [Bytes]
|
||||
uint32_t max_stack_usage_usb;
|
||||
uint32_t max_stack_usage_uart;
|
||||
uint32_t max_stack_usage_startup;
|
||||
uint32_t max_stack_usage_can;
|
||||
uint32_t max_stack_usage_analog;
|
||||
|
||||
uint32_t stack_size_axis;
|
||||
uint32_t stack_size_usb;
|
||||
uint32_t stack_size_uart;
|
||||
uint32_t stack_size_startup;
|
||||
uint32_t stack_size_can;
|
||||
uint32_t stack_size_analog;
|
||||
|
||||
int32_t prio_axis;
|
||||
int32_t prio_usb;
|
||||
int32_t prio_uart;
|
||||
int32_t prio_startup;
|
||||
int32_t prio_can;
|
||||
int32_t prio_analog;
|
||||
|
||||
USBStats_t& usb = usb_stats_;
|
||||
I2CStats_t& i2c = i2c_stats_;
|
||||
} SystemStats_t;
|
||||
|
||||
struct PWMMapping_t {
|
||||
endpoint_ref_t endpoint = {0, 0};
|
||||
float min = 0;
|
||||
float max = 0;
|
||||
};
|
||||
|
||||
// @brief general user configurable board configuration
|
||||
struct BoardConfig_t {
|
||||
ODriveIntf::GpioMode gpio_modes[GPIO_COUNT] = {
|
||||
DEFAULT_GPIO_MODES
|
||||
};
|
||||
|
||||
bool enable_uart_a = true;
|
||||
bool enable_uart_b = false;
|
||||
bool enable_uart_c = false;
|
||||
uint32_t uart_a_baudrate = 115200;
|
||||
uint32_t uart_b_baudrate = 115200;
|
||||
uint32_t uart_c_baudrate = 115200;
|
||||
bool enable_can_a = true;
|
||||
bool enable_i2c_a = false;
|
||||
ODriveIntf::StreamProtocolType uart0_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
|
||||
ODriveIntf::StreamProtocolType uart1_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
|
||||
ODriveIntf::StreamProtocolType uart2_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
|
||||
ODriveIntf::StreamProtocolType usb_cdc_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
|
||||
float max_regen_current = 0.0f;
|
||||
float brake_resistance = DEFAULT_BRAKE_RESISTANCE;
|
||||
bool enable_brake_resistor = false;
|
||||
float dc_bus_undervoltage_trip_level = DEFAULT_MIN_DC_VOLTAGE; //<! [V] minimum voltage below which the motor stops operating
|
||||
float dc_bus_overvoltage_trip_level = 1.07f * HW_VERSION_VOLTAGE; //<! [V] maximum voltage above which the motor stops operating.
|
||||
//<! This protects against cases in which the power supply fails to dissipate
|
||||
//<! the brake power if the brake resistor is disabled.
|
||||
//<! The default is 26V for the 24V board version and 52V for the 48V board version.
|
||||
|
||||
/**
|
||||
* If enabled, if the measured DC voltage exceeds `dc_bus_overvoltage_ramp_start`,
|
||||
* the ODrive will sink more power than usual into the the brake resistor
|
||||
* in an attempt to bring the voltage down again.
|
||||
*
|
||||
* The brake duty cycle is increased by the following amount:
|
||||
* vbus_voltage == dc_bus_overvoltage_ramp_start => brake_duty_cycle += 0%
|
||||
* vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100%
|
||||
*
|
||||
* Remarks:
|
||||
* - This feature is active even when all motors are disarmed.
|
||||
* - This feature is disabled if `brake_resistance` is non-positive.
|
||||
*/
|
||||
bool enable_dc_bus_overvoltage_ramp = false;
|
||||
float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
|
||||
//!< Do not set this lower than your usual vbus_voltage,
|
||||
//!< unless you like fried brake resistors.
|
||||
float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
|
||||
//!< Must be larger than `dc_bus_overvoltage_ramp_start`,
|
||||
//!< otherwise the ramp feature is disabled.
|
||||
|
||||
float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source
|
||||
float dc_max_negative_current = -0.01f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable.
|
||||
uint32_t error_gpio_pin = DEFAULT_ERROR_PIN;
|
||||
PWMMapping_t pwm_mappings[4];
|
||||
PWMMapping_t analog_mappings[GPIO_COUNT];
|
||||
};
|
||||
|
||||
struct TaskTimes {
|
||||
TaskTimer sampling;
|
||||
TaskTimer control_loop_misc;
|
||||
TaskTimer control_loop_checks;
|
||||
TaskTimer dc_calib_wait;
|
||||
};
|
||||
|
||||
|
||||
// Forward Declarations
|
||||
class Axis;
|
||||
class Motor;
|
||||
|
||||
// TODO: move
|
||||
// this is technically not thread-safe but practically it might be
|
||||
#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \
|
||||
inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) | static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) & static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) ^ static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) |= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) &= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) ^= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
|
||||
inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
|
||||
|
||||
#include "autogen/interfaces.hpp"
|
||||
|
||||
// ODrive specific includes
|
||||
#include <utils.hpp>
|
||||
#include <low_level.h>
|
||||
#include <encoder.hpp>
|
||||
#include <sensorless_estimator.hpp>
|
||||
#include <controller.hpp>
|
||||
#include <current_limiter.hpp>
|
||||
#include <thermistor.hpp>
|
||||
#include <trapTraj.hpp>
|
||||
#include <endstop.hpp>
|
||||
#include <mechanical_brake.hpp>
|
||||
#include <axis.hpp>
|
||||
#include <oscilloscope.hpp>
|
||||
#include <communication/communication.h>
|
||||
#include <communication/can/odrive_can.hpp>
|
||||
|
||||
// Defined in autogen/version.c based on git-derived version numbers
|
||||
extern "C" {
|
||||
extern const unsigned char fw_version_major_;
|
||||
extern const unsigned char fw_version_minor_;
|
||||
extern const unsigned char fw_version_revision_;
|
||||
extern const unsigned char fw_version_unreleased_;
|
||||
}
|
||||
|
||||
static Stm32Gpio get_gpio(size_t gpio_num) {
|
||||
return (gpio_num < GPIO_COUNT) ? gpios[gpio_num] : GPIO_COUNT ? gpios[0] : Stm32Gpio::none;
|
||||
}
|
||||
|
||||
// general system functions defined in main.cpp
|
||||
class ODrive : public ODriveIntf {
|
||||
public:
|
||||
bool save_configuration() override;
|
||||
void erase_configuration() override;
|
||||
void reboot() override { NVIC_SystemReset(); }
|
||||
void enter_dfu_mode() override;
|
||||
bool any_error();
|
||||
void clear_errors() override;
|
||||
|
||||
float get_adc_voltage(uint32_t gpio) override {
|
||||
return ::get_adc_voltage(get_gpio(gpio));
|
||||
}
|
||||
|
||||
int32_t test_function(int32_t delta) override {
|
||||
static int cnt = 0;
|
||||
return cnt += delta;
|
||||
}
|
||||
|
||||
void do_fast_checks();
|
||||
void sampling_cb();
|
||||
void control_loop_cb(uint32_t timestamp);
|
||||
|
||||
Axis& get_axis(int num) { return axes[num]; }
|
||||
|
||||
uint32_t get_interrupt_status(int32_t irqn);
|
||||
uint32_t get_dma_status(uint8_t stream_num);
|
||||
uint32_t get_gpio_states();
|
||||
uint64_t get_drv_fault();
|
||||
void disarm_with_error(Error error);
|
||||
|
||||
Error error_ = ERROR_NONE;
|
||||
float& vbus_voltage_ = ::vbus_voltage; // TODO: make this the actual variable
|
||||
float& ibus_ = ::ibus_; // TODO: make this the actual variable
|
||||
float ibus_report_filter_k_ = 1.0f;
|
||||
|
||||
const uint64_t& serial_number_ = ::serial_number;
|
||||
|
||||
// Hardware version is compared with OTP on startup to ensure that we're
|
||||
// running on the right board version.
|
||||
const uint8_t hw_version_major_ = HW_VERSION_MAJOR;
|
||||
const uint8_t hw_version_minor_ = HW_VERSION_MINOR;
|
||||
const uint8_t hw_version_variant_ = HW_VERSION_VOLTAGE;
|
||||
|
||||
// the corresponding macros are defined in the autogenerated version.h
|
||||
const uint8_t fw_version_major_ = ::fw_version_major_;
|
||||
const uint8_t fw_version_minor_ = ::fw_version_minor_;
|
||||
const uint8_t fw_version_revision_ = ::fw_version_revision_;
|
||||
const uint8_t fw_version_unreleased_ = ::fw_version_unreleased_; // 0 for official releases, 1 otherwise
|
||||
|
||||
bool& brake_resistor_armed_ = ::brake_resistor_armed; // TODO: make this the actual variable
|
||||
bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable
|
||||
float& brake_resistor_current_ = ::brake_resistor_current;
|
||||
|
||||
SystemStats_t system_stats_;
|
||||
|
||||
// Edit these to suit your capture needs
|
||||
Oscilloscope oscilloscope_{
|
||||
nullptr, // trigger_src
|
||||
0.5f, // trigger_threshold
|
||||
nullptr // data_src TODO: change data type
|
||||
};
|
||||
|
||||
ODriveCAN can_;
|
||||
|
||||
BoardConfig_t config_;
|
||||
uint32_t user_config_loaded_ = 0;
|
||||
bool misconfigured_ = false;
|
||||
|
||||
uint32_t test_property_ = 0;
|
||||
|
||||
uint32_t last_update_timestamp_ = 0;
|
||||
uint32_t n_evt_sampling_ = 0;
|
||||
uint32_t n_evt_control_loop_ = 0;
|
||||
bool task_timers_armed_ = false;
|
||||
TaskTimes task_times_;
|
||||
const bool otp_valid_ = ((uint8_t*)FLASH_OTP_BASE)[0] != 0xff;
|
||||
};
|
||||
|
||||
extern ODrive odrv; // defined in main.cpp
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
#endif /* __ODRIVE_MAIN_H */
|
||||
@@ -0,0 +1,30 @@
|
||||
|
||||
#include "open_loop_controller.hpp"
|
||||
#include <board.h>
|
||||
|
||||
void OpenLoopController::update(uint32_t timestamp) {
|
||||
auto [prev_Id, prev_Iq] = Idq_setpoint_.previous().value_or(float2D{0.0f, 0.0f});
|
||||
auto [prev_Vd, prev_Vq] = Vdq_setpoint_.previous().value_or(float2D{0.0f, 0.0f});
|
||||
float phase = phase_.previous().value_or(initial_phase_);
|
||||
float phase_vel = phase_vel_.previous().value_or(0.0f);
|
||||
|
||||
(void)prev_Iq; // unused
|
||||
(void)prev_Vq; // unused
|
||||
|
||||
float dt = (float)(timestamp - timestamp_) / (float)TIM_1_8_CLOCK_HZ;
|
||||
|
||||
Idq_setpoint_ = {
|
||||
std::clamp(target_current_, prev_Id - max_current_ramp_ * dt, prev_Id + max_current_ramp_ * dt),
|
||||
0.0f
|
||||
};
|
||||
Vdq_setpoint_ = {
|
||||
std::clamp(target_voltage_, prev_Vd - max_voltage_ramp_ * dt, prev_Vd + max_voltage_ramp_ * dt),
|
||||
0.0f
|
||||
};
|
||||
|
||||
phase_vel = std::clamp(target_vel_, phase_vel - max_phase_vel_ramp_ * dt, phase_vel + max_phase_vel_ramp_ * dt);
|
||||
phase_vel_ = phase_vel;
|
||||
phase_ = wrap_pm_pi(phase + phase_vel * dt);
|
||||
total_distance_ = total_distance_.previous().value_or(0.0f) + phase_vel * dt;
|
||||
timestamp_ = timestamp;
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
#ifndef __OPEN_LOOP_CONTROLLER_HPP
|
||||
#define __OPEN_LOOP_CONTROLLER_HPP
|
||||
|
||||
#include "component.hpp"
|
||||
#include <cmath>
|
||||
#include <autogen/interfaces.hpp>
|
||||
|
||||
class OpenLoopController : public ComponentBase {
|
||||
public:
|
||||
void update(uint32_t timestamp) final;
|
||||
|
||||
// Config
|
||||
float max_current_ramp_ = INFINITY; // [A/s]
|
||||
float max_voltage_ramp_ = INFINITY; // [V/s]
|
||||
float max_phase_vel_ramp_ = INFINITY; // [rad/s^2]
|
||||
|
||||
// Inputs
|
||||
float target_vel_ = 0.0f;
|
||||
float target_current_ = 0.0f;
|
||||
float target_voltage_ = 0.0f;
|
||||
float initial_phase_ = 0.0f;
|
||||
|
||||
// State/Outputs
|
||||
uint32_t timestamp_ = 0;
|
||||
OutputPort<float2D> Idq_setpoint_ = {{0.0f, 0.0f}};
|
||||
OutputPort<float2D> Vdq_setpoint_ = {{0.0f, 0.0f}};
|
||||
OutputPort<float> phase_ = 0.0f;
|
||||
OutputPort<float> phase_vel_ = 0.0f;
|
||||
OutputPort<float> total_distance_ = 0.0f;
|
||||
};
|
||||
|
||||
#endif // __OPEN_LOOP_CONTROLLER_HPP
|
||||
@@ -0,0 +1,27 @@
|
||||
|
||||
#include "oscilloscope.hpp"
|
||||
|
||||
// if you use the oscilloscope feature you can bump up this value
|
||||
#define OSCILLOSCOPE_SIZE 4096
|
||||
|
||||
void Oscilloscope::update() {
|
||||
float trigger_data = trigger_src_ ? *trigger_src_ : 0.0f;
|
||||
float trigger_threshold = trigger_threshold_;
|
||||
float sample_data = data_src_ ? **data_src_ : 0.0f;
|
||||
|
||||
if (trigger_data < trigger_threshold) {
|
||||
ready_ = true;
|
||||
}
|
||||
if (ready_ && trigger_data >= trigger_threshold) {
|
||||
capturing_ = true;
|
||||
ready_ = false;
|
||||
}
|
||||
if (capturing_) {
|
||||
if (pos_ < OSCILLOSCOPE_SIZE) {
|
||||
data_[pos_++] = sample_data;
|
||||
} else {
|
||||
pos_ = 0;
|
||||
capturing_ = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
#ifndef __OSCILLOSCOPE_HPP
|
||||
#define __OSCILLOSCOPE_HPP
|
||||
|
||||
#include <autogen/interfaces.hpp>
|
||||
|
||||
// if you use the oscilloscope feature you can bump up this value
|
||||
#define OSCILLOSCOPE_SIZE 4096
|
||||
|
||||
class Oscilloscope : public ODriveIntf::OscilloscopeIntf {
|
||||
public:
|
||||
Oscilloscope(float* trigger_src, float trigger_threshold, float** data_src)
|
||||
: trigger_src_(trigger_src), trigger_threshold_(trigger_threshold), data_src_(data_src) {}
|
||||
|
||||
float get_val(uint32_t index) override {
|
||||
return index < OSCILLOSCOPE_SIZE ? data_[index] : NAN;
|
||||
}
|
||||
|
||||
void update();
|
||||
|
||||
const uint32_t size_ = OSCILLOSCOPE_SIZE;
|
||||
const float* trigger_src_;
|
||||
const float trigger_threshold_;
|
||||
float* const * data_src_;
|
||||
|
||||
float data_[OSCILLOSCOPE_SIZE] = {0};
|
||||
size_t pos_ = 0;
|
||||
bool ready_ = false;
|
||||
bool capturing_ = false;
|
||||
};
|
||||
|
||||
#endif // __OSCILLOSCOPE_HPP
|
||||
@@ -0,0 +1,97 @@
|
||||
#ifndef __PHASE_CONTROL_LAW_HPP
|
||||
#define __PHASE_CONTROL_LAW_HPP
|
||||
|
||||
#include <autogen/interfaces.hpp>
|
||||
#include <variant>
|
||||
|
||||
template<size_t N_PHASES>
|
||||
class PhaseControlLaw {
|
||||
public:
|
||||
/**
|
||||
* @brief Called when this controller becomes the active controller.
|
||||
*/
|
||||
virtual void reset() = 0;
|
||||
|
||||
/**
|
||||
* @brief Informs the control law about a new set of measurements.
|
||||
*
|
||||
* This function gets called in a high priority interrupt context and should
|
||||
* run fast.
|
||||
*
|
||||
* Beware that all inputs can be NAN.
|
||||
*
|
||||
* @param vbus_voltage: The most recently measured DC link voltage. Can be
|
||||
* std::nullopt if the measurement is not available or valid for any
|
||||
* reason.
|
||||
* @param currents: The most recently measured (or inferred) phase currents
|
||||
* in Amps. Can be std::nullopt if no valid measurements are available
|
||||
* (e.g. because the opamp isn't started or because the sensors were
|
||||
* saturated).
|
||||
* @param input_timestamp: The timestamp (in HCLK ticks) corresponding to
|
||||
* the vbus_voltage and current measurement.
|
||||
*/
|
||||
virtual ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<std::array<float, N_PHASES>> currents,
|
||||
uint32_t input_timestamp) = 0;
|
||||
|
||||
/**
|
||||
* @brief Shall calculate the PWM timings for the specified target time.
|
||||
*
|
||||
* This function gets called in a high priority interrupt context and should
|
||||
* run fast.
|
||||
*
|
||||
* Beware that this function can be called before a call to on_measurement().
|
||||
*
|
||||
* @param output_timestamp: The timestamp (in HCLK ticks) corresponding to
|
||||
* the middle of the time span during which the output will be
|
||||
* active.
|
||||
* @param pwm_timings: This array referenced by this argument shall be
|
||||
* filled with the desired PWM timings. Each item corresponds to one
|
||||
* phase and must lie in [0.0f, 1.0f].
|
||||
* The function is not required to return valid PWM timings in case
|
||||
* of an error.
|
||||
* @param ibus: The variable pointed to by this argument is set to the
|
||||
* estimated DC current around the output timestamp when the desired
|
||||
* PWM timings get applied.
|
||||
* The function is not required to return a valid I_bus estimate in
|
||||
* case of an error.
|
||||
*
|
||||
* @returns: An error code or ERROR_NONE. If the function returns an error
|
||||
* the motor gets disarmed with one exception: If the controller
|
||||
* never returned valid PWM timings since it became active then it
|
||||
* is allowed to return ERROR_CONTROLLER_INITIALIZING without
|
||||
* triggering a motor disarm. In this phase the PWMs will not yet
|
||||
* be truly active.
|
||||
*/
|
||||
virtual ODriveIntf::MotorIntf::Error get_output(
|
||||
uint32_t output_timestamp,
|
||||
float (&pwm_timings)[N_PHASES],
|
||||
std::optional<float>* ibus) = 0;
|
||||
};
|
||||
|
||||
class AlphaBetaFrameController : public PhaseControlLaw<3> {
|
||||
private:
|
||||
ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<std::array<float, 3>> currents,
|
||||
uint32_t input_timestamp) final;
|
||||
|
||||
ODriveIntf::MotorIntf::Error get_output(
|
||||
uint32_t output_timestamp,
|
||||
float (&pwm_timings)[3],
|
||||
std::optional<float>* ibus) final;
|
||||
|
||||
protected:
|
||||
virtual ODriveIntf::MotorIntf::Error on_measurement(
|
||||
std::optional<float> vbus_voltage,
|
||||
std::optional<float2D> Ialpha_beta,
|
||||
uint32_t input_timestamp) = 0;
|
||||
|
||||
virtual ODriveIntf::MotorIntf::Error get_alpha_beta_output(
|
||||
uint32_t output_timestamp,
|
||||
std::optional<float2D>* mod_alpha_beta,
|
||||
std::optional<float>* ibus) = 0;
|
||||
};
|
||||
|
||||
#endif // __PHASE_CONTROL_LAW_HPP
|
||||
@@ -0,0 +1,91 @@
|
||||
|
||||
#include "pwm_input.hpp"
|
||||
#include "odrive_main.h"
|
||||
|
||||
void PwmInput::init() {
|
||||
TIM_IC_InitTypeDef sConfigIC;
|
||||
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
|
||||
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
|
||||
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
|
||||
sConfigIC.ICFilter = 15;
|
||||
|
||||
uint32_t channels[] = {TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4};
|
||||
|
||||
for (size_t i = 0; i < 4; ++i) {
|
||||
if (!fibre::is_endpoint_ref_valid(odrv.config_.pwm_mappings[i].endpoint))
|
||||
continue;
|
||||
HAL_TIM_IC_ConfigChannel(htim_, &sConfigIC, channels[i]);
|
||||
HAL_TIM_IC_Start_IT(htim_, channels[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz
|
||||
#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ
|
||||
#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse
|
||||
#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward
|
||||
#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms
|
||||
#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms
|
||||
#define PWM_INVERT_INPUT false
|
||||
|
||||
/**
|
||||
* @param channel: A channel number in [0, 3]
|
||||
*/
|
||||
void handle_pulse(int channel, uint32_t high_time) {
|
||||
if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME)
|
||||
return;
|
||||
|
||||
if (high_time < PWM_MIN_HIGH_TIME)
|
||||
high_time = PWM_MIN_HIGH_TIME;
|
||||
if (high_time > PWM_MAX_HIGH_TIME)
|
||||
high_time = PWM_MAX_HIGH_TIME;
|
||||
float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME);
|
||||
float value = odrv.config_.pwm_mappings[channel].min +
|
||||
(fraction * (odrv.config_.pwm_mappings[channel].max - odrv.config_.pwm_mappings[channel].min));
|
||||
|
||||
fibre::set_endpoint_from_float(odrv.config_.pwm_mappings[channel].endpoint, value);
|
||||
}
|
||||
|
||||
/**
|
||||
* @param channel: A channel number in [0, 3]
|
||||
*/
|
||||
void PwmInput::on_capture(int channel, uint32_t timestamp) {
|
||||
static uint32_t last_timestamp[4] = { 0 };
|
||||
static bool last_pin_state[4] = { false };
|
||||
static bool last_sample_valid[4] = { false };
|
||||
|
||||
if (channel >= 4)
|
||||
return;
|
||||
Stm32Gpio gpio = get_gpio(gpios_[channel]);
|
||||
if (!gpio)
|
||||
return;
|
||||
bool current_pin_state = gpio.read();
|
||||
|
||||
if (last_sample_valid[channel]
|
||||
&& (last_pin_state[channel] != PWM_INVERT_INPUT)
|
||||
&& (current_pin_state == PWM_INVERT_INPUT)) {
|
||||
handle_pulse(channel, timestamp - last_timestamp[channel]);
|
||||
}
|
||||
|
||||
last_timestamp[channel] = timestamp;
|
||||
last_pin_state[channel] = current_pin_state;
|
||||
last_sample_valid[channel] = true;
|
||||
}
|
||||
|
||||
void PwmInput::on_capture() {
|
||||
if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC1)) {
|
||||
__HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC1);
|
||||
on_capture(0, htim_->Instance->CCR1);
|
||||
}
|
||||
if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC2)) {
|
||||
__HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC2);
|
||||
on_capture(1, htim_->Instance->CCR2);
|
||||
}
|
||||
if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC3)) {
|
||||
__HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC3);
|
||||
on_capture(2, htim_->Instance->CCR3);
|
||||
}
|
||||
if(__HAL_TIM_GET_FLAG(htim_, TIM_FLAG_CC4)) {
|
||||
__HAL_TIM_CLEAR_IT(htim_, TIM_IT_CC4);
|
||||
on_capture(3, htim_->Instance->CCR4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
#ifndef __PWM_INPUT_HPP
|
||||
#define __PWM_INPUT_HPP
|
||||
|
||||
#include <tim.h>
|
||||
#include <array>
|
||||
|
||||
class PwmInput {
|
||||
public:
|
||||
PwmInput(TIM_HandleTypeDef* htim, std::array<uint16_t, 4> gpios)
|
||||
: htim_(htim), gpios_(gpios) {}
|
||||
|
||||
void init();
|
||||
void on_capture();
|
||||
|
||||
private:
|
||||
void on_capture(int channel, uint32_t timestamp);
|
||||
|
||||
TIM_HandleTypeDef* htim_;
|
||||
std::array<uint16_t, 4> gpios_;
|
||||
};
|
||||
|
||||
#endif // __PWM_INPUT_HPP
|
||||
@@ -0,0 +1,108 @@
|
||||
|
||||
#include "odrive_main.h"
|
||||
|
||||
void SensorlessEstimator::reset() {
|
||||
pll_pos_ = 0.0f;
|
||||
vel_estimate_ = 0.0f;
|
||||
V_alpha_beta_memory_[0] = 0.0f;
|
||||
V_alpha_beta_memory_[1] = 0.0f;
|
||||
flux_state_[0] = 0.0f;
|
||||
flux_state_[1] = 0.0f;
|
||||
}
|
||||
|
||||
bool SensorlessEstimator::update() {
|
||||
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
|
||||
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
|
||||
// In particular, equation 8 (and by extension eqn 4 and 6).
|
||||
|
||||
// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
|
||||
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
|
||||
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
|
||||
|
||||
// PLL
|
||||
// TODO: the PLL part has some code duplication with the encoder PLL
|
||||
// Pll gains as a function of bandwidth
|
||||
float pll_kp = 2.0f * config_.pll_bandwidth;
|
||||
// Critically damped
|
||||
float pll_ki = 0.25f * (pll_kp * pll_kp);
|
||||
|
||||
// Check that we don't get problems with discrete time approximation
|
||||
if (!(current_meas_period * pll_kp < 1.0f)) {
|
||||
error_ |= ERROR_UNSTABLE_GAIN;
|
||||
axis_->error_ |= Axis::ERROR_SENSORLESS_ESTIMATOR_FAILED;
|
||||
reset(); // Reset state for when the next valid current measurement comes in.
|
||||
return false;
|
||||
}
|
||||
|
||||
// TODO: we read values here which are modified by a higher priority interrupt.
|
||||
// This is not thread-safe.
|
||||
auto current_meas = axis_->motor_.current_meas_;
|
||||
if (!axis_->motor_.is_armed_) {
|
||||
// While the motor is disarmed the current is not measurable so we
|
||||
// assume that it's zero.
|
||||
current_meas = {0.0f, 0.0f};
|
||||
}
|
||||
if (!current_meas.has_value()) {
|
||||
error_ |= ERROR_UNKNOWN_CURRENT_MEASUREMENT;
|
||||
axis_->error_ |= Axis::ERROR_SENSORLESS_ESTIMATOR_FAILED;
|
||||
reset(); // Reset state for when the next valid current measurement comes in.
|
||||
return false;
|
||||
}
|
||||
|
||||
// Clarke transform
|
||||
float I_alpha_beta[2] = {
|
||||
current_meas->phA,
|
||||
one_by_sqrt3 * (current_meas->phB - current_meas->phC)};
|
||||
|
||||
// alpha-beta vector operations
|
||||
float eta[2];
|
||||
for (int i = 0; i <= 1; ++i) {
|
||||
// y is the total flux-driving voltage (see paper eqn 4)
|
||||
float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
|
||||
// flux dynamics (prediction)
|
||||
float x_dot = y;
|
||||
// integrate prediction to current timestep
|
||||
flux_state_[i] += x_dot * current_meas_period;
|
||||
|
||||
// eta is the estimated permanent magnet flux (see paper eqn 6)
|
||||
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
|
||||
}
|
||||
|
||||
// Non-linear observer (see paper eqn 8):
|
||||
float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage;
|
||||
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
|
||||
float bandwidth_factor = 1.0f / pm_flux_sqr;
|
||||
float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
|
||||
|
||||
// alpha-beta vector operations
|
||||
for (int i = 0; i <= 1; ++i) {
|
||||
// add observer action to flux estimate dynamics
|
||||
float x_dot = eta_factor * eta[i];
|
||||
// convert action to discrete-time
|
||||
flux_state_[i] += x_dot * current_meas_period;
|
||||
// update new eta
|
||||
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
|
||||
}
|
||||
|
||||
// Flux state estimation done, store V_alpha_beta for next timestep
|
||||
V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha_;
|
||||
V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta_;
|
||||
|
||||
float phase_vel = phase_vel_.previous().value_or(0.0f);
|
||||
|
||||
// predict PLL phase with velocity
|
||||
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * phase_vel);
|
||||
// update PLL phase with observer permanent magnet phase
|
||||
float phase = fast_atan2(eta[1], eta[0]);
|
||||
float delta_phase = wrap_pm_pi(phase - pll_pos_);
|
||||
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase);
|
||||
// update PLL velocity
|
||||
phase_vel += current_meas_period * pll_ki * delta_phase;
|
||||
|
||||
// set outputs
|
||||
phase_ = phase;
|
||||
phase_vel_ = phase_vel;
|
||||
vel_estimate_ = phase_vel / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI);
|
||||
|
||||
return true;
|
||||
};
|
||||
@@ -0,0 +1,31 @@
|
||||
#ifndef __SENSORLESS_ESTIMATOR_HPP
|
||||
#define __SENSORLESS_ESTIMATOR_HPP
|
||||
|
||||
#include "component.hpp"
|
||||
|
||||
class SensorlessEstimator : public ODriveIntf::SensorlessEstimatorIntf {
|
||||
public:
|
||||
struct Config_t {
|
||||
float observer_gain = 1000.0f; // [rad/s]
|
||||
float pll_bandwidth = 1000.0f; // [rad/s]
|
||||
float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
|
||||
};
|
||||
|
||||
void reset();
|
||||
bool update();
|
||||
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
Config_t config_;
|
||||
|
||||
// TODO: expose on protocol
|
||||
Error error_ = ERROR_NONE;
|
||||
float pll_pos_ = 0.0f; // [rad]
|
||||
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
|
||||
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
|
||||
|
||||
OutputPort<float> phase_ = 0.0f; // [rad]
|
||||
OutputPort<float> phase_vel_ = 0.0f; // [rad/s]
|
||||
OutputPort<float> vel_estimate_ = 0.0f; // [turns/s]
|
||||
};
|
||||
|
||||
#endif /* __SENSORLESS_ESTIMATOR_HPP */
|
||||
@@ -0,0 +1,65 @@
|
||||
#ifndef __TASK_TIMER_HPP
|
||||
#define __TASK_TIMER_HPP
|
||||
|
||||
#include <stdint.h>
|
||||
#include <board.h>
|
||||
|
||||
#define MEASURE_START_TIME
|
||||
#define MEASURE_END_TIME
|
||||
#define MEASURE_LENGTH
|
||||
#define MEASURE_MAX_LENGTH
|
||||
|
||||
inline uint16_t sample_TIM13() {
|
||||
constexpr uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
|
||||
return clocks_per_cnt * TIM13->CNT; // TODO: Use a hw_config
|
||||
}
|
||||
|
||||
struct TaskTimer {
|
||||
uint32_t start_time_ = 0;
|
||||
uint32_t end_time_ = 0;
|
||||
uint32_t length_ = 0;
|
||||
uint32_t max_length_ = 0;
|
||||
|
||||
static bool enabled;
|
||||
|
||||
uint32_t start() {
|
||||
return sample_TIM13();
|
||||
}
|
||||
|
||||
void stop(uint32_t start_time) {
|
||||
uint32_t end_time = sample_TIM13();
|
||||
uint32_t length = end_time - start_time;
|
||||
|
||||
if (enabled) {
|
||||
#ifdef MEASURE_START_TIME
|
||||
start_time_ = start_time;
|
||||
#endif
|
||||
#ifdef MEASURE_END_TIME
|
||||
end_time_ = end_time;
|
||||
#endif
|
||||
#ifdef MEASURE_LENGTH
|
||||
length_ = length;
|
||||
#endif
|
||||
}
|
||||
#ifdef MEASURE_MAX_LENGTH
|
||||
max_length_ = std::max(max_length_, length);
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
struct TaskTimerContext {
|
||||
TaskTimerContext(const TaskTimerContext&) = delete;
|
||||
TaskTimerContext(const TaskTimerContext&&) = delete;
|
||||
void operator=(const TaskTimerContext&) = delete;
|
||||
void operator=(const TaskTimerContext&&) = delete;
|
||||
TaskTimerContext(TaskTimer& timer) : timer_(timer), start_time(timer.start()) {}
|
||||
~TaskTimerContext() { timer_.stop(start_time); }
|
||||
|
||||
TaskTimer& timer_;
|
||||
uint32_t start_time;
|
||||
bool exit_ = false;
|
||||
};
|
||||
|
||||
#define MEASURE_TIME(timer) for (TaskTimerContext __task_timer_ctx{timer}; !__task_timer_ctx.exit_; __task_timer_ctx.exit_ = true)
|
||||
|
||||
#endif // __TASK_TIMER_HPP
|
||||
@@ -0,0 +1,89 @@
|
||||
#include "odrive_main.h"
|
||||
|
||||
#include "low_level.h"
|
||||
|
||||
ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
|
||||
const float* const coefficients,
|
||||
size_t num_coeffs,
|
||||
const float& temp_limit_lower,
|
||||
const float& temp_limit_upper,
|
||||
const bool& enabled) :
|
||||
adc_channel_(adc_channel),
|
||||
coefficients_(coefficients),
|
||||
num_coeffs_(num_coeffs),
|
||||
temperature_(NAN),
|
||||
temp_limit_lower_(temp_limit_lower),
|
||||
temp_limit_upper_(temp_limit_upper),
|
||||
enabled_(enabled)
|
||||
{
|
||||
}
|
||||
|
||||
void ThermistorCurrentLimiter::update() {
|
||||
const float normalized_voltage = get_adc_relative_voltage_ch(adc_channel_);
|
||||
float raw_temperature_ = horner_poly_eval(normalized_voltage, coefficients_, num_coeffs_);
|
||||
|
||||
constexpr float tau = 0.1f; // [sec]
|
||||
float k = current_meas_period / tau;
|
||||
float val = raw_temperature_;
|
||||
for (float& lpf_val : lpf_vals_) {
|
||||
lpf_val += k * (val - lpf_val);
|
||||
val = lpf_val;
|
||||
}
|
||||
if (is_nan(val)) {
|
||||
lpf_vals_.fill(0.0f);
|
||||
}
|
||||
temperature_ = lpf_vals_.back();
|
||||
}
|
||||
|
||||
bool ThermistorCurrentLimiter::do_checks() {
|
||||
if (enabled_ && temperature_ >= temp_limit_upper_ + 5) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
float ThermistorCurrentLimiter::get_current_limit(float base_current_lim) const {
|
||||
if (!enabled_) {
|
||||
return base_current_lim;
|
||||
}
|
||||
|
||||
const float temp_margin = temp_limit_upper_ - temperature_;
|
||||
const float derating_range = temp_limit_upper_ - temp_limit_lower_;
|
||||
float thermal_current_lim = base_current_lim * (temp_margin / derating_range);
|
||||
if (thermal_current_lim < 0.0f || is_nan(thermal_current_lim)) {
|
||||
thermal_current_lim = 0.0f;
|
||||
}
|
||||
|
||||
return std::min(thermal_current_lim, base_current_lim);
|
||||
}
|
||||
|
||||
OnboardThermistorCurrentLimiter::OnboardThermistorCurrentLimiter(uint16_t adc_channel, const float* const coefficients, size_t num_coeffs) :
|
||||
ThermistorCurrentLimiter(adc_channel,
|
||||
coefficients,
|
||||
num_coeffs,
|
||||
config_.temp_limit_lower,
|
||||
config_.temp_limit_upper,
|
||||
config_.enabled)
|
||||
{
|
||||
}
|
||||
|
||||
OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter() :
|
||||
ThermistorCurrentLimiter(UINT16_MAX,
|
||||
&config_.thermistor_poly_coeffs[0],
|
||||
num_coeffs_,
|
||||
config_.temp_limit_lower,
|
||||
config_.temp_limit_upper,
|
||||
config_.enabled)
|
||||
{
|
||||
decode_pin();
|
||||
}
|
||||
|
||||
bool OffboardThermistorCurrentLimiter::apply_config() {
|
||||
config_.parent = this;
|
||||
decode_pin();
|
||||
return true;
|
||||
}
|
||||
|
||||
void OffboardThermistorCurrentLimiter::decode_pin() {
|
||||
adc_channel_ = channel_from_gpio(get_gpio(config_.gpio_pin));
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
#ifndef __THERMISTOR_HPP
|
||||
#define __THERMISTOR_HPP
|
||||
|
||||
class Motor; // declared in motor.hpp
|
||||
|
||||
#include "current_limiter.hpp"
|
||||
#include <autogen/interfaces.hpp>
|
||||
|
||||
class ThermistorCurrentLimiter : public CurrentLimiter, public ODriveIntf::ThermistorCurrentLimiterIntf {
|
||||
public:
|
||||
virtual ~ThermistorCurrentLimiter() = default;
|
||||
|
||||
ThermistorCurrentLimiter(uint16_t adc_channel,
|
||||
const float* const coefficients,
|
||||
size_t num_coeffs,
|
||||
const float& temp_limit_lower,
|
||||
const float& temp_limit_upper,
|
||||
const bool& enabled);
|
||||
|
||||
void update();
|
||||
bool do_checks();
|
||||
float get_current_limit(float base_current_lim) const override;
|
||||
|
||||
uint16_t adc_channel_;
|
||||
const float* const coefficients_;
|
||||
const size_t num_coeffs_;
|
||||
float temperature_ = NAN; // [°C] NaN while the ODrive is initializing.
|
||||
const float& temp_limit_lower_;
|
||||
const float& temp_limit_upper_;
|
||||
const bool& enabled_;
|
||||
Motor* motor_ = nullptr; // set by Motor::apply_config()
|
||||
std::array<float, 2> lpf_vals_ = { 0.0f };
|
||||
};
|
||||
|
||||
class OnboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OnboardThermistorCurrentLimiterIntf {
|
||||
public:
|
||||
struct Config_t {
|
||||
float temp_limit_lower = 100;
|
||||
float temp_limit_upper = 120;
|
||||
bool enabled = true;
|
||||
};
|
||||
|
||||
virtual ~OnboardThermistorCurrentLimiter() = default;
|
||||
OnboardThermistorCurrentLimiter(uint16_t adc_channel, const float* const coefficients, size_t num_coeffs);
|
||||
|
||||
Config_t config_;
|
||||
};
|
||||
|
||||
class OffboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OffboardThermistorCurrentLimiterIntf {
|
||||
public:
|
||||
static const size_t num_coeffs_ = 4;
|
||||
|
||||
struct Config_t {
|
||||
float thermistor_poly_coeffs[num_coeffs_];
|
||||
|
||||
#if HW_VERSION_MAJOR == 3
|
||||
uint16_t gpio_pin = 4;
|
||||
#elif HW_VERSION_MAJOR == 4
|
||||
uint16_t gpio_pin = 2;
|
||||
#endif
|
||||
float temp_limit_lower = 100;
|
||||
float temp_limit_upper = 120;
|
||||
bool enabled = false;
|
||||
|
||||
// custom setters
|
||||
OffboardThermistorCurrentLimiter* parent;
|
||||
void set_gpio_pin(uint16_t value) { gpio_pin = value; parent->decode_pin(); }
|
||||
};
|
||||
|
||||
virtual ~OffboardThermistorCurrentLimiter() = default;
|
||||
OffboardThermistorCurrentLimiter();
|
||||
|
||||
Config_t config_;
|
||||
|
||||
bool apply_config();
|
||||
|
||||
private:
|
||||
void decode_pin();
|
||||
};
|
||||
|
||||
#endif // __THERMISTOR_HPP
|
||||
@@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
template <class T>
|
||||
class Timer {
|
||||
public:
|
||||
void setTimeout(const T timeout) {
|
||||
timeout_ = timeout;
|
||||
}
|
||||
|
||||
void setIncrement(const T increment) {
|
||||
increment_ = increment;
|
||||
}
|
||||
|
||||
void start() {
|
||||
running_ = true;
|
||||
}
|
||||
|
||||
void stop() {
|
||||
running_ = false;
|
||||
}
|
||||
|
||||
// If the timer is started, increment the timer
|
||||
void update() {
|
||||
if (running_)
|
||||
timer_ = std::min<T>(timer_ + increment_, timeout_);
|
||||
}
|
||||
|
||||
void reset() {
|
||||
timer_ = static_cast<T>(0);
|
||||
}
|
||||
|
||||
bool expired() {
|
||||
return timer_ >= timeout_;
|
||||
}
|
||||
|
||||
private:
|
||||
T timer_ = static_cast<T>(0); // Current state
|
||||
T timeout_ = static_cast<T>(0); // Time to count
|
||||
T increment_ = static_cast<T>(0); // Amount to increment each time update() is called
|
||||
bool running_ = false; // update() only increments if runing_ is true
|
||||
};
|
||||
@@ -0,0 +1,92 @@
|
||||
#include <cmath>
|
||||
#include "odrive_main.h"
|
||||
#include "utils.hpp"
|
||||
|
||||
// A sign function where input 0 has positive sign (not 0)
|
||||
float sign_hard(float val) {
|
||||
return (std::signbit(val)) ? -1.0f : 1.0f;
|
||||
}
|
||||
|
||||
// Symbol Description
|
||||
// Ta, Tv and Td Duration of the stages of the AL profile
|
||||
// Xi and Vi Adapted initial conditions for the AL profile
|
||||
// Xf Position set-point
|
||||
// s Direction (sign) of the trajectory
|
||||
// Vmax, Amax, Dmax and jmax Kinematic bounds
|
||||
// Ar, Dr and Vr Reached values of acceleration and velocity
|
||||
|
||||
bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
|
||||
float Vmax, float Amax, float Dmax) {
|
||||
float dX = Xf - Xi; // Distance to travel
|
||||
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
|
||||
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
|
||||
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
|
||||
Ar_ = s * Amax; // Maximum Acceleration (signed)
|
||||
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
|
||||
Vr_ = s * Vmax; // Maximum Velocity (signed)
|
||||
|
||||
// If we start with a speed faster than cruising, then we need to decel instead of accel
|
||||
// aka "double deceleration move" in the paper
|
||||
if ((s * Vi) > (s * Vr_)) {
|
||||
Ar_ = -s * Amax;
|
||||
}
|
||||
|
||||
// Time to accel/decel to/from Vr (cruise speed)
|
||||
Ta_ = (Vr_ - Vi) / Ar_;
|
||||
Td_ = -Vr_ / Dr_;
|
||||
|
||||
// Integral of velocity ramps over the full accel and decel times to get
|
||||
// minimum displacement required to reach cuising speed
|
||||
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
|
||||
|
||||
// Are we displacing enough to reach cruising speed?
|
||||
if (s*dX < s*dXmin) {
|
||||
// Short move (triangle profile)
|
||||
Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
|
||||
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
|
||||
Td_ = std::max(0.0f, -Vr_ / Dr_);
|
||||
Tv_ = 0.0f;
|
||||
} else {
|
||||
// Long move (trapezoidal profile)
|
||||
Tv_ = (dX - dXmin) / Vr_;
|
||||
}
|
||||
|
||||
// Fill in the rest of the values used at evaluation-time
|
||||
Tf_ = Ta_ + Tv_ + Td_;
|
||||
Xi_ = Xi;
|
||||
Xf_ = Xf;
|
||||
Vi_ = Vi;
|
||||
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
|
||||
Step_t trajStep;
|
||||
if (t < 0.0f) { // Initial Condition
|
||||
trajStep.Y = Xi_;
|
||||
trajStep.Yd = Vi_;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else if (t < Ta_) { // Accelerating
|
||||
trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
|
||||
trajStep.Yd = Vi_ + Ar_*t;
|
||||
trajStep.Ydd = Ar_;
|
||||
} else if (t < Ta_ + Tv_) { // Coasting
|
||||
trajStep.Y = yAccel_ + Vr_*(t - Ta_);
|
||||
trajStep.Yd = Vr_;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else if (t < Tf_) { // Deceleration
|
||||
float td = t - Tf_;
|
||||
trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td);
|
||||
trajStep.Yd = Dr_*td;
|
||||
trajStep.Ydd = Dr_;
|
||||
} else if (t >= Tf_) { // Final Condition
|
||||
trajStep.Y = Xf_;
|
||||
trajStep.Yd = 0.0f;
|
||||
trajStep.Ydd = 0.0f;
|
||||
} else {
|
||||
// TODO: report error here
|
||||
}
|
||||
|
||||
return trajStep;
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
#ifndef _TRAP_TRAJ_H
|
||||
#define _TRAP_TRAJ_H
|
||||
|
||||
class TrapezoidalTrajectory {
|
||||
public:
|
||||
struct Config_t {
|
||||
float vel_limit = 2.0f; // [turn/s]
|
||||
float accel_limit = 0.5f; // [turn/s^2]
|
||||
float decel_limit = 0.5f; // [turn/s^2]
|
||||
};
|
||||
|
||||
struct Step_t {
|
||||
float Y;
|
||||
float Yd;
|
||||
float Ydd;
|
||||
};
|
||||
|
||||
bool planTrapezoidal(float Xf, float Xi, float Vi,
|
||||
float Vmax, float Amax, float Dmax);
|
||||
Step_t eval(float t);
|
||||
|
||||
Axis* axis_ = nullptr; // set by Axis constructor
|
||||
Config_t config_;
|
||||
|
||||
float Xi_;
|
||||
float Xf_;
|
||||
float Vi_;
|
||||
|
||||
float Ar_;
|
||||
float Vr_;
|
||||
float Dr_;
|
||||
|
||||
float Ta_;
|
||||
float Tv_;
|
||||
float Td_;
|
||||
float Tf_;
|
||||
|
||||
float yAccel_;
|
||||
|
||||
float t_;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,190 @@
|
||||
|
||||
#include <utils.hpp>
|
||||
#include <board.h>
|
||||
|
||||
|
||||
// Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
|
||||
// as per the magnitude invariant clarke transform
|
||||
// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
|
||||
// Returns true on success, and false if the input was out of range
|
||||
std::tuple<float, float, float, bool> SVM(float alpha, float beta) {
|
||||
float tA, tB, tC;
|
||||
int Sextant;
|
||||
|
||||
if (beta >= 0.0f) {
|
||||
if (alpha >= 0.0f) {
|
||||
//quadrant I
|
||||
if (one_by_sqrt3 * beta > alpha)
|
||||
Sextant = 2; //sextant v2-v3
|
||||
else
|
||||
Sextant = 1; //sextant v1-v2
|
||||
} else {
|
||||
//quadrant II
|
||||
if (-one_by_sqrt3 * beta > alpha)
|
||||
Sextant = 3; //sextant v3-v4
|
||||
else
|
||||
Sextant = 2; //sextant v2-v3
|
||||
}
|
||||
} else {
|
||||
if (alpha >= 0.0f) {
|
||||
//quadrant IV
|
||||
if (-one_by_sqrt3 * beta > alpha)
|
||||
Sextant = 5; //sextant v5-v6
|
||||
else
|
||||
Sextant = 6; //sextant v6-v1
|
||||
} else {
|
||||
//quadrant III
|
||||
if (one_by_sqrt3 * beta > alpha)
|
||||
Sextant = 4; //sextant v4-v5
|
||||
else
|
||||
Sextant = 5; //sextant v5-v6
|
||||
}
|
||||
}
|
||||
|
||||
switch (Sextant) {
|
||||
// sextant v1-v2
|
||||
case 1: {
|
||||
// Vector on-times
|
||||
float t1 = alpha - one_by_sqrt3 * beta;
|
||||
float t2 = two_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tA = (1.0f - t1 - t2) * 0.5f;
|
||||
tB = tA + t1;
|
||||
tC = tB + t2;
|
||||
} break;
|
||||
|
||||
// sextant v2-v3
|
||||
case 2: {
|
||||
// Vector on-times
|
||||
float t2 = alpha + one_by_sqrt3 * beta;
|
||||
float t3 = -alpha + one_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tB = (1.0f - t2 - t3) * 0.5f;
|
||||
tA = tB + t3;
|
||||
tC = tA + t2;
|
||||
} break;
|
||||
|
||||
// sextant v3-v4
|
||||
case 3: {
|
||||
// Vector on-times
|
||||
float t3 = two_by_sqrt3 * beta;
|
||||
float t4 = -alpha - one_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tB = (1.0f - t3 - t4) * 0.5f;
|
||||
tC = tB + t3;
|
||||
tA = tC + t4;
|
||||
} break;
|
||||
|
||||
// sextant v4-v5
|
||||
case 4: {
|
||||
// Vector on-times
|
||||
float t4 = -alpha + one_by_sqrt3 * beta;
|
||||
float t5 = -two_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tC = (1.0f - t4 - t5) * 0.5f;
|
||||
tB = tC + t5;
|
||||
tA = tB + t4;
|
||||
} break;
|
||||
|
||||
// sextant v5-v6
|
||||
case 5: {
|
||||
// Vector on-times
|
||||
float t5 = -alpha - one_by_sqrt3 * beta;
|
||||
float t6 = alpha - one_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tC = (1.0f - t5 - t6) * 0.5f;
|
||||
tA = tC + t5;
|
||||
tB = tA + t6;
|
||||
} break;
|
||||
|
||||
// sextant v6-v1
|
||||
case 6: {
|
||||
// Vector on-times
|
||||
float t6 = -two_by_sqrt3 * beta;
|
||||
float t1 = alpha + one_by_sqrt3 * beta;
|
||||
|
||||
// PWM timings
|
||||
tA = (1.0f - t6 - t1) * 0.5f;
|
||||
tC = tA + t1;
|
||||
tB = tC + t6;
|
||||
} break;
|
||||
}
|
||||
|
||||
bool result_valid =
|
||||
tA >= 0.0f && tA <= 1.0f
|
||||
&& tB >= 0.0f && tB <= 1.0f
|
||||
&& tC >= 0.0f && tC <= 1.0f;
|
||||
return {tA, tB, tC, result_valid};
|
||||
}
|
||||
|
||||
// based on https://math.stackexchange.com/a/1105038/81278
|
||||
float fast_atan2(float y, float x) {
|
||||
// a := min (|x|, |y|) / max (|x|, |y|)
|
||||
float abs_y = std::abs(y);
|
||||
float abs_x = std::abs(x);
|
||||
// inject FLT_MIN in denominator to avoid division by zero
|
||||
float a = std::min(abs_x, abs_y) / (std::max(abs_x, abs_y) + std::numeric_limits<float>::min());
|
||||
// s := a * a
|
||||
float s = a * a;
|
||||
// r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
|
||||
float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
|
||||
// if |y| > |x| then r := 1.57079637 - r
|
||||
if (abs_y > abs_x)
|
||||
r = 1.57079637f - r;
|
||||
// if x < 0 then r := 3.14159274 - r
|
||||
if (x < 0.0f)
|
||||
r = 3.14159274f - r;
|
||||
// if y < 0 then r := -r
|
||||
if (y < 0.0f)
|
||||
r = -r;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
// @brief: Returns how much time is left until the deadline is reached.
|
||||
// If the deadline has already passed, the return value is 0 (except if
|
||||
// the deadline is very far in the past)
|
||||
uint32_t deadline_to_timeout(uint32_t deadline_ms) {
|
||||
uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
|
||||
uint32_t timeout_ms = deadline_ms - now_ms;
|
||||
return (timeout_ms & 0x80000000) ? 0 : timeout_ms;
|
||||
}
|
||||
|
||||
// @brief: Converts a timeout to a deadline based on the current time.
|
||||
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 a non-zero value if the specified system time (in ms)
|
||||
// is in the future or 0 otherwise.
|
||||
// If the time lies far in the past this may falsely return a non-zero value.
|
||||
int is_in_the_future(uint32_t time_ms) {
|
||||
return deadline_to_timeout(time_ms);
|
||||
}
|
||||
|
||||
// @brief: Returns number of microseconds since system startup
|
||||
uint32_t micros(void) {
|
||||
register uint32_t ms, cycle_cnt;
|
||||
do {
|
||||
ms = HAL_GetTick();
|
||||
cycle_cnt = TIM_TIME_BASE->CNT;
|
||||
} while (ms != HAL_GetTick());
|
||||
|
||||
return (ms * 1000) + cycle_cnt;
|
||||
}
|
||||
|
||||
// @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 volatile ("nop");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,158 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <limits>
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <tuple>
|
||||
#include <cmath>
|
||||
|
||||
/**
|
||||
* @brief Flash size register address
|
||||
*/
|
||||
#define ID_FLASH_ADDRESS (0x1FFF7A22)
|
||||
|
||||
/**
|
||||
* @brief Device ID register address
|
||||
*/
|
||||
#define ID_DBGMCU_IDCODE (0xE0042000)
|
||||
|
||||
/**
|
||||
* "Returns" the device signature
|
||||
*
|
||||
* Possible returns:
|
||||
* - 0x0413: STM32F405xx/07xx and STM32F415xx/17xx)
|
||||
* - 0x0419: STM32F42xxx and STM32F43xxx
|
||||
* - 0x0423: STM32F401xB/C
|
||||
* - 0x0433: STM32F401xD/E
|
||||
* - 0x0431: STM32F411xC/E
|
||||
*
|
||||
* Returned data is in 16-bit mode, but only bits 11:0 are valid, bits 15:12 are always 0.
|
||||
* Defined as macro
|
||||
*/
|
||||
#define STM_ID_GetSignature() ((*(uint16_t *)(ID_DBGMCU_IDCODE)) & 0x0FFF)
|
||||
|
||||
/**
|
||||
* "Returns" the device revision
|
||||
*
|
||||
* Revisions possible:
|
||||
* - 0x1000: Revision A
|
||||
* - 0x1001: Revision Z
|
||||
* - 0x1003: Revision Y
|
||||
* - 0x1007: Revision 1
|
||||
* - 0x2001: Revision 3
|
||||
*
|
||||
* Returned data is in 16-bit mode.
|
||||
*/
|
||||
#define STM_ID_GetRevision() (*(uint16_t *)(ID_DBGMCU_IDCODE + 2))
|
||||
|
||||
/**
|
||||
* "Returns" the Flash size
|
||||
*
|
||||
* Returned data is in 16-bit mode, returned value is flash size in kB (kilo bytes).
|
||||
*/
|
||||
#define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS))
|
||||
|
||||
#ifdef M_PI
|
||||
#undef M_PI
|
||||
#endif
|
||||
|
||||
// Math Constants
|
||||
constexpr float M_PI = 3.14159265358979323846f;
|
||||
constexpr float one_by_sqrt3 = 0.57735026919f;
|
||||
constexpr float two_by_sqrt3 = 1.15470053838f;
|
||||
constexpr float sqrt3_by_2 = 0.86602540378f;
|
||||
|
||||
// Function prototypes for implementations in utils.cpp
|
||||
std::tuple<float, float, float, bool> SVM(float alpha, float beta);
|
||||
float fast_atan2(float y, float x);
|
||||
uint32_t deadline_to_timeout(uint32_t deadline_ms);
|
||||
uint32_t timeout_to_deadline(uint32_t timeout_ms);
|
||||
int is_in_the_future(uint32_t time_ms);
|
||||
uint32_t micros(void);
|
||||
void delay_us(uint32_t us);
|
||||
|
||||
extern "C" {
|
||||
float our_arm_sin_f32(float x);
|
||||
float our_arm_cos_f32(float x);
|
||||
}
|
||||
|
||||
// ----------------
|
||||
// Inline functions
|
||||
|
||||
template<typename T>
|
||||
constexpr T SQ(const T& x){
|
||||
return x * x;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Small helper to make array with known size
|
||||
* in contrast to initializer lists the number of arguments
|
||||
* has to match exactly. Whereas initializer lists allow
|
||||
* less arguments.
|
||||
*/
|
||||
template <class T, class... Tail>
|
||||
std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail) {
|
||||
return std::array<T, 1 + sizeof...(Tail)>({head, tail...});
|
||||
}
|
||||
|
||||
// To allow use of -ffast-math we need to have a special check for nan
|
||||
// that bypasses the "ignore nan" flag
|
||||
__attribute__((optimize("-fno-finite-math-only")))
|
||||
inline bool is_nan(float x) {
|
||||
return __builtin_isnan(x);
|
||||
}
|
||||
|
||||
// Round to integer
|
||||
// Default rounding mode: round to nearest
|
||||
inline int round_int(float x) {
|
||||
#ifdef __arm__
|
||||
int res;
|
||||
asm("vcvtr.s32.f32 %[res], %[x]"
|
||||
: [res] "=X" (res)
|
||||
: [x] "w" (x) );
|
||||
return res;
|
||||
#else
|
||||
return (int)nearbyint(x);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Wrap value to range.
|
||||
// With default rounding mode (round to nearest),
|
||||
// the result will be in range -y/2 to y/2
|
||||
inline float wrap_pm(float x, float y) {
|
||||
#ifdef FPU_FPV4
|
||||
float intval = (float)round_int(x / y);
|
||||
#else
|
||||
float intval = nearbyintf(x / y);
|
||||
#endif
|
||||
return x - intval * y;
|
||||
}
|
||||
|
||||
// Same as fmodf but result is positive and y must be positive
|
||||
inline float fmodf_pos(float x, float y) {
|
||||
float res = wrap_pm(x, y);
|
||||
if (res < 0) res += y;
|
||||
return res;
|
||||
}
|
||||
|
||||
inline float wrap_pm_pi(float x) {
|
||||
return wrap_pm(x, 2 * M_PI);
|
||||
}
|
||||
|
||||
// Evaluate polynomials in an efficient way
|
||||
// coeffs[0] is highest order, as per numpy.polyfit
|
||||
// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
|
||||
inline float horner_poly_eval(float x, const float *coeffs, size_t count) {
|
||||
float result = 0.0f;
|
||||
for (size_t idx = 0; idx < count; ++idx)
|
||||
result = (result * x) + coeffs[idx];
|
||||
return result;
|
||||
}
|
||||
|
||||
// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
|
||||
inline int mod(const int dividend, const int divisor){
|
||||
int r = dividend % divisor;
|
||||
if (r < 0) r += divisor;
|
||||
return r;
|
||||
}
|
||||
Reference in New Issue
Block a user