diff --git a/src/subsystem/CMakeLists.txt b/src/subsystem/CMakeLists.txt index 4520832..40e66cd 100644 --- a/src/subsystem/CMakeLists.txt +++ b/src/subsystem/CMakeLists.txt @@ -1,8 +1,13 @@ add_subdirectory(can) -add_executable(subsystem subsystem.cpp drive_controller.hpp drive_controller.cpp) +add_subdirectory(task_scheduler) -find_package(Boost COMPONENTS system REQUIRED) -target_include_directories(subsystem PUBLIC ${Boost_INCLUDE_DIR} rover_can rover_system_messages) -target_link_libraries(subsystem PUBLIC network rover_can rover_system_messages Boost::system) +find_package(Boost COMPONENTS system) +if (Boost_FOUND AND Boost_system_FOUND) + add_executable(subsystem subsystem.cpp drive_controller.hpp drive_controller.cpp) + target_include_directories(subsystem PUBLIC ${Boost_INCLUDE_DIR} rover_can rover_system_messages) + target_link_libraries(subsystem PUBLIC network rover_can rover_system_messages Boost::system) -target_compile_features(subsystem PRIVATE cxx_std_17) \ No newline at end of file + target_compile_features(subsystem PRIVATE cxx_std_17) +else() + message(STATUS "rover_subsystem: excluded from build since Boost is missing") +endif() diff --git a/src/subsystem/can/CMakeLists.txt b/src/subsystem/can/CMakeLists.txt index 3bc99d3..102fa85 100644 --- a/src/subsystem/can/CMakeLists.txt +++ b/src/subsystem/can/CMakeLists.txt @@ -1,22 +1,23 @@ -add_library(rover_can rover_can.hpp constants.hpp commands.hpp commands.cpp) -target_include_directories(rover_can PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_compile_features(rover_can PRIVATE cxx_std_20) - -# If ONBOARD_CAN_BUS has been manually defined (or is cached), then don't override it with auto-detection -if (NOT DEFINED ONBOARD_CAN_BUS) +# If ROVER_CAN_AVAIL has been manually defined (or is cached), then don't override it with auto-detection +if (NOT DEFINED ROVER_CAN_AVAIL) if (${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - set(ONBOARD_CAN_BUS ON CACHE BOOL "Compile subsystem with Linux CAN bus libraries") + set(ROVER_CAN_AVAIL ON CACHE BOOL "Compile subsystem with Linux CAN bus libraries") message(STATUS "rover_can: Platform supports full CAN functionality") else() - set(ONBOARD_CAN_BUS OFF CACHE BOOL "Compile subsystem with Linux CAN bus libraries") + set(ROVER_CAN_AVAIL OFF CACHE BOOL "Compile subsystem with Linux CAN bus libraries") message(STATUS "rover_can: Platform does not support rover CAN bus") endif() endif() -if (ONBOARD_CAN_BUS) - target_compile_definitions(rover_can PUBLIC ONBOARD_CAN_BUS) - message(STATUS "rover_can: Building in ONBOARD mode") +add_library(rover_can rover_can.hpp constants.hpp commands.hpp commands.cpp) + +target_include_directories(rover_can PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_compile_features(rover_can PRIVATE cxx_std_20) + +if (ROVER_CAN_AVAIL) + target_compile_definitions(rover_can PUBLIC ROVER_CAN_AVAIL) + message(STATUS "rover_can: Building with full CAN bus support") else() - message(STATUS "rover_can: Building in OFFLINE mode. CAN bus is disabled") + message(STATUS "rover_can: Building in placeholder/OFFLINE mode. CAN bus is disabled") endif() \ No newline at end of file diff --git a/src/subsystem/can/commands.cpp b/src/subsystem/can/commands.cpp index 60085b5..1c18f07 100644 --- a/src/subsystem/can/commands.cpp +++ b/src/subsystem/can/commands.cpp @@ -1,5 +1,14 @@ #include "commands.hpp" +#ifdef ROVER_CAN_AVAIL + #include + #include + #include + #include + #include + #include +#endif + //Can socket number static int can_socket = 0; static bool socket_open = false; @@ -29,7 +38,7 @@ int can_send(Node device, Command command, int num_bytes, unsigned int data) { //Set status to unset status = CAN_Status::UNSET; -#ifdef ONBOARD_CAN_BUS + #ifdef ROVER_CAN_AVAIL //Socket is not open, nothing can send if (!socket_open) { return 1; } @@ -47,7 +56,7 @@ int can_send(Node device, Command command, int num_bytes, unsigned int data) { status = CAN_Status::FAILED_WRITE; return 1; } -#endif + #endif //Successfully sent status = CAN_Status::SUCCESS; return 0; @@ -58,7 +67,7 @@ int can_request(Node device, Command command, float f) { //Set status to unset status = CAN_Status::UNSET; -#ifdef ONBOARD_CAN_BUS + #ifdef ROVER_CAN_AVAIL //Socket is not open, nothing can send if (!socket_open) { return 1; } @@ -71,13 +80,14 @@ int can_request(Node device, Command command, float f) { status = CAN_Status::FAILED_REQUEST; return 1; } -#endif + #endif //Successfully sent status = CAN_Status::SUCCESS; return 0; } void can_read_all(const std::function& callback) { + #ifdef ROVER_CAN_AVAIL if (socket_open) { can_frame received_frame; @@ -85,12 +95,13 @@ void can_read_all(const std::function& callback) { callback(&received_frame); } } + #endif } uint64_t canframe_get_u64(can_frame* frame) { uint64_t x = 0; for (int i = 0; i < 8; i++) { - x |= ((uint64_t)(frame->data[i]) << (8 * (-i + 7))); + x |= ((uint64_t)(can_frame_get_data(frame)[i]) << (8 * (-i + 7))); } return x; } @@ -143,9 +154,10 @@ void parse_environmental_analysis_information(EAInformation& write_to, uint64_t write_to.humidity = (float((0x00000000FF000000l & p) >> 24)) / 10.0f; } -#ifdef ONBOARD_CAN_BUS +// Exclude these internal functions entirely if CAN isn't available +#ifdef ROVER_CAN_AVAIL //Create a can frame -can_frame get_can_frame(int modifier, Node device, Command command, int num_bytes, unsigned int data) { +static can_frame get_can_frame(int modifier, Node device, Command command, int num_bytes, unsigned int data) { can_frame frame; frame.can_id = command | (device << 5) | (modifier << 8); frame.can_dlc = num_bytes; @@ -162,7 +174,7 @@ can_frame get_can_frame(int modifier, Node device, Command command, int num_byte } //Create a frame for receiving -can_frame get_can_request_frame(int modifier, Node device, Command command, float f) { +static can_frame get_can_request_frame(int modifier, Node device, Command command, float f) { can_frame frame; frame.can_id = (0x40000000) | command | (device << 5) | (modifier << 8); frame.can_dlc = 0; @@ -187,7 +199,7 @@ unsigned int get_big_endian(unsigned int u) { //Open can socket bool can_open_socket() { -#ifdef ONBOARD_CAN_BUS + #ifdef ROVER_CAN_AVAIL //Get socket number can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (can_socket < 0) { return false; } @@ -218,17 +230,21 @@ bool can_open_socket() { //Disable receive filter, then open socket if (bind(can_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { return false; } -#endif //Socket has been successfully opened socket_open = true; return true; + + #else + return false; + #endif } //Close can socket void can_close_socket() { -#ifdef ONBOARD_CAN_BUS + + #ifdef ROVER_CAN_AVAIL close(can_socket); -#endif + #endif socket_open = false; } diff --git a/src/subsystem/can/commands.hpp b/src/subsystem/can/commands.hpp index 10aa06d..4ba00e2 100644 --- a/src/subsystem/can/commands.hpp +++ b/src/subsystem/can/commands.hpp @@ -3,29 +3,19 @@ #include #include #include -#include -#ifdef ONBOARD_CAN_BUS -#include -#include -#include -#include -#include -#include -#else - -#warning Compiling rover_can in offline mode - -// Declare that can_frame exists so offline devices can still compile -extern "C" { - struct can_frame; -} +#include "constants.hpp" +#ifdef ROVER_CAN_AVAIL + #include +#else + // Declare that can_frame exists so offline devices can still compile + extern "C" { + struct can_frame; + } #endif -#include "constants.hpp" - //Overloads for can_send int can_send(Node device, Command command, unsigned int data); int can_send(Node device, Command command, int data); @@ -44,6 +34,35 @@ constexpr int can_id(Node device, Command command) { return command | (device << 5); } +/// Return the can_id from a can_frame pointer. +/// +/// This function allows programs that use rover_can to compile even if +/// Linux/CAN isn't available. However, programs should try to avoid calling +/// these functions if CAN isn't available since the return value does not +/// make sense. +inline uint32_t can_frame_get_can_id(const can_frame* frame) { + #ifdef ROVER_CAN_AVAIL + return frame->can_id; + #else + return 0; + #endif +} + +/// Return the data pointer from a can_frame pointer. +/// +/// This function allows programs that use rover_can to compile even if +/// Linux/CAN isn't available. This function returns null if CAN is not +/// available, so users should ensure this function is not called if CAN did not +/// initialize successfully. +inline uint8_t* can_frame_get_data(const can_frame* frame) { + #ifdef ROVER_CAN_AVAIL + return frame->data; + #else + // Returning null is okay since these functions should never be called if CAN isn't available + return nullptr; + #endif +} + uint64_t canframe_get_u64(can_frame* frame); void parse_control_information(ControlInformation& write_to, uint64_t p1, uint64_t p2); @@ -53,12 +72,6 @@ void parse_arm_information(ArmInformation& write_to, uint64_t p); void parse_gripper_information(GripperInformation& write_to, uint64_t p); void parse_environmental_analysis_information(EAInformation& write_to, uint64_t p); -#ifdef ONBOARD_CAN_BUS -//Create a can frame -can_frame get_can_frame(int modifier, Node device, Command command, int num_bytes, unsigned int data); -can_frame get_can_request_frame(int modifier, Node device, Command command, float f); -#endif - //Converts unsigned int to big endian unsigned int get_big_endian(unsigned int u); diff --git a/src/subsystem/subsystem.cpp b/src/subsystem/subsystem.cpp index ba4b9a7..4da4e3e 100644 --- a/src/subsystem/subsystem.cpp +++ b/src/subsystem/subsystem.cpp @@ -140,7 +140,7 @@ int main() { } can_read_all([] (can_frame* frame) { - switch (frame->can_id) { + switch (can_frame_get_can_id(frame)) { case can_id(Node::CONTROL_TEENSY, Command::TEENSY_DATA_PACKET_1): parse_control_p1(rover_sensor_information, canframe_get_u64(frame)); break; @@ -150,7 +150,7 @@ int main() { case can_id(Node::DRIVE_AXIS_0, Command::GET_VBUS_VOLTAGE): unsigned int v = 0; for (int i = 0; i < 4; i++) { - v |= ((unsigned int)frame->data[i] << (8 * i)); + v |= (static_cast(can_frame_get_data(frame)[i]) << (8 * i)); } union { unsigned int ul; float fl; } conv = { .ul = v }; rover_sensor_information.ps_batt = conv.fl; diff --git a/src/subsystem/task_scheduler/CMakeLists.txt b/src/subsystem/task_scheduler/CMakeLists.txt new file mode 100644 index 0000000..e449021 --- /dev/null +++ b/src/subsystem/task_scheduler/CMakeLists.txt @@ -0,0 +1,3 @@ +add_library(task_scheduler task_scheduler.hpp task_scheduler.cpp) +target_include_directories(task_scheduler PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_compile_features(task_scheduler PUBLIC cxx_std_14) \ No newline at end of file diff --git a/src/subsystem/task_scheduler/README.md b/src/subsystem/task_scheduler/README.md new file mode 100644 index 0000000..2e8f0ce --- /dev/null +++ b/src/subsystem/task_scheduler/README.md @@ -0,0 +1,80 @@ +# Task Scheduler Manual +## Overview +The task scheduler is for running multiple tasks in the same thread in a controlled and predictable manner. Simply put, with this task scheduler, functions can schedule a time to be called again. When no functions are ready to dispatch, the program can sleep instead of busy wait. + +Tasks are vaguely inspired by FreeRTOS Tasks and Boost.Fiber but are much, much simpler. A task is only a function callback that may be dispatched synchronously when calling `TaskScheduler::poll()`. + +## Problems Addressed + +Formerly, the Main Event Loop in the subsystem computer checked for work by polling. A polling-style loop basically looks like: + 1. Check if any network packets have arrived + 2. Check if the drive controller should update the motors + 3. Check if a CAN heartbeat should be sent + 4. Check if a system status update should be sent + 5. Repeat (no delay) + +On most iterations, this program does no work: it wastes nearly 100% of its CPU time *checking* for work. This method scales poorly. On small microcontrollers, busy-waiting is standard. On Raspberry Pi, spinning hogs valuable CPU time and reduces the system's overall capacity. Busy-waiting on a PC also wastes a ton of electricity. + +## Usage +### Including +Link to the CMake target `task_scheduler`. The target has no dependencies other than the C++ STL. + +In source files, include the header `task_scheduler.hpp` +### Code +Create a `TaskScheduler()` to manage tasks. + +For each task, create a `Task(const std::function& callback)` for desired tasks. The `Task&` is a reference to the task itself. Providing a null callback and then scheduling the task will result in undefined behavior. +```C++ +// Prints "Fizz" every 3 seconds +Task fizz([] (Task& self) { + // Tasks must reschedule themselves + self.set_next_occurrence(std::chrono::steady_clock::now() + std::chrono::seconds(3)); + std::cout << "Fizz\n"; +})); +``` +After creating the task, add it to the scheduler with `TaskScheduler::add_task(Task&)`. +```C++ +sched.add_task(fizz); +``` +Tasks must be allocated as long as they are scheduled. If a scheduled task is destructed, it will be canceled safely and automatically. + +To dispatch all tasks which are ready, call `TaskScheduler::poll()`. This can be combined with `TaskScheduler::get_next_dispatch_time()` to sleep during inactivity. +```C++ +while (!sched.empty()) { + std::this_thread::sleep_until(sched.get_next_dispatch_time()); + sched.poll(); +} +``` +#### Complete Example +```C++ +#include +#include +#include + +TaskScheduler sched; + +void print_buzz_forever(Task& self) { + self.set_next_occurrence(std::chrono::steady_clock::now() + std::chrono::seconds(5)); + std::cout << "Buzz\n"; +} + +int main() { + // Prints "Fizz" every 3 seconds + Task fizz([] (Task& self) { + // Tasks must reschedule themselves + self.set_next_occurrence(std::chrono::steady_clock::now() + std::chrono::seconds(3)); + std::cout << "Fizz\n"; + })); + sched.add_task(fizz); + + // Another way to add a task by binding a function + Task buzz_task(print_buzz_forever); + sched.add_task(buzz_task); + + while (!sched.empty()) { + std::this_thread::sleep_until(sched.get_next_dispatch_time()); + sched.poll(); + } + return 0; +} +``` \ No newline at end of file diff --git a/src/subsystem/task_scheduler/task_scheduler.cpp b/src/subsystem/task_scheduler/task_scheduler.cpp new file mode 100644 index 0000000..b607108 --- /dev/null +++ b/src/subsystem/task_scheduler/task_scheduler.cpp @@ -0,0 +1,96 @@ +#include + +Task::Task(const std::function& callback) : event_callback(callback) {} + +Task::~Task() { + cancel(); +} + +std::chrono::steady_clock::time_point TaskScheduler::get_next_dispatch_time() const { + if (tasks.empty()) + return std::chrono::steady_clock::time_point{}; + else + return (*tasks.top()).get_next_occurrence(); +} + +bool TaskScheduler::TaskEntry::operator<(const TaskEntry& other) const { + return ptr->next_event < other.ptr->next_event; +} +bool TaskScheduler::TaskEntry::operator>(const TaskEntry& other) const { + return ptr->next_event > other.ptr->next_event; +} +Task& TaskScheduler::TaskEntry::operator*() const { + return *ptr; +} + +TaskScheduler::TaskEntry::TaskEntry(Task& t) : + ptr(&t), + entry_active(true) { + + // Tasks cannot be scheduled multiple times at once: invalidate any previous entries + if (t.latest_entry != nullptr) { + t.latest_entry->ptr = nullptr; + t.latest_entry->entry_active = false; + } + t.latest_entry = this; + +} + +// Tasks point back to their task entry, so when moving an entry, update this pointer +// to the new location +TaskScheduler::TaskEntry::TaskEntry(TaskEntry&& old) : + ptr(old.ptr), + entry_active(old.entry_active) { + + old.ptr = nullptr; + old.entry_active = false; + + ptr->latest_entry = this; + +} + +// When destructing an entry, update the task's entry pointer to reflect that it no longer +// has an entry with the scheduler. Note that this entry may not point to a task, and if it +// does point to a task, it may have been rescheduled with another entry +TaskScheduler::TaskEntry::~TaskEntry() { + if (ptr != nullptr && ptr->latest_entry == this) { + ptr->latest_entry = nullptr; + } +} + +void Task::cancel() { + if (latest_entry) { + latest_entry->entry_active = false; + } +} + +void Task::set_next_occurrence(const std::chrono::steady_clock::time_point& time_pt) { + // If this task is already scheduled, invalidate the latest scheduler entry + if (latest_entry) { + latest_entry->entry_active = false; + } + + next_event = time_pt; + parent_scheduler->add_task(*this); +} + +void TaskScheduler::add_task(Task& task) { + tasks.emplace(TaskEntry(task)); + task.parent_scheduler = this; +} + +void TaskScheduler::poll() { + // While the soonest task is ready for dispatch, run the event callback + // Additionally, pop any inactive tasks + while (!tasks.empty() && (!tasks.top().entry_active || (*tasks.top()).get_next_occurrence() <= std::chrono::steady_clock::now())) { + // Entries should be de-queued before dispatch, so make a copy of the entry information + TaskEntry current(tasks.top()); + tasks.pop(); + + if (current.entry_active) { + Task& c = *current; + c.event_callback(c); + } + + } +} \ No newline at end of file diff --git a/src/subsystem/task_scheduler/task_scheduler.hpp b/src/subsystem/task_scheduler/task_scheduler.hpp new file mode 100644 index 0000000..65b87f9 --- /dev/null +++ b/src/subsystem/task_scheduler/task_scheduler.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include + +// The scheduler and tasks are closely related and almost function as one unit +class TaskScheduler; +class Task; + +/// Manage and dispatch synchronous tasks in one thread +/// +/// This is ideal for procedures that must be scheduled on a regular basis but +/// need to run synchronously in a single thread. Using a scheduler allows the +/// process to sleep when nothing needs to run. +class TaskScheduler { + friend class Task; + + public: + /// Add a new task to this scheduler + /// + /// The scheduler saves a reference to the task, but the Task container + /// is safe. The Task will be automatically removed from the scheduler + /// at the end of its lifetime. + void add_task(Task&); + + /// Run any tasks which are ready for dispatch and return + void poll(); + + inline bool empty() const { + return tasks.empty(); + } + + std::chrono::steady_clock::time_point get_next_dispatch_time() const; + + private: + + // Wraps the task pointer to provide a comparator based on what is being + // pointed to rather than the address. This also allows invalidating + // entries in the tasks queue since elements can't be removed + struct TaskEntry { + TaskEntry(Task&); + TaskEntry(const TaskEntry&) = default; + TaskEntry(TaskEntry&&); + ~TaskEntry(); + Task* ptr; + bool entry_active; + + bool operator<(const TaskEntry& other) const; + bool operator>(const TaskEntry& other) const; + Task& operator*() const; + + TaskEntry& operator=(TaskEntry&) = default; + TaskEntry& operator=(const TaskEntry&) = default; + }; + // The soonest tasks will be at the top + std::priority_queue, std::greater> tasks; + +}; + +/// Control structure for an individual task for the TaskScheduler +class Task { + friend class TaskScheduler; + + public: + Task(const std::function& callback); + ~Task(); + + void set_next_occurrence(const std::chrono::steady_clock::time_point& p); + void cancel(); + + inline const std::chrono::steady_clock::time_point& get_next_occurrence() const { + return next_event; + } + + inline bool operator<(const Task& other) const { + return next_event < other.next_event; + } + inline bool operator>(const Task& other) const { + return next_event > other.next_event; + } + + private: + std::chrono::steady_clock::time_point next_event = {}; + TaskScheduler* parent_scheduler = nullptr; + TaskScheduler::TaskEntry* latest_entry = nullptr; + + std::function event_callback; + +}; \ No newline at end of file