mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-17 17:01:58 +08:00
301 lines
12 KiB
C++
301 lines
12 KiB
C++
/*
|
|
see protocol.md for the protocol specification
|
|
*/
|
|
|
|
#ifndef __PROTOCOL_HPP
|
|
#define __PROTOCOL_HPP
|
|
|
|
#include <functional>
|
|
#include <limits>
|
|
#include <cmath>
|
|
//#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <cstring>
|
|
#include <fibre/cpp_utils.hpp>
|
|
#include <fibre/bufptr.hpp>
|
|
#include <fibre/simple_serdes.hpp>
|
|
|
|
|
|
typedef struct {
|
|
uint16_t json_crc;
|
|
uint16_t endpoint_id;
|
|
} endpoint_ref_t;
|
|
|
|
|
|
namespace fibre {
|
|
// These symbols are defined in the autogenerated endpoints.hpp
|
|
extern const unsigned char embedded_json[];
|
|
extern const size_t embedded_json_length;
|
|
extern const uint16_t json_crc_;
|
|
extern const uint32_t json_version_id_;
|
|
bool endpoint_handler(int idx, cbufptr_t* input_buffer, bufptr_t* output_buffer);
|
|
bool endpoint0_handler(cbufptr_t* input_buffer, bufptr_t* output_buffer);
|
|
bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref);
|
|
bool set_endpoint_from_float(endpoint_ref_t endpoint_ref, float value);
|
|
}
|
|
|
|
|
|
|
|
namespace fibre {
|
|
template<typename T, typename = void>
|
|
struct Codec {
|
|
static std::optional<T> decode(cbufptr_t* buffer) { return std::nullopt; }
|
|
};
|
|
|
|
template<> struct Codec<bool> {
|
|
static std::optional<bool> decode(cbufptr_t* buffer) { return (buffer->begin() == buffer->end()) ? std::nullopt : std::make_optional((bool)*(buffer->begin()++)); }
|
|
static bool encode(bool value, bufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<int8_t> {
|
|
static std::optional<int8_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int8_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(int8_t value, bufptr_t* buffer) { return SimpleSerializer<int8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<uint8_t> {
|
|
static std::optional<uint8_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(uint8_t value, bufptr_t* buffer) { return SimpleSerializer<uint8_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<int16_t> {
|
|
static std::optional<int16_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int16_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(int16_t value, bufptr_t* buffer) { return SimpleSerializer<int16_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<uint16_t> {
|
|
static std::optional<uint16_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(uint16_t value, bufptr_t* buffer) { return SimpleSerializer<uint16_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<int32_t> {
|
|
static std::optional<int32_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int32_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(int32_t value, bufptr_t* buffer) { return SimpleSerializer<int32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<uint32_t> {
|
|
static std::optional<uint32_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint32_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(uint32_t value, bufptr_t* buffer) { return SimpleSerializer<uint32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<int64_t> {
|
|
static std::optional<int64_t> decode(cbufptr_t* buffer) { return SimpleSerializer<int64_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(int64_t value, bufptr_t* buffer) { return SimpleSerializer<int64_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<uint64_t> {
|
|
static std::optional<uint64_t> decode(cbufptr_t* buffer) { return SimpleSerializer<uint64_t, false>::read(&(buffer->begin()), buffer->end()); }
|
|
static bool encode(uint64_t value, bufptr_t* buffer) { return SimpleSerializer<uint64_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<float> {
|
|
static std::optional<float> decode(cbufptr_t* buffer) {
|
|
std::optional<uint32_t> int_val = Codec<uint32_t>::decode(buffer);
|
|
return int_val.has_value() ? std::optional<float>(*reinterpret_cast<float*>(&*int_val)) : std::nullopt;
|
|
}
|
|
static bool encode(float value, bufptr_t* buffer) {
|
|
void* ptr = &value;
|
|
return Codec<uint32_t>::encode(*reinterpret_cast<uint32_t*>(ptr), buffer);
|
|
}
|
|
};
|
|
template<typename T>
|
|
struct Codec<T, std::enable_if_t<std::is_enum<T>::value>> {
|
|
static std::optional<T> decode(cbufptr_t* buffer) {
|
|
std::optional<int32_t> int_val = SimpleSerializer<int32_t, false>::read(&(buffer->begin()), buffer->end());
|
|
return int_val.has_value() ? std::make_optional(static_cast<T>(*int_val)) : std::nullopt;
|
|
}
|
|
static bool encode(T value, bufptr_t* buffer) { return SimpleSerializer<int32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
|
|
};
|
|
template<> struct Codec<endpoint_ref_t> {
|
|
static std::optional<endpoint_ref_t> decode(cbufptr_t* buffer) {
|
|
std::optional<uint16_t> val0 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
|
|
std::optional<uint16_t> val1 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
|
|
return (val0.has_value() && val1.has_value()) ? std::make_optional(endpoint_ref_t{*val1, *val0}) : std::nullopt;
|
|
}
|
|
static bool encode(endpoint_ref_t value, bufptr_t* buffer) {
|
|
return SimpleSerializer<uint16_t, false>::write(value.endpoint_id, &(buffer->begin()), buffer->end())
|
|
&& SimpleSerializer<uint16_t, false>::write(value.json_crc, &(buffer->begin()), buffer->end());
|
|
}
|
|
};
|
|
}
|
|
|
|
|
|
/* ToString / FromString functions -------------------------------------------*/
|
|
/*
|
|
* These functions are currently not used by Fibre and only here to
|
|
* support the ODrive ASCII protocol.
|
|
* TODO: find a general way for client code to augment endpoints with custom
|
|
* functions
|
|
*/
|
|
|
|
template<typename T>
|
|
struct format_traits_t;
|
|
|
|
// template<> struct format_traits_t<float> { using type = void;
|
|
// static constexpr const char * fmt = "%f";
|
|
// static constexpr const char * fmtp = "%f";
|
|
// };
|
|
template<> struct format_traits_t<long long> { using type = void;
|
|
static constexpr const char * fmt = "%lld";
|
|
static constexpr const char * fmtp = "%lld";
|
|
};
|
|
template<> struct format_traits_t<unsigned long long> { using type = void;
|
|
static constexpr const char * fmt = "%llu";
|
|
static constexpr const char * fmtp = "%llu";
|
|
};
|
|
template<> struct format_traits_t<long> { using type = void;
|
|
static constexpr const char * fmt = "%ld";
|
|
static constexpr const char * fmtp = "%ld";
|
|
};
|
|
template<> struct format_traits_t<unsigned long> { using type = void;
|
|
static constexpr const char * fmt = "%lu";
|
|
static constexpr const char * fmtp = "%lu";
|
|
};
|
|
template<> struct format_traits_t<int> { using type = void;
|
|
static constexpr const char * fmt = "%d";
|
|
static constexpr const char * fmtp = "%d";
|
|
};
|
|
template<> struct format_traits_t<unsigned int> { using type = void;
|
|
static constexpr const char * fmt = "%ud";
|
|
static constexpr const char * fmtp = "%ud";
|
|
};
|
|
template<> struct format_traits_t<short> { using type = void;
|
|
static constexpr const char * fmt = "%hd";
|
|
static constexpr const char * fmtp = "%hd";
|
|
};
|
|
template<> struct format_traits_t<unsigned short> { using type = void;
|
|
static constexpr const char * fmt = "%hu";
|
|
static constexpr const char * fmtp = "%hu";
|
|
};
|
|
template<> struct format_traits_t<char> { using type = void;
|
|
static constexpr const char * fmt = "%hhd";
|
|
static constexpr const char * fmtp = "%d";
|
|
};
|
|
template<> struct format_traits_t<unsigned char> { using type = void;
|
|
static constexpr const char * fmt = "%hhu";
|
|
static constexpr const char * fmtp = "%u";
|
|
};
|
|
|
|
template<typename T, typename = typename format_traits_t<T>::type>
|
|
static bool to_string(const T& value, char * buffer, size_t length, int) {
|
|
snprintf(buffer, length, format_traits_t<T>::fmtp, value);
|
|
return true;
|
|
}
|
|
// Special case for float because printf promotes float to double, and we get warnings
|
|
template<typename T = float>
|
|
static bool to_string(const float& value, char * buffer, size_t length, int) {
|
|
snprintf(buffer, length, "%f", (double)value);
|
|
return true;
|
|
}
|
|
template<typename T = bool>
|
|
static bool to_string(const bool& value, char * buffer, size_t length, int) {
|
|
buffer[0] = value ? '1' : '0';
|
|
buffer[1] = 0;
|
|
return true;
|
|
}
|
|
template<typename T>
|
|
static bool to_string(const T& value, char * buffer, size_t length, ...) {
|
|
return false;
|
|
}
|
|
|
|
template<typename T, typename = typename format_traits_t<T>::type>
|
|
static bool from_string(const char * buffer, size_t length, T* property, int) {
|
|
// Note for T == uint8_t: Even though we supposedly use the correct format
|
|
// string sscanf treats our pointer as pointer-to-int instead of
|
|
// pointer-to-uint8_t. To avoid an unexpected memory access we first read
|
|
// into a union.
|
|
union { T t; int i; } val;
|
|
if (sscanf(buffer, format_traits_t<T>::fmt, &val.t) == 1) {
|
|
*property = val.t;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
// Special case for float because printf promotes float to double, and we get warnings
|
|
template<typename T = float>
|
|
static bool from_string(const char * buffer, size_t length, float* property, int) {
|
|
return sscanf(buffer, "%f", property) == 1;
|
|
}
|
|
template<typename T = bool>
|
|
static bool from_string(const char * buffer, size_t length, bool* property, int) {
|
|
int val;
|
|
if (sscanf(buffer, "%d", &val) != 1)
|
|
return false;
|
|
*property = val;
|
|
return true;
|
|
}
|
|
template<typename T>
|
|
static bool from_string(const char * buffer, size_t length, T* property, ...) {
|
|
return false;
|
|
}
|
|
|
|
|
|
//template<typename T, typename = typename std>
|
|
//bool set_from_float_ex(float value, T* property) {
|
|
// return false;
|
|
//}
|
|
|
|
namespace conversion {
|
|
//template<typename T>
|
|
template<typename T>
|
|
bool set_from_float_ex(float value, float* property, int) {
|
|
return *property = value, true;
|
|
}
|
|
template<typename T>
|
|
bool set_from_float_ex(float value, bool* property, int) {
|
|
return *property = (value >= 0.0f), true;
|
|
}
|
|
template<typename T, typename = std::enable_if_t<std::is_integral<T>::value && !std::is_const<T>::value>>
|
|
bool set_from_float_ex(float value, T* property, int) {
|
|
return *property = static_cast<T>(std::round(value)), true;
|
|
}
|
|
template<typename T>
|
|
bool set_from_float_ex(float value, T* property, ...) {
|
|
return false;
|
|
}
|
|
template<typename T>
|
|
bool set_from_float(float value, T* property) {
|
|
return set_from_float_ex<T>(value, property, 0);
|
|
}
|
|
}
|
|
|
|
|
|
template<typename T>
|
|
struct Property {
|
|
Property(void* ctx, T(*getter)(void*), void(*setter)(void*, T))
|
|
: ctx_(ctx), getter_(getter), setter_(setter) {}
|
|
Property(T* ctx)
|
|
: ctx_(ctx), getter_([](void* ctx){ return *(T*)ctx; }), setter_([](void* ctx, T val){ *(T*)ctx = val; }) {}
|
|
Property& operator*() { return *this; }
|
|
Property* operator->() { return this; }
|
|
|
|
T read() const {
|
|
return (*getter_)(ctx_);
|
|
}
|
|
|
|
T exchange(std::optional<T> value) const {
|
|
T old_value = (*getter_)(ctx_);
|
|
if (value.has_value()) {
|
|
(*setter_)(ctx_, *value);
|
|
}
|
|
return old_value;
|
|
}
|
|
|
|
void* ctx_;
|
|
T(*getter_)(void*);
|
|
void(*setter_)(void*, T);
|
|
};
|
|
|
|
template<typename T>
|
|
struct Property<const T> {
|
|
Property(void* ctx, T(*getter)(void*))
|
|
: ctx_(ctx), getter_(getter) {}
|
|
Property(const T* ctx)
|
|
: ctx_(const_cast<T*>(ctx)), getter_([](void* ctx){ return *(const T*)ctx; }) {}
|
|
Property& operator*() { return *this; }
|
|
Property* operator->() { return this; }
|
|
|
|
T read() const {
|
|
return (*getter_)(ctx_);
|
|
}
|
|
|
|
void* ctx_;
|
|
T(*getter_)(void*);
|
|
};
|
|
|
|
|
|
#endif
|