#include "DataLinkManager.h" #include "esp_log.h" #include "esp_timer.h" #include "freertos/semphr.h" void DataLinkManager::init_scheduler(){ for (int i = 0; i < num_channels; i++){ sq_handle[i] = xSemaphoreCreateMutex(); async_rx_queue_mutex[i] = xSemaphoreCreateMutex(); } ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task"); xTaskCreate(DataLinkManager::frame_scheduler, "Scheduler", 4096, static_cast(this), 5, &scheduler_task); xTaskCreate(DataLinkManager::receive_thread_main, "Scheduler", 8192, static_cast(this), 5, &receive_task); } [[noreturn]] void DataLinkManager::frame_scheduler(void* args){ DataLinkManager* link_layer_obj = static_cast(args); if (link_layer_obj == nullptr){ ESP_LOGE(DEBUG_LINK_TAG, "Frame Scheduler failed to start due to invalid pointer"); vTaskDelete(nullptr); } ESP_LOGI(DEBUG_LINK_TAG, "Starting Frame Scheduler task"); while(!link_layer_obj->stop_tasks){ for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){ 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 */ esp_err_t DataLinkManager::push_frame_to_scheduler(SchedulerMetadata frame, uint8_t channel){ if (frame.data == nullptr){ ESP_LOGE(DEBUG_LINK_TAG, "Frame data is null"); return ESP_ERR_INVALID_ARG; } if (channel >= num_channels){ ESP_LOGE(DEBUG_LINK_TAG, "Invalid channels"); return ESP_ERR_INVALID_ARG; } if (frame.len == 0){ ESP_LOGE(DEBUG_LINK_TAG, "Invalid Frame Length"); return ESP_ERR_INVALID_ARG; } int64_t now = esp_timer_get_time(); frame.enqueue_time_ns = now; if (sq_handle[channel] == nullptr){ 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]); } else { //Failed to obtain mutex ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex"); return ESP_ERR_TIMEOUT; } // ESP_LOGI(DEBUG_LINK_TAG, "Pushed frame to queue on channel %d", channel); return ESP_OK; } esp_err_t DataLinkManager::scheduler_send(uint8_t channel){ if (phys_comms == nullptr){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to send frame due to no RMT object"); return ESP_ERR_INVALID_ARG; } 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]); // ESP_LOGI(DEBUG_LINK_TAG, "Scheduler queue for channel %d is empty", channel); return ESP_OK; } frame = frame_queue[channel].top(); frame_queue[channel].pop(); xSemaphoreGive(sq_handle[channel]); } else { ESP_LOGE(DEBUG_LINK_TAG, "Failed to get mutex when trying to send"); //Failed to obtain mutex return ESP_ERR_TIMEOUT; } if (frame.data == nullptr){ ESP_LOGE(DEBUG_LINK_TAG, "Data array does not exist"); return ESP_ERR_INVALID_ARG; } if (this_board_id == PC_ADDR){ ESP_LOGE(DEBUG_LINK_TAG, "This board is not assigned a board id"); vPortFree(frame.data); return ESP_ERR_INVALID_ARG; } esp_err_t res; bool isControlFrame = IS_CONTROL_FRAME(static_cast(frame.header.type_flag)); size_t frame_size = isControlFrame ? sizeof(ControlFrame) : sizeof(GenericFrame); uint8_t send_data[frame_size]; if (isControlFrame){ //control frame res = create_control_frame(frame.data, frame.len, make_control_frame_from_header(frame.header), send_data, &frame_size); vPortFree(frame.data); if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to create control frame"); return res; } // ESP_LOGI(DEBUG_LINK_TAG, "Sending %d bytes", frame_size); } else { //generic frame if (frame.len > MAX_CONTROL_DATA_LEN){ //fragment here uint16_t curr_offset = frame.generic_frame_data_offset; uint16_t fragment_size = 0; //calculate fragment data size if (curr_offset + MAX_CONTROL_DATA_LEN <= frame.len){ fragment_size = curr_offset + MAX_CONTROL_DATA_LEN; } else { fragment_size = frame.len - curr_offset; } //create fragment 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); //free entire data for now - will need to implement retries/sliding window soon return res; } //need to schedule the next fragment if (curr_offset != MAX_CONTROL_DATA_LEN){ frame.generic_frame_data_offset += fragment_size; frame.header.frag_info += 1; //increment frag_num res = push_frame_to_scheduler(frame, channel); if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to schedule next generic frame fragment"); vPortFree(frame.data); //free entire data for now - will need to implement retries/sliding window soon return res; } } else { //Done fragmenting, can free data array vPortFree(frame.data); } } else { 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; } } uint8_t channel_to_route = MAX_CHANNELS; rmt_transmit_config_t config = { .loop_count = 0, .flags = { .eot_level = 0 // typically 0 or 1, depending on your output idle level } }; if (frame.header.receiver_id == BROADCAST_ADDR){ // printf("Sending on channel %d\n", i); phys_comms->send(send_data, frame_size, &config, channel); } else { res = route_frame(frame.header.receiver_id, &channel_to_route); if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to find entry for %d", frame.header.receiver_id); return ESP_FAIL; } // ESP_LOGI(DEBUG_LINK_TAG, "Sending message to %d", frame.header.receiver_id); phys_comms->send(send_data, frame_size, &config, channel_to_route); } if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to send message"); return ESP_FAIL; } else{ // printf("Sent message to board %d\n", frame.header.receiver_id); } return ESP_OK; }