diff --git a/components/dataLink/DataLinkScheduler.cpp b/components/dataLink/DataLinkScheduler.cpp index f54a0b5..b521109 100644 --- a/components/dataLink/DataLinkScheduler.cpp +++ b/components/dataLink/DataLinkScheduler.cpp @@ -21,11 +21,11 @@ void DataLinkManager::init_scheduler(){ /** * @brief Schedules which frame to send - * - * Scheduler: + * + * Scheduler: * - All frames will be pushed to the back onto a queue * - When a generic frame sends a chunk, it will be pushed back to the queue for the next chunk to be sent - * + * * Scheduling may change (above scheduler will lead to starvation of control frames depending on the number of generic frames/fragments to send) */ [[noreturn]] void DataLinkManager::frame_scheduler(void* args){ @@ -41,17 +41,17 @@ void DataLinkManager::init_scheduler(){ link_layer_obj->scheduler_send(i); } vTaskDelay(pdMS_TO_TICKS(SCHEDULER_PERIOD_MS)); - + } vTaskDelete(nullptr); } /** * @brief Pushes a frame to the scheduler - * - * @param frame - * @param channel - * @return esp_err_t + * + * @param frame + * @param channel + * @return esp_err_t */ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel){ if (frame.data == nullptr){ @@ -77,7 +77,7 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint ESP_LOGE(DEBUG_LINK_TAG, "Invalid scheduler queue handle"); return ESP_FAIL; } - + if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){ frame_queue[channel].push(frame); xSemaphoreGive(sq_handle[channel]); @@ -93,9 +93,9 @@ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint } /** - * @brief Scheduler sending the actual frame at the top of the heap on a channel - * - * @return esp_err_t + * @brief Scheduler sending the actual frame at the top of the heap on a channel + * + * @return esp_err_t */ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (phys_comms == nullptr){ @@ -106,9 +106,9 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (sq_handle[channel] == nullptr){ return ESP_FAIL; } - + SchedulerMetadata frame; - + if (xSemaphoreTake(sq_handle[channel], pdMS_TO_TICKS(SCHEDULER_MUTEX_WAIT)) == pdTRUE){ if (frame_queue[channel].empty()){ xSemaphoreGive(sq_handle[channel]); @@ -144,7 +144,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (isControlFrame){ //control frame - res = create_control_frame(frame.data, frame.len, + res = create_control_frame(frame.data, frame.len, make_control_frame_from_header(frame.header), send_data, &frame_size); vPortFree(frame.data); @@ -160,7 +160,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ //generic frame if (frame.len > (MAX_GENERIC_DATA_LEN)){ //fragment here - + if (frame.timeout == 0){ frame.timeout = GENERIC_FRAME_MIN_TIMEOUT; } else { @@ -169,7 +169,7 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); vPortFree(frame.data); - return res; + return res; } return ESP_OK; } @@ -235,16 +235,16 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ // ESP_LOGI(DEBUG_LINK_TAG, "frame %d curr offset %d\n", frame.header.seq_num, curr_offset); // ESP_LOGI(DEBUG_LINK_TAG, "frame %d fragment size %d\n", frame.header.seq_num, fragment_size); - + frame.header.frag_info = (frame.header.frag_info & 0xFFFF0000) | frame.curr_fragment; //increment frag_num //create fragment - res = create_generic_frame(frame.data, fragment_size, + res = create_generic_frame(frame.data, fragment_size, make_generic_frame_from_header(frame.header), curr_offset, send_data, &frame_size); if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame fragment"); vPortFree(frame.data); - return res; + return res; } res = scheduler_send_rmt(channel, frame, send_data, frame_size, true); @@ -255,12 +255,12 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); vPortFree(frame.data); - } - return res; + } + return res; } //need to schedule the next fragment (if total_frags != frag_num) - if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFF) || (frame.last_ack != (frame.header.frag_info >> 16) && + if ((frame.header.frag_info >> 16) > (frame.header.frag_info & 0xFF) || (frame.last_ack != (frame.header.frag_info >> 16) && static_cast(GET_TYPE(frame.header.type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE)){ // frame.generic_frame_data_offset += fragment_size; // ESP_LOGI(DEBUG_LINK_TAG, "scheduling frame %d with frag_info 0x%X", frame.header.seq_num, frame.header.frag_info); @@ -268,20 +268,20 @@ esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); vPortFree(frame.data); - return res; - } + return res; + } } else { //Done fragmenting, can free data array - ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info); + // ESP_LOGI(DEBUG_LINK_TAG, "finished fragmenting seq num %d frag_info 0x%X", frame.header.seq_num, frame.header.frag_info); vPortFree(frame.data); } } else { //no fragmenting - res = create_generic_frame(frame.data, frame.len, + res = create_generic_frame(frame.data, frame.len, make_generic_frame_from_header(frame.header), 0, send_data, &frame_size); vPortFree(frame.data); - + if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to create generic frame"); return res; @@ -331,22 +331,22 @@ esp_err_t DataLinkManager::scheduler_send_rmt(uint8_t channel, SchedulerMetadata /** * @brief Increases the head of the sliding window associated with the board id and sequence number - * - * @param channel + * + * @param channel * @param board_id Receiving Board ID (the board who ACK'd) - * @param seq_num - * @param ack_record - * @return esp_err_t + * @param seq_num + * @param ack_record + * @return esp_err_t */ esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num, FrameAckRecord* ack_record){ if (ack_record == NULL){ return ESP_ERR_INVALID_ARG; } - if (ack_record->total_frags == 0 || ack_record->total_frags > MAX_GENERIC_NUM_FRAG + if (ack_record->total_frags == 0 || ack_record->total_frags > MAX_GENERIC_NUM_FRAG || ack_record->last_ack == 0 || ack_record->total_frags < ack_record->last_ack){ return ESP_ERR_INVALID_ARG; - } + } if (sliding_window_mutex[channel] == NULL){ return ESP_ERR_INVALID_STATE; @@ -362,7 +362,7 @@ esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t boar xSemaphoreGive(sliding_window_mutex[channel]); return ESP_ERR_INVALID_ARG; } - + record.last_ack = ack_record->last_ack; if (record.total_frags == 0){ record.total_frags = ack_record->total_frags; @@ -375,12 +375,12 @@ esp_err_t DataLinkManager::inc_head_sliding_window(uint8_t channel, uint8_t boar /** * @brief Gets the current record associated with the board id and sequence number from the sliding window - * - * @param channel + * + * @param channel * @param board_id Receiving Board ID (the board who ACK'd) - * @param seq_num - * @param ack_record - * @return esp_err_t + * @param seq_num + * @param ack_record + * @return esp_err_t */ esp_err_t DataLinkManager::get_record_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num, FrameAckRecord* ack_record){ if (ack_record == NULL){ @@ -415,11 +415,11 @@ esp_err_t DataLinkManager::get_record_sliding_window(uint8_t channel, uint8_t bo /** * @brief Removes the board id + sequence number record fromt the sliding window (map) - * - * @param channel + * + * @param channel * @param board_id Receiving Board ID (the board who ACK'd) - * @param seq_num - * @return esp_err_t + * @param seq_num + * @return esp_err_t */ esp_err_t DataLinkManager::complete_record_sliding_window(uint8_t channel, uint8_t board_id, uint16_t seq_num){ if (sliding_window_mutex[channel] == NULL){ @@ -441,4 +441,4 @@ esp_err_t DataLinkManager::complete_record_sliding_window(uint8_t channel, uint8 xSemaphoreGive(sliding_window_mutex[channel]); return ESP_OK; -} \ No newline at end of file +} diff --git a/components/flatbuffers/CMakeLists.txt b/components/flatbuffers/CMakeLists.txt index a47fe49..a7b031f 100644 --- a/components/flatbuffers/CMakeLists.txt +++ b/components/flatbuffers/CMakeLists.txt @@ -1,2 +1,2 @@ -idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" +idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp" "TopologyMessageBuilder.cpp" "SensorMessageBuilder.cpp" INCLUDE_DIRS "include") diff --git a/components/flatbuffers/SensorMessageBuilder.cpp b/components/flatbuffers/SensorMessageBuilder.cpp new file mode 100644 index 0000000..8e00fa8 --- /dev/null +++ b/components/flatbuffers/SensorMessageBuilder.cpp @@ -0,0 +1,31 @@ +// +// Created by Johnathon Slightham on 2025-06-30. +// + +#include "SensorMessageBuilder.h" +#include "SerializedMessage.h" +#include "flatbuffers_generated/SensorMessage_generated.h" + +namespace Flatbuffers { + + SerializedMessage SensorMessageBuilder::build_sensor_message(std::vector& values) { + builder_.Clear(); + + std::vector> values_vec; + std::vector sensor_values_vec; + + for (const auto& v : values) { + values_vec.push_back(Messaging::CreateAngle(builder_, v.angle).Union()); + sensor_values_vec.push_back(Messaging::SensorValue_Angle); + } + + auto values_fb_vec = builder_.CreateVector(values_vec); + const auto values_type_fb_vec = builder_.CreateVector(sensor_values_vec); + + const auto message = Messaging::CreateSensorMessage(builder_, values_type_fb_vec, values_fb_vec); + + builder_.Finish(message); + + return { builder_.GetBufferPointer(), builder_.GetSize() }; + } +} diff --git a/components/flatbuffers/include/SensorMessageBuilder.h b/components/flatbuffers/include/SensorMessageBuilder.h new file mode 100644 index 0000000..2c8be27 --- /dev/null +++ b/components/flatbuffers/include/SensorMessageBuilder.h @@ -0,0 +1,24 @@ + +#ifndef SENSORMESSAGEBUILDER_H +#define SENSORMESSAGEBUILDER_H + +#include "SerializedMessage.h" +#include "flatbuffers_generated/SensorMessage_generated.h" + +namespace Flatbuffers { + struct SensorValueInstance { + uint16_t angle; // todo: change to a variant + }; + + class SensorMessageBuilder{ + public: + SensorMessageBuilder() : builder_(128) {} + + SerializedMessage build_sensor_message(std::vector& values); + + private: + flatbuffers::FlatBufferBuilder builder_; + }; +} + +#endif //SENSORMESSAGEBUILDER_H diff --git a/components/flatbuffers/include/SerializedMessage.h b/components/flatbuffers/include/SerializedMessage.h index 916a627..4be0a6e 100644 --- a/components/flatbuffers/include/SerializedMessage.h +++ b/components/flatbuffers/include/SerializedMessage.h @@ -5,10 +5,12 @@ #ifndef SERIALIZEDMESSAGE_H #define SERIALIZEDMESSAGE_H +#include + namespace Flatbuffers { struct SerializedMessage { void* data; - size_t size; + std::size_t size; }; } diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index 8d80b3d..43abd2d 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -35,9 +35,8 @@ CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); } while (true) { if (std::chrono::system_clock::now() - that->m_last_leader_updated > - std::chrono::seconds(15)) { + std::chrono::seconds(2)) { that->m_last_leader_updated = std::chrono::system_clock::now(); - ESP_LOGI(TAG, "Updating leader"); that->update_leader(); } @@ -88,6 +87,7 @@ void CommunicationRouter::update_leader() { // Leader has changed, we may need to change PC connection state if (this->m_leader != max) { + ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max); if (max == m_module_id) { m_pc_connection->connect(); m_lossless_server->startup(); diff --git a/main/LoopManager.cpp b/main/LoopManager.cpp index ee78369..9a56f9f 100644 --- a/main/LoopManager.cpp +++ b/main/LoopManager.cpp @@ -2,35 +2,37 @@ // Created by Johnathon Slightham on 2025-07-05. // - -#include #include #include "LoopManager.h" -#include "MessagingInterface.h" +#include "SensorMessageBuilder.h" #include "TopologyMessageBuilder.h" -#include "control/ActuatorFactory.h" -#include "esp_log.h" #define ACTUATOR_CMD_TAG 5 #define TOPOLOGY_CMD_TAG 6 #define METADATA_RX_TAG 7 +#define SENSOR_TAG 8 #define METADATA_PERIOD_MS 1000 +#define SENSOR_DATA_PERIOD_MS 1000 [[noreturn]] void LoopManager::control_loop() const { - const auto actuator = ActuatorFactory::create_actuator(m_config_manager.get_module_type()); // todo: this needs to be moved higher up with one factory that returns shared ptr for both actuator and sensor. - uint8_t buffer[512]; while (true) { m_messaging_interface->recv(reinterpret_cast(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG); - actuator->actuate(buffer); + m_actuator->actuate(buffer); + send_sensor_reading(false); } } -[[noreturn]] void LoopManager::sensor_loop() const { - // todo: impl +[[noreturn]] void LoopManager::sensor_loop(char * args) { + const auto that = reinterpret_cast(args); + + while (true) { + that->send_sensor_reading(true); + vTaskDelay(SENSOR_DATA_PERIOD_MS / portTICK_PERIOD_MS); + } } [[noreturn]] void LoopManager::metadata_tx_loop(char * args) { @@ -63,7 +65,13 @@ buffer->resize(512); while (true) { that->m_messaging_interface->recv(buffer->data(), 512, PC_ADDR, METADATA_RX_TAG); - - } } + +void LoopManager::send_sensor_reading(bool durable) const { + Flatbuffers::SensorMessageBuilder smb{}; + // todo: get data from sensor + auto data = m_actuator->get_sensor_data(); + const auto [ptr, size] = smb.build_sensor_message(data); + m_messaging_interface->send(reinterpret_cast(ptr), size, PC_ADDR, SENSOR_TAG, durable); +} diff --git a/main/control/DCMotorActuator.cpp b/main/control/DCMotorActuator.cpp index 587cb2b..94edc9d 100644 --- a/main/control/DCMotorActuator.cpp +++ b/main/control/DCMotorActuator.cpp @@ -1,3 +1,5 @@ +#include + #include "control/DCMotorActuator.h" #include "esp_attr.h" #include "util/number_utils.h" @@ -104,9 +106,9 @@ void DCMotorActuator::pid_task(char* args) { const auto that = reinterpret_cast(args); while (true) { - const double degrees = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION); + that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION); - const double error = degrees - that->m_target_angle; + const double error = that->m_current_angle - that->m_target_angle; that->m_integral += error * KI; const double detivative = (error - that->m_last_error) * KD; that->m_last_error = error; @@ -146,3 +148,8 @@ void DCMotorActuator::pid_task(char* args) { vTaskDelay(75 / portTICK_PERIOD_MS); } } + +std::vector DCMotorActuator::get_sensor_data() { + // todo: this really needs to return a int32, should also return two sensor data items, one for target one for current + return {{(uint16_t)(m_current_angle)}}; +} diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp index 5d279bf..acc941a 100644 --- a/main/control/Servo1Actuator.cpp +++ b/main/control/Servo1Actuator.cpp @@ -6,7 +6,9 @@ #include "AngleControlMessageBuilder.h" #include "constants/module.h" #include "driver/ledc.h" +#include "flatbuffers_generated/SensorMessage_generated.h" #include "util/number_utils.h" +#include "SensorMessageBuilder.h" #define LOW_DUTY 200 #define HIGH_DUTY 1000 @@ -41,8 +43,13 @@ void Servo1Actuator::actuate(uint8_t *cmd) { const auto newDuty = util::mapRange(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY); + m_target = angleControlCmd->angle(); std::cout << "actuating to " << angleControlCmd->angle() << std::endl; ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); } + +std::vector Servo1Actuator::get_sensor_data() { + return {{m_target}}; +} diff --git a/main/include/LoopManager.h b/main/include/LoopManager.h index 8a27c43..1c24b9f 100644 --- a/main/include/LoopManager.h +++ b/main/include/LoopManager.h @@ -8,19 +8,25 @@ #include #include "MessagingInterface.h" +#include "control/ActuatorFactory.h" +#include "control/IActuator.h" class LoopManager { public: LoopManager() : m_config_manager(ConfigManager::get_instance()), - m_messaging_interface(std::make_unique()) {} + m_messaging_interface(std::make_unique()), + m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {} [[noreturn]] void control_loop() const; // gets control commands - [[noreturn]] void sensor_loop() const; // sends sensor data commands continually + [[noreturn]] static void sensor_loop(char * args); // sends sensor data commands continually [[noreturn]] static void metadata_tx_loop(char * args); // sends metadata continually (low duty cycle) [[noreturn]] static void metadata_rx_loop(char * args); // gets other commands from PC (ie. f/w updates, nvs updates) private: ConfigManager& m_config_manager; std::unique_ptr m_messaging_interface; + std::unique_ptr m_actuator; + + void send_sensor_reading(bool durable) const; }; #endif //LOOPMANAGER_H diff --git a/main/include/control/DCMotorActuator.h b/main/include/control/DCMotorActuator.h index 5dc4d9b..80bc28c 100644 --- a/main/include/control/DCMotorActuator.h +++ b/main/include/control/DCMotorActuator.h @@ -12,10 +12,12 @@ public: DCMotorActuator(); ~DCMotorActuator() override; void actuate(uint8_t *cmd) override; + std::vector get_sensor_data() override; private: void setup_encoder(); static void pid_task(char* args); + double m_current_angle; int64_t m_target_angle; TaskHandle_t m_pid_task; diff --git a/main/include/control/IActuator.h b/main/include/control/IActuator.h index da459fa..886a8da 100644 --- a/main/include/control/IActuator.h +++ b/main/include/control/IActuator.h @@ -5,10 +5,16 @@ #ifndef IACTUATOR_H #define IACTUATOR_H +#include + +#include "SensorMessageBuilder.h" + + class IActuator { public: virtual ~IActuator() {} virtual void actuate(uint8_t *cmd) = 0; + virtual std::vector get_sensor_data() = 0; }; #endif //IACTUATOR_H diff --git a/main/include/control/Servo1Actuator.h b/main/include/control/Servo1Actuator.h index 471ff5b..4475372 100644 --- a/main/include/control/Servo1Actuator.h +++ b/main/include/control/Servo1Actuator.h @@ -15,6 +15,9 @@ class Servo1Actuator final : public IActuator { public: Servo1Actuator(); void actuate(std::uint8_t *cmd) override; + std::vector get_sensor_data() override; +private: + uint16_t m_target = 90; }; #endif //SERVO1ACTUATOR_H diff --git a/main/main.cpp b/main/main.cpp index 19563ed..8118ebb 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -18,6 +18,8 @@ extern "C" [[noreturn]] void app_main(void) { const auto loop_manager = std::make_unique(); xTaskCreate(reinterpret_cast(LoopManager::metadata_tx_loop), "metadata_tx", 3096, loop_manager.get(), 3, nullptr); + xTaskCreate(reinterpret_cast(LoopManager::sensor_loop), + "sensor_tx", 3096, loop_manager.get(), 3, nullptr); loop_manager->control_loop(); } -#endif \ No newline at end of file +#endif