From 4fa09d6bbca4a591c54a45036279f2450604cd08 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 5 Oct 2018 22:20:04 -0400 Subject: [PATCH] Get CAN_SIMPLE to compile --- Firmware/MotorControl/axis.cpp | 43 +++++----- Firmware/MotorControl/axis.hpp | 10 ++- Firmware/communication/can_simple.cpp | 103 +++++++++++++++++++++-- Firmware/communication/can_simple.hpp | 8 +- Firmware/communication/interface_can.cpp | 77 +++++------------ Firmware/communication/interface_can.hpp | 2 + 6 files changed, 156 insertions(+), 87 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 4ff660fd..3aaf5ed4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -3,8 +3,9 @@ #include #include "gpio.h" -#include "utils.h" #include "odrive_main.h" +#include "utils.h" +#include "communication/interface_can.hpp" Axis::Axis(const AxisHardwareConfig_t& hw_config, Config_t& config, @@ -19,8 +20,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, sensorless_estimator_(sensorless_estimator), controller_(controller), motor_(motor), - trap_(trap) -{ + trap_(trap) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; @@ -46,7 +46,7 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { - osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512); + osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512); thread_id_ = osThreadCreate(osThread(thread_def), this); thread_id_valid_ = true; } @@ -85,7 +85,7 @@ void Axis::set_step_dir_enabled(bool enable) { // Subscribe to rising edges of the step GPIO GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN, - step_cb_wrapper, this); + step_cb_wrapper, this); enable_step_dir_ = true; } else { @@ -136,7 +136,9 @@ bool Axis::do_updates() { // Sub-components should use set_error which will propegate to this error_ encoder_.update(); sensorless_estimator_.update(); - return check_for_errors(); + bool ret = check_for_errors(); + odCAN->send_heartbeat(this); + return ret; } float Axis::get_temp() { @@ -148,7 +150,7 @@ float Axis::get_temp() { bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; - run_control_loop([&](){ + 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; @@ -158,11 +160,11 @@ bool Axis::run_sensorless_spin_up() { }); if (error_ != ERROR_NONE) 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); - run_control_loop([&](){ + run_control_loop([&]() { vel += config_.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); float I_mag = config_.spin_up_current; @@ -181,7 +183,7 @@ bool Axis::run_sensorless_spin_up() { // 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](){ + run_control_loop([this]() { if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; @@ -190,7 +192,7 @@ bool Axis::run_sensorless_control_loop() { if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, ¤t_setpoint)) return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, sensorless_estimator_.phase_)) - return false; // set_error should update axis.error_ + return false; // set_error should update axis.error_ return true; }); set_step_dir_enabled(false); @@ -199,13 +201,13 @@ bool Axis::run_sensorless_control_loop() { bool Axis::run_closed_loop_control_loop() { set_step_dir_enabled(config_.enable_step_dir); - run_control_loop([this](){ + run_control_loop([this]() { // Note that all estimators are updated in the loop prefix in run_control_loop float current_setpoint; if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, ¤t_setpoint)) - return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error + return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error if (!motor_.update(current_setpoint, encoder_.phase_)) - return false; // set_error should update axis.error_ + return false; // set_error should update axis.error_ return true; }); set_step_dir_enabled(false); @@ -216,7 +218,7 @@ bool Axis::run_idle_loop() { // run_control_loop ignores missed modulation timing updates // if and only if we're in AXIS_STATE_IDLE safety_critical_disarm_motor_pwm(motor_); - run_control_loop([this](){ + run_control_loop([this]() { return true; }); return check_for_errors(); @@ -224,7 +226,6 @@ bool Axis::run_idle_loop() { // Infinite loop that does calibration and enters main control loop as appropriate 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 @@ -238,7 +239,7 @@ void Axis::run_state_machine_loop() { // arm! motor_.arm(); - + for (;;) { // Load the task chain if a specific request is pending if (requested_state_ != AXIS_STATE_UNDEFINED) { @@ -265,7 +266,7 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = requested_state_; task_chain_[pos++] = AXIS_STATE_IDLE; } - task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking + task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking requested_state_ = AXIS_STATE_UNDEFINED; // Auto-clear any invalid state error error_ &= ~ERROR_INVALID_STATE; @@ -296,7 +297,7 @@ void Axis::run_state_machine_loop() { break; case AXIS_STATE_SENSORLESS_CONTROL: - status = run_sensorless_spin_up(); // TODO: restart if desired + status = run_sensorless_spin_up(); // TODO: restart if desired if (status) status = run_sensorless_control_loop(); break; @@ -307,12 +308,12 @@ void Axis::run_state_machine_loop() { case AXIS_STATE_IDLE: run_idle_loop(); - status = motor_.arm(); // done with idling - try to arm the motor + status = motor_.arm(); // done with idling - try to arm the motor break; default: error_ |= ERROR_INVALID_STATE; - status = false; // this will set the state to idle + status = false; // this will set the state to idle break; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index ec8fa8ef..3b46e347 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -20,6 +20,7 @@ public: ERROR_ENCODER_FAILED = 0x100, ERROR_CONTROLLER_FAILED = 0x200, ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, + ERROR_ESTOP_REQUESTED = 0x800 }; // Warning: Do not reorder these enum values. @@ -55,7 +56,7 @@ public: float spin_up_acceleration = 400.0f; // [rad/s^2] float spin_up_target_vel = 400.0f; // [rad/s] - uint8_t can_node_id = 0; // If both axes are 0, only the first one will get commands. + uint8_t can_node_id = 0; // Both axes will have the same id to start }; enum thread_signals { @@ -113,10 +114,9 @@ public: // Update all estimators // Note: updates run even if checks fail bool updates_ok = do_updates(); - if (!checks_ok || !updates_ok) break; - + // Run main loop function, defer quitting for after wait // TODO: change arming logic to arm after waiting bool main_continue = update_handler(); @@ -165,6 +165,7 @@ public: State_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; State_t& current_state_ = task_chain_[0]; uint32_t loop_counter_ = 0; + uint32_t last_heartbeat_ = 0; // Communication protocol definitions auto make_protocol_definitions() { @@ -186,7 +187,8 @@ public: 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_property("spin_up_target_vel", &config_.spin_up_target_vel), + make_protocol_property("can_node_id", &config_.can_node_id) ), make_protocol_function("get_temp", *this, &Axis::get_temp), make_protocol_object("motor", motor_.make_protocol_definitions()), diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index dc4c8cba..92acf2a8 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -1,3 +1,4 @@ + #include "can_simple.hpp" #include "odrive_main.h" @@ -14,13 +15,105 @@ void CANSimple::handle_can_message(CAN_message_t& msg) { // nodeID | CMD // 4 bits | 7 bits auto nodeID = (msg.id >> 7 & 0x15); - for(int i = 0; i < AXIS_COUNT; i++){ - if(axes[i]->config_.nodeID) + Axis* axis = nullptr; + + for (uint8_t i = 0; i < AXIS_COUNT; i++) { + if (axes[i]->config_.can_node_id == nodeID) { + axis = axes[i]; + } } - switch (msg.id & 0x7F) { - case 0x010: move_to_pos_callback(); break; + if (axis != nullptr) { + switch (msg.id & 0x7F) { + case 0x010: + move_to_pos_callback(axis, msg); + break; + case 0x011: + set_pos_setpoint_callback(axis, msg); + break; + case 0x012: + set_vel_setpoint_callback(axis, msg); + break; + case 0x013: + set_current_setpoint_callback(axis, msg); + break; + } } } -void move_to_pos_callback(Axis& axis, uint32_t pos){ +void CANSimple::estop_callback(){ + for(Axis* axis : axes){ + axis->error_ |= Axis::ERROR_ESTOP_REQUESTED; + } +} + +void CANSimple::move_to_pos_callback(Axis* axis, CAN_message_t& msg) { + float pos = msg.buf[0]; + pos += msg.buf[1] << 8; + pos += msg.buf[2] << 16; + pos += msg.buf[3] << 24; + + axis->controller_.move_to_pos(pos); +} + +void CANSimple::set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg) { + float pos = msg.buf[0]; + pos += msg.buf[1] << 8; + pos += msg.buf[2] << 16; + pos += msg.buf[3] << 24; + + float vel = msg.buf[4]; + vel += msg.buf[5] << 8; + vel *= 0.1f; // Factor of 10 + + float current = msg.buf[6]; + current += (msg.buf[7] << 8); + current *= 0.01f; // Factor of 100 + + axis->controller_.set_pos_setpoint(pos, vel, current); +} + +void CANSimple::set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg) { + float vel = msg.buf[0]; + vel += msg.buf[1] << 8; + vel += msg.buf[2] << 16; + vel += msg.buf[3] << 24; + vel *= 0.01f; + + float current = msg.buf[4]; + current += msg.buf[5] << 8; + current += msg.buf[6] << 16; + current += msg.buf[7] << 24; + current *= 0.01f; + + axis->controller_.set_vel_setpoint(vel, current); +} + +void CANSimple::set_current_setpoint_callback(Axis* axis, CAN_message_t& msg) { + float current = msg.buf[0]; + current += msg.buf[1] << 8; + current += msg.buf[2] << 16; + current += msg.buf[3] << 24; + current *= 0.01f; + + axis->controller_.set_current_setpoint(current); +} + +void CANSimple::send_heartbeat(Axis* axis){ + CAN_message_t txmsg; + txmsg.id = axis->config_.can_node_id << 7; + txmsg.id += 0x1; + txmsg.isExt = false; + txmsg.len = 8; + + // Axis errors in 1st 32-bit value + txmsg.buf[0] = axis->error_; + txmsg.buf[1] = axis->error_ >> 8; + txmsg.buf[2] = axis->error_ >> 16; + txmsg.buf[3] = axis->error_ >> 24; + + // Current state of axis in 2nd 32-bit value + txmsg.buf[4] = axis->current_state_; + txmsg.buf[5] = axis->current_state_ >> 8; + txmsg.buf[6] = axis->current_state_ >> 16; + txmsg.buf[7] = axis->current_state_ >> 24; } \ No newline at end of file diff --git a/Firmware/communication/can_simple.hpp b/Firmware/communication/can_simple.hpp index f2ed42ed..7d87f2da 100644 --- a/Firmware/communication/can_simple.hpp +++ b/Firmware/communication/can_simple.hpp @@ -6,10 +6,16 @@ class CANSimple { public: static void handle_can_message(CAN_message_t& msg); + static void send_heartbeat(Axis* axis); private: + static void estop_callback(); + // Controller - static void move_to_pos_callback(Axis& axis, uint32_t pos); + static void move_to_pos_callback(Axis* axis, CAN_message_t& msg); + static void set_pos_setpoint_callback(Axis* axis, CAN_message_t& msg); + static void set_vel_setpoint_callback(Axis* axis, CAN_message_t& msg); + static void set_current_setpoint_callback(Axis* axis, CAN_message_t& msg); // This functional way of handling the messages is neat and is much cleaner from // a data security point of view, but it will require some tweaking diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp index 21edf0ca..716821d2 100644 --- a/Firmware/communication/interface_can.cpp +++ b/Firmware/communication/interface_can.cpp @@ -1,36 +1,3 @@ -/* -* -* Zero-config node ID negotiation -* ------------------------------- -* -* A heartbeat message is a message with a 8 byte unique serial number as payload. -* A regular message is any message that is not a heartbeat message. -* -* All nodes MUST obey these four rules: -* -* a) At a given point in time, a node MUST consider a node ID taken (by others) -* if any of the following is true: -* - the node received a (not self-emitted) heartbeat message with that node ID -* within the last second -* - the node attempted and failed at sending a heartbeat message with that -* node ID within the last second (failed in the sense of not ACK'd) -* -* b) At a given point in time, a node MUST NOT consider a node ID self-assigned -* if, within the last second, it did not succeed in sending a heartbeat -* message with that node ID. -* -* c) At a given point in time, a node MUST NOT send any heartbeat message with -* a node ID that is taken. -* -* d) At a given point in time, a node MUST NOT send any regular message with -* a node ID that is not self-assigned. -* -* Hardware allocation -* ------------------- -* RX FIFO0: -* - filter bank 0: heartbeat messages -*/ - #include "interface_can.hpp" #include "fibre/crc.hpp" @@ -58,32 +25,18 @@ ODriveCAN::ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config) } void ODriveCAN::can_server_thread() { - CAN_message_t heartbeat; - heartbeat.id = 0x700 + config_.node_id; - uint32_t lastHeartbeatTick = osKernelSysTick(); - for (;;) { CAN_message_t rxmsg; - osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status + osSemaphoreWait(sem_can, 10); // Poll every 10ms regardless of sempahore status while (available()) { read(rxmsg); - switch(config_.protocol) { - case CAN_PROTOCOL_SIMPLE: CANSimple::handle_can_message(rxmsg); break; + switch (config_.protocol) { + case CAN_PROTOCOL_SIMPLE: + CANSimple::handle_can_message(rxmsg); + break; } } - - // Handle heartbeat message - heartbeat.buf[0] = axes[0]->error_; - heartbeat.buf[1] = axes[0]->current_state_; - heartbeat.buf[2] = axes[1]->error_; - heartbeat.buf[3] = axes[1]->current_state_; - uint32_t now = osKernelSysTick(); - if(now - lastHeartbeatTick >= 100){ - write(heartbeat); - lastHeartbeatTick = now; - } - HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING); } } @@ -140,8 +93,6 @@ bool ODriveCAN::start_can_server() { return true; } - - // Send a CAN message on the bus uint32_t ODriveCAN::write(CAN_message_t &txmsg) { CAN_TxHeaderTypeDef header; @@ -181,8 +132,7 @@ bool ODriveCAN::read(CAN_message_t &rxmsg) { return validRead; } - -// Set one of only a few common baud rates. CAN doesn't do arbitrary baud rates well due to the time-quanta issue. +// Set one of only a few common baud rates. CAN doesn't do arbitrary baud rates well due to the time-quanta issue. // 21 TQ allows for easy sampling at exactly 80% (recommended by Vector Informatik GmbH for high reliability systems) // Conveniently, the CAN peripheral's 42MHz clock lets us easily create 21TQs for all common baud rates void ODriveCAN::set_baud_rate(uint32_t baudRate) { @@ -217,6 +167,21 @@ void ODriveCAN::set_node_id(uint8_t nodeID) { config_.node_id = nodeID; } +// This function is called by each axis. +// It provides an abstraction from the specific CAN protocol in use +void ODriveCAN::send_heartbeat(Axis *axis) { + // Handle heartbeat message + uint32_t now = osKernelSysTick(); + if (now - axis->last_heartbeat_ >= 100) { + switch (config_.protocol) { + case CAN_PROTOCOL_SIMPLE: + CANSimple::send_heartbeat(axis); + break; + } + axis->last_heartbeat_ = now; + } +} + void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {} void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {} void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {} diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 356c6cde..db44d3d0 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -4,6 +4,7 @@ #include #include #include "fibre/protocol.hpp" +#include "odrive_main.h" #define CAN_CLK_HZ (42000000) #define CAN_CLK_MHZ (42) @@ -44,6 +45,7 @@ class ODriveCAN { volatile bool thread_id_valid_ = false; bool start_can_server(); void can_server_thread(); + void send_heartbeat(Axis* axis); // I/O Functions uint32_t available();