initial commit
This commit is contained in:
commit
d118b4fbb9
GPG key ID:
4EC67D34D594104D
38 changed files with 2173 additions and 0 deletions
3
.gitignore
vendored
Normal file
3
.gitignore
vendored
Normal file
|
|
@ -0,0 +1,3 @@
|
|||
.cache/
|
||||
build/
|
||||
.godot/
|
||||
19
CMakeLists.txt
Normal file
19
CMakeLists.txt
Normal file
|
|
@ -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)
|
||||
30
ant/CMakeLists.txt
Normal file
30
ant/CMakeLists.txt
Normal file
|
|
@ -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)
|
||||
121
ant/src/ant/ant.hpp
Normal file
121
ant/src/ant/ant.hpp
Normal file
|
|
@ -0,0 +1,121 @@
|
|||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
115
ant/src/ant/ant_device.cpp
Normal file
115
ant/src/ant/ant_device.cpp
Normal file
|
|
@ -0,0 +1,115 @@
|
|||
#include "ant_device.hpp"
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <sys/select.h>
|
||||
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
|
||||
40
ant/src/ant/ant_device.hpp
Normal file
40
ant/src/ant/ant_device.hpp
Normal file
|
|
@ -0,0 +1,40 @@
|
|||
#pragma once
|
||||
#include "message.hpp"
|
||||
#include "message_processor.hpp"
|
||||
#include <vector>
|
||||
|
||||
#include "ant.hpp"
|
||||
#include <cerrno>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#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
|
||||
95
ant/src/ant/bicycle_power.hpp
Normal file
95
ant/src/ant/bicycle_power.hpp
Normal file
|
|
@ -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
|
||||
198
ant/src/ant/channel/channel.cpp
Normal file
198
ant/src/ant/channel/channel.cpp
Normal file
|
|
@ -0,0 +1,198 @@
|
|||
#include "channel.hpp"
|
||||
#include <cassert>
|
||||
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<std::mutex> 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<std::mutex> 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<std::mutex> 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
|
||||
75
ant/src/ant/channel/channel.hpp
Normal file
75
ant/src/ant/channel/channel.hpp
Normal file
|
|
@ -0,0 +1,75 @@
|
|||
#pragma once
|
||||
|
||||
#include "../ant.hpp"
|
||||
#include "../ant_device.hpp"
|
||||
#include <mutex>
|
||||
#include <queue>
|
||||
|
||||
namespace ant {
|
||||
|
||||
enum class ChannelState {
|
||||
kClosed,
|
||||
kOpening,
|
||||
kSearching,
|
||||
kConnected,
|
||||
kUndefined,
|
||||
};
|
||||
|
||||
template <std::size_t n> 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<Message> 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
|
||||
138
ant/src/ant/channel/fitness_equipment_channel.cpp
Normal file
138
ant/src/ant/channel/fitness_equipment_channel.cpp
Normal file
|
|
@ -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
|
||||
69
ant/src/ant/channel/fitness_equipment_channel.hpp
Normal file
69
ant/src/ant/channel/fitness_equipment_channel.hpp
Normal file
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
|
||||
#include "../common.hpp"
|
||||
#include "../fitness_equipment.hpp"
|
||||
#include "channel.hpp"
|
||||
#include <algorithm>
|
||||
|
||||
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<uint8_t>((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<uint8_t>(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<void(uint16_t)> _cb) { power_cb_ = _cb; }
|
||||
void SetCadenceCallback(std::function<void(uint8_t)> _cb) {
|
||||
cadence_cb_ = _cb;
|
||||
}
|
||||
|
||||
private:
|
||||
uint8_t bike_data_event_count_{0};
|
||||
uint16_t bike_data_accumulated_power_{0};
|
||||
std::function<void(uint16_t)> power_cb_{nullptr};
|
||||
std::function<void(uint8_t)> cadence_cb_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace ant
|
||||
30
ant/src/ant/channel/heart_rate_channel.cpp
Normal file
30
ant/src/ant/channel/heart_rate_channel.cpp
Normal file
|
|
@ -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
|
||||
20
ant/src/ant/channel/heart_rate_channel.hpp
Normal file
20
ant/src/ant/channel/heart_rate_channel.hpp
Normal file
|
|
@ -0,0 +1,20 @@
|
|||
#pragma once
|
||||
|
||||
#include "channel.hpp"
|
||||
|
||||
namespace ant {
|
||||
|
||||
typedef std::function<void(uint8_t)> 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
|
||||
86
ant/src/ant/channel/power_channel.cpp
Normal file
86
ant/src/ant/channel/power_channel.cpp
Normal file
|
|
@ -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
|
||||
49
ant/src/ant/channel/power_channel.hpp
Normal file
49
ant/src/ant/channel/power_channel.hpp
Normal file
|
|
@ -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
|
||||
6
ant/src/ant/channels.hpp
Normal file
6
ant/src/ant/channels.hpp
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
#include "channel/heart_rate_channel.hpp"
|
||||
#include "channel/power_channel.hpp"
|
||||
#include "channel/fitness_equipment_channel.hpp"
|
||||
|
||||
84
ant/src/ant/common.hpp
Normal file
84
ant/src/ant/common.hpp
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
#pragma once
|
||||
#include "message.hpp"
|
||||
#include <stdint.h>
|
||||
|
||||
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
|
||||
63
ant/src/ant/fitness_equipment.hpp
Normal file
63
ant/src/ant/fitness_equipment.hpp
Normal file
|
|
@ -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
|
||||
33
ant/src/ant/message.cpp
Normal file
33
ant/src/ant/message.cpp
Normal file
|
|
@ -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
|
||||
348
ant/src/ant/message.hpp
Normal file
348
ant/src/ant/message.hpp
Normal file
|
|
@ -0,0 +1,348 @@
|
|||
#pragma once
|
||||
#include "ant.hpp"
|
||||
#include <array>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#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<uint8_t>(value & 0xFF);
|
||||
}
|
||||
inline uint8_t upper_byte(uint16_t value) {
|
||||
return static_cast<uint8_t>((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<uint8_t, 8> 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<uint8_t, 3> _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<uint8_t, 8> _data) {
|
||||
return BatteryStatusMessage(_data[2] & 0x0f, (_data[2] & 0xf0) >> 4,
|
||||
_data[3] | (_data[4] << 8) | (_data[5] << 16),
|
||||
static_cast<float>(_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<uint8_t>(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<uint8_t>(device_type),
|
||||
static_cast<uint8_t>(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
|
||||
145
ant/src/ant/message_processor.cpp
Normal file
145
ant/src/ant/message_processor.cpp
Normal file
|
|
@ -0,0 +1,145 @@
|
|||
#include "message_processor.hpp"
|
||||
#include "ant.hpp"
|
||||
#include <cassert>
|
||||
#include <chrono>
|
||||
#include <cstdio>
|
||||
|
||||
namespace ant {
|
||||
MessageProcessor::MessageProcessor() {}
|
||||
void MessageProcessor::SetOnChannelResponseCallback(
|
||||
ChannelNumber _channel_id,
|
||||
std::function<void(ChannelResponseMessage)> _callback) {
|
||||
channel_response_cbs_[_channel_id] = _callback;
|
||||
}
|
||||
void MessageProcessor::SetOnBroadcastDataCallback(
|
||||
ChannelNumber _channel_id, std::function<void(BroadcastPayload)> _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<uint8_t, 3> 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
|
||||
43
ant/src/ant/message_processor.hpp
Normal file
43
ant/src/ant/message_processor.hpp
Normal file
|
|
@ -0,0 +1,43 @@
|
|||
#pragma once
|
||||
#include "message.hpp"
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
namespace ant {
|
||||
|
||||
typedef std::function<void(ChannelNumber, DeviceNumber)> DeviceNumberCallback;
|
||||
typedef std::function<void(ChannelResponseMessage)> ChannelResponseCallback;
|
||||
typedef std::function<void(BroadcastPayload)> 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<uint8_t> msg_buffer_;
|
||||
std::map<ChannelNumber, ChannelResponseCallback> channel_response_cbs_;
|
||||
std::map<ChannelNumber, BroadcastCallback> broadcast_data_cbs_;
|
||||
DeviceNumberCallback device_number_cb_{nullptr};
|
||||
};
|
||||
|
||||
} // namespace ant
|
||||
72
ant/src/ant_controller.cpp
Normal file
72
ant/src/ant_controller.cpp
Normal file
|
|
@ -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::HeartRateChannel>(*ant_device_, _channel,
|
||||
_device_number);
|
||||
|
||||
hr_channel_->SetOnHeartRateCallback([this](auto heart_rate) {
|
||||
emit_signal("heart_rate_updated", static_cast<int>(heart_rate));
|
||||
});
|
||||
}
|
||||
|
||||
void AntController::configure_fitness_equipment_channel(
|
||||
ant::ChannelNumber _channel, ant::DeviceNumber _device_number) {
|
||||
fe_channel_ = std::make_unique<ant::FitnessEquipmentChannel>(
|
||||
*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::PowerChannel>(*ant_device_, _channel, _device_number);
|
||||
}
|
||||
|
||||
void AntController::connect_ant_channels() {
|
||||
ant_device_ = std::make_unique<ant::ANTDevice>();
|
||||
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() {}
|
||||
46
ant/src/ant_controller.hpp
Normal file
46
ant/src/ant_controller.hpp
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
#pragma once
|
||||
|
||||
#include "ant/ant_device.hpp"
|
||||
#include "ant/channels.hpp"
|
||||
#include "ant/message.hpp"
|
||||
#include <godot_cpp/classes/node.hpp>
|
||||
#include <memory>
|
||||
|
||||
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::ANTDevice> ant_device_;
|
||||
std::unique_ptr<ant::HeartRateChannel> hr_channel_;
|
||||
std::unique_ptr<ant::FitnessEquipmentChannel> fe_channel_;
|
||||
std::unique_ptr<ant::PowerChannel> power_channel_;
|
||||
double time_passed_;
|
||||
};
|
||||
} // namespace godot
|
||||
51
ant/src/main.cpp
Normal file
51
ant/src/main.cpp
Normal file
|
|
@ -0,0 +1,51 @@
|
|||
|
||||
#include "ant/ant_device.hpp"
|
||||
#include "ant/channels.hpp"
|
||||
#include "ant/message.hpp"
|
||||
#include <memory>
|
||||
|
||||
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;
|
||||
}
|
||||
36
ant/src/register_types.cpp
Normal file
36
ant/src/register_types.cpp
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
#include "register_types.hpp"
|
||||
|
||||
#include "ant_controller.hpp"
|
||||
|
||||
#include <gdextension_interface.h>
|
||||
#include <godot_cpp/core/defs.hpp>
|
||||
#include <godot_cpp/godot.hpp>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
8
ant/src/register_types.hpp
Normal file
8
ant/src/register_types.hpp
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <godot_cpp/core/class_db.hpp>
|
||||
|
||||
using namespace godot;
|
||||
|
||||
void initialize_ant_module(ModuleInitializationLevel p_level);
|
||||
void uninitialize_ant_module(ModuleInitializationLevel p_level);
|
||||
4
demo/.editorconfig
Normal file
4
demo/.editorconfig
Normal file
|
|
@ -0,0 +1,4 @@
|
|||
root = true
|
||||
|
||||
[*]
|
||||
charset = utf-8
|
||||
15
demo/.gitignore
vendored
Normal file
15
demo/.gitignore
vendored
Normal file
|
|
@ -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
|
||||
7
demo/bin/ant.gdextension
Normal file
7
demo/bin/ant.gdextension
Normal file
|
|
@ -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"
|
||||
1
demo/bin/ant.gdextension.uid
Normal file
1
demo/bin/ant.gdextension.uid
Normal file
|
|
@ -0,0 +1 @@
|
|||
uid://cs56m8cu8a7ql
|
||||
BIN
demo/bin/libant.so
Executable file
BIN
demo/bin/libant.so
Executable file
Binary file not shown.
1
demo/icon.svg
Normal file
1
demo/icon.svg
Normal file
|
|
@ -0,0 +1 @@
|
|||
<svg xmlns="http://www.w3.org/2000/svg" width="128" height="128"><rect width="124" height="124" x="2" y="2" fill="#363d52" stroke="#212532" stroke-width="4" rx="14"/><g fill="#fff" transform="translate(12.322 12.322)scale(.101)"><path d="M105 673v33q407 354 814 0v-33z"/><path fill="#478cbf" d="m105 673 152 14q12 1 15 14l4 67 132 10 8-61q2-11 15-15h162q13 4 15 15l8 61 132-10 4-67q3-13 15-14l152-14V427q30-39 56-81-35-59-83-108-43 20-82 47-40-37-88-64 7-51 8-102-59-28-123-42-26 43-46 89-49-7-98 0-20-46-46-89-64 14-123 42 1 51 8 102-48 27-88 64-39-27-82-47-48 49-83 108 26 42 56 81zm0 33v39c0 276 813 276 814 0v-39l-134 12-5 69q-2 10-14 13l-162 11q-12 0-16-11l-10-65H446l-10 65q-4 11-16 11l-162-11q-12-3-14-13l-5-69z"/><path d="M483 600c0 34 58 34 58 0v-86c0-34-58-34-58 0z"/><circle cx="725" cy="526" r="90"/><circle cx="299" cy="526" r="90"/></g><g fill="#414042" transform="translate(12.322 12.322)scale(.101)"><circle cx="307" cy="532" r="60"/><circle cx="717" cy="532" r="60"/></g></svg>
|
||||
|
After Width: | Height: | Size: 994 B |
37
demo/icon.svg.import
Normal file
37
demo/icon.svg.import
Normal file
|
|
@ -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
|
||||
27
demo/node_3d.gd
Normal file
27
demo/node_3d.gd
Normal file
|
|
@ -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
|
||||
1
demo/node_3d.gd.uid
Normal file
1
demo/node_3d.gd.uid
Normal file
|
|
@ -0,0 +1 @@
|
|||
uid://cuyhf18taa5if
|
||||
41
demo/node_3d.tscn
Normal file
41
demo/node_3d.tscn
Normal file
|
|
@ -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"]
|
||||
16
demo/project.godot
Normal file
16
demo/project.godot
Normal file
|
|
@ -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"
|
||||
Loading…
Reference in a new issue