diff --git a/README.md b/README.md new file mode 100644 index 0000000..c442590 --- /dev/null +++ b/README.md @@ -0,0 +1,12 @@ +## Configure + + +```bash +cmake -S . -B ./build +``` + +## Build + +```bash +cmake --build ./build -j`nproc` +``` diff --git a/ant/CMakeLists.txt b/ant/CMakeLists.txt index 25b6b6b..f6b833f 100644 --- a/ant/CMakeLists.txt +++ b/ant/CMakeLists.txt @@ -4,17 +4,6 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) -add_executable(test - ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/ant_device.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/channel.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/heart_rate_channel.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/power_channel.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/fitness_equipment_channel.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/message.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/message_processor.cpp -) - add_library(${PROJECT_NAME} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/ant_controller.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/register_types.cpp @@ -23,7 +12,7 @@ add_library(${PROJECT_NAME} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/heart_rate_channel.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/power_channel.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/channel/fitness_equipment_channel.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/message.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/message/message.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/ant/message_processor.cpp ) target_include_directories(${PROJECT_NAME} PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/src") diff --git a/ant/src/ant/ant.hpp b/ant/src/ant/ant.hpp index c675116..56d6465 100644 --- a/ant/src/ant/ant.hpp +++ b/ant/src/ant/ant.hpp @@ -45,6 +45,7 @@ struct FitnessEquipmentPages { kWindResistance, kTrackResistance, kCapabilities = 0x36, + kUserConfiguration = 0x37, }; }; @@ -86,6 +87,7 @@ struct DeviceNumbers { enum : DeviceNumber { Wildcard = 0x0000, LennartsGarminVector3 = 0x5ad0, + LennartsKickr = 0x0c49, LennartsPolar = 0x27f6 }; }; @@ -93,19 +95,27 @@ typedef uint16_t ChannelPeriod; static constexpr DeviceNumber DeviceNumberWildcard = 0x00; -enum class DeviceType : uint8_t { - kAny = 0x00, - kHeartRate = 0x78, - kPower = 0x0B, - kFitnessEquipment = 0x11, +typedef uint8_t DeviceType; +struct DeviceTypes { + enum : DeviceType { + kAny = 0x00, + kHeartRate = 0x78, + kPower = 0x0B, + kFitnessEquipment = 0x11, + + }; }; -enum class ChannelType : uint8_t { - kQuickSearch = 0x10, - kWaiting = 0x20, - kRX = 0x0, - kTX = 0x10, - kPair = 0x40, +typedef uint8_t ChannelType; +struct ChannelTypes { + enum : ChannelType { + kQuickSearch = 0x10, + kWaiting = 0x20, + kRX = 0x0, + kTX = 0x10, + kPair = 0x40, + + }; }; struct ChannelConfig { diff --git a/ant/src/ant/ant_device.cpp b/ant/src/ant/ant_device.cpp index d4f20a1..7a5585e 100644 --- a/ant/src/ant/ant_device.cpp +++ b/ant/src/ant/ant_device.cpp @@ -1,21 +1,92 @@ #include "ant_device.hpp" +#include #include #include #include namespace ant { -ANTDevice::ANTDevice() {} +void ANTDevice::_bind_methods() { + godot::ClassDB::bind_method(godot::D_METHOD("init"), &ANTDevice::Init); + godot::ClassDB::bind_method(godot::D_METHOD("receive"), + &ANTDevice::ReceiveMessage); + ADD_SIGNAL(godot::MethodInfo( + "channel_response_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::OBJECT, "msg_ref"))); + ADD_SIGNAL(godot::MethodInfo( + "broadcast_data_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::OBJECT, "msg_ref"))); + ADD_SIGNAL(godot::MethodInfo( + "device_number_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::INT, "device_number"))); + + godot::ClassDB::bind_method( + godot::D_METHOD("on_channel_response", "channel", "msg"), + &ANTDevice::OnChannelResponse); + + godot::ClassDB::bind_method( + godot::D_METHOD("on_broadcast_data", "channel", "msg"), + &ANTDevice::OnBroadcastDataReceived); + godot::ClassDB::bind_method( + godot::D_METHOD("on_device_number", "channel", "device_number"), + &ANTDevice::OnDeviceNumberReceived); +} + +ANTDevice::ANTDevice() { + processor.instantiate(); + processor->connect("channel_response_received", + godot::Callable(this, "on_channel_response")); + processor->connect("broadcast_data_received", + godot::Callable(this, "on_broadcast_data")); + processor->connect("device_number_received", + godot::Callable(this, "on_device_number")); +} + +godot::PackedStringArray ANTDevice::_get_configuration_warnings() const { + godot::PackedStringArray arr; + godot::Array children = get_children(); + printf("hello!\n"); + std::vector channel_numbers; + for (int i = 0; i < children.size(); ++i) { + godot::Node *node = cast_to(children[i]); + if (node->has_method("get_channel")) { + ChannelNumber channel = node->call("get_channel"); + if (std::find(channel_numbers.begin(), channel_numbers.end(), channel) != + channel_numbers.end()) { + arr.push_back( + godot::String("Channel number not unique: {0}").format({channel})); + } + } + } + return arr; +} + +void ANTDevice::OnChannelResponse(int channel, + godot::Ref msg) { + emit_signal("channel_response_received", channel, msg); +} + +void ANTDevice::OnBroadcastDataReceived(int channel, + godot::Ref msg) { + emit_signal("broadcast_data_received", channel, msg); +} + +void ANTDevice::OnDeviceNumberReceived(int channel, + DeviceNumber device_number) { + emit_signal("device_number_received", channel, device_number); +} bool ANTDevice::Init() { if (!Open()) { return false; } printf("Sending a reset command!\n"); - sleep(1); - SendMessage(Message::SystemReset()); - sleep(1); + // sleep(1); + SendMessage(msg::control::ResetSystem()); uint8_t network = 1; - SendMessage(Message::SetNetworkKey(network, key)); + SendMessage(msg::config::SetNetworkKey(network, key)); while (ReceiveMessage()) ; return true; @@ -25,9 +96,9 @@ bool ANTDevice::ReceiveMessage() { int n; uint8_t buf[1]; do { - n = Read(buf, 1, 500); + n = Read(buf, 1, 1); for (int i = 0; i < n; ++i) { - if (processor.FeedData(buf[i])) { + if (processor->FeedData(buf[i])) { return true; } } diff --git a/ant/src/ant/ant_device.hpp b/ant/src/ant/ant_device.hpp index 6183c04..8914fac 100644 --- a/ant/src/ant/ant_device.hpp +++ b/ant/src/ant/ant_device.hpp @@ -1,5 +1,6 @@ #pragma once -#include "message.hpp" +#include +#include "messages.hpp" #include "message_processor.hpp" #include @@ -15,7 +16,8 @@ #define GARMIN_USB2_VENDOR_ID 0x0fcf #define GARMIN_USB2_PRODUCT_ID 0x1008 namespace ant { -class ANTDevice { +class ANTDevice: public godot::Node { + GDCLASS(ANTDevice, godot::Node) public: ANTDevice(); ~ANTDevice() { Close(); } @@ -26,7 +28,14 @@ public: bool Open(); bool Close(); bool Init(); - MessageProcessor processor; + void OnChannelResponse(int channel, godot::Ref msg); + void OnBroadcastDataReceived(int channel, godot::Ref msg); + void OnDeviceNumberReceived(int channel, DeviceNumber device_number); + godot::PackedStringArray _get_configuration_warnings() const override; + godot::Ref processor; + +protected: + static void _bind_methods(); private: struct Buffer { diff --git a/ant/src/ant/bicycle_power.hpp b/ant/src/ant/bicycle_power.hpp deleted file mode 100644 index 36de00c..0000000 --- a/ant/src/ant/bicycle_power.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once -#include "message.hpp" - -namespace ant { -namespace bicycle_power { -class AdvancedCapabilities { -public: - AdvancedCapabilities(uint8_t _mask = 0xff, uint8_t _value = 0xff, - uint8_t _properties = 0xff) - : mask(_mask), value(_value), properties(_properties) {} - static AdvancedCapabilities FromPayload(BroadcastPayload payload) { - return AdvancedCapabilities(payload.raw_data.at(4), payload.raw_data.at(6), - payload.raw_data.at(2)); - } - bool Supports4Hz() { return SupportsProperty(0); } - bool Supports8Hz() { return SupportsProperty(1); } - bool SupportsAutoZero() { return SupportsProperty(4); } - bool SupportsAutoCrankLength() { return SupportsProperty(5); } - bool SupportsTorqueEfficiencyAndSmoothness() { return SupportsProperty(6); } - - bool Is4HzEnabled() { return IsPropertyEnabled(0); } - bool Is8HzEnabled() { return IsPropertyEnabled(1); } - bool IsAutoZeroEnabled() { return IsPropertyEnabled(4); } - bool IsAutoCrankLengthEnabled() { return IsPropertyEnabled(5); } - bool IsTorqueEfficiencyAndSmoothnessEnabled() { return IsPropertyEnabled(6); } - - void EnableTorqueEfficiencyAndSmoothness() { value &= 0b10111111; } - void UnmaskTorqueEfficiencyAndSmoothness() { mask &= 0b10111111; } - - void SetMask(uint8_t _mask) { mask = _mask; } - uint8_t Mask() { return mask; } - void SetValue(uint8_t _value) { value = _value; } - uint8_t Value() { return value; } - -private: - bool SupportsProperty(uint8_t bit_position) { - return !(mask & (1 << bit_position)); - } - bool IsPropertyEnabled(uint8_t bit_position) { - return !(value & (1 << bit_position)); - } - uint8_t mask; - uint8_t value; - uint8_t properties; -}; -class AdvancedCapabilities2 { -public: - AdvancedCapabilities2(uint8_t _mask = 0xff, uint8_t _value = 0xff) - : mask(_mask), value(_value) {} - - static AdvancedCapabilities2 FromPayload(BroadcastPayload payload) { - return AdvancedCapabilities2(payload.raw_data.at(4), - payload.raw_data.at(6)); - } - bool Supports4Hz() { return SupportsProperty(0); } - bool Supports8Hz() { return SupportsProperty(1); } - bool SupportsPowerPhase() { return SupportsProperty(3); } - bool SupportsPCO() { return SupportsProperty(4); } - bool SupportsRiderPosition() { return SupportsProperty(5); } - bool SupportsTorqueBarycenter() { return SupportsProperty(6); } - - bool Is4HzEnabled() { return IsPropertyEnabled(0); } - bool Is8HzEnabled() { return IsPropertyEnabled(1); } - bool IsPowerPhaseEnabled() { return IsPropertyEnabled(3); } - bool IsPCOEnabled() { return IsPropertyEnabled(4); } - bool IsRiderPositionEnabled() { return IsPropertyEnabled(5); } - bool IsTorqueBarycenterEnabled() { return IsPropertyEnabled(6); } - - void EnableAllDynamics() { value &= 0b10000111; } - void UnmaskAllDynamics() { mask &= 0b10000111; } - void Enable8Hz() { value &= 0b11111101; } - void Unmask8Hz() { mask &= 0b11111101; } - void Enable4Hz() { value &= 0b11111110; } - void Unmask4Hz() { mask &= 0b11111110; } - - void SetMask(uint8_t _mask) { mask = _mask; } - uint8_t Mask() { return mask; } - void SetValue(uint8_t _value) { value = _value; } - uint8_t Value() { return value; } - -private: - bool SupportsProperty(uint8_t bit_position) { - // 0: does support - // 1: does NOT support - return !(mask & (1 << bit_position)); - } - bool IsPropertyEnabled(uint8_t bit_position) { - return !(value & (1 << bit_position)); - } - uint8_t mask; - uint8_t value; -}; - -} // namespace bicycle_power -} // namespace ant diff --git a/ant/src/ant/channel/channel.cpp b/ant/src/ant/channel/channel.cpp index 17c9578..2c00f94 100644 --- a/ant/src/ant/channel/channel.cpp +++ b/ant/src/ant/channel/channel.cpp @@ -2,31 +2,71 @@ #include namespace ant { -Channel::Channel(ANTDevice &_ant_device, ChannelNumber _channel_id, - DeviceNumber _device_number) - : ant_device_(_ant_device) { - channel_config_.frequency = 57; - channel_config_.transmission_type = 0; - channel_config_.device_number = _device_number; - id_ = _channel_id; +void Channel::_bind_methods() { + godot::ClassDB::bind_method( + godot::D_METHOD("set_device_number", "_device_number"), + &Channel::SetDeviceNumber); + godot::ClassDB::bind_method(godot::D_METHOD("get_device_number"), + &Channel::GetDeviceNumber); + ADD_PROPERTY(godot::PropertyInfo(godot::Variant::INT, "device_number"), + "set_device_number", "get_device_number"); - _ant_device.processor.SetOnChannelResponseCallback( - _channel_id, - [this](ChannelResponseMessage msg) { OnChannelResponse(msg); }); + godot::ClassDB::bind_method(godot::D_METHOD("set_channel", "_channel"), + &Channel::SetDeviceNumber); + godot::ClassDB::bind_method(godot::D_METHOD("get_channel"), + &Channel::GetDeviceNumber); + ADD_PROPERTY(godot::PropertyInfo(godot::Variant::INT, "channel"), + "set_channel", "get_channel"); - _ant_device.processor.SetOnBroadcastDataCallback( - _channel_id, [this](BroadcastPayload msg) { OnBroadcastData(msg); }); + ADD_SIGNAL(godot::MethodInfo("connected")); + ADD_SIGNAL(godot::MethodInfo("disconnected")); + ADD_SIGNAL(godot::MethodInfo("search_timed_out")); + + godot::ClassDB::bind_method(godot::D_METHOD("on_broadcast_data", "data"), + &Channel::OnBroadcastData); + godot::ClassDB::bind_method(godot::D_METHOD("on_channel_response", "data"), + &Channel::OnChannelResponse); + godot::ClassDB::bind_method(godot::D_METHOD("start_searching"), + &Channel::StartSearch); } -Channel::~Channel() {} +Channel::Channel() { + channel_config_.frequency = 57; + channel_config_.transmission_type = 0; + device_number_ = DeviceNumbers::Wildcard; + channel_ = 0; +} + +godot::PackedStringArray Channel::_get_configuration_warnings() const { + godot::PackedStringArray arr; + return arr; +} + +void Channel::Initialize() { + parent_ = cast_to(get_parent()); + if (parent_ == nullptr) { + godot::UtilityFunctions::push_error(godot::String("Parent is null!")); + return; + } + godot::UtilityFunctions::push_warning(parent_->get_name()); +} + +void Channel::_notification(const int p_what) { + switch (p_what) { + case NOTIFICATION_ENTER_TREE: + Initialize(); + break; + case NOTIFICATION_EXIT_TREE: + parent_ = nullptr; + break; + } +} void Channel::StartSearch() { SetChannelState(ChannelState::kSearching); printf("Assigning Channel\n"); - int n = 0; - n = ant_device_.SendMessage( - Message::AssignChannel(id_, channel_config_.type, 1)); - printf("Sent %d bytes.\n", n); + msg::config::AssignChannel msg(channel_, channel_config_.type, 1); + parent_->SendMessage(msg); } bool Channel::SendBuffered(const Message &_msg) { @@ -36,63 +76,65 @@ bool Channel::SendBuffered(const Message &_msg) { } if (ack_send_queue_idle_) { printf("queue is idle. Sending immediately.\n"); - ant_device_.SendMessage(ack_send_queue_.front()); + parent_->SendMessage(ack_send_queue_.front()); ack_send_queue_idle_ = false; } return true; } void Channel::OnSearchTimeout() { - printf("[channel %hu] search timed out.\n", id_); - printf("[channel %hu] restarting search.\n", id_); - StartSearch(); + printf("[channel %hu] search timed out.\n", channel_); + emit_signal("search_timed_out"); } -void Channel::OnChannelResponse(ChannelResponseMessage msg) { - if (msg.channel_id_ != id_) { +void Channel::OnChannelResponse(godot::Ref _msg) { + if (_msg->channel_id_ != channel_) { fprintf( stderr, "Channel %hu: Received msg for %hu. This should not be possible...\n", - id_, msg.channel_id_); + channel_, _msg->channel_id_); return; } if (state_ == ChannelState::kConnected) { - if (msg.msg_id_ == MessageIDs::kRFEvent) { - switch (msg.msg_code_) { + if (_msg->msg_id_ == MessageIDs::kRFEvent) { + switch (_msg->msg_code_) { case MessageCodes::kEventRXFail: { rx_fail_count_++; break; } + case MessageCodes::kEventRXFailGoToSearch: + SetChannelState(ChannelState::kSearching); + break; case MessageCodes::kEventTransferTXCompleted: { - printf("[channel %hu] acknowledgment received.\n", id_); + printf("[channel %hu] acknowledgment received.\n", channel_); // we can now send the next message if we have some more in the buffer std::lock_guard lock(ack_send_queue_mutex_); // we have successfully sent the last message. so remove it from the // queue. ack_send_queue_.pop(); if (!ack_send_queue_.empty()) { - ant_device_.SendMessage(ack_send_queue_.front()); + parent_->SendMessage(ack_send_queue_.front()); } else { ack_send_queue_idle_ = true; } break; } case MessageCodes::kEventTransferTXFailed: { - printf("[channel %hu] failed to receive acknowledgment.\n", id_); + printf("[channel %hu] failed to receive acknowledgment.\n", channel_); std::lock_guard lock(ack_send_queue_mutex_); if (!ack_send_queue_.empty()) { ++ack_retries_; if (ack_retries_ < 3) { - ant_device_.SendMessage(ack_send_queue_.front()); + parent_->SendMessage(ack_send_queue_.front()); } else { ack_retries_ = 0; printf("[channel %hu] Giving up to send acknowledged message after " "max retries.\n", - id_); + channel_); ack_send_queue_.pop(); if (ack_send_queue_.empty()) { ack_send_queue_idle_ = true; } else { - ant_device_.SendMessage(ack_send_queue_.front()); + parent_->SendMessage(ack_send_queue_.front()); } } } else { @@ -103,93 +145,120 @@ void Channel::OnChannelResponse(ChannelResponseMessage msg) { default: printf("[channel %hu] Connected and received channel response: 0x%02x " "%hu\n", - id_, msg.msg_id_, msg.msg_code_); + channel_, _msg->msg_id_, _msg->msg_code_); break; } } return; } + if (state_ == ChannelState::kSearchTimedOut) { + if (_msg->msg_id_ == MessageIDs::kRFEvent && + _msg->msg_code_ == MessageCodes::kEventChannelClosed) { + printf("[channel %hu] closed because of timeout.\n", channel_); + auto msg = msg::config::UnassignChannel(channel_); + parent_->SendMessage(msg); + SetChannelState(ChannelState::kClosed); + OnSearchTimeout(); + } + } if (state_ == ChannelState::kSearching) { - // printf("Channel Response: 0x%02x %hu\n", msg.msg_id_, msg.msg_code_); - switch (msg.msg_id_) { + // printf("Channel Response: 0x%02x %hu\n", msg.msg_channel_, + // msg.msg_code_); + switch (_msg->msg_id_) { // RF Event case MessageIDs::kRFEvent: - switch (msg.msg_code_) { + switch (_msg->msg_code_) { case MessageCodes::kEventRXSearchTimeout: // nothing to do state wise. we will receive a channel closed event - fprintf(stderr, "[channel %hu] Search timed out.\n", id_); - break; - case MessageCodes::kEventChannelClosed: - printf("[channel %hu] closed.\n", id_); - SetChannelState(ChannelState::kClosed); + SetChannelState(ChannelState::kSearchTimedOut); break; } break; - case MessageIDs::kAssignChannel: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to assign channel.\n", id_); + case MessageIDs::kAssignChannel: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to assign channel.\n", channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage(Message::SetChannelID( - id_, channel_config_.device_number, channel_config_.device_type, - channel_config_.transmission_type)); + printf("[channel %hu] channel assigned.\n", channel_); + msg::config::ChannelID msg(channel_, channel_config_.device_number, + channel_config_.device_type, + channel_config_.transmission_type); + parent_->SendMessage(msg); break; - case MessageIDs::kChannelID: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to set channel id!\n", id_); + } + case MessageIDs::kChannelID: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel id!\n", channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage( - Message::SetChannelFrequency(id_, channel_config_.frequency)); + printf("[channel %hu] channel id set.\n", channel_); + msg::config::ChannelRFFrequency msg(channel_, channel_config_.frequency); + parent_->SendMessage(msg); break; - case MessageIDs::kChannelFrequency: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { + } + case MessageIDs::kChannelFrequency: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { fprintf(stderr, "[channel %hu] failed to set channel frequency!\n", - id_); + channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage( - Message::SetChannelPeriod(id_, channel_config_.channel_period)); + printf("[channel %hu] channel frequency set\n", channel_); + msg::config::ChannelPeriod msg(channel_, channel_config_.channel_period); + parent_->SendMessage(msg); break; - case MessageIDs::kChannelPeriod: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to set channel period!\n", id_); + } + case MessageIDs::kChannelPeriod: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel period!\n", + channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage(Message::SetLPSearchTimeout(id_, 4)); + printf("[channel %hu] channel period set.\n", channel_); + msg::config::LowPrioritySearchTimeout msg(channel_, 4); + parent_->SendMessage(msg); break; - case MessageIDs::kLPSearchTimeout: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to set channel period!\n", id_); + } + case MessageIDs::kLPSearchTimeout: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel period!\n", + channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage(Message::SetHPSearchTimeout(id_, 0)); + printf("[channel %hu] low priority search timeout set.\n", channel_); + msg::config::SearchTimeout msg(channel_, 0); + parent_->SendMessage(msg); break; - case MessageIDs::kSearchTimeout: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to set channel timeout\n", id_); + } + case MessageIDs::kSearchTimeout: { + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel timeout\n", + channel_); SetChannelState(ChannelState::kUndefined); return; } - ant_device_.SendMessage(Message::OpenChannel(id_)); + printf("[channel %hu] High priority search timeout set\n", channel_); + msg::control::OpenChannel msg(channel_); + parent_->SendMessage(msg); break; + } case MessageIDs::kOpenChannel: - if (msg.msg_code_ != MessageCodes::kResponseNoError) { - fprintf(stderr, "[channel %hu] failed to open channel\n", id_); + if (_msg->msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to open channel\n", channel_); SetChannelState(ChannelState::kUndefined); return; } + printf("[channel %hu] channel opened.\n", channel_); SetChannelState(ChannelState::kSearching); break; default: printf("[channel %hu] Unhandled Channel Response with id: 0x%02x code: " "%hu\n", - id_, msg.msg_id_, msg.msg_code_); + channel_, _msg->msg_id_, _msg->msg_code_); break; } } diff --git a/ant/src/ant/channel/channel.hpp b/ant/src/ant/channel/channel.hpp index 1ab512d..647b7af 100644 --- a/ant/src/ant/channel/channel.hpp +++ b/ant/src/ant/channel/channel.hpp @@ -2,15 +2,19 @@ #include "../ant.hpp" #include "../ant_device.hpp" +#include #include #include namespace ant { +typedef std::function SearchTimeoutCallback; + enum class ChannelState { kClosed, kOpening, kSearching, + kSearchTimedOut, kConnected, kUndefined, }; @@ -45,19 +49,54 @@ private: std::queue queue_; }; -class Channel { +class Channel : public godot::Node { + GDCLASS(Channel, godot::Node) public: - Channel(ANTDevice &, ChannelNumber, DeviceNumber); - virtual ~Channel() = 0; + Channel(); + virtual ~Channel() {} + + godot::PackedStringArray _get_configuration_warnings() const override; + void StartSearch(); + void Close() { + msg::control::CloseChannel msg(channel_); + parent_->SendMessage(msg); + } void OnSearchTimeout(); - virtual void OnBroadcastData(BroadcastPayload) = 0; - void OnChannelResponse(ChannelResponseMessage); - ChannelNumber channel_id() const { return id_; } + void SetOnSearchTimeoutCallback(SearchTimeoutCallback _callback) { + search_timeout_cb_ = _callback; + } + virtual void OnBroadcastData(godot::Ref) {} + void OnChannelResponse(godot::Ref); + ChannelNumber channel_id() const { return channel_; } bool SendBuffered(const Message &); protected: - void SetChannelState(ChannelState _state) { state_ = _state; } + static void _bind_methods(); + void _notification(const int p_what); + void SetChannelState(ChannelState _state) { + if (_state != state_) { + switch (_state) { + case ant::ChannelState::kClosed: + emit_signal("disconnected"); + break; + case ChannelState::kSearchTimedOut: + // emit_signal("search_timed_out"); + break; + case ChannelState::kConnected: + emit_signal("connected"); + break; + } + } + state_ = _state; + } + void SetDeviceNumber(const DeviceNumber device_number) { + device_number_ = device_number; + } + DeviceNumber GetDeviceNumber() const { return device_number_; } + + void SetChannel(const ChannelNumber channel) { channel_ = channel; } + ChannelNumber GetChannel() const { return channel_; } MessageQueue<10> ack_send_queue_; std::mutex ack_send_queue_mutex_; @@ -66,10 +105,17 @@ protected: uint64_t rx_fail_count_{0}; ChannelConfig channel_config_; - ANTDevice ant_device_; - ::ant::ChannelNumber id_; + ANTDevice *parent_; ChannelState state_{ChannelState::kUndefined}; bool connected_{false}; + SearchTimeoutCallback search_timeout_cb_{nullptr}; + + // PROPERTIES + ChannelNumber channel_; + DeviceNumber device_number_; + +private: + void Initialize(); }; } // namespace ant diff --git a/ant/src/ant/channel/fitness_equipment_channel.cpp b/ant/src/ant/channel/fitness_equipment_channel.cpp index bf9b9b0..9578709 100644 --- a/ant/src/ant/channel/fitness_equipment_channel.cpp +++ b/ant/src/ant/channel/fitness_equipment_channel.cpp @@ -1,45 +1,59 @@ #include "fitness_equipment_channel.hpp" #include "../common.hpp" -#include "../fitness_equipment.hpp" namespace ant { -FitnessEquipmentChannel::FitnessEquipmentChannel(ANTDevice &_ant_device, - ChannelNumber _channel_id, - DeviceNumber _device_number) - : Channel(_ant_device, _channel_id, _device_number) { - channel_config_.type = ChannelType::kRX; +void FitnessEquipmentChannel::_bind_methods() { + godot::ClassDB::bind_method( + godot::D_METHOD("set_track_resistance", "slope", "resistance_coeff"), + &FitnessEquipmentChannel::SetTrackResistance); + godot::ClassDB::bind_method(godot::D_METHOD("set_user_config", "user_weight", + "bicycle_weight", + "wheel_diameter", "gear_ratio"), + &FitnessEquipmentChannel::SetUserConfiguration); + + ADD_SIGNAL(godot::MethodInfo( + "power_received", godot::PropertyInfo(godot::Variant::INT, "power"))); + ADD_SIGNAL(godot::MethodInfo( + "cadence_received", godot::PropertyInfo(godot::Variant::INT, "cadence"))); +} + +FitnessEquipmentChannel::FitnessEquipmentChannel() { + channel_config_.type = ChannelTypes::kRX; channel_config_.transmission_type = 0; - channel_config_.device_type = DeviceType::kFitnessEquipment; + channel_config_.device_type = DeviceTypes::kFitnessEquipment; channel_config_.channel_period = 8192; } -void FitnessEquipmentChannel::OnBroadcastData(BroadcastPayload payload) { +void FitnessEquipmentChannel::OnBroadcastData( + godot::Ref _payload) { + BroadcastPayload payload(**_payload); if (state_ != ChannelState::kConnected) { printf("Received first broadcast data!\n"); SetChannelState(ChannelState::kConnected); - ant_device_.SendMessage(Message::RequestChannelID(id_)); + msg::common::RequestChannelID msg(channel_); + SendBuffered(msg); } uint8_t page_number = payload.raw_data[0]; switch (page_number) { case FitnessEquipmentPages::kCalibrationRequestAndResponse: { - printf("[channel %hu] CalibrationRequestAndResponse received.\n", id_); + printf("[channel %hu] CalibrationRequestAndResponse received.\n", channel_); break; } case FitnessEquipmentPages::kCalibrationInProgress: { - printf("[channel %hu] CalibrationInProgress received.\n", id_); + printf("[channel %hu] CalibrationInProgress received.\n", channel_); break; } case FitnessEquipmentPages::kGeneralData: { - printf("[channel %hu] GeneralData received.\n", id_); + printf("[channel %hu] GeneralData received.\n", channel_); break; } case FitnessEquipmentPages::kGeneralSettings: { - printf("[channel %hu] GeneralSettings received.\n", id_); + printf("[channel %hu] GeneralSettings received.\n", channel_); break; } case FitnessEquipmentPages::kGeneralMetabolicData: { - printf("[channel %hu] MetabolicData received.\n", id_); + printf("[channel %hu] MetabolicData received.\n", channel_); break; } case FitnessEquipmentPages::kBikeSepcificData: { diff --git a/ant/src/ant/channel/fitness_equipment_channel.hpp b/ant/src/ant/channel/fitness_equipment_channel.hpp index c656272..0d80010 100644 --- a/ant/src/ant/channel/fitness_equipment_channel.hpp +++ b/ant/src/ant/channel/fitness_equipment_channel.hpp @@ -1,31 +1,33 @@ #pragma once #include "../common.hpp" -#include "../fitness_equipment.hpp" +#include "../profile/fitness_equipment.hpp" #include "channel.hpp" #include namespace ant { class FitnessEquipmentChannel : public Channel { + GDCLASS(FitnessEquipmentChannel, Channel); + public: - FitnessEquipmentChannel(ANTDevice &, ChannelNumber, DeviceNumber); + FitnessEquipmentChannel(); ~FitnessEquipmentChannel() {} - void OnBroadcastData(BroadcastPayload) final; + void OnBroadcastData(godot::Ref) final; bool SetTargetPower(uint16_t power) { - auto msg = Message::FitnessEquipmentTargetPower(id_, power * 4); + auto msg = Message::FitnessEquipmentTargetPower(channel_, power * 4); return SendBuffered(msg); } bool RequestCapabilies(uint8_t requested_transmissions) { auto msg = Message::FitnessEquipmentRequestCapabilities( - id_, requested_transmissions); + channel_, requested_transmissions); return SendBuffered(msg); } bool RequestCommandStatus(uint8_t requested_transmissions) { auto msg = - Message::CommonRequestCommandStatus(id_, requested_transmissions); + Message::CommonRequestCommandStatus(channel_, requested_transmissions); return SendBuffered(msg); } @@ -38,19 +40,35 @@ public: drafting_factor = std::clamp(drafting_factor, 0.0, 1.0); uint8_t drafting_raw = static_cast(drafting_factor * 100.0); auto msg = Message::FitnessEquipmentWindResistance( - id_, coeff_raw, windspeed_raw, drafting_raw); + channel_, coeff_raw, windspeed_raw, drafting_raw); return SendBuffered(msg); } bool SetTrackResistance(double slope, double resistance_coeff) { + slope = std::clamp(slope, -20.0, 20.0); uint16_t slope_raw = (slope + 200.0) * 100.0; resistance_coeff = std::clamp(resistance_coeff, 0.0, 0.0127); uint8_t resistance_coeff_raw = resistance_coeff * 0.2 * 100000; - auto msg = Message::FitnessEquipmentTrackResistance(id_, slope_raw, + auto msg = Message::FitnessEquipmentTrackResistance(channel_, slope_raw, resistance_coeff_raw); return SendBuffered(msg); } + bool SetUserConfiguration(double user_weight, double bicycle_weight, + double wheel_diameter, double gear_ratio) { + user_weight = std::clamp(user_weight, 0.0, 655.0); + bicycle_weight = std::clamp(bicycle_weight, 0.0, 50.0); + wheel_diameter = std::clamp(wheel_diameter, 0.0, 2.5); + gear_ratio = std::clamp(gear_ratio, 0.03, 7.65); + uint16_t user_weight_raw = 100.0 * user_weight; + uint16_t bicycle_weight_raw = 20.0 * bicycle_weight; + uint8_t wheel_diameter_raw = 100.0 * wheel_diameter; + uint8_t gear_ratio_raw = 33.3333333333333333333 * gear_ratio; + SendBuffered(Message::FitnessEquipmentUserConfiguration( + channel_, user_weight_raw, bicycle_weight_raw, wheel_diameter_raw, + gear_ratio_raw)); + } + void PrintCommandStatus(const common::CommandStatusPage &); void PrintCapabilities(const fitness_equipment::Capabilities &); @@ -59,6 +77,9 @@ public: cadence_cb_ = _cb; } +protected: + static void _bind_methods(); + private: uint8_t bike_data_event_count_{0}; uint16_t bike_data_accumulated_power_{0}; diff --git a/ant/src/ant/channel/heart_rate_channel.cpp b/ant/src/ant/channel/heart_rate_channel.cpp index 0651d18..02e8988 100644 --- a/ant/src/ant/channel/heart_rate_channel.cpp +++ b/ant/src/ant/channel/heart_rate_channel.cpp @@ -2,28 +2,31 @@ namespace ant { -HeartRateChannel::HeartRateChannel(ANTDevice &_ant_device, - ChannelNumber _channel_id, - DeviceNumber _device_number) - : Channel(_ant_device, _channel_id, _device_number) { - channel_config_.type = ChannelType::kRX; +void HeartRateChannel::_bind_methods() { + ADD_SIGNAL(godot::MethodInfo( + "heart_rate_updated", + godot::PropertyInfo(godot::Variant::INT, "heart_rate"))); +} + +HeartRateChannel::HeartRateChannel() { + channel_config_.type = ChannelTypes::kRX; channel_config_.transmission_type = 0; - channel_config_.device_type = DeviceType::kHeartRate; + channel_config_.device_type = DeviceTypes::kHeartRate; channel_config_.channel_period = 8070; } -void HeartRateChannel::OnBroadcastData(BroadcastPayload data) { +void HeartRateChannel::OnBroadcastData(godot::Ref data) { if (state_ != ChannelState::kConnected) { printf("Received first broadcast data!\n"); SetChannelState(ChannelState::kConnected); - ant_device_.SendMessage(Message::RequestChannelID(id_)); + emit_signal("connected"); + msg::common::RequestChannelID msg(channel_); + SendBuffered(msg); } // mask out the toggle bit. only the first 7 bits declare the page number - uint8_t page_number = data.raw_data[0] & (~0x80); - bool page_toggled = data.raw_data[0] & 0x80; - uint8_t heart_rate = data.raw_data[7]; - if (heart_rate_cb_) { - heart_rate_cb_(heart_rate); - } + uint8_t page_number = data->raw_data[0] & (~0x80); + bool page_toggled = data->raw_data[0] & 0x80; + uint8_t heart_rate = data->raw_data[7]; + emit_signal("heart_rate_updated", heart_rate); printf("Heart Rate: %hu\n", heart_rate); } diff --git a/ant/src/ant/channel/heart_rate_channel.hpp b/ant/src/ant/channel/heart_rate_channel.hpp index 771c2d8..1d3236a 100644 --- a/ant/src/ant/channel/heart_rate_channel.hpp +++ b/ant/src/ant/channel/heart_rate_channel.hpp @@ -7,11 +7,15 @@ namespace ant { typedef std::function HeartRateCallback; class HeartRateChannel : public Channel { + GDCLASS(HeartRateChannel, Channel) public: - HeartRateChannel(ANTDevice &, ChannelNumber, DeviceNumber); + HeartRateChannel(); ~HeartRateChannel() {} - void OnBroadcastData(BroadcastPayload) final; - void SetOnHeartRateCallback(HeartRateCallback cb) {heart_rate_cb_ = cb;} + void OnBroadcastData(godot::Ref) final; + void SetOnHeartRateCallback(HeartRateCallback cb) { heart_rate_cb_ = cb; } + +protected: + static void _bind_methods(); private: HeartRateCallback heart_rate_cb_{nullptr}; diff --git a/ant/src/ant/channel/power_channel.cpp b/ant/src/ant/channel/power_channel.cpp index 8644253..fc70c1b 100644 --- a/ant/src/ant/channel/power_channel.cpp +++ b/ant/src/ant/channel/power_channel.cpp @@ -2,58 +2,104 @@ namespace ant { -PowerChannel::PowerChannel(ANTDevice &_ant_device, ChannelNumber channel_id, - DeviceNumber device_number) - : Channel(_ant_device, channel_id, device_number) { - channel_config_.type = ChannelType::kRX; - channel_config_.frequency = 57; - channel_config_.transmission_type = 0; - channel_config_.device_type = DeviceType::kPower; - channel_config_.channel_period = 8192; +void PowerChannel::_bind_methods() { + ADD_SIGNAL(godot::MethodInfo( + "battery_status_received", + godot::PropertyInfo(godot::Variant::INT, "operating_time"), + godot::PropertyInfo(godot::Variant::STRING, "status"), + godot::PropertyInfo(godot::Variant::FLOAT, "voltage"))); + ADD_SIGNAL( + godot::MethodInfo("crank_torque_received", + godot::PropertyInfo(godot::Variant::FLOAT, "power"), + godot::PropertyInfo(godot::Variant::FLOAT, "torque"), + godot::PropertyInfo(godot::Variant::INT, "cadence"))); + ADD_SIGNAL(godot::MethodInfo( + "power_received", godot::PropertyInfo(godot::Variant::FLOAT, "power"), + godot::PropertyInfo(godot::Variant::INT, "cadence"))); } -void PowerChannel::OnBroadcastData(BroadcastPayload data) { +PowerChannel::PowerChannel() { + channel_config_.type = ChannelTypes::kRX; + channel_config_.frequency = 57; + channel_config_.transmission_type = 0; + channel_config_.device_type = DeviceTypes::kPower; + channel_config_.channel_period = 8070; +} + +void PowerChannel::OnBroadcastData(godot::Ref data) { if (state_ != ChannelState::kConnected) { // First data received from master printf("Received first broadcast data!\n"); SetChannelState(ChannelState::kConnected); - SendBuffered(Message::RequestChannelID(id_)); - //bicycle_power::AdvancedCapabilities2 caps2; - // caps2.EnableAllDynamics(); - // caps2.UnmaskAllDynamics(); - // caps2.Enable8Hz(); - // caps2.Unmask8Hz(); - // SetAdvancedCapabilities2(caps2); - // bicycle_power::AdvancedCapabilities caps; - // RequestAdvancedCapabilities2(); - // RequestAdvancedCapabilities1(); - // caps.EnableTorqueEfficiencyAndSmoothness(); - // caps.UnmaskTorqueEfficiencyAndSmoothness(); - // SetAdvancedCapabilities(caps); + msg::common::RequestChannelID msg(channel_); + SendBuffered(msg); + // bicycle_power::AdvancedCapabilities2 caps2; + // caps2.EnableAllDynamics(); + // caps2.UnmaskAllDynamics(); + // caps2.Enable8Hz(); + // caps2.Unmask8Hz(); + // SetAdvancedCapabilities2(caps2); + // bicycle_power::AdvancedCapabilities caps; + // RequestAdvancedCapabilities2(); + // RequestAdvancedCapabilities1(); + // caps.EnableTorqueEfficiencyAndSmoothness(); + // caps.UnmaskTorqueEfficiencyAndSmoothness(); + // SetAdvancedCapabilities(caps); } - uint8_t page_number = data.raw_data[0]; + uint8_t page_number = data->raw_data[0]; + //printf("[channel %hu] received page: %hu\n", channel_, page_number); switch (page_number) { - printf("[channel %hu] received page: %hu\n", id_, page_number); case PowerProfilePages::kBattery: { - BatteryStatusMessage msg(BatteryStatusMessage::FromPayload(data.raw_data)); + BatteryStatusMessage msg(BatteryStatusMessage::FromPayload(data->raw_data)); printf("BatteryState: %s\n", msg.Status().c_str()); printf("BatteryVoltage: %f\n", msg.battery_voltage); printf("BatteryTime: %02u:%02u:%02u\n", msg.operating_time / 3600, (msg.operating_time % 3600) / 60, (msg.operating_time % 60)); + emit_signal("battery_status_received", msg.operating_time, + godot::String(msg.Status().c_str()), msg.battery_voltage); break; } + case PowerProfilePages::kStandardPower: { + bicycle_power::StandardPowerOnly msg = + bicycle_power::StandardPowerOnly::FromRawData(data->raw_data); + if (power_msg_) { + double power = msg.AveragePower(power_msg_->accumulated_power(), + power_msg_->event_count()); + uint8_t cadence = msg.inst_cadence(); + emit_signal("power_received", power, cadence); + } + power_msg_ = std::make_unique(msg); + break; + } case PowerProfilePages::kStandardTorqueAtCrank: { - printf("[channel %hu] received StandardTorqueAtCrank\n", id_); + bicycle_power::StandardCrankTorque msg = + bicycle_power::StandardCrankTorque::FromPayload(**data); + if (crank_torque_msg_) { + if (crank_torque_msg_->event_count() != msg.event_count()) { + double power = + msg.AveragePower(crank_torque_msg_->raw_accumulated_torque(), + crank_torque_msg_->raw_crank_period()); + double torque = + msg.AverageTorque(crank_torque_msg_->event_count(), + crank_torque_msg_->raw_accumulated_torque()); + int cadence = msg.AverageCadence(crank_torque_msg_->event_count(), + crank_torque_msg_->raw_crank_period()); + emit_signal("crank_torque_received", power, torque, cadence); + } + } + crank_torque_msg_ = + std::make_unique(msg); + printf("[channel %hu] received StandardTorqueAtCrank\n", channel_); break; } case PowerProfilePages::kGetSetPage: - switch (data.raw_data[1]) { + switch (data->raw_data[1]) { case PowerProfileSubpages::kAdvancedCapabilities2: { std::string fmt_string{"%s\n\tsupported: %d\n\tenabled: %d\n"}; auto fmt = fmt_string.c_str(); bicycle_power::AdvancedCapabilities2 caps = - bicycle_power::AdvancedCapabilities2::FromPayload(data); + bicycle_power::AdvancedCapabilities2::FromPayload(**data); printf("----------\nCaps2\n----------\n"); printf(fmt, "4Hz", caps.Supports4Hz(), caps.Is4HzEnabled()); printf(fmt, "8Hz", caps.Supports8Hz(), caps.Is8HzEnabled()); @@ -68,7 +114,7 @@ void PowerChannel::OnBroadcastData(BroadcastPayload data) { case PowerProfileSubpages::kAdvancedCapabilities: { std::string fmt_string{"%s\n\tsupported: %d\n\tenabled: %d\n"}; auto fmt = fmt_string.c_str(); - auto caps = bicycle_power::AdvancedCapabilities::FromPayload(data); + auto caps = bicycle_power::AdvancedCapabilities::FromPayload(**data); printf("----------\nCaps1\n----------\n"); printf(fmt, "4Hz", caps.Supports4Hz(), caps.Is4HzEnabled()); printf(fmt, "8Hz", caps.Supports8Hz(), caps.Is8HzEnabled()); diff --git a/ant/src/ant/channel/power_channel.hpp b/ant/src/ant/channel/power_channel.hpp index 7df93d6..cc1e3c8 100644 --- a/ant/src/ant/channel/power_channel.hpp +++ b/ant/src/ant/channel/power_channel.hpp @@ -1,49 +1,55 @@ #pragma once -#include "../bicycle_power.hpp" +#include "../profile/bicycle_power.hpp" #include "channel.hpp" +#include namespace ant { class PowerChannel : public Channel { + GDCLASS(PowerChannel, Channel) public: - PowerChannel(ANTDevice &, ChannelNumber, DeviceNumber); + PowerChannel(); ~PowerChannel() {} - void OnBroadcastData(BroadcastPayload) final; + void OnBroadcastData(godot::Ref) final; // type specific void RequestCrankParameters(uint8_t n_transmissions = 2) { auto msg = Message::BicyclePowerRequestPage( - id_, PowerProfileSubpages::kCrankParameters, n_transmissions); + channel_, PowerProfileSubpages::kCrankParameters, n_transmissions); SendBuffered(msg); } void RequestAdvancedCapabilities1(uint8_t n_transmissions = 2) { auto msg = Message::BicyclePowerRequestPage( - id_, PowerProfileSubpages::kAdvancedCapabilities, n_transmissions); + channel_, PowerProfileSubpages::kAdvancedCapabilities, n_transmissions); SendBuffered(msg); } void RequestAdvancedCapabilities2(uint8_t n_tramsissions = 2) { auto msg = Message::BicyclePowerRequestPage( - id_, PowerProfileSubpages::kAdvancedCapabilities2, n_tramsissions); + channel_, PowerProfileSubpages::kAdvancedCapabilities2, n_tramsissions); SendBuffered(msg); } void Set8Hz(bool enabled) { - auto msg = Message::SetParameter(id_, 0x02, 0xfe, 0xff, 0xff, ~(1 << 1), + auto msg = Message::SetParameter(channel_, 0x02, 0xfe, 0xff, 0xff, ~(1 << 1), 0xff, 0xff * (1 - enabled), 0xff); SendBuffered(msg); } void SetAdvancedCapabilities(bicycle_power::AdvancedCapabilities caps) { auto msg = Message::SetParameter( - id_, PowerProfilePages::kGetSetPage, + channel_, PowerProfilePages::kGetSetPage, PowerProfileSubpages::kAdvancedCapabilities, kReservedByte, kReservedByte, caps.Mask(), kReservedByte, caps.Value(), kReservedByte); SendBuffered(msg); } void SetAdvancedCapabilities2(bicycle_power::AdvancedCapabilities2 caps) { auto msg = Message::SetParameter( - id_, PowerProfilePages::kGetSetPage, + channel_, PowerProfilePages::kGetSetPage, PowerProfileSubpages::kAdvancedCapabilities2, kReservedByte, kReservedByte, caps.Mask(), kReservedByte, caps.Value(), kReservedByte); SendBuffered(msg); } +protected: + static void _bind_methods(); + std::unique_ptr crank_torque_msg_{nullptr}; + std::unique_ptr power_msg_{nullptr}; }; } // namespace ant diff --git a/ant/src/ant/channels.hpp b/ant/src/ant/channels.hpp index ae1e5ce..8af90c4 100644 --- a/ant/src/ant/channels.hpp +++ b/ant/src/ant/channels.hpp @@ -1,6 +1,5 @@ #pragma once +#include "channel/fitness_equipment_channel.hpp" #include "channel/heart_rate_channel.hpp" #include "channel/power_channel.hpp" -#include "channel/fitness_equipment_channel.hpp" - diff --git a/ant/src/ant/common.hpp b/ant/src/ant/common.hpp index 818bd38..4790d3c 100644 --- a/ant/src/ant/common.hpp +++ b/ant/src/ant/common.hpp @@ -1,5 +1,5 @@ #pragma once -#include "message.hpp" +#include "messages.hpp" #include typedef uint8_t CommandStatus; diff --git a/ant/src/ant/message/common_msgs.hpp b/ant/src/ant/message/common_msgs.hpp new file mode 100644 index 0000000..3088f6d --- /dev/null +++ b/ant/src/ant/message/common_msgs.hpp @@ -0,0 +1,35 @@ +#pragma once +#include "control_messages.hpp" +#include "message.hpp" + +namespace ant { +namespace msg { +namespace common { + +class RequestChannelID : public control::RequestMessage { +public: + RequestChannelID(ChannelNumber channel) + : control::RequestMessage(channel, MessageIDs::kChannelID) {} +}; + +class RequestPage : public Message { +public: + RequestPage(ChannelNumber channel, uint8_t descriptor1, uint8_t descriptor2, + uint8_t requested_transmissions, uint8_t page_number, + CommandType command_type) + : Message(9, MessageIDs::kAckData, channel, CommonPages::kRequestDataPage, + kReservedByte, kReservedByte, descriptor1, descriptor2, + requested_transmissions, page_number, command_type) {} +}; + +class RequestCommandStatus : public RequestPage { +public: + RequestCommandStatus(ChannelNumber channel, uint8_t requested_transmissions) + : RequestPage(channel, kReservedByte, kReservedByte, + requested_transmissions, CommonPages::kCommandStatus, + CommandTypes::RequestDataPage) {} +}; + +} // namespace common +} // namespace msg +} // namespace ant diff --git a/ant/src/ant/message/config_messages.hpp b/ant/src/ant/message/config_messages.hpp new file mode 100644 index 0000000..6fc705e --- /dev/null +++ b/ant/src/ant/message/config_messages.hpp @@ -0,0 +1,62 @@ +#pragma once +#include "message.hpp" + +namespace ant { +namespace msg { +namespace config { + +class UnassignChannel : public Message { +public: + UnassignChannel(ChannelNumber channel) + : Message(1, MessageIDs::kUnassignChannel, channel) {} +}; + +class AssignChannel : public Message { +public: + AssignChannel(ChannelNumber channel, ChannelType type, uint8_t network = 1) + : Message(3, MessageIDs::kAssignChannel, channel, type, network) {} +}; + +class ChannelID : public Message { +public: + ChannelID(ChannelNumber channel, DeviceNumber device_number, + DeviceType device_type, TransmissionType transmission_type) + : Message(5, MessageIDs::kChannelID, channel, lower_byte(device_number), + upper_byte(device_number), device_type, transmission_type) {} +}; + +class ChannelPeriod : public Message { +public: + ChannelPeriod(ChannelNumber channel, uint16_t period) + : Message(3, MessageIDs::kChannelPeriod, channel, lower_byte(period), + upper_byte(period)) {} +}; + +class SearchTimeout : public Message { +public: + SearchTimeout(ChannelNumber channel, uint8_t timeout) + : Message(2, MessageIDs::kSearchTimeout, channel, timeout) {} +}; + +class ChannelRFFrequency : public Message { +public: + ChannelRFFrequency(ChannelNumber channel, uint8_t freq) + : Message(2, MessageIDs::kChannelFrequency, channel, freq) {} +}; + +class SetNetworkKey : public Message { +public: + SetNetworkKey(uint8_t network_number, const uint8_t *key) + : Message(9, MessageIDs::kSetNetworkKey, network_number, key[0], key[1], + key[2], key[3], key[4], key[5], key[6], key[7]) {} +}; + +class LowPrioritySearchTimeout : public Message { +public: + LowPrioritySearchTimeout(ChannelNumber channel, uint8_t timeout) + : Message(2, MessageIDs::kLowPrioritySearchTimeout, channel, timeout) {} +}; + +} // namespace config +} // namespace msg +} // namespace ant diff --git a/ant/src/ant/message/control_messages.hpp b/ant/src/ant/message/control_messages.hpp new file mode 100644 index 0000000..05f2770 --- /dev/null +++ b/ant/src/ant/message/control_messages.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include "message.hpp" + +namespace ant { +namespace msg { +namespace control { + +class ResetSystem : public Message { +public: + ResetSystem() : Message(1, MessageIDs::kResetSystem, 0) {} +}; + +class OpenChannel : public Message { +public: + OpenChannel(ChannelNumber channel) + : Message(1, MessageIDs::kOpenChannel, channel) {} +}; + +class CloseChannel : public Message { +public: + CloseChannel(ChannelNumber channel) + : Message(1, MessageIDs::kCloseChannel, channel) {} +}; + +class RequestMessage : public Message { +public: + RequestMessage(ChannelNumber channel, MessageID requested_msg_id) + : Message(2, MessageIDs::kRequestMessage, channel, requested_msg_id) {} +}; + +} // namespace control +} // namespace msg +} // namespace ant diff --git a/ant/src/ant/message.cpp b/ant/src/ant/message/message.cpp similarity index 97% rename from ant/src/ant/message.cpp rename to ant/src/ant/message/message.cpp index 1960615..0129e28 100644 --- a/ant/src/ant/message.cpp +++ b/ant/src/ant/message/message.cpp @@ -1,5 +1,5 @@ #include "message.hpp" -#include "ant.hpp" +#include "../ant.hpp" namespace ant { Message::Message() { Init(); } diff --git a/ant/src/ant/message.hpp b/ant/src/ant/message/message.hpp similarity index 73% rename from ant/src/ant/message.hpp rename to ant/src/ant/message/message.hpp index 3f311dd..c22a538 100644 --- a/ant/src/ant/message.hpp +++ b/ant/src/ant/message/message.hpp @@ -1,7 +1,8 @@ #pragma once -#include "ant.hpp" +#include "../ant.hpp" #include #include +#include #include #define ANT_UNASSIGN_CHANNEL 0x41 #define ANT_ASSIGN_CHANNEL 0x42 @@ -43,6 +44,9 @@ inline uint8_t lower_byte(uint16_t value) { inline uint8_t upper_byte(uint16_t value) { return static_cast((value >> 8) & 0xFF); } +inline uint16_t to_word(uint8_t lower, uint8_t upper) { + return static_cast(lower | (upper << 8)); +} typedef uint8_t MessageID; struct MessageIDs { @@ -53,12 +57,19 @@ struct MessageIDs { kChannelResponse = 0x40, kUnassignChannel = 0x41, kAssignChannel = 0x42, + kChannelPeriod = 0x43, + kSearchTimeout = 0x44, + kChannelFrequency = 0x45, + kSetNetworkKey = 0x46, + kTransmitPower = 0x47, + kSearchWaveform = 0x49, + kAddChannelToList = 0x59, + kConfigIDList = 0x5a, + kLowPrioritySearchTimeout = 0x63, kChannelID = 0x51, kResetSystem = 0x4a, kOpenChannel = 0x4b, kCloseChannel = 0x4c, - kChannelFrequency = 0x45, - kChannelPeriod = 0x43, kBroadcastData = 0x4e, kAckData = 0x4f, kBurstTransferData = 0x50, @@ -68,7 +79,6 @@ struct MessageIDs { kCapabilities = 0x54, kSerialNumber = 0x61, kEventBufferConfiguration = 0x74, - kSearchTimeout = 0x44, kLPSearchTimeout = 0x63, kRequestMessage = 0x4d, }; @@ -135,37 +145,48 @@ struct CommandTypes { }; }; -class BroadcastPayload { +class BroadcastPayload : public godot::RefCounted { + GDCLASS(BroadcastPayload, godot::RefCounted) public: - BroadcastPayload(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, - uint8_t b5, uint8_t b6, uint8_t b7) { - raw_data[0] = b0; - raw_data[1] = b1; - raw_data[2] = b2; - raw_data[3] = b3; - raw_data[4] = b4; - raw_data[5] = b5; - raw_data[6] = b6; - raw_data[7] = b7; - } - static BroadcastPayload FromSequentialBuffer(uint8_t *buffer) { - return BroadcastPayload(buffer[0], buffer[1], buffer[2], buffer[3], - buffer[4], buffer[5], buffer[6], buffer[7]); + BroadcastPayload() {} + static godot::Ref FromSequentialBuffer(uint8_t *buffer) { + godot::Ref new_payload; + new_payload.instantiate(); + new_payload->raw_data[0] = buffer[0]; + new_payload->raw_data[1] = buffer[1]; + new_payload->raw_data[2] = buffer[2]; + new_payload->raw_data[3] = buffer[3]; + new_payload->raw_data[4] = buffer[4]; + new_payload->raw_data[5] = buffer[5]; + new_payload->raw_data[6] = buffer[6]; + new_payload->raw_data[7] = buffer[7]; + return new_payload; } std::array raw_data; + +protected: + static void _bind_methods() {} }; -class ChannelResponseMessage { +class ChannelResponseMessage : public godot::RefCounted { + GDCLASS(ChannelResponseMessage, godot::RefCounted) public: - ChannelResponseMessage(ChannelNumber channel_id, MessageID msg_id, - MessageCode msg_code) - : channel_id_(channel_id), msg_id_(msg_id), msg_code_(msg_code) {} - static ChannelResponseMessage FromPayload(std::array _data) { - return ChannelResponseMessage(_data[0], _data[1], _data[2]); + ChannelResponseMessage() {} + static godot::Ref + FromPayload(std::array _data) { + godot::Ref new_instance; + new_instance.instantiate(); + new_instance->channel_id_ = _data[0]; + new_instance->msg_id_ = _data[1]; + new_instance->msg_code_ = _data[2]; + return new_instance; } ChannelNumber channel_id_; MessageID msg_id_; MessageCode msg_code_; + +protected: + static void _bind_methods() {} }; class BatteryStatusMessage { @@ -220,32 +241,7 @@ public: uint8_t b5 = '\0', uint8_t b6 = '\0', uint8_t b7 = '\0', uint8_t b8 = '\0', uint8_t b9 = '\0', uint8_t b10 = '\0', uint8_t b11 = '\0', uint8_t b12 = '\0'); - static Message SystemReset() { return Message(1, ANT_SYSTEM_RESET); } - static Message UnassignChannel(ChannelNumber channel) { - return Message(1, ANT_UNASSIGN_CHANNEL, channel); - } - static Message AssignChannel(ChannelNumber channel, ChannelType type, - uint8_t network) { - return Message(3, ANT_ASSIGN_CHANNEL, channel, static_cast(type), - network); - } - static Message SetChannelPeriod(ChannelNumber channel, ChannelPeriod period) { - return Message(3, ANT_CHANNEL_PERIOD, channel, period & 0xFF, - (period >> 8) & 0xFF); - } - static Message SetChannelFrequency(uint8_t channel, uint8_t frequency) { - return Message(2, ANT_CHANNEL_FREQUENCY, channel, frequency); - } - - static Message SetHPSearchTimeout(uint8_t channel, uint8_t timeout) { - return Message(2, ANT_SEARCH_TIMEOUT, channel, timeout); - } - - static Message RequestChannelID(ChannelNumber channel) { - return Message(2, MessageIDs::kRequestMessage, channel, - MessageIDs::kChannelID); - } static Message CommonRequestPage(ChannelNumber channel, uint8_t descriptor1, uint8_t descriptor2, uint8_t requested_transmissions, @@ -297,6 +293,20 @@ public: lower_byte(slope), upper_byte(slope), resistance_coeff); } + static Message FitnessEquipmentUserConfiguration(ChannelNumber channel, + uint16_t user_weight, + uint16_t bicycle_weight, + uint8_t wheel_diameter, + uint8_t gear_ratio) { + uint8_t lower_nibble = (bicycle_weight & 0x00F) << 4 | 0x00; + uint8_t my_upper_byte = static_cast((bicycle_weight & 0xFF0) >> 4); + return Message(9, MessageIDs::kAckData, channel, + FitnessEquipmentPages::kUserConfiguration, + lower_byte(user_weight), upper_byte(user_weight), + kReservedByte, lower_nibble, my_upper_byte, wheel_diameter, + gear_ratio); + } + static Message FitnessEquipmentRequestCapabilities(ChannelNumber channel, uint8_t requested_transmissions) { @@ -312,33 +322,6 @@ public: subpage_number, b2, b3, b4, b5, b6, b7); } - /** - * @brief Set the channel ID - * - * @param channel Number of the channel - * @param device Device number. Use 0 as wildcard to match any. - * @param device_type Device type. use 0 as wildcard to match any. - * @param transmission_type Transmission type. use 0 as wildcard to match any. - * @return - */ - static Message SetChannelID(ChannelNumber channel, DeviceNumber device, - DeviceType device_type, - TransmissionType transmission_type) { - return Message(5, ANT_CHANNEL_ID, channel, device & 0xFF, - (device >> 8) & 0xFF, static_cast(device_type), - static_cast(transmission_type)); - } - static Message SetNetworkKey(uint8_t network, const uint8_t *key) { - return Message(9, ANT_SET_NETWORK, network, key[0], key[1], key[2], key[3], - key[4], key[5], key[6], key[7]); - } - static Message SetLPSearchTimeout(ChannelNumber channel, uint8_t timeout) { - return Message(2, ANT_LP_SEARCH_TIMEOUT, channel, timeout); - } - - static Message OpenChannel(ChannelNumber channel) { - return Message(1, ANT_OPEN_CHANNEL, channel); - } uint8_t data[kMaxMessageSize]; uint8_t data_length; diff --git a/ant/src/ant/message_processor.cpp b/ant/src/ant/message_processor.cpp index d9f882b..b7701ab 100644 --- a/ant/src/ant/message_processor.cpp +++ b/ant/src/ant/message_processor.cpp @@ -5,16 +5,7 @@ #include namespace ant { -MessageProcessor::MessageProcessor() {} -void MessageProcessor::SetOnChannelResponseCallback( - ChannelNumber _channel_id, - std::function _callback) { - channel_response_cbs_[_channel_id] = _callback; -} -void MessageProcessor::SetOnBroadcastDataCallback( - ChannelNumber _channel_id, std::function _callback) { - broadcast_data_cbs_[_channel_id] = _callback; -} + bool MessageProcessor::FeedData(uint8_t _data) { switch (state_) { case State::kWaitForSync: @@ -106,10 +97,8 @@ void MessageProcessor::ProcessMessage() { iterator end = msg_buffer_.begin() + kOffsetPayloadStart + 3; assert(msg_buffer_.size() >= kOffsetPayloadStart + 3); std::copy(start, end, payload.begin()); - ChannelResponseMessage msg = ChannelResponseMessage::FromPayload(payload); - if (channel_response_cbs_.count(msg.channel_id_)) { - channel_response_cbs_[msg.channel_id_](msg); - } + godot::Ref msg = ChannelResponseMessage::FromPayload(payload); + emit_signal("channel_response_received", msg->channel_id_, msg); break; } case MessageIDs::kChannelID: { @@ -118,22 +107,19 @@ void MessageProcessor::ProcessMessage() { // 3: device type // 4: transmission type ChannelNumber channel = msg_buffer_.at(kOffsetPayloadStart); - uint16_t device_number = (msg_buffer_.at(kOffsetPayloadStart + 1)) | - (msg_buffer_.at(kOffsetPayloadStart + 2) << 8); + DeviceNumber device_number = (msg_buffer_.at(kOffsetPayloadStart + 1)) | + (msg_buffer_.at(kOffsetPayloadStart + 2) << 8); printf("----------\nDevice Number: 0x%04x\n----------\n", device_number); - if (device_number_cb_) { - device_number_cb_(channel, device_number); - } + emit_signal("device_number_received", channel, device_number); break; } case MessageIDs::kBroadcastData: { - // printf("Broadcast page: 0x%02x\n", msg_buffer_.at(kOffsetPayloadStart + 1)); + // printf("Broadcast page: 0x%02x\n", msg_buffer_.at(kOffsetPayloadStart + + // 1)); ChannelNumber channel_id = msg_buffer_.at(kOffsetPayloadStart); - BroadcastPayload msg(BroadcastPayload::FromSequentialBuffer( - &msg_buffer_.at(kOffsetPayloadStart + 1))); - if (broadcast_data_cbs_.count(channel_id)) { - broadcast_data_cbs_[channel_id](msg); - } + godot::Ref msg = BroadcastPayload::FromSequentialBuffer( + &msg_buffer_.at(kOffsetPayloadStart + 1)); + emit_signal("broadcast_data_received", channel_id, msg); break; } default: diff --git a/ant/src/ant/message_processor.hpp b/ant/src/ant/message_processor.hpp index fcc0bd9..4a3905a 100644 --- a/ant/src/ant/message_processor.hpp +++ b/ant/src/ant/message_processor.hpp @@ -1,19 +1,21 @@ #pragma once -#include "message.hpp" +#include "messages.hpp" #include #include +#include #include #include namespace ant { -typedef std::function DeviceNumberCallback; +typedef std::function DeviceNumberCallback; typedef std::function ChannelResponseCallback; typedef std::function BroadcastCallback; -class MessageProcessor { +class MessageProcessor : public godot::RefCounted { + GDCLASS(MessageProcessor, godot::RefCounted) public: - MessageProcessor(); + MessageProcessor() {} enum class State { kWaitForSync = 0, kGetLength, @@ -23,21 +25,27 @@ public: }; bool FeedData(uint8_t data); void ProcessMessage(); - void SetOnChannelResponseCallback(ChannelNumber channel_id, - ChannelResponseCallback callback); - void SetOnBroadcastDataCallback(ChannelNumber channel_id, - BroadcastCallback callback); - void SetDeviceNumberCallback(DeviceNumberCallback _cb) { - device_number_cb_ = _cb; + +protected: + static void _bind_methods() { + ADD_SIGNAL(godot::MethodInfo( + "channel_response_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::OBJECT, "msg_ref"))); + ADD_SIGNAL(godot::MethodInfo( + "broadcast_data_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::OBJECT, "msg_ref"))); + ADD_SIGNAL(godot::MethodInfo( + "device_number_received", + godot::PropertyInfo(godot::Variant::INT, "channel"), + godot::PropertyInfo(godot::Variant::INT, "device_number"))); } private: State state_{State::kWaitForSync}; uint8_t checksum_; std::vector msg_buffer_; - std::map channel_response_cbs_; - std::map broadcast_data_cbs_; - DeviceNumberCallback device_number_cb_{nullptr}; }; } // namespace ant diff --git a/ant/src/ant/messages.hpp b/ant/src/ant/messages.hpp new file mode 100644 index 0000000..99a9019 --- /dev/null +++ b/ant/src/ant/messages.hpp @@ -0,0 +1,5 @@ +#pragma once +#include "message/message.hpp" +#include "message/control_messages.hpp" +#include "message/config_messages.hpp" +#include "message/common_msgs.hpp" diff --git a/ant/src/ant/profile/bicycle_power.hpp b/ant/src/ant/profile/bicycle_power.hpp new file mode 100644 index 0000000..e0ae712 --- /dev/null +++ b/ant/src/ant/profile/bicycle_power.hpp @@ -0,0 +1,197 @@ +#pragma once +#include "../messages.hpp" + +namespace ant { +namespace bicycle_power { +class AdvancedCapabilities { +public: + AdvancedCapabilities(uint8_t _mask = 0xff, uint8_t _value = 0xff, + uint8_t _properties = 0xff) + : mask(_mask), value(_value), properties(_properties) {} + static AdvancedCapabilities FromPayload(BroadcastPayload payload) { + return AdvancedCapabilities(payload.raw_data.at(4), payload.raw_data.at(6), + payload.raw_data.at(2)); + } + bool Supports4Hz() { return SupportsProperty(0); } + bool Supports8Hz() { return SupportsProperty(1); } + bool SupportsAutoZero() { return SupportsProperty(4); } + bool SupportsAutoCrankLength() { return SupportsProperty(5); } + bool SupportsTorqueEfficiencyAndSmoothness() { return SupportsProperty(6); } + + bool Is4HzEnabled() { return IsPropertyEnabled(0); } + bool Is8HzEnabled() { return IsPropertyEnabled(1); } + bool IsAutoZeroEnabled() { return IsPropertyEnabled(4); } + bool IsAutoCrankLengthEnabled() { return IsPropertyEnabled(5); } + bool IsTorqueEfficiencyAndSmoothnessEnabled() { return IsPropertyEnabled(6); } + + void EnableTorqueEfficiencyAndSmoothness() { value &= 0b10111111; } + void UnmaskTorqueEfficiencyAndSmoothness() { mask &= 0b10111111; } + + void SetMask(uint8_t _mask) { mask = _mask; } + uint8_t Mask() { return mask; } + void SetValue(uint8_t _value) { value = _value; } + uint8_t Value() { return value; } + +private: + bool SupportsProperty(uint8_t bit_position) { + return !(mask & (1 << bit_position)); + } + bool IsPropertyEnabled(uint8_t bit_position) { + return !(value & (1 << bit_position)); + } + uint8_t mask; + uint8_t value; + uint8_t properties; +}; +class AdvancedCapabilities2 { +public: + AdvancedCapabilities2(uint8_t _mask = 0xff, uint8_t _value = 0xff) + : mask(_mask), value(_value) {} + + static AdvancedCapabilities2 FromPayload(BroadcastPayload payload) { + return AdvancedCapabilities2(payload.raw_data.at(4), + payload.raw_data.at(6)); + } + bool Supports4Hz() { return SupportsProperty(0); } + bool Supports8Hz() { return SupportsProperty(1); } + bool SupportsPowerPhase() { return SupportsProperty(3); } + bool SupportsPCO() { return SupportsProperty(4); } + bool SupportsRiderPosition() { return SupportsProperty(5); } + bool SupportsTorqueBarycenter() { return SupportsProperty(6); } + + bool Is4HzEnabled() { return IsPropertyEnabled(0); } + bool Is8HzEnabled() { return IsPropertyEnabled(1); } + bool IsPowerPhaseEnabled() { return IsPropertyEnabled(3); } + bool IsPCOEnabled() { return IsPropertyEnabled(4); } + bool IsRiderPositionEnabled() { return IsPropertyEnabled(5); } + bool IsTorqueBarycenterEnabled() { return IsPropertyEnabled(6); } + + void EnableAllDynamics() { value &= 0b10000111; } + void UnmaskAllDynamics() { mask &= 0b10000111; } + void Enable8Hz() { value &= 0b11111101; } + void Unmask8Hz() { mask &= 0b11111101; } + void Enable4Hz() { value &= 0b11111110; } + void Unmask4Hz() { mask &= 0b11111110; } + + void SetMask(uint8_t _mask) { mask = _mask; } + uint8_t Mask() { return mask; } + void SetValue(uint8_t _value) { value = _value; } + uint8_t Value() { return value; } + +private: + bool SupportsProperty(uint8_t bit_position) { + // 0: does support + // 1: does NOT support + return !(mask & (1 << bit_position)); + } + bool IsPropertyEnabled(uint8_t bit_position) { + return !(value & (1 << bit_position)); + } + uint8_t mask; + uint8_t value; +}; + +class StandardCrankTorque { +public: + StandardCrankTorque(uint8_t event_count, uint8_t crank_ticks, + uint8_t inst_cadence, uint16_t crank_period, + uint16_t accumulated_torque) + : event_count_(event_count), crank_ticks_(crank_ticks), + inst_cadence_(inst_cadence), raw_crank_period_(crank_period), + raw_accumulated_torque_(accumulated_torque) {} + static StandardCrankTorque FromPayload(BroadcastPayload payload) { + uint16_t crank_period = + payload.raw_data.at(4) | (payload.raw_data.at(5) << 8); + uint16_t accumulated_torque = + payload.raw_data.at(6) | (payload.raw_data.at(7) << 8); + return StandardCrankTorque(payload.raw_data.at(1), payload.raw_data.at(2), + payload.raw_data.at(3), crank_period, + accumulated_torque); + } + uint8_t event_count() const { return event_count_; } + uint8_t crank_ticks() const { return crank_ticks_; } + uint8_t inst_cadence() const { return inst_cadence_; } + uint16_t raw_crank_period() const { return raw_crank_period_; } + uint16_t raw_accumulated_torque() const { return raw_accumulated_torque_; } + + /** + * @brief Only valid if previous_event_count != event_count() + * + * @param previous_event_count + * @param previous_accumulated_torque + * @return Torque in [Nm] + */ + double AverageTorque(uint8_t previous_event_count, + uint16_t previous_accumulated_torque) { + uint16_t delta_torque = + raw_accumulated_torque_ - previous_accumulated_torque; + uint8_t delta_event = event_count_ - previous_event_count; + return 1.0 * delta_torque / (32.0 * delta_event); + } + double AveragePower(uint16_t previous_accumulated_torque, + uint16_t previous_crank_period) { + uint16_t delta_torque = + raw_accumulated_torque_ - previous_accumulated_torque; + uint16_t delta_period = raw_crank_period_ - previous_crank_period; + if (delta_period == 0) { + return 0.0; + } + return 128.0 * 3.1416 * delta_torque / delta_period; + } + int AverageCadence(uint8_t previous_event_count, + uint16_t previous_crank_period) { + uint8_t delta_event = event_count_ - previous_event_count; + uint16_t delta_period = raw_crank_period_ - previous_crank_period; + if (delta_period == 0) { + return 0; + } + return 60.0 * delta_event / (delta_period / 2048.0); + } + +private: + uint8_t event_count_; + uint8_t crank_ticks_; + uint8_t inst_cadence_; + uint16_t raw_crank_period_; + uint16_t raw_accumulated_torque_; +}; + +class StandardPowerOnly { +public: + StandardPowerOnly(uint8_t event_count, uint8_t pedal_power, + uint8_t inst_cadence, uint16_t accumulated_power, + uint16_t inst_power) + : event_count_(event_count), pedal_power_(pedal_power), + inst_cadence_(inst_cadence), accumulated_power_(accumulated_power), + inst_power_(inst_power) {} + static StandardPowerOnly FromRawData(std::array raw) { + return StandardPowerOnly(raw.at(1), raw.at(2), raw.at(3), + to_word(raw.at(4), raw.at(5)), + to_word(raw.at(6), raw.at(7))); + } + + double AveragePower(uint16_t previous_accumulated_power, + uint8_t previous_event_count) { + uint8_t delta_event = event_count_ - previous_event_count; + uint16_t delta_power = accumulated_power_ - previous_accumulated_power; + if (delta_event == 0) { + return 0.0; + } + return 1.0 * delta_power / delta_event; + } + uint8_t inst_cadence() const { return inst_cadence_; } + uint16_t accumulated_power() const { return accumulated_power_; } + uint8_t event_count() const { return event_count_; } + uint16_t inst_power() const { return inst_power_; } + bool IsCadenceValid() const { return inst_cadence_ != 0xFF; } + +private: + uint8_t event_count_; + uint8_t pedal_power_; + uint8_t inst_cadence_; + uint16_t accumulated_power_; + uint16_t inst_power_; +}; + +} // namespace bicycle_power +} // namespace ant diff --git a/ant/src/ant/fitness_equipment.hpp b/ant/src/ant/profile/fitness_equipment.hpp similarity index 70% rename from ant/src/ant/fitness_equipment.hpp rename to ant/src/ant/profile/fitness_equipment.hpp index 2d8e712..40ceb25 100644 --- a/ant/src/ant/fitness_equipment.hpp +++ b/ant/src/ant/profile/fitness_equipment.hpp @@ -1,5 +1,5 @@ #pragma once -#include "message.hpp" +#include "../messages.hpp" namespace ant { namespace fitness_equipment { @@ -7,9 +7,13 @@ class BikeSpecificDataPage { public: static constexpr Page id = FitnessEquipmentPages::kBikeSepcificData; BikeSpecificDataPage(uint8_t update_event_count, uint8_t inst_cadence, - uint16_t accumulated_power, uint16_t inst_power) + uint16_t accumulated_power, uint16_t inst_power, + uint8_t status_bits, uint8_t flag_bits, + uint8_t fe_state_bits) : update_event_count_(update_event_count), inst_cadence_(inst_cadence), - accumulated_power_(accumulated_power), inst_power_(inst_power) {} + accumulated_power_(accumulated_power), inst_power_(inst_power), + status_bits_(status_bits), flag_bits_(flag_bits), + fe_state_bits_(fe_state_bits) {} static BikeSpecificDataPage FromPayload(BroadcastPayload payload) { uint8_t update_event_count = payload.raw_data.at(1); @@ -18,8 +22,12 @@ public: payload.raw_data.at(3) | ((payload.raw_data.at(4) & 0x0F) << 8); uint16_t inst_power = payload.raw_data.at(5) | ((payload.raw_data.at(6) & 0x0F) << 8); + uint8_t status_bits = (payload.raw_data.at(6) & 0xF0) >> 4; + uint8_t flag_bits = (payload.raw_data.at(7) & 0x0F); + uint8_t fe_state_bits = (payload.raw_data.at(7) & 0xF0) >> 4; return BikeSpecificDataPage(update_event_count, inst_cadence, acc_power, - inst_power); + inst_power, status_bits, flag_bits, + fe_state_bits); } bool IsPowerInvalid() { return inst_power_ == 0xFFF; } @@ -27,16 +35,23 @@ public: uint16_t InstantaneousPower() const { return inst_power_; } uint16_t AccumulatedPower() const { return accumulated_power_; } uint8_t UpdateEventCount() const { return update_event_count_; } + bool IsPowerCalibrationRequired() const { return status_bits_ & (1 << 0); } + bool IsResistanceCalibrationRequired() const { return status_bits_ & (1 << 1); } + bool IsUserConfigurationRequired() const { return status_bits_ & (1 << 2); } private: uint8_t update_event_count_; uint8_t inst_cadence_; uint16_t accumulated_power_; uint16_t inst_power_; + uint8_t status_bits_; + uint8_t flag_bits_; + uint8_t fe_state_bits_; }; class Capabilities { public: + static constexpr Page id = FitnessEquipmentPages::kCapabilities; Capabilities(uint16_t max_resistance, uint8_t capabilities_bitfield) : max_resistance_(max_resistance), capabilities_(capabilities_bitfield) {} static Capabilities FromPayload(BroadcastPayload payload) { diff --git a/ant/src/ant_controller.cpp b/ant/src/ant_controller.cpp index 03a3303..9c14ba3 100644 --- a/ant/src/ant_controller.cpp +++ b/ant/src/ant_controller.cpp @@ -3,33 +3,112 @@ using namespace godot; void AntController::_bind_methods() { - ClassDB::bind_method(D_METHOD("set_enable_fitness_equipment", "value"), &AntController::set_enable_fitness_equipment); - ClassDB::bind_method(D_METHOD("get_enable_fitness_equipment"), &AntController::get_enable_fitness_equipment); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_fitness_equipment"), "set_enable_fitness_equipment", "get_enable_fitness_equipment"); + ClassDB::bind_method(D_METHOD("set_enable_fitness_equipment", "value"), + &AntController::set_enable_fitness_equipment); + ClassDB::bind_method(D_METHOD("get_enable_fitness_equipment"), + &AntController::get_enable_fitness_equipment); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_fitness_equipment"), + "set_enable_fitness_equipment", "get_enable_fitness_equipment"); - ClassDB::bind_method(D_METHOD("set_enable_heart_rate", "value"), &AntController::set_enable_heart_rate); - ClassDB::bind_method(D_METHOD("get_enable_heart_rate"), &AntController::get_enable_heart_rate); - ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_heart_rate"), "set_enable_heart_rate", "get_enable_heart_rate"); + ClassDB::bind_method(D_METHOD("set_enable_heart_rate", "value"), + &AntController::set_enable_heart_rate); + ClassDB::bind_method(D_METHOD("get_enable_heart_rate"), + &AntController::get_enable_heart_rate); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_heart_rate"), + "set_enable_heart_rate", "get_enable_heart_rate"); + ClassDB::bind_method(D_METHOD("set_enable_bicycle_power", "value"), + &AntController::set_enable_bicycle_power); + ClassDB::bind_method(D_METHOD("get_enable_bicycle_power"), + &AntController::get_enable_bicycle_power); + ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enable_bicycle_power"), + "set_enable_bicycle_power", "get_enable_bicycle_power"); + ClassDB::bind_method( + D_METHOD("set_heart_rate_device_number", "p_heart_rate_device_number"), + &AntController::set_heart_rate_device_number); + ClassDB::bind_method(D_METHOD("get_heart_rate_device_number"), + &AntController::get_heart_rate_device_number); + ADD_PROPERTY(PropertyInfo(Variant::INT, "heart_rate_device_number"), + "set_heart_rate_device_number", "get_heart_rate_device_number"); + + ClassDB::bind_method(D_METHOD("set_fitness_equipment_device_number", + "p_fitness_eqipment_device_number"), + &AntController::set_fitness_equipment_device_number); + ClassDB::bind_method(D_METHOD("get_fitness_equipment_device_number"), + &AntController::get_fitness_equipment_device_number); + ADD_PROPERTY(PropertyInfo(Variant::INT, "fitness_equipment_device_number"), + "set_fitness_equipment_device_number", + "get_fitness_equipment_device_number"); + + ClassDB::bind_method(D_METHOD("set_bicycle_power_device_number", + "p_bicycle_power_device_number"), + &AntController::set_bicycle_power_device_number); + ClassDB::bind_method(D_METHOD("get_bicycle_power_device_number"), + &AntController::get_bicycle_power_device_number); + ADD_PROPERTY(PropertyInfo(Variant::INT, "bicycle_power_device_number"), + "set_bicycle_power_device_number", + "get_bicycle_power_device_number"); + + ADD_SIGNAL(MethodInfo("device_number_received", + PropertyInfo(Variant::INT, "device_number"))); ADD_SIGNAL(MethodInfo("heart_rate_updated", PropertyInfo(Variant::INT, "heart_rate"))); ADD_SIGNAL( MethodInfo("fec_power_updated", PropertyInfo(Variant::INT, "power"))); ADD_SIGNAL( MethodInfo("fec_cadence_updated", PropertyInfo(Variant::INT, "cadence"))); + ClassDB::bind_method(D_METHOD("connect_ant_channels"), &AntController::connect_ant_channels); ClassDB::bind_method(D_METHOD("parse_messages"), &AntController::parse_messages); + + ClassDB::bind_method(D_METHOD("search_fitness_equipment", "channel_number"), + &AntController::search_fitness_equipment); + + ClassDB::bind_method(D_METHOD("init_ant_device"), + &AntController::init_ant_device); + ClassDB::bind_method(D_METHOD("configure_enabled_channels"), + &AntController::configure_enabled_channels); } -AntController::AntController() {} +AntController::AntController() { + enable_fitness_equipment_ = false; + enable_heart_rate_ = false; + enable_bicycle_power_ = false; + fitness_equipment_device_number_ = ant::DeviceNumbers::Wildcard; + heart_rate_device_number_ = ant::DeviceNumbers::Wildcard; + bicycle_power_device_number_ = ant::DeviceNumbers::Wildcard; +} + +void AntController::configure_enabled_channels() { + ant::ChannelNumber n = 1; + ant::DeviceNumber device_number = ant::DeviceNumbers::Wildcard; + + if (enable_fitness_equipment_) { + configure_fitness_equipment_channel(n++, device_number); + } + if (enable_heart_rate_) { + configure_heart_rate_channel(n++, device_number); + } + if (enable_bicycle_power_) { + configure_power_channel(n++, device_number); + } +} + +void AntController::search_fitness_equipment( + ant::ChannelNumber channel_number) { + configure_fitness_equipment_channel(channel_number, + ant::DeviceNumbers::Wildcard); + fe_channel_->StartSearch(); +} void AntController::configure_heart_rate_channel( ant::ChannelNumber _channel, ant::DeviceNumber _device_number) { - hr_channel_ = std::make_unique(*ant_device_, _channel, - _device_number); + if (!hr_channel_) { + hr_channel_ = std::make_unique(); + } hr_channel_->SetOnHeartRateCallback([this](auto heart_rate) { emit_signal("heart_rate_updated", static_cast(heart_rate)); @@ -38,33 +117,43 @@ void AntController::configure_heart_rate_channel( void AntController::configure_fitness_equipment_channel( ant::ChannelNumber _channel, ant::DeviceNumber _device_number) { - fe_channel_ = std::make_unique( - *ant_device_, _channel, _device_number); + if (!fe_channel_) { + fe_channel_ = std::make_unique(); + } fe_channel_->SetPowerCallback( [this](auto power) { emit_signal("fec_power_updated", power); }); fe_channel_->SetCadenceCallback( [this](auto cadence) { emit_signal("fec_cadence_updated"); }); - } -void AntController::configure_power_channel(ant::ChannelNumber _channel, ant::DeviceNumber _device_number) { - power_channel_ = std::make_unique(*ant_device_, _channel, _device_number); +void AntController::configure_power_channel(ant::ChannelNumber _channel, + ant::DeviceNumber _device_number) { + if (!power_channel_) { + power_channel_ = std::make_unique(); + } +} + +bool AntController::init_ant_device() { + + ant_device_ = std::make_unique(); + if (!ant_device_) { + return false; + } + if (!ant_device_->Init()) { + return false; + } + return true; } void AntController::connect_ant_channels() { - ant_device_ = std::make_unique(); - if (!ant_device_->Init()) { - printf("Failed to initialize ant device!.\n"); - } - ant::ChannelNumber channel = 1; // configure_heart_rate_channel(channel++, ant::DeviceNumbers::Wildcard); // hr_channel_->StartSearch(); - // configure_fitness_equipment_channel(channel++, ant::DeviceNumbers::Wildcard); - // fe_channel_->StartSearch(); - configure_power_channel(channel++, ant::DeviceNumbers::Wildcard); - power_channel_->StartSearch(); + configure_fitness_equipment_channel(channel++, ant::DeviceNumbers::Wildcard); + fe_channel_->StartSearch(); + // configure_power_channel(channel++, ant::DeviceNumbers::Wildcard); + // power_channel_->StartSearch(); } void AntController::parse_messages() { ant_device_->ReceiveMessage(); } diff --git a/ant/src/ant_controller.hpp b/ant/src/ant_controller.hpp index e4572f3..ce9103b 100644 --- a/ant/src/ant_controller.hpp +++ b/ant/src/ant_controller.hpp @@ -2,7 +2,7 @@ #include "ant/ant_device.hpp" #include "ant/channels.hpp" -#include "ant/message.hpp" +#include "ant/messages.hpp" #include #include @@ -19,6 +19,7 @@ public: void configure_fitness_equipment_channel(ant::ChannelNumber, ant::DeviceNumber); void configure_power_channel(ant::ChannelNumber, ant::DeviceNumber); + void search_fitness_equipment(ant::ChannelNumber); void set_enable_fitness_equipment(const bool value) { enable_fitness_equipment_ = value; @@ -26,16 +27,46 @@ public: bool get_enable_fitness_equipment() const { return enable_fitness_equipment_; } - void set_enable_heart_rate(const bool value) { enable_heart_rate_ = value; } bool get_enable_heart_rate() const { return enable_heart_rate_; } + void set_enable_bicycle_power(const bool value) { + enable_bicycle_power_ = value; + } + bool get_enable_bicycle_power() const { return enable_bicycle_power_; } + + void set_heart_rate_device_number(const int p_heart_rate_device_number) { + heart_rate_device_number_ = p_heart_rate_device_number; + } + int get_heart_rate_device_number() const { return heart_rate_device_number_; } + void set_fitness_equipment_device_number( + const int p_fitness_eqipment_device_number) { + fitness_equipment_device_number_ = p_fitness_eqipment_device_number; + } + int get_fitness_equipment_device_number() const { + return fitness_equipment_device_number_; + } + void + set_bicycle_power_device_number(const int p_bicycle_power_device_number) { + bicycle_power_device_number_ = p_bicycle_power_device_number; + } + int get_bicycle_power_device_number() const { + return bicycle_power_device_number_; + } + + bool init_ant_device(); + + void configure_enabled_channels(); protected: static void _bind_methods(); private: - bool enable_fitness_equipment_{false}; - bool enable_heart_rate_{false}; + bool enable_fitness_equipment_; + int fitness_equipment_device_number_; + bool enable_heart_rate_; + int heart_rate_device_number_; + bool enable_bicycle_power_; + int bicycle_power_device_number_; std::unique_ptr ant_device_; std::unique_ptr hr_channel_; diff --git a/ant/src/main.cpp b/ant/src/main.cpp index 25d79b6..5b86a6f 100644 --- a/ant/src/main.cpp +++ b/ant/src/main.cpp @@ -1,7 +1,7 @@ #include "ant/ant_device.hpp" #include "ant/channels.hpp" -#include "ant/message.hpp" +#include "ant/messages.hpp" #include int main() { diff --git a/ant/src/register_types.cpp b/ant/src/register_types.cpp index 9d021ba..167dc11 100644 --- a/ant/src/register_types.cpp +++ b/ant/src/register_types.cpp @@ -1,6 +1,8 @@ #include "register_types.hpp" #include "ant_controller.hpp" +#include "ant/ant_device.hpp" +#include "ant/channels.hpp" #include #include @@ -14,6 +16,14 @@ void initialize_ant_module(ModuleInitializationLevel p_level) { } GDREGISTER_RUNTIME_CLASS(AntController); + GDREGISTER_RUNTIME_CLASS(ant::ANTDevice); + GDREGISTER_RUNTIME_CLASS(ant::BroadcastPayload); + GDREGISTER_RUNTIME_CLASS(ant::ChannelResponseMessage); + GDREGISTER_RUNTIME_CLASS(ant::MessageProcessor); + GDREGISTER_RUNTIME_CLASS(ant::Channel); + GDREGISTER_RUNTIME_CLASS(ant::FitnessEquipmentChannel); + GDREGISTER_RUNTIME_CLASS(ant::HeartRateChannel); + GDREGISTER_RUNTIME_CLASS(ant::PowerChannel); } void uninitialize_ant_module(ModuleInitializationLevel p_level) { diff --git a/demo/ant_device.gd b/demo/ant_device.gd new file mode 100644 index 0000000..393cd4e --- /dev/null +++ b/demo/ant_device.gd @@ -0,0 +1,92 @@ +extends ANTDevice + +@onready var hr_label = $"../PanelContainer/hr_container/VBoxContainer/value_label" +@onready var toruqe_cnt_label = $"../PanelContainer/pwr_container/VBoxContainer/HBoxContainer2/torque_cnt_value" +@onready var power_cnt_label = $"../PanelContainer/pwr_container/VBoxContainer/HBoxContainer/power_cnt_value" +@onready var operating_time_label = $"../PanelContainer/pwr_container/VBoxContainer/operating_time_label" +@onready var voltage_label = $"../PanelContainer/pwr_container/VBoxContainer/voltage_label" +@onready var power_label = $"../PanelContainer/pwr_container/VBoxContainer/power_label" + +var torque_counter: int = 0 +var power_counter: int = 0 + +# Called when the node enters the scene tree for the first time. +func _ready() -> void: + pass # Replace with function body. + + +# Called every frame. 'delta' is the elapsed time since the previous frame. +func _process(delta: float) -> void: + pass + + +func _on_broadcast_data_received(channel: int, msg_ref: Object) -> void: + var children = get_children() + for child in children: + if child is Channel: + var chan: Channel = child as Channel + print("Dispatching broadcast data to %s" % chan.name) + chan.on_broadcast_data(msg_ref) + +func _set_channel_numbers(): + var i: int = 1 + var children = get_children() + for child in children: + if child is Channel: + var chan: Channel = child as Channel + chan.channel = i + i += 1 + + +func _on_channel_response_received(channel: int, msg_ref: Object) -> void: + var children = get_children() + for child in children: + if child is Channel: + var chan: Channel = child as Channel + if chan.channel == channel: + print("Dispatching channel_response data to %s" % chan.name) + chan.on_channel_response(msg_ref) + + +func _on_device_number_received(channel: int, device_number: int) -> void: + var children = get_children() + for child in children: + if child is Channel: + var chan: Channel = child as Channel + if chan.channel == channel: + print("Dispatching device_number data to %s" % chan.name) + chan.device_number = device_number + + +func _on_heart_rate_channel_heart_rate_updated(heart_rate: int) -> void: + hr_label.text = "%d" % heart_rate + + +func _on_connect_button_pressed() -> void: + $HeartRateChannel.start_searching() + + +func _on_power_channel_battery_status_received(operating_time: int, status: String, voltage: float) -> void: + var hours = int(operating_time / 3600) + var minutes = int((operating_time % 3600) / 60) + var seconds = operating_time % 60 + operating_time_label.text = "%02d:%02d:%02d" % [hours, minutes, seconds] + + +func _on_power_channel_crank_torque_received(power: float, torque: float, cadence: int) -> void: + torque_counter += 1 + toruqe_cnt_label.text = "%d" % torque_counter + + +func _on_power_channel_power_received(power: float, cadence: int) -> void: + power_counter += 1 + power_cnt_label.text = "%d" % power_counter + power_label.text = "%d" % (int(power)) + + +func _on_power_connect_button_pressed() -> void: + $PowerChannel.start_searching() + + +func _on_power_channel_search_timed_out() -> void: + $PowerChannel.start_searching() diff --git a/demo/ant_device.gd.uid b/demo/ant_device.gd.uid new file mode 100644 index 0000000..d8a4f95 --- /dev/null +++ b/demo/ant_device.gd.uid @@ -0,0 +1 @@ +uid://dujw1jse1b42x diff --git a/demo/bin/libant.so b/demo/bin/libant.so index 230d3c7..e0fb28f 100755 Binary files a/demo/bin/libant.so and b/demo/bin/libant.so differ diff --git a/demo/node_3d.gd b/demo/node_3d.gd index 0e30365..b7f93ad 100644 --- a/demo/node_3d.gd +++ b/demo/node_3d.gd @@ -1,27 +1,21 @@ extends Node3D -@onready var ant_controller = $AntController +@onready var ant_device = $ANTDevice @onready var hr_label = $PanelContainer/hr_container/VBoxContainer/value_label @onready var pwr_label = $PanelContainer/pwr_container/VBoxContainer/value_label var timer: Timer func _ready() -> void: - ant_controller.connect_ant_channels() + ant_device.init() timer = Timer.new() add_child(timer) timer.timeout.connect(on_process_ant_messages) timer.start(0.05) -func delayed_connect(): - ant_controller.connect_ant_channels() func _on_ant_controller_heart_rate_updated(heart_rate: int) -> void: hr_label.text = "%d" % heart_rate func on_process_ant_messages(): - ant_controller.parse_messages() - - -func _on_ant_controller_fec_power_updated(power: int) -> void: - pwr_label.text = "%d" % power + ant_device.receive() diff --git a/demo/node_3d.tscn b/demo/node_3d.tscn index 66a7f53..dad9457 100644 --- a/demo/node_3d.tscn +++ b/demo/node_3d.tscn @@ -1,11 +1,18 @@ -[gd_scene load_steps=2 format=3 uid="uid://cl7rfid2g3c4d"] +[gd_scene load_steps=3 format=3 uid="uid://cl7rfid2g3c4d"] [ext_resource type="Script" uid="uid://cuyhf18taa5if" path="res://node_3d.gd" id="1_a202f"] +[ext_resource type="Script" uid="uid://dujw1jse1b42x" path="res://ant_device.gd" id="2_noarx"] [node name="Node3D" type="Node3D"] script = ExtResource("1_a202f") -[node name="AntController" type="AntController" parent="."] +[node name="ANTDevice" type="ANTDevice" parent="."] +script = ExtResource("2_noarx") + +[node name="HeartRateChannel" type="HeartRateChannel" parent="ANTDevice"] +channel = 2 + +[node name="PowerChannel" type="PowerChannel" parent="ANTDevice"] [node name="PanelContainer" type="BoxContainer" parent="."] offset_right = 40.0 @@ -17,6 +24,10 @@ layout_mode = 2 [node name="VBoxContainer" type="VBoxContainer" parent="PanelContainer/hr_container"] layout_mode = 2 +[node name="connect_button" type="Button" parent="PanelContainer/hr_container/VBoxContainer"] +layout_mode = 2 +text = "Connect" + [node name="title_label" type="Label" parent="PanelContainer/hr_container/VBoxContainer"] layout_mode = 2 text = "Hear Rate" @@ -30,12 +41,55 @@ layout_mode = 2 [node name="VBoxContainer" type="VBoxContainer" parent="PanelContainer/pwr_container"] layout_mode = 2 +[node name="connect_button" type="Button" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 +text = "Connect" + [node name="title_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] layout_mode = 2 text = "Pwr" +horizontal_alignment = 1 -[node name="value_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] +[node name="power_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] layout_mode = 2 -[connection signal="fec_power_updated" from="AntController" to="." method="_on_ant_controller_fec_power_updated"] -[connection signal="heart_rate_updated" from="AntController" to="." method="_on_ant_controller_heart_rate_updated"] +[node name="voltage_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 +horizontal_alignment = 1 + +[node name="operating_time_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 +horizontal_alignment = 1 + +[node name="HBoxContainer" type="HBoxContainer" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 + +[node name="power_cnt_title" type="Label" parent="PanelContainer/pwr_container/VBoxContainer/HBoxContainer"] +layout_mode = 2 +text = "pwr_cnt: " + +[node name="power_cnt_value" type="Label" parent="PanelContainer/pwr_container/VBoxContainer/HBoxContainer"] +layout_mode = 2 +horizontal_alignment = 1 + +[node name="HBoxContainer2" type="HBoxContainer" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 + +[node name="torque_cnt_title" type="Label" parent="PanelContainer/pwr_container/VBoxContainer/HBoxContainer2"] +layout_mode = 2 +text = "torque cnt:" + +[node name="torque_cnt_value" type="Label" parent="PanelContainer/pwr_container/VBoxContainer/HBoxContainer2"] +layout_mode = 2 +horizontal_alignment = 1 + +[connection signal="broadcast_data_received" from="ANTDevice" to="ANTDevice" method="_on_broadcast_data_received"] +[connection signal="channel_response_received" from="ANTDevice" to="ANTDevice" method="_on_channel_response_received"] +[connection signal="device_number_received" from="ANTDevice" to="ANTDevice" method="_on_device_number_received"] +[connection signal="heart_rate_updated" from="ANTDevice/HeartRateChannel" to="ANTDevice" method="_on_heart_rate_channel_heart_rate_updated"] +[connection signal="battery_status_received" from="ANTDevice/PowerChannel" to="ANTDevice" method="_on_power_channel_battery_status_received"] +[connection signal="crank_torque_received" from="ANTDevice/PowerChannel" to="ANTDevice" method="_on_power_channel_crank_torque_received"] +[connection signal="power_received" from="ANTDevice/PowerChannel" to="ANTDevice" method="_on_power_channel_power_received"] +[connection signal="search_timed_out" from="ANTDevice/PowerChannel" to="ANTDevice" method="_on_power_channel_search_timed_out"] +[connection signal="pressed" from="PanelContainer/hr_container/VBoxContainer/connect_button" to="ANTDevice" method="_on_connect_button_pressed"] +[connection signal="pressed" from="PanelContainer/pwr_container/VBoxContainer/connect_button" to="ANTDevice" method="_on_power_connect_button_pressed"]