From d02f5fe808b3ea44a9796a5ced729944ee936765 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Oct 2017 00:51:14 +0200 Subject: [PATCH] make float read/write work --- Firmware/MotorControl/commands.cpp | 10 ++++---- Firmware/MotorControl/protocol.cpp | 8 +++--- Firmware/MotorControl/protocol.hpp | 40 ++++++++++++++++-------------- tools/demo.py | 11 ++++++++ tools/odrive/core.py | 35 +++++++++++++++++--------- tools/odrive/protocol.py | 6 +++-- 6 files changed, 69 insertions(+), 41 deletions(-) diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 3bf034fa..5cb57341 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -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(vbus_voltage)), - Endpoint("elec_rad_per_enc", static_cast(elec_rad_per_enc)), + Endpoint("vbus_voltage", const_cast(&vbus_voltage)), + Endpoint("elec_rad_per_enc", const_cast(&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 diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index 874608bf..bc118812 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -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(offset32, input); + read_le(&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(&buffer, &length); diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 93b55412..2cb9d064 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -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 inline size_t write_le(T value, uint8_t* buffer); template -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 value, uint8_t* buffer) { @@ -94,15 +94,15 @@ inline size_t write_le(float value, uint8_t* buffer) { } template<> -inline size_t read_le(uint16_t& value, const uint8_t* buffer) { - value = (static_cast(buffer[0]) << 0) | +inline size_t read_le(uint16_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | (static_cast(buffer[1]) << 8); return 2; } template<> -inline size_t read_le(uint32_t& value, const uint8_t* buffer) { - value = (static_cast(buffer[0]) << 0) | +inline size_t read_le(uint32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | (static_cast(buffer[1]) << 8) | (static_cast(buffer[2]) << 16) | (static_cast(buffer[3]) << 24); @@ -110,17 +110,17 @@ inline size_t read_le(uint32_t& value, const uint8_t* buffer) { } template<> -inline size_t read_le(float& value, const uint8_t* buffer) { +inline size_t read_le(float* value, const uint8_t* buffer) { static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - return read_le(*reinterpret_cast(&value), buffer); + return read_le(reinterpret_cast(value), buffer); } template 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 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(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(*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(*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(value, buffer); } } @@ -199,14 +201,14 @@ public: } template - Endpoint(const char* name, const T& ctx) : + Endpoint(const char* name, const T* ctx) : Endpoint(name, AS_FLOAT, default_read_endpoint_handler, "\"access\":\"r\"", - const_cast(&ctx) /* it's safe to cast the const away here because we + const_cast(ctx) /* it's safe to cast the const away here because we know that the default_read_endpoint_handler immediately adds it back */) {} template - Endpoint(const char* name, T& ctx) : - Endpoint(name, AS_FLOAT, default_readwrite_endpoint_handler, "\"access\":\"rw\"", &ctx) {} + Endpoint(const char* name, T* ctx) : + Endpoint(name, AS_FLOAT, default_readwrite_endpoint_handler, "\"access\":\"rw\"", ctx) {} void write_json(size_t id, size_t* skip, uint8_t** output, size_t* output_length, bool* need_comma); diff --git a/tools/demo.py b/tools/demo.py index 08f69034..808d46c6 100755 --- a/tools/demo.py +++ b/tools/demo.py @@ -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: diff --git a/tools/odrive/core.py b/tools/odrive/core.py index bb2ee653..4d048350 100644 --- a/tools/odrive/core.py +++ b/tools/odrive/core.py @@ -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 = "