Skip to content

Commit

Permalink
Merge branch 'master' into accelerometerTestingSensors
Browse files Browse the repository at this point in the history
  • Loading branch information
TomLonergan03 committed Jul 1, 2024
2 parents 49d9dc6 + 4f0f768 commit d70b3f9
Show file tree
Hide file tree
Showing 51 changed files with 1,052 additions and 430 deletions.
4 changes: 2 additions & 2 deletions .github/workflows/telemetry.yml
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ jobs:
- name: Docker
uses: docker/setup-buildx-action@v3

- name: Lint
run: ./telemetry.sh ci
- name: Build, lint and test
run: ./telemetry.sh ci -m

telemetry-formatting:
name: Telemetry formatting
Expand Down
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ include_directories(${CURSES_INCLUDE_DIR})
find_package(Boost REQUIRED)
find_package(PahoMqttCpp REQUIRED)
include_directories(${PahoMqttCpp_INCLUDE_DIRS})
find_library(GPIOD_LIBRARY REQUIRED NAMES libgpiodcxx.so)
include(FetchContent)
FetchContent_Declare(
rapidjson GIT_REPOSITORY https://github.com/Tencent/rapidjson.git
Expand Down
14 changes: 11 additions & 3 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,23 @@ ARG DEBIAN_FRONTEND=noninteractive

# Install dependencies
RUN apt-get update && \
apt-get install -y git clang clang-format clang-tidy libboost-all-dev libeigen3-dev rapidjson-dev build-essential gcc make cmake libssl-dev libncurses5-dev libncursesw5-dev sudo
apt-get install -y git clang clang-format clang-tidy libboost-all-dev libeigen3-dev rapidjson-dev build-essential gcc make cmake libssl-dev libncurses5-dev libncursesw5-dev sudo autoconf-archive

# Install Paho C++
WORKDIR /home
RUN git clone --recurse-submodules https://github.com/eclipse/paho.mqtt.cpp.git
WORKDIR /home/paho.mqtt.cpp
RUN git checkout v1.3.2
RUN cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_STATIC=ON -DPAHO_BUILD_DOCUMENTATION=OFF -DPAHO_BUILD_SAMPLES=OFF
RUN git checkout v1.4.0
RUN cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_STATIC=OFF -DPAHO_BUILD_DOCUMENTATION=OFF -DPAHO_BUILD_SAMPLES=OFF -DPAHO_BUILD_SHARED=ON
RUN sudo cmake --build build/ --target install

# Install libgpiod
WORKDIR /home
RUN git clone https://git.kernel.org/pub/scm/libs/libgpiod/libgpiod.git
WORKDIR /home/libgpiod
RUN git checkout v2.1.x
RUN ./autogen.sh --enable-tools=yes --enable-bindings-cxx --prefix=/usr && make -j && sudo make install

RUN sudo ldconfig

WORKDIR /home/hyped_entrypoint
Expand Down
4 changes: 4 additions & 0 deletions config/debug.toml
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@ address = 0x38
enabled = false
bus = "can1"

[sensors.keyence]
enabled = false
pin = 1

[motors]
enabled = false
bus = "can1"
Expand Down
5 changes: 4 additions & 1 deletion config/pod.toml
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ host = "192.168.1.0"
port = 4556

[raspberry]
nodes = ["state_machine", "mqtt_broker"]
nodes = ["state_machine", "mqtt_broker", "navigator"]

[state_machine]
# Possible transition tables are "full_run",
Expand All @@ -26,3 +26,6 @@ topic = 'accelerometer'
format = 'float'
type = 'acceleration'
unit = 'm/s²'

[navigator]
maxVelocity = 10
2 changes: 1 addition & 1 deletion lib/core/mqtt_logger.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ MqttLogger::MqttLogger(const char *const label,
void MqttLogger::log(const LogLevel level, const char *format, ...)
{
logger_.log(level, format);
const auto topic = MqttTopic::kTest;
const auto topic = MqttTopic::kLogs;
std::shared_ptr<rapidjson::Document> message_payload = std::make_shared<rapidjson::Document>();
message_payload->SetObject();
message_payload->AddMember("log", *format, message_payload->GetAllocator());
Expand Down
36 changes: 32 additions & 4 deletions lib/core/mqtt_topics.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,39 @@

namespace hyped::core {

enum class MqttTopic { kTest, kState };
enum class MqttTopic {
kState,
kStateRequest,
kAccelerometer,
kOpticalFlow,
kKeyence,
kStarted,
kDisplacement,
kVelocity,
kAcceleration,
kLogs
};

const std::unordered_map<MqttTopic, std::string> mqtt_topic_to_string
= {{MqttTopic::kTest, "test"}, {MqttTopic::kState, "state"}};
= {{MqttTopic::kState, "hyped/cart_2024/state/state"},
{MqttTopic::kStateRequest, "hyped/cart_2024/state/state_request"},
{MqttTopic::kAccelerometer, "hyped/cart_2024/measurement/accelerometer"},
{MqttTopic::kOpticalFlow, "hyped/cart_2024/measurement/optical_flow"},
{MqttTopic::kKeyence, "hyped/cart_2024/measurement/keyence"},
{MqttTopic::kDisplacement, "hyped/cart_2024/navigation/displacement"},
{MqttTopic::kVelocity, "hyped/cart_2024/navigation/velocity"},
{MqttTopic::kAcceleration, "hyped/cart_2024/navigation/acceleration"},
{MqttTopic::kLogs, "hyped/cart_2024/logs"}};

const std::unordered_map<std::string, MqttTopic> mqtt_string_to_topic
= {{"test", MqttTopic::kTest}, {"state", MqttTopic::kState}};
= {{"hyped/cart_2024/state/state", MqttTopic::kState},
{"hyped/cart_2024/state/state_request", MqttTopic::kState},
{"hyped/cart_2024/measurement/accelerometer", MqttTopic::kAccelerometer},
{"hyped/cart_2024/measurement/optical_flow", MqttTopic::kOpticalFlow},
{"hyped/cart_2024/measurement/keyence", MqttTopic::kKeyence},
{"hyped/cart_2024/navigation/displacement", MqttTopic::kDisplacement},
{"hyped/cart_2024/navigation/velocity", MqttTopic::kVelocity},
{"hyped/cart_2024/navigation/acceleration", MqttTopic::kAcceleration},
{"hyped/cart_2024/logs", MqttTopic::kLogs}};

} // namespace hyped::core
} // namespace hyped::core
29 changes: 7 additions & 22 deletions lib/core/types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,9 @@ struct Trajectory {
};

// number of each type of sensors
static constexpr std::uint8_t kNumAccelerometers = 4;
static constexpr std::uint8_t kNumAccelerometers = 1;
static constexpr std::uint8_t kNumAxis = 3;
static constexpr std::uint8_t kNumEncoders = 4;
static constexpr std::uint8_t kNumKeyence = 2;
static constexpr std::uint8_t kNumKeyence = 1;
static constexpr std::uint8_t kNumOptical = 1;

// data format for raw sensor data
Expand All @@ -38,25 +37,11 @@ using OpticalData = std::array<std::array<Float, 2>, kNumOptical>;
// data produced by the accelerometer sensor
// values are in milli-g (standard gravity)
struct RawAccelerationData {
RawAccelerationData(const std::int32_t x,
const std::int32_t y,
const std::int32_t z,
const TimePoint measured_at,
const bool is_sensor_active)
: x(x),
y(y),
z(z),
measured_at(measured_at),
is_sensor_active(is_sensor_active)

{
}

const std::int32_t x;
const std::int32_t y;
const std::int32_t z;
const TimePoint measured_at;
const bool is_sensor_active;
std::int32_t x;
std::int32_t y;
std::int32_t z;
TimePoint measured_at;
bool is_sensor_active;
};

} // namespace hyped::core
26 changes: 14 additions & 12 deletions lib/debug/commands/gpio_commands.cpp
Original file line number Diff line number Diff line change
@@ -1,26 +1,28 @@
#include "gpio_commands.hpp"

#include <io/hardware_gpio.hpp>

namespace hyped::debug {

core::Result GpioCommands::addCommands(core::ILogger &logger, std::shared_ptr<Repl> &repl)
{
{
const auto *const gpio_read_command_name = "gpio read";
const auto *const gpio_read_command_description = "Read from GPIO pins";
const auto *const gpio_read_command_usage = "gpio read <pin_number>";
const std::string gpio_read_command_name = "gpio read";
const std::string gpio_read_command_description = "Read from GPIO pins";
const std::string gpio_read_command_usage = "gpio read <pin_number>";
const auto gpio_read_command_handler = [&logger, repl](std::vector<std::string> args) {
if (args.size() != 1) {
logger.log(core::LogLevel::kFatal, "Invalid number of arguments");
return;
}
const auto gpio = repl->getGpio();
const auto pin = std::stoi(args[0]);
auto optional_gpio_reader = gpio->getReader(pin, io::Edge::kNone);
auto optional_gpio_reader = gpio->getReader(pin);
if (!optional_gpio_reader) {
logger.log(core::LogLevel::kFatal, "Failed to get GPIO reader on pin %d", pin);
logger.log(core::LogLevel::kFatal, "Failed to create GPIO reader for pin %d", pin);
return;
}
const auto gpio_reader = std::move(*optional_gpio_reader);
const auto gpio_reader = *optional_gpio_reader;
const auto value = gpio_reader->read();
if (!value) {
logger.log(core::LogLevel::kFatal, "Failed to read from GPIO pin %d", pin);
Expand All @@ -38,22 +40,22 @@ core::Result GpioCommands::addCommands(core::ILogger &logger, std::shared_ptr<Re
repl->addCommand(std::move(gpio_read_command));
}
{
const auto *const gpio_write_command_name = "gpio write";
const auto *const gpio_write_command_description = "Write to GPIO pin (0 or 1)";
const auto *const gpio_write_command_usage = "gpio write <pin_number> <value>";
const std::string gpio_write_command_name = "gpio write";
const std::string gpio_write_command_description = "Write to GPIO pin (0 or 1)";
const std::string gpio_write_command_usage = "gpio write <pin_number> <value>";
const auto gpio_write_command_handler = [&logger, repl](std::vector<std::string> args) {
if (args.size() != 2) {
logger.log(core::LogLevel::kFatal, "Invalid number of arguments");
return;
}
const auto gpio = repl->getGpio();
const auto pin = std::stoi(args[0]);
auto optional_gpio_writer = gpio->getWriter(pin, io::Edge::kNone);
auto optional_gpio_writer = gpio->getWriter(pin);
if (!optional_gpio_writer) {
logger.log(core::LogLevel::kFatal, "Failed to create GPIO writer on pin %d", pin);
logger.log(core::LogLevel::kFatal, "Failed to create GPIO writer for pin %d", pin);
return;
}
const auto gpio_writer = std::move(*optional_gpio_writer);
const auto gpio_writer = *optional_gpio_writer;
const auto value = std::stoi(args[1]);
core::DigitalSignal signal;
switch (value) {
Expand Down
66 changes: 66 additions & 0 deletions lib/debug/commands/keyence_commands.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
#include "keyence_commands.hpp"

#include <cstdint>
#include <thread>

#include "core/logger.hpp"
#include "core/time.hpp"
#include "core/types.hpp"
#include "sensors/keyence.hpp"

namespace hyped::debug {

core::Result KeyenceCommands::addCommands(core::ILogger &logger,
std::shared_ptr<Repl> &repl,
core::ITimeSource &time,
toml::v3::node_view<toml::v3::node> config)
{
const auto optional_pin = config["pin"].value<std::uint8_t>();
if (!optional_pin) {
logger.log(core::LogLevel::kFatal, "Invalid pin");
return core::Result::kFailure;
}
const auto pin = *optional_pin;
const auto gpio = repl->getGpio();
auto optional_keyence = sensors::Keyence::create(logger, gpio, pin);
if (!optional_keyence) {
logger.log(core::LogLevel::kFatal, "Failed to create keyence sensor");
return core::Result::kFailure;
}
const auto keyence = *optional_keyence;
const std::string keyence_command_name = "keyence read";
const std::string keyence_command_usage = "keyence read <time> <interval>";
const std::string keyence_command_description
= "Read from the keyence sensor every <interval> seconds for <time> seconds";
const auto keyence_command_handler = [&logger, keyence, &time](std::vector<std::string> args) {
if (args.size() != 2) {
logger.log(core::LogLevel::kFatal, "Invalid number of arguments");
return;
}
const std::uint32_t run_time = std::stoul(args[0], nullptr, 10);
const std::uint32_t interval = std::stof(args[1]);
const auto start_time = time.now();
auto count = 0;
while (time.now() - start_time < std::chrono::seconds(run_time)) {
const auto result = keyence->updateStripeCount();
if (result != core::Result::kSuccess) {
logger.log(core::LogLevel::kFatal, "Failed to update stripe count");
return;
}
const auto stripe_count = keyence->getStripeCount();
if (stripe_count != count) {
logger.log(core::LogLevel::kInfo, "Stripe count: %d", stripe_count);
count = stripe_count;
}
std::this_thread::sleep_for(std::chrono::seconds(interval));
}
};
auto keyence_command = std::make_unique<Command>(keyence_command_name,
keyence_command_description,
keyence_command_usage,
keyence_command_handler);
repl->addCommand(std::move(keyence_command));
return core::Result::kSuccess;
}

} // namespace hyped::debug
27 changes: 27 additions & 0 deletions lib/debug/commands/keyence_commands.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
#pragma once

#include <memory>

#include <core/logger.hpp>
#include <core/types.hpp>
#include <debug/repl.hpp>

namespace hyped::debug {

class KeyenceCommands {
public:
/**
* @brief Add commands to the REPL for reading from keyence stripe counters
*
* @param logger
* @param repl
* @param config
* @return core::Result kSuccess if commands were added successfully, kFailure otherwise
*/
static core::Result addCommands(core::ILogger &logger,
std::shared_ptr<Repl> &repl,
core::ITimeSource &time,
toml::v3::node_view<toml::v3::node> config);
};

} // namespace hyped::debug
10 changes: 10 additions & 0 deletions lib/debug/repl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include "commands/can_commands.hpp"
#include "commands/gpio_commands.hpp"
#include "commands/i2c_commands.hpp"
#include "commands/keyence_commands.hpp"
#include "commands/pwm_commands.hpp"
#include "commands/spi_commands.hpp"
#include "commands/temperature_commands.hpp"
Expand All @@ -21,6 +22,7 @@ namespace hyped::debug {
// NOLINTBEGIN(readability-function-cognitive-complexity)
std::optional<std::shared_ptr<Repl>> Repl::create(core::ILogger &logger,
Terminal &terminal,
core::ITimeSource &time,
const std::string &filename)
{
auto repl = std::make_shared<Repl>(logger, terminal);
Expand Down Expand Up @@ -88,6 +90,14 @@ std::optional<std::shared_ptr<Repl>> Repl::create(core::ILogger &logger,
return std::nullopt;
}
}
if (config["sensors"]["keyence"]["enabled"].value_or(false)) {
const auto result
= KeyenceCommands::addCommands(logger, repl, time, config["sensors"]["keyence"]);
if (result == core::Result::kFailure) {
logger.log(core::LogLevel::kFatal, "Error adding keyence commands");
return std::nullopt;
}
}
if (config["sensors"]["temperature"]["enabled"].value_or(false)) {
const auto result
= TemperatureCommands::addCommands(logger, repl, config["sensors"]["temperature"]);
Expand Down
9 changes: 6 additions & 3 deletions lib/debug/repl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ class Repl {
public:
static std::optional<std::shared_ptr<Repl>> create(core::ILogger &logger,
Terminal &terminal,
core::ITimeSource &time,
const std::string &filename);
Repl(core::ILogger &logger, Terminal &terminal);
void run();
Expand Down Expand Up @@ -63,9 +64,11 @@ class Repl {
*
* @return std::shared_ptr<io::HardwareGpio> containing the Gpio object
*/
std::shared_ptr<io::HardwareGpio> getGpio() { return gpio_; };
std::shared_ptr<io::IGpio> getGpio() { return gpio_; };

/**
* @brief Get the I2c object associated with the given bus or create a new one if it doesn't exist
* @brief Get the I2c object associated with the given bus or create a new one if it doesn't
* exist
* @param bus target bus for the I2c object
* @return std::optional<std::shared_ptr<io::II2c>> containing the I2c object at bus or
* std::nullopt if the I2c could not be created
Expand Down Expand Up @@ -125,7 +128,7 @@ class Repl {

std::unordered_map<std::uint8_t, std::shared_ptr<io::IAdc>> adc_;
std::unordered_map<std::string, std::shared_ptr<io::ICan>> can_;
std::shared_ptr<io::HardwareGpio> gpio_;
std::shared_ptr<io::IGpio> gpio_;
std::unordered_map<std::uint8_t, std::shared_ptr<io::II2c>> i2c_;
std::unordered_map<io::PwmModule, std::shared_ptr<io::Pwm>> pwm_;
std::unordered_map<io::SpiBus, std::shared_ptr<io::ISpi>> spi_;
Expand Down
1 change: 1 addition & 0 deletions lib/io/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,4 @@ include_directories(
"${CMAKE_SOURCE_DIR}/lib"
"${CMAKE_CURRENT_SOURCE_DIR}"
)
target_link_libraries(${target} gpiodcxx)
Loading

0 comments on commit d70b3f9

Please sign in to comment.