Hello,
I’ve been trying to implement a mod to the Odrive 3.6 firmware. I created a new axis attribute consisting of 6 float struct. The reason is so that using the native protocole I can access all the values in one call to increase communication fequency. I need the encoder positions, velocities and motor torques. So far I manage to implement everything except that now I need to write the encode and decode function in protocole.hpp. The problem is that the size of the output buffer is zero and so I cannot encode my structure into it.
here is everything I did :
in my odrive-interface.yaml file I adde this line to Odrive.axis attributes :
sensor_estimates: readonly DualAxisSensorData
in protocol.hpp I adde my struct definition:
typedef struct {
float p0;
float v0;
float t0;
float p1;
float v1;
float t1;
float& operator[](int index) {
switch (index) {
case 0: return p0;
case 1: return v0;
case 2: return t0;
case 3: return p1;
case 4: return v1;
case 5: return t1;
default:
return p0;
}
}
// Also need a const version for reading:
const float& operator[](int index) const {
switch (index) {
case 0: return p0;
case 1: return v0;
case 2: return t0;
case 3: return p1;
case 4: return v1;
case 5: return t1;
default:
return p0;
}
}
} DualAxisSensorData;
in Axis.hpp I defined the attribute and function to udate it :
void set_sensor();
DualAxisSensorData sensor_estimates_ = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
in Axis.cpp i defined the set_sensor function :
void Axis::set_sensor(){
sensor_estimates_[0] = axes[0].encoder_.pos_estimate_.any().value_or(0.0f);
sensor_estimates_[1] = axes[0].encoder_.vel_estimate_.any().value_or(0.0f);
sensor_estimates_[2] = axes[0].controller_.torque_output_.any().value_or(0.0f);
sensor_estimates_[3] = axes[1].encoder_.pos_estimate_.any().value_or(0.0f);
sensor_estimates_[4] = axes[1].encoder_.vel_estimate_.any().value_or(0.0f);
sensor_estimates_[5] = axes[1].controller_.torque_output_.any().value_or(0.0f);
}
in interface_generator I modified the value_types to add mine :
value_types = OrderedDict({
'bool': {'builtin': True, 'fullname': 'bool', 'name': 'bool', 'c_name': 'bool', 'py_type': 'bool'},
'float32': {'builtin': True, 'fullname': 'float32', 'name': 'float32', 'c_name': 'float', 'py_type': 'float'},
'uint8': {'builtin': True, 'fullname': 'uint8', 'name': 'uint8', 'c_name': 'uint8_t', 'py_type': 'int'},
'uint16': {'builtin': True, 'fullname': 'uint16', 'name': 'uint16', 'c_name': 'uint16_t', 'py_type': 'int'},
'uint32': {'builtin': True, 'fullname': 'uint32', 'name': 'uint32', 'c_name': 'uint32_t', 'py_type': 'int'},
'uint64': {'builtin': True, 'fullname': 'uint64', 'name': 'uint64', 'c_name': 'uint64_t', 'py_type': 'int'},
'int8': {'builtin': True, 'fullname': 'int8', 'name': 'int8', 'c_name': 'int8_t', 'py_type': 'int'},
'int16': {'builtin': True, 'fullname': 'int16', 'name': 'int16', 'c_name': 'int16_t', 'py_type': 'int'},
'int32': {'builtin': True, 'fullname': 'int32', 'name': 'int32', 'c_name': 'int32_t', 'py_type': 'int'},
'int64': {'builtin': True, 'fullname': 'int64', 'name': 'int64', 'c_name': 'int64_t', 'py_type': 'int'},
'endpoint_ref': {'builtin': True, 'fullname': 'endpoint_ref', 'name': 'endpoint_ref', 'c_name': 'endpoint_ref_t', 'py_type': '[not implemented]'},
'DualAxisSensorData': {'builtin': True, 'fullname': 'DualAxisSensorData', 'name': 'DualAxisSensorData', 'c_name': 'DualAxisSensorData', 'py_type': 'list'},
})
and finally I modified libfibre.py to decode on the python side:
class DualAxisSensorDataStructCodec():
"""
Serializer/deserializer for the custom DualAxisSensorData struct (6 consecutive floats).
"""
_struct_format = "<ffffff" # Little-endian (<) for 6 floats (f). Total size: 24 bytes.
_length = struct.calcsize(_struct_format)
def get_length(self):
return self._length
def serialize(self, libfibre, value):
# This assumes 'value' is an object (or dict/tuple) containing the six fields.
# Since the higher layer of ODrive is designed to pass field-named objects,
# we try to access them by name.
return struct.pack(self._struct_format,
value.p0, value.v0, value.t0,
value.p1, value.v1, value.t1)
def deserialize(self, libfibre, buffer):
# Deserializing a struct returns a tuple of its primitive values.
# The higher-level ODrive code will map these to the named fields.
return struct.unpack(self._struct_format, buffer)
codecs = {
'int8': StructCodec("<b", int),
'uint8': StructCodec("<B", int),
'int16': StructCodec("<h", int),
'uint16': StructCodec("<H", int),
'int32': StructCodec("<i", int),
'uint32': StructCodec("<I", int),
'int64': StructCodec("<q", int),
'uint64': StructCodec("<Q", int),
'bool': StructCodec("<?", bool),
'float': StructCodec("<f", float),
'object_ref': ObjectPtrCodec(),
'DualAxisSensorData': DualAxisSensorDataStructCodec()
}
for debugging purpose I show the expected and actual buffer size in the RemoteFunction class :
async def async_call(self, args, cancellation_token):
#print("making call on " + hex(args[0]._obj_handle))
tx_buf = bytes()
for i, arg in enumerate(self._inputs):
tx_buf += arg[2].serialize(self._libfibre, args[i])
rx_buf = bytes()
agen = Call(self)
if not cancellation_token is None:
cancellation_token.add_done_callback(agen.cancel)
try:
assert(await agen.asend(None) is None)
is_closed = False
while not is_closed:
tx_buf, rx_chunk, is_closed = await agen.asend((tx_buf, self._rx_size - len(rx_buf), True))
rx_buf += rx_chunk
finally:
if not cancellation_token is None:
cancellation_token.remove_done_callback(agen.cancel)
# --- DEBUGING ---
import sys
print(f"\n[FIBRE DEBUG] Expected RX size (self._rx_size): {self._rx_size}", file=sys.stderr)
print(f"[FIBRE DEBUG] Actual RX size (len(rx_buf)): {len(rx_buf)}", file=sys.stderr)
print(f"[FIBRE DEBUG] Raw Buffer: {rx_buf}", file=sys.stderr)
# --- END DEBUGING ---
assert(len(rx_buf) == self._rx_size)
And on the Odrive I implemented the encoding and decoding here and also use debugging to print the buffer size :
template<> struct Codec<DualAxisSensorData> {
static std::optional<DualAxisSensorData> decode(cbufptr_t* buffer) {
// printf("DEBUG: DualAxisSensorData decode entered.\n");
std::optional<float> p0 = Codec<float>::decode(buffer);
std::optional<float> v0 = Codec<float>::decode(buffer);
std::optional<float> t0 = Codec<float>::decode(buffer);
std::optional<float> p1 = Codec<float>::decode(buffer);
std::optional<float> v1 = Codec<float>::decode(buffer);
std::optional<float> t1 = Codec<float>::decode(buffer);
return (p0.has_value() && v0.has_value() && t0.has_value() && p1.has_value() && v1.has_value() && t1.has_value()) ? std::make_optional(DualAxisSensorData({*p0, *v0, *t0, *p1, *v1, *t1})) : std::nullopt;
}
static bool encode(const DualAxisSensorData& value, bufptr_t* output_buffer) {
printf("DEBUG: buffer size %d\n",output_buffer->size()); // here size is 0
bool success = Codec<float>::encode(value.p0, output_buffer)
&& Codec<float>::encode(value.v0, output_buffer)
&& Codec<float>::encode(value.t0, output_buffer)
&& Codec<float>::encode(value.p1, output_buffer)
&& Codec<float>::encode(value.v1, output_buffer)
&& Codec<float>::encode(value.t1, output_buffer);
if (success) {
printf("DEBUG: encode success.\n");
} else {
// This is where a failure from an internal Codec call will land.
printf("DEBUG: encode fail (Buffer exhausted or internal float codec failure).\n");
}
return success;
}
};
everything build and I can flash the firmware on the Odrive. I can see the attribute but when I try to read it i get the size error :
[FIBRE DEBUG] Expected RX size (self._rx_size): 24
[FIBRE DEBUG] Actual RX size (len(rx_buf)): 0
[FIBRE DEBUG] Raw Buffer: b’’
If you could help me I would be very greatful.
thanks
