From afbad8227c13c51004d54697c57d4530cb584692 Mon Sep 17 00:00:00 2001 From: Thies Lennart Alff Date: Wed, 2 Apr 2025 11:34:30 +0200 Subject: [PATCH] initial commit --- .gitignore | 2 + CMakeLists.txt | 22 ++ src/ant/ant.hpp | 116 +++++++ src/ant/ant_device.cpp | 119 +++++++ src/ant/ant_device.hpp | 40 +++ src/ant/bicycle_power.hpp | 95 ++++++ src/ant/channel/channel.cpp | 124 +++++++ src/ant/channel/channel.hpp | 35 ++ src/ant/channel/fitness_equipment_channel.cpp | 124 +++++++ src/ant/channel/fitness_equipment_channel.hpp | 36 ++ src/ant/channel/heart_rate_channel.cpp | 28 ++ src/ant/channel/heart_rate_channel.hpp | 13 + src/ant/channel/power_channel.cpp | 80 +++++ src/ant/channel/power_channel.hpp | 45 +++ src/ant/channels.hpp | 6 + src/ant/common.hpp | 84 +++++ src/ant/fitness_equipment.hpp | 63 ++++ src/ant/message.cpp | 33 ++ src/ant/message.hpp | 321 ++++++++++++++++++ src/ant/message_processor.cpp | 138 ++++++++ src/ant/message_processor.hpp | 37 ++ src/main.cpp | 51 +++ 22 files changed, 1612 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 src/ant/ant.hpp create mode 100644 src/ant/ant_device.cpp create mode 100644 src/ant/ant_device.hpp create mode 100644 src/ant/bicycle_power.hpp create mode 100644 src/ant/channel/channel.cpp create mode 100644 src/ant/channel/channel.hpp create mode 100644 src/ant/channel/fitness_equipment_channel.cpp create mode 100644 src/ant/channel/fitness_equipment_channel.hpp create mode 100644 src/ant/channel/heart_rate_channel.cpp create mode 100644 src/ant/channel/heart_rate_channel.hpp create mode 100644 src/ant/channel/power_channel.cpp create mode 100644 src/ant/channel/power_channel.hpp create mode 100644 src/ant/channels.hpp create mode 100644 src/ant/common.hpp create mode 100644 src/ant/fitness_equipment.hpp create mode 100644 src/ant/message.cpp create mode 100644 src/ant/message.hpp create mode 100644 src/ant/message_processor.cpp create mode 100644 src/ant/message_processor.hpp create mode 100644 src/main.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a4fb4fb --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +.cache/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..a388053 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.5) +project(ant) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_BUILD_TYPE DEBUG) +set(CMAKE_CXX_FLAGS_DEBUG "-g -O0 -Wall -Wextra -Wswitch-enum") + +get_cmake_property(_variableNames VARIABLES) +list (SORT _variableNames) +foreach (_variableName ${_variableNames}) + message(STATUS "${_variableName}=${${_variableName}}") +endforeach() + +add_executable(test src/main.cpp + src/ant/ant_device.cpp + src/ant/channel/channel.cpp + src/ant/channel/heart_rate_channel.cpp + src/ant/channel/power_channel.cpp + src/ant/channel/fitness_equipment_channel.cpp + src/ant/message.cpp + src/ant/message_processor.cpp +) diff --git a/src/ant/ant.hpp b/src/ant/ant.hpp new file mode 100644 index 0000000..e091dcd --- /dev/null +++ b/src/ant/ant.hpp @@ -0,0 +1,116 @@ +#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 { + kBikeSepcificData = 0x19, + kBasicResistance = 0x30, + kTargetPower, + kWindResistance, + kTrackResistance, + kCapabilities = 0x36, + }; +}; + +struct PowerProfilePages { + enum : Page { + kCalibrationMessage = 0x01, + kGetSetPage = 0x02, + kMeasurement = 0x03, + kStandardPower = 0x10, + kStandardTorqueAtWheel = 0x11, + kStandardTorqueAtCrnak = 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 ChannelID; +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/src/ant/ant_device.cpp b/src/ant/ant_device.cpp new file mode 100644 index 0000000..eae062a --- /dev/null +++ b/src/ant/ant_device.cpp @@ -0,0 +1,119 @@ +#include "ant_device.hpp" +#include +#include +#include +namespace ant { + +ANTDevice::ANTDevice() {} + +void ANTDevice::Init() { + if (!Open()) { + return; + } + uint8_t buf[300]; + printf("Sending a reset command!\n"); + sleep(1); + SendMessage(Message::SystemReset()); + sleep(1); + uint8_t network = 1; + SendMessage(Message::SetNetworkKey(network, key)); + while (ReceiveMessage()) + ; +} + +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; +} + +int ANTDevice::AddDevice(int device_number, int device_type, + int channel_number) { + for (int i = 0; i < 8; ++i) { + } +} + +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; +} + +void ANTDevice::SendMessage(Message msg) { 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) { + fprintf(stderr, "Wanted to write %i bytes but have written %i 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/src/ant/ant_device.hpp b/src/ant/ant_device.hpp new file mode 100644 index 0000000..ad3f7d6 --- /dev/null +++ b/src/ant/ant_device.hpp @@ -0,0 +1,40 @@ +#pragma once +#include "message.hpp" +#include "message_processor.hpp" +#include + +#include +#include +#include +#include +#include +#include +#include +#include "ant.hpp" + +#define GARMIN_USB2_VENDOR_ID 0x0fcf +#define GARMIN_USB2_PRODUCT_ID 0x1008 +namespace ant { +class ANTDevice { +public: + ANTDevice(); + int Read(uint8_t *buf, int bytes, int timeout_ms = 125); + int Write(uint8_t *bytes, int size, int timeout = 125); + void SendMessage(Message); + int AddDevice(int device_number, int device_type, int channel_number); + bool ReceiveMessage(); + bool Open(); + bool Close(); + void 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/src/ant/bicycle_power.hpp b/src/ant/bicycle_power.hpp new file mode 100644 index 0000000..36de00c --- /dev/null +++ b/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/src/ant/channel/channel.cpp b/src/ant/channel/channel.cpp new file mode 100644 index 0000000..1cf9ff4 --- /dev/null +++ b/src/ant/channel/channel.cpp @@ -0,0 +1,124 @@ +#include "channel.hpp" +#include +namespace ant { + +Channel::Channel(ANTDevice &_ant_device, ChannelID _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::StartSearch() { + SetChannelState(ChannelState::kSearching); + ant_device_.SendMessage(Message::AssignChannel(id_, channel_config_.type, 1)); +} + +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) { + printf( + "[channel %hu] Connected and received channel response: 0x%02x %hu\n", + id_, msg.msg_id_, msg.msg_code_); + 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 1: + 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/src/ant/channel/channel.hpp b/src/ant/channel/channel.hpp new file mode 100644 index 0000000..9a1c58e --- /dev/null +++ b/src/ant/channel/channel.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include "../ant.hpp" +#include "../ant_device.hpp" + +namespace ant { + +enum class ChannelState { + kClosed, + kOpening, + kSearching, + kConnected, + kUndefined, +}; + +class Channel { +public: + Channel(ANTDevice &, ChannelID, DeviceNumber); + void StartSearch(); + void OnSearchTimeout(); + virtual void OnBroadcastData(BroadcastPayload) = 0; + void OnChannelResponse(ChannelResponseMessage); + ChannelID channel_id() const { return id_; } + +protected: + void SetChannelState(ChannelState _state) { state_ = _state; } + + ChannelConfig channel_config_; + ANTDevice ant_device_; + ::ant::ChannelID id_; + ChannelState state_{ChannelState::kUndefined}; + bool connected_{false}; +}; + +} // namespace ant diff --git a/src/ant/channel/fitness_equipment_channel.cpp b/src/ant/channel/fitness_equipment_channel.cpp new file mode 100644 index 0000000..e17d410 --- /dev/null +++ b/src/ant/channel/fitness_equipment_channel.cpp @@ -0,0 +1,124 @@ +#include "fitness_equipment_channel.hpp" +#include "../common.hpp" +#include "../fitness_equipment.hpp" + +namespace ant { + +FitnessEquipmentChannel::FitnessEquipmentChannel(ANTDevice &_ant_device, + ChannelID _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) { + static uint8_t counter = 0; + if (connected_) { + if (!counter) { + SetTargetPower(100); + } + counter++; + } + if (!connected_) { + printf("Received first broadcast data!\n"); + connected_ = true; + SetChannelState(ChannelState::kConnected); + ant_device_.SendMessage(Message::RequestChannelID(id_)); + SetTargetPower(100); + } + uint8_t page_number = payload.raw_data[0]; + switch (page_number) { + 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(); + 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); + 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/src/ant/channel/fitness_equipment_channel.hpp b/src/ant/channel/fitness_equipment_channel.hpp new file mode 100644 index 0000000..2143dd3 --- /dev/null +++ b/src/ant/channel/fitness_equipment_channel.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "../common.hpp" +#include "../fitness_equipment.hpp" +#include "channel.hpp" + +namespace ant { + +class FitnessEquipmentChannel : public Channel { +public: + FitnessEquipmentChannel(ANTDevice &, ChannelID, DeviceNumber); + void OnBroadcastData(BroadcastPayload) final; + + void SetTargetPower(uint16_t power) { + ant_device_.SendMessage( + Message::FitnessEquipmentTargetPower(id_, power * 4)); + } + + void RequestCapabilies(uint8_t requested_transmissions) { + ant_device_.SendMessage(Message::FitnessEquipmentRequestCapabilities( + id_, requested_transmissions)); + } + void RequestCommandStatus(uint8_t requested_transmissions) { + ant_device_.SendMessage( + Message::CommonRequestCommandStatus(id_, requested_transmissions)); + } + + void PrintCommandStatus(const common::CommandStatusPage &); + void PrintCapabilities(const fitness_equipment::Capabilities &); + +private: + uint8_t bike_data_event_count_{0}; + uint16_t bike_data_accumulated_power_{0}; +}; + +} // namespace ant diff --git a/src/ant/channel/heart_rate_channel.cpp b/src/ant/channel/heart_rate_channel.cpp new file mode 100644 index 0000000..0ea8dd1 --- /dev/null +++ b/src/ant/channel/heart_rate_channel.cpp @@ -0,0 +1,28 @@ +#include "heart_rate_channel.hpp" + +namespace ant { + +HeartRateChannel::HeartRateChannel(ANTDevice &_ant_device, + ChannelID _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 (!connected_) { + printf("Received first broadcast data!\n"); + connected_ = true; + 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]; + printf("Heart Rate: %hu\n", heart_rate); +} + +} // namespace ant diff --git a/src/ant/channel/heart_rate_channel.hpp b/src/ant/channel/heart_rate_channel.hpp new file mode 100644 index 0000000..3fc86f2 --- /dev/null +++ b/src/ant/channel/heart_rate_channel.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include "channel.hpp" + +namespace ant { + +class HeartRateChannel : public Channel { +public: + HeartRateChannel(ANTDevice &, ChannelID, DeviceNumber); + void OnBroadcastData(BroadcastPayload) final; +}; + +} // namespace ant diff --git a/src/ant/channel/power_channel.cpp b/src/ant/channel/power_channel.cpp new file mode 100644 index 0000000..f2a238b --- /dev/null +++ b/src/ant/channel/power_channel.cpp @@ -0,0 +1,80 @@ +#include "power_channel.hpp" + +namespace ant { + +PowerChannel::PowerChannel(ANTDevice &_ant_device, ChannelID 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 (!connected_) { + // First data received from master + printf("Received first broadcast data!\n"); + connected_ = true; + SetChannelState(ChannelState::kConnected); + ant_device_.SendMessage(Message::RequestChannelID(id_)); + bicycle_power::AdvancedCapabilities2 caps2; + // caps2.EnableAllDynamics(); + caps2.Enable8Hz(); + // caps2.UnmaskAllDynamics(); + 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) { + case PowerProfilePages::kBattery: + // BatteryStatusMessage msg(BatteryStatusMessage::FromPayload(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::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/src/ant/channel/power_channel.hpp b/src/ant/channel/power_channel.hpp new file mode 100644 index 0000000..22bcd27 --- /dev/null +++ b/src/ant/channel/power_channel.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include "channel.hpp" +#include "../bicycle_power.hpp" + +namespace ant { + +class PowerChannel : public Channel { +public: + PowerChannel(ANTDevice &, ChannelID, DeviceNumber); + void OnBroadcastData(BroadcastPayload) final; + // type specific + void RequestCrankParameters(uint8_t n_transmissions = 10) { + ant_device_.SendMessage(Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kCrankParameters, n_transmissions)); + } + void RequestAdvancedCapabilities1(uint8_t n_transmissions = 10) { + ant_device_.SendMessage(Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kAdvancedCapabilities, n_transmissions)); + } + void RequestAdvancedCapabilities2(uint8_t n_tramsissions = 10) { + ant_device_.SendMessage(Message::BicyclePowerRequestPage( + id_, PowerProfileSubpages::kAdvancedCapabilities2, n_tramsissions)); + } + void Set8Hz(bool enabled) { + ant_device_.SendMessage(Message::SetParameter(id_, 0x02, 0xfe, 0xff, 0xff, + ~(1 << 1), 0xff, + 0xff * (1 - enabled), 0xff)); + } + void SetAdvancedCapabilities(bicycle_power::AdvancedCapabilities caps) { + ant_device_.SendMessage( + Message::SetParameter(id_, PowerProfilePages::kGetSetPage, + PowerProfileSubpages::kAdvancedCapabilities, + kReservedByte, kReservedByte, caps.Mask(), + kReservedByte, caps.Value(), kReservedByte)); + } + void SetAdvancedCapabilities2(bicycle_power::AdvancedCapabilities2 caps) { + ant_device_.SendMessage( + Message::SetParameter(id_, PowerProfilePages::kGetSetPage, + PowerProfileSubpages::kAdvancedCapabilities2, + kReservedByte, kReservedByte, caps.Mask(), + kReservedByte, caps.Value(), kReservedByte)); + } +}; +} // namespace ant diff --git a/src/ant/channels.hpp b/src/ant/channels.hpp new file mode 100644 index 0000000..ae1e5ce --- /dev/null +++ b/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/src/ant/common.hpp b/src/ant/common.hpp new file mode 100644 index 0000000..818bd38 --- /dev/null +++ b/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/src/ant/fitness_equipment.hpp b/src/ant/fitness_equipment.hpp new file mode 100644 index 0000000..2d8e712 --- /dev/null +++ b/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/src/ant/message.cpp b/src/ant/message.cpp new file mode 100644 index 0000000..1960615 --- /dev/null +++ b/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/src/ant/message.hpp b/src/ant/message.hpp new file mode 100644 index 0000000..7bba27e --- /dev/null +++ b/src/ant/message.hpp @@ -0,0 +1,321 @@ +#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 { + +typedef uint8_t MessageID; +struct MessageIDs { + enum : MessageID { + 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 int kMaxPayloadSize = 8; +static constexpr int kHeaderSize = 3; +static constexpr int kCrcSize = 1; + +static constexpr int kOffsetSyncByte = 0; +static constexpr int kOffsetLengthByte = 1; +static constexpr int kOffsetMessageIDByte = 2; +static constexpr 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(ChannelID 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]); + } + ChannelID 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(ChannelID channel) { + return Message(1, ANT_UNASSIGN_CHANNEL, channel); + } + static Message AssignChannel(ChannelID channel, ChannelType type, + uint8_t network) { + return Message(3, ANT_ASSIGN_CHANNEL, channel, static_cast(type), + network); + } + + static Message SetChannelPeriod(ChannelID 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(ChannelID channel) { + return Message(2, MessageIDs::kRequestMessage, channel, + MessageIDs::kChannelID); + } + static Message CommonRequestPage(ChannelID 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(ChannelID channel, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, kReservedByte, kReservedByte, requested_transmissions, + CommonPages::kCommandStatus, CommandTypes::RequestDataPage); + } + static Message BicyclePowerRequestPage(ChannelID channel, Page subpage, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, subpage, kReservedByte, requested_transmissions, + PowerProfilePages::kGetSetPage, CommandTypes::RequestDataPage); + } + + static Message FitnessEquipmentTargetPower(ChannelID 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 + FitnessEquipmentRequestCapabilities(ChannelID channel, + uint8_t requested_transmissions) { + return CommonRequestPage( + channel, kReservedByte, kReservedByte, requested_transmissions, + FitnessEquipmentPages::kCapabilities, CommandTypes::RequestDataPage); + } + + static Message SetParameter(ChannelID 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(ChannelID 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(ChannelID channel, uint8_t timeout) { + return Message(2, ANT_LP_SEARCH_TIMEOUT, channel, timeout); + } + + static Message OpenChannel(ChannelID channel) { + return Message(1, ANT_OPEN_CHANNEL, channel); + } + uint8_t data[kMaxMessageSize]; + uint8_t data_length; + +private: + void Init(); +}; +} // namespace ant diff --git a/src/ant/message_processor.cpp b/src/ant/message_processor.cpp new file mode 100644 index 0000000..106ae46 --- /dev/null +++ b/src/ant/message_processor.cpp @@ -0,0 +1,138 @@ +#include "message_processor.hpp" +#include "ant.hpp" +#include +#include +#include + +namespace ant { +MessageProcessor::MessageProcessor() {} +void MessageProcessor::SetOnChannelResponseCallback( + ChannelID _channel_id, + std::function _callback) { + channel_response_cbs_[_channel_id] = _callback; +} +void MessageProcessor::SetOnBroadcastDataCallback( + ChannelID _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: { + 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: { + uint16_t device_number = (msg_buffer_.at(kOffsetPayloadStart + 1)) | + (msg_buffer_.at(kOffsetPayloadStart + 2) << 8); + printf("----------\nDevice Number: 0x%04x\n----------\n", device_number); + printf("%hu, %hu\n", msg_buffer_.at(kOffsetPayloadStart + 1), + msg_buffer_.at(kOffsetPayloadStart + 2)); + break; + } + case MessageIDs::kBroadcastData: { + //printf("Broadcast page: 0x%02x\n", msg_buffer_.at(kOffsetPayloadStart + 1)); + ChannelID 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/src/ant/message_processor.hpp b/src/ant/message_processor.hpp new file mode 100644 index 0000000..209e595 --- /dev/null +++ b/src/ant/message_processor.hpp @@ -0,0 +1,37 @@ +#pragma once +#include "message.hpp" +#include +#include +#include +#include + +namespace ant { + +class MessageProcessor { +public: + MessageProcessor(); + enum class State { + kWaitForSync = 0, + kGetLength, + kGetMessageID, + kGetData, + kValidatePacket, + }; + bool FeedData(uint8_t data); + void ProcessMessage(); + void SetOnChannelResponseCallback( + ChannelID channel_id, + std::function callback); + void SetOnBroadcastDataCallback( + ChannelID channel_id, + std::function callback); + +private: + State state_{State::kWaitForSync}; + uint8_t checksum_; + std::vector msg_buffer_; + std::map> channel_response_cbs_; + std::map> broadcast_data_cbs_; +}; + +} // namespace ant diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..406e5b1 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,51 @@ + +#include "ant/ant_device.hpp" +#include "ant/channels.hpp" +#include "ant/message.hpp" + +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; +}