commit d118b4fbb9a824f54f8152894a160d498c5aa6d1 Author: Thies Lennart Alff Date: Sun Apr 6 16:56:16 2025 +0200 initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1ada2f1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.cache/ +build/ +.godot/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7ef8f99 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.19) +project(gdextension-ant) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +include(FetchContent) + +FetchContent_Declare( + GDExtension + GIT_REPOSITORY https://github.com/godotengine/godot-cpp.git + GIT_TAG godot-4.4-stable +) + +FetchContent_MakeAvailable(GDExtension) +add_subdirectory(ant) diff --git a/ant/CMakeLists.txt b/ant/CMakeLists.txt new file mode 100644 index 0000000..25b6b6b --- /dev/null +++ b/ant/CMakeLists.txt @@ -0,0 +1,30 @@ +project(ant) + +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 + ${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 +) +target_include_directories(${PROJECT_NAME} PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/src") +target_link_libraries(${PROJECT_NAME} PUBLIC godot::cpp) diff --git a/ant/src/ant/ant.hpp b/ant/src/ant/ant.hpp new file mode 100644 index 0000000..c675116 --- /dev/null +++ b/ant/src/ant/ant.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include + +namespace ant { + +typedef uint8_t Page; + +struct CommonPages { + enum : Page { + kRequestDataPage = 0x46, + kCommandStatus = 0x47, + kSubfieldData = 0x54, + + }; +}; + +struct HeartRateProfilePages { + enum : Page { + kDefaultPage = 0x00, + kCumulativeOperatingTime, + kManufacturerInformation, + kProductInformation, + kPreviousHeartbeatEventTime, + kSwimIntervalSummary, + kCapabilities, + kBatteryStatus, + kDeviceInformation, + kHeartRateFeature = 32, + kRequestDataPage = 70, + kModeSettings = 76, + }; +}; + +struct FitnessEquipmentPages { + enum : Page { + kCalibrationRequestAndResponse = 0x01, + kCalibrationInProgress = 0x02, + kGeneralData = 0x10, + kGeneralSettings = 0x11, + kGeneralMetabolicData = 0x12, + kBikeSepcificData = 0x19, + kBasicResistance = 0x30, + kTargetPower, + kWindResistance, + kTrackResistance, + kCapabilities = 0x36, + }; +}; + +struct PowerProfilePages { + enum : Page { + kCalibrationMessage = 0x01, + kGetSetPage = 0x02, + kMeasurement = 0x03, + kStandardPower = 0x10, + kStandardTorqueAtWheel = 0x11, + kStandardTorqueAtCrank = 0x12, + kStandardTorqueEffectiveness = 0x13, + kCrankTorqueFrequency = 0x20, + kRightForceAngle = 0xe0, + kLeftForceAngle = 0xe1, + kPedalPosition = 0xe2, + kBattery = 0x52, + }; +}; +struct PowerProfileSubpages { + enum : Page { + kCrankParameters = 0x01, + kAdvancedCapabilities = 0xfd, + kAdvancedCapabilities2 = 0xfe, + }; +}; + +static constexpr uint8_t key[8] = {0xB9, 0xA5, 0x21, 0xFB, + 0xBD, 0x72, 0xC3, 0x45}; + +static constexpr uint8_t kSyncByte = 0xA4; +static constexpr uint8_t kReservedByte = 0xff; + +typedef uint8_t ChannelNumber; +typedef uint8_t ChannelFrequency; +typedef uint8_t TransmissionType; +typedef uint16_t DeviceNumber; +struct DeviceNumbers { + enum : DeviceNumber { + Wildcard = 0x0000, + LennartsGarminVector3 = 0x5ad0, + LennartsPolar = 0x27f6 + }; +}; +typedef uint16_t ChannelPeriod; + +static constexpr DeviceNumber DeviceNumberWildcard = 0x00; + +enum class DeviceType : uint8_t { + kAny = 0x00, + kHeartRate = 0x78, + kPower = 0x0B, + kFitnessEquipment = 0x11, +}; + +enum class ChannelType : uint8_t { + kQuickSearch = 0x10, + kWaiting = 0x20, + kRX = 0x0, + kTX = 0x10, + kPair = 0x40, +}; + +struct ChannelConfig { + ChannelType type; + uint8_t *network_key; + ChannelFrequency frequency; + TransmissionType transmission_type; + DeviceType device_type; + DeviceNumber device_number; + ChannelPeriod channel_period; +}; + +} // namespace ant diff --git a/ant/src/ant/ant_device.cpp b/ant/src/ant/ant_device.cpp new file mode 100644 index 0000000..d4f20a1 --- /dev/null +++ b/ant/src/ant/ant_device.cpp @@ -0,0 +1,115 @@ +#include "ant_device.hpp" +#include +#include +#include +namespace ant { + +ANTDevice::ANTDevice() {} + +bool ANTDevice::Init() { + if (!Open()) { + return false; + } + printf("Sending a reset command!\n"); + sleep(1); + SendMessage(Message::SystemReset()); + sleep(1); + uint8_t network = 1; + SendMessage(Message::SetNetworkKey(network, key)); + while (ReceiveMessage()) + ; + return true; +} + +bool ANTDevice::ReceiveMessage() { + int n; + uint8_t buf[1]; + do { + n = Read(buf, 1, 500); + for (int i = 0; i < n; ++i) { + if (processor.FeedData(buf[i])) { + return true; + } + } + } while (n > 0); + return false; +} + +bool ANTDevice::Open() { + serial_port_ = open("/dev/ttyUSB0", O_RDWR | O_NOCTTY | O_NONBLOCK); + if (serial_port_ < 0) { + perror("Failed to open port"); + return false; + } + + // remove all lingering data from the buffers + tcflush(serial_port_, TCIOFLUSH); + + int line_discipline = N_TTY; + if (ioctl(serial_port_, TIOCSETD, &line_discipline) < 0) { + perror("Failed to set line discipline."); + return false; + } + + struct termios settings; + tcgetattr(serial_port_, &settings); + cfmakeraw(&settings); + cfsetspeed(&settings, B115200); + settings.c_iflag = IGNPAR; + settings.c_oflag = 0; + // clear size and stop bit settings so we can configure them + settings.c_cflag &= (~CSIZE & CSTOPB); + settings.c_cflag |= (CS8 | CREAD | HUPCL | CRTSCTS); + settings.c_lflag = 0; + + // return reads immediately + settings.c_cc[VMIN] = 0; + settings.c_cc[VTIME] = 0; + + if (tcsetattr(serial_port_, TCSANOW, &settings) < 0) { + perror("Failed to set serial settings."); + return false; + } + return true; +} + +bool ANTDevice::Close() { + tcflush(serial_port_, TCIOFLUSH); + if (close(serial_port_) == -1) { + perror("Failed to close serial port"); + return false; + } + return true; +} + +int ANTDevice::SendMessage(Message msg) { + return Write(msg.data, msg.data_length); +} + +int ANTDevice::Write(uint8_t *buf, int size, int timeout) { + int n_written = write(serial_port_, buf, size); + // if (n_written != size) { + // printf("Wanted to write %d bytes but have written %d bytes.\n", size, + // n_written); + // } + return n_written; +} + +int ANTDevice::Read(uint8_t *buf, int bytes, int timeout_ms) { + fd_set readfs; + timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 1000 * timeout_ms; + FD_ZERO(&readfs); + FD_SET(serial_port_, &readfs); + select(serial_port_ + 1, &readfs, NULL, NULL, &tv); + int n_bytes = 0; + if (FD_ISSET(serial_port_, &readfs)) { + n_bytes = read(serial_port_, buf, bytes); + if (n_bytes < 0) { + perror("Failed to read from serial device.\n"); + } + } + return n_bytes; +} +} // namespace ant diff --git a/ant/src/ant/ant_device.hpp b/ant/src/ant/ant_device.hpp new file mode 100644 index 0000000..6183c04 --- /dev/null +++ b/ant/src/ant/ant_device.hpp @@ -0,0 +1,40 @@ +#pragma once +#include "message.hpp" +#include "message_processor.hpp" +#include + +#include "ant.hpp" +#include +#include +#include +#include +#include +#include +#include + +#define GARMIN_USB2_VENDOR_ID 0x0fcf +#define GARMIN_USB2_PRODUCT_ID 0x1008 +namespace ant { +class ANTDevice { +public: + ANTDevice(); + ~ANTDevice() { Close(); } + int Read(uint8_t *buf, int bytes, int timeout_ms = 125); + int Write(uint8_t *bytes, int size, int timeout = 125); + int SendMessage(Message); + bool ReceiveMessage(); + bool Open(); + bool Close(); + bool Init(); + MessageProcessor processor; + +private: + struct Buffer { + static constexpr int kBufferSize = 128; + uint8_t data[kBufferSize]; + uint8_t index{0}; + }; + Buffer read_buffer_; + int serial_port_; +}; +} // namespace ant diff --git a/ant/src/ant/bicycle_power.hpp b/ant/src/ant/bicycle_power.hpp new file mode 100644 index 0000000..36de00c --- /dev/null +++ b/ant/src/ant/bicycle_power.hpp @@ -0,0 +1,95 @@ +#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 new file mode 100644 index 0000000..17c9578 --- /dev/null +++ b/ant/src/ant/channel/channel.cpp @@ -0,0 +1,198 @@ +#include "channel.hpp" +#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; + + _ant_device.processor.SetOnChannelResponseCallback( + _channel_id, + [this](ChannelResponseMessage msg) { OnChannelResponse(msg); }); + + _ant_device.processor.SetOnBroadcastDataCallback( + _channel_id, [this](BroadcastPayload msg) { OnBroadcastData(msg); }); +} + +Channel::~Channel() {} + +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); +} + +bool Channel::SendBuffered(const Message &_msg) { + std::lock_guard lock(ack_send_queue_mutex_); + if (!ack_send_queue_.push(_msg)) { + return false; + } + if (ack_send_queue_idle_) { + printf("queue is idle. Sending immediately.\n"); + ant_device_.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(); +} +void Channel::OnChannelResponse(ChannelResponseMessage msg) { + if (msg.channel_id_ != id_) { + fprintf( + stderr, + "Channel %hu: Received msg for %hu. This should not be possible...\n", + id_, msg.channel_id_); + return; + } + if (state_ == ChannelState::kConnected) { + if (msg.msg_id_ == MessageIDs::kRFEvent) { + switch (msg.msg_code_) { + case MessageCodes::kEventRXFail: { + rx_fail_count_++; + break; + } + case MessageCodes::kEventTransferTXCompleted: { + printf("[channel %hu] acknowledgment received.\n", id_); + // 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()); + } else { + ack_send_queue_idle_ = true; + } + break; + } + case MessageCodes::kEventTransferTXFailed: { + printf("[channel %hu] failed to receive acknowledgment.\n", id_); + 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()); + } else { + ack_retries_ = 0; + printf("[channel %hu] Giving up to send acknowledged message after " + "max retries.\n", + id_); + ack_send_queue_.pop(); + if (ack_send_queue_.empty()) { + ack_send_queue_idle_ = true; + } else { + ant_device_.SendMessage(ack_send_queue_.front()); + } + } + } else { + ack_send_queue_idle_ = true; + } + break; + } + default: + printf("[channel %hu] Connected and received channel response: 0x%02x " + "%hu\n", + id_, msg.msg_id_, msg.msg_code_); + break; + } + } + return; + } + if (state_ == ChannelState::kSearching) { + // printf("Channel Response: 0x%02x %hu\n", msg.msg_id_, msg.msg_code_); + switch (msg.msg_id_) { + // RF Event + case MessageIDs::kRFEvent: + 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); + break; + } + break; + case MessageIDs::kAssignChannel: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to assign channel.\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage(Message::SetChannelID( + id_, channel_config_.device_number, channel_config_.device_type, + channel_config_.transmission_type)); + break; + case MessageIDs::kChannelID: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel id!\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage( + Message::SetChannelFrequency(id_, channel_config_.frequency)); + break; + case MessageIDs::kChannelFrequency: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel frequency!\n", + id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage( + Message::SetChannelPeriod(id_, channel_config_.channel_period)); + break; + case MessageIDs::kChannelPeriod: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel period!\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage(Message::SetLPSearchTimeout(id_, 4)); + break; + case MessageIDs::kLPSearchTimeout: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel period!\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage(Message::SetHPSearchTimeout(id_, 0)); + break; + case MessageIDs::kSearchTimeout: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to set channel timeout\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + ant_device_.SendMessage(Message::OpenChannel(id_)); + break; + case MessageIDs::kOpenChannel: + if (msg.msg_code_ != MessageCodes::kResponseNoError) { + fprintf(stderr, "[channel %hu] failed to open channel\n", id_); + SetChannelState(ChannelState::kUndefined); + return; + } + 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_); + break; + } + } +} + +} // namespace ant diff --git a/ant/src/ant/channel/channel.hpp b/ant/src/ant/channel/channel.hpp new file mode 100644 index 0000000..1ab512d --- /dev/null +++ b/ant/src/ant/channel/channel.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include "../ant.hpp" +#include "../ant_device.hpp" +#include +#include + +namespace ant { + +enum class ChannelState { + kClosed, + kOpening, + kSearching, + kConnected, + kUndefined, +}; + +template class MessageQueue { +public: + void pop() { + if (queue_.empty()) { + return; + } + queue_.pop(); + } + bool push(const Message &msg) { + if (queue_.size() < n) { + queue_.push(msg); + return true; + } + return false; + } + bool push(Message &&msg) { + if (queue_.size() < n) { + queue_.push(msg); + return true; + } + return false; + } + bool empty() const { return queue_.empty(); } + Message &front() { return queue_.front(); } + std::size_t size() const { return queue_.size(); } + +private: + std::queue queue_; +}; + +class Channel { +public: + Channel(ANTDevice &, ChannelNumber, DeviceNumber); + virtual ~Channel() = 0; + void StartSearch(); + void OnSearchTimeout(); + virtual void OnBroadcastData(BroadcastPayload) = 0; + void OnChannelResponse(ChannelResponseMessage); + ChannelNumber channel_id() const { return id_; } + bool SendBuffered(const Message &); + +protected: + void SetChannelState(ChannelState _state) { state_ = _state; } + + MessageQueue<10> ack_send_queue_; + std::mutex ack_send_queue_mutex_; + bool ack_send_queue_idle_{true}; + int ack_retries_{0}; + uint64_t rx_fail_count_{0}; + + ChannelConfig channel_config_; + ANTDevice ant_device_; + ::ant::ChannelNumber id_; + ChannelState state_{ChannelState::kUndefined}; + bool connected_{false}; +}; + +} // namespace ant diff --git a/ant/src/ant/channel/fitness_equipment_channel.cpp b/ant/src/ant/channel/fitness_equipment_channel.cpp new file mode 100644 index 0000000..bf9b9b0 --- /dev/null +++ b/ant/src/ant/channel/fitness_equipment_channel.cpp @@ -0,0 +1,138 @@ +#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; + channel_config_.transmission_type = 0; + channel_config_.device_type = DeviceType::kFitnessEquipment; + channel_config_.channel_period = 8192; +} + +void FitnessEquipmentChannel::OnBroadcastData(BroadcastPayload payload) { + if (state_ != ChannelState::kConnected) { + printf("Received first broadcast data!\n"); + SetChannelState(ChannelState::kConnected); + ant_device_.SendMessage(Message::RequestChannelID(id_)); + } + uint8_t page_number = payload.raw_data[0]; + switch (page_number) { + case FitnessEquipmentPages::kCalibrationRequestAndResponse: { + printf("[channel %hu] CalibrationRequestAndResponse received.\n", id_); + break; + } + case FitnessEquipmentPages::kCalibrationInProgress: { + printf("[channel %hu] CalibrationInProgress received.\n", id_); + break; + } + case FitnessEquipmentPages::kGeneralData: { + printf("[channel %hu] GeneralData received.\n", id_); + break; + } + case FitnessEquipmentPages::kGeneralSettings: { + printf("[channel %hu] GeneralSettings received.\n", id_); + break; + } + case FitnessEquipmentPages::kGeneralMetabolicData: { + printf("[channel %hu] MetabolicData received.\n", id_); + break; + } + case FitnessEquipmentPages::kBikeSepcificData: { + fitness_equipment::BikeSpecificDataPage bike_data( + fitness_equipment::BikeSpecificDataPage::FromPayload(payload)); + uint16_t avg_power = + (bike_data.AccumulatedPower() - bike_data_accumulated_power_) / + (bike_data.UpdateEventCount() - bike_data_event_count_); + bike_data_event_count_ = bike_data.UpdateEventCount(); + if (power_cb_) { + power_cb_(avg_power); + } + bike_data_accumulated_power_ = bike_data.AccumulatedPower(); + printf("Cadence: %hu, InstPower: %hu, EventCount: %hu, AvgPower: %hu\n", + bike_data.InstantaneousCadence(), bike_data.InstantaneousPower(), + bike_data.UpdateEventCount(), avg_power); + // RequestCapabilies(1); + RequestCommandStatus(1); + break; + } + case FitnessEquipmentPages::kTargetPower: { + uint16_t power = (payload.raw_data.at(7) << 8 | payload.raw_data.at(6)) / 4; + printf("Currently set power target: %hu\n", power); + break; + } + case CommonPages::kCommandStatus: { + common::CommandStatusPage page( + common::CommandStatusPage::FromPayload(payload)); + PrintCommandStatus(page); + break; + } + case FitnessEquipmentPages::kCapabilities: { + fitness_equipment::Capabilities caps( + fitness_equipment::Capabilities::FromPayload(payload)); + PrintCapabilities(caps); + break; + } + } +} + +void FitnessEquipmentChannel::PrintCommandStatus( + const common::CommandStatusPage &page) { + std::string data_str; + std::string command_name; + + switch (page.PreviousCommand()) { + case FitnessEquipmentPages::kBasicResistance: { + uint8_t resistance = page.Data3(); + command_name = "Basic Resistance"; + data_str = "Resistance: " + std::to_string(resistance); + break; + } + case FitnessEquipmentPages::kTargetPower: { + uint16_t power = (page.Data2() | (page.Data3() << 8)) / 4; + command_name = "Target Power"; + data_str = "Power: " + std::to_string(power); + break; + } + case FitnessEquipmentPages::kWindResistance: { + uint8_t wind_resistance_coeff = page.Data1(); + uint8_t wind_speed = page.Data2(); + uint8_t drafting_factor = page.Data3(); + command_name = "Wind Resistance"; + data_str = + "wind_resistance_coeff: " + std::to_string(wind_resistance_coeff) + + " wind_speed: " + std::to_string(wind_speed) + + " drafting_factor: " + std::to_string(drafting_factor); + break; + } + case FitnessEquipmentPages::kTrackResistance: { + uint16_t slope = page.Data1() | (page.Data2() << 8); + uint8_t coeff = page.Data3(); + command_name = "Track Resistance"; + data_str = + "Slope: " + std::to_string(slope) + " Coeff: " + std::to_string(coeff); + break; + } + default: + command_name = + "Unhandled Command Page: " + std::to_string(page.PreviousCommand()); + data_str = ""; + break; + } + printf("%s (%s)\n%s\n", command_name.c_str(), page.StatusString().c_str(), + data_str.c_str()); +} + +void FitnessEquipmentChannel::PrintCapabilities( + const fitness_equipment::Capabilities &caps) { + printf("Capabilities:\n\tMax Resistance: %hu\n\tSupported Caps:\n\t\tBasic " + "Resistance: %hu\n\t\tTarget Power: %hu\n\t\tSimulation: %hu\n", + caps.MaxResistance(), caps.IsBasicResistanceSupported(), + caps.IsTargetPowerSupported(), caps.IsSimulationSupported()); +} + +} // namespace ant diff --git a/ant/src/ant/channel/fitness_equipment_channel.hpp b/ant/src/ant/channel/fitness_equipment_channel.hpp new file mode 100644 index 0000000..c656272 --- /dev/null +++ b/ant/src/ant/channel/fitness_equipment_channel.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include "../common.hpp" +#include "../fitness_equipment.hpp" +#include "channel.hpp" +#include + +namespace ant { + +class FitnessEquipmentChannel : public Channel { +public: + FitnessEquipmentChannel(ANTDevice &, ChannelNumber, DeviceNumber); + ~FitnessEquipmentChannel() {} + void OnBroadcastData(BroadcastPayload) final; + + bool SetTargetPower(uint16_t power) { + auto msg = Message::FitnessEquipmentTargetPower(id_, power * 4); + return SendBuffered(msg); + } + + bool RequestCapabilies(uint8_t requested_transmissions) { + auto msg = Message::FitnessEquipmentRequestCapabilities( + id_, requested_transmissions); + return SendBuffered(msg); + } + bool RequestCommandStatus(uint8_t requested_transmissions) { + auto msg = + Message::CommonRequestCommandStatus(id_, requested_transmissions); + return SendBuffered(msg); + } + + bool SetWindResistance(double wind_resistance_coeff = 0.51, + double windspeed = 0.0, double drafting_factor = 1.0) { + wind_resistance_coeff = std::clamp(wind_resistance_coeff, 0.0, 1.86); + + uint8_t coeff_raw = static_cast((wind_resistance_coeff * 100.0)); + uint8_t windspeed_raw = windspeed * 3.6 + 127; + 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); + return SendBuffered(msg); + } + + bool SetTrackResistance(double slope, double resistance_coeff) { + 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, + resistance_coeff_raw); + return SendBuffered(msg); + } + + void PrintCommandStatus(const common::CommandStatusPage &); + void PrintCapabilities(const fitness_equipment::Capabilities &); + + void SetPowerCallback(std::function _cb) { power_cb_ = _cb; } + void SetCadenceCallback(std::function _cb) { + cadence_cb_ = _cb; + } + +private: + uint8_t bike_data_event_count_{0}; + uint16_t bike_data_accumulated_power_{0}; + std::function power_cb_{nullptr}; + std::function cadence_cb_{nullptr}; +}; + +} // namespace ant diff --git a/ant/src/ant/channel/heart_rate_channel.cpp b/ant/src/ant/channel/heart_rate_channel.cpp new file mode 100644 index 0000000..0651d18 --- /dev/null +++ b/ant/src/ant/channel/heart_rate_channel.cpp @@ -0,0 +1,30 @@ +#include "heart_rate_channel.hpp" + +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; + channel_config_.transmission_type = 0; + channel_config_.device_type = DeviceType::kHeartRate; + channel_config_.channel_period = 8070; +} +void HeartRateChannel::OnBroadcastData(BroadcastPayload data) { + if (state_ != ChannelState::kConnected) { + printf("Received first broadcast data!\n"); + SetChannelState(ChannelState::kConnected); + ant_device_.SendMessage(Message::RequestChannelID(id_)); + } + // 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); + } + printf("Heart Rate: %hu\n", heart_rate); +} + +} // namespace ant diff --git a/ant/src/ant/channel/heart_rate_channel.hpp b/ant/src/ant/channel/heart_rate_channel.hpp new file mode 100644 index 0000000..771c2d8 --- /dev/null +++ b/ant/src/ant/channel/heart_rate_channel.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "channel.hpp" + +namespace ant { + +typedef std::function HeartRateCallback; + +class HeartRateChannel : public Channel { +public: + HeartRateChannel(ANTDevice &, ChannelNumber, DeviceNumber); + ~HeartRateChannel() {} + void OnBroadcastData(BroadcastPayload) final; + void SetOnHeartRateCallback(HeartRateCallback cb) {heart_rate_cb_ = cb;} + +private: + HeartRateCallback heart_rate_cb_{nullptr}; +}; + +} // namespace ant diff --git a/ant/src/ant/channel/power_channel.cpp b/ant/src/ant/channel/power_channel.cpp new file mode 100644 index 0000000..8644253 --- /dev/null +++ b/ant/src/ant/channel/power_channel.cpp @@ -0,0 +1,86 @@ +#include "power_channel.hpp" + +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::OnBroadcastData(BroadcastPayload 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); + } + uint8_t page_number = data.raw_data[0]; + switch (page_number) { + printf("[channel %hu] received page: %hu\n", id_, page_number); + case PowerProfilePages::kBattery: { + 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)); + + break; + } + case PowerProfilePages::kStandardTorqueAtCrank: { + printf("[channel %hu] received StandardTorqueAtCrank\n", id_); + break; + } + case PowerProfilePages::kGetSetPage: + 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); + printf("----------\nCaps2\n----------\n"); + printf(fmt, "4Hz", caps.Supports4Hz(), caps.Is4HzEnabled()); + printf(fmt, "8Hz", caps.Supports8Hz(), caps.Is8HzEnabled()); + printf(fmt, "PowerPhase", caps.SupportsPowerPhase(), + caps.IsPowerPhaseEnabled()); + printf(fmt, "PCO", caps.SupportsPCO(), caps.IsPCOEnabled()); + printf(fmt, "RiderPosition", caps.SupportsRiderPosition(), + caps.IsRiderPositionEnabled()); + printf(fmt, "TorqueBarycenter", caps.SupportsTorqueBarycenter(), + caps.IsTorqueBarycenterEnabled()); + } break; + 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); + printf("----------\nCaps1\n----------\n"); + printf(fmt, "4Hz", caps.Supports4Hz(), caps.Is4HzEnabled()); + printf(fmt, "8Hz", caps.Supports8Hz(), caps.Is8HzEnabled()); + printf(fmt, "AutoZero", caps.SupportsAutoZero(), + caps.IsAutoZeroEnabled()); + printf(fmt, "AutoCrankLength", caps.SupportsAutoCrankLength(), + caps.IsAutoCrankLengthEnabled()); + printf(fmt, "TE/PS", caps.SupportsTorqueEfficiencyAndSmoothness(), + caps.IsTorqueEfficiencyAndSmoothnessEnabled()); + } + } + break; + } +} +} // namespace ant diff --git a/ant/src/ant/channel/power_channel.hpp b/ant/src/ant/channel/power_channel.hpp new file mode 100644 index 0000000..7df93d6 --- /dev/null +++ b/ant/src/ant/channel/power_channel.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include "../bicycle_power.hpp" +#include "channel.hpp" + +namespace ant { + +class PowerChannel : public Channel { +public: + PowerChannel(ANTDevice &, ChannelNumber, DeviceNumber); + ~PowerChannel() {} + void OnBroadcastData(BroadcastPayload) final; + // type specific + void RequestCrankParameters(uint8_t n_transmissions = 2) { + auto msg = Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kCrankParameters, n_transmissions); + SendBuffered(msg); + } + void RequestAdvancedCapabilities1(uint8_t n_transmissions = 2) { + auto msg = Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kAdvancedCapabilities, n_transmissions); + SendBuffered(msg); + } + void RequestAdvancedCapabilities2(uint8_t n_tramsissions = 2) { + auto msg = Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kAdvancedCapabilities2, n_tramsissions); + SendBuffered(msg); + } + void Set8Hz(bool enabled) { + auto msg = Message::SetParameter(id_, 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, + 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, + PowerProfileSubpages::kAdvancedCapabilities2, kReservedByte, + kReservedByte, caps.Mask(), kReservedByte, caps.Value(), kReservedByte); + SendBuffered(msg); + } +}; +} // namespace ant diff --git a/ant/src/ant/channels.hpp b/ant/src/ant/channels.hpp new file mode 100644 index 0000000..ae1e5ce --- /dev/null +++ b/ant/src/ant/channels.hpp @@ -0,0 +1,6 @@ +#pragma once + +#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 new file mode 100644 index 0000000..818bd38 --- /dev/null +++ b/ant/src/ant/common.hpp @@ -0,0 +1,84 @@ +#pragma once +#include "message.hpp" +#include + +typedef uint8_t CommandStatus; + +namespace ant { + +namespace common { + +struct CommandStatuses { + enum : CommandStatus { + kPass = 0x00, + kFail, + kNotSupported, + kRejected, + kPending, + kNotInitialized = 0xFF, + }; +}; + +class CommandStatusPage { +public: + static constexpr Page id = CommonPages::kCommandStatus; + CommandStatusPage(Page previous_command_page, uint8_t sequence, + CommandStatus status, uint8_t data0, uint8_t data1, + uint8_t data2, uint8_t data3) + : previous_command_(previous_command_page), sequence_(sequence), + status_(status), data0_(data0), data1_(data1), data2_(data2), + data3_(data3) {} + static CommandStatusPage FromPayload(BroadcastPayload payload) { + return CommandStatusPage(payload.raw_data[1], payload.raw_data[2], + payload.raw_data[3], payload.raw_data[4], + payload.raw_data[5], payload.raw_data[6], + payload.raw_data[7]); + } + Page PreviousCommand() const { return previous_command_; } + std::string StatusString() const { + switch (status_) { + case CommandStatuses::kPass: + return "Pass"; + case CommandStatuses::kFail: + return "Fail"; + case CommandStatuses::kPending: + return "Pending"; + case CommandStatuses::kNotSupported: + return "Not Supported"; + case CommandStatuses::kRejected: + return "Rejected"; + case CommandStatuses::kNotInitialized: + return "Not Initialized"; + default: + return "Reserved Value"; + } + } + bool HasCommandPassed() const { return status_ == CommandStatuses::kPass; } + bool HasCommandFailed() const { return status_ == CommandStatuses::kFail; } + bool IsCommandPending() const { return status_ == CommandStatuses::kPending; } + bool IsCommandUnsupported() const { + return status_ == CommandStatuses::kNotSupported; + } + bool IsCommandRejected() const { + return status_ == CommandStatuses::kRejected; + } + bool IsCommandNotInitialized() const { + return status_ == CommandStatuses::kNotInitialized; + } + uint8_t Data0() const { return data0_; } + uint8_t Data1() const { return data1_; } + uint8_t Data2() const { return data2_; } + uint8_t Data3() const { return data3_; } + +protected: + Page previous_command_; + uint8_t sequence_; + CommandStatus status_; + uint8_t data0_; + uint8_t data1_; + uint8_t data2_; + uint8_t data3_; +}; + +} // namespace common +} // namespace ant diff --git a/ant/src/ant/fitness_equipment.hpp b/ant/src/ant/fitness_equipment.hpp new file mode 100644 index 0000000..2d8e712 --- /dev/null +++ b/ant/src/ant/fitness_equipment.hpp @@ -0,0 +1,63 @@ +#pragma once +#include "message.hpp" + +namespace ant { +namespace fitness_equipment { +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) + : update_event_count_(update_event_count), inst_cadence_(inst_cadence), + accumulated_power_(accumulated_power), inst_power_(inst_power) {} + + static BikeSpecificDataPage FromPayload(BroadcastPayload payload) { + uint8_t update_event_count = payload.raw_data.at(1); + uint8_t inst_cadence = payload.raw_data.at(2); + uint16_t acc_power = + 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); + return BikeSpecificDataPage(update_event_count, inst_cadence, acc_power, + inst_power); + } + + bool IsPowerInvalid() { return inst_power_ == 0xFFF; } + uint8_t InstantaneousCadence() const { return inst_cadence_; } + uint16_t InstantaneousPower() const { return inst_power_; } + uint16_t AccumulatedPower() const { return accumulated_power_; } + uint8_t UpdateEventCount() const { return update_event_count_; } + +private: + uint8_t update_event_count_; + uint8_t inst_cadence_; + uint16_t accumulated_power_; + uint16_t inst_power_; +}; + +class Capabilities { +public: + Capabilities(uint16_t max_resistance, uint8_t capabilities_bitfield) + : max_resistance_(max_resistance), capabilities_(capabilities_bitfield) {} + static Capabilities FromPayload(BroadcastPayload payload) { + uint16_t max_resistance = + payload.raw_data.at(5) | (payload.raw_data.at(6) << 8); + uint8_t capabilites = payload.raw_data.at(7); + return Capabilities(max_resistance, capabilites); + } + + uint16_t MaxResistance() const { return max_resistance_; } + + bool IsBasicResistanceSupported() const { + return capabilities_ & (0x01 << 0); + } + bool IsTargetPowerSupported() const { return capabilities_ & (0x01 << 1); } + bool IsSimulationSupported() const { return capabilities_ & (0x01 << 2); } + +private: + uint16_t max_resistance_; + uint8_t capabilities_; +}; + +} // namespace fitness_equipment +} // namespace ant diff --git a/ant/src/ant/message.cpp b/ant/src/ant/message.cpp new file mode 100644 index 0000000..1960615 --- /dev/null +++ b/ant/src/ant/message.cpp @@ -0,0 +1,33 @@ +#include "message.hpp" +#include "ant.hpp" + +namespace ant { +Message::Message() { Init(); } + +Message::Message(uint8_t length, uint8_t type, uint8_t b3, uint8_t b4, + uint8_t b5, uint8_t b6, uint8_t b7, uint8_t b8, uint8_t b9, + uint8_t b10, uint8_t b11, uint8_t b12) { + data[0] = kSyncByte; + data[1] = length; + data[2] = type; + data[3] = b3; + data[4] = b4; + data[5] = b5; + data[6] = b6; + data[7] = b7; + data[8] = b8; + data[9] = b9; + data[10] = b10; + data[11] = b11; + uint8_t crc = 0; + // iterate over header + actual payload length + for (int i = 0; i < (length + kHeaderSize); ++i) { + crc ^= data[i]; + } + // crc goes after header + payload + data[length + kHeaderSize] = crc; + data_length = length + kHeaderSize + kCrcSize; +} + +void Message::Init() {} +} // namespace ant diff --git a/ant/src/ant/message.hpp b/ant/src/ant/message.hpp new file mode 100644 index 0000000..3f311dd --- /dev/null +++ b/ant/src/ant/message.hpp @@ -0,0 +1,348 @@ +#pragma once +#include "ant.hpp" +#include +#include +#include +#define ANT_UNASSIGN_CHANNEL 0x41 +#define ANT_ASSIGN_CHANNEL 0x42 +#define ANT_CHANNEL_ID 0x51 +#define ANT_CHANNEL_PERIOD 0x43 +#define ANT_SEARCH_TIMEOUT 0x44 +#define ANT_CHANNEL_FREQUENCY 0x45 +#define ANT_SET_NETWORK 0x46 +#define ANT_TX_POWER 0x47 +#define ANT_ID_LIST_ADD 0x59 +#define ANT_ID_LIST_CONFIG 0x5A +#define ANT_CHANNEL_TX_POWER 0x60 +#define ANT_LP_SEARCH_TIMEOUT 0x63 +#define ANT_SET_SERIAL_NUMBER 0x65 +#define ANT_ENABLE_EXT_MSGS 0x66 +#define ANT_ENABLE_LED 0x68 +#define ANT_SYSTEM_RESET 0x4A +#define ANT_OPEN_CHANNEL 0x4B +#define ANT_CLOSE_CHANNEL 0x4C +#define ANT_OPEN_RX_SCAN_CH 0x5B +#define ANT_REQ_MESSAGE 0x4D +#define ANT_BROADCAST_DATA 0x4E +#define ANT_ACK_DATA 0x4F +#define ANT_BURST_DATA 0x50 +#define ANT_CHANNEL_EVENT 0x40 +#define ANT_CHANNEL_STATUS 0x52 +#define ANT_CHANNEL_ID 0x51 +#define ANT_VERSION 0x3E +#define ANT_CAPABILITIES 0x54 +#define ANT_SERIAL_NUMBER 0x61 +#define ANT_NOTIF_STARTUP 0x6F +#define ANT_CW_INIT 0x53 +#define ANT_CW_TEST 0x48 +namespace ant { + +inline uint8_t lower_byte(uint16_t value) { + return static_cast(value & 0xFF); +} +inline uint8_t upper_byte(uint16_t value) { + return static_cast((value >> 8) & 0xFF); +} + +typedef uint8_t MessageID; +struct MessageIDs { + enum : MessageID { + kRFEvent = 0x01, + kStartupMessage = 0x6f, + kSerialErrorMessage = 0xae, + kChannelResponse = 0x40, + kUnassignChannel = 0x41, + kAssignChannel = 0x42, + kChannelID = 0x51, + kResetSystem = 0x4a, + kOpenChannel = 0x4b, + kCloseChannel = 0x4c, + kChannelFrequency = 0x45, + kChannelPeriod = 0x43, + kBroadcastData = 0x4e, + kAckData = 0x4f, + kBurstTransferData = 0x50, + kAdvancedBurstData = 0x72, + kChannelStatus = 0x52, + kANTVersion = 0x3e, + kCapabilities = 0x54, + kSerialNumber = 0x61, + kEventBufferConfiguration = 0x74, + kSearchTimeout = 0x44, + kLPSearchTimeout = 0x63, + kRequestMessage = 0x4d, + }; +}; +static constexpr unsigned int kMaxPayloadSize = 8; +static constexpr unsigned int kHeaderSize = 3; +static constexpr unsigned int kCrcSize = 1; + +static constexpr unsigned int kOffsetSyncByte = 0; +static constexpr unsigned int kOffsetLengthByte = 1; +static constexpr unsigned int kOffsetMessageIDByte = 2; +static constexpr unsigned int kOffsetPayloadStart = 3; + +static constexpr int kMaxMessageSize = 12 + 1; + +typedef uint8_t MessageCode; + +struct MessageCodes { + enum : MessageCode { + kResponseNoError = 0, + kEventRXSearchTimeout, + kEventRXFail, + kEventTX, + kEventTransferRXFailed, + kEventTransferTXCompleted, + kEventTransferTXFailed, + kEventChannelClosed, + kEventRXFailGoToSearch, + kEventChannelCollision, + kEventTransferTXStart, + kEventTransferNextDataBlock, + kChannelInWrongState, + kChannelNotOpened, + kChannelIDNotSet, + kCloseAllChannels, + kTransferInProgress, + kTransferSequenceNumberError, + kTransferInError, + kMesageSizeExceedsLimit, + kInvalidMessage, + kInvalidNetworkNumber, + kInvalidListID, + kInvalidScanTXChannel, + kInvalidParameterProvided, + kEventSerialQueOverflow, + kEventQueOverflow, + kEncryptNegotiationSuccess, + kEncryptNegotiationFail, + kNVMFullError, + kNVMWriteError, + kUSBStringWriteFail, + kMesgSerialSerrorID, + + }; +}; + +typedef uint8_t CommandType; +struct CommandTypes { + enum : CommandType { + RequestDataPage = 0x01, + RequestANTFSSession = 0x02, + RequestDataPageFromSlave = 0x03, + RequestDataPageSet = 0x04, + }; +}; + +class BroadcastPayload { +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]); + } + std::array raw_data; +}; + +class ChannelResponseMessage { +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]); + } + ChannelNumber channel_id_; + MessageID msg_id_; + MessageCode msg_code_; +}; + +class BatteryStatusMessage { +public: + BatteryStatusMessage(uint8_t _n_batteries, uint8_t _identifier, + uint32_t _operating_time, float _battery_voltage, + uint8_t _status, bool _high_resolution) + : n_batteries(_n_batteries), identifier(_identifier), + operating_time(_operating_time), battery_voltage(_battery_voltage), + status(_status), high_resolution(_high_resolution) { + operating_time = high_resolution ? operating_time * 2 : operating_time * 16; + } + static BatteryStatusMessage FromPayload(std::array _data) { + return BatteryStatusMessage(_data[2] & 0x0f, (_data[2] & 0xf0) >> 4, + _data[3] | (_data[4] << 8) | (_data[5] << 16), + static_cast(_data[7] & 0x0f) + + _data[6] / 256.0, + (_data[7] & 0b1110000) >> 4, _data[7] & 0x80); + } + std::string Status() { + switch (status) { + case 1: + return "new"; + case 2: + return "good"; + case 3: + return "ok"; + case 4: + return "low"; + case 5: + return "critical"; + case 7: + return "invalid"; + default: + return "undefined"; + } + } + uint8_t n_batteries; + uint8_t identifier; + uint32_t operating_time; + float battery_voltage; + uint8_t status; + bool high_resolution; +}; + +class Message { + uint8_t n_batteries; + +public: + Message(); + Message(uint8_t b1, uint8_t b2 = '\0', uint8_t b3 = '\0', uint8_t b4 = '\0', + 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, + uint8_t page_number, + CommandType command_type) { + return Message(9, MessageIDs::kAckData, channel, + CommonPages::kRequestDataPage, kReservedByte, kReservedByte, + descriptor1, descriptor2, requested_transmissions, + page_number, command_type); + } + + static Message CommonRequestCommandStatus(ChannelNumber channel, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, kReservedByte, kReservedByte, requested_transmissions, + CommonPages::kCommandStatus, CommandTypes::RequestDataPage); + } + static Message BicyclePowerRequestPage(ChannelNumber channel, Page subpage, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, subpage, kReservedByte, requested_transmissions, + PowerProfilePages::kGetSetPage, CommandTypes::RequestDataPage); + } + + static Message FitnessEquipmentTargetPower(ChannelNumber channel, + uint16_t power) { + return Message(9, MessageIDs::kAckData, channel, + FitnessEquipmentPages::kTargetPower, kReservedByte, + kReservedByte, kReservedByte, kReservedByte, kReservedByte, + (uint8_t)power & 0xFF, (uint8_t)(power >> 8)); + } + + static Message FitnessEquipmentWindResistance(ChannelNumber channel, + uint8_t wind_resistance_coeff, + uint8_t wind_speed, + uint8_t drafting_factor) { + return Message(9, MessageIDs::kAckData, channel, + FitnessEquipmentPages::kWindResistance, kReservedByte, + kReservedByte, kReservedByte, kReservedByte, + wind_resistance_coeff, wind_speed, drafting_factor); + } + + static Message FitnessEquipmentTrackResistance(ChannelNumber channel, + uint16_t slope, + uint8_t resistance_coeff) { + return Message(9, MessageIDs::kAckData, channel, + FitnessEquipmentPages::kTrackResistance, kReservedByte, + kReservedByte, kReservedByte, kReservedByte, + lower_byte(slope), upper_byte(slope), resistance_coeff); + } + + static Message + FitnessEquipmentRequestCapabilities(ChannelNumber channel, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, kReservedByte, kReservedByte, requested_transmissions, + FitnessEquipmentPages::kCapabilities, CommandTypes::RequestDataPage); + } + + static Message SetParameter(ChannelNumber channel, uint8_t page_number, + uint8_t subpage_number, uint8_t b2, uint8_t b3, + uint8_t b4, uint8_t b5, uint8_t b6, uint8_t b7) { + return Message(9, MessageIDs::kAckData, channel, page_number, + 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; + +private: + void Init(); +}; +} // namespace ant diff --git a/ant/src/ant/message_processor.cpp b/ant/src/ant/message_processor.cpp new file mode 100644 index 0000000..d9f882b --- /dev/null +++ b/ant/src/ant/message_processor.cpp @@ -0,0 +1,145 @@ +#include "message_processor.hpp" +#include "ant.hpp" +#include +#include +#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: + if (_data == kSyncByte) { + // next byte is the message length byte + state_ = State::kGetLength; + // the XOR checksum is initialized with the first byte, i.e. sync byte + checksum_ = kSyncByte; + msg_buffer_.clear(); + msg_buffer_.reserve(kMaxMessageSize); + msg_buffer_.push_back(_data); + } + break; + case State::kGetLength: + if (_data == 0 || _data > 128) { + fprintf(stderr, "Ignoring invalid message length in header: %hu\n", + _data); + // lets start to process data as new message + state_ = State::kWaitForSync; + } else { + msg_buffer_.push_back(_data); + checksum_ ^= _data; + state_ = State::kGetMessageID; + } + break; + case State::kGetMessageID: + msg_buffer_.push_back(_data); + checksum_ ^= _data; + state_ = State::kGetData; + break; + case State::kGetData: + msg_buffer_.push_back(_data); + checksum_ ^= _data; + if (msg_buffer_.size() >= msg_buffer_.at(kOffsetLengthByte) + kHeaderSize) { + // we are done with the payload! + state_ = State::kValidatePacket; + } + break; + case State::kValidatePacket: + // now let's start all over again + state_ = State::kWaitForSync; + // last data byte we receive is the checksum. + // let's check that it is the same as our self computed one. + if (checksum_ == _data) { + ProcessMessage(); + return true; + } + break; + } + return false; +} + +void MessageProcessor::ProcessMessage() { + // printf("Got a message! ID=0x%02x\n", msg_buffer_.at(kOffsetMessageIDByte)); + uint8_t byte = msg_buffer_.at(kOffsetPayloadStart); + MessageID id = msg_buffer_.at(kOffsetMessageIDByte); + + switch (id) { + case MessageIDs::kStartupMessage: + if (byte == 0) { + printf("POWER_ON_RESET\n"); + } else if (byte == (1 << 0)) { + printf("HARDWARE_RESET_LINE\n"); + } else if (byte == (1 << 1)) { + printf("WATCHDOG_RESET\n"); + } else if (byte == (1 << 5)) { + printf("COMMAND_RESET\n"); + } else if (byte == (1 << 6)) { + printf("SYNCHRONOUS_RESET\n"); + } else if (byte == (1 << 7)) { + printf("SUSPEND_RESET\n"); + } else { + printf("??\n"); + } + break; + case MessageIDs::kSerialErrorMessage: + printf("Errno from ANT: %hu\n", byte); + printf("Wrong message: "); + for (int i = 0; i < msg_buffer_.at(kOffsetLengthByte); ++i) { + printf("%02x, ", msg_buffer_.at(i + kOffsetPayloadStart)); + } + printf("\n"); + break; + case MessageIDs::kChannelResponse: { + printf("Channel response\n"); + std::array payload; + typedef decltype(msg_buffer_)::const_iterator iterator; + iterator start = msg_buffer_.begin() + kOffsetPayloadStart; + 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); + } + break; + } + case MessageIDs::kChannelID: { + // 0: channel number + // 1-2: device number + // 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); + printf("----------\nDevice Number: 0x%04x\n----------\n", device_number); + if (device_number_cb_) { + device_number_cb_(channel, device_number); + } + break; + } + case MessageIDs::kBroadcastData: { + // 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); + } + break; + } + default: + printf("RECEIVED MESSAGE with id: 0x%02x\n", + msg_buffer_.at(kOffsetMessageIDByte)); + break; + } +} +} // namespace ant diff --git a/ant/src/ant/message_processor.hpp b/ant/src/ant/message_processor.hpp new file mode 100644 index 0000000..fcc0bd9 --- /dev/null +++ b/ant/src/ant/message_processor.hpp @@ -0,0 +1,43 @@ +#pragma once +#include "message.hpp" +#include +#include +#include +#include + +namespace ant { + +typedef std::function DeviceNumberCallback; +typedef std::function ChannelResponseCallback; +typedef std::function BroadcastCallback; + +class MessageProcessor { +public: + MessageProcessor(); + enum class State { + kWaitForSync = 0, + kGetLength, + kGetMessageID, + kGetData, + kValidatePacket, + }; + 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; + } + +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_controller.cpp b/ant/src/ant_controller.cpp new file mode 100644 index 0000000..03a3303 --- /dev/null +++ b/ant/src/ant_controller.cpp @@ -0,0 +1,72 @@ +#include "ant_controller.hpp" + +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_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"); + + + 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); +} + +AntController::AntController() {} + +void AntController::configure_heart_rate_channel( + ant::ChannelNumber _channel, ant::DeviceNumber _device_number) { + hr_channel_ = std::make_unique(*ant_device_, _channel, + _device_number); + + hr_channel_->SetOnHeartRateCallback([this](auto heart_rate) { + emit_signal("heart_rate_updated", static_cast(heart_rate)); + }); +} + +void AntController::configure_fitness_equipment_channel( + ant::ChannelNumber _channel, ant::DeviceNumber _device_number) { + fe_channel_ = std::make_unique( + *ant_device_, _channel, _device_number); + + 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::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(); +} + +void AntController::parse_messages() { ant_device_->ReceiveMessage(); } + +AntController::~AntController() {} diff --git a/ant/src/ant_controller.hpp b/ant/src/ant_controller.hpp new file mode 100644 index 0000000..e4572f3 --- /dev/null +++ b/ant/src/ant_controller.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "ant/ant_device.hpp" +#include "ant/channels.hpp" +#include "ant/message.hpp" +#include +#include + +namespace godot { +class AntController : public Node { + GDCLASS(AntController, Node) +public: + AntController(); + ~AntController(); + void on_process_ant_messages(); + void connect_ant_channels(); + void parse_messages(); + void configure_heart_rate_channel(ant::ChannelNumber, ant::DeviceNumber); + void configure_fitness_equipment_channel(ant::ChannelNumber, + ant::DeviceNumber); + void configure_power_channel(ant::ChannelNumber, ant::DeviceNumber); + + void set_enable_fitness_equipment(const bool value) { + enable_fitness_equipment_ = value; + } + 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_; } + +protected: + static void _bind_methods(); + +private: + bool enable_fitness_equipment_{false}; + bool enable_heart_rate_{false}; + + std::unique_ptr ant_device_; + std::unique_ptr hr_channel_; + std::unique_ptr fe_channel_; + std::unique_ptr power_channel_; + double time_passed_; +}; +} // namespace godot diff --git a/ant/src/main.cpp b/ant/src/main.cpp new file mode 100644 index 0000000..25d79b6 --- /dev/null +++ b/ant/src/main.cpp @@ -0,0 +1,51 @@ + +#include "ant/ant_device.hpp" +#include "ant/channels.hpp" +#include "ant/message.hpp" +#include + +int main() { + ant::ANTDevice ant_device; + ant_device.Init(); + + // ant::PowerChannel power_channel(ant_device, 2, + // ant::DeviceNumbers::Wildcard); + ant::HeartRateChannel hr_channel(ant_device, 3, ant::DeviceNumbers::Wildcard); + ant::FitnessEquipmentChannel fe_channel(ant_device, 4, + ant::DeviceNumbers::Wildcard); + + // ant_device.processor.SetOnChannelResponseCallback( + // power_channel.channel_id(), + // [&power_channel](ant::ChannelResponseMessage msg) { + // power_channel.OnChannelResponse(msg); + // }); + // ant_device.processor.SetOnBroadcastDataCallback( + // power_channel.channel_id(), [&power_channel](ant::BroadcastPayload + // data) { + // power_channel.OnBroadcastData(data); + // }); + // power_channel.StartSearch(); + ant_device.processor.SetOnChannelResponseCallback( + hr_channel.channel_id(), [&hr_channel](ant::ChannelResponseMessage msg) { + hr_channel.OnChannelResponse(msg); + }); + ant_device.processor.SetOnBroadcastDataCallback( + hr_channel.channel_id(), [&hr_channel](ant::BroadcastPayload data) { + hr_channel.OnBroadcastData(data); + }); + hr_channel.StartSearch(); + ant_device.processor.SetOnBroadcastDataCallback( + fe_channel.channel_id(), [&fe_channel](ant::BroadcastPayload data) { + fe_channel.OnBroadcastData(data); + }); + + ant_device.processor.SetOnChannelResponseCallback( + fe_channel.channel_id(), [&fe_channel](ant::ChannelResponseMessage msg) { + fe_channel.OnChannelResponse(msg); + }); + // fe_channel.StartSearch(); + while (true) { + ant_device.ReceiveMessage(); + } + return 0; +} diff --git a/ant/src/register_types.cpp b/ant/src/register_types.cpp new file mode 100644 index 0000000..9d021ba --- /dev/null +++ b/ant/src/register_types.cpp @@ -0,0 +1,36 @@ +#include "register_types.hpp" + +#include "ant_controller.hpp" + +#include +#include +#include + +using namespace godot; + +void initialize_ant_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { + return; + } + + GDREGISTER_RUNTIME_CLASS(AntController); +} + +void uninitialize_ant_module(ModuleInitializationLevel p_level) { + if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) { + return; + } +} + +extern "C" { +// Initialization. +GDExtensionBool GDE_EXPORT ant_library_init(GDExtensionInterfaceGetProcAddress p_get_proc_address, const GDExtensionClassLibraryPtr p_library, GDExtensionInitialization *r_initialization) { + godot::GDExtensionBinding::InitObject init_obj(p_get_proc_address, p_library, r_initialization); + + init_obj.register_initializer(initialize_ant_module); + init_obj.register_terminator(uninitialize_ant_module); + init_obj.set_minimum_library_initialization_level(MODULE_INITIALIZATION_LEVEL_SCENE); + + return init_obj.init(); +} +} diff --git a/ant/src/register_types.hpp b/ant/src/register_types.hpp new file mode 100644 index 0000000..f1edc4b --- /dev/null +++ b/ant/src/register_types.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include + +using namespace godot; + +void initialize_ant_module(ModuleInitializationLevel p_level); +void uninitialize_ant_module(ModuleInitializationLevel p_level); diff --git a/demo/.editorconfig b/demo/.editorconfig new file mode 100644 index 0000000..f28239b --- /dev/null +++ b/demo/.editorconfig @@ -0,0 +1,4 @@ +root = true + +[*] +charset = utf-8 diff --git a/demo/.gitignore b/demo/.gitignore new file mode 100644 index 0000000..bb71180 --- /dev/null +++ b/demo/.gitignore @@ -0,0 +1,15 @@ +# Godot 4+ specific ignores +.godot/ + +# Godot-specific ignores +.import/ +export.cfg +export_credentials.cfg + +# Imported translations (automatically generated from CSV files) +*.translation + +# Mono-specific ignores +.mono/ +data_*/ +mono_crash.*.json diff --git a/demo/bin/ant.gdextension b/demo/bin/ant.gdextension new file mode 100644 index 0000000..4b5eba7 --- /dev/null +++ b/demo/bin/ant.gdextension @@ -0,0 +1,7 @@ +[configuration] +entry_symbol = "ant_library_init" +compatibility_minimum = "4.4" + +[libraries] +linux.debug.x86_64 = "res://bin/libant.so" +linux.release.x86_64 = "res://bin/libant.so" diff --git a/demo/bin/ant.gdextension.uid b/demo/bin/ant.gdextension.uid new file mode 100644 index 0000000..3f11f7e --- /dev/null +++ b/demo/bin/ant.gdextension.uid @@ -0,0 +1 @@ +uid://cs56m8cu8a7ql diff --git a/demo/bin/libant.so b/demo/bin/libant.so new file mode 100755 index 0000000..230d3c7 Binary files /dev/null and b/demo/bin/libant.so differ diff --git a/demo/icon.svg b/demo/icon.svg new file mode 100644 index 0000000..9d8b7fa --- /dev/null +++ b/demo/icon.svg @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/demo/icon.svg.import b/demo/icon.svg.import new file mode 100644 index 0000000..62338a0 --- /dev/null +++ b/demo/icon.svg.import @@ -0,0 +1,37 @@ +[remap] + +importer="texture" +type="CompressedTexture2D" +uid="uid://kr6jnlma2anb" +path="res://.godot/imported/icon.svg-218a8f2b3041327d8a5756f3a245f83b.ctex" +metadata={ +"vram_texture": false +} + +[deps] + +source_file="res://icon.svg" +dest_files=["res://.godot/imported/icon.svg-218a8f2b3041327d8a5756f3a245f83b.ctex"] + +[params] + +compress/mode=0 +compress/high_quality=false +compress/lossy_quality=0.7 +compress/hdr_compression=1 +compress/normal_map=0 +compress/channel_pack=0 +mipmaps/generate=false +mipmaps/limit=-1 +roughness/mode=0 +roughness/src_normal="" +process/fix_alpha_border=true +process/premult_alpha=false +process/normal_map_invert_y=false +process/hdr_as_srgb=false +process/hdr_clamp_exposure=false +process/size_limit=0 +detect_3d/compress_to=1 +svg/scale=1.0 +editor/scale_with_editor_scale=false +editor/convert_colors_with_editor_theme=false diff --git a/demo/node_3d.gd b/demo/node_3d.gd new file mode 100644 index 0000000..0e30365 --- /dev/null +++ b/demo/node_3d.gd @@ -0,0 +1,27 @@ +extends Node3D + +@onready var ant_controller = $AntController +@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() + 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 diff --git a/demo/node_3d.gd.uid b/demo/node_3d.gd.uid new file mode 100644 index 0000000..78e2047 --- /dev/null +++ b/demo/node_3d.gd.uid @@ -0,0 +1 @@ +uid://cuyhf18taa5if diff --git a/demo/node_3d.tscn b/demo/node_3d.tscn new file mode 100644 index 0000000..66a7f53 --- /dev/null +++ b/demo/node_3d.tscn @@ -0,0 +1,41 @@ +[gd_scene load_steps=2 format=3 uid="uid://cl7rfid2g3c4d"] + +[ext_resource type="Script" uid="uid://cuyhf18taa5if" path="res://node_3d.gd" id="1_a202f"] + +[node name="Node3D" type="Node3D"] +script = ExtResource("1_a202f") + +[node name="AntController" type="AntController" parent="."] + +[node name="PanelContainer" type="BoxContainer" parent="."] +offset_right = 40.0 +offset_bottom = 40.0 + +[node name="hr_container" type="PanelContainer" parent="PanelContainer"] +layout_mode = 2 + +[node name="VBoxContainer" type="VBoxContainer" parent="PanelContainer/hr_container"] +layout_mode = 2 + +[node name="title_label" type="Label" parent="PanelContainer/hr_container/VBoxContainer"] +layout_mode = 2 +text = "Hear Rate" + +[node name="value_label" type="Label" parent="PanelContainer/hr_container/VBoxContainer"] +layout_mode = 2 + +[node name="pwr_container" type="PanelContainer" parent="PanelContainer"] +layout_mode = 2 + +[node name="VBoxContainer" type="VBoxContainer" parent="PanelContainer/pwr_container"] +layout_mode = 2 + +[node name="title_label" type="Label" parent="PanelContainer/pwr_container/VBoxContainer"] +layout_mode = 2 +text = "Pwr" + +[node name="value_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"] diff --git a/demo/project.godot b/demo/project.godot new file mode 100644 index 0000000..8edeb92 --- /dev/null +++ b/demo/project.godot @@ -0,0 +1,16 @@ +; Engine configuration file. +; It's best edited using the editor UI and not directly, +; since the parameters that go here are not all obvious. +; +; Format: +; [section] ; section goes between [] +; param=value ; assign values to parameters + +config_version=5 + +[application] + +config/name="demo" +run/main_scene="uid://cl7rfid2g3c4d" +config/features=PackedStringArray("4.4", "Forward Plus") +config/icon="res://icon.svg"