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_")}), ]