From 4013ab30057ebac0156735d3f83f7b629707f2ae Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 26 Jun 2021 20:11:45 -0700 Subject: [PATCH] implement encoder reference position updates --- Firmware/MotorControl/encoder.cpp | 6 ++++++ Firmware/MotorControl/encoder.hpp | 2 ++ Firmware/communication/can/can_simple.cpp | 7 +++++++ Firmware/communication/can/can_simple.hpp | 3 +++ Firmware/odrive-interface.yaml | 3 ++- 5 files changed, 20 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 4b25f5a2..c522c2eb 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -845,3 +845,9 @@ bool Encoder::update() { return true; } + +void Encoder::update_pos_offset(float ref_pos) { + float base_pos = pos_estimate_counts_ / (float)config_.cpr; + float pos_offset = ref_pos - base_pos; + pos_offset_target_ += config_.pos_offset_update_gain * (pos_offset - pos_offset_target_); +} diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 789cd799..e224990e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -23,6 +23,7 @@ public: float calib_scan_omega = 4.0f * M_PI; // rad/s electrical float bandwidth = 1000.0f; float pos_offset_bandwidth = 1.0f; + float pos_offset_update_gain = 0.2f; int32_t phase_offset = 0; // Offset between encoder count and rotor electrical phase float phase_offset_float = 0.0f; // Sub-count phase alignment offset int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, @@ -84,6 +85,7 @@ public: void decode_hall_samples(); int32_t hall_model(float internal_pos); bool update(); + void update_pos_offset(float ref_pos); TIM_HandleTypeDef* timer_; Stm32Gpio index_gpio_; diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 1c47c3dc..a4cece39 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -138,6 +138,9 @@ void CANSimple::do_command(Axis& axis, const can_Message_t& msg) { if (msg.rtr) get_vbus_voltage_callback(axis); break; + case MSG_ENCODER_REF_UPDATE: + encoder_ref_update(axis, msg); + break; case MSG_CLEAR_ERRORS: clear_errors_callback(axis, msg); break; @@ -289,6 +292,10 @@ void CANSimple::set_linear_count_callback(Axis& axis, const can_Message_t& msg){ axis.encoder_.set_linear_count(can_getSignal(msg, 0, 32, true)); } +void CANSimple::encoder_ref_update(Axis& axis, const can_Message_t& msg){ + axis.encoder_.update_pos_offset(can_getSignal(msg, 0, 32, true)); +} + bool CANSimple::get_iq_callback(const Axis& axis) { can_Message_t txmsg; txmsg.id = axis.config_.can.node_id << NUM_CMD_ID_BITS; diff --git a/Firmware/communication/can/can_simple.hpp b/Firmware/communication/can/can_simple.hpp index 9c6ac96c..2ebe6d57 100644 --- a/Firmware/communication/can/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -32,6 +32,7 @@ class CANSimple { MSG_RESET_ODRIVE, MSG_GET_VBUS_VOLTAGE, MSG_CLEAR_ERRORS, + MSG_ENCODER_REF_UPDATE, MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND }; @@ -73,6 +74,7 @@ class CANSimple { static void set_traj_accel_limits_callback(Axis& axis, const can_Message_t& msg); static void set_traj_inertia_callback(Axis& axis, const can_Message_t& msg); static void set_linear_count_callback(Axis& axis, const can_Message_t& msg); + static void encoder_ref_update(Axis& axis, const can_Message_t& msg); // Other functions static void nmt_callback(const Axis& axis, const can_Message_t& msg); @@ -80,6 +82,7 @@ class CANSimple { static void clear_errors_callback(Axis& axis, const can_Message_t& msg); static void start_anticogging_callback(const Axis& axis, const can_Message_t& msg); + static constexpr uint8_t NUM_NODE_ID_BITS = 6; static constexpr uint8_t NUM_CMD_ID_BITS = 11 - NUM_NODE_ID_BITS; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 2928d695..5c2b9c28 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -1073,7 +1073,6 @@ interfaces: hall_state: readonly uint8 vel_estimate: {type: readonly float32, c_getter: vel_estimate_.any().value_or(0.0f)} vel_estimate_counts: readonly float32 - pos_offset_target: float32 pos_offset: float32 calib_scan_response: readonly float32 pos_abs: int32 @@ -1096,6 +1095,7 @@ interfaces: enable_phase_interpolation: bool bandwidth: {type: float32, c_setter: set_bandwidth} pos_offset_bandwidth: float32 + pos_offset_update_gain: float32 calib_range: float32 calib_scan_distance: float32 calib_scan_omega: float32 @@ -1110,6 +1110,7 @@ interfaces: doc: Analog cosine signal of a sin/cos encoder. The corresponding GPIO must be in `GPIO_MODE_ANALOG_IN`. functions: set_linear_count: {in: {count: int32}} + update_pos_offset: {in: {ref_pos: float32}} ODrive.SensorlessEstimator: