From 9486f9ed15c994d704ccd7dd20bfd0560e3e1496 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 13:37:07 -0800 Subject: [PATCH] make stuff working again, improve state machine design, add underscore to member names --- Firmware/Board/v3.3/Src/usbd_cdc_if.c | 2 +- Firmware/MotorControl/axis.cpp | 270 ++++----- Firmware/MotorControl/axis.hpp | 136 +++-- Firmware/MotorControl/commands.cpp | 512 ------------------ Firmware/MotorControl/communication.cpp | 293 ++++++++++ .../{commands.h => communication.h} | 10 - Firmware/MotorControl/controller.cpp | 91 ++-- Firmware/MotorControl/controller.hpp | 58 +- Firmware/MotorControl/encoder.cpp | 132 ++--- Firmware/MotorControl/encoder.hpp | 49 +- Firmware/MotorControl/low_level.cpp | 86 +-- Firmware/MotorControl/low_level.h | 7 - Firmware/MotorControl/main.cpp | 6 +- Firmware/MotorControl/motor.cpp | 169 +++--- Firmware/MotorControl/motor.hpp | 101 ++-- Firmware/MotorControl/odrive_main.hpp | 2 - Firmware/MotorControl/protocol.cpp | 9 +- Firmware/MotorControl/protocol.hpp | 34 +- .../MotorControl/sensorless_estimator.cpp | 48 +- .../MotorControl/sensorless_estimator.hpp | 24 +- Firmware/Tupfile.lua | 2 +- 21 files changed, 972 insertions(+), 1069 deletions(-) delete mode 100644 Firmware/MotorControl/commands.cpp create mode 100644 Firmware/MotorControl/communication.cpp rename Firmware/MotorControl/{commands.h => communication.h} (81%) diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c index b38385f7..bf94296b 100644 --- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c @@ -52,7 +52,7 @@ #include "cmsis_os.h" #include "freertos_vars.h" #include "utils.h" -#include "commands.h" +#include "communication.h" #include /* USER CODE END INCLUDE */ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f23cc91f..dd14249f 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -12,24 +12,24 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, SensorlessEstimator& sensorless_estimator, Controller& controller, Motor& motor) - : hw_config(hw_config), - config(config), - encoder(encoder), - sensorless_estimator(sensorless_estimator), - controller(controller), - motor(motor) + : hw_config_(hw_config), + config_(config), + encoder_(encoder), + sensorless_estimator_(sensorless_estimator), + controller_(controller), + motor_(motor) { - encoder.axis = this; - sensorless_estimator.axis = this; - controller.axis = this; - motor.axis = this; + encoder_.axis_ = this; + sensorless_estimator_.axis_ = this; + controller_.axis_ = this; + motor_.axis_ = this; } // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { - encoder.setup(); - motor.setup(); + encoder_.setup(); + motor_.setup(); } static void run_state_machine_loop_wrapper(void* ctx) { @@ -38,16 +38,24 @@ 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, 512); - thread_id = osThreadCreate(osThread(thread_def), this); - thread_id_valid = true; + osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 512); + thread_id_ = osThreadCreate(osThread(thread_def), this); + thread_id_valid_ = true; } // @brief Unblocks the control loop thread. // This is called from the current sense interrupt handler. -void Axis::signal_thread(thread_signals sig) { - if (thread_id_valid) - osSignalSet(thread_id, sig); +void Axis::signal_current_meas() { + if (thread_id_valid_) + osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS); +} + +// @brief Blocks until a current measurement is completed +// @returns True on success, false otherwise +bool Axis::wait_for_current_meas() { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) + return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; + return true; } static void step_cb_wrapper(void* ctx) { @@ -56,10 +64,10 @@ static void step_cb_wrapper(void* ctx) { // step/direction interface void Axis::step_cb() { - if (enable_step_dir) { - GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config.dir_port, hw_config.dir_pin); + if (enable_step_dir_) { + GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin); float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - controller.pos_setpoint += dir * config.counts_per_step; + controller_.pos_setpoint_ += dir * config_.counts_per_step; } }; @@ -68,38 +76,38 @@ void Axis::set_step_dir_enabled(bool enable) { if (enable) { // Set up the direction GPIO as input GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = hw_config.dir_pin; + GPIO_InitStruct.Pin = hw_config_.dir_pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(hw_config.dir_port, &GPIO_InitStruct); + HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct); // Subscribe to rising edges of the step GPIO - GPIO_subscribe(hw_config.step_port, hw_config.step_pin, GPIO_PULLDOWN, + GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN, step_cb_wrapper, this); - enable_step_dir = true; + enable_step_dir_ = true; } else { - enable_step_dir = false; + enable_step_dir_ = false; // Unsubscribe from step GPIO - GPIO_unsubscribe(hw_config.step_port, hw_config.step_pin); + GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin); } } // @brief Returns true if the power supply is within range bool Axis::check_PSU_brownout() { - if(vbus_voltage < config.dc_bus_brownout_trip_level) - return error = ERROR_BAD_VOLTAGE, false; + if(vbus_voltage < config_.dc_bus_brownout_trip_level) + return error_ = ERROR_BAD_VOLTAGE, false; return true; } // @brief Returns true if everything is ok. // Sets error and returns false otherwise. bool Axis::do_checks() { - if (!motor.do_checks()) - return error = ERROR_MOTOR_FAILED, false; + if (!motor_.do_checks()) + return error_ = ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error = ERROR_BAD_VOLTAGE, false; + return error_ = ERROR_BAD_VOLTAGE, false; return true; } @@ -107,74 +115,81 @@ bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; run_control_loop([&](){ - float phase = wrap_pm_pi(config.ramp_up_distance * x); - float I_mag = config.spin_up_current * x; - x += current_meas_period / config.ramp_up_time; - if (!motor.update(I_mag, phase)) - return error = ERROR_MOTOR_FAILED, false; + float phase = wrap_pm_pi(config_.ramp_up_distance * x); + float I_mag = config_.spin_up_current * x; + x += current_meas_period / config_.ramp_up_time; + if (!motor_.update(I_mag, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return x < 1.0f; }); - if (error != ERROR_NO_ERROR) + if (error_ != ERROR_NO_ERROR) return false; // Late Spin-up: accelerate - float vel = config.ramp_up_distance / config.ramp_up_time; - float phase = wrap_pm_pi(config.ramp_up_distance); + float vel = config_.ramp_up_distance / config_.ramp_up_time; + float phase = wrap_pm_pi(config_.ramp_up_distance); run_control_loop([&](){ - vel += config.spin_up_acceleration * current_meas_period; + vel += config_.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - float I_mag = config.spin_up_current; - if (!motor.update(I_mag, phase)) - return error = ERROR_MOTOR_FAILED, false; - return vel < config.spin_up_target_vel; + float I_mag = config_.spin_up_current; + if (!motor_.update(I_mag, phase)) + return error_ = ERROR_MOTOR_FAILED, false; + return vel < config_.spin_up_target_vel; }); - return error == ERROR_NO_ERROR; + return error_ == ERROR_NO_ERROR; } // 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() { + set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ float pos_estimate, vel_estimate, phase, current_setpoint; - if (controller.config.control_mode >= CTRL_MODE_POSITION_CONTROL) - return error = ERROR_POS_CTRL_DURING_SENSORLESS, false; + if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) + return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false; // We update the encoder just in case someone needs the output for testing - encoder.update(nullptr, nullptr, nullptr); - if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase)) - return error = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; - if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error = ERROR_CONTROLLER_FAILED, false; - if (!motor.update(current_setpoint, phase)) - return error = ERROR_MOTOR_FAILED, false; + encoder_.update(nullptr, nullptr, nullptr); + if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase)) + return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; + if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return error_ = ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return true; }); - return error == ERROR_NO_ERROR; + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; } bool Axis::run_closed_loop_control_loop() { + set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ float pos_estimate, vel_estimate, phase, current_setpoint; // We update the sensorless estimator just in case someone needs the output for testing - sensorless_estimator.update(nullptr, nullptr, nullptr); - if (!encoder.update(&pos_estimate, &vel_estimate, &phase)) - return error = ERROR_ENCODER_FAILED, false; - if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error = ERROR_CONTROLLER_FAILED, false; - if (!motor.update(current_setpoint, phase)) - return error = ERROR_MOTOR_FAILED, false; + sensorless_estimator_.update(nullptr, nullptr, nullptr); + if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) + return error_ = ERROR_ENCODER_FAILED, false; + if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return error_ = ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return true; }); - return error == ERROR_NO_ERROR; + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; } bool Axis::run_idle_loop() { - while (requested_state == AXIS_STATE_DONT_CARE) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) - return error = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; - } - return error == ERROR_NO_ERROR; + // run_control_loop ignores missed modulation timing updates + // if and only if we're in AXIS_STATE_IDLE + run_control_loop([this](){ + sensorless_estimator_.update(nullptr, nullptr, nullptr); + encoder_.update(nullptr, nullptr, nullptr); + return true; + }); + return error_ == ERROR_NO_ERROR; } // Infinite loop that does calibration and enters main control loop as appropriate @@ -183,84 +198,91 @@ void Axis::run_state_machine_loop() { // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f // TODO: Move this somewhere else // TODO: respect changes of CPR - int encoder_cpr = encoder.config.cpr; - controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); - if (controller.anticogging.cogging_map != NULL) { + int encoder_cpr = encoder_.config_.cpr; + controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); + if (controller_.anticogging_.cogging_map != NULL) { for (int i = 0; i < encoder_cpr; i++) { - controller.anticogging.cogging_map[i] = 0.0f; + controller_.anticogging_.cogging_map[i] = 0.0f; } } - current_state = AXIS_STATE_MOTOR_CALIBRATION; - bool force_state = false; + // arm! + motor_.arm(); for (;;) { - AxisState_t next_state = AXIS_STATE_DONT_CARE; - - switch (current_state) { - - case AXIS_STATE_MOTOR_CALIBRATION: - { - bool skip = !force_state && !config.enable_motor_calibration; - if (skip || motor.run_calibration()) { - next_state = AXIS_STATE_ENCODER_CALIBRATION; - } else { - next_state = AXIS_STATE_IDLE; - } + // Load the task chain if a specific request is pending + if (requested_state_ != AXIS_STATE_UNDEFINED) { + size_t pos = 0; + if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) { + if (config_.startup_motor_calibration) + task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; + if (config_.startup_encoder_calibration) + task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (config_.startup_closed_loop_control) + task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_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) { + task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; + task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + task_chain_[pos++] = AXIS_STATE_IDLE; + } else if (requested_state_ != AXIS_STATE_UNDEFINED) { + task_chain_[pos++] = requested_state_; + task_chain_[pos++] = AXIS_STATE_IDLE; } + task_chain_[pos++] = AXIS_STATE_UNDEFINED; + // TODO: bounds checking + requested_state_ = AXIS_STATE_UNDEFINED; + } + + // Note that current_state is a reference to task_chain_[0] + + // Validate the state before running it + if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) + current_state_ = AXIS_STATE_UNDEFINED; + if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_) + current_state_ = AXIS_STATE_UNDEFINED; + + // Run the specified state + // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED + bool status; + switch (current_state_) { + case AXIS_STATE_MOTOR_CALIBRATION: + status = motor_.run_calibration(); break; case AXIS_STATE_ENCODER_CALIBRATION: - { - bool skip = !force_state && !config.enable_encoder_calibration; - if (skip || encoder.run_calibration()) { - next_state = config.enable_closed_loop_control ? - AXIS_STATE_CLOSED_LOOP_CONTROL : - config.enable_sensorless_control ? - AXIS_STATE_SENSORLESS_SPINUP : - AXIS_STATE_IDLE; - if (next_state != AXIS_STATE_IDLE) - set_step_dir_enabled(config.enable_step_dir); - } else { - next_state = AXIS_STATE_IDLE; - } - } - break; - - case AXIS_STATE_SENSORLESS_SPINUP: - if (run_sensorless_spin_up()) { - next_state = AXIS_STATE_SENSORLESS_CONTROL; - } else { - next_state = AXIS_STATE_IDLE; - } + status = encoder_.run_calibration(); break; case AXIS_STATE_SENSORLESS_CONTROL: - run_sensorless_control_loop(); - next_state = AXIS_STATE_IDLE; // TODO: restart if desired + status = run_sensorless_spin_up(); // TODO: restart if desired + if (status) + status = run_sensorless_control_loop(); break; case AXIS_STATE_CLOSED_LOOP_CONTROL: - run_closed_loop_control_loop(); - next_state = AXIS_STATE_IDLE; + status = run_closed_loop_control_loop(); break; case AXIS_STATE_IDLE: - default: - current_state = AXIS_STATE_IDLE; run_idle_loop(); + status = motor_.arm(); // done with idling - try to arm the motor + break; + + default: + error_ = ERROR_INVALID_STATE; + status = false; // this will set the state to idle break; } - if (requested_state != AXIS_STATE_DONT_CARE) { - current_state = requested_state; - requested_state = AXIS_STATE_DONT_CARE; - force_state = true; - } else { - current_state = next_state; - force_state = false; - } + // If the state failed, go to idle, else advance task chain + if (!status) + current_state_ = AXIS_STATE_IDLE; + else + memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0])); } - thread_id_valid = false; + thread_id_valid_ = false; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index ff52a51b..6f401154 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,23 +5,24 @@ #error "This file should not be included directly. Include odrive_main.hpp instead." #endif - +// Warning: Do not reorder these enum values. +// The state machine uses ">" comparision on them. enum AxisState_t { - AXIS_STATE_STARTUP, - AXIS_STATE_MOTOR_CALIBRATION, - AXIS_STATE_ENCODER_CALIBRATION, - AXIS_STATE_SENSORLESS_SPINUP, - AXIS_STATE_SENSORLESS_CONTROL, - AXIS_STATE_CLOSED_LOOP_CONTROL, - AXIS_STATE_IDLE, - AXIS_STATE_DONT_CARE // used to indicate that no state request is pending + AXIS_STATE_UNDEFINED, // void run_control_loop(const T& update_handler) { - motor.arm(); - while (requested_state == AXIS_STATE_DONT_CARE - && error == ERROR_NO_ERROR /* error may be set by interrupt handler */ ) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - error = ERROR_CURRENT_MEASUREMENT_TIMEOUT; + while (requested_state_ == AXIS_STATE_UNDEFINED) { + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ = ERROR_MOTOR_FAILED; + break; + } + if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) { + error_ = ERROR_CONTROL_LOOP_TIMEOUT; break; } - - // Proactively set phase voltages to 0. If the control deadline is missed, - // the voltages will go to zero. - motor.enqueue_voltage_timings(0.0f, 0.0f); if (!do_checks()) // error set during function call break; @@ -111,21 +113,15 @@ public: if (!update_handler()) // error set during function call break; - update_brake_current(); - // Check we meet deadlines after queueing - motor.last_cpu_time = motor.check_timing(); - if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) { - error = ERROR_CONTROL_LOOP_TIMEOUT; + ++loop_counter_; + + // Wait until the current measurement interrupt fires + if (!wait_for_current_meas()) { // error set by function call + motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases break; } - ++loop_counter; } - - // We are exiting control: disarm motor, reset Ibus, and update brake current - motor.disarm(); - motor.current_control.Ibus = 0.0f; - update_brake_current(); } bool run_sensorless_spin_up(); @@ -135,23 +131,57 @@ public: void run_state_machine_loop(); - const AxisHardwareConfig_t& hw_config; - AxisConfig_t& config; + const AxisHardwareConfig_t& hw_config_; + AxisConfig_t& config_; - Encoder& encoder; - SensorlessEstimator& sensorless_estimator; - Controller& controller; - Motor& motor; + Encoder& encoder_; + SensorlessEstimator& sensorless_estimator_; + Controller& controller_; + Motor& motor_; - osThreadId thread_id; - volatile bool thread_id_valid = false; + osThreadId thread_id_; + volatile bool thread_id_valid_ = false; // variables exposed on protocol - Error_t error = ERROR_NO_ERROR; - bool enable_step_dir = false; // auto enabled after calibration, based on config.enable_step_dir - AxisState_t current_state = AXIS_STATE_STARTUP; - AxisState_t requested_state = AXIS_STATE_DONT_CARE; - uint32_t loop_counter = 0; + Error_t error_ = ERROR_NO_ERROR; + bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler + // whenever there's no active control loop that + // sets the timings. The flag must be explicitly + // cleared by a call to motors.arm(). + bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir + AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; + AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; + AxisState_t& current_state_ = task_chain_[0]; + uint32_t loop_counter_ = 0; + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_ro_property("error", &error_), + make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_), + make_protocol_property("enable_step_dir", &enable_step_dir_), + make_protocol_ro_property("current_state", ¤t_state_), + make_protocol_property("requested_state", &requested_state_), + make_protocol_ro_property("loop_counter", &loop_counter_), + make_protocol_object("config", + make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), + make_protocol_property("startup_encoder_calibration", &config_.startup_encoder_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("enable_step_dir", &config_.enable_step_dir), + make_protocol_property("counts_per_step", &config_.counts_per_step), + make_protocol_property("dc_bus_brownout_trip_level", &config_.dc_bus_brownout_trip_level), + make_protocol_property("ramp_up_time", &config_.ramp_up_time), + make_protocol_property("ramp_up_distance", &config_.ramp_up_distance), + make_protocol_property("spin_up_current", &config_.spin_up_current), + make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration), + make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel) + ), + make_protocol_object("motor", motor_.make_protocol_definitions()), + make_protocol_object("controller", controller_.make_protocol_definitions()), + make_protocol_object("encoder", encoder_.make_protocol_definitions()) + ); + } }; #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp deleted file mode 100644 index 727fea65..00000000 --- a/Firmware/MotorControl/commands.cpp +++ /dev/null @@ -1,512 +0,0 @@ -#if 0 -/* Includes ------------------------------------------------------------------*/ - -// TODO: remove this option -// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py -// todo: make clean switches for protocol -#define ENABLE_LEGACY_PROTOCOL - -#include "commands.h" -#include "low_level.h" -#include "odrive_main.hpp" -#include "protocol.hpp" -#include "freertos_vars.h" -#include "utils.h" -#include "config.h" - -#ifdef ENABLE_LEGACY_PROTOCOL -#include "legacy_commands.h" -#endif - -#include -#include -#include -#include -#include -#include - -#define UART_TX_BUFFER_SIZE 64 - -/* Private defines -----------------------------------------------------------*/ -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ - -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern USBD_HandleTypeDef hUsbDeviceFS; - -/* Private constant data -----------------------------------------------------*/ -// TODO: make command to switch gpio_mode during run-time -#if defined(USE_GPIO_MODE_STEP_DIR) -static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir -#elif !defined(UART_PROTOCOL_NONE) -static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx -#else -static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured -#endif - -/* Private variables ---------------------------------------------------------*/ - -static uint8_t* usb_buf; -static uint32_t usb_len; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; - -/* Variables exposed to USB & UART via read/write commands */ -// TODO: include range information in JSON description - - -// TODO: Autogenerate these functions -void motors_0_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[0], - motors[0].set_pos_setpoint_args.pos_setpoint, - motors[0].set_pos_setpoint_args.vel_feed_forward, - motors[0].set_pos_setpoint_args.current_feed_forward); -} -void motors_0_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[0], - motors[0].set_vel_setpoint_args.vel_setpoint, - motors[0].set_vel_setpoint_args.current_feed_forward); -} -void motors_0_set_current_setpoint_func(void) { - set_current_setpoint(&motors[0], - motors[0].set_current_setpoint_args.current_setpoint); -} -void motors_1_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[1], - motors[1].set_pos_setpoint_args.pos_setpoint, - motors[1].set_pos_setpoint_args.vel_feed_forward, - motors[1].set_pos_setpoint_args.current_feed_forward); -} -void motors_1_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[1], - motors[1].set_vel_setpoint_args.vel_setpoint, - motors[1].set_vel_setpoint_args.current_feed_forward); -} -void motors_1_set_current_setpoint_func(void) { - set_current_setpoint(&motors[1], - motors[1].set_current_setpoint_args.current_setpoint); -} -void motors_run_anticogging_calibration_func() { - for (uint8_t i = 0; i < num_motors; i++) { - // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) { - motors[i].anticogging.calib_anticogging = true; - } - } -} - -// This table specifies which fields and functions are exposed on the USB and UART ports. -// TODO: Autogenerate this table. It will come up again very soon in the Arduino library. -// clang-format off -const Endpoint endpoints[] = { - Endpoint::make_property("vbus_voltage", const_cast(&vbus_voltage)), - Endpoint::make_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), - Endpoint::make_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), - Endpoint::make_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), - Endpoint::make_function("run_anticogging_calibration", &motors_run_anticogging_calibration_func), - // No parameters, but still requires a close_tree() - Endpoint::close_tree(), - Endpoint::make_object("config"), - Endpoint::make_property("brake_resistance", &brake_resistance), - Endpoint::close_tree(), - Endpoint::make_object("axis0"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control", &axis_configs[0].enable_control_at_start), - Endpoint::make_property("do_calibration", &axis_configs[0].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor0"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast(&motors[0].control_mode)), - Endpoint::make_property("counts_per_step", &motors[0].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[0].pole_pairs), - Endpoint::make_property("pos_gain", &motors[0].pos_gain), - Endpoint::make_property("vel_gain", &motors[0].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[0].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[0].vel_limit), - Endpoint::make_property("calibration_current", &motors[0].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[0].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[0].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[0].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast(&motors[0].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast(&motors[0].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast(&motors[0].error)), - Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[0].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[0].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[0].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[0].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain), - Endpoint::make_property("thread_id_valid", &motors[0].thread_id_valid), - Endpoint::make_property("control_deadline", &motors[0].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[0].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[0].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[0].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[0].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[0].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast(&motors[0].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[0].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[0].encoder.use_index), - Endpoint::make_property("calibrated", &motors[0].encoder.calibrated), - Endpoint::make_property("idx_search_speed", &motors[0].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[0].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[0].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor0 - Endpoint::make_object("axis1"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control", &axis_configs[1].enable_control_at_start), - Endpoint::make_property("do_calibration", &axis_configs[1].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor1"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast(&motors[1].control_mode)), - Endpoint::make_property("counts_per_step", &motors[1].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[1].pole_pairs), - Endpoint::make_property("pos_gain", &motors[1].pos_gain), - Endpoint::make_property("vel_gain", &motors[1].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[1].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[1].vel_limit), - Endpoint::make_property("calibration_current", &motors[1].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[1].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[1].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[1].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast(&motors[1].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast(&motors[1].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast(&motors[1].error)), - Endpoint::make_property("pos_setpoint", &motors[1].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[1].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[1].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[1].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[1].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[1].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain), - Endpoint::make_property("thread_id_valid", &motors[1].thread_id_valid), - Endpoint::make_property("control_deadline", &motors[1].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[1].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[1].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[1].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[1].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[1].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast(&motors[1].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[1].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[1].encoder.use_index), - Endpoint::make_property("calibrated", &motors[1].encoder.calibrated), - Endpoint::make_property("idx_search_speed", &motors[1].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[1].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[1].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[1].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[1].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_1_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[1].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[1].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor1 - Endpoint::make_function("save_configuration", &save_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("erase_configuration", &erase_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("reboot", &NVIC_SystemReset), - // no arguments - Endpoint::close_tree() -}; -// clang-format on - -constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); - - -#if defined(USB_PROTOCOL_NATIVE) - -class USBSender : public PacketSink { -public: - int process_packet(const uint8_t* buffer, size_t length) { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit packet - uint8_t status = CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length); - return (status == USBD_OK) ? 0 : -1; - } -} usb_sender; - -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); - -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -class USBSender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - if (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, chunk) != USBD_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -} usb_sender; - -PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_channel); - -#endif - -#if defined(UART_PROTOCOL_NATIVE) -class UART4Sender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_sender; - -PacketToStreamConverter uart4_packet_sender(uart4_sender); -BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_channel); -#endif - -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -void init_communication(void) { - switch (gpio_mode) { - case GPIO_MODE_NONE: - break; //do nothing - case GPIO_MODE_UART: { -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 - SetGPIO12toUART(); -#endif - } break; - case GPIO_MODE_STEP_DIR: { - SetGPIO12toStepDir(); - } break; - default: - //TODO: report error unexpected mode - break; - } -} - -// Thread to handle deffered processing of USB interrupt, and -// read commands out of the UART DMA circular buffer -void communication_task(void const * argument) { - (void) argument; - - -#if !defined(UART_PROTOCOL_NONE) - //DMA open loop continous circular buffer - //1ms delay periodic, chase DMA ptr around - - #define UART_RX_BUFFER_SIZE 64 - static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; - - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; -#endif - - // Re-run state-machine forever - for (;;) { -#if !defined(UART_PROTOCOL_NONE) - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(UART_PROTOCOL_NATIVE) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#elif defined(UART_PROTOCOL_LEGACY) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#endif -#endif - -#if !defined(USB_PROTOCOL_NONE) - // When we reach here, we are out of immediate characters to fetch out of UART buffer - // Now we check if there is any USB processing to do: we wait for up to 1 ms, - // before going back to checking UART again. - const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); - if (sem_stat == osOK) { - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(USB_PROTOCOL_NATIVE) - usb_channel.process_packet(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_stream_sink.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_LEGACY) - legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); -#endif - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet - } -#endif - } - - // If we get here, then this task is done - vTaskDelete(osThreadGetId()); -} - -// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the -// incoming USB data -void set_cmd_buffer(uint8_t *buf, uint32_t len) { - usb_buf = buf; - usb_len = len; -} - -void usb_update_thread() { - for (;;) { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - } - - vTaskDelete(osThreadGetId()); -} -#endif \ No newline at end of file diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp new file mode 100644 index 00000000..2a082459 --- /dev/null +++ b/Firmware/MotorControl/communication.cpp @@ -0,0 +1,293 @@ + +/* Includes ------------------------------------------------------------------*/ + +// TODO: remove this option +// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py +// todo: make clean switches for protocol +#define ENABLE_LEGACY_PROTOCOL + +#include "communication.h" +//#include "low_level.h" +#include "odrive_main.hpp" +#include "protocol.hpp" +#include "freertos_vars.h" +#include "utils.h" + +#ifdef ENABLE_LEGACY_PROTOCOL +#include "legacy_commands.h" +#endif + +#include +#include +#include +#include +#include +#include + +#define UART_TX_BUFFER_SIZE 64 + +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ + +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; +extern USBD_HandleTypeDef hUsbDeviceFS; + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ + +static uint8_t* usb_buf; +static uint32_t usb_len; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + + +#if defined(USB_PROTOCOL_NATIVE) + +class USBSender : public PacketSink { +public: + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit packet + uint8_t status = CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, length); + return (status == USBD_OK) ? 0 : -1; + } +} usb_sender; + +BidirectionalPacketBasedChannel usb_channel(usb_sender); + +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + +class USBSender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + if (CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, chunk) != USBD_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +} usb_sender; + +PacketToStreamConverter usb_packet_sender(usb_sender); +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); +StreamToPacketConverter usb_stream_sink(usb_channel); + +#endif + +#if defined(UART_PROTOCOL_NATIVE) +class UART4Sender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_sender; + +PacketToStreamConverter uart4_packet_sender(uart4_sender); +BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); +StreamToPacketConverter UART4_stream_sink(uart4_channel); +#endif + + +class test_class { +public: + uint32_t property1; + float property2; + + float set_both(uint32_t arg1, float arg2) { + printf("set_both called with %u and %.3f\n", (unsigned int)arg1, arg2); + property1 = arg1; + property2 = arg2; + return arg1 + arg2; + } +}; + +float bla; + +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void init_communication(void) { + printf("hi!\r\n"); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 4*512); + thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); +} + + + +static auto make_obj_tree() { + return make_protocol_member_list( + make_protocol_property("bla2", &bla), + make_protocol_object("axis0", axes[0]->make_protocol_definitions()), + make_protocol_object("axis1", axes[1]->make_protocol_definitions()) + ); +} + +using tree_type = decltype(make_obj_tree()); +uint8_t tree_buffer[sizeof(tree_type)]; + +// the protocol has one additional built-in endpoint +constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1; +Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; +const size_t max_endpoints_ = MAX_ENDPOINTS; +size_t n_endpoints_ = 0; + +// Thread to handle deffered processing of USB interrupt, and +// read commands out of the UART DMA circular buffer +void communication_task(void * ctx) { + (void) ctx; // unused parameter + + auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); + auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); + set_application_endpoints(&endpoint_provider); + +#if !defined(UART_PROTOCOL_NONE) + //DMA open loop continous circular buffer + //1ms delay periodic, chase DMA ptr around + + #define UART_RX_BUFFER_SIZE 64 + static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; + + // DMA is set up to recieve in a circular buffer forever. + // We dont use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); + uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; +#endif + + // Re-run state-machine forever + for (;;) { +#if !defined(UART_PROTOCOL_NONE) + // Check for UART errors and restart recieve DMA transfer if required + if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { + HAL_UART_AbortReceive(&huart4); + HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(UART_PROTOCOL_NATIVE) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#elif defined(UART_PROTOCOL_LEGACY) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#endif +#endif + +#if !defined(USB_PROTOCOL_NONE) + // When we reach here, we are out of immediate characters to fetch out of UART buffer + // Now we check if there is any USB processing to do: we wait for up to 1 ms, + // before going back to checking UART again. + const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + if (sem_stat == osOK) { + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(USB_PROTOCOL_NATIVE) + usb_channel.process_packet(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + usb_stream_sink.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_LEGACY) + legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + } +#endif + +#if defined(USB_PROTOCOL_NONE) && defined(UART_PROTOCOL_NONE) + osDelay(1); // don't starve other threads +#endif + } + + // If we get here, then this task is done + vTaskDelete(osThreadGetId()); +} + +// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the +// incoming USB data +void set_cmd_buffer(uint8_t *buf, uint32_t len) { + usb_buf = buf; + usb_len = len; +} + +void usb_update_thread(void * ctx) { + (void) ctx; // unused parameter + + for (;;) { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } + + vTaskDelete(osThreadGetId()); +} diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/communication.h similarity index 81% rename from Firmware/MotorControl/commands.h rename to Firmware/MotorControl/communication.h index 2c82ad88..f02d1ae8 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/communication.h @@ -21,16 +21,6 @@ // #define UART_PROTOCOL_LEGACY #define UART_PROTOCOL_NONE -// Use GPIO 1/2 for step/dir input instead of UART -// #define USE_GPIO_MODE_STEP_DIR - - -typedef enum { - GPIO_MODE_NONE, - GPIO_MODE_UART, - GPIO_MODE_STEP_DIR, -} GpioMode_t; - extern "C" { #endif diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index b3ccd70c..c20cf504 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -3,7 +3,7 @@ Controller::Controller(ControllerConfig_t& config) : - config(config) + config_(config) {} //-------------------------------- @@ -11,32 +11,39 @@ Controller::Controller(ControllerConfig_t& config) : //-------------------------------- void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - pos_setpoint = pos_setpoint; - vel_setpoint = vel_feed_forward; - current_setpoint = current_feed_forward; - config.control_mode = CTRL_MODE_POSITION_CONTROL; + pos_setpoint_ = pos_setpoint; + vel_setpoint_ = vel_feed_forward; + current_setpoint_ = current_feed_forward; + config_.control_mode = CTRL_MODE_POSITION_CONTROL; #ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); + printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_); #endif } void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) { - vel_setpoint = vel_setpoint; - current_setpoint = current_feed_forward; - config.control_mode = CTRL_MODE_VELOCITY_CONTROL; + vel_setpoint_ = vel_setpoint; + current_setpoint_ = current_feed_forward; + config_.control_mode = CTRL_MODE_VELOCITY_CONTROL; #ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); + printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_); #endif } void Controller::set_current_setpoint(float current_setpoint) { - current_setpoint = current_setpoint; - config.control_mode = CTRL_MODE_CURRENT_CONTROL; + current_setpoint_ = current_setpoint; + config_.control_mode = CTRL_MODE_CURRENT_CONTROL; #ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); + printf("CURRENT_CONTROL %3.3f\n", current_setpoint_); #endif } +void Controller::start_anticogging_calibration() { + // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating + if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) { + anticogging_.calib_anticogging = true; + } +} + /* * This anti-cogging implementation iterates through each encoder position, * waits for zero velocity & position error, @@ -44,21 +51,21 @@ void Controller::set_current_setpoint(float current_setpoint) { * * This holding current is added as a feedforward term in the control loop. */ -bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) { - if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) { - float pos_err = anticogging.index - pos_estimate; - if (fabsf(pos_err) <= anticogging.calib_pos_threshold && - fabsf(vel_estimate) < anticogging.calib_vel_threshold) { - anticogging.cogging_map[anticogging.index++] = vel_integrator_current; +bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) { + if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) { + float pos_err = anticogging_.index - pos_estimate; + if (fabsf(pos_err) <= anticogging_.calib_pos_threshold && + fabsf(vel_estimate) < anticogging_.calib_vel_threshold) { + anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_; } - if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR - set_pos_setpoint(anticogging.index, 0.0f, 0.0f); + if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR + set_pos_setpoint(anticogging_.index, 0.0f, 0.0f); return false; } else { - anticogging.index = 0; + anticogging_.index = 0; set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home - anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - anticogging.calib_anticogging = false; + anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging + anticogging_.calib_anticogging = false; return true; } } @@ -66,42 +73,42 @@ bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate } bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { - // Only runs if anticogging.calib_anticogging is true; non-blocking - anti_cogging_calibration(pos_estimate, vel_estimate); + // Only runs if anticogging_.calib_anticogging is true; non-blocking + anticogging_calibration(pos_estimate, vel_estimate); // Position control // TODO Decide if we want to use encoder or pll position here - float vel_des = vel_setpoint; - if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) { - float pos_err = pos_setpoint - pos_estimate; - vel_des += config.pos_gain * pos_err; + float vel_des = vel_setpoint_; + if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { + float pos_err = pos_setpoint_ - pos_estimate; + vel_des += config_.pos_gain * pos_err; } // Velocity limiting - float vel_lim = config.vel_limit; + float vel_lim = config_.vel_limit; if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control - float Iq = current_setpoint; + float Iq = current_setpoint_; // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (anticogging.use_anticogging) { - Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)]; + if (anticogging_.use_anticogging) { + Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; - if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += config.vel_gain * v_err; + if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += config_.vel_gain * v_err; } // Velocity integral action before limiting - Iq += vel_integrator_current; + Iq += vel_integrator_current_; // Current limiting - float Ilim = std::min(axis->motor.config.current_lim, axis->motor.current_control.max_allowed_current); + float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; @@ -113,15 +120,15 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s } // Velocity integrator (behaviour dependent on limiting) - if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) { + if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use - vel_integrator_current = 0.0f; + vel_integrator_current_ = 0.0f; } else { if (limited) { // TODO make decayfactor configurable - vel_integrator_current *= 0.99f; + vel_integrator_current_ *= 0.99f; } else { - vel_integrator_current += (config.vel_integrator_gain * current_meas_period) * v_err; + vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err; } } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index df6e22e9..b5767b6f 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -30,14 +30,15 @@ public: void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); - + // TODO: make this more similar to other calibration loops - bool anti_cogging_calibration(float pos_estimate, float vel_estimate); + void start_anticogging_calibration(); + bool anticogging_calibration(float pos_estimate, float vel_estimate); bool update(float pos_estimate, float vel_estimate, float* current_setpoint); - ControllerConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + ControllerConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: // - expose selected (all?) variables on protocol @@ -53,7 +54,7 @@ public: float calib_pos_threshold; float calib_vel_threshold; } Anticogging_t; - Anticogging_t anticogging = { + Anticogging_t anticogging_ = { .index = 0, .cogging_map = nullptr, .use_anticogging = false, @@ -63,25 +64,38 @@ public: }; // variables exposed on protocol - float pos_setpoint = 0.0f; - float vel_setpoint = 0.0f; + float pos_setpoint_ = 0.0f; + float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; - float vel_integrator_current = 0.0f; // [A] - float current_setpoint = 0.0f; // [A] + float vel_integrator_current_ = 0.0f; // [A] + float current_setpoint_ = 0.0f; // [A] - // Cache for remote procedure calls arguments TODO: remove - struct { - float pos_setpoint; - float vel_feed_forward; - float current_feed_forward; - } set_pos_setpoint_args; - struct { - float vel_setpoint; - float current_feed_forward; - } set_vel_setpoint_args; - struct { - float current_setpoint; - } set_current_setpoint_args; + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("pos_setpoint", &pos_setpoint_), + make_protocol_property("vel_setpoint", &vel_setpoint_), + make_protocol_property("vel_integrator_current", &vel_integrator_current_), + make_protocol_property("current_setpoint", ¤t_setpoint_), + make_protocol_object("config", + make_protocol_property("control_mode", &config_.control_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_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, + "pos_setpoint", + "vel_feed_forward", + "current_feed_forward"), + make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, + "vel_setpoint", + "current_feed_forward"), + make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, + "current_setpoint"), + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + ); + } }; #endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index f4d81da7..7bfd7123 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -5,16 +5,16 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, EncoderConfig_t& config) : - hw_config(hw_config), - config(config) + hw_config_(hw_config), + config_(config) { // Calculate encoder pll gains // This calculation is currently identical to the PLL in SensorlessEstimator float pll_bandwidth = 1000.0f; // [rad/s] - pll_kp = 2.0f * pll_bandwidth; + pll_kp_ = 2.0f * pll_bandwidth; // Critically damped - pll_ki = 0.25f * (pll_kp * pll_kp); + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); } static void enc_index_cb_wrapper(void* ctx) { @@ -22,8 +22,8 @@ static void enc_index_cb_wrapper(void* ctx) { } void Encoder::setup() { - HAL_TIM_Encoder_Start(hw_config.timer, TIM_CHANNEL_ALL); - GPIO_subscribe(hw_config.index_port, hw_config.index_pin, GPIO_NOPULL, + HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL); + GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL, enc_index_cb_wrapper, this); } @@ -33,22 +33,22 @@ void Encoder::setup() { // Triggered when an encoder passes over the "Index" pin // TODO: only arm index edge interrupt when we know encoder has powered up -// TODO: disarm interrupt once we found the index +// TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { - if (!index_found) { + if (!index_found_) { set_count(0); - index_found = true; + index_found_ = true; } } // Function that sets the current encoder count to a desired 32-bit value. -void Encoder::set_count(uint32_t count) { +void Encoder::set_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); - state = count; - hw_config.timer->Instance->CNT = count; - pll_pos = (float)count; + state_ = count; + hw_config_.timer->Instance->CNT = count; + pll_pos_ = (float)count; __set_PRIMASK(prim); } @@ -59,109 +59,109 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { static const float start_lock_duration = 1.0f; static const float scan_omega = 4.0f * M_PI; static const float scan_distance = 16.0f * M_PI; - static const size_t num_steps = scan_distance / scan_omega * current_meas_hz; + static const int num_steps = scan_distance / scan_omega * current_meas_hz; // go to motor zero phase for start_lock_duration to get ready to scan - size_t i = 0; - axis->run_control_loop([&](){ - axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f); + int i = 0; + axis_->run_control_loop([&](){ + axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f); return ++i < start_lock_duration * current_meas_hz; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT; + int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT; int64_t encvaluesum = 0; // scan forward i = 0; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; return ++i < num_steps; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //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 elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = scan_distance / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) + float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error = ERROR_CPR_OUT_OF_RANGE; + error_ = ERROR_CPR_OUT_OF_RANGE; return false; } // check direction - if ((int16_t)hw_config.timer->Instance->CNT > init_enc_val + 8) { + if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) { // motor same dir as encoder - axis->motor.config.direction = 1; - } else if ((int16_t)hw_config.timer->Instance->CNT < init_enc_val - 8) { + axis_->motor_.config_.direction = 1; + } else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) { // motor opposite dir as encoder - axis->motor.config.direction = -1; + axis_->motor_.config_.direction = -1; } else { // Encoder response error - error = ERROR_RESPONSE; + error_ = ERROR_RESPONSE; return false; } // scan backwards i = 0; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; return ++i < num_steps; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; int offset = encvaluesum / (num_steps * 2); - config.offset = offset; - config.calibrated = true; + config_.offset = offset; + is_calibrated_ = true; return true; } bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { - index_found = false; + index_found_ = false; float phase = 0.0f; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ phase = wrap_pm_pi(phase + omega * current_meas_period); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); // continue until the index is found - return !index_found; + return !index_found_; }); - return axis->error == Axis::ERROR_NO_ERROR; + return axis_->error_ == Axis::ERROR_NO_ERROR; } bool Encoder::run_calibration() { float enc_calibration_voltage; - if (axis->motor.config.motor_type == MOTOR_TYPE_HIGH_CURRENT) - enc_calibration_voltage = axis->motor.config.calibration_current * axis->motor.config.phase_resistance; - else if (axis->motor.config.motor_type == MOTOR_TYPE_GIMBAL) - enc_calibration_voltage = axis->motor.config.calibration_current; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + enc_calibration_voltage = axis_->motor_.config_.calibration_current; else return false; - if (config.use_index && !index_found) + if (config_.use_index && !index_found_) if (!scan_for_enc_idx( - /*(float)(axis->motor.config.direction) * */ config.idx_search_speed, + (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, enc_calibration_voltage)) return false; - if (!config.calibrated) + if (!config_.hand_calibrated) // if (!calib_enc_offset(enc_calibration_voltage)) return false; return true; @@ -169,38 +169,38 @@ bool Encoder::run_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp < 1.0f)) { - error = ERROR_NUMERICAL; + if (!(current_meas_period * pll_kp_ < 1.0f)) { + error_ = ERROR_NUMERICAL; return false; } // update internal encoder state - int16_t delta_enc = (int16_t)hw_config.timer->Instance->CNT - (int16_t)state; - state += (int32_t)delta_enc; + int16_t delta_enc = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)state_; + state_ += (int32_t)delta_enc; // compute electrical phase - int corrected_enc = state % config.cpr; - corrected_enc -= config.offset; - //corrected_enc *= axis->motor.config.direction; TODO: verify if this still works + int corrected_enc = state_ % config_.cpr; + corrected_enc -= config_.offset; + //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works //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 elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float ph = elec_rad_per_enc * (float)corrected_enc; // ph = fmodf(ph, 2*M_PI); - phase = wrap_pm_pi(ph); + phase_ = wrap_pm_pi(ph); // run pll (for now pll is in units of encoder counts) // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? // Predict current pos - pll_pos += current_meas_period * pll_vel; + pll_pos_ += current_meas_period * pll_vel_; // discrete phase detector - float delta_pos = (float)(state - (int32_t)floorf(pll_pos)); + float delta_pos = (float)(state_ - (int32_t)floorf(pll_pos_)); // pll feedback - pll_pos += current_meas_period * pll_kp * delta_pos; - pll_vel += current_meas_period * pll_ki * delta_pos; + pll_pos_ += current_meas_period * pll_kp_ * delta_pos; + pll_vel_ += current_meas_period * pll_ki_ * delta_pos; // Assign output arguments - if (*pos_estimate) *pos_estimate = pll_pos; - if (*vel_estimate) *vel_estimate = pll_vel; - if (*phase_output) *phase_output = phase; + if (pos_estimate) *pos_estimate = pll_pos_; + if (vel_estimate) *vel_estimate = pll_vel_; + if (phase_output) *phase_output = phase_; return true; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 6ab419b8..1082594e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -7,7 +7,7 @@ struct EncoderConfig_t { bool use_index = false; - bool calibrated = false; + bool hand_calibrated = false; float idx_search_speed = 10.0f; // [rad/s electrical] int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; @@ -30,25 +30,48 @@ public: void enc_index_cb(); - void set_count(uint32_t count); + void set_count(int32_t count); bool calib_enc_offset(float voltage_magnitude); bool scan_for_enc_idx(float omega, float voltage_magnitude); bool update(float* pos_estimate, float* vel_estimate, float* phase); bool run_calibration(); - const EncoderHardwareConfig_t& hw_config; - EncoderConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + const EncoderHardwareConfig_t& hw_config_; + EncoderConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor - Error_t error = ERROR_NONE; - volatile bool index_found = false; - int32_t state = 0; - float phase = 0.0f; // [rad] - float pll_pos = 0.0f; // [rad] - float pll_vel = 0.0f; // [rad/s] - float pll_kp = 0.0f; // [rad/s / rad] - float pll_ki = 0.0f; // [(rad/s^2) / rad] + Error_t error_ = ERROR_NONE; + bool index_found_ = false; + bool is_calibrated_ = config_.hand_calibrated; + int32_t state_ = 0; + float phase_ = 0.0f; // [rad] + float pll_pos_ = 0.0f; // [rad] + float pll_vel_ = 0.0f; // [rad/s] + float pll_kp_ = 0.0f; // [rad/s / rad] + float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_object("config", + make_protocol_property("use_index", &config_.use_index), + make_protocol_property("hand_calibrated", &config_.hand_calibrated), + make_protocol_property("idx_search_speed", &config_.idx_search_speed), + make_protocol_property("cpr", &config_.cpr), + make_protocol_property("offset", &config_.offset), + make_protocol_property("calib_range", &config_.calib_range) + ), + make_protocol_property("error", &error_), + make_protocol_ro_property("index_found", const_cast(&index_found_)), + make_protocol_property("state", &state_), + make_protocol_property("phase", &phase_), + make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("pll_kp", &pll_kp_), + make_protocol_property("pll_ki", &pll_ki_) + ); + } }; #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 1dc791e9..5b1b44a5 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -76,20 +76,6 @@ void start_adc_pwm() { HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void halt_motors(Motor::Error_t error) { - // Disable motors NOW! - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor.disarm(); - } - // Set fault codes, etc. - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor.error = error; - axes[i]->error = Axis::ERROR_MOTOR_FAILED; - } - // disable brake resistor - set_brake_current(0.0f); -} - void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM int half_load = TIM_1_8_PERIOD_CLOCKS / 2; @@ -155,6 +141,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } +// @brief Floats ALL phases immediately and sets the brake current to 0. +void disable_all_pwms(Motor::Error_t error) { + // Disable all motors NOW! + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor_.disarm(); + axes[i]->motor_.error_ = error; + } +} + //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -175,7 +170,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Ensure ADCs are expected ones to simplify the logic below if (!(hadc == &hadc2 || hadc == &hadc3)) { - halt_motors(Motor::ERROR_ADC_FAILED); + disable_all_pwms(Motor::ERROR_ADC_FAILED); return; }; @@ -185,27 +180,35 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Axis& axis = injected ? *axes[0] : *axes[1]; Axis& other_axis = injected ? *axes[1] : *axes[0]; - bool counting_down = axis.motor.hw_config.timer->Instance->CR1 & TIM_CR1_DIR; + bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR; bool current_meas_not_DC_CAL = !counting_down; - if (&axis == axes[1] && counting_down) { - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; - other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; - other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; - } - } else if (&axis == axes[0] && !counting_down) { - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; - other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; - other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + bool update_timings = false; + if (hadc == &hadc2) { + if (&axis == axes[1] && counting_down) + update_timings = true; // update timings of M0 + else if (&axis == axes[0] && !counting_down) + update_timings = true; // update timings of M1 + } + + // Load next timings for the motor that we're not currently sampling + if (update_timings) { + if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) { + other_axis.motor_.next_timings_valid_ = false; + other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0]; + other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1]; + other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2]; + __HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs + update_brake_current(); + } else { + // the motor control loop failed to update the timings in time + // we must assume that it died and therefore float all phases + other_axis.motor_.disarm(); } } // Check the timing of the sequencing - axis.motor.check_timing(); + axis.motor_.log_timing(); uint32_t ADCValue; if (injected) { @@ -213,7 +216,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - float current = axis.motor.phase_current_from_adcval(ADCValue); + float current = axis.motor_.phase_current_from_adcval(ADCValue); if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -224,33 +227,32 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // return or continue if (hadc == &hadc2) { - axis.motor.current_meas.phB = current - axis.motor.DC_calib.phB; + axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB; return; } else { - axis.motor.current_meas.phC = current - axis.motor.DC_calib.phC; + axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC; } // Trigger axis thread - axis.signal_thread(Axis::thread_signals::M_SIGNAL_PH_CURRENT_MEAS); + axis.signal_current_meas(); } else { // DC_CAL measurement if (hadc == &hadc2) { - axis.motor.DC_calib.phB += (current - axis.motor.DC_calib.phB) * calib_filter_k; + axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k; } else { - axis.motor.DC_calib.phC += (current - axis.motor.DC_calib.phC) * calib_filter_k; + axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k; } } } +// @brief Sums up the Ibus contribution of each motor and updates the +// brake resistor PWM accordingly. void update_brake_current() { float Ibus_sum = 0.0f; for (size_t i = 0; i < AXIS_COUNT; ++i) { - Ibus_sum += axes[i]->motor.current_control.Ibus; + Ibus_sum += axes[i]->motor_.current_control_.Ibus; } - // Note: set_brake_current will clip negative values to 0.0f - set_brake_current(-Ibus_sum); -} - -void set_brake_current(float brake_current) { + float brake_current = -Ibus_sum; + // Clip negative values to 0.0f if (brake_current < 0.0f) brake_current = 0.0f; float brake_duty = brake_current * brake_resistance / vbus_voltage; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 7654dfaa..3105bae0 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -12,12 +12,6 @@ extern "C" { #include /* Exported types ------------------------------------------------------------*/ - -typedef struct{ - int type; - int index; -} monitoring_slot; - /* Exported constants --------------------------------------------------------*/ /* Exported variables --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ @@ -35,7 +29,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); void update_brake_current(); -void set_brake_current(float brake_current); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6dde3afd..e834b075 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,7 +1,7 @@ #include "odrive_main.hpp" -#include - +#include "nvm_config.hpp" +#include "communication.h" EncoderConfig_t encoder_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; @@ -67,7 +67,7 @@ int odrive_main(void) { // TODO: make dynamically reconfigurable #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (enable_uart) { - axes[0]->config.enable_step_dir = false; + axes[0]->config_.enable_step_dir = false; axes[0]->set_step_dir_enabled(false); SetGPIO12toUART(); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 044f4264..1ef1e9ce 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -9,33 +9,61 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_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, + 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, }) { } -void Motor::arm() { - __HAL_TIM_MOE_ENABLE(hw_config.timer); // enable pwm outputs +// @brief Arms the PWM outputs that belong to this motor. +// +// Note that this does not yet activate the PWM outputs, it just unlocks them. +// +// While the motor is armed, the control loop must set new modulation timings +// between any two interrupts (that is, enqueue_modulation_timings must be executed). +// If the control loop fails to do so, the next interrupt handler floats the +// phases. Once this happens, missed_control_deadline is set to true and +// the motor can be considered disarmed. +// +// @returns: True on success, false otherwise +bool Motor::arm() { + // Wait until the interrupt handler triggers twice. After the first wait there is an + // undefined period until the next trigger. After the second wait we know for sure + // that we have exactly one full interrupt period until the third trigger. This gives + // the control loop the correct time quota to set up modulation timings. + if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) + return false; + next_timings_valid_ = false; + axis_->missed_control_deadline_ = false; + return true; } +// @brief Floats the phases of this motor immediately and updates +// the brake current accordingly. void Motor::disarm() { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer); // disables pwm outputs + // disable pwm + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer); + // set this motor's contribution to 0 + current_control_.Ibus = 0.0f; + update_brake_current(); + // ensure the PWM is not re-enabled without the state machine explicitly + // calling motor.arm() + axis_->missed_control_deadline_ = true; } -// Set up the gate drivers +// @brief Set up the gate drivers void Motor::DRV8301_setup() { - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; - DRV8301_enable(&gate_driver); - DRV8301_setupSpi(&gate_driver, local_regs); + DRV8301_enable(&gate_driver_); + DRV8301_setupSpi(&gate_driver_, local_regs); // TODO we can use reporting only if we actually wire up the nOCTW pin local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; @@ -50,41 +78,42 @@ void Motor::DRV8301_setup() { switch (local_regs->Ctrl_Reg_2.GAIN) { case DRV8301_ShuntAmpGain_10VpV: - phase_current_rev_gain = 1.0f / 10.0f; + phase_current_rev_gain_ = 1.0f / 10.0f; break; case DRV8301_ShuntAmpGain_20VpV: - phase_current_rev_gain = 1.0f / 20.0f; + phase_current_rev_gain_ = 1.0f / 20.0f; break; case DRV8301_ShuntAmpGain_40VpV: - phase_current_rev_gain = 1.0f / 40.0f; + phase_current_rev_gain_ = 1.0f / 40.0f; break; case DRV8301_ShuntAmpGain_80VpV: - phase_current_rev_gain = 1.0f / 80.0f; + phase_current_rev_gain_ = 1.0f / 80.0f; break; } float margin = 0.90f; - float max_input = margin * 0.3f * hw_config.shunt_conductance; - float max_swing = margin * 1.6f * hw_config.shunt_conductance * phase_current_rev_gain; - current_control.max_allowed_current = std::min(max_input, max_swing); + float max_input = margin * 0.3f * hw_config_.shunt_conductance; + float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_; + current_control_.max_allowed_current = std::min(max_input, max_swing); local_regs->SndCmd = true; - DRV8301_writeData(&gate_driver, local_regs); + DRV8301_writeData(&gate_driver_, local_regs); local_regs->RcvCmd = true; - DRV8301_readData(&gate_driver, local_regs); + DRV8301_readData(&gate_driver_, local_regs); } -//Returns true if everything is OK (no fault) +// @brief Checks if the gate driver is in operational state. +// @returns: true if the gate driver is OK (no fault), false otherwise bool Motor::check_DRV_fault() { //TODO: make this pin configurable per motor ch - GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config.nFAULT_port, gate_driver_config.nFAULT_pin); + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin); if (nFAULT_state == GPIO_PIN_RESET) { // Update DRV Fault Code - drv_fault = DRV8301_getFaultType(&gate_driver); + drv_fault_ = DRV8301_getFaultType(&gate_driver_); // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; local_regs->RcvCmd = true; - DRV8301_readData(&gate_driver, local_regs); + DRV8301_readData(&gate_driver_, local_regs); return false; }; return true; @@ -92,14 +121,14 @@ bool Motor::check_DRV_fault() { bool Motor::do_checks() { if (!check_DRV_fault()) { - error = ERROR_DRV_FAULT; + error_ = ERROR_DRV_FAULT; return false; } return true; } -uint16_t Motor::check_timing() { - TIM_HandleTypeDef* htim = hw_config.timer; +void Motor::log_timing() { + TIM_HandleTypeDef* htim = hw_config_.timer; uint16_t timing = htim->Instance->CNT; bool down = htim->Instance->CR1 & TIM_CR1_DIR; if (down) { @@ -107,19 +136,17 @@ uint16_t Motor::check_timing() { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if (++(timing_log_index) == TIMING_LOG_SIZE) { - timing_log_index = 0; + if (++(timing_log_index_) == TIMING_LOG_SIZE) { + timing_log_index_ = 0; } - timing_log[timing_log_index] = timing; - - return timing; + timing_log_[timing_log_index_] = timing; } float Motor::phase_current_from_adcval(uint32_t ADCValue) { 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; } @@ -134,25 +161,25 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { float test_voltage = 0.0f; size_t i = 0; - axis->run_control_loop([&](){ - float Ialpha = -(current_meas.phB + current_meas.phC); + 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) - return error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A enqueue_voltage_timings(test_voltage, 0.0f); return ++i < num_test_cycles; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //// De-energize motor //enqueue_voltage_timings(motor, 0.0f, 0.0f); float R = test_voltage / test_current; - config.phase_resistance = R; + config_.phase_resistance = R; return true; // if we ran to completion that means success } @@ -162,16 +189,16 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { 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; + Ialphas[i] += -current_meas_.phB - current_meas_.phC; // Test voltage along phase A enqueue_voltage_timings(test_voltages[i], 0.0f); return ++t < (num_cycles << 1); }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //// De-energize motor @@ -183,24 +210,24 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - config.phase_inductance = L; + config_.phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) - return error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } bool Motor::run_calibration() { - error = ERROR_NO_ERROR; + error_ = ERROR_NO_ERROR; - float R_calib_max_voltage = config.resistance_calib_max_voltage; - if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(config.calibration_current, R_calib_max_voltage)) + float R_calib_max_voltage = config_.resistance_calib_max_voltage; + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) return false; if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage)) return false; - } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { // no calibration needed } else { return false; @@ -208,19 +235,21 @@ bool Motor::run_calibration() { // Calculate current control gains float current_control_bandwidth = 1000.0f; // [rad/s] - current_control.p_gain = current_control_bandwidth * config.phase_inductance; - float plant_pole = config.phase_resistance / config.phase_inductance; - current_control.i_gain = plant_pole * current_control.p_gain; - + current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; + float plant_pole = config_.phase_resistance / config_.phase_inductance; + current_control_.i_gain = plant_pole * current_control_.p_gain; + + is_calibrated_ = true; return true; } void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - 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; } void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { @@ -242,14 +271,14 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { - Current_control_t* ictrl = ¤t_control; + Current_control_t* ictrl = ¤t_control_; // For Reporting ictrl->Iq_setpoint = Iq_des; // Clarke transform - float Ialpha = -current_meas.phB - current_meas.phC; - float Ibeta = one_by_sqrt3 * (current_meas.phB - current_meas.phC); + float Ialpha = -current_meas_.phB - current_meas_.phC; + float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform float c = arm_cos_f32(phase); @@ -306,21 +335,21 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { bool Motor::update(float current_setpoint, float phase) { - current_setpoint *= config.direction; - phase *= config.direction; + current_setpoint *= config_.direction; + phase *= config_.direction; // Execute current command // TODO: move this into the mot - if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { if(!FOC_current(0.0f, current_setpoint, phase)){ return false; } - } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. if(!FOC_voltage(0.0f, current_setpoint, phase)) return false; } else { - error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 877921f9..2566a28d 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,6 +36,7 @@ typedef struct { // 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. typedef struct { + bool hand_calibrated = true; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -67,7 +68,7 @@ public: const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config); - void arm(); + bool arm(); void disarm(); void setup() { DRV8301_setup(); @@ -75,7 +76,7 @@ public: void DRV8301_setup(); bool check_DRV_fault(); bool do_checks(); - uint16_t check_timing(); + void log_timing(); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); bool measure_phase_inductance(float voltage_low, float voltage_high); @@ -86,30 +87,32 @@ public: bool FOC_current(float Id_des, float Iq_des, float phase); bool update(float current_setpoint, float phase); - const MotorHardwareConfig_t& hw_config; - const GateDriverHardwareConfig_t gate_driver_config; - MotorConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + const MotorHardwareConfig_t& hw_config_; + const GateDriverHardwareConfig_t gate_driver_config_; + MotorConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor //private: - DRV8301_Obj gate_driver; // initialized in constructor - uint16_t next_timings[3] = { + 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 }; - uint16_t last_cpu_time = 0; - int timing_log_index = 0; - uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; + bool next_timings_valid_ = false; + uint16_t last_cpu_time_ = 0; + int timing_log_index_ = 0; + uint16_t timing_log_[TIMING_LOG_SIZE] = { 0 }; // variables exposed on protocol - Error_t error = ERROR_NO_ERROR; - Iph_BC_t current_meas = {0.0f, 0.0f}; - Iph_BC_t DC_calib = {0.0f, 0.0f}; - const float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] - float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) - Current_control_t current_control = { + Error_t error_ = ERROR_NO_ERROR; + bool is_calibrated_ = config_.hand_calibrated; + Iph_BC_t current_meas_ = {0.0f, 0.0f}; + Iph_BC_t DC_calib_ = {0.0f, 0.0f}; + const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] + float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) + Current_control_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 .v_current_control_integral_d = 0.0f, @@ -121,47 +124,47 @@ public: .Iq_measured = 0.0f, .max_allowed_current = 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) + 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) // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( - make_protocol_property("error", reinterpret_cast(&this->error)), - make_protocol_ro_property("current_meas.phB", &this->current_meas.phB), - make_protocol_ro_property("current_meas.phC", &this->current_meas.phC), - make_protocol_property("DC_calib.phB", &this->DC_calib.phB), - make_protocol_property("DC_calib.phC", &this->DC_calib.phC), - make_protocol_property("shunt_conductance", &this->shunt_conductance), - make_protocol_property("phase_current_rev_gain", &this->phase_current_rev_gain), + make_protocol_property("error", &error_), + make_protocol_ro_property("current_meas_phB", ¤t_meas_.phB), + make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), + make_protocol_property("DC_calib_phB", &DC_calib_.phB), + make_protocol_property("DC_calib_phC", &DC_calib_.phC), + make_protocol_property("shunt_conductance", &shunt_conductance_), + make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), make_protocol_object("current_control", - make_protocol_property("p_gain", &this->current_control.p_gain), - make_protocol_property("i_gain", &this->current_control.i_gain), - make_protocol_property("v_current_control_integral_d", &this->current_control.v_current_control_integral_d), - make_protocol_property("v_current_control_integral_q", &this->current_control.v_current_control_integral_q), - make_protocol_property("Ibus", &this->current_control.Ibus), - make_protocol_property("final_v_alpha", &this->current_control.final_v_alpha), - make_protocol_property("final_v_beta", &this->current_control.final_v_beta), - make_protocol_property("Iq_setpoint", &this->current_control.Iq_setpoint), - make_protocol_property("Iq_measured", &this->current_control.Iq_measured), - make_protocol_property("max_allowed_current", &this->current_control.max_allowed_current) + 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("max_allowed_current", ¤t_control_.max_allowed_current) ), make_protocol_object("gate_driver", - make_protocol_ro_property("drv_fault", reinterpret_cast(&this->drv_fault)), - make_protocol_ro_property("status_reg_1", &this->gate_driver_regs.Stat_Reg_1_Value), - make_protocol_ro_property("status_reg_2", &this->gate_driver_regs.Stat_Reg_2_Value), - make_protocol_ro_property("ctrl_reg_1", &this->gate_driver_regs.Ctrl_Reg_1_Value), - make_protocol_ro_property("ctrl_reg_2", &this->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("config", - make_protocol_property("pole_pairs", &this->config.pole_pairs), - make_protocol_property("calibration_current", &this->config.calibration_current), - make_protocol_property("resistance_calib_max_voltage", &this->config.resistance_calib_max_voltage), - make_protocol_property("phase_inductance", &this->config.phase_inductance), - make_protocol_property("phase_resistance", &this->config.phase_resistance), - make_protocol_property("direction", &this->config.direction), - make_protocol_property("motor_type", reinterpret_cast(&this->config.motor_type)), - make_protocol_property("current_lim", &this->config.current_lim) + 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) ) ); } diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index e9bb285d..ef9e3df8 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -44,8 +44,6 @@ extern Axis *axes[AXIS_COUNT]; #include #include -#include // TODO: remove - // defined in main.cpp void save_configuration(void); void erase_configuration(void); diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index 3e0336fa..c0cd07f0 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -118,10 +118,6 @@ JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint(); EndpointProvider* application_endpoints; uint16_t json_crc_; -Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; -size_t n_endpoints_ = 0; -EndpointProvider* endpoint_provider_ = nullptr; - void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) { write_string("{\"name\":\"\",", output); @@ -162,9 +158,9 @@ void set_application_endpoints(EndpointProvider* endpoints) { application_endpoints = endpoints; n_endpoints_ = 0; - json_file_endpoint.register_endpoints(endpoints_, 0, MAX_ENDPOINTS); + json_file_endpoint.register_endpoints(endpoints_, 0, max_endpoints_); n_endpoints_ += decltype(json_file_endpoint)::endpoint_count; - application_endpoints->register_endpoints(endpoints_, n_endpoints_, MAX_ENDPOINTS); + application_endpoints->register_endpoints(endpoints_, n_endpoints_, max_endpoints_); n_endpoints_ += application_endpoints->get_endpoint_count(); // Calculates the CRC16 of the JSON file. @@ -174,7 +170,6 @@ void set_application_endpoints(EndpointProvider* endpoints) { json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator); json_crc_ = crc16_calculator.get_crc16(); - CRC16Calculator crc16_calculator2(PROTOCOL_VERSION); endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2); json_crc_ = crc16_calculator2.get_crc16(); diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index c4e1652f..b3606211 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -369,8 +369,6 @@ inline constexpr const char* get_default_json_modifier() { return "\"type\":\"bool\",\"access\":\"rw\""; } -constexpr size_t MAX_ENDPOINTS = 100; - class Endpoint { public: //const char* const name_; @@ -417,7 +415,6 @@ template<> struct MemberList<> { public: static constexpr size_t endpoint_count = 0; - size_t get_endpoint_count() { return endpoint_count; } static constexpr bool is_empty = true; void write_json(size_t id, StreamSink* output) { // no action @@ -432,7 +429,6 @@ template struct MemberList { public: static constexpr size_t endpoint_count = TMember::endpoint_count + MemberList::endpoint_count; - size_t get_endpoint_count() { return endpoint_count; } static constexpr bool is_empty = false; MemberList(TMember&& this_member, TMembers&&... subsequent_members) : @@ -441,7 +437,7 @@ public: MemberList(TMember&& this_member, MemberList&& subsequent_members) : this_member_(std::forward(this_member)), - subsequent_members_(std::forward(subsequent_members)) {} + subsequent_members_(std::forward>(subsequent_members)) {} // @brief Move constructor /* MemberList(MemberList&& other) : @@ -572,16 +568,30 @@ public: TProperty* property_; }; -template +// Non-const non-enum types +template::value>> ProtocolProperty make_protocol_property(const char * name, TProperty* property) { return ProtocolProperty(name, property); }; -template +// Const non-enum types +template::value>> ProtocolProperty make_protocol_ro_property(const char * name, const TProperty* property) { return ProtocolProperty(name, property); }; +// Non-const enum types +template::value>> +ProtocolProperty> make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + +// Const enum types +template::value>> +ProtocolProperty> make_protocol_ro_property(const char * name, const TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + template @@ -636,7 +646,7 @@ struct PropertyListFactory { static MemberList, ProtocolProperty...> make_property_list(std::array names, std::tuple& values) { return MemberList, ProtocolProperty...>( - make_protocol_property(std::get(names), std::get(values)), + make_protocol_property(std::get(names), &std::get(values)), PropertyListFactory::template make_property_list(names, values) ); } @@ -715,7 +725,7 @@ class EndpointProvider_from_MemberList : public EndpointProvider { public: EndpointProvider_from_MemberList(T& member_list) : member_list_(member_list) {} size_t get_endpoint_count() final { - return member_list_.get_endpoint_count(); + return T::endpoint_count; } void write_json(size_t id, StreamSink* output) final { return member_list_.write_json(id, output); @@ -728,4 +738,10 @@ public: void set_application_endpoints(EndpointProvider* endpoints); + +// defined in communication.cpp +extern Endpoint* endpoints_[]; +extern size_t n_endpoints_; +extern const size_t max_endpoints_; + #endif diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 1ba30015..c1358150 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -7,10 +7,10 @@ SensorlessEstimator::SensorlessEstimator() // Calculate pll gains // This calculation is currently identical to the PLL in Encoder float pll_bandwidth = 1000.0f; // [rad/s] - pll_kp = 2.0f * pll_bandwidth; + pll_kp_ = 2.0f * pll_bandwidth; // Critically damped - pll_ki = 0.25f * (pll_kp * pll_kp); + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); } bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) { @@ -23,35 +23,35 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp < 1.0f)) { - error = ERROR_NUMERICAL; + if (!(current_meas_period * pll_kp_ < 1.0f)) { + error_ = ERROR_NUMERICAL; return false; } // Clarke transform float I_alpha_beta[2] = { - -axis->motor.current_meas.phB - axis->motor.current_meas.phC, - one_by_sqrt3 * (axis->motor.current_meas.phB - axis->motor.current_meas.phC)}; + -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, + one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)}; // alpha-beta vector operations float eta[2]; for (int i = 0; i <= 1; ++i) { // y is the total flux-driving voltage (see paper eqn 4) - float y = -axis->motor.config.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory[i]; + float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i]; // flux dynamics (prediction) float x_dot = y; // integrate prediction to current timestep - flux_state[i] += x_dot * current_meas_period; + flux_state_[i] += x_dot * current_meas_period; // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i]; } // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = pm_flux_linkage * pm_flux_linkage; + float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_; float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (pm_flux_linkage * pm_flux_linkage); - float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + float bandwidth_factor = 1.0f / pm_flux_sqr; + float eta_factor = 0.5f * (observer_gain_ * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); static float eta_factor_avg_test = 0.0f; eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); @@ -61,25 +61,25 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // add observer action to flux estimate dynamics float x_dot = eta_factor * eta[i]; // convert action to discrete-time - flux_state[i] += x_dot * current_meas_period; + flux_state_[i] += x_dot * current_meas_period; // update new eta - eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i]; } // Flux state estimation done, store V_alpha_beta for next timestep - V_alpha_beta_memory[0] = axis->motor.current_control.final_v_alpha; - V_alpha_beta_memory[1] = axis->motor.current_control.final_v_beta; + V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha; + V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta; // PLL // TODO: the PLL part has some code duplication with the encoder PLL // predict PLL phase with velocity - pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_vel); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_); // update PLL phase with observer permanent magnet phase - phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(phase - pll_pos); - pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_kp * delta_phase); + phase_ = fast_atan2(eta[1], eta[0]); + float delta_phase = wrap_pm_pi(phase_ - pll_pos_); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp_ * delta_phase); // update PLL velocity - pll_vel += current_meas_period * pll_ki * delta_phase; + pll_vel_ += current_meas_period * pll_ki_ * delta_phase; //TODO TEMP TEST HACK // static int trigger_ctr = 0; @@ -94,8 +94,8 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // motor->rotor_mode = ROTOR_MODE_SENSORLESS; // } - if (pos_estimate) *pos_estimate = pll_pos; - if (vel_estimate) *vel_estimate = pll_vel; - if (phase_output) *phase_output = phase; + if (pos_estimate) *pos_estimate = pll_pos_; + if (vel_estimate) *vel_estimate = pll_vel_; + if (phase_output) *phase_output = phase_; return true; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 18b291ca..569c9a09 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -12,20 +12,20 @@ public: bool update(float* pos_estimate, float* vel_estimate, float* phase); - Axis* axis = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor // TODO: expose on protocol - Error_t error = ERROR_NONE; - float phase = 0.0f; // [rad] - float pll_pos = 0.0f; // [rad] - float pll_vel = 0.0f; // [rad/s] - float pll_kp = 0.0f; // [rad/s / rad] - float pll_ki = 0.0f; // [(rad/s^2) / rad] - float observer_gain = 1000.0f; // [rad/s] - float flux_state[2] = {0.0f, 0.0f}; // [Vs] - float V_alpha_beta_memory[2] = {0.0f, 0.0f}; // [V] - float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } - bool estimator_good = false; + Error_t error_ = ERROR_NONE; + float phase_ = 0.0f; // [rad] + float pll_pos_ = 0.0f; // [rad] + float pll_vel_ = 0.0f; // [rad/s] + float pll_kp_ = 0.0f; // [rad/s / rad] + float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + float observer_gain_ = 1000.0f; // [rad/s] + float flux_state_[2] = {0.0f, 0.0f}; // [Vs] + float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] + float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + bool estimator_good_ = false; }; #endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index e3908cde..b7cb2838 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -68,7 +68,7 @@ build{ 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', - 'MotorControl/commands.cpp', + 'MotorControl/communication.cpp', 'MotorControl/protocol.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp',