diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 76420adf..d85489dd 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -3,9 +3,9 @@ #include #include "gpio.h" +#include "communication/interface_can.hpp" #include "odrive_main.h" #include "utils.h" -#include "communication/interface_can.hpp" Axis::Axis(int axis_num, const AxisHardwareConfig_t& hw_config, @@ -26,13 +26,12 @@ Axis::Axis(int axis_num, motor_(motor), trap_(trap), min_endstop_(min_endstop), - max_endstop_(max_endstop) -{ - encoder_.axis_ = this; + max_endstop_(max_endstop) { + encoder_.axis_ = this; sensorless_estimator_.axis_ = this; - controller_.axis_ = this; - motor_.axis_ = this; - trap_.axis_ = this; + controller_.axis_ = this; + motor_.axis_ = this; + trap_.axis_ = this; decode_step_dir_pins(); watchdog_feed(); min_endstop_.axis_ = this; @@ -41,29 +40,29 @@ Axis::Axis(int axis_num, Axis::LockinConfig_t Axis::default_calibration() { Axis::LockinConfig_t config; - config.current = 10.0f; // [A] - config.ramp_time = 0.4f; // [s] - config.ramp_distance = 1 * M_PI; // [rad] - config.accel = 20.0f; // [rad/s^2] - config.vel = 40.0f; // [rad/s] - config.finish_distance = 100.0f * 2.0f * M_PI; // [rad] - config.finish_on_vel = false; + config.current = 10.0f; // [A] + config.ramp_time = 0.4f; // [s] + config.ramp_distance = 1 * M_PI; // [rad] + config.accel = 20.0f; // [rad/s^2] + config.vel = 40.0f; // [rad/s] + config.finish_distance = 100.0f * 2.0f * M_PI; // [rad] + config.finish_on_vel = false; config.finish_on_distance = true; - config.finish_on_enc_idx = true; + config.finish_on_enc_idx = true; return config; } Axis::LockinConfig_t Axis::default_sensorless() { Axis::LockinConfig_t config; - config.current = 10.0f; // [A] - config.ramp_time = 0.4f; // [s] - config.ramp_distance = 1 * M_PI; // [rad] - config.accel = 200.0f; // [rad/s^2] - config.vel = 400.0f; // [rad/s] - config.finish_distance = 100.0f; // [rad] - config.finish_on_vel = true; + config.current = 10.0f; // [A] + config.ramp_time = 0.4f; // [s] + config.ramp_distance = 1 * M_PI; // [rad] + config.accel = 200.0f; // [rad/s^2] + config.vel = 400.0f; // [rad/s] + config.finish_distance = 100.0f; // [rad] + config.finish_on_vel = true; config.finish_on_distance = false; - config.finish_on_enc_idx = false; + config.finish_on_enc_idx = false; return config; } @@ -71,7 +70,6 @@ static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } - // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { @@ -87,7 +85,7 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512); - thread_id_ = osThreadCreate(osThread(thread_def), this); + thread_id_ = osThreadCreate(osThread(thread_def), this); thread_id_valid_ = true; } @@ -108,27 +106,27 @@ bool Axis::wait_for_current_meas() { void Axis::step_cb() { if (step_dir_active_) { GPIO_PinState dir_pin = HAL_GPIO_ReadPin(dir_port_, dir_pin_); - float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; controller_.input_pos_ += dir * config_.counts_per_step; controller_.input_pos_updated(); } }; void Axis::load_default_step_dir_pin_config( - const AxisHardwareConfig_t& hw_config, Config_t* config) { + const AxisHardwareConfig_t& hw_config, Config_t* config) { config->step_gpio_pin = hw_config.step_gpio_pin; - config->dir_gpio_pin = hw_config.dir_gpio_pin; + config->dir_gpio_pin = hw_config.dir_gpio_pin; } -void Axis::load_default_can_id(const int& id, Config_t& config){ +void Axis::load_default_can_id(const int& id, Config_t& config) { config.can_node_id = id; } void Axis::decode_step_dir_pins() { step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin); - step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin); - dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin); - dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin); + step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin); + dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin); + dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin); } // @brief (de)activates step/dir input @@ -136,7 +134,7 @@ void Axis::set_step_dir_active(bool active) { if (active) { // Set up the direction GPIO as input GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = dir_pin_; + GPIO_InitStruct.Pin = dir_pin_; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(dir_port_, &GPIO_InitStruct); @@ -174,9 +172,8 @@ bool Axis::do_checks() { } } - if(board_config.power_supply_wattage > 0.0f && - (Ibus_sum * vbus_voltage) > board_config.power_supply_wattage) - { + if (board_config.power_supply_wattage > 0.0f && + (Ibus_sum * vbus_voltage) > board_config.power_supply_wattage) { error_ |= ERROR_DC_BUS_OVER_POWER; } @@ -221,10 +218,10 @@ bool Axis::watchdog_check() { } } -bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { +bool Axis::run_lockin_spin(const LockinConfig_t& lockin_config) { // Spiral up current for softer rotor lock-in lockin_state_ = LOCKIN_STATE_RAMP; - float x = 0.0f; + float x = 0.0f; run_control_loop([&]() { float phase = wrap_pm_pi(lockin_config.ramp_distance * x); float I_mag = lockin_config.current * x; @@ -233,11 +230,11 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { return false; return x < 1.0f; }); - + // Spin states float distance = lockin_config.ramp_distance; - float phase = wrap_pm_pi(distance); - float vel = distance / lockin_config.ramp_time; + float phase = wrap_pm_pi(distance); + float vel = distance / lockin_config.ramp_time; // Function of states to check if we are done auto spin_done = [&](bool vel_override = false) -> bool { @@ -260,7 +257,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { if (!motor_.update(lockin_config.current, phase, vel)) return false; - return !spin_done(true); //vel_override to go to next phase + return !spin_done(true); //vel_override to go to next phase }); if (!encoder_.index_found_) @@ -269,7 +266,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { // Constant speed if (!spin_done()) { lockin_state_ = LOCKIN_STATE_CONST_VEL; - vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error + vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error run_control_loop([&]() { distance += vel * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); @@ -286,7 +283,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) { // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. bool Axis::run_sensorless_control_loop() { - run_control_loop([this](){ + run_control_loop([this]() { if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; @@ -295,7 +292,7 @@ bool Axis::run_sensorless_control_loop() { if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) - return false; // set_error should update axis.error_ + return false; // set_error should update axis.error_ return true; }); return check_for_errors(); @@ -305,14 +302,14 @@ bool Axis::run_closed_loop_control_loop() { // To avoid any transient on startup, we intialize the setpoint to be the current position controller_.pos_setpoint_ = encoder_.pos_estimate_; set_step_dir_active(config_.enable_step_dir); - run_control_loop([this](){ + run_control_loop([this]() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) - return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error - float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; + return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error + float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs; if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel)) - return false; // set_error should update axis.error_ + return false; // set_error should update axis.error_ // Handle the homing case if (homing_.homing_state == HOMING_STATE_HOMING) { @@ -325,21 +322,21 @@ bool Axis::run_closed_loop_control_loop() { encoder_.set_linear_count(min_endstop_.config_.offset); controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; - controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; + controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; controller_.input_pos_ = 0.0f; controller_.input_pos_updated(); - controller_.input_vel_ = 0.0f; + controller_.input_vel_ = 0.0f; controller_.input_current_ = 0.0f; homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO; } } else if (homing_.homing_state == HOMING_STATE_MOVE_TO_ZERO) { - if(!min_endstop_.getEndstopState() && controller_.trajectory_done_){ + if (!min_endstop_.getEndstopState() && controller_.trajectory_done_) { controller_.config_.control_mode = homing_.storedControlMode; - controller_.config_.input_mode = homing_.storedInputMode; - homing_.homing_state = HOMING_STATE_IDLE; - homing_.isHomed = true; + controller_.config_.input_mode = homing_.storedInputMode; + homing_.homing_state = HOMING_STATE_IDLE; + homing_.isHomed = true; } } else { // Check for endstop presses @@ -367,7 +364,6 @@ bool Axis::run_idle_loop() { // Infinite loop that does calibration and enters main control loop as appropriate void Axis::run_state_machine_loop() { - // arm! motor_.arm(); @@ -382,12 +378,11 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; if (config_.startup_encoder_offset_calibration) task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; - if (config_.startup_closed_loop_control){ - if(config_.startup_homing) + if (config_.startup_closed_loop_control) { + if (config_.startup_homing) task_chain_[pos++] = AXIS_STATE_HOMING; task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; - } - else if (config_.startup_sensorless_control) + } else if (config_.startup_sensorless_control) task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { @@ -401,7 +396,7 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_IDLE; } task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking - requested_state_ = AXIS_STATE_UNDEFINED; + requested_state_ = AXIS_STATE_UNDEFINED; // Auto-clear any invalid state error error_ &= ~ERROR_INVALID_STATE; } @@ -419,7 +414,7 @@ void Axis::run_state_machine_loop() { case AXIS_STATE_ENCODER_INDEX_SEARCH: { if (!motor_.is_calibrated_) goto invalid_state_label; - if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0) + if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction == 0) goto invalid_state_label; status = encoder_.run_index_search(); @@ -443,25 +438,25 @@ void Axis::run_state_machine_loop() { } break; case AXIS_STATE_LOCKIN_SPIN: { - if (!motor_.is_calibrated_ || motor_.config_.direction==0) + if (!motor_.is_calibrated_ || motor_.config_.direction == 0) goto invalid_state_label; status = run_lockin_spin(config_.lockin); } break; case AXIS_STATE_SENSORLESS_CONTROL: { - if (!motor_.is_calibrated_ || motor_.config_.direction==0) - goto invalid_state_label; - status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired + if (!motor_.is_calibrated_ || motor_.config_.direction == 0) + goto invalid_state_label; + status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired if (status) { // call to controller.reset() that happend when arming means that vel_setpoint // is zeroed. So we make the setpoint the spinup target for smooth transition. controller_.vel_setpoint_ = config_.sensorless_ramp.vel; - status = run_sensorless_control_loop(); + status = run_sensorless_control_loop(); } } break; case AXIS_STATE_CLOSED_LOOP_CONTROL: { - if (!motor_.is_calibrated_ || motor_.config_.direction==0) + if (!motor_.is_calibrated_ || motor_.config_.direction == 0) goto invalid_state_label; if (!encoder_.is_ready_) goto invalid_state_label; @@ -470,7 +465,7 @@ void Axis::run_state_machine_loop() { case AXIS_STATE_IDLE: { run_idle_loop(); - status = motor_.arm(); // done with idling - try to arm the motor + status = motor_.arm(); // done with idling - try to arm the motor } break; default: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 060e3005..af305a43 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,7 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif - enum HomingState_t { HOMING_STATE_IDLE, HOMING_STATE_HOMING, @@ -13,52 +12,52 @@ enum HomingState_t { }; class Axis { -public: + public: enum Error_t { - ERROR_NONE = 0x00, - ERROR_INVALID_STATE = 0x01, // + template void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { // look for errors at axis level and also all subcomponents bool checks_ok = do_checks(); // Update all estimators // Note: updates run even if checks fail - bool updates_ok = do_updates(); + bool updates_ok = do_updates(); - // make sure the watchdog is being fed. + // make sure the watchdog is being fed. bool watchdog_ok = watchdog_check(); - + if (!checks_ok || !updates_ok || !watchdog_ok) { // It's not useful to quit idle since that is the safe action // Also leaving idle would rearm the motors @@ -216,7 +215,7 @@ public: } } - bool run_lockin_spin(const LockinConfig_t &lockin_config); + bool run_lockin_spin(const LockinConfig_t& lockin_config); bool run_sensorless_control_loop(); bool run_closed_loop_control_loop(); bool run_idle_loop(); @@ -243,8 +242,8 @@ public: volatile bool thread_id_valid_ = false; // variables exposed on protocol - Error_t error_ = ERROR_NONE; - bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir + Error_t error_ = ERROR_NONE; + bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir // updated from config in constructor, and on protocol hook GPIO_TypeDef* step_port_; @@ -252,16 +251,16 @@ public: GPIO_TypeDef* dir_port_; uint16_t dir_pin_; - State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; - State_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; - State_t& current_state_ = task_chain_[0]; - uint32_t loop_counter_ = 0; + State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; + State_t task_chain_[10] = {AXIS_STATE_UNDEFINED}; + State_t& current_state_ = task_chain_[0]; + uint32_t loop_counter_ = 0; LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE; Homing_t homing_; uint32_t last_heartbeat_ = 0; // watchdog - uint32_t watchdog_current_value_= 0; + uint32_t watchdog_current_value_ = 0; // Communication protocol definitions auto make_protocol_definitions() { @@ -275,51 +274,47 @@ public: make_protocol_ro_property("homing_state", &homing_.homing_state), make_protocol_property("is_homed", &homing_.isHomed), make_protocol_object("config", - make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), - make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search), - make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration), - make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control), - make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control), - make_protocol_property("startup_homing", &config_.startup_homing), - make_protocol_property("enable_step_dir", &config_.enable_step_dir), - make_protocol_property("counts_per_step", &config_.counts_per_step), - make_protocol_property("watchdog_timeout", &config_.watchdog_timeout), - make_protocol_property("step_gpio_pin", &config_.step_gpio_pin, - [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), - make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, - [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), - make_protocol_object("calibration_lockin", - make_protocol_property("current", &config_.calibration_lockin.current), - make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time), - make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance), - make_protocol_property("accel", &config_.calibration_lockin.accel), - make_protocol_property("vel", &config_.calibration_lockin.vel) - ), - make_protocol_object("sensorless_ramp", - make_protocol_property("current", &config_.sensorless_ramp.current), - make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time), - make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance), - make_protocol_property("accel", &config_.sensorless_ramp.accel), - make_protocol_property("vel", &config_.sensorless_ramp.vel), - make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance), - make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel), - make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance), - make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx) - ), - make_protocol_object("general_lockin", - make_protocol_property("current", &config_.lockin.current), - make_protocol_property("ramp_time", &config_.lockin.ramp_time), - make_protocol_property("ramp_distance", &config_.lockin.ramp_distance), - make_protocol_property("accel", &config_.lockin.accel), - make_protocol_property("vel", &config_.lockin.vel), - make_protocol_property("finish_distance", &config_.lockin.finish_distance), - make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel), - make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance), - make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx) - ), - make_protocol_property("can_node_id", &config_.can_node_id), - make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms) - ), + make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), + make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search), + make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration), + make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control), + make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control), + make_protocol_property("startup_homing", &config_.startup_homing), + make_protocol_property("enable_step_dir", &config_.enable_step_dir), + make_protocol_property("counts_per_step", &config_.counts_per_step), + make_protocol_property("watchdog_timeout", &config_.watchdog_timeout), + make_protocol_property("step_gpio_pin", &config_.step_gpio_pin, + [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), + make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, + [](void* ctx) { static_cast(ctx)->decode_step_dir_pins(); }, this), + make_protocol_object("calibration_lockin", + make_protocol_property("current", &config_.calibration_lockin.current), + make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time), + make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance), + make_protocol_property("accel", &config_.calibration_lockin.accel), + make_protocol_property("vel", &config_.calibration_lockin.vel)), + make_protocol_object("sensorless_ramp", + make_protocol_property("current", &config_.sensorless_ramp.current), + make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time), + make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance), + make_protocol_property("accel", &config_.sensorless_ramp.accel), + make_protocol_property("vel", &config_.sensorless_ramp.vel), + make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance), + make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel), + make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance), + make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx)), + make_protocol_object("general_lockin", + make_protocol_property("current", &config_.lockin.current), + make_protocol_property("ramp_time", &config_.lockin.ramp_time), + make_protocol_property("ramp_distance", &config_.lockin.ramp_distance), + make_protocol_property("accel", &config_.lockin.accel), + make_protocol_property("vel", &config_.lockin.vel), + make_protocol_property("finish_distance", &config_.lockin.finish_distance), + make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel), + make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance), + make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)), + make_protocol_property("can_node_id", &config_.can_node_id), + make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)), make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), @@ -328,12 +323,10 @@ public: make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()), make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()), make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed), - make_protocol_function("clear_errors", *this, &Axis::clear_errors) - ); + make_protocol_function("clear_errors", *this, &Axis::clear_errors)); } }; - DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t) #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 5016f7dc..a4d1355b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -3,17 +3,15 @@ #include -Controller::Controller(Config_t& config) : - config_(config) -{ +Controller::Controller(Config_t& config) : config_(config) { update_filter_gains(); } void Controller::reset() { - pos_setpoint_ = 0.0f; - vel_setpoint_ = 0.0f; + pos_setpoint_ = 0.0f; + vel_setpoint_ = 0.0f; vel_integrator_current_ = 0.0f; - current_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; } void Controller::set_error(Error_t error) { @@ -35,13 +33,13 @@ void Controller::move_to_pos(float goal_point) { axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); traj_start_loop_count_ = axis_->loop_counter_; - trajectory_done_ = false; + trajectory_done_ = false; } -void Controller::move_incremental(float displacement, bool from_input_pos = true){ - if(from_input_pos){ +void Controller::move_incremental(float displacement, bool from_input_pos = true) { + if (from_input_pos) { input_pos_ += displacement; - } else{ + } else { input_pos_ = pos_setpoint_ + displacement; } @@ -62,17 +60,17 @@ void Controller::start_anticogging_calibration() { bool Controller::home_axis() { if (axis_->min_endstop_.config_.enabled) { axis_->homing_.storedControlMode = config_.control_mode; - axis_->homing_.storedInputMode = config_.input_mode; + axis_->homing_.storedInputMode = config_.input_mode; config_.control_mode = CTRL_MODE_VELOCITY_CONTROL; - config_.input_mode = INPUT_MODE_VEL_RAMP; + config_.input_mode = INPUT_MODE_VEL_RAMP; input_pos_ = 0.0f; input_pos_updated(); - input_vel_ = -config_.homing_speed; + input_vel_ = -config_.homing_speed; input_current_ = 0.0f; - axis_->homing_.isHomed = false; + axis_->homing_.isHomed = false; axis_->homing_.homing_state = HOMING_STATE_HOMING; } else { return false; @@ -96,19 +94,19 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } if (config_.anticogging.index < 3600) { config_.control_mode = CTRL_MODE_POSITION_CONTROL; - input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio(); - input_vel_ = 0.0f; - input_current_ = 0.0f; + input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio(); + input_vel_ = 0.0f; + input_current_ = 0.0f; input_pos_updated(); return false; } else { config_.anticogging.index = 0; - config_.control_mode = CTRL_MODE_POSITION_CONTROL; - input_pos_ = 0.0f; // Send the motor home - input_vel_ = 0.0f; - input_current_ = 0.0f; + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + input_pos_ = 0.0f; // Send the motor home + input_vel_ = 0.0f; + input_current_ = 0.0f; input_pos_updated(); - anticogging_valid_ = true; + anticogging_valid_ = true; config_.anticogging.calib_anticogging = false; return true; } @@ -117,8 +115,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } void Controller::update_filter_gains() { - input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time - input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped + input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time + input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } namespace { @@ -140,32 +138,32 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // do nothing } break; case INPUT_MODE_PASSTHROUGH: { - pos_setpoint_ = input_pos_; - vel_setpoint_ = input_vel_; + pos_setpoint_ = input_pos_; + vel_setpoint_ = input_vel_; current_setpoint_ = input_current_; } break; case INPUT_MODE_VEL_RAMP: { float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate); - float full_step = input_vel_ - vel_setpoint_; - float step = std::clamp(full_step, -max_step_size, max_step_size); + float full_step = input_vel_ - vel_setpoint_; + float step = std::clamp(full_step, -max_step_size, max_step_size); vel_setpoint_ += step; current_setpoint_ = step / current_meas_period * config_.inertia; } break; case INPUT_MODE_POS_FILTER: { // 2nd order pos tracking filter - float delta_pos = input_pos_ - pos_setpoint_; // Pos error - float delta_vel = input_vel_ - vel_setpoint_; // Vel error - float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback - current_setpoint_ = accel * config_.inertia; // Accel - vel_setpoint_ += current_meas_period * accel; // delta vel - pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos + float delta_pos = input_pos_ - pos_setpoint_; // Pos error + float delta_vel = input_vel_ - vel_setpoint_; // Vel error + float accel = input_filter_kp_ * delta_pos + input_filter_ki_ * delta_vel; // Feedback + current_setpoint_ = accel * config_.inertia; // Accel + vel_setpoint_ += current_meas_period * accel; // delta vel + pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; // case INPUT_MODE_MIX_CHANNELS: { // // NOT YET IMPLEMENTED // } break; case INPUT_MODE_TRAP_TRAJ: { - if(input_pos_updated_){ + if (input_pos_updated_) { move_to_pos(input_pos_); input_pos_updated_ = false; } @@ -178,29 +176,28 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s if (t > axis_->trap_.Tf_) { // Drop into position control mode when done to avoid problems on loop counter delta overflow config_.control_mode = CTRL_MODE_POSITION_CONTROL; - pos_setpoint_ = input_pos_; - vel_setpoint_ = 0.0f; - current_setpoint_ = 0.0f; - trajectory_done_ = true; + pos_setpoint_ = input_pos_; + vel_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; + trajectory_done_ = true; } else { TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); - pos_setpoint_ = traj_step.Y; - vel_setpoint_ = traj_step.Yd; - current_setpoint_ = traj_step.Ydd * config_.inertia; + pos_setpoint_ = traj_step.Y; + vel_setpoint_ = traj_step.Yd; + current_setpoint_ = traj_step.Ydd * config_.inertia; } - anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate + anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } break; default: { set_error(ERROR_INVALID_INPUT_MODE); return false; } - } // Position control // TODO Decide if we want to use encoder or pll position here float gain_scheduling_multiplier = 1.0f; - float vel_des = vel_setpoint_; + float vel_des = vel_setpoint_; if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { float pos_err; if (config_.setpoints_in_cpr) { @@ -216,7 +213,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s pos_err = pos_setpoint_ - pos_estimate; } vel_des += config_.pos_gain * pos_err; - // V-shaped gain shedule based on position error + // V-shaped gain shedule based on position error float abs_pos_err = fabsf(pos_err); if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) { gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width; @@ -261,14 +258,14 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // Current limiting bool limited = false; - float Ilim = axis_->motor_.effective_current_lim(); + float Ilim = axis_->motor_.effective_current_lim(); if (Iq > Ilim) { limited = true; - Iq = Ilim; + Iq = Ilim; } if (Iq < -Ilim) { limited = true; - Iq = -Ilim; + Iq = -Ilim; } // Velocity integrator (behaviour dependent on limiting) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index d3cee06a..502b097a 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -6,24 +6,24 @@ #endif class Controller { -public: + public: enum Error_t { - ERROR_NONE = 0, - ERROR_OVERSPEED = 0x01, + ERROR_NONE = 0, + ERROR_OVERSPEED = 0x01, ERROR_INVALID_INPUT_MODE = 0x02, - ERROR_UNSTABLE_GAIN = 0x04, + ERROR_UNSTABLE_GAIN = 0x04, }; // Note: these should be sorted from lowest level of control to // highest level of control, to allow "<" style comparisons. - enum ControlMode_t{ - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, + enum ControlMode_t { + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, CTRL_MODE_VELOCITY_CONTROL = 2, CTRL_MODE_POSITION_CONTROL = 3 }; - enum InputMode_t{ + enum InputMode_t { INPUT_MODE_INACTIVE, INPUT_MODE_PASSTHROUGH, INPUT_MODE_VEL_RAMP, @@ -35,27 +35,27 @@ public: typedef struct { uint32_t index = 0; float cogging_map[3600]; - bool pre_calibrated = false; - bool calib_anticogging = false; + bool pre_calibrated = false; + bool calib_anticogging = false; float calib_pos_threshold = 1.0f; float calib_vel_threshold = 1.0f; - float cogging_ratio = 1.0f; + float cogging_ratio = 1.0f; } Anticogging_t; struct Config_t { ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t - InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t - float pos_gain = 20.0f; // [(counts/s) / counts] - float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] - float vel_limit = 20000.0f; // [counts/s] - float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable - float vel_ramp_rate = 10000.0f; // [(counts/s) / s] - bool setpoints_in_cpr = false; - float inertia = 0.0f; // [A/(count/s^2)] - float input_filter_bandwidth = 2.0f; // [1/s] - float homing_speed = 2000.0f; // [counts/s] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] + float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable + float vel_ramp_rate = 10000.0f; // [(counts/s) / s] + bool setpoints_in_cpr = false; + float inertia = 0.0f; // [A/(count/s^2)] + float input_filter_bandwidth = 2.0f; // [1/s] + float homing_speed = 2000.0f; // [counts/s] Anticogging_t anticogging; float gain_scheduling_width = 10.0f; bool enable_gain_scheduling = false; @@ -72,7 +72,7 @@ public: void move_incremental(float displacement, bool from_goal_point); bool home_axis(); - + // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); bool anticogging_calibration(float pos_estimate, float vel_estimate); @@ -81,7 +81,7 @@ public: bool update(float pos_estimate, float vel_estimate, float* current_setpoint); Config_t& config_; - Axis* axis_ = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: // - expose selected (all?) variables on protocol @@ -95,18 +95,18 @@ public: float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; float vel_integrator_current_ = 0.0f; // [A] - float current_setpoint_ = 0.0f; // [A] + float current_setpoint_ = 0.0f; // [A] - float input_pos_ = 0.0f; - float input_vel_ = 0.0f; - float input_current_ = 0.0f; + float input_pos_ = 0.0f; + float input_vel_ = 0.0f; + float input_current_ = 0.0f; float input_filter_kp_ = 0.0f; float input_filter_ki_ = 0.0f; bool input_pos_updated_ = false; - + uint32_t traj_start_loop_count_ = 0; - bool trajectory_done_ = true; + bool trajectory_done_ = true; bool anticogging_valid_ = false; @@ -115,7 +115,7 @@ public: return make_protocol_member_list( make_protocol_property("error", &error_), make_protocol_property("input_pos", &input_pos_, - [](void* ctx) { static_cast(ctx)->input_pos_updated(); }, this), + [](void* ctx) { static_cast(ctx)->input_pos_updated(); }, this), make_protocol_property("input_vel", &input_vel_), make_protocol_property("input_current", &input_current_), make_protocol_ro_property("pos_setpoint", &pos_setpoint_), @@ -127,34 +127,31 @@ public: make_protocol_property("gain_scheduling_width", &config_.gain_scheduling_width), make_protocol_property("enable_gain_scheduling", &config_.enable_gain_scheduling), make_protocol_object("config", - make_protocol_property("control_mode", &config_.control_mode), - make_protocol_property("input_mode", &config_.input_mode), - make_protocol_property("pos_gain", &config_.pos_gain), - make_protocol_property("vel_gain", &config_.vel_gain), - make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), - make_protocol_property("vel_limit", &config_.vel_limit), - make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance), - make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate), - make_protocol_property("homing_speed", &config_.homing_speed), - make_protocol_property("inertia", &config_.inertia), - make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth, - [](void* ctx) { static_cast(ctx)->update_filter_gains(); }, this), - make_protocol_object("anticogging", - make_protocol_ro_property("index", &config_.anticogging.index), - make_protocol_property("pre_calibrated", &config_.anticogging.pre_calibrated), - make_protocol_ro_property("calib_anticogging", &config_.anticogging.calib_anticogging), - make_protocol_property("calib_pos_threshold", &config_.anticogging.calib_pos_threshold), - make_protocol_property("calib_vel_threshold", &config_.anticogging.calib_vel_threshold), - make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio) - ) - ), + make_protocol_property("control_mode", &config_.control_mode), + make_protocol_property("input_mode", &config_.input_mode), + make_protocol_property("pos_gain", &config_.pos_gain), + make_protocol_property("vel_gain", &config_.vel_gain), + make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance), + make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate), + make_protocol_property("homing_speed", &config_.homing_speed), + make_protocol_property("inertia", &config_.inertia), + make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth, + [](void* ctx) { static_cast(ctx)->update_filter_gains(); }, this), + make_protocol_object("anticogging", + make_protocol_ro_property("index", &config_.anticogging.index), + make_protocol_property("pre_calibrated", &config_.anticogging.pre_calibrated), + make_protocol_ro_property("calib_anticogging", &config_.anticogging.calib_anticogging), + make_protocol_property("calib_pos_threshold", &config_.anticogging.calib_pos_threshold), + make_protocol_property("calib_vel_threshold", &config_.anticogging.calib_vel_threshold), + make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio))), make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), - make_protocol_function("home_axis", *this, &Controller::home_axis) - ); + make_protocol_function("home_axis", *this, &Controller::home_axis)); } }; DEFINE_ENUM_FLAG_OPERATORS(Controller::Error_t) -#endif // __CONTROLLER_HPP +#endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b21c8cef..fc2eac2a 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -1,12 +1,9 @@ #include "odrive_main.h" - Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config) : - hw_config_(hw_config), - config_(config) -{ + Config_t& config) : hw_config_(hw_config), + config_(config) { update_pll_gains(); if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) { @@ -22,7 +19,7 @@ void Encoder::setup() { HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL); set_idx_subscribe(); - if(config_.mode & MODE_FLAG_ABS){ + if (config_.mode & MODE_FLAG_ABS) { abs_spi_cs_pin_init(); abs_spi_init(); } @@ -33,7 +30,7 @@ void Encoder::set_error(Error_t error) { axis_->error_ |= Axis::ERROR_ENCODER_FAILED; } -bool Encoder::do_checks(){ +bool Encoder::do_checks() { return error_ == ERROR_NONE; } @@ -48,10 +45,10 @@ void Encoder::enc_index_cb() { if (config_.use_index) { set_circular_count(0, false); if (config_.zero_count_on_find_idx) - set_linear_count(0); // Avoid position control transient after search + set_linear_count(0); // Avoid position control transient after search if (config_.pre_calibrated) { is_ready_ = true; - if(axis_->controller_.config_.anticogging.pre_calibrated){ + if (axis_->controller_.config_.anticogging.pre_calibrated) { axis_->controller_.anticogging_valid_ = true; } } else { @@ -70,15 +67,15 @@ void Encoder::enc_index_cb() { void Encoder::set_idx_subscribe(bool override_enable) { if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) { GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN, - enc_index_cb_wrapper, this); + enc_index_cb_wrapper, this); } else if (!config_.use_index || config_.find_idx_on_lockin_only) { GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin); } } void Encoder::update_pll_gains() { - pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time - pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped + pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { @@ -99,8 +96,8 @@ void Encoder::set_linear_count(int32_t count) { uint32_t prim = cpu_enter_critical(); // Update states - shadow_count_ = count; - pos_estimate_ = (float)count; + shadow_count_ = count; + pos_estimate_ = (float)count; tim_cnt_sample_ = count; //Write hardware last @@ -122,14 +119,14 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { // Update states count_in_cpr_ = mod(count, config_.cpr); - pos_cpr_ = (float)count_in_cpr_; + pos_cpr_ = (float)count_in_cpr_; cpu_exit_critical(prim); } bool Encoder::run_index_search() { config_.use_index = true; - index_found_ = false; + index_found_ = false; if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) { axis_->motor_.config_.direction = 1; } @@ -140,11 +137,11 @@ bool Encoder::run_index_search() { } bool Encoder::run_direction_find() { - int32_t init_enc_val = shadow_count_; - bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance; + int32_t init_enc_val = shadow_count_; + bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance; axis_->config_.calibration_lockin.finish_on_distance = true; - axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic - bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin); + axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic + bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin); axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance; if (status) { @@ -169,7 +166,7 @@ bool Encoder::run_direction_find() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz); // Require index found if enabled if (config_.use_index && !index_found_) { @@ -191,9 +188,9 @@ bool Encoder::run_offset_calibration() { // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; - axis_->run_control_loop([&](){ + axis_->run_control_loop([&]() { if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) - return false; // error set inside enqueue_voltage_timings + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); return ++i < start_lock_duration * current_meas_hz; }); @@ -201,20 +198,20 @@ bool Encoder::run_offset_calibration() { return false; int32_t init_enc_val = shadow_count_; - int64_t encvaluesum = 0; + int64_t encvaluesum = 0; // scan forward i = 0; - axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f); + axis_->run_control_loop([&]() { + float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); - float v_beta = voltage_magnitude * our_arm_sin_f32(phase); + float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) - return false; // error set inside enqueue_voltage_timings + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); encvaluesum += shadow_count_; - + return ++i < num_steps; }); if (axis_->error_ != Axis::ERROR_NONE) @@ -235,35 +232,34 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time // Check CPR - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; - calib_scan_response_ = fabsf(shadow_count_-init_enc_val); - if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) - { + calib_scan_response_ = fabsf(shadow_count_ - init_enc_val); + if (fabsf(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) { set_error(ERROR_CPR_OUT_OF_RANGE); return false; } // scan backwards i = 0; - axis_->run_control_loop([&](){ - float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f); + axis_->run_control_loop([&]() { + float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); - float v_beta = voltage_magnitude * our_arm_sin_f32(phase); + float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) - return false; // error set inside enqueue_voltage_timings + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); encvaluesum += shadow_count_; - + return ++i < num_steps; }); if (axis_->error_ != Axis::ERROR_NONE) return false; - config_.offset = encvaluesum / (num_steps * 2); - int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); - config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase + config_.offset = encvaluesum / (num_steps * 2); + int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); + config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase is_ready_ = true; return true; @@ -271,13 +267,26 @@ bool Encoder::run_offset_calibration() { static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { switch (hall_state) { - case 0b001: *hall_cnt = 0; return true; - case 0b011: *hall_cnt = 1; return true; - case 0b010: *hall_cnt = 2; return true; - case 0b110: *hall_cnt = 3; return true; - case 0b100: *hall_cnt = 4; return true; - case 0b101: *hall_cnt = 5; return true; - default: return false; + case 0b001: + *hall_cnt = 0; + return true; + case 0b011: + *hall_cnt = 1; + return true; + case 0b010: + *hall_cnt = 2; + return true; + case 0b110: + *hall_cnt = 3; + return true; + case 0b100: + *hall_cnt = 4; + return true; + case 0b101: + *hall_cnt = 5; + return true; + default: + return false; } } @@ -297,37 +306,36 @@ void Encoder::sample_now() { } break; case MODE_SPI_ABS_AMS: - case MODE_SPI_ABS_CUI: - { + case MODE_SPI_ABS_CUI: { // Do nothing } break; default: { - set_error(ERROR_UNSUPPORTED_ENCODER_MODE); + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); } break; } } -bool Encoder::abs_spi_init(){ +bool Encoder::abs_spi_init() { if ((config_.mode & MODE_FLAG_ABS) == 0x0) return false; - uint32_t cr1,cr2; + uint32_t cr1, cr2; cr1 = hw_config_.spi->Instance->CR1; cr2 = hw_config_.spi->Instance->CR2; - SPI_HandleTypeDef * spi = hw_config_.spi; - spi->Init.Mode = SPI_MODE_MASTER; - spi->Init.Direction = SPI_DIRECTION_2LINES; - spi->Init.DataSize = SPI_DATASIZE_16BIT; - spi->Init.CLKPolarity = SPI_POLARITY_LOW; - spi->Init.CLKPhase = SPI_PHASE_2EDGE; - spi->Init.NSS = SPI_NSS_SOFT; + SPI_HandleTypeDef* spi = hw_config_.spi; + spi->Init.Mode = SPI_MODE_MASTER; + spi->Init.Direction = SPI_DIRECTION_2LINES; + spi->Init.DataSize = SPI_DATASIZE_16BIT; + spi->Init.CLKPolarity = SPI_POLARITY_LOW; + spi->Init.CLKPhase = SPI_PHASE_2EDGE; + spi->Init.NSS = SPI_NSS_SOFT; spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; - spi->Init.FirstBit = SPI_FIRSTBIT_MSB; - spi->Init.TIMode = SPI_TIMODE_DISABLE; - spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - spi->Init.CRCPolynomial = 10; + spi->Init.FirstBit = SPI_FIRSTBIT_MSB; + spi->Init.TIMode = SPI_TIMODE_DISABLE; + spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + spi->Init.CRCPolynomial = 10; HAL_SPI_DeInit(spi); HAL_SPI_Init(spi); @@ -340,9 +348,9 @@ bool Encoder::abs_spi_init(){ return true; } -bool Encoder::abs_spi_start_transaction(){ - if (config_.mode & MODE_FLAG_ABS){ - if(hw_config_.spi->State != HAL_SPI_STATE_READY){ +bool Encoder::abs_spi_start_transaction() { + if (config_.mode & MODE_FLAG_ABS) { + if (hw_config_.spi->State != HAL_SPI_STATE_READY) { set_error(ERROR_ABS_SPI_NOT_READY); return false; } @@ -350,54 +358,54 @@ bool Encoder::abs_spi_start_transaction(){ hw_config_.spi->Instance->CR1 = abs_spi_cr1; hw_config_.spi->Instance->CR2 = abs_spi_cr2; HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET); - HAL_SPI_TransmitReceive_DMA(hw_config_.spi,(uint8_t*)abs_spi_dma_tx_,(uint8_t*)abs_spi_dma_rx_,1); + HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1); } return true; } -uint8_t parity(uint16_t v){ +uint8_t parity(uint16_t v) { v ^= v >> 8; v ^= v >> 4; v ^= v >> 2; v ^= v >> 1; return v & 1; } -void Encoder::abs_spi_cb(){ +void Encoder::abs_spi_cb() { HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET); switch (config_.mode) { case MODE_SPI_ABS_AMS: { - uint8_t parity_calc, parity_bit; - parity_calc = parity(abs_spi_dma_rx_[0]&0x7FFF); - parity_bit = abs_spi_dma_rx_[0] >>15; + uint8_t parity_calc, parity_bit; + parity_calc = parity(abs_spi_dma_rx_[0] & 0x7FFF); + parity_bit = abs_spi_dma_rx_[0] >> 15; - if(parity_calc == parity_bit){ - pos_abs_ = abs_spi_dma_rx_[0] & 0x3FFF; - // We are going to ignore values all high or low - // This might happen in normal operation, but its unlikely - // The filter will handle these cases - if(pos_abs_ != 0 && pos_abs_ != 0x3FFF) - abs_spi_pos_updated_ = true; - } - }break; + if (parity_calc == parity_bit) { + pos_abs_ = abs_spi_dma_rx_[0] & 0x3FFF; + // We are going to ignore values all high or low + // This might happen in normal operation, but its unlikely + // The filter will handle these cases + if (pos_abs_ != 0 && pos_abs_ != 0x3FFF) + abs_spi_pos_updated_ = true; + } + } break; default: { - set_error(ERROR_UNSUPPORTED_ENCODER_MODE); + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); } break; } is_ready_ = true; } -void Encoder::abs_spi_cs_pin_init(){ +void Encoder::abs_spi_cs_pin_init() { // Decode cs pin abs_spi_cs_port_ = get_gpio_port_by_pin(config_.abs_spi_cs_gpio_pin); - abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin); + abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin); // Init cs pin HAL_GPIO_DeInit(abs_spi_cs_port_, abs_spi_cs_pin_); GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = abs_spi_cs_pin_; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Pin = abs_spi_cs_pin_; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(abs_spi_cs_port_, &GPIO_InitStruct); @@ -414,7 +422,7 @@ bool Encoder::update() { //TODO: use count_in_cpr_ instead as shadow_count_ can overflow //or use 64 bit int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_; - delta_enc = (int32_t)delta_enc_16; //sign extend + delta_enc = (int32_t)delta_enc_16; //sign extend } break; case MODE_HALL: { @@ -433,41 +441,40 @@ bool Encoder::update() { } break; case MODE_SINCOS: { - float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_); + float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_); int fake_count = (int)(1000.0f * phase); //CPR = 6283 = 2pi * 1k delta_enc = fake_count - count_in_cpr_; delta_enc = mod(delta_enc, 6283); - if (delta_enc > 6283/2) + if (delta_enc > 6283 / 2) delta_enc -= 6283; } break; - + case MODE_SPI_ABS_AMS: - case MODE_SPI_ABS_CUI:{ - if(abs_spi_pos_updated_ == false && abs_spi_pos_init_once_){ + case MODE_SPI_ABS_CUI: { + if (abs_spi_pos_updated_ == false && abs_spi_pos_init_once_) { // Low pass filter the error spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_); // if (spi_error_rate_ > 0.005f) // set_error(ERROR_ABS_SPI_COM_FAIL); - } - else + } else // Low pass filter the error spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_); abs_spi_pos_updated_ = false; - delta_enc = pos_abs_ - count_in_cpr_; - delta_enc = mod(delta_enc, config_.cpr); - if (delta_enc > config_.cpr/2) + delta_enc = pos_abs_ - count_in_cpr_; + delta_enc = mod(delta_enc, config_.cpr); + if (delta_enc > config_.cpr / 2) delta_enc -= config_.cpr; - if(!abs_spi_pos_init_once_ && delta_enc != 0){ + if (!abs_spi_pos_init_once_ && delta_enc != 0) { abs_spi_pos_init_once_ = true; } - }break; + } break; default: { - set_error(ERROR_UNSUPPORTED_ENCODER_MODE); - return false; + set_error(ERROR_UNSUPPORTED_ENCODER_MODE); + return false; } break; } @@ -475,25 +482,25 @@ bool Encoder::update() { count_in_cpr_ += delta_enc; count_in_cpr_ = mod(count_in_cpr_, config_.cpr); - if(config_.mode & MODE_FLAG_ABS) + if (config_.mode & MODE_FLAG_ABS) count_in_cpr_ = pos_abs_; //// run pll (for now pll is in units of encoder counts) // Predict current pos pos_estimate_ += current_meas_period * vel_estimate_; - pos_cpr_ += current_meas_period * vel_estimate_; + pos_cpr_ += current_meas_period * vel_estimate_; // discrete phase detector float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; - pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; + pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); - vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; + vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; bool snap_to_zero_vel = false; if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) { - vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter + vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter snap_to_zero_vel = true; } @@ -502,7 +509,7 @@ bool Encoder::update() { // if we are stopped, make sure we don't randomly drift if (snap_to_zero_vel || !config_.enable_phase_interpolation) { interpolation_ = 0.5f; - // reset interpolation if encoder edge comes + // reset interpolation if encoder edge comes } else if (delta_enc > 0) { interpolation_ = 0.0f; } else if (delta_enc < 0) { @@ -519,7 +526,7 @@ bool Encoder::update() { //// compute electrical phase //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); - float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); + float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index c93c08d8..9cf5291b 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -6,18 +6,18 @@ #endif class Encoder { -public: + public: enum Error_t { - ERROR_NONE = 0, - ERROR_UNSTABLE_GAIN = 0x01, - ERROR_CPR_OUT_OF_RANGE = 0x02, - ERROR_NO_RESPONSE = 0x04, + ERROR_NONE = 0, + ERROR_UNSTABLE_GAIN = 0x01, + ERROR_CPR_OUT_OF_RANGE = 0x02, + ERROR_NO_RESPONSE = 0x04, ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, - ERROR_ILLEGAL_HALL_STATE = 0x10, - ERROR_INDEX_NOT_FOUND_YET = 0x20, - ERROR_ABS_SPI_TIMEOUT = 0x40, - ERROR_ABS_SPI_COM_FAIL = 0x80, - ERROR_ABS_SPI_NOT_READY = 0x100, + ERROR_ILLEGAL_HALL_STATE = 0x10, + ERROR_INDEX_NOT_FOUND_YET = 0x20, + ERROR_ABS_SPI_TIMEOUT = 0x40, + ERROR_ABS_SPI_COM_FAIL = 0x80, + ERROR_ABS_SPI_NOT_READY = 0x100, }; enum Mode_t { @@ -31,30 +31,30 @@ public: struct Config_t { Encoder::Mode_t mode = Encoder::MODE_INCREMENTAL; - bool use_index = false; - bool pre_calibrated = false; // If true, this means the offset stored in - // configuration is valid and does not need - // be determined by run_offset_calibration. - // In this case the encoder will enter ready - // state as soon as the index is found. - bool zero_count_on_find_idx = true; - int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, - int32_t offset = 0; // Offset between encoder count and rotor electrical phase - float offset_float = 0.0f; // Sub-count phase alignment offset - bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state - float calib_range = 0.02f; // Accuracy required to pass encoder cpr check - float calib_scan_distance = 16.0f * M_PI; // rad electrical - float calib_scan_omega = 4.0f * M_PI; // rad/s electrical - float bandwidth = 1000.0f; - bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state - bool idx_search_unidirectional = false; // Only allow index search in known direction - bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111 - uint16_t abs_spi_cs_gpio_pin = 0; + bool use_index = false; + bool pre_calibrated = false; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. + bool zero_count_on_find_idx = true; + int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, + int32_t offset = 0; // Offset between encoder count and rotor electrical phase + float offset_float = 0.0f; // Sub-count phase alignment offset + bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state + float calib_range = 0.02f; // Accuracy required to pass encoder cpr check + float calib_scan_distance = 16.0f * M_PI; // rad electrical + float calib_scan_omega = 4.0f * M_PI; // rad/s electrical + float bandwidth = 1000.0f; + bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state + bool idx_search_unidirectional = false; // Only allow index search in known direction + bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111 + uint16_t abs_spi_cs_gpio_pin = 0; }; Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config); - + Config_t& config); + void setup(); void set_error(Error_t error); bool do_checks(); @@ -76,27 +76,27 @@ public: const EncoderHardwareConfig_t& hw_config_; Config_t& config_; - Axis* axis_ = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor - Error_t error_ = ERROR_NONE; - bool index_found_ = false; - bool is_ready_ = false; - int32_t shadow_count_ = 0; - int32_t count_in_cpr_ = 0; - float interpolation_ = 0.0f; - float phase_ = 0.0f; // [count] - float pos_estimate_ = 0.0f; // [count] - float pos_cpr_ = 0.0f; // [count] - float vel_estimate_ = 0.0f; // [count/s] - float pll_kp_ = 0.0f; // [count/s / count] - float pll_ki_ = 0.0f; // [(count/s^2) / count] - float calib_scan_response_ = 0.0f; // debug report from offset calib - int32_t pos_abs_ = 0; - float spi_error_rate_ = 0.0f; + Error_t error_ = ERROR_NONE; + bool index_found_ = false; + bool is_ready_ = false; + int32_t shadow_count_ = 0; + int32_t count_in_cpr_ = 0; + float interpolation_ = 0.0f; + float phase_ = 0.0f; // [count] + float pos_estimate_ = 0.0f; // [count] + float pos_cpr_ = 0.0f; // [count] + float vel_estimate_ = 0.0f; // [count/s] + float pll_kp_ = 0.0f; // [count/s / count] + float pll_ki_ = 0.0f; // [(count/s^2) / count] + float calib_scan_response_ = 0.0f; // debug report from offset calib + int32_t pos_abs_ = 0; + float spi_error_rate_ = 0.0f; - int16_t tim_cnt_sample_ = 0; // + int16_t tim_cnt_sample_ = 0; // // Updated by low_level pwm_adc_cb - uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC + uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC float sincos_sample_s_ = 0.0f; float sincos_sample_c_ = 0.0f; @@ -106,14 +106,14 @@ public: void abs_spi_cs_pin_init(); uint16_t abs_spi_dma_tx_[2] = {0xFFFF, 0x0000}; uint16_t abs_spi_dma_rx_[2]; - bool abs_spi_pos_updated_ = false; + bool abs_spi_pos_updated_ = false; bool abs_spi_pos_init_once_ = false; GPIO_TypeDef* abs_spi_cs_port_; uint16_t abs_spi_cs_pin_; uint32_t abs_spi_cr1; uint32_t abs_spi_cr2; - constexpr float getCoggingRatio(){ + constexpr float getCoggingRatio() { return config_.cpr / 3600.0f; } @@ -136,34 +136,32 @@ public: make_protocol_ro_property("spi_error_rate", &spi_error_rate_), make_protocol_object("config", - make_protocol_property("mode", &config_.mode, - [](void* ctx) { static_cast(ctx)->abs_spi_init(); }, this), - make_protocol_property("use_index", &config_.use_index, - [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), - make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, - [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), - make_protocol_property("pre_calibrated", &config_.pre_calibrated, - [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), - make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin, - [](void* ctx) { static_cast(ctx)->abs_spi_cs_pin_init(); }, this), - make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), - make_protocol_property("cpr", &config_.cpr), - make_protocol_property("offset", &config_.offset), - make_protocol_property("offset_float", &config_.offset_float), - make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation), - make_protocol_property("bandwidth", &config_.bandwidth, - [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), - make_protocol_property("calib_range", &config_.calib_range), - make_protocol_property("calib_scan_distance", &config_.calib_scan_distance), - make_protocol_property("calib_scan_omega", &config_.calib_scan_omega), - make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), - make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state) - ), - make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count") - ); + make_protocol_property("mode", &config_.mode, + [](void* ctx) { static_cast(ctx)->abs_spi_init(); }, this), + make_protocol_property("use_index", &config_.use_index, + [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, + [](void* ctx) { static_cast(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("pre_calibrated", &config_.pre_calibrated, + [](void* ctx) { static_cast(ctx)->check_pre_calibrated(); }, this), + make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin, + [](void* ctx) { static_cast(ctx)->abs_spi_cs_pin_init(); }, this), + make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx), + make_protocol_property("cpr", &config_.cpr), + make_protocol_property("offset", &config_.offset), + make_protocol_property("offset_float", &config_.offset_float), + make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation), + make_protocol_property("bandwidth", &config_.bandwidth, + [](void* ctx) { static_cast(ctx)->update_pll_gains(); }, this), + make_protocol_property("calib_range", &config_.calib_range), + make_protocol_property("calib_scan_distance", &config_.calib_scan_distance), + make_protocol_property("calib_scan_omega", &config_.calib_scan_omega), + make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional), + make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)), + make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count")); } }; DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t) -#endif // __ENCODER_HPP +#endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index 809e52f1..da8e260a 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -6,18 +6,18 @@ Endstop::Endstop(Endstop::Config_t& config) } void Endstop::update() { - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); - auto last_pin_state = pin_state_; - pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); + auto last_pin_state = pin_state_; + pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin); if (pin_state_ != last_pin_state) { debounce_timer_ = axis_->loop_counter_ * current_meas_period; } if (config_.enabled) { float now = axis_->loop_counter_ * current_meas_period; - if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state - endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state - debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues + if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state + endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state + debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues } else { endstop_state_ = endstop_state_; // Do nothing } @@ -30,18 +30,18 @@ bool Endstop::getEndstopState() { return endstop_state_; } -void Endstop::update_endstop_config(){ +void Endstop::update_endstop_config() { set_endstop_enabled(config_.enabled); } void Endstop::set_endstop_enabled(bool enable) { if (config_.gpio_num != 0) { - uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); + uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num); GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num); if (enable) { HAL_GPIO_DeInit(gpio_port, gpio_pin); GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = gpio_pin; + GPIO_InitStruct.Pin = gpio_pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP; HAL_GPIO_Init(gpio_port, &GPIO_InitStruct); diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index cdca3fdc..f1262ed9 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -5,10 +5,10 @@ class Endstop { public: struct Config_t { uint16_t gpio_num; - bool enabled = false; - int32_t offset = 0; + bool enabled = false; + int32_t offset = 0; bool is_active_high = false; - float debounce_ms = 100.0f; + float debounce_ms = 100.0f; }; Endstop(Endstop::Config_t& config); @@ -38,7 +38,7 @@ class Endstop { } private: - bool pin_state_ = false; + bool pin_state_ = false; volatile float debounce_timer_ = 0; }; #endif \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 12d70d39..79349a8a 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -28,21 +28,21 @@ /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ -const float adc_full_scale = (float)(1 << 12); +const float adc_full_scale = (float)(1 << 12); const float adc_ref_voltage = 3.3f; /* Global variables ----------------------------------------------------------*/ // This value is updated by the DC-bus reading ADC. // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late -float vbus_voltage = 12.0f; +float vbus_voltage = 12.0f; bool brake_resistor_armed = false; /* Private constant data -----------------------------------------------------*/ -static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC }; -static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]); +static const GPIO_TypeDef* GPIOs_to_samp[] = {GPIOA, GPIOB, GPIOC}; +static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]); /* Private variables ---------------------------------------------------------*/ // Two motors, sampling port A,B,C (coherent with current meas timing) -static uint16_t GPIO_port_samples [2][num_GPIO]; +static uint16_t GPIO_port_samples[2][num_GPIO]; /* CPU critical section helpers ----------------------------------------------*/ /* Safety critical functions -------------------------------------------------*/ @@ -109,8 +109,8 @@ void safety_critical_arm_motor_pwm(Motor& motor) { // safety_critical_arm_motor_phases is called. // @returns true if the motor was in a state other than disarmed before bool safety_critical_disarm_motor_pwm(Motor& motor) { - uint32_t mask = cpu_enter_critical(); - bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED; + uint32_t mask = cpu_enter_critical(); + bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED; motor.armed_state_ = Motor::ARMED_STATE_DISARMED; __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); cpu_exit_critical(mask); @@ -154,7 +154,7 @@ void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) // @brief Arms the brake resistor void safety_critical_arm_brake_resistor() { - uint32_t mask = cpu_enter_critical(); + uint32_t mask = cpu_enter_critical(); brake_resistor_armed = true; htim2.Instance->CCR3 = 0; htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; @@ -166,7 +166,7 @@ void safety_critical_arm_brake_resistor() { // After calling this, the brake resistor can only be armed again // by calling safety_critical_arm_brake_resistor(). void safety_critical_disarm_brake_resistor() { - uint32_t mask = cpu_enter_critical(); + uint32_t mask = cpu_enter_critical(); brake_resistor_armed = false; htim2.Instance->CCR3 = 0; htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; @@ -217,7 +217,7 @@ void start_adc_pwm() { start_pwm(&htim8); // TODO: explain why this offset sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128, - &htim13); + &htim13); // Motor output starts in the disabled state __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); @@ -242,7 +242,7 @@ void start_adc_pwm() { void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS / 2; + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; htim->Instance->CCR1 = half_load; htim->Instance->CCR2 = half_load; htim->Instance->CCR3 = half_load; @@ -265,8 +265,8 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, // Store intial timer configs uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t CR2_store = htim_a->Instance->CR2; - uint16_t SMCR_store = htim_b->Instance->SMCR; + uint16_t CR2_store = htim_a->Instance->CR2; + uint16_t SMCR_store = htim_b->Instance->SMCR; // Turn off output htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); @@ -299,12 +299,12 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, // Set and start reference timebase timer (if used) if (htim_refbase) { htim_refbase->Instance->CNT = count_offset; - htim_refbase->Instance->CR1 |= (TIM_CR1_CEN); // start + htim_refbase->Instance->CR1 |= (TIM_CR1_CEN); // start } // Start Timer a htim_a->Instance->CR1 |= (TIM_CR1_CEN); // Restore timer configs - htim_a->Instance->CR2 = CR2_store; + htim_a->Instance->CR2 = CR2_store; htim_b->Instance->SMCR = SMCR_store; // restore output htim_a->Instance->BDTR |= MOE_store_a; @@ -312,7 +312,7 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, } // @brief ADC1 measurements are written to this buffer by DMA -uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = { 0 }; +uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = {0}; // @brief Starts the general purpose ADC on the ADC1 peripheral. // The measured ADC voltages can be read with get_adc_voltage(). @@ -327,20 +327,19 @@ void start_general_purpose_adc() { ADC_ChannelConfTypeDef sConfig; // Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) - hadc1.Instance = ADC1; - hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; - hadc1.Init.Resolution = ADC_RESOLUTION_12B; - hadc1.Init.ScanConvMode = ENABLE; - hadc1.Init.ContinuousConvMode = ENABLE; + hadc1.Instance = ADC1; + hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc1.Init.Resolution = ADC_RESOLUTION_12B; + hadc1.Init.ScanConvMode = ENABLE; + hadc1.Init.ContinuousConvMode = ENABLE; hadc1.Init.DiscontinuousConvMode = DISABLE; - hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; - hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; - hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT; + hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT; hadc1.Init.DMAContinuousRequests = ENABLE; - hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; - if (HAL_ADC_Init(&hadc1) != HAL_OK) - { + hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc1) != HAL_OK) { _Error_Handler((char*)__FILE__, __LINE__); } @@ -348,7 +347,7 @@ void start_general_purpose_adc() { sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES; for (uint32_t channel = 0; channel < ADC_CHANNEL_COUNT; ++channel) { sConfig.Channel = channel << ADC_CR1_AWDCH_Pos; - sConfig.Rank = channel + 1; // rank numbering starts at 1 + sConfig.Rank = channel + 1; // rank numbering starts at 1 if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) _Error_Handler((char*)__FILE__, __LINE__); } @@ -414,7 +413,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { if (channel < ADC_CHANNEL_COUNT) return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale); else - return 0.0f / 0.0f; // NaN + return 0.0f / 0.0f; // NaN } //-------------------------------- @@ -425,7 +424,7 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { static const float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale; // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - vbus_voltage = ADCValue * voltage_scale; + vbus_voltage = ADCValue * voltage_scale; if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) { if (oscilloscope_pos >= OSCILLOSCOPE_SIZE) oscilloscope_pos = 0; @@ -478,10 +477,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current // If we are counting down, we just sampled in SVM vector 7, with zero current - Axis& axis = injected ? *axes[0] : *axes[1]; - int axis_num = injected ? 0 : 1; - Axis& other_axis = injected ? *axes[1] : *axes[0]; - bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR; + Axis& axis = injected ? *axes[0] : *axes[1]; + int axis_num = injected ? 0 : 1; + Axis& other_axis = injected ? *axes[1] : *axes[0]; + bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR; bool current_meas_not_DC_CAL = !counting_down; // Check the timing of the sequencing @@ -493,12 +492,12 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { bool update_timings = false; if (hadc == &hadc2) { if (&axis == axes[1] && counting_down) - update_timings = true; // update timings of M0 + update_timings = true; // update timings of M0 else if (&axis == axes[0] && !counting_down) - update_timings = true; // update timings of M1 + update_timings = true; // update timings of M1 - if((current_meas_not_DC_CAL && !axis_num) || - (axis_num && !current_meas_not_DC_CAL)){ + if ((current_meas_not_DC_CAL && !axis_num) || + (axis_num && !current_meas_not_DC_CAL)) { axis.encoder_.abs_spi_start_transaction(); } } @@ -515,8 +514,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { other_axis.motor_.next_timings_valid_ = false; safety_critical_apply_motor_pwm_timings( - other_axis.motor_, other_axis.motor_.next_timings_ - ); + other_axis.motor_, other_axis.motor_.next_timings_); } update_brake_current(); } @@ -559,21 +557,20 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } void tim_update_cb(TIM_HandleTypeDef* htim) { - // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current // If we are counting down, we just sampled in SVM vector 7, with zero current bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR; if (counting_down) return; - + int sample_ch; Axis* axis; if (htim == &htim1) { sample_ch = 0; - axis = axes[0]; + axis = axes[0]; } else if (htim == &htim8) { sample_ch = 1; - axis = axes[1]; + axis = axes[1]; } else { low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK); return; @@ -595,10 +592,10 @@ void update_brake_current() { Ibus_sum += axes[i]->motor_.current_control_.Ibus; } } - + // Don't start braking until -Ibus > regen_current_allowed - float brake_current = std::max(-Ibus_sum - board_config.max_regen_current, 0.0f); - float brake_duty = std::max(brake_current * std::abs(board_config.brake_resistance) / vbus_voltage, 0.0f); + float brake_current = std::max(-Ibus_sum - board_config.max_regen_current, 0.0f); + float brake_duty = std::max(brake_current * std::abs(board_config.brake_resistance) / vbus_voltage, 0.0f); // Duty limit at 90% to allow bootstrap caps to charge // If brake_duty is NaN, this expression will also evaluate to false @@ -613,7 +610,6 @@ void update_brake_current() { } } - /* RC PWM input --------------------------------------------------------------*/ // @brief Returns the ODrive GPIO number for a given @@ -641,11 +637,16 @@ int tim_2_5_channel_num_to_gpio_num(int channel) { uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 switch (gpio_num) { - case 1: return TIM_CHANNEL_1; - case 2: return TIM_CHANNEL_2; - case 3: return TIM_CHANNEL_3; - case 4: return TIM_CHANNEL_4; - default: return 0; + case 1: + return TIM_CHANNEL_1; + case 2: + return TIM_CHANNEL_2; + case 3: + return TIM_CHANNEL_3; + case 4: + return TIM_CHANNEL_4; + default: + return 0; } #else // Only ch4 is available on v3.2 @@ -659,21 +660,22 @@ uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { void pwm_in_init() { GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; TIM_IC_InitTypeDef sConfigIC; - sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; - sConfigIC.ICFilter = 15; + sConfigIC.ICFilter = 15; #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 for (int gpio_num = 1; gpio_num <= 4; ++gpio_num) { #else - int gpio_num = 4; { + int gpio_num = 4; + { #endif if (is_endpoint_ref_valid(board_config.pwm_mappings[gpio_num - 1].endpoint)) { GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num); @@ -686,12 +688,12 @@ void pwm_in_init() { } //TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz -#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ -#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse -#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward -#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms -#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms -#define PWM_INVERT_INPUT false +#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ +#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse +#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward +#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms +#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms +#define PWM_INVERT_INPUT false void handle_pulse(int gpio_num, uint32_t high_time) { if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME) @@ -702,7 +704,7 @@ void handle_pulse(int gpio_num, uint32_t high_time) { if (high_time > PWM_MAX_HIGH_TIME) high_time = PWM_MAX_HIGH_TIME; float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME); - float value = board_config.pwm_mappings[gpio_num - 1].min + + float value = board_config.pwm_mappings[gpio_num - 1].min + (fraction * (board_config.pwm_mappings[gpio_num - 1].max - board_config.pwm_mappings[gpio_num - 1].min)); Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num - 1].endpoint); @@ -713,41 +715,36 @@ void handle_pulse(int gpio_num, uint32_t high_time) { } void pwm_in_cb(int channel, uint32_t timestamp) { - static uint32_t last_timestamp[GPIO_COUNT] = { 0 }; - static bool last_pin_state[GPIO_COUNT] = { false }; - static bool last_sample_valid[GPIO_COUNT] = { false }; + static uint32_t last_timestamp[GPIO_COUNT] = {0}; + static bool last_pin_state[GPIO_COUNT] = {false}; + static bool last_sample_valid[GPIO_COUNT] = {false}; int gpio_num = tim_2_5_channel_num_to_gpio_num(channel); if (gpio_num < 1 || gpio_num > GPIO_COUNT) return; bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET; - if (last_sample_valid[gpio_num - 1] - && (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT) - && (current_pin_state == PWM_INVERT_INPUT)) { + if (last_sample_valid[gpio_num - 1] && (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT) && (current_pin_state == PWM_INVERT_INPUT)) { handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num - 1]); } - last_timestamp[gpio_num - 1] = timestamp; - last_pin_state[gpio_num - 1] = current_pin_state; + last_timestamp[gpio_num - 1] = timestamp; + last_pin_state[gpio_num - 1] = current_pin_state; last_sample_valid[gpio_num - 1] = true; } - /* Analog speed control input */ -static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio) -{ +static void update_analog_endpoint(const struct PWMMapping_t* map, int gpio) { float fraction = get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio)) / 3.3f; - float value = map->min + (fraction * (map->max - map->min)); + float value = map->min + (fraction * (map->max - map->min)); get_endpoint(map->endpoint)->set_from_float(value); } -static void analog_polling_thread(void *) -{ +static void analog_polling_thread(void*) { while (true) { for (int i = 0; i < GPIO_COUNT; i++) { - struct PWMMapping_t *map = &board_config.analog_mappings[i]; + struct PWMMapping_t* map = &board_config.analog_mappings[i]; if (is_endpoint_ref_valid(map->endpoint)) update_analog_endpoint(map, i + 1); @@ -756,16 +753,13 @@ static void analog_polling_thread(void *) } } -void start_analog_thread() -{ - osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4*512); +void start_analog_thread() { + osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4 * 512); osThreadCreate(osThread(thread_def), NULL); } - -void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) -{ - if(hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_) +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi) { + if (hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_) axes[0]->encoder_.abs_spi_cb(); else if (hspi->pRxBuffPtr == (uint8_t*)axes[1]->encoder_.abs_spi_dma_rx_) axes[1]->encoder_.abs_spi_cb(); diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 503b98e1..e56f4adc 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -11,9 +11,9 @@ extern "C" { #endif /* Includes ------------------------------------------------------------------*/ +#include #include #include -#include /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ @@ -69,4 +69,4 @@ inline void cpu_exit_critical(uint32_t priority_mask) { } #endif -#endif //__LOW_LEVEL_H +#endif //__LOW_LEVEL_H diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 17707eaa..173d0239 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,13 +1,13 @@ #define __MAIN_CPP__ -#include "odrive_main.h" #include "nvm_config.hpp" +#include "odrive_main.h" -#include "freertos_vars.h" -#include -#include #include +#include +#include #include +#include "freertos_vars.h" BoardConfig_t board_config; ODriveCAN::Config_t can_config; @@ -21,7 +21,7 @@ Endstop::Config_t min_endstop_configs[AXIS_COUNT]; Endstop::Config_t max_endstop_configs[AXIS_COUNT]; bool user_config_loaded_; -SystemStats_t system_stats_ = { 0 }; +SystemStats_t system_stats_ = {0}; Axis *axes[AXIS_COUNT]; ODriveCAN *odCAN; @@ -36,7 +36,8 @@ typedef Config< TrapezoidalTrajectory::Config_t[AXIS_COUNT], Endstop::Config_t[AXIS_COUNT], Endstop::Config_t[AXIS_COUNT], - Axis::Config_t[AXIS_COUNT]> ConfigFormat; + Axis::Config_t[AXIS_COUNT]> + ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -50,7 +51,8 @@ void save_configuration(void) { &min_endstop_configs, &max_endstop_configs, &axis_configs)) { - printf("saving configuration failed\r\n"); osDelay(5); + printf("saving configuration failed\r\n"); + osDelay(5); } else { user_config_loaded_ = true; } @@ -60,26 +62,26 @@ extern "C" int load_configuration(void) { // Try to load configs if (NVM_init() || ConfigFormat::safe_load_config( - &board_config, - &can_config, - &encoder_configs, - &sensorless_configs, - &controller_configs, - &motor_configs, - &trap_configs, - &min_endstop_configs, - &max_endstop_configs, - &axis_configs)) { + &board_config, + &can_config, + &encoder_configs, + &sensorless_configs, + &controller_configs, + &motor_configs, + &trap_configs, + &min_endstop_configs, + &max_endstop_configs, + &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); - can_config = ODriveCAN::Config_t(); + can_config = ODriveCAN::Config_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { - encoder_configs[i] = Encoder::Config_t(); + encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = Controller::Config_t(); - motor_configs[i] = Motor::Config_t(); - trap_configs[i] = TrapezoidalTrajectory::Config_t(); - axis_configs[i] = Axis::Config_t(); + motor_configs[i] = Motor::Config_t(); + trap_configs[i] = TrapezoidalTrajectory::Config_t(); + axis_configs[i] = Axis::Config_t(); // Default step/dir pins are different, so we need to explicitly load them Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]); Axis::load_default_can_id(i, axis_configs[i]); @@ -98,7 +100,8 @@ void erase_configuration(void) { void enter_dfu_mode() { if ((hw_version_major == 3) && (hw_version_minor >= 5)) { - __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts + __asm volatile("CPSID I\n\t" :: + : "memory"); // disable interrupts _reboot_cookie = 0xDEADBEEF; NVIC_SystemReset(); } else { @@ -118,26 +121,26 @@ extern "C" { int odrive_main(void); void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) { - for (;;); // TODO: safe action + for (;;) + ; // TODO: safe action } void vApplicationIdleHook(void) { if (system_stats_.fully_booted) { - system_stats_.uptime = xTaskGetTickCount(); - system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize(); - system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t); - system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t); - system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t); - system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); - system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); + system_stats_.uptime = xTaskGetTickCount(); + system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize(); + system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t); system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); - system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t); } } } int odrive_main(void) { - #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (board_config.enable_i2c_instead_of_can) { // Set up the direction GPIO as input @@ -166,7 +169,7 @@ int odrive_main(void) { GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Pin = GPIO_1_Pin; + GPIO_InitStruct.Pin = GPIO_1_Pin; HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct); GPIO_InitStruct.Pin = GPIO_2_Pin; HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); @@ -182,20 +185,20 @@ int odrive_main(void) { // Construct all objects. odCAN = new ODriveCAN(&hcan1, can_config); for (size_t i = 0; i < AXIS_COUNT; ++i) { - Encoder *encoder = new Encoder(hw_configs[i].encoder_config, + Encoder *encoder = new Encoder(hw_configs[i].encoder_config, encoder_configs[i]); SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]); - Controller *controller = new Controller(controller_configs[i]); - Motor *motor = new Motor(hw_configs[i].motor_config, + Controller *controller = new Controller(controller_configs[i]); + Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); - TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); - Endstop *min_endstop = new Endstop(min_endstop_configs[i]); - Endstop *max_endstop = new Endstop(max_endstop_configs[i]); - axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i], - *encoder, *sensorless_estimator, *controller, *motor, *trap, *min_endstop, *max_endstop); + TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); + Endstop *min_endstop = new Endstop(min_endstop_configs[i]); + Endstop *max_endstop = new Endstop(max_endstop_configs[i]); + axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i], + *encoder, *sensorless_estimator, *controller, *motor, *trap, *min_endstop, *max_endstop); } - + // Start ADC for temperature measurements and user measurements start_general_purpose_adc(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 2a9cd703..92ba073f 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -4,20 +4,18 @@ #include "drv8301.h" #include "odrive_main.h" - Motor::Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, - Config_t& config) : - hw_config_(hw_config), - gate_driver_config_(gate_driver_config), - config_(config), - gate_driver_({ - .spiHandle = gate_driver_config_.spi, - .EngpioHandle = gate_driver_config_.enable_port, - .EngpioNumber = gate_driver_config_.enable_pin, - .nCSgpioHandle = gate_driver_config_.nCS_port, - .nCSgpioNumber = gate_driver_config_.nCS_pin, - }) { + Config_t& config) : hw_config_(hw_config), + gate_driver_config_(gate_driver_config), + config_(config), + gate_driver_({ + .spiHandle = gate_driver_config_.spi, + .EngpioHandle = gate_driver_config_.enable_port, + .EngpioNumber = gate_driver_config_.enable_pin, + .nCSgpioHandle = gate_driver_config_.nCS_port, + .nCSgpioNumber = gate_driver_config_.nCS_pin, + }) { update_current_controller_gains(); } @@ -33,7 +31,6 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, // // @returns: True on success, false otherwise bool Motor::arm() { - // Reset controller states, integrators, setpoints, etc. axis_->controller_.reset(); reset_current_control(); @@ -58,7 +55,7 @@ void Motor::reset_current_control() { void Motor::update_current_controller_gains() { // Calculate current control gains current_control_.p_gain = config_.current_control_bandwidth * config_.phase_inductance; - float plant_pole = config_.phase_resistance / config_.phase_inductance; + float plant_pole = config_.phase_resistance / config_.phase_inductance; current_control_.i_gain = plant_pole * current_control_.p_gain; } @@ -72,29 +69,28 @@ void Motor::DRV8301_setup() { // Solve for exact gain, then snap down to have equal or larger range as requested // or largest possible range otherwise - static const float kMargin = 0.90f; - static const float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer - static const float max_output_swing = 1.35f; // [V] out of amplifier - float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A] - float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V] + static const float kMargin = 0.90f; + static const float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer + static const float max_output_swing = 1.35f; // [V] out of amplifier + float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A] + float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V] // Decoding array for snapping gain - std::array, 4> gain_choices = { + std::array, 4> gain_choices = { std::make_pair(10.0f, DRV8301_ShuntAmpGain_10VpV), std::make_pair(20.0f, DRV8301_ShuntAmpGain_20VpV), std::make_pair(40.0f, DRV8301_ShuntAmpGain_40VpV), - std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV) - }; + std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV)}; // We use lower_bound in reverse because it snaps up by default, we want to snap down. - auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain, - [](std::pair pair, float val){ - return pair.first > val; - }); + auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain, + [](std::pair pair, float val) { + return pair.first > val; + }); // If we snap to outside the array, clip to smallest val - if(gain_snap_down == gain_choices.crend()) - --gain_snap_down; + if (gain_snap_down == gain_choices.crend()) + --gain_snap_down; // Values for current controller phase_current_rev_gain_ = 1.0f / gain_snap_down->first; @@ -111,7 +107,7 @@ void Motor::DRV8301_setup() { local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; // Overcurrent set to approximately 150A at 100degC. This may need tweaking. local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second; + local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second; local_regs->SndCmd = true; DRV8301_writeData(&gate_driver_, local_regs); @@ -136,7 +132,7 @@ bool Motor::check_DRV_fault() { return true; } -void Motor::set_error(Motor::Error_t error){ +void Motor::set_error(Motor::Error_t error) { error_ |= error; axis_->error_ |= Axis::ERROR_MOTOR_FAILED; safety_critical_disarm_motor_pwm(*this); @@ -144,17 +140,17 @@ void Motor::set_error(Motor::Error_t error){ } float Motor::get_inverter_temp() { - float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch]; + float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch]; float normalized_voltage = adc / adc_full_scale; return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); } bool Motor::update_thermal_limits() { - float fet_temp = get_inverter_temp(); - float temp_margin = config_.inverter_temp_limit_upper - fet_temp; + float fet_temp = get_inverter_temp(); + float temp_margin = config_.inverter_temp_limit_upper - fet_temp; float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower; thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range); - if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN + if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN thermal_current_lim_ = 0.0f; } if (fet_temp > config_.inverter_temp_limit_upper + 5) { @@ -181,7 +177,7 @@ float Motor::effective_current_lim() { float current_lim = config_.current_lim; // Hardware limit if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) { - current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); + current_lim = std::min(current_lim, 0.98f * one_by_sqrt3 * vbus_voltage); } else { current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current); } @@ -193,7 +189,7 @@ float Motor::effective_current_lim() { void Motor::log_timing(TimingLog_t log_idx) { static const uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ); - uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config + uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config if (log_idx < TIMING_LOG_NUM_SLOTS) { timing_log_[log_idx] = timing; @@ -201,10 +197,10 @@ void Motor::log_timing(TimingLog_t log_idx) { } float Motor::phase_current_from_adcval(uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1 << 11); + int adcval_bal = (int)ADCValue - (1 << 11); float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * phase_current_rev_gain_; - float current = shunt_volt * hw_config_.shunt_conductance; + float shunt_volt = amp_out_volt * phase_current_rev_gain_; + float current = shunt_volt * hw_config_.shunt_conductance; return current; } @@ -214,12 +210,12 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) { // TODO check Ibeta balance to verify good motor connection bool Motor::measure_phase_resistance(float test_current, float max_voltage) { - static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s - float test_voltage = 0.0f; - + static const float kI = 10.0f; // [(V/s)/A] + static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s + float test_voltage = 0.0f; + size_t i = 0; - axis_->run_control_loop([&](){ + axis_->run_control_loop([&]() { float Ialpha = -(current_meas_.phB + current_meas_.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage || test_voltage < -max_voltage) @@ -227,7 +223,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { // Test voltage along phase A if (!enqueue_voltage_timings(test_voltage, 0.0f)) - return false; // error set inside enqueue_voltage_timings + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_R); return ++i < num_test_cycles; @@ -239,24 +235,24 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f)) // return false; // error set inside enqueue_voltage_timings - float R = test_voltage / test_current; + float R = test_voltage / test_current; config_.phase_resistance = R; - return true; // if we ran to completion that means success + return true; // if we ran to completion that means success } bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { - float test_voltages[2] = {voltage_low, voltage_high}; - float Ialphas[2] = {0.0f}; + float test_voltages[2] = {voltage_low, voltage_high}; + float Ialphas[2] = {0.0f}; static const int num_cycles = 5000; size_t t = 0; - axis_->run_control_loop([&](){ + axis_->run_control_loop([&]() { int i = t & 1; Ialphas[i] += -current_meas_.phB - current_meas_.phC; // Test voltage along phase A if (!enqueue_voltage_timings(test_voltages[i], 0.0f)) - return false; // error set inside enqueue_voltage_timings + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_L); return ++t < (num_cycles << 1); @@ -272,7 +268,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { // Note: A more correct formula would also take into account that there is a finite timestep. // However, the discretisation in the current control loop inverts the same discrepancy float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); - float L = v_L / dI_by_dt; + float L = v_L / dI_by_dt; config_.phase_inductance = L; // TODO arbitrary values set for now @@ -281,7 +277,6 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { return true; } - bool Motor::run_calibration() { float R_calib_max_voltage = config_.resistance_calib_max_voltage; if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { @@ -296,7 +291,7 @@ bool Motor::run_calibration() { } update_current_controller_gains(); - + is_calibrated_ = true; return true; } @@ -305,17 +300,17 @@ bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) return set_error(ERROR_MODULATION_MAGNITUDE), false; - next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_valid_ = true; return true; } bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); + float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); float mod_alpha = vfactor * v_alpha; - float mod_beta = vfactor * v_beta; + float mod_beta = vfactor * v_beta; if (!enqueue_modulation_timings(mod_alpha, mod_beta)) return false; log_timing(TIMING_LOG_FOC_VOLTAGE); @@ -324,10 +319,10 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { // We should probably make FOC Current call FOC Voltage to avoid duplication. bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) { - float c = our_arm_cos_f32(pwm_phase); - float s = our_arm_sin_f32(pwm_phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; + float c = our_arm_cos_f32(pwm_phase); + float s = our_arm_sin_f32(pwm_phase); + float v_alpha = c * v_d - s * v_q; + float v_beta = c * v_q + s * v_d; return enqueue_voltage_timings(v_alpha, v_beta); } @@ -339,21 +334,20 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha ictrl.Iq_setpoint = Iq_des; // Check for current sense saturation - if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level - || fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) { + if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level || fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) { set_error(ERROR_CURRENT_SENSE_SATURATION); return false; } // Clarke transform float Ialpha = -current_meas_.phB - current_meas_.phC; - float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); + float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform 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; + float Id = c_I * Ialpha + s_I * Ibeta; + float Iq = c_I * Ibeta - s_I * Ialpha; ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured); ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured); @@ -375,8 +369,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha 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 = V_to_mod * Vd; + float mod_q = V_to_mod * Vq; // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable @@ -396,24 +390,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform - float c_p = our_arm_cos_f32(pwm_phase); - float s_p = our_arm_sin_f32(pwm_phase); + float c_p = our_arm_cos_f32(pwm_phase); + float s_p = our_arm_sin_f32(pwm_phase); float mod_alpha = c_p * mod_d - s_p * mod_q; float mod_beta = c_p * mod_q + s_p * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) ictrl.final_v_alpha = mod_to_V * mod_alpha; - ictrl.final_v_beta = mod_to_V * mod_beta; + ictrl.final_v_beta = mod_to_V * mod_beta; // Apply SVM if (!enqueue_modulation_timings(mod_alpha, mod_beta)) - return false; // error set inside enqueue_modulation_timings + return false; // error set inside enqueue_modulation_timings log_timing(TIMING_LOG_FOC_CURRENT); return true; } - bool Motor::update(float current_setpoint, float phase, float phase_vel) { current_setpoint *= config_.direction; phase *= config_.direction; @@ -424,12 +417,12 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) { // Execute current command // TODO: move this into the mot if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){ + if (!FOC_current(0.0f, current_setpoint, phase, pwm_phase)) { return false; } } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(0.0f, current_setpoint, pwm_phase)) + if (!FOC_voltage(0.0f, current_setpoint, pwm_phase)) return false; } else { set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index d31985ee..5edefc5b 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -8,22 +8,22 @@ #include "drv8301.h" class Motor { -public: + public: enum Error_t { - ERROR_NONE = 0, + ERROR_NONE = 0, ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x0001, ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x0002, - ERROR_ADC_FAILED = 0x0004, - ERROR_DRV_FAULT = 0x0008, - ERROR_CONTROL_DEADLINE_MISSED = 0x0010, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x0020, - ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x0040, - ERROR_MODULATION_MAGNITUDE = 0x0080, - ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100, - ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, - ERROR_CURRENT_SENSE_SATURATION = 0x0400, - ERROR_INVERTER_OVER_TEMP = 0x0800, - ERROR_CURRENT_UNSTABLE = 0x1000 + ERROR_ADC_FAILED = 0x0004, + ERROR_DRV_FAULT = 0x0008, + ERROR_CONTROL_DEADLINE_MISSED = 0x0010, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x0020, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x0040, + ERROR_MODULATION_MAGNITUDE = 0x0080, + ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100, + ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, + ERROR_CURRENT_SENSE_SATURATION = 0x0400, + ERROR_INVERTER_OVER_TEMP = 0x0800, + ERROR_CURRENT_UNSTABLE = 0x1000 }; enum MotorType_t { @@ -37,41 +37,41 @@ public: float phC; }; - struct CurrentControl_t{ - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] + struct CurrentControl_t { + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; // [A] - float Iq_measured; // [A] - float Id_measured; // [A] + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; // [A] + float Iq_measured; // [A] + float Id_measured; // [A] float I_measured_report_filter_k; - float max_allowed_current; // [A] - float overcurrent_trip_level; // [A] + float max_allowed_current; // [A] + float overcurrent_trip_level; // [A] }; // NOTE: for gimbal motors, all units of A are instead V. // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. struct Config_t { - bool pre_calibrated = false; // can be set to true to indicate that all values here are valid - int32_t pole_pairs = 7; - float calibration_current = 10.0f; // [A] - float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - float phase_inductance = 0.0f; // to be set by measure_phase_inductance - float phase_resistance = 0.0f; // to be set by measure_phase_resistance - int32_t direction = 0; // 1 or -1 (0 = unspecified) - MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT; + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid + int32_t pole_pairs = 7; + float calibration_current = 10.0f; // [A] + float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + float phase_inductance = 0.0f; // to be set by measure_phase_inductance + float phase_resistance = 0.0f; // to be set by measure_phase_resistance + int32_t direction = 0; // 1 or -1 (0 = unspecified) + MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT; // Read out max_allowed_current to see max supported value for current_lim. // float current_lim = 70.0f; //[A] - float current_lim = 10.0f; //[A] + float current_lim = 10.0f; //[A] float current_lim_tolerance = 1.25f; // multiple of current_lim // Value used to compute shunt amplifier gains - float requested_current_range = 60.0f; // [A] + float requested_current_range = 60.0f; // [A] float current_control_bandwidth = 1000.0f; // [rad/s] float inverter_temp_limit_lower = 100; float inverter_temp_limit_upper = 120; @@ -98,8 +98,8 @@ public: }; Motor(const MotorHardwareConfig_t& hw_config, - const GateDriverHardwareConfig_t& gate_driver_config, - Config_t& config); + const GateDriverHardwareConfig_t& gate_driver_config, + Config_t& config); bool arm(); void disarm(); @@ -130,48 +130,47 @@ public: const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; Config_t& config_; - Axis* axis_ = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor -//private: + //private: - DRV8301_Obj gate_driver_; // initialized in constructor + DRV8301_Obj gate_driver_; // initialized in constructor uint16_t next_timings_[3] = { TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, - TIM_1_8_PERIOD_CLOCKS / 2 - }; - bool next_timings_valid_ = false; - uint16_t last_cpu_time_ = 0; - int timing_log_index_ = 0; - uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 }; + TIM_1_8_PERIOD_CLOCKS / 2}; + bool next_timings_valid_ = false; + uint16_t last_cpu_time_ = 0; + int timing_log_index_ = 0; + uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = {0}; // variables exposed on protocol Error_t error_ = ERROR_NONE; // Do not write to this variable directly! // It is for exclusive use by the safety_critical_... functions. - ArmedState_t armed_state_ = ARMED_STATE_DISARMED; - bool is_calibrated_ = config_.pre_calibrated; - Iph_BC_t current_meas_ = {0.0f, 0.0f}; - Iph_BC_t DC_calib_ = {0.0f, 0.0f}; - float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) + ArmedState_t armed_state_ = ARMED_STATE_DISARMED; + bool is_calibrated_ = config_.pre_calibrated; + Iph_BC_t current_meas_ = {0.0f, 0.0f}; + Iph_BC_t DC_calib_ = {0.0f, 0.0f}; + float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) CurrentControl_t current_control_ = { - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .Id_measured = 0.0f, - .I_measured_report_filter_k = 1.0f, - .max_allowed_current = 0.0f, - .overcurrent_trip_level = 0.0f, + .Ibus = 0.0f, + .final_v_alpha = 0.0f, + .final_v_beta = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, + .Id_measured = 0.0f, + .I_measured_report_filter_k = 1.0f, + .max_allowed_current = 0.0f, + .overcurrent_trip_level = 0.0f, }; DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault; - DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) - float thermal_current_lim_ = 10.0f; //[A] + DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) + float thermal_current_lim_ = 10.0f; //[A] // Communication protocol definitions auto make_protocol_definitions() { @@ -187,59 +186,55 @@ public: make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_), make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp), make_protocol_object("current_control", - make_protocol_property("p_gain", ¤t_control_.p_gain), - make_protocol_property("i_gain", ¤t_control_.i_gain), - make_protocol_property("v_current_control_integral_d", ¤t_control_.v_current_control_integral_d), - make_protocol_property("v_current_control_integral_q", ¤t_control_.v_current_control_integral_q), - make_protocol_property("Ibus", ¤t_control_.Ibus), - make_protocol_property("final_v_alpha", ¤t_control_.final_v_alpha), - make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), - make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), - make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), - make_protocol_property("Id_measured", ¤t_control_.Id_measured), - make_protocol_property("I_measured_report_filter_k", ¤t_control_.I_measured_report_filter_k), - make_protocol_ro_property("max_allowed_current", ¤t_control_.max_allowed_current), - make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level) - ), + make_protocol_property("p_gain", ¤t_control_.p_gain), + make_protocol_property("i_gain", ¤t_control_.i_gain), + make_protocol_property("v_current_control_integral_d", ¤t_control_.v_current_control_integral_d), + make_protocol_property("v_current_control_integral_q", ¤t_control_.v_current_control_integral_q), + make_protocol_property("Ibus", ¤t_control_.Ibus), + make_protocol_property("final_v_alpha", ¤t_control_.final_v_alpha), + make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), + make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), + make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), + make_protocol_property("Id_measured", ¤t_control_.Id_measured), + make_protocol_property("I_measured_report_filter_k", ¤t_control_.I_measured_report_filter_k), + make_protocol_ro_property("max_allowed_current", ¤t_control_.max_allowed_current), + make_protocol_ro_property("overcurrent_trip_level", ¤t_control_.overcurrent_trip_level)), make_protocol_object("gate_driver", - make_protocol_ro_property("drv_fault", &drv_fault_) - // make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), - // make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), - // make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), - // make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) - ), + make_protocol_ro_property("drv_fault", &drv_fault_) + // make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), + // make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), + // make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), + // make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) + ), make_protocol_object("timing_log", - make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]), - make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]), - make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]), - make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]), - make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]), - make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]), - make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]), - make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]), - make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) - ), + make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]), + make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]), + make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]), + make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]), + make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]), + make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]), + make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])), make_protocol_object("config", - make_protocol_property("pre_calibrated", &config_.pre_calibrated), - make_protocol_property("pole_pairs", &config_.pole_pairs), - make_protocol_property("calibration_current", &config_.calibration_current), - make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage), - make_protocol_property("phase_inductance", &config_.phase_inductance), - make_protocol_property("phase_resistance", &config_.phase_resistance), - make_protocol_property("direction", &config_.direction), - make_protocol_property("motor_type", &config_.motor_type), - make_protocol_property("current_lim", &config_.current_lim), - make_protocol_property("current_lim_tolerance", &config_.current_lim_tolerance), - make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower), - make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper), - make_protocol_property("requested_current_range", &config_.requested_current_range), - make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth, - [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this) - ) - ); + make_protocol_property("pre_calibrated", &config_.pre_calibrated), + make_protocol_property("pole_pairs", &config_.pole_pairs), + make_protocol_property("calibration_current", &config_.calibration_current), + make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage), + make_protocol_property("phase_inductance", &config_.phase_inductance), + make_protocol_property("phase_resistance", &config_.phase_resistance), + make_protocol_property("direction", &config_.direction), + make_protocol_property("motor_type", &config_.motor_type), + make_protocol_property("current_lim", &config_.current_lim), + make_protocol_property("current_lim_tolerance", &config_.current_lim_tolerance), + make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower), + make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper), + make_protocol_property("requested_current_range", &config_.requested_current_range), + make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth, + [](void* ctx) { static_cast(ctx)->update_current_controller_gains(); }, this))); } }; DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t) -#endif // __MOTOR_HPP +#endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index 2a96f595..a7195427 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -11,9 +11,8 @@ #include #include -#include "nvm.h" #include - +#include "nvm.h" /* Private defines -----------------------------------------------------------*/ #define CONFIG_CRC16_INIT 0xabcd @@ -33,7 +32,6 @@ static constexpr uint16_t config_version = 0x0001; /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ - // @brief Manages configuration load and store operations from and to NVM // // The NVM stores consecutive one-to-one copies of arbitrary objects. @@ -43,10 +41,10 @@ static constexpr uint16_t config_version = 0x0001; // - Config handles loading/storing of the first object (type T) and leaves // the rest of the objects to an "inner" class Config. // - Config<> represents the leaf of the recursion. -template +template struct Config; -template<> +template <> struct Config<> { static size_t get_size() { return 0; @@ -59,7 +57,7 @@ struct Config<> { } }; -template +template struct Config { static size_t get_size() { return sizeof(T) + Config::get_size(); @@ -71,13 +69,13 @@ struct Config { // of the last comitted NVM block // @param crc16: the result of the CRC calculation is written to this address // @param val0, vals: the values to be loaded - static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) { + static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts*... vals) { size_t size = sizeof(T); // save current CRC (in case val0 and crc16 point to the same address) size_t previous_crc16 = *crc16; - if (NVM_read(offset, (uint8_t *)val0, size)) + if (NVM_read(offset, (uint8_t*)val0, size)) return -1; - *crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size); + *crc16 = calc_crc16(previous_crc16, (uint8_t*)val0, size); if (Config::load_config(offset + size, crc16, vals...)) return -1; return 0; @@ -89,13 +87,13 @@ struct Config { // of the currently active NVM write block // @param crc16: the result of the CRC calculation is written to this address // @param val0, vals: the values to be stored - static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) { + static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts*... vals) { size_t size = sizeof(T); - if (NVM_write(offset, (uint8_t *)val0, size)) + if (NVM_write(offset, (uint8_t*)val0, size)) return -1; // update CRC _after_ writing (in case val0 and crc16 point to the same address) if (crc16) - *crc16 = calc_crc16(*crc16, (uint8_t *)val0, size); + *crc16 = calc_crc16(*crc16, (uint8_t*)val0, size); if (Config::store_config(offset + size, crc16, vals...)) return -1; return 0; @@ -103,7 +101,7 @@ struct Config { // @brief Loads one or more consecutive objects from the NVM. The loaded data // is validated using a CRC value that is stored at the beginning of the data. - static int safe_load_config(T* val0, Ts* ... vals) { + static int safe_load_config(T* val0, Ts*... vals) { //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); if (Config::get_size() > NVM_get_max_read_length()) return -1; @@ -122,7 +120,7 @@ struct Config { // changes of the config structs during firmware update. Note that if the total // config data length changes, the CRC validation will fail even if the developer // forgets to update the config version number. - static int safe_store_config(const T* val0, const Ts* ... vals) { + static int safe_store_config(const T* val0, const Ts*... vals) { size_t size = Config::get_size() + 2; //printf("config is %d bytes\r\n", size); osDelay(5); if (size > NVM_get_max_write_length()) @@ -132,7 +130,7 @@ struct Config { uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; if (Config::store_config(0, &crc16, val0, vals...)) return -1; - if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) + if (Config::store_config(size - 2, nullptr, (uint8_t*)&crc16 + 1, (uint8_t*)&crc16)) return -1; if (NVM_commit()) return -1; diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 43191ce3..59971d82 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,9 +1,7 @@ #include "odrive_main.h" -SensorlessEstimator::SensorlessEstimator(Config_t& config) : - config_(config) - {}; +SensorlessEstimator::SensorlessEstimator(Config_t& config) : config_(config){}; bool SensorlessEstimator::update() { // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer @@ -37,10 +35,10 @@ bool SensorlessEstimator::update() { } // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage; - float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; + float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage; + float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; float bandwidth_factor = 1.0f / pm_flux_sqr; - float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); // alpha-beta vector operations for (int i = 0; i <= 1; ++i) { @@ -71,9 +69,9 @@ bool SensorlessEstimator::update() { // predict PLL phase with velocity pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_); // update PLL phase with observer permanent magnet phase - phase_ = fast_atan2(eta[1], eta[0]); + 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); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase); // update PLL velocity vel_estimate_ += current_meas_period * pll_ki * delta_phase; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 719a3227..f97a8ab4 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -2,35 +2,35 @@ #define __SENSORLESS_ESTIMATOR_HPP class SensorlessEstimator { -public: + public: enum Error_t { - ERROR_NONE = 0, + ERROR_NONE = 0, ERROR_UNSTABLE_GAIN = 0x01, }; struct Config_t { - float observer_gain = 1000.0f; // [rad/s] - float pll_bandwidth = 1000.0f; // [rad/s] - float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + float observer_gain = 1000.0f; // [rad/s] + float pll_bandwidth = 1000.0f; // [rad/s] + float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } }; explicit SensorlessEstimator(Config_t& config); bool update(); - Axis* axis_ = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor Config_t& config_; // TODO: expose on protocol - Error_t error_ = ERROR_NONE; - float phase_ = 0.0f; // [rad] - float pll_pos_ = 0.0f; // [rad] - float vel_estimate_ = 0.0f; // [rad/s] + Error_t error_ = ERROR_NONE; + float phase_ = 0.0f; // [rad] + float pll_pos_ = 0.0f; // [rad] + float vel_estimate_ = 0.0f; // [rad/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; + float flux_state_[2] = {0.0f, 0.0f}; // [Vs] + float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] + bool estimator_good_ = false; // Communication protocol definitions auto make_protocol_definitions() { @@ -42,11 +42,9 @@ public: // make_protocol_property("pll_kp", &pll_kp_), // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", - make_protocol_property("observer_gain", &config_.observer_gain), - make_protocol_property("pll_bandwidth", &config_.pll_bandwidth), - make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage) - ) - ); + make_protocol_property("observer_gain", &config_.observer_gain), + make_protocol_property("pll_bandwidth", &config_.pll_bandwidth), + make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage))); } }; diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index f1e41aa5..2cd050a4 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -19,13 +19,13 @@ TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { - float dX = Xf - Xi; // Distance to travel - float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance - float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement - float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any) - Ar_ = s * Amax; // Maximum Acceleration (signed) - Dr_ = -s * Dmax; // Maximum Deceleration (signed) - Vr_ = s * Vmax; // Maximum Velocity (signed) + float dX = Xf - Xi; // Distance to travel + float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance + float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement + float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any) + Ar_ = s * Amax; // Maximum Acceleration (signed) + Dr_ = -s * Dmax; // Maximum Deceleration (signed) + Vr_ = s * Vmax; // Maximum Velocity (signed) // If we start with a speed faster than cruising, then we need to decel instead of accel // aka "double deceleration move" in the paper @@ -39,12 +39,12 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, // Integral of velocity ramps over the full accel and decel times to get // minimum displacement required to reach cuising speed - float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_; + float dXmin = 0.5f * Ta_ * (Vr_ + Vi) + 0.5f * Td_ * Vr_; // Are we displacing enough to reach cruising speed? - if (s*dX < s*dXmin) { + if (s * dX < s * dXmin) { // Short move (triangle profile) - Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_)); + Vr_ = s * sqrtf((Dr_ * SQ(Vi) + 2 * Ar_ * Dr_ * dX) / (Dr_ - Ar_)); Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); Td_ = std::max(0.0f, -Vr_ / Dr_); Tv_ = 0.0f; @@ -54,11 +54,11 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, } // Fill in the rest of the values used at evaluation-time - Tf_ = Ta_ + Tv_ + Td_; - Xi_ = Xi; - Xf_ = Xf; - Vi_ = Vi; - yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase + Tf_ = Ta_ + Tv_ + Td_; + Xi_ = Xi; + Xf_ = Xf; + Vi_ = Vi; + yAccel_ = Xi + Vi * Ta_ + 0.5f * Ar_ * SQ(Ta_); // pos at end of accel phase return true; } @@ -70,17 +70,17 @@ TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) { trajStep.Yd = Vi_; trajStep.Ydd = 0.0f; } else if (t < Ta_) { // Accelerating - trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t); - trajStep.Yd = Vi_ + Ar_*t; + trajStep.Y = Xi_ + Vi_ * t + 0.5f * Ar_ * SQ(t); + trajStep.Yd = Vi_ + Ar_ * t; trajStep.Ydd = Ar_; } else if (t < Ta_ + Tv_) { // Coasting - trajStep.Y = yAccel_ + Vr_*(t - Ta_); + trajStep.Y = yAccel_ + Vr_ * (t - Ta_); trajStep.Yd = Vr_; trajStep.Ydd = 0.0f; } else if (t < Tf_) { // Deceleration float td = t - Tf_; - trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); - trajStep.Yd = Dr_*td; + trajStep.Y = Xf_ + 0.5f * Dr_ * SQ(td); + trajStep.Yd = Dr_ * td; trajStep.Ydd = Dr_; } else if (t >= Tf_) { // Final Condition trajStep.Y = Xf_; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 6c343c42..69cba7ed 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -2,13 +2,13 @@ #define _TRAP_TRAJ_H class TrapezoidalTrajectory { -public: + public: struct Config_t { - float vel_limit = 20000.0f; // [count/s] - float accel_limit = 5000.0f; // [count/s^2] - float decel_limit = 5000.0f; // [count/s^2] + float vel_limit = 20000.0f; // [count/s] + float accel_limit = 5000.0f; // [count/s^2] + float decel_limit = 5000.0f; // [count/s^2] }; - + struct Step_t { float Y; float Yd; @@ -23,11 +23,9 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_object("config", - make_protocol_property("vel_limit", &config_.vel_limit), - make_protocol_property("accel_limit", &config_.accel_limit), - make_protocol_property("decel_limit", &config_.decel_limit) - ) - ); + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("decel_limit", &config_.decel_limit))); } Axis* axis_ = nullptr; // set by Axis constructor diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 3278d614..6bf2e167 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -1,10 +1,9 @@ -#include -#include -#include #include +#include +#include #include - +#include int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; @@ -13,30 +12,30 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { if (alpha >= 0.0f) { //quadrant I if (one_by_sqrt3 * beta > alpha) - Sextant = 2; //sextant v2-v3 + Sextant = 2; //sextant v2-v3 else - Sextant = 1; //sextant v1-v2 + Sextant = 1; //sextant v1-v2 } else { //quadrant II if (-one_by_sqrt3 * beta > alpha) - Sextant = 3; //sextant v3-v4 + Sextant = 3; //sextant v3-v4 else - Sextant = 2; //sextant v2-v3 + Sextant = 2; //sextant v2-v3 } } else { if (alpha >= 0.0f) { //quadrant IV if (-one_by_sqrt3 * beta > alpha) - Sextant = 5; //sextant v5-v6 + Sextant = 5; //sextant v5-v6 else - Sextant = 6; //sextant v6-v1 + Sextant = 6; //sextant v6-v1 } else { //quadrant III if (one_by_sqrt3 * beta > alpha) - Sextant = 4; //sextant v4-v5 + Sextant = 4; //sextant v4-v5 else - Sextant = 5; //sextant v5-v6 + Sextant = 5; //sextant v5-v6 } } @@ -116,9 +115,7 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { // if any of the results becomes NaN, result_valid will evaluate to false int result_valid = - *tA >= 0.0f && *tA <= 1.0f - && *tB >= 0.0f && *tB <= 1.0f - && *tC >= 0.0f && *tC <= 1.0f; + *tA >= 0.0f && *tA <= 1.0f && *tB >= 0.0f && *tB <= 1.0f && *tC >= 0.0f && *tC <= 1.0f; return result_valid ? 0 : -1; } @@ -149,7 +146,7 @@ float fast_atan2(float y, float x) { // Evaluate polynomials using Fused Multiply Add intrisic instruction. // coeffs[0] is highest order, as per numpy.polyfit // p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg" -float horner_fma(float x, const float *coeffs, size_t count) { +float horner_fma(float x, const float* coeffs, size_t count) { float result = 0.0f; for (int idx = 0; idx < count; ++idx) result = fmaf(result, x, coeffs[idx]); @@ -157,7 +154,7 @@ float horner_fma(float x, const float *coeffs, size_t count) { } // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271 -int mod(int dividend, int divisor){ +int mod(int dividend, int divisor) { int r = dividend % divisor; return (r < 0) ? (r + divisor) : r; } @@ -166,7 +163,7 @@ int mod(int dividend, int divisor){ // If the deadline has already passed, the return value is 0 (except if // the deadline is very far in the past) uint32_t deadline_to_timeout(uint32_t deadline_ms) { - uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency); + uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency); uint32_t timeout_ms = deadline_ms - now_ms; return (timeout_ms & 0x80000000) ? 0 : timeout_ms; } @@ -188,18 +185,17 @@ int is_in_the_future(uint32_t time_ms) { uint32_t micros(void) { register uint32_t ms, cycle_cnt; do { - ms = HAL_GetTick(); + ms = HAL_GetTick(); cycle_cnt = TIM_TIME_BASE->CNT; - } while (ms != HAL_GetTick()); + } while (ms != HAL_GetTick()); return (ms * 1000) + cycle_cnt; } // @brief: Busy wait delay for given amount of microseconds (us) -void delay_us(uint32_t us) -{ +void delay_us(uint32_t us) { uint32_t start = micros(); - while (micros() - start < (uint32_t) us) { + while (micros() - start < (uint32_t)us) { __ASM("nop"); } } diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 3145c19a..bd847d7a 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -6,8 +6,8 @@ extern "C" { #endif -#include #include +#include /** * @brief Flash size register address @@ -67,7 +67,7 @@ extern "C" { static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; -static const float sqrt3_by_2 = 0.86602540378f; +static const float sqrt3_by_2 = 0.86602540378f; //beware of inserting large values! static inline float wrap_pm(float x, float pm_range) { @@ -93,7 +93,7 @@ static inline float fmodf_pos(float x, float y) { // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 // Returns 0 on success, and -1 if the input was out of range -int SVM(float alpha, float beta, float* tA, float* tB, float* tC); +int SVM(float alpha, float beta, float *tA, float *tB, float *tC); float fast_atan2(float y, float x); float horner_fma(float x, const float *coeffs, size_t count);