Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-21 06:23:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

make float read/write work

Browse Source
This commit is contained in:
Samuel Sadok
2017-10-24 00:51:14 +02:00
parent 392f71d558
commit d02f5fe808
6 changed files with 69 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/commands.cpp
+5 -5
View File
@@ -41,12 +41,12 @@ static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx
// clang-format off
Endpoint endpoints[] = {
Endpoint("vbus_voltage", static_cast<const float>(vbus_voltage)),
Endpoint("elec_rad_per_enc", static_cast<const float>(elec_rad_per_enc)),
Endpoint("vbus_voltage", const_cast<const float*>(&vbus_voltage)),
Endpoint("elec_rad_per_enc", const_cast<const float*>(&elec_rad_per_enc)),
Endpoint("motor0", BEGIN_TREE, nullptr, nullptr, nullptr),
Endpoint("pos_setpoint", motors[0].pos_setpoint),
Endpoint("pos_gain", motors[0].pos_gain),
Endpoint("vel_setpoint", motors[0].vel_setpoint),
Endpoint("pos_setpoint", &motors[0].pos_setpoint),
Endpoint("pos_gain", &motors[0].pos_gain),
Endpoint("vel_setpoint", &motors[0].vel_setpoint),
Endpoint(nullptr, END_TREE, nullptr, nullptr, nullptr) // motor0
};
// clang-format on
Firmware/MotorControl/protocol.cpp
+5 -3
View File
@@ -153,6 +153,7 @@ int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length)
//printf("send payload:\r\n"); osDelay(5); hexdump(buffer, length);
if (output_.write_bytes(buffer, length))
return -1;
//osDelay(5); printf("sent!\r\n"); osDelay(5);
return 0;
}
@@ -194,7 +195,7 @@ void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size
if (input_length < 4)
return;
uint32_t offset32 = 0;
read_le<uint32_t>(offset32, input);
read_le<uint32_t>(&offset32, input);
size_t offset = offset32;
bool need_comma = false;
@@ -241,13 +242,14 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t
// For endpoint 0 this is just the protocol version, for all other endpoints it's a
// CRC over the entire JSON descriptor tree (this may change in future versions).
if (endpoint_id) {
crc16_termination[1] = (json_crc_ >> 0) & 0xff;
crc16_termination[0] = (json_crc_ >> 8) & 0xff;
crc16_termination[0] = (json_crc_ >> 0) & 0xff;
crc16_termination[1] = (json_crc_ >> 8) & 0xff;
}
if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination))) {
//printf("crc16 for endpoint %d failed: expected termination %02x %02x\r\n", endpoint_id, crc16_termination[0], crc16_termination[1]); osDelay(5);
return -1;
}
//printf("crc16 ok\r\n"); osDelay(5);
// TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
Firmware/MotorControl/protocol.hpp
+21 -19
View File
@@ -61,14 +61,14 @@ constexpr uint8_t SYNC_BYTE = '$';
constexpr uint8_t CRC8_INIT = 0x42;
constexpr uint16_t CRC16_INIT = 0x1337;
constexpr uint16_t PROTOCOL_VERSION = 1;
constexpr uint16_t TX_BUF_SIZE = 64;
constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason
template<typename T>
inline size_t write_le(T value, uint8_t* buffer);
template<typename T>
inline size_t read_le(T& value, const uint8_t* buffer);
inline size_t read_le(T* value, const uint8_t* buffer);
template<>
inline size_t write_le<uint16_t>(uint16_t value, uint8_t* buffer) {
@@ -94,15 +94,15 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
}
template<>
inline size_t read_le<uint16_t>(uint16_t& value, const uint8_t* buffer) {
value = (static_cast<uint16_t>(buffer[0]) << 0) |
inline size_t read_le<uint16_t>(uint16_t* value, const uint8_t* buffer) {
*value = (static_cast<uint16_t>(buffer[0]) << 0) |
(static_cast<uint16_t>(buffer[1]) << 8);
return 2;
}
template<>
inline size_t read_le<uint32_t>(uint32_t& value, const uint8_t* buffer) {
value = (static_cast<uint32_t>(buffer[0]) << 0) |
inline size_t read_le<uint32_t>(uint32_t* value, const uint8_t* buffer) {
*value = (static_cast<uint32_t>(buffer[0]) << 0) |
(static_cast<uint32_t>(buffer[1]) << 8) |
(static_cast<uint32_t>(buffer[2]) << 16) |
(static_cast<uint32_t>(buffer[3]) << 24);
@@ -110,17 +110,17 @@ inline size_t read_le<uint32_t>(uint32_t& value, const uint8_t* buffer) {
}
template<>
inline size_t read_le<float>(float& value, const uint8_t* buffer) {
inline size_t read_le<float>(float* value, const uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
return read_le(*reinterpret_cast<uint32_t*>(&value), buffer);
return read_le(reinterpret_cast<uint32_t*>(value), buffer);
}
template<typename T>
static inline T read_le(const uint8_t** buffer, size_t* length) {
T result;
size_t cnt = read_le(result, *buffer);
size_t cnt = read_le(&result, *buffer);
*buffer += cnt;
*length -= cnt;
return result;
@@ -159,14 +159,14 @@ typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length,
template<typename T>
void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
const T* value = reinterpret_cast<const T*>(ctx);
// If the old value was requested, call the corresponding little endian serialization function
if (*output_length) {
uint8_t buffer[8]; // TODO: make buffer size dependent on the type
size_t cnt = write_le<T>(*value, buffer);
if (cnt < *output_length)
*output_length = cnt;
memcpy(output, buffer, *output_length);
if (cnt > *output_length)
cnt = *output_length;
memcpy(output, buffer, cnt);
*output_length -= cnt;
}
}
@@ -180,8 +180,10 @@ void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t
// If a new value was passed, call the corresponding little endian deserialization function
if (input_length) {
uint8_t buffer[8] = { 0 }; // TODO: make buffer size dependent on the type
memcpy(output, buffer, input_length); // TODO: abort if not enough bytes received
read_le<T>(*value, buffer);
if (input_length > sizeof(buffer))
input_length = sizeof(buffer);
memcpy(buffer, input, input_length); // TODO: abort if not enough bytes received
read_le<T>(value, buffer);
}
}
@@ -199,14 +201,14 @@ public:
}
template<typename T>
Endpoint(const char* name, const T& ctx) :
Endpoint(const char* name, const T* ctx) :
Endpoint(name, AS_FLOAT, default_read_endpoint_handler<T>, "\"access\":\"r\"",
const_cast<T*>(&ctx) /* it's safe to cast the const away here because we
const_cast<T*>(ctx) /* it's safe to cast the const away here because we
know that the default_read_endpoint_handler immediately adds it back */) {}
template<typename T>
Endpoint(const char* name, T& ctx) :
Endpoint(name, AS_FLOAT, default_readwrite_endpoint_handler<T>, "\"access\":\"rw\"", &ctx) {}
Endpoint(const char* name, T* ctx) :
Endpoint(name, AS_FLOAT, default_readwrite_endpoint_handler<T>, "\"access\":\"rw\"", ctx) {}
void write_json(size_t id, size_t* skip, uint8_t** output, size_t* output_length, bool* need_comma);
tools/demo.py
+11
View File
@@ -4,6 +4,8 @@ Example usage of the ODrive python library to monitor and control ODrive devices
"""
import odrive.core
import time
import math
# Find a connected ODrive (this will block until you connect one)
my_drive = odrive.core.find_any(printer=print)
@@ -23,6 +25,15 @@ my_drive.motor0.pos_setpoint = 3.14
print("Position setpoint is " + str(my_drive.motor0.pos_setpoint))
# little sine wave to test
t0 = time.monotonic()
while True:
setpoint = 10000.0 * math.sin((time.monotonic() - t0)*2)
print("goto " + str(int(setpoint)))
my_drive.motor0.pos_setpoint = setpoint
time.sleep(0.01)
# Some more things you can try:
# Write to a read-only property:
tools/odrive/core.py
+23 -12
View File
@@ -16,29 +16,33 @@ import time
import os
import odrive.protocol
import itertools
import struct
def noprint(x):
pass
class DeviceProperty(property):
def __init__(self, device, id, type, can_read, can_write):
self._device = device
class SimpleDeviceProperty(property):
def __init__(self, channel, id, type, struct_format, can_read, can_write):
self._channel = channel
self._id = id
self._type = type
self._struct_format = struct_format
property.__init__(self,
self.fget if can_read else None,
self.fset if can_write else None)
def fget(self, obj):
self._device.send("r " + str(self._id) + "\n")
# TODO: message based receive
response = self._device.receive_until('\n')
return self._type(response.strip('\n'))
size = struct.calcsize(self._struct_format)
buffer = self._channel.remote_endpoint_operation(self._id, None, True, size)
return struct.unpack(self._struct_format, buffer)[0]
def fset(self, obj, value):
if not isinstance(value, self._type):
raise TypeError("expected value of type {}".format(self._type.__name__))
self._device.send("w " + str(self._id) + " " + str(value) + "\n")
buffer = struct.pack(self._struct_format, value)
# TODO: Currenly we wait for an ack here. Settle on the default guarantee.
self._channel.remote_endpoint_operation(self._id, buffer, True, 0)
def create_object(json_data, namespace, channel):
@@ -65,12 +69,16 @@ def create_object(json_data, namespace, channel):
else:
if type_str == "float":
property_type = float
struct_format = "<f"
elif type_str == "int":
property_type = int
struct_format = "<i"
elif type_str == "bool":
property_type = bool
struct_format = "<?"
elif type_str == "uint16":
property_type = int
struct_format = "<H"
else:
sys.stderr.write("property {} has unsupported type {}".format(name, type_str))
continue
@@ -81,9 +89,10 @@ def create_object(json_data, namespace, channel):
continue
access_mode = item.get("mode", "rw")
properties[name] = DeviceProperty(channel, id_str, property_type,
'r' in access_mode,
'w' in access_mode)
properties[name] = SimpleDeviceProperty(channel, id_str, property_type,
struct_format,
'r' in access_mode,
'w' in access_mode)
# Create a type from the property list and instantiate it
jit_type = type(namespace, (object,), properties)
@@ -171,12 +180,14 @@ def find_all(printer=noprint):
except (odrive.protocol.TimeoutException, odrive.protocol.ChannelBrokenException):
printer("no response - probably incompatible")
continue
json_crc16 = odrive.protocol.calc_crc16(odrive.protocol.PROTOCOL_VERSION, json_bytes)
channel._interface_definition_crc = struct.pack("<H", json_crc16)
try:
json_string = json_bytes.decode("ascii")
except UnicodeDecodeError:
printer("device responded on endpoint 0 with something that is not ASCII")
continue
print("JSON: " + json_string)
#printer("JSON: " + json_string)
try:
json_data = json.loads(json_string)
except json.decoder.JSONDecodeError:
tools/odrive/protocol.py
+4 -2
View File
@@ -137,6 +137,7 @@ class PacketFromStreamConverter(PacketReader, StreamWriter):
while True:
header = bytes()
# TODO: sometimes this call hangs, even though the device apparently sent something
header = header + self._input.read_bytes_or_fail(1, deadline)
if (header[0] != SYNC_BYTE):
#print("sync byte mismatch")
@@ -153,6 +154,7 @@ class PacketFromStreamConverter(PacketReader, StreamWriter):
continue
packet_length = header[1]
#print("wait for {} bytes".format(packet_length))
return self._input.read_bytes_or_fail(packet_length, deadline)
@@ -193,10 +195,10 @@ class Channel(PacketWriter):
crc16 = calc_crc16(CRC16_INIT, packet)
if (endpoint_id & 0x7fff == 0):
print("append crc16 for " + str(struct.pack('<H', PROTOCOL_VERSION)))
#print("append crc16 for " + str(struct.pack('<H', PROTOCOL_VERSION)))
crc16 = calc_crc16(crc16, struct.pack('<H', PROTOCOL_VERSION))
else:
# TODO: set _interface_definition_crc
#print("append crc16 for " + str(self._interface_definition_crc))
crc16 = calc_crc16(crc16, self._interface_definition_crc)
# append CRC in big endian