Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-02-05 22:52:02 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #630 from Wetmelon/can_gains

Browse Source 
Add pos_gain and vel_gains CAN messages
This commit is contained in:
Paul Guenette
2021-09-29 18:26:01 -04:00
committed by GitHub
parent a137541404 3e88bc4e3a
commit 98f7d37b75
5 changed files with 65 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
Firmware/communication/can/can_simple.cpp
View File

@@ -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<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])
|| (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;

4
Firmware/communication/can/can_simple.hpp
View File

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

2
docs/can-protocol.md
View File

@@ -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 | - | - | - | - | - | -
-|-|-|----------------------------------|-|--------------------|-|-|-

19
tools/create_can_dbc.py
View File

@@ -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
]
)

15
tools/odrive-cansimple.dbc
View File

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