Merge pull request #630 from Wetmelon/can_gains

Add pos_gain and vel_gains CAN messages

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) {
+    return canbus_->subscribe(
-        ((CANSimple*)ctx)->handle_can_message(msg);
+        filter, [](void* ctx, const can_Message_t& msg) {
-    }, this, &subscription_handles_[i]);
+            ((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<float>(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<int32_t>(msg, 0, 32, true));
 }
 
+void CANSimple::set_pos_gain_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.config_.pos_gain = can_getSignal<float>(msg, 0, 32, true);
+}
+
+void CANSimple::set_vel_gains_callback(Axis& axis, const can_Message_t& msg) {
+    axis.controller_.config_.vel_gain = can_getSignal<float>(msg, 0, 32, true);
+    axis.controller_.config_.vel_integrator_gain = can_getSignal<float>(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<float>(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])
+        bool node_id_changed = (axes[i].config_.can.node_id != node_ids_[i]) || (axes[i].config_.can.is_extended != extended_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;

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);

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<br>Vel Integrator Gain | 0<br>4 | IEEE 754 Float<br>IEEE 754 Float | 32<br>32 | 1<br>1 | 0<br>0
 0x700 | CANOpen Heartbeat Message\*\* | Slave | - | -  | - | - | - | -
 -|-|-|----------------------------------|-|--------------------|-|-|-
 

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
@@ -144,6 +148,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 +183,8 @@ db = cantools.database.can.Database(
         getVbusVMsg,
         clearErrorsMsg,
         setLinearCountMsg,
+        setPosGainMsg,
+        setVelGainsMsg
     ]
 )
 

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
@@ -115,6 +118,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 +150,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;