From 965c0a14e61aae9220970bc389c39de54a518cfb Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Sep 2021 00:09:44 -0400 Subject: [PATCH 1/4] Allow setting controller gains on CAN Simple --- Firmware/communication/can/can_simple.cpp | 41 ++++++++++++++++------- Firmware/communication/can/can_simple.hpp | 4 +++ 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 3a24ead1..26b7f1db 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -22,8 +22,7 @@ bool CANSimple::renew_subscription(size_t i) { MsgIdFilterSpecs filter = { .id = {}, - .mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS) - }; + .mask = (uint32_t)(0xffffffff << NUM_CMD_ID_BITS)}; if (axis.config_.can.is_extended) { filter.id = (uint32_t)(axis.config_.can.node_id << NUM_CMD_ID_BITS); } else { @@ -34,9 +33,11 @@ bool CANSimple::renew_subscription(size_t i) { canbus_->unsubscribe(subscription_handles_[i]); } - return canbus_->subscribe(filter, [](void* ctx, const can_Message_t& msg) { - ((CANSimple*)ctx)->handle_can_message(msg); - }, this, &subscription_handles_[i]); + return canbus_->subscribe( + filter, [](void* ctx, const can_Message_t& msg) { + ((CANSimple*)ctx)->handle_can_message(msg); + }, + this, &subscription_handles_[i]); } void CANSimple::handle_can_message(const can_Message_t& msg) { @@ -144,6 +145,12 @@ void CANSimple::do_command(Axis& axis, const can_Message_t& msg) { case MSG_SET_LINEAR_COUNT: set_linear_count_callback(axis, msg); break; + case MSG_SET_POS_GAIN: + set_pos_gain_callback(axis, msg); + break; + case MSG_SET_VEL_GAINS: + set_vel_gains_callback(axis, msg); + break; default: break; } @@ -288,10 +295,19 @@ void CANSimple::set_traj_inertia_callback(Axis& axis, const can_Message_t& msg) axis.controller_.config_.inertia = can_getSignal(msg, 0, 32, true); } -void CANSimple::set_linear_count_callback(Axis& axis, const can_Message_t& msg){ +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::set_pos_gain_callback(Axis& axis, const can_Message_t& msg) { + axis.controller_.config_.pos_gain = can_getSignal(msg, 0, 32, true); +} + +void CANSimple::set_vel_gains_callback(Axis& axis, const can_Message_t& msg) { + axis.controller_.config_.vel_gain = can_getSignal(msg, 0, 32, true); + axis.controller_.config_.vel_integrator_gain = can_getSignal(msg, 32, 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; @@ -303,7 +319,7 @@ bool CANSimple::get_iq_callback(const Axis& axis) { if (!Idq_setpoint.has_value()) { Idq_setpoint = {0.0f, 0.0f}; } - + static_assert(sizeof(float) == sizeof(Idq_setpoint->first)); static_assert(sizeof(float) == sizeof(Idq_setpoint->second)); can_setSignal(txmsg, Idq_setpoint->first, 0, 32, true); @@ -328,7 +344,7 @@ bool CANSimple::get_vbus_voltage_callback(const Axis& axis) { } void CANSimple::clear_errors_callback(Axis& axis, const can_Message_t& msg) { - odrv.clear_errors(); // TODO: might want to clear axis errors only + odrv.clear_errors(); // TODO: might want to clear axis errors only } uint32_t CANSimple::service_stack() { @@ -337,14 +353,13 @@ uint32_t CANSimple::service_stack() { // TODO: remove this polling loop and replace with protocol hook for (size_t i = 0; i < AXIS_COUNT; ++i) { - bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) - || (axes[i].config_.can.is_extended != extended_node_ids_[i]); + bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) || (axes[i].config_.can.is_extended != extended_node_ids_[i]); if (node_id_changed) { renew_subscription(i); } } - for (auto& a: axes) { + for (auto& a : axes) { MEASURE_TIME(a.task_times_.can_heartbeat) { if (a.config_.can.heartbeat_rate_ms > 0) { if ((now - a.can_.last_heartbeat) >= a.config_.can.heartbeat_rate_ms) { @@ -382,10 +397,10 @@ bool CANSimple::send_heartbeat(const Axis& axis) { can_setSignal(txmsg, uint8_t(axis.current_state_), 32, 8, true); // Motor flags - uint8_t motorFlags = 0; // reserved + uint8_t motorFlags = 0; // reserved // Encoder flags - uint8_t encoderFlags = 0; // reserved + uint8_t encoderFlags = 0; // reserved // Controller flags uint8_t controllerFlags = 0; diff --git a/Firmware/communication/can/can_simple.hpp b/Firmware/communication/can/can_simple.hpp index b2e14018..83cd0551 100644 --- a/Firmware/communication/can/can_simple.hpp +++ b/Firmware/communication/can/can_simple.hpp @@ -33,6 +33,8 @@ class CANSimple { MSG_GET_VBUS_VOLTAGE, MSG_CLEAR_ERRORS, MSG_SET_LINEAR_COUNT, + MSG_SET_POS_GAIN, + MSG_SET_VEL_GAINS, MSG_CO_HEARTBEAT_CMD = 0x700, // CANOpen NMT Heartbeat SEND }; @@ -74,6 +76,8 @@ 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 set_pos_gain_callback(Axis& axis, const can_Message_t& msg); + static void set_vel_gains_callback(Axis& axis, const can_Message_t& msg); // Other functions static void nmt_callback(const Axis& axis, const can_Message_t& msg); From e0aaa04bdf293d3ebde8255ec6b88faf523276cb Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Sep 2021 00:13:56 -0400 Subject: [PATCH 2/4] Add pos_gain and vel_gain messages to .dbc file --- tools/create_can_dbc.py | 11 +++++++++++ tools/odrive-cansimple.dbc | 10 ++++++++++ 2 files changed, 21 insertions(+) diff --git a/tools/create_can_dbc.py b/tools/create_can_dbc.py index 732dcb0a..fc5d7dd9 100644 --- a/tools/create_can_dbc.py +++ b/tools/create_can_dbc.py @@ -144,6 +144,15 @@ clearErrorsMsg = cantools.database.can.Message(0x018, "Clear_Errors", 0, []) position = cantools.database.can.Signal("Position", 0, 32, is_signed=True) setLinearCountMsg = cantools.database.can.Message(0x019, "Set_Linear_Count", 8, [position]) +# 0x01A - Set Pos gain +posGain = cantools.database.can.Signal("Pos_Gain", 0, 32, is_float=True) +setPosGainMsg = cantools.database.can.Message(0x01A, "Set_Pos_Gain", 8, [posGain]) + +# 0x01B - Set Vel Gains +velGain = cantools.database.can.Signal("Vel_Gain", 0, 32, is_float=True) +velIntGain = cantools.database.can.Signal("Vel_Integrator_Gain", 32, 32, is_float=True) +setVelGainsMsg = cantools.database.can.Message(0x01B, "Set_Vel_gains", 8, [velGain, velIntGain]) + db = cantools.database.can.Database( [ heartbeatMsg, @@ -170,6 +179,8 @@ db = cantools.database.can.Database( getVbusVMsg, clearErrorsMsg, setLinearCountMsg, + setPosGainMsg, + setVelGainsMsg ] ) diff --git a/tools/odrive-cansimple.dbc b/tools/odrive-cansimple.dbc index b16251f5..647aaaa8 100644 --- a/tools/odrive-cansimple.dbc +++ b/tools/odrive-cansimple.dbc @@ -115,6 +115,13 @@ BO_ 24 Clear_Errors: 0 Vector__XXX BO_ 25 Set_Linear_Count: 8 Vector__XXX SG_ Position : 0|32@1- (1,0) [0|0] "" Vector__XXX +BO_ 26 Set_Pos_Gain: 8 Vector__XXX + SG_ Pos_Gain : 0|32@1+ (1,0) [0|0] "" Vector__XXX + +BO_ 27 Set_Vel_gains: 8 Vector__XXX + SG_ Vel_Integrator_Gain : 32|32@1+ (1,0) [0|0] "" Vector__XXX + SG_ Vel_Gain : 0|32@1+ (1,0) [0|0] "" Vector__XXX + @@ -140,5 +147,8 @@ SIG_VALTYPE_ 20 Iq_Measured : 1; SIG_VALTYPE_ 21 Sensorless_Pos_Estimate : 1; SIG_VALTYPE_ 21 Sensorless_Vel_Estimate : 1; SIG_VALTYPE_ 23 Vbus_Voltage : 1; +SIG_VALTYPE_ 26 Pos_Gain : 1; +SIG_VALTYPE_ 27 Vel_Gain : 1; +SIG_VALTYPE_ 27 Vel_Integrator_Gain : 1; From b92677db32009828ba957441ca11d9652ae4f78f Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Sep 2021 00:16:15 -0400 Subject: [PATCH 3/4] Add missing flags to heartbeat message --- tools/create_can_dbc.py | 8 ++++++-- tools/odrive-cansimple.dbc | 5 ++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/tools/create_can_dbc.py b/tools/create_can_dbc.py index fc5d7dd9..ccc0f5b5 100644 --- a/tools/create_can_dbc.py +++ b/tools/create_can_dbc.py @@ -4,9 +4,13 @@ import cantools # 0x001 - Heartbeat axisError = cantools.database.can.Signal("Axis_Error", 0, 32) -axisState = cantools.database.can.Signal("Axis_State", 32, 32) +axisState = cantools.database.can.Signal("Axis_State", 32, 8) +motorFlags = cantools.database.can.Signal("Motor_Flags", 40, 8) +encoderFlags = cantools.database.can.Signal("Encoder_Flags", 48, 8) +controllerFlags = cantools.database.can.Signal("Controller_Flags", 56, 8) + heartbeatMsg = cantools.database.can.Message( - 0x001, "Heartbeat", 8, [axisError, axisState] + 0x001, "Heartbeat", 8, [axisError, axisState, motorFlags, encoderFlags, controllerFlags] ) # 0x002 - E-Stop Message diff --git a/tools/odrive-cansimple.dbc b/tools/odrive-cansimple.dbc index 647aaaa8..01b9b816 100644 --- a/tools/odrive-cansimple.dbc +++ b/tools/odrive-cansimple.dbc @@ -37,7 +37,10 @@ BU_: BO_ 1 Heartbeat: 8 Vector__XXX - SG_ Axis_State : 32|32@1+ (1,0) [0|0] "" Vector__XXX + SG_ Controller_Flags : 56|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ Encoder_Flags : 48|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ Motor_Flags : 40|8@1+ (1,0) [0|0] "" Vector__XXX + SG_ Axis_State : 32|8@1+ (1,0) [0|0] "" Vector__XXX SG_ Axis_Error : 0|32@1+ (1,0) [0|0] "" Vector__XXX BO_ 2 Estop: 0 Vector__XXX From 3e88bc4e3a8da5a7bcde5f2b9bc83b8b8ff88904 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 23 Sep 2021 00:19:20 -0400 Subject: [PATCH 4/4] Update can_protocol docs with gain messages --- docs/can-protocol.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/can-protocol.md b/docs/can-protocol.md index 8bfaa8cb..ac396a0f 100644 --- a/docs/can-protocol.md +++ b/docs/can-protocol.md @@ -71,6 +71,8 @@ CMD ID | Name | Sender | Signals | Start byte | Signal Type | Bits | Factor | Of 0x017 | Get Vbus Voltage | Master\*\*\* | Vbus Voltage | 0 | IEEE 754 Float | 32 | 1 | 0 0x018 | Clear Errors | Master | - | - | - | - | - | - 0x019 | Set Linear Count | Master | Position | 0 | Signed Int | 32 | 1 | 0 +0x01A | Set Position Gain | Master | Pos Gain | 0 | IEEE 754 Float | 32 | 1 | 0 +0x01B | Set Vel Gains | Master | Vel Gain
Vel Integrator Gain | 0
4 | IEEE 754 Float
IEEE 754 Float | 32
32 | 1
1 | 0
0 0x700 | CANOpen Heartbeat Message\*\* | Slave | - | - | - | - | - | - -|-|-|----------------------------------|-|--------------------|-|-|-