From 7fd0806d493edd5eacb569d82dfcf2836435f984 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 23 Sep 2020 16:20:10 +0200 Subject: [PATCH] introduce InputPort and OutputPort, don't use NAN The InputPort/OutputPort infrastructure facilitates safer data paths between components: OutputPorts store a value and the age of the value measured in number of control loop iterations. InputPorts can be connected to various sources, for instance an OutputPort. InputPorts expose the values to consumers in the form of std::optional to reflect the fact that an InputPort can be dangling or connected to a stale OutputPort. --- CHANGELOG.md | 1 + Firmware/Board/v3/board.cpp | 45 ++-- Firmware/MotorControl/async_estimator.cpp | 33 ++- Firmware/MotorControl/async_estimator.hpp | 16 +- Firmware/MotorControl/axis.cpp | 101 ++++---- Firmware/MotorControl/component.hpp | 162 +++++++++++++ Firmware/MotorControl/controller.cpp | 61 +++-- Firmware/MotorControl/controller.hpp | 11 +- Firmware/MotorControl/encoder.cpp | 61 ++--- Firmware/MotorControl/encoder.hpp | 11 +- Firmware/MotorControl/foc.cpp | 184 ++++++++------- Firmware/MotorControl/foc.hpp | 40 ++-- Firmware/MotorControl/main.cpp | 42 +++- Firmware/MotorControl/motor.cpp | 215 ++++++++++-------- Firmware/MotorControl/motor.hpp | 30 +-- Firmware/MotorControl/odrive_main.h | 2 +- .../MotorControl/open_loop_controller.cpp | 34 +-- .../MotorControl/open_loop_controller.hpp | 19 +- Firmware/MotorControl/phase_control_law.hpp | 44 ++-- .../MotorControl/sensorless_estimator.cpp | 72 +++--- .../MotorControl/sensorless_estimator.hpp | 13 +- Firmware/Tupfile.lua | 2 + Firmware/communication/ascii_protocol.cpp | 4 +- Firmware/communication/can_simple.cpp | 30 ++- Firmware/fibre/cpp/interfaces_template.j2 | 2 + Firmware/fibre/tools/interface_generator.py | 6 +- Firmware/odrive-interface.yaml | 83 ++++--- tools/odrive/enums.py | 23 +- tools/odrive/tests/encoder_test.py | 2 +- tools/odrive/tests/test_runner.py | 2 +- tools/odrive/utils.py | 1 + 31 files changed, 833 insertions(+), 519 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index aad81ce5..04633143 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -50,6 +50,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * `.motor.config.acim_slip_velocity` was moved to `.async_estimator.config.slip_velocity`. * `.encoder.config.idx_search_unidirectional` was removed. Offset calibration direction is fully defined by the sign of `.encoder.config.calib_scan_omega` and how the motor is wired up. * The unit of `.sensorless_estimator.vel_estimate` was changed from `rad/s` to `turns/s`. +* Several properties were changed to readonly. # Release Candidate ## [0.5.1] - Date TBD diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index 2f0c7a10..447264a4 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -400,22 +400,33 @@ void start_timers() { } } -static bool fetch_and_reset_adcs(float* current0_phB, float* current0_phC, float* current1_phB, float* current1_phC) { +static bool fetch_and_reset_adcs( + std::optional* current0, + std::optional* current1) { bool all_adcs_done = (ADC1->SR & ADC_SR_JEOC) == ADC_SR_JEOC && (ADC2->SR & (ADC_SR_EOC | ADC_SR_JEOC)) == (ADC_SR_EOC | ADC_SR_JEOC) && (ADC3->SR & (ADC_SR_EOC | ADC_SR_JEOC)) == (ADC_SR_EOC | ADC_SR_JEOC); if (!all_adcs_done) { return false; } - - bool m0_current_valid = m0_gate_driver.is_ready(); - bool m1_current_valid = m1_gate_driver.is_ready(); vbus_sense_adc_cb(ADC1->JDR1); - *current0_phB = m0_current_valid ? motors[0].phase_current_from_adcval(ADC2->JDR1) : NAN; - *current0_phC = m0_current_valid ? motors[0].phase_current_from_adcval(ADC3->JDR1) : NAN; - *current1_phB = m1_current_valid ? motors[1].phase_current_from_adcval(ADC2->DR) : NAN; - *current1_phC = m1_current_valid ? motors[1].phase_current_from_adcval(ADC3->DR) : NAN; + + if (m0_gate_driver.is_ready()) { + std::optional phB = motors[0].phase_current_from_adcval(ADC2->JDR1); + std::optional phC = motors[0].phase_current_from_adcval(ADC3->JDR1); + if (phB.has_value() && phC.has_value()) { + *current0 = {-*phB - *phC, *phB, *phC}; + } + } + + if (m1_gate_driver.is_ready()) { + std::optional phB = motors[1].phase_current_from_adcval(ADC2->DR); + std::optional phC = motors[1].phase_current_from_adcval(ADC3->DR); + if (phB.has_value() && phC.has_value()) { + *current1 = {-*phB - *phC, *phB, *phC}; + } + } ADC1->SR = ~(ADC_SR_JEOC); ADC2->SR = ~(ADC_SR_EOC | ADC_SR_JEOC | ADC_SR_OVR); @@ -492,18 +503,16 @@ void ControlLoop_IRQHandler(void) { uint32_t timestamp = timestamp_; // Ensure that all the ADCs are done - float current0_phB; - float current0_phC; - float current1_phB; - float current1_phC; + std::optional current0; + std::optional current1; - if (!fetch_and_reset_adcs(¤t0_phB, ¤t0_phC, ¤t1_phB, ¤t1_phC)) { + if (!fetch_and_reset_adcs(¤t0, ¤t1)) { motors[0].disarm_with_error(Motor::ERROR_BAD_TIMING); motors[1].disarm_with_error(Motor::ERROR_BAD_TIMING); } - motors[0].current_meas_cb(timestamp - TIM1_INIT_COUNT, {-current0_phB - current0_phC, current0_phB, current0_phC}); - motors[1].current_meas_cb(timestamp, {-current1_phB - current1_phC, current1_phB, current1_phC}); + motors[0].current_meas_cb(timestamp - TIM1_INIT_COUNT, current0); + motors[1].current_meas_cb(timestamp, current1); odrv.control_loop_cb(timestamp); @@ -511,13 +520,13 @@ void ControlLoop_IRQHandler(void) { // let's wait for them just to be sure. while (!(ADC2->SR & ADC_SR_EOC)); - if (!fetch_and_reset_adcs(¤t0_phB, ¤t0_phC, ¤t1_phB, ¤t1_phC)) { + if (!fetch_and_reset_adcs(¤t0, ¤t1)) { motors[0].disarm_with_error(Motor::ERROR_BAD_TIMING); motors[1].disarm_with_error(Motor::ERROR_BAD_TIMING); } - motors[0].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT, {-current0_phB - current0_phC, current0_phB, current0_phC}); - motors[1].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1), {-current1_phB - current1_phC, current1_phB, current1_phC}); + motors[0].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT, current0); + motors[1].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1), current1); motors[0].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT); motors[1].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)); diff --git a/Firmware/MotorControl/async_estimator.cpp b/Firmware/MotorControl/async_estimator.cpp index 24541b56..88cad20a 100644 --- a/Firmware/MotorControl/async_estimator.cpp +++ b/Firmware/MotorControl/async_estimator.cpp @@ -3,30 +3,28 @@ #include void AsyncEstimator::update(uint32_t timestamp) { - float rotor_phase = rotor_phase_src_ ? *rotor_phase_src_ : NAN; - float rotor_phase_vel = rotor_phase_vel_src_ ? *rotor_phase_vel_src_ : NAN; - float id = id_src_ ? *id_src_ : NAN; - float iq = iq_src_ ? *iq_src_ : NAN; + std::optional rotor_phase = rotor_phase_src_.get_current(); + std::optional rotor_phase_vel = rotor_phase_vel_src_.get_current(); + std::optional idq = idq_src_.get_current(); - if (std::isnan(rotor_phase) || std::isnan(rotor_phase_vel)) { - stator_phase_vel_ = NAN; - stator_phase_ = NAN; + 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_) { - last_timestamp_ = timestamp; - stator_phase_vel_ = 0.0f; - stator_phase_ = 0.0f; + // Skip first iteration and use it to reset state + rotor_flux_ = 0.0f; + phase_offset_ = 0.0f; active_ = true; return; } - last_timestamp_ = timestamp; - - float dt = (float)(timestamp - last_timestamp_) / (float)TIM_1_8_CLOCK_HZ; - // 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 @@ -40,9 +38,8 @@ void AsyncEstimator::update(uint32_t timestamp) { if (!acceptable_vel) slip_velocity = 0.0f; slip_vel_ = slip_velocity; // reporting only - stator_phase_vel_ = rotor_phase_vel + slip_velocity; - phase_offset_ += slip_velocity * dt; - phase_offset_ = wrap_pm_pi(phase_offset_); - stator_phase_ = wrap_pm_pi(rotor_phase + phase_offset_); + 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_); } diff --git a/Firmware/MotorControl/async_estimator.hpp b/Firmware/MotorControl/async_estimator.hpp index ea0189f7..3505ce53 100644 --- a/Firmware/MotorControl/async_estimator.hpp +++ b/Firmware/MotorControl/async_estimator.hpp @@ -3,6 +3,7 @@ #include #include +#include class AsyncEstimator : public ComponentBase { public: @@ -16,21 +17,20 @@ public: Config_t config_; // Inputs - float* rotor_phase_src_ = nullptr; - float* rotor_phase_vel_src_ = nullptr; - float* id_src_ = nullptr; - float* iq_src_ = nullptr; + InputPort rotor_phase_src_; + InputPort rotor_phase_vel_src_; + InputPort idq_src_; // State variables float active_ = false; uint32_t last_timestamp_ = 0; float rotor_flux_ = 0.0f; // [A] - float slip_vel_ = 0.0f; // [rad/s electrical] - float phase_offset_ = 0.0f; // [rad electrical] + float phase_offset_ = 0.0f; // [A] // Outputs - float stator_phase_vel_ = NAN; // [rad/s] rotor flux angular velocity estimate - float stator_phase_ = NAN; // [rad] rotor flux phase angle estimate + OutputPort slip_vel_ = 0.0f; // [rad/s electrical] + OutputPort stator_phase_vel_ = 0.0f; // [rad/s] rotor flux angular velocity estimate + OutputPort stator_phase_ = 0.0f; // [rad] rotor flux phase angle estimate }; #endif // __ASYNC_ESTIMATOR_HPP \ No newline at end of file diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 99aa7b80..a0b5ef8e 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -202,10 +202,8 @@ bool Axis::watchdog_check() { bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_armed) { CRITICAL_SECTION() { // Reset state variables - open_loop_controller_.Id_setpoint_ = NAN; - open_loop_controller_.Iq_setpoint_ = NAN; - open_loop_controller_.Vd_setpoint_ = NAN; - open_loop_controller_.Vq_setpoint_ = NAN; + 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_ = NAN; @@ -218,17 +216,15 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme 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_.Id_setpoint_src_ = &open_loop_controller_.Id_setpoint_; - motor_.current_control_.Iq_setpoint_src_ = &open_loop_controller_.Iq_setpoint_; - motor_.current_control_.Vd_setpoint_src_ = &open_loop_controller_.Vd_setpoint_; - motor_.current_control_.Vq_setpoint_src_ = &open_loop_controller_.Vq_setpoint_; - motor_.current_control_.phase_src_ = - async_estimator_.rotor_phase_src_ = - &open_loop_controller_.phase_; - motor_.phase_vel_src_ = - motor_.current_control_.phase_vel_src_ = - async_estimator_.rotor_phase_vel_src_ = - &open_loop_controller_.phase_vel_; + 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_); + async_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_); + async_estimator_.rotor_phase_vel_src_.connect_to(&open_loop_controller_.phase_vel_); } wait_for_control_iteration(); @@ -239,8 +235,8 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config, bool remain_arme 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_ - lockin_config.vel) <= std::numeric_limits::epsilon(); - bool reached_target_dist = open_loop_controller_.total_distance_ * dir >= lockin_config.finish_distance * dir; + bool reached_target_vel = std::abs(open_loop_controller_.phase_vel_.get_any().value_or(0.0f) - lockin_config.vel) <= std::numeric_limits::epsilon(); + bool reached_target_dist = open_loop_controller_.total_distance_.get_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) @@ -282,21 +278,21 @@ bool Axis::start_closed_loop_control() { // Hook up the data paths between the components CRITICAL_SECTION() { if (sensorless_mode) { - controller_.pos_estimate_linear_src_ = nullptr; - controller_.pos_estimate_circular_src_ = nullptr; - controller_.pos_wrap_src_ = nullptr; - controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_; + 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_ = &ax->encoder_.pos_circular_; - controller_.pos_wrap_src_ = &controller_.config_.circular_setpoint_range; - controller_.pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_; - controller_.vel_estimate_src_ = &ax->encoder_.vel_estimate_; + 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_ = nullptr; - controller_.pos_estimate_linear_src_ = nullptr; - controller_.pos_wrap_src_ = nullptr; - controller_.vel_estimate_src_ = nullptr; + 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; } @@ -304,14 +300,14 @@ bool Axis::start_closed_loop_control() { // To avoid any transient on startup, we intialize the setpoint to be the current position // note - input_pos_ is not set here. It is set to 0 earlier in this method and velocity control is used. if (controller_.config_.control_mode >= Controller::CONTROL_MODE_POSITION_CONTROL) { - float* pos_init_src = controller_.config_.circular_setpoints ? + std::optional pos_init = (controller_.config_.circular_setpoints ? controller_.pos_estimate_circular_src_ : - controller_.pos_estimate_linear_src_; - if (!pos_init_src) { + controller_.pos_estimate_linear_src_).get_any(); + if (!pos_init.has_value()) { return false; } else { - controller_.pos_setpoint_ = *pos_init_src; - controller_.input_pos_ = *pos_init_src; + controller_.pos_setpoint_ = *pos_init; + controller_.input_pos_ = *pos_init; } } controller_.input_pos_updated(); @@ -319,27 +315,28 @@ bool Axis::start_closed_loop_control() { // Avoid integrator windup issues controller_.vel_integrator_torque_ = 0.0f; - motor_.torque_setpoint_src_ = &controller_.torque_output_; + 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_.Id_setpoint_src_ = &motor_.Id_setpoint_; - motor_.current_control_.Iq_setpoint_src_ = &motor_.Iq_setpoint_; - motor_.current_control_.Vd_setpoint_src_ = &motor_.Vd_setpoint_; - motor_.current_control_.Vq_setpoint_src_ = &motor_.Vq_setpoint_; - motor_.current_control_.phase_src_ = - async_estimator_.rotor_phase_src_ = - sensorless_mode ? &sensorless_estimator_.phase_ : &encoder_.phase_; - motor_.phase_vel_src_ = - motor_.current_control_.phase_vel_src_ = - async_estimator_.rotor_phase_vel_src_ = - sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_; + motor_.current_control_.Idq_setpoint_src_.connect_to(&motor_.Idq_setpoint_); + motor_.current_control_.Vdq_setpoint_src_.connect_to(&motor_.Vdq_setpoint_); + + OutputPort* phase_src = sensorless_mode ? &sensorless_estimator_.phase_ : &encoder_.phase_; + motor_.current_control_.phase_src_.connect_to(phase_src); + async_estimator_.rotor_phase_src_.connect_to(phase_src); + + OutputPort* phase_vel_src = sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_; + motor_.phase_vel_src_.connect_to(phase_vel_src); + motor_.current_control_.phase_vel_src_.connect_to(phase_vel_src); + async_estimator_.rotor_phase_vel_src_.connect_to(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. - controller_.input_vel_ = config_.sensorless_ramp.vel / (2 * M_PI); - controller_.vel_setpoint_ = config_.sensorless_ramp.vel / (2 * M_PI); + float vel = config_.sensorless_ramp.vel / (2.0f * M_PI * motor_.config_.pole_pairs); + controller_.input_vel_ = vel; + controller_.vel_setpoint_ = vel; } } @@ -452,14 +449,14 @@ void Axis::run_state_machine_loop() { // 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 motor_is_ready = std::isnan(motor_.current_meas_.phA) - && std::isnan(motor_.current_meas_.phB) - && std::isnan(motor_.current_meas_.phC); - if (motor_is_ready) { + if (motor_.current_meas_.has_value()) { break; } + osDelay(1); } + sensorless_estimator_.error_ &= ~SensorlessEstimator::ERROR_UNKNOWN_CURRENT_MEASUREMENT; + for (;;) { // Load the task chain if a specific request is pending if (requested_state_ != AXIS_STATE_UNDEFINED) { diff --git a/Firmware/MotorControl/component.hpp b/Firmware/MotorControl/component.hpp index cfaeb828..aa156e1c 100644 --- a/Firmware/MotorControl/component.hpp +++ b/Firmware/MotorControl/component.hpp @@ -2,6 +2,8 @@ #define __COMPONENT_HPP #include +#include +#include class ComponentBase { public: @@ -17,4 +19,164 @@ public: virtual void update(uint32_t timestamp) = 0; }; + +template +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 +class OutputPort { +public: + /** + * @brief Initializes the output port with the specified value. + * + * An initialization value is required for get_any() to work properly. + * get_current() and get_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 get_current() 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 get_current() { + 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 get_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 get_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 +class InputPort { +public: + void connect_to(OutputPort* input_port) { + content_ = input_port; + } + + void connect_to(T* input_ptr) { + content_ = input_ptr; + } + + void disconnect() { + content_ = (OutputPort*)nullptr; + } + + std::optional get_current() { + if (content_.index() == 2) { + OutputPort* ptr = std::get<2>(content_); + return ptr ? ptr->get_current() : 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 get_previous() { + // if (content_.index() == 2) { + // OutputPort* ptr = std::get<2>(content_); + // return ptr ? ptr->get_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 get_any() { + if (content_.index() == 2) { + OutputPort* ptr = std::get<2>(content_); + return ptr ? ptr->get_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*> content_; +}; + + #endif // __COMPONENT_HPP \ No newline at end of file diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index a93acb51..c67c7133 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -99,23 +99,21 @@ static float limitVel(const float vel_limit, const float vel_estimate, const flo } bool Controller::update() { - float pos_estimate_linear = pos_estimate_linear_src_ ? *pos_estimate_linear_src_ : NAN; - float pos_estimate_circular = pos_estimate_circular_src_ ? *pos_estimate_circular_src_ : NAN; - float pos_wrap = pos_wrap_src_ ? *pos_wrap_src_ : NAN; - float vel_estimate = vel_estimate_src_ ? *vel_estimate_src_ : NAN; + std::optional pos_estimate_linear = pos_estimate_linear_src_.get_current(); + std::optional pos_estimate_circular = pos_estimate_circular_src_.get_current(); + std::optional pos_wrap = pos_wrap_src_.get_current(); + std::optional vel_estimate = vel_estimate_src_.get_current(); - // Reset output just in case the controller fails for any reason - torque_output_ = NAN; + std::optional anticogging_pos_estimate = axis_->encoder_.pos_estimate_.get_current(); + std::optional anticogging_vel_estimate = axis_->encoder_.vel_estimate_.get_current(); - // Calib_anticogging is only true when calibration is occurring, so we can't block anticogging_pos - float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio(); if (config_.anticogging.calib_anticogging) { - if (std::isnan(axis_->encoder_.pos_estimate_) || std::isnan(axis_->encoder_.vel_estimate_)) { + if (!anticogging_pos_estimate.has_value() || !anticogging_vel_estimate.has_value()) { set_error(ERROR_INVALID_ESTIMATE); return false; } // non-blocking - anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_); + anticogging_calibration(*anticogging_pos_estimate, *anticogging_vel_estimate); } // TODO also enable circular deltas for 2nd order filter, etc. @@ -160,8 +158,16 @@ bool Controller::update() { } break; case INPUT_MODE_MIRROR: { if (config_.axis_to_mirror < AXIS_COUNT) { - pos_setpoint_ = axes[config_.axis_to_mirror].encoder_.pos_estimate_ * config_.mirror_ratio; - vel_setpoint_ = axes[config_.axis_to_mirror].encoder_.vel_estimate_ * config_.mirror_ratio; + std::optional other_pos = axes[config_.axis_to_mirror].encoder_.pos_estimate_.get_current(); + std::optional other_vel = axes[config_.axis_to_mirror].encoder_.vel_estimate_.get_current(); + + if (!other_pos.has_value() || !other_vel.has_value()) { + set_error(ERROR_INVALID_ESTIMATE); + return false; + } + + pos_setpoint_ = *other_pos * config_.mirror_ratio; + vel_setpoint_ = *other_vel * config_.mirror_ratio; } else { set_error(ERROR_INVALID_MIRROR_AXIS); return false; @@ -193,7 +199,7 @@ bool Controller::update() { torque_setpoint_ = traj_step.Ydd * config_.inertia; axis_->trap_traj_.t_ += current_meas_period; } - anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate + anticogging_pos_estimate = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } break; default: { set_error(ERROR_INVALID_INPUT_MODE); @@ -210,21 +216,21 @@ bool Controller::update() { float pos_err; if (config_.circular_setpoints) { - if (std::isnan(pos_estimate_circular) || std::isnan(pos_wrap)) { + 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_src_); + pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap); // Circular delta - pos_err = pos_setpoint_ - pos_estimate_circular; - pos_err = wrap_pm(pos_err, 0.5f * pos_wrap); + pos_err = pos_setpoint_ - *pos_estimate_circular; + pos_err = wrap_pm(pos_err, 0.5f * *pos_wrap); } else { - if (std::isnan(pos_estimate_linear)) { + if (!pos_estimate_linear.has_value()) { set_error(ERROR_INVALID_ESTIMATE); return false; } - pos_err = pos_setpoint_ - pos_estimate_linear; + pos_err = pos_setpoint_ - *pos_estimate_linear; } vel_des += config_.pos_gain * pos_err; @@ -243,11 +249,11 @@ bool Controller::update() { // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim) if (config_.enable_overspeed_error) { // 0.0f to disable - if (std::isnan(vel_estimate)) { + if (!vel_estimate.has_value()) { set_error(ERROR_INVALID_ESTIMATE); return false; } - if (std::abs(vel_estimate) > config_.vel_limit_tolerance * vel_lim) { + if (std::abs(*vel_estimate) > config_.vel_limit_tolerance * vel_lim) { set_error(ERROR_OVERSPEED); return false; } @@ -275,17 +281,22 @@ bool Controller::update() { // 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 (std::isnan(vel_estimate)) { + if (!vel_estimate.has_value()) { set_error(ERROR_INVALID_ESTIMATE); return false; } - v_err = vel_des - vel_estimate; + v_err = vel_des - *vel_estimate; torque += (vel_gain * gain_scheduling_multiplier) * v_err; // Velocity integral action before limiting @@ -294,11 +305,11 @@ bool Controller::update() { // Velocity limiting in current mode if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_current_mode_vel_limit) { - if (std::isnan(vel_estimate)) { + if (!vel_estimate.has_value()) { set_error(ERROR_INVALID_ESTIMATE); return false; } - torque = limitVel(config_.vel_limit, vel_estimate, vel_gain, torque); + torque = limitVel(config_.vel_limit, *vel_estimate, vel_gain, torque); } // Torque limiting diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 483dc316..bcd2a9c6 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -75,10 +75,10 @@ public: Error error_ = ERROR_NONE; // Inputs - float* pos_estimate_linear_src_ = nullptr; - float* pos_estimate_circular_src_ = nullptr; - float* vel_estimate_src_ = nullptr; - float* pos_wrap_src_ = nullptr; + InputPort pos_estimate_linear_src_; + InputPort pos_estimate_circular_src_; + InputPort vel_estimate_src_; + InputPort pos_wrap_src_; float pos_setpoint_ = 0.0f; // [turns] float vel_setpoint_ = 0.0f; // [turn/s] @@ -99,11 +99,10 @@ public: bool anticogging_valid_ = false; // Outputs - float torque_output_ = NAN; + OutputPort torque_output_ = 0.0f; // custom setters void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); } - }; #endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 39a7a904..3efc142e 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -215,10 +215,8 @@ bool Encoder::run_offset_calibration() { CRITICAL_SECTION() { // Reset state variables - axis_->open_loop_controller_.Id_setpoint_ = NAN; - axis_->open_loop_controller_.Iq_setpoint_ = NAN; - axis_->open_loop_controller_.Vd_setpoint_ = NAN; - axis_->open_loop_controller_.Vq_setpoint_ = NAN; + 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_ = NAN; @@ -232,17 +230,15 @@ bool Encoder::run_offset_calibration() { axis_->open_loop_controller_.total_distance_ = 0.0f; axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL); - axis_->motor_.current_control_.Id_setpoint_src_ = &axis_->open_loop_controller_.Id_setpoint_; - axis_->motor_.current_control_.Iq_setpoint_src_ = &axis_->open_loop_controller_.Iq_setpoint_; - axis_->motor_.current_control_.Vd_setpoint_src_ = &axis_->open_loop_controller_.Vd_setpoint_; - axis_->motor_.current_control_.Vq_setpoint_src_ = &axis_->open_loop_controller_.Vq_setpoint_; - axis_->motor_.current_control_.phase_src_ = - axis_->async_estimator_.rotor_phase_src_ = - &axis_->open_loop_controller_.phase_; - axis_->motor_.phase_vel_src_ = - axis_->motor_.current_control_.phase_vel_src_ = - axis_->async_estimator_.rotor_phase_vel_src_ = - &axis_->open_loop_controller_.phase_vel_; + 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_->async_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_->async_estimator_.rotor_phase_vel_src_.connect_to(&axis_->open_loop_controller_.phase_vel_); } axis_->wait_for_control_iteration(); @@ -272,7 +268,7 @@ bool Encoder::run_offset_calibration() { // scan forward while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) { - bool reached_target_dist = axis_->open_loop_controller_.total_distance_ >= config_.calib_scan_distance; + bool reached_target_dist = axis_->open_loop_controller_.total_distance_.get_any().value_or(-INFINITY) >= config_.calib_scan_distance; if (reached_target_dist) { break; } @@ -312,7 +308,7 @@ bool Encoder::run_offset_calibration() { // scan backwards while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) { - bool reached_target_dist = axis_->open_loop_controller_.total_distance_ <= 0.0f; + bool reached_target_dist = axis_->open_loop_controller_.total_distance_.get_any().value_or(INFINITY) <= 0.0f; if (reached_target_dist) { break; } @@ -511,10 +507,6 @@ bool Encoder::update() { } else { if (!config_.ignore_illegal_hall_state) { set_error(ERROR_ILLEGAL_HALL_STATE); - pos_estimate_ = NAN; - vel_estimate_ = NAN; - phase_ = NAN; - phase_vel_ = NAN; return false; } } @@ -540,10 +532,6 @@ bool Encoder::update() { spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_); if (spi_error_rate_ > 0.005f) { set_error(ERROR_ABS_SPI_COM_FAIL); - pos_estimate_ = NAN; - vel_estimate_ = NAN; - phase_ = NAN; - phase_vel_ = NAN; return false; } } else { @@ -561,11 +549,7 @@ bool Encoder::update() { }break; default: { set_error(ERROR_UNSUPPORTED_ENCODER_MODE); - pos_estimate_ = NAN; - vel_estimate_ = NAN; - phase_ = NAN; - phase_vel_ = NAN; - return false; + return false; } break; } @@ -601,8 +585,14 @@ bool Encoder::update() { // Outputs from Encoder for Controller pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr; vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr; - pos_circular_ += wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 0.5f); - pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range); + + // 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_.get_any().value_or(0.0f); + pos_circular += wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 0.5f); + 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_.offset; @@ -628,13 +618,10 @@ bool Encoder::update() { //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_.offset_float); - // ph = fmodf(ph, 2*M_PI); + if (is_ready_) { phase_ = wrap_pm_pi(ph) * config_.direction; - phase_vel_ = (2*M_PI) * vel_estimate_ * axis_->motor_.config_.pole_pairs * config_.direction; - } else { - phase_ = NAN; - phase_vel_ = NAN; + phase_vel_ = (2*M_PI) * *vel_estimate_.get_current() * axis_->motor_.config_.pole_pairs * config_.direction; } return true; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index e439a104..3cfe357d 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -5,6 +5,7 @@ #include #include "utils.hpp" #include +#include "component.hpp" class Encoder : public ODriveIntf::EncoderIntf { @@ -86,8 +87,8 @@ public: int32_t shadow_count_ = 0; int32_t count_in_cpr_ = 0; float interpolation_ = 0.0f; - float phase_ = 0.0f; // [rad] - float phase_vel_ = 0.0f; // [rad/s] + OutputPort phase_ = 0.0f; // [rad] + OutputPort phase_vel_ = 0.0f; // [rad/s] float pos_estimate_counts_ = 0.0f; // [count] float pos_cpr_counts_ = 0.0f; // [count] float vel_estimate_counts_ = 0.0f; // [count/s] @@ -97,9 +98,9 @@ public: int32_t pos_abs_ = 0; float spi_error_rate_ = 0.0f; - float pos_estimate_ = 0.0f; // [turn] - float vel_estimate_ = 0.0f; // [turn/s] - float pos_circular_ = 0.0f; // [turn] + OutputPort pos_estimate_ = 0.0f; // [turn] + OutputPort vel_estimate_ = 0.0f; // [turn/s] + OutputPort pos_circular_ = 0.0f; // [turn] bool pos_estimate_valid_ = false; bool vel_estimate_valid_ = false; diff --git a/Firmware/MotorControl/foc.cpp b/Firmware/MotorControl/foc.cpp index a1b2d3df..babf683f 100644 --- a/Firmware/MotorControl/foc.cpp +++ b/Firmware/MotorControl/foc.cpp @@ -3,26 +3,34 @@ #include Motor::Error AlphaBetaFrameController::on_measurement( - float vbus_voltage, std::array currents, + std::optional vbus_voltage, + std::optional> currents, uint32_t input_timestamp) { - // Clarke transform - float Ialpha = currents[0]; - float Ibeta = one_by_sqrt3 * (currents[1] - currents[2]); - return on_measurement(vbus_voltage, Ialpha, Ibeta, input_timestamp); + + std::optional 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], float* ibus) { - float mod_alpha = NAN; - float mod_beta = NAN; - - Motor::Error status = get_alpha_beta_output(output_timestamp, &mod_alpha, &mod_beta, ibus); + uint32_t output_timestamp, float (&pwm_timings)[3], + std::optional* ibus) { + std::optional 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 (std::isnan(mod_alpha) || std::isnan(mod_alpha)) { + } else if (!mod_alpha_beta.has_value() || std::isnan(mod_alpha_beta->first) || std::isnan(mod_alpha_beta->second)) { return Motor::ERROR_MODULATION_IS_NAN; - } else if (SVM(mod_alpha, mod_beta, &pwm_timings[0], &pwm_timings[1], &pwm_timings[2]) != 0) { + } else if (SVM(mod_alpha_beta->first, mod_alpha_beta->second, &pwm_timings[0], &pwm_timings[1], &pwm_timings[2]) != 0) { return Motor::ERROR_MODULATION_MAGNITUDE; } @@ -32,27 +40,26 @@ Motor::Error AlphaBetaFrameController::get_output( void FieldOrientedController::reset() { v_current_control_integral_d_ = 0.0f; v_current_control_integral_q_ = 0.0f; - vbus_voltage_measured_ = NAN; - Ialpha_measured_ = NAN; - Ibeta_measured_ = NAN; + vbus_voltage_measured_ = std::nullopt; + Ialpha_beta_measured_ = std::nullopt; } Motor::Error FieldOrientedController::on_measurement( - float vbus_voltage, float Ialpha, float Ibeta, - uint32_t input_timestamp) { + std::optional vbus_voltage, std::optional Ialpha_beta, + uint32_t input_timestamp) { // Store the measurements for later processing. i_timestamp_ = input_timestamp; vbus_voltage_measured_ = vbus_voltage; - Ialpha_measured_ = Ialpha; - Ibeta_measured_ = Ibeta; + Ialpha_beta_measured_ = Ialpha_beta; return Motor::ERROR_NONE; } ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output( - uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) { + uint32_t output_timestamp, std::optional* mod_alpha_beta, + std::optional* ibus) { - if (std::isnan(vbus_voltage_measured_) || std::isnan(Ialpha_measured_) || std::isnan(Ibeta_measured_)) { + 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) { @@ -64,56 +71,66 @@ ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output( // rate than current sensor updates. In this case we can reuse mod_d and // mod_q from a previous iteration. - // Fetch member variables into local variables to make the optimizer's life easier. - float vbus_voltage = vbus_voltage_measured_; - float Ialpha = Ialpha_measured_; - float Ibeta = Ibeta_measured_; - float Vd = Vd_setpoint_; - float Vq = Vq_setpoint_; - float Id_setpoint = Id_setpoint_; - float Iq_setpoint = Iq_setpoint_; - float phase = phase_; - float phase_vel = phase_vel_; - - if (std::isnan(phase) || std::isnan(phase_vel)) { - return Motor::ERROR_UNKNOWN_PHASE; - } - - // Park transform - 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); - float Id = c_I * Ialpha + s_I * Ibeta; - float Iq = c_I * Ibeta - s_I * Ialpha; - Iq_measured_ += I_measured_report_filter_k_ * (Iq - Iq_measured_); - Id_measured_ += I_measured_report_filter_k_ * (Id - Id_measured_); - - // Current error - float Ierr_d = Id_setpoint - Id; - float Ierr_q = Iq_setpoint - Iq; - - - if (enable_current_control_) { - // Check for current sense saturation - if (std::isnan(Ierr_d) || std::isnan(Ierr_q)) { - return Motor::ERROR_UNKNOWN_CURRENT; - } - - // Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode) - Vd += v_current_control_integral_d_ + Ierr_d * p_gain_; - Vq += v_current_control_integral_q_ + Ierr_q * p_gain_; - } - - if (std::isnan(vbus_voltage)) { + 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 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 = V_to_mod * Vd; - float mod_q = V_to_mod * Vq; + 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 / sqrtf(mod_d * mod_d + mod_q * mod_q); @@ -124,25 +141,34 @@ ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output( 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); + 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 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_temp = c_p * mod_d - s_p * mod_q; - float mod_beta_temp = c_p * mod_q + s_p * mod_d; + 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_temp; - final_v_beta_ = mod_to_V * mod_beta_temp; + final_v_alpha_ = mod_to_V * mod_alpha; + final_v_beta_ = mod_to_V * mod_beta; - *mod_alpha = mod_alpha_temp; - *mod_beta = mod_beta_temp; - *ibus = mod_d * Id + mod_q * Iq; + *mod_alpha_beta = {mod_alpha, mod_beta}; + + if (Idq.has_value()) { + auto [Id, Iq] = *Idq; + *ibus = mod_d * Id + mod_q * Iq; + } + return Motor::ERROR_NONE; } @@ -150,11 +176,9 @@ void FieldOrientedController::update(uint32_t timestamp) { CRITICAL_SECTION() { ctrl_timestamp_ = timestamp; enable_current_control_ = enable_current_control_src_; - Id_setpoint_ = Id_setpoint_src_ ? *Id_setpoint_src_ : NAN; - Iq_setpoint_ = Iq_setpoint_src_ ? *Iq_setpoint_src_ : NAN; - Vd_setpoint_ = Vd_setpoint_src_ ? *Vd_setpoint_src_ : NAN; - Vq_setpoint_ = Vq_setpoint_src_ ? *Vq_setpoint_src_ : NAN; - phase_ = phase_src_ ? *phase_src_ : NAN; - phase_vel_ = phase_vel_src_ ? *phase_vel_src_ : NAN; + Idq_setpoint_ = Idq_setpoint_src_.get_current(); + Vdq_setpoint_ = Vdq_setpoint_src_.get_current(); + phase_ = phase_src_.get_current(); + phase_vel_ = phase_vel_src_.get_current(); } } diff --git a/Firmware/MotorControl/foc.hpp b/Firmware/MotorControl/foc.hpp index ba2b3ec7..30fc072e 100644 --- a/Firmware/MotorControl/foc.hpp +++ b/Firmware/MotorControl/foc.hpp @@ -17,43 +17,41 @@ public: void reset() final; ODriveIntf::MotorIntf::Error on_measurement( - float vbus_voltage, float Ialpha, float Ibeta, uint32_t input_timestamp) final; + std::optional vbus_voltage, + std::optional Ialpha_beta, + uint32_t input_timestamp) final; ODriveIntf::MotorIntf::Error get_alpha_beta_output( - uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) final; + uint32_t output_timestamp, + std::optional* mod_alpha_beta, + std::optional* ibus) final; // Config - these values are set while this controller is inactive - float p_gain_ = NAN; // [V/A] should be auto set after resistance and inductance measurement - float i_gain_ = NAN; // [V/As] should be auto set after resistance and inductance measurement + std::optional 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; - float* Id_setpoint_src_ = nullptr; - float* Iq_setpoint_src_ = nullptr; - float* Vd_setpoint_src_ = nullptr; - float* Vq_setpoint_src_ = nullptr; - float* phase_src_ = nullptr; - float* phase_vel_src_ = nullptr; + InputPort Idq_setpoint_src_; + InputPort Vdq_setpoint_src_; + InputPort phase_src_; + InputPort 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 - float Id_setpoint_; // [A] only used if enable_current_control_ == true - float Iq_setpoint_; // [A] only used if enable_current_control_ == true - float Vd_setpoint_; // [V] acts as input if enable_current_control_ == false and as output otherwise - float Vq_setpoint_; // [V] acts as input if enable_current_control_ == false and as output otherwise - float phase_; // [rad] - float phase_vel_; // [rad/s] + std::optional Idq_setpoint_; // [A] only used if enable_current_control_ == true + std::optional Vdq_setpoint_; // [V] feed-forward voltage term (or standalone setpoint if enable_current_control_ == false) + std::optional phase_; // [rad] + std::optional phase_vel_; // [rad/s] // These values (or some of them) are updated inside on_measurement() and get_alpha_beta_output() uint32_t i_timestamp_; - float vbus_voltage_measured_ = NAN; // [V] - float Ialpha_measured_ = NAN; // [A] - float Ibeta_measured_ = NAN; // [A] - float Id_measured_ = 0.0f; // [A] - float Iq_measured_ = 0.0f; // [A] + std::optional vbus_voltage_measured_; // [V] + std::optional 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; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6a9d8a48..02deeed5 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -272,6 +272,34 @@ void ODrive::control_loop_cb(uint32_t timestamp) { // 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.async_estimator_.slip_vel_.reset(); + axis.async_estimator_.stator_phase_vel_.reset(); + axis.async_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(); } @@ -300,7 +328,12 @@ void ODrive::control_loop_cb(uint32_t timestamp) { 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(); @@ -318,11 +351,8 @@ void ODrive::control_loop_cb(uint32_t timestamp) { MEASURE_TIME(axis.task_times_.open_loop_controller_update) axis.open_loop_controller_.update(timestamp); - MEASURE_TIME(axis.task_times_.async_estimator_update) - axis.async_estimator_.update(timestamp); - MEASURE_TIME(axis.task_times_.motor_update) - axis.motor_.update(); // uses torque from controller and phase_vel from encoder + 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 async_estimator_ @@ -406,6 +436,10 @@ static void rtos_main(void*) { axis.encoder_.setup(); } + for(auto& axis: axes){ + axis.async_estimator_.idq_src_.connect_to(&axis.motor_.Idq_setpoint_); + } + // Start PWM and enable adc interrupts/callbacks start_adc_pwm(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 109a7f00..f88ffe30 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -18,35 +18,43 @@ struct ResistanceMeasurementControlLaw : AlphaBetaFrameController { void reset() final { test_voltage_ = 0.0f; - test_mod_ = NAN; + test_mod_ = std::nullopt; } ODriveIntf::MotorIntf::Error on_measurement( - float vbus_voltage, float Ialpha, float Ibeta, - uint32_t input_timestamp) final - { - actual_current_ = Ialpha; - test_voltage_ += (kI * current_meas_period) * (target_current_ - actual_current_); + std::optional vbus_voltage, + std::optional 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_); + } 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 (std::isnan(vbus_voltage)) { + } else if (!vbus_voltage.has_value()) { return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE; } else { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); + 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, float* mod_alpha, float* mod_beta, float* ibus) { - if (std::isnan(test_mod_)) { + ODriveIntf::MotorIntf::Error get_alpha_beta_output( + uint32_t output_timestamp, + std::optional* mod_alpha_beta, + std::optional* ibus) final { + if (!test_mod_.has_value()) { return Motor::ERROR_CONTROLLER_INITIALIZING; } else { - *mod_alpha = test_mod_; - *mod_beta = 0.0f; - *ibus = test_mod_ * actual_current_; + *mod_alpha_beta = {*test_mod_, 0.0f}; + *ibus = *test_mod_ * actual_current_; return Motor::ERROR_NONE; } } @@ -60,7 +68,7 @@ struct ResistanceMeasurementControlLaw : AlphaBetaFrameController { float actual_current_ = 0.0f; float target_current_ = 0.0f; float test_voltage_ = 0.0f; - float test_mod_ = NAN; + std::optional test_mod_ = NAN; }; /** @@ -75,13 +83,17 @@ struct InductanceMeasurementControlLaw : AlphaBetaFrameController { attached_ = false; } - ODriveIntf::MotorIntf::Error on_measurement(float vbus_voltage, - float Ialpha, float Ibeta, uint32_t input_timestamp) final + ODriveIntf::MotorIntf::Error on_measurement( + std::optional vbus_voltage, + std::optional Ialpha_beta, + uint32_t input_timestamp) final { - if (std::isnan(Ialpha) || std::isnan(vbus_voltage)) { - return {Motor::ERROR_UNKNOWN_VBUS_VOLTAGE}; + 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_); @@ -97,12 +109,12 @@ struct InductanceMeasurementControlLaw : AlphaBetaFrameController { } ODriveIntf::MotorIntf::Error get_alpha_beta_output( - uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) final + uint32_t output_timestamp, std::optional* mod_alpha_beta, + std::optional* ibus) final { test_voltage_ *= -1.0f; float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); - *mod_alpha = test_voltage_ * vfactor; - *mod_beta = 0.0f; + *mod_alpha_beta = {test_voltage_ * vfactor, 0.0f}; *ibus = 0.0f; return Motor::ERROR_NONE; } @@ -263,9 +275,9 @@ bool Motor::disarm(bool* was_armed) { // TODO: allow update on user-request or update automatically via hooks void Motor::update_current_controller_gains() { // Calculate current control gains - current_control_.p_gain_ = config_.current_control_bandwidth * config_.phase_inductance; + float p_gain = config_.current_control_bandwidth * config_.phase_inductance; float plant_pole = config_.phase_resistance / config_.phase_inductance; - current_control_.i_gain_ = plant_pole * current_control_.p_gain_; + current_control_.pi_gains_ = {p_gain, plant_pole * p_gain}; } bool Motor::apply_config() { @@ -352,10 +364,11 @@ float Motor::max_available_torque() { } } -float Motor::phase_current_from_adcval(uint32_t ADCValue) { +std::optional 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) { - disarm_with_error(ERROR_CURRENT_SENSE_SATURATION); + error_ |= ERROR_CURRENT_SENSE_SATURATION; + return std::nullopt; } int adcval_bal = (int)ADCValue - (1 << 11); @@ -461,24 +474,22 @@ bool Motor::run_calibration() { return true; } -void Motor::update() { - float torque = torque_setpoint_src_ ? *torque_setpoint_src_ : NAN; - float phase_vel = phase_vel_src_ ? *phase_vel_src_ : NAN; +void Motor::update(uint32_t timestamp) { + std::optional torque = torque_setpoint_src_.get_current(); - // Reset output just in case the controller fails for any reason - Iq_setpoint_ = NAN; - // Id_setpoint_ = NAN; // this doubles as a state variable so we can't reset it + if (!torque.has_value()) { + error_ |= ERROR_UNKNOWN_TORQUE; + return; + } - float vd = 0.0f; - float vq = 0.0f; - float id = Id_setpoint_; - float iq; + auto [id, iq] = Idq_setpoint_.get_previous() + .value_or(float2D{0.0f, 0.0f}); // Id doubles as a state variable // Convert torque to current if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) { - iq = torque / (axis_->motor_.config_.torque_constant * fmax(axis_->async_estimator_.rotor_flux_, config_.acim_gain_min_flux)); + iq = *torque / (axis_->motor_.config_.torque_constant * fmax(axis_->async_estimator_.rotor_flux_, config_.acim_gain_min_flux)); } else { - iq = torque / axis_->motor_.config_.torque_constant; + iq = *torque / axis_->motor_.config_.torque_constant; } iq *= direction_; @@ -495,44 +506,61 @@ void Motor::update() { id = std::clamp(id, config_.acim_autoflux_min_Id, ilim); } + 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_.async_estimator_update) + axis_->async_estimator_.update(timestamp); + + float vd = 0.0f; + float vq = 0.0f; + + std::optional phase_vel = phase_vel_src_.get_current(); + if (config_.R_wL_FF_enable) { - vd -= phase_vel * config_.phase_inductance * iq; - vq += phase_vel * config_.phase_inductance * id; + 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) { - vq += phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs); - } + 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 - vd += id; - vq += iq; - id = NAN; - iq = NAN; + Vdq_setpoint_ = {vd + id, vq + iq}; + } else { + Vdq_setpoint_ = {vd, vq}; } - - Vd_setpoint_ = vd; - Vq_setpoint_ = vq; - Id_setpoint_ = id; - Iq_setpoint_ = iq; } /** * @brief Called when the underlying hardware timer triggers an update event. */ -void Motor::current_meas_cb(uint32_t timestamp, Iph_ABC_t current) { +void Motor::current_meas_cb(uint32_t timestamp, std::optional current) { // TODO: this is platform specific //const float current_meas_period = static_cast(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ; TaskTimerContext tmr{axis_->task_times_.current_sense}; - bool current_valid = !std::isnan(current.phA) - && !std::isnan(current.phB) - && !std::isnan(current.phC); - n_evt_current_measurement_++; bool dc_calib_valid = (dc_calib_running_since_ >= config_.dc_calib_tau * 7.5f) @@ -540,23 +568,14 @@ void Motor::current_meas_cb(uint32_t timestamp, Iph_ABC_t current) { && (abs(DC_calib_.phB) < max_dc_calib_) && (abs(DC_calib_.phC) < max_dc_calib_); - if (current_valid && dc_calib_valid) { - current.phA -= DC_calib_.phA; - current.phB -= DC_calib_.phB; - current.phC -= DC_calib_.phC; - I_leak_ = current.phA + current.phB + current.phC; // sum should be close to 0 - current_meas_.phA = current.phA - I_leak_ / 3.0f; - current_meas_.phB = current.phB - I_leak_ / 3.0f; - current_meas_.phC = current.phC - I_leak_ / 3.0f; + 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 { - I_leak_ = NAN; - current_meas_.phA = NAN; - current_meas_.phB = NAN; - current_meas_.phC = NAN; - } - - if (abs(I_leak_) > config_.I_leak_max) { - disarm_with_error(ERROR_I_LEAK_OUT_OF_RANGE); + current_meas_ = std::nullopt; } // Run system-level checks (e.g. overvoltage/undervoltage condition) @@ -565,17 +584,29 @@ void Motor::current_meas_cb(uint32_t timestamp, Iph_ABC_t current) { // effect on the PWM. odrv.do_fast_checks(); - // 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; - if (2.0f / 3.0f * (SQ(current_meas_.phA) + SQ(current_meas_.phB) + SQ(current_meas_.phC)) > SQ(Itrip)) { - disarm_with_error(ERROR_CURRENT_LIMIT_VIOLATION); + 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)); + 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_.phA, current_meas_.phB, current_meas_.phC}, + current_meas_.has_value() ? + std::make_optional(std::array{current_meas_->phA, current_meas_->phB, current_meas_->phC}) + : std::nullopt, timestamp); if (err != ERROR_NONE) { disarm_with_error(err); @@ -586,19 +617,15 @@ void Motor::current_meas_cb(uint32_t timestamp, Iph_ABC_t current) { /** * @brief Called when the underlying hardware timer triggers an update event. */ -void Motor::dc_calib_cb(uint32_t timestamp, Iph_ABC_t current) { +void Motor::dc_calib_cb(uint32_t timestamp, std::optional current) { const float dc_calib_period = static_cast(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ; TaskTimerContext tmr{axis_->task_times_.dc_calib}; - bool current_valid = !std::isnan(current.phA) - && !std::isnan(current.phB) - && !std::isnan(current.phC); - - if (current_valid) { + 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_.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; @@ -615,7 +642,7 @@ void Motor::pwm_update_cb(uint32_t output_timestamp) { Error control_law_status = ERROR_CONTROLLER_FAILED; float pwm_timings[3] = {NAN, NAN, NAN}; - float i_bus = 0.0f; + std::optional i_bus; if (control_law_) { control_law_status = control_law_->get_output( @@ -632,23 +659,27 @@ void Motor::pwm_update_cb(uint32_t output_timestamp) { apply_pwm_timings(next_timings, false); } else if (is_armed_) { - i_bus = 0.0f; 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 something above failed, reset I_bus to 0A. 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; + I_bus_ = *i_bus; - if (i_bus < config_.I_bus_hard_min || i_bus > config_.I_bus_hard_max) { + if (*i_bus < config_.I_bus_hard_min || *i_bus > config_.I_bus_hard_max) { disarm_with_error(ERROR_I_BUS_OUT_OF_RANGE); } diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 27834014..0b3c0b9f 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -10,11 +10,6 @@ class Motor; class Motor : public ODriveIntf::MotorIntf { public: - struct Iph_ABC_t { - float phA; - float phB; - float phC; - }; // NOTE: for gimbal motors, all units of Nm are instead V. // example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)] @@ -82,15 +77,15 @@ public: bool do_checks(uint32_t timestamp); float effective_current_lim(); float max_available_torque(); - float phase_current_from_adcval(uint32_t ADCValue); + std::optional 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(); + void update(uint32_t timestamp); // These functions are called as appropriate from the board.cpp file. - void current_meas_cb(uint32_t timestamp, Iph_ABC_t current); - void dc_calib_cb(uint32_t timestamp, Iph_ABC_t current); + void current_meas_cb(uint32_t timestamp, std::optional current); + void dc_calib_cb(uint32_t timestamp, std::optional current); void pwm_update_cb(uint32_t output_timestamp); // hardware config @@ -113,26 +108,23 @@ public: // Do not write to this variable directly! // It is for exclusive use by the safety_critical_... functions. bool is_armed_ = false; - bool is_calibrated_ = config_.pre_calibrated; - Iph_ABC_t current_meas_ = {NAN, NAN, NAN}; + bool is_calibrated_ = false; // Set in apply_config() + std::optional 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_leak_ = NAN; // close to zero if only two current sensors are available float I_bus_ = 0.0f; // this motors contribution to the bus current - bool current_meas_valid_ = false; // if false, the measured current values must not be used for control 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() - float* torque_setpoint_src_ = nullptr; // Usually points to the Controller object's output - float* phase_vel_src_ = nullptr; // Usually points to the Encoder object's output + InputPort torque_setpoint_src_; // Usually points to the Controller object's output + InputPort 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 - float Vd_setpoint_ = NAN; // fed to the FOC - float Vq_setpoint_ = NAN; // fed to the FOC - float Id_setpoint_ = 0.0f; // fed to the FOC - float Iq_setpoint_ = NAN; // fed to the FOC + OutputPort Vdq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC + OutputPort Idq_setpoint_ = {{0.0f, 0.0f}}; // fed to the FOC PhaseControlLaw<3>* control_law_; }; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index ee17e125..bc78d7b0 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -228,7 +228,7 @@ public: Oscilloscope oscilloscope_{ &axes[0].motor_.current_control_.v_current_control_integral_d_, // trigger_src 0.5f, // trigger_threshold - &axes[0].motor_.current_control_.Ialpha_measured_ // data_src + nullptr // &axes[0].motor_.current_control_.Ialpha_measured_ // data_src TODO: change data type }; BoardConfig_t config_; diff --git a/Firmware/MotorControl/open_loop_controller.cpp b/Firmware/MotorControl/open_loop_controller.cpp index b111f41c..506e4898 100644 --- a/Firmware/MotorControl/open_loop_controller.cpp +++ b/Firmware/MotorControl/open_loop_controller.cpp @@ -3,25 +3,25 @@ #include void OpenLoopController::update(uint32_t timestamp) { - if (std::isnan(Id_setpoint_) || std::isnan(Id_setpoint_) || std::isnan(phase_) || std::isnan(phase_vel_)) { - Id_setpoint_ = 0.0f; - Iq_setpoint_ = 0.0f; - Vd_setpoint_ = 0.0f; - Vq_setpoint_ = 0.0f; - phase_ = 0.0f; - phase_vel_ = 0.0f; - timestamp_ = timestamp; - } + auto [prev_Id, prev_Iq] = Idq_setpoint_.get_previous().value_or(float2D{0.0f, 0.0f}); + auto [prev_Vd, prev_Vq] = Vdq_setpoint_.get_previous().value_or(float2D{0.0f, 0.0f}); + float phase = phase_.get_previous().value_or(0.0f); + float phase_vel = phase_vel_.get_previous().value_or(0.0f); float dt = (float)(timestamp - timestamp_) / (float)TIM_1_8_CLOCK_HZ; - Id_setpoint_ = std::clamp(target_current_, Id_setpoint_ - max_current_ramp_ * dt, Id_setpoint_ + max_current_ramp_ * dt); - Iq_setpoint_ = 0.0f; - Vd_setpoint_ = std::clamp(target_voltage_, Vd_setpoint_ - max_voltage_ramp_ * dt, Vd_setpoint_ + max_voltage_ramp_ * dt); - Vq_setpoint_ = 0.0f; - - phase_vel_ = std::clamp(target_vel_, phase_vel_ - max_phase_vel_ramp_ * dt, phase_vel_ + max_phase_vel_ramp_ * dt); - phase_ = wrap_pm_pi(phase_ + phase_vel_ * dt); - total_distance_ += phase_vel_ * dt; + 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_.get_previous().value_or(0.0f) + phase_vel * dt; timestamp_ = timestamp; } diff --git a/Firmware/MotorControl/open_loop_controller.hpp b/Firmware/MotorControl/open_loop_controller.hpp index 143a42e5..82356a23 100644 --- a/Firmware/MotorControl/open_loop_controller.hpp +++ b/Firmware/MotorControl/open_loop_controller.hpp @@ -3,6 +3,7 @@ #include "component.hpp" #include +#include class OpenLoopController : public ComponentBase { public: @@ -14,19 +15,17 @@ public: float max_phase_vel_ramp_ = INFINITY; // [rad/s^2] // Inputs - float target_vel_ = NAN; - float target_current_ = NAN; - float target_voltage_ = NAN; + float target_vel_ = 0.0f; + float target_current_ = 0.0f; + float target_voltage_ = 0.0f; // State/Outputs uint32_t timestamp_ = 0; - float Id_setpoint_ = NAN; - float Iq_setpoint_ = NAN; - float Vd_setpoint_ = NAN; - float Vq_setpoint_ = NAN; - float phase_ = NAN; - float phase_vel_ = NAN; - float total_distance_ = NAN; + OutputPort Idq_setpoint_ = {{0.0f, 0.0f}}; + OutputPort Vdq_setpoint_ = {{0.0f, 0.0f}}; + OutputPort phase_ = 0.0f; + OutputPort phase_vel_ = 0.0f; + OutputPort total_distance_ = 0.0f; }; #endif // __OPEN_LOOP_CONTROLLER_HPP \ No newline at end of file diff --git a/Firmware/MotorControl/phase_control_law.hpp b/Firmware/MotorControl/phase_control_law.hpp index bda2ccdc..1a95ac29 100644 --- a/Firmware/MotorControl/phase_control_law.hpp +++ b/Firmware/MotorControl/phase_control_law.hpp @@ -20,16 +20,20 @@ public: * * Beware that all inputs can be NAN. * - * @param vbus_voltage: The most recently measured DC link voltage. NAN if - * the measurement is not available or valid for some reason. + * @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. Any of the values can be NAN if the measurement is not - * available or valid for some reason. + * 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(float vbus_voltage, - std::array currents, uint32_t input_timestamp) = 0; + virtual ODriveIntf::MotorIntf::Error on_measurement( + std::optional vbus_voltage, + std::optional> currents, + uint32_t input_timestamp) = 0; /** * @brief Shall calculate the PWM timings for the specified target time. @@ -60,28 +64,34 @@ public: * 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], - float* ibus) = 0; + virtual ODriveIntf::MotorIntf::Error get_output( + uint32_t output_timestamp, + float (&pwm_timings)[N_PHASES], + std::optional* ibus) = 0; }; class AlphaBetaFrameController : public PhaseControlLaw<3> { private: - ODriveIntf::MotorIntf::Error on_measurement(float vbus_voltage, - std::array currents, uint32_t input_timestamp) final; + ODriveIntf::MotorIntf::Error on_measurement( + std::optional vbus_voltage, + std::optional> currents, + uint32_t input_timestamp) final; - ODriveIntf::MotorIntf::Error get_output(uint32_t output_timestamp, - float (&pwm_timings)[3], - float* ibus) final; + ODriveIntf::MotorIntf::Error get_output( + uint32_t output_timestamp, + float (&pwm_timings)[3], + std::optional* ibus) final; protected: virtual ODriveIntf::MotorIntf::Error on_measurement( - float vbus_voltage, float Ialpha, float Ibeta, uint32_t input_timestamp) = 0; + std::optional vbus_voltage, + std::optional Ialpha_beta, + uint32_t input_timestamp) = 0; virtual ODriveIntf::MotorIntf::Error get_alpha_beta_output( uint32_t output_timestamp, - float* mod_alpha, float* mod_beta, - float* ibus) = 0; + std::optional* mod_alpha_beta, + std::optional* ibus) = 0; }; #endif // __PHASE_CONTROL_LAW_HPP \ No newline at end of file diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 70819875..c3951056 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,6 +1,15 @@ #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 @@ -10,20 +19,33 @@ bool SensorlessEstimator::update() { // 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. - if (std::isnan(flux_state_[0]) || std::isnan(flux_state_[1]) || std::isnan(pll_pos_)) { - // Automatically reset state if it becomes NAN. The state becomes NAN - // when invalid current measurements are processed (e.g. because of the - // opamp being uninitialized). - flux_state_[0] = 0.0f; - flux_state_[1] = 0.0f; - pll_pos_ = 0.0f; - phase_vel_ = 0.0f; + // 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; + 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 (!current_meas.has_value()) { + error_ |= ERROR_UNKNOWN_CURRENT_MEASUREMENT; + reset(); // Reset state for when the next valid current measurement comes in. + return false; } // Clarke transform float I_alpha_beta[2] = { - -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, - one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)}; + current_meas->phA, + one_by_sqrt3 * (current_meas->phB - current_meas->phC)}; // alpha-beta vector operations float eta[2]; @@ -59,31 +81,21 @@ bool SensorlessEstimator::update() { V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha_; V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta_; - // 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; - pll_pos_ = NAN; - phase_ = NAN; - vel_estimate_ = NAN; - return false; - } + float phase_vel = phase_vel_.get_previous().value_or(0.0f); // predict PLL phase with velocity - pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * phase_vel_); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * phase_vel); // update PLL phase with observer permanent magnet phase - phase_ = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(phase_ - pll_pos_); + 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; - // convert to mechanical turns/s for controller usage. - vel_estimate_ = phase_vel_ / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI); + 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; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index b15aef25..3ac6f488 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -1,6 +1,8 @@ #ifndef __SENSORLESS_ESTIMATOR_HPP #define __SENSORLESS_ESTIMATOR_HPP +#include "component.hpp" + class SensorlessEstimator : public ODriveIntf::SensorlessEstimatorIntf { public: struct Config_t { @@ -9,6 +11,7 @@ public: float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } }; + void reset(); bool update(); Axis* axis_ = nullptr; // set by Axis constructor @@ -16,15 +19,13 @@ public: // TODO: expose on protocol Error error_ = ERROR_NONE; - float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] - float phase_vel_ = 0.0f; // [rad/s] - float vel_estimate_ = 0.0f; // [turns/s] - // float pll_kp_ = 0.0f; // [rad/s / rad] - // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] float flux_state_[2] = {0.0f, 0.0f}; // [Vs] float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] - bool estimator_good_ = false; + + OutputPort phase_ = 0.0f; // [rad] + OutputPort phase_vel_ = 0.0f; // [rad/s] + OutputPort vel_estimate_ = 0.0f; // [turns/s] }; #endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index c6c69d8c..9f690e80 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -132,6 +132,7 @@ FLAGS += '-DUSE_HAL_DRIVER' FLAGS += '-mthumb' FLAGS += '-mfloat-abi=hard' +FLAGS += '-Wno-psabi' -- suppress unimportant note about ABI compatibility in GCC 10 FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} -- linker flags @@ -145,6 +146,7 @@ if tup.getconfig("DEBUG") == "true" then FLAGS += '-g -gdwarf-2' OPT += '-Og' else + FLAGS += '-g' OPT += '-O2' end diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index ae23a614..b5ac9ce3 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -256,8 +256,8 @@ void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checks } else { Axis& axis = axes[motor_number]; respond(response_channel, use_checksum, "%f %f", - (double)axis.encoder_.pos_estimate_, - (double)axis.encoder_.vel_estimate_); + (double)axis.encoder_.pos_estimate_.get_any().value_or(0.0f), + (double)axis.encoder_.vel_estimate_.get_any().value_or(0.0f)); } } diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 16fac7fd..a2ac1f1f 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -206,16 +206,19 @@ void CANSimple::get_encoder_estimates_callback(Axis* axis, can_Message_t& msg) { // uint32_t floatBytes = *(reinterpret_cast(&(axis->encoder_.pos_estimate_))); uint32_t floatBytes; - static_assert(sizeof axis->encoder_.pos_estimate_ == sizeof floatBytes); - std::memcpy(&floatBytes, &axis->encoder_.pos_estimate_, sizeof floatBytes); + + float pos_estimate = axis->encoder_.pos_estimate_.get_any().value_or(0.0f); + static_assert(sizeof pos_estimate == sizeof floatBytes); + std::memcpy(&floatBytes, &pos_estimate, sizeof floatBytes); txmsg.buf[0] = floatBytes; txmsg.buf[1] = floatBytes >> 8; txmsg.buf[2] = floatBytes >> 16; txmsg.buf[3] = floatBytes >> 24; - static_assert(sizeof floatBytes == sizeof axis->encoder_.vel_estimate_); - std::memcpy(&floatBytes, &axis->encoder_.vel_estimate_, sizeof floatBytes); + float vel_estimate = axis->encoder_.vel_estimate_.get_any().value_or(0.0f); + static_assert(sizeof floatBytes == sizeof vel_estimate); + std::memcpy(&floatBytes, &vel_estimate, sizeof floatBytes); txmsg.buf[4] = floatBytes; txmsg.buf[5] = floatBytes >> 8; txmsg.buf[6] = floatBytes >> 16; @@ -245,8 +248,9 @@ void CANSimple::get_sensorless_estimates_callback(Axis* axis, can_Message_t& msg txmsg.buf[2] = floatBytes >> 16; txmsg.buf[3] = floatBytes >> 24; - static_assert(sizeof floatBytes == sizeof axis->sensorless_estimator_.vel_estimate_); - std::memcpy(&floatBytes, &axis->sensorless_estimator_.vel_estimate_, sizeof floatBytes); + float vel_estimate = axis->sensorless_estimator_.vel_estimate_.get_any().value_or(0.0f); + static_assert(sizeof floatBytes == sizeof vel_estimate); + std::memcpy(&floatBytes, &vel_estimate, sizeof floatBytes); txmsg.buf[4] = floatBytes; txmsg.buf[5] = floatBytes >> 8; txmsg.buf[6] = floatBytes >> 16; @@ -328,17 +332,23 @@ void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) { txmsg.isExt = axis->config_.can_node_id_extended; txmsg.len = 8; + // TODO: read variable in a thread-safe way + std::optional Idq_setpoint = axis->motor_.current_control_.Idq_setpoint_; + if (!Idq_setpoint.has_value()) { + Idq_setpoint = {0.0f, 0.0f}; + } + uint32_t floatBytes; - static_assert(sizeof axis->motor_.current_control_.Iq_setpoint_ == sizeof floatBytes); - std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_setpoint_, sizeof floatBytes); + static_assert(sizeof Idq_setpoint->first == sizeof floatBytes); + std::memcpy(&floatBytes, &Idq_setpoint->first, sizeof floatBytes); txmsg.buf[0] = floatBytes; txmsg.buf[1] = floatBytes >> 8; txmsg.buf[2] = floatBytes >> 16; txmsg.buf[3] = floatBytes >> 24; - static_assert(sizeof floatBytes == sizeof axis->motor_.current_control_.Iq_measured_); - std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_measured_, sizeof floatBytes); + static_assert(sizeof Idq_setpoint->second == sizeof floatBytes); + std::memcpy(&floatBytes, &Idq_setpoint->second, sizeof floatBytes); txmsg.buf[4] = floatBytes; txmsg.buf[5] = floatBytes >> 8; txmsg.buf[6] = floatBytes >> 16; diff --git a/Firmware/fibre/cpp/interfaces_template.j2 b/Firmware/fibre/cpp/interfaces_template.j2 index cbf7e72f..d901327e 100644 --- a/Firmware/fibre/cpp/interfaces_template.j2 +++ b/Firmware/fibre/cpp/interfaces_template.j2 @@ -12,6 +12,8 @@ #ifndef __FIBRE_INTERFACES_HPP #define __FIBRE_INTERFACES_HPP +[[userdata.c_preamble]] + #include #pragma GCC push_options diff --git a/Firmware/fibre/tools/interface_generator.py b/Firmware/fibre/tools/interface_generator.py index 659d97f6..0b899607 100644 --- a/Firmware/fibre/tools/interface_generator.py +++ b/Firmware/fibre/tools/interface_generator.py @@ -86,6 +86,8 @@ properties: valuetypes: type: object additionalProperties: { "$ref": "#/definitions/valuetype" } + userdata: + type: object __line__: {type: object} __column__: {type: object} additionalProperties: false @@ -159,8 +161,8 @@ value_types = OrderedDict({ }) enums = OrderedDict() - interfaces = OrderedDict() +userdata = OrderedDict() # Arbitrary data passed from the definition file to the template def make_property_type(typeargs): value_type = resolve_valuetype('', typeargs['fibre.Property.type']) @@ -529,6 +531,7 @@ for definition_file in definition_files: raise Exception(err.message + '\nat ' + str(list(err.absolute_path))) interfaces.update(get_dict(file_content, 'interfaces')) value_types.update(get_dict(file_content, 'valuetypes')) + userdata.update(get_dict(file_content, 'userdata')) dictionary += file_content.get('dictionary', None) or [] @@ -660,6 +663,7 @@ template_args = { 'interfaces': interfaces, 'value_types': value_types, 'toplevel_interfaces': toplevel_interfaces, + 'userdata': userdata, 'endpoints': endpoints, 'embedded_endpoint_definitions': embedded_endpoint_definitions } diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 4d2f27ce..09800f16 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -5,6 +5,12 @@ summary: ODrive Interface Definitions dictionary: [ODrive] # Prevent the word 'ODrive' from being detected as two words 'O' and 'Drive' +userdata: + c_preamble: | + #include + using float2D = std::pair; + struct Iph_ABC_t { float phA; float phB; float phC; }; + interfaces: ODrive: c_is_class: True @@ -637,7 +643,6 @@ interfaces: TimerUpdateMissed: {doc: A timer update event was missed. Perhaps the previous timer update took too much time. This is not expected in official release firmware.} CurrentMeasurementUnavailable: {doc: The phase current measurement is not available. The ADC failed to sample the current sensor in time. This is not expected in official release firmware.} ControllerFailed: {doc: The motor was disarmed because the underlying controller failed. Usually this is the FOC controller.} - ILeakOutOfRange: {doc: '`i_leak` exceeded `config.max_leak_current`. This can happen if there is a short from a motor phase to DC- or DC+.'} IBusOutOfRange: doc: | The DC current sourced/sunk by this motor exceeded the configured @@ -649,19 +654,23 @@ interfaces: The motor had to be disarmed because of a system level error. See `ODrive.Error` for more details. BadTiming: {doc: The main control loop got out of sync with the motor control loop. This could indicate that the main control loop got stuck.} - UnknownPhase: {doc: The current controller did not get a valid angle input. Maybe you didn't calibrate the encoder.} - UnknownCurrent: {doc: The current controller did not get a valid current measurement or setpoint. Maybe you didn't configure the controller correctly or there is a low level system issue.} + UnknownPhaseEstimate: {doc: The current controller did not get a valid angle input. Maybe you didn't calibrate the encoder.} + UnknownPhaseVel: {doc: The motor controller did not get a valid phase velocity input.} + UnknownTorque: {doc: The motor controller did not get a valid torque input.} + UnknownCurrentCommand: {doc: The current controller did not get a valid current setpoint. Maybe you didn't configure the controller correctly.} + UnknownCurrentMeasurement: {doc: The current controller did not get a valid current measurement.} UnknownVbusVoltage: {doc: The current controller did not get a valid `vbus_voltage` measurement.} + UnknownVoltageCommand: {doc: The current controller did not get a valid feedforward voltage setpoint.} + UnknownGains: {doc: The current controller gains were not configured. Run motor calibration or set `config.phase_resistance` and `config.phase_inductance` manually.} ControllerInitializing: {doc: Internal value used while the controller is not yet ready to generate PWM timings.} is_armed: readonly bool is_calibrated: readonly bool - current_meas_phA: {type: readonly float32, c_name: current_meas_.phA} - current_meas_phB: {type: readonly float32, c_name: current_meas_.phB} - current_meas_phC: {type: readonly float32, c_name: current_meas_.phC} + current_meas_phA: {type: readonly float32, c_getter: 'current_meas_.value_or(Iph_ABC_t{0.0f, 0.0f, 0.0f}).phA'} + current_meas_phB: {type: readonly float32, c_getter: 'current_meas_.value_or(Iph_ABC_t{0.0f, 0.0f, 0.0f}).phB'} + current_meas_phC: {type: readonly float32, c_getter: 'current_meas_.value_or(Iph_ABC_t{0.0f, 0.0f, 0.0f}).phC'} DC_calib_phA: {type: float32, c_name: DC_calib_.phA} DC_calib_phB: {type: float32, c_name: DC_calib_.phB} DC_calib_phC: {type: float32, c_name: DC_calib_.phC} - I_leak: {type: readonly float32, unit: A} I_bus: {type: readonly float32, unit: A} phase_current_rev_gain: float32 effective_current_lim: readonly float32 @@ -676,17 +685,17 @@ interfaces: current_control: c_is_class: True attributes: - p_gain: float32 - i_gain: float32 + p_gain: {type: readonly float32, c_getter: 'pi_gains_.value_or(float2D{0.0f, 0.0f}).first'} + i_gain: {type: readonly float32, c_getter: 'pi_gains_.value_or(float2D{0.0f, 0.0f}).second'} I_measured_report_filter_k: float32 - Id_setpoint: readonly float32 - Iq_setpoint: readonly float32 - Vd_setpoint: readonly float32 - Vq_setpoint: readonly float32 - phase: readonly float32 - phase_vel: readonly float32 - Ialpha_measured: readonly float32 - Ibeta_measured: readonly float32 + Id_setpoint: {type: readonly float32, c_getter: 'Idq_setpoint_.value_or(float2D{0.0f, 0.0f}).first'} + Iq_setpoint: {type: readonly float32, c_getter: 'Idq_setpoint_.value_or(float2D{0.0f, 0.0f}).second'} + Vd_setpoint: {type: readonly float32, c_getter: 'Vdq_setpoint_.value_or(float2D{0.0f, 0.0f}).first'} + Vq_setpoint: {type: readonly float32, c_getter: 'Vdq_setpoint_.value_or(float2D{0.0f, 0.0f}).second'} + phase: {type: readonly float32, c_getter: 'phase_.value_or(0.0f)'} + phase_vel: {type: readonly float32, c_getter: 'phase_vel_.value_or(0.0f)'} + Ialpha_measured: {type: readonly float32, c_getter: 'Ialpha_beta_measured_.value_or(float2D{0.0f, 0.0f}).first'} + Ibeta_measured: {type: readonly float32, c_getter: 'Ialpha_beta_measured_.value_or(float2D{0.0f, 0.0f}).second'} Id_measured: readonly float32 Iq_measured: readonly float32 v_current_control_integral_d: float32 @@ -761,10 +770,25 @@ interfaces: c_is_class: True attributes: rotor_flux: {type: readonly float32, unit: A, doc: estimated magnitude of the rotor flux} - slip_vel: {type: readonly float32, unit: rad/s, doc: estimated slip between physical and electrical angular velocity} - phase_offset: {type: readonly float32, unit: rad, doc: estimate offset between physical and electrical angular position} - stator_phase_vel: {type: readonly float32, unit: rad/s, doc: calculated setpoint for the electrical velocity} - stator_phase: {type: readonly float32, unit: rad, doc: calculated setpoint for the electrical phase} + slip_vel: + type: readonly float32 + unit: rad/s + doc: estimated slip between physical and electrical angular velocity} + c_getter: slip_vel_.get_any().value_or(0.0f) + phase_offset: + type: readonly float32 + unit: rad + doc: estimate offset between physical and electrical angular position} + stator_phase_vel: + type: readonly float32 + unit: rad/s + doc: calculated setpoint for the electrical velocity} + c_getter: stator_phase_vel_.get_any().value_or(0.0f) + stator_phase: + type: readonly float32 + unit: rad + doc: calculated setpoint for the electrical phase} + c_getter: stator_phase_.get_any().value_or(0.0f) config: c_is_class: False attributes: @@ -920,13 +944,13 @@ interfaces: shadow_count: readonly int32 count_in_cpr: readonly int32 interpolation: readonly float32 - phase: readonly float32 - pos_estimate: readonly float32 + phase: {type: readonly float32, c_getter: phase_.get_any().value_or(0.0f)} + pos_estimate: {type: readonly float32, c_getter: pos_estimate_.get_any().value_or(0.0f)} pos_estimate_counts: readonly float32 pos_cpr_counts: readonly float32 - pos_circular: readonly float32 + pos_circular: {type: readonly float32, c_getter: pos_circular_.get_any().value_or(0.0f)} hall_state: readonly uint8 - vel_estimate: readonly float32 + vel_estimate: {type: readonly float32, c_getter: vel_estimate_.get_any().value_or(0.0f)} vel_estimate_counts: readonly float32 calib_scan_response: readonly float32 pos_abs: int32 @@ -967,10 +991,11 @@ interfaces: nullflag: None flags: UnstableGain: - phase: {type: float32, unit: rad} - pll_pos: {type: float32, unit: rad} - phase_vel: {type: float32, unit: rad/s} - vel_estimate: {type: float32, unit: turns/s} + UnknownCurrentMeasurement: + phase: {type: readonly float32, unit: rad, c_getter: phase_.get_any().value_or(0.0f)} + pll_pos: {type: readonly float32, unit: rad} + phase_vel: {type: readonly float32, unit: rad/s, c_getter: phase_vel_.get_any().value_or(0.0f)} + vel_estimate: {type: readonly float32, unit: turns/s, c_getter: vel_estimate_.get_any().value_or(0.0f)} # pll_kp: float32 # pll_ki: float32 config: diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index c2a6e329..bd5ddd8a 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -107,15 +107,19 @@ MOTOR_ERROR_MODULATION_IS_NAN = 0x00010000 MOTOR_ERROR_TIMER_UPDATE_MISSED = 0x00020000 MOTOR_ERROR_CURRENT_MEASUREMENT_UNAVAILABLE = 0x00040000 MOTOR_ERROR_CONTROLLER_FAILED = 0x00080000 -MOTOR_ERROR_I_LEAK_OUT_OF_RANGE = 0x00100000 -MOTOR_ERROR_I_BUS_OUT_OF_RANGE = 0x00200000 -MOTOR_ERROR_BRAKE_RESISTOR_DISARMED = 0x00400000 -MOTOR_ERROR_SYSTEM_LEVEL = 0x00800000 -MOTOR_ERROR_BAD_TIMING = 0x01000000 -MOTOR_ERROR_UNKNOWN_PHASE = 0x02000000 -MOTOR_ERROR_UNKNOWN_CURRENT = 0x04000000 -MOTOR_ERROR_UNKNOWN_VBUS_VOLTAGE = 0x08000000 -MOTOR_ERROR_CONTROLLER_INITIALIZING = 0x10000000 +MOTOR_ERROR_I_BUS_OUT_OF_RANGE = 0x00100000 +MOTOR_ERROR_BRAKE_RESISTOR_DISARMED = 0x00200000 +MOTOR_ERROR_SYSTEM_LEVEL = 0x00400000 +MOTOR_ERROR_BAD_TIMING = 0x00800000 +MOTOR_ERROR_UNKNOWN_PHASE_ESTIMATE = 0x01000000 +MOTOR_ERROR_UNKNOWN_PHASE_VEL = 0x02000000 +MOTOR_ERROR_UNKNOWN_TORQUE = 0x04000000 +MOTOR_ERROR_UNKNOWN_CURRENT_COMMAND = 0x08000000 +MOTOR_ERROR_UNKNOWN_CURRENT_MEASUREMENT = 0x10000000 +MOTOR_ERROR_UNKNOWN_VBUS_VOLTAGE = 0x20000000 +MOTOR_ERROR_UNKNOWN_VOLTAGE_COMMAND = 0x40000000 +MOTOR_ERROR_UNKNOWN_GAINS = 0x80000000 +MOTOR_ERROR_CONTROLLER_INITIALIZING = 0x100000000 # ODrive.Controller.Error CONTROLLER_ERROR_NONE = 0x00000000 @@ -141,3 +145,4 @@ ENCODER_ERROR_ABS_SPI_NOT_READY = 0x00000100 # ODrive.SensorlessEstimator.Error SENSORLESS_ESTIMATOR_ERROR_NONE = 0x00000000 SENSORLESS_ESTIMATOR_ERROR_UNSTABLE_GAIN = 0x00000001 +SENSORLESS_ESTIMATOR_ERROR_UNKNOWN_CURRENT_MEASUREMENT = 0x00000002 diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index 29cbc794..9d045650 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -48,7 +48,7 @@ class TestEncoderBase(): # encoder.count_in_cpr slope, offset, fitted_curve = fit_sawtooth(data[:,(0,2)], true_cpr if reverse else 0, 0 if reverse else true_cpr) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02 * noise) # encoder.pos_estimate slope, offset, fitted_curve = fit_line(data[:,(0,4)]) diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index ea093d7b..770cb1c7 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -622,7 +622,7 @@ def test_assert_no_error(axis_ctx: ODriveAxisComponent): any_error = (axis_ctx.handle.motor.error | axis_ctx.handle.encoder.error | axis_ctx.handle.sensorless_estimator.error | - axis_ctx.handle.error) != 0 + axis_ctx.handle.error) != 0 # TODO: this is not the complete list of components if any_error: lines = [] diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index e6208816..339099d5 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -104,6 +104,7 @@ def dump_errors(odrv, clear=False, printfunc = print): ('motor', axis, 'motor.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("MOTOR_ERROR_")}), ('fet_thermistor', axis, 'fet_thermistor.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("THERMISTOR_CURRENT_LIMITER_ERROR")}), ('motor_thermistor', axis, 'motor_thermistor.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("THERMISTOR_CURRENT_LIMITER_ERROR")}), + ('sensorless_estimator', axis, 'sensorless_estimator.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("SENSORLESS_ESTIMATOR_ERROR")}), ('encoder', axis, 'encoder.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("ENCODER_ERROR_")}), ('controller', axis, 'controller.error', {v: k for k, v in odrive.enums.__dict__ .items() if k.startswith("CONTROLLER_ERROR_")}), ]