diff --git a/components/RFM69_LowPowerLab b/components/RFM69_LowPowerLab index 240306d..034caf2 160000 --- a/components/RFM69_LowPowerLab +++ b/components/RFM69_LowPowerLab @@ -1 +1 @@ -Subproject commit 240306de6ebe5ef16ab474f8862366f96b54a711 +Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b diff --git a/components/drone_comms b/components/drone_comms index 1a1fccc..d7debd6 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca +Subproject commit d7debd6a59298850dd7371a90ed331175b64ab12 diff --git a/components/drone_controller b/components/drone_controller index 17f62e4..9d8fccc 160000 --- a/components/drone_controller +++ b/components/drone_controller @@ -1 +1 @@ -Subproject commit 17f62e4827fb94694799f091ab93c09628effef8 +Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502 diff --git a/main/env_sens.cpp b/main/env_sens.cpp index 2f8ad9d..700fdc0 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -29,7 +29,7 @@ void setup() { ESP_LOGI(TAG, "BARO SETUP COMPLETE."); bme.setSampling(Adafruit_BME280::MODE_NORMAL, Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_X8, Adafruit_BME280::SAMPLING_NONE, - Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_10); + Adafruit_BME280::FILTER_OFF, Adafruit_BME280::STANDBY_MS_20); bme_temp->printSensorDetails(); bme_pressure->printSensorDetails(); @@ -109,7 +109,7 @@ void baro_poll_task(void *_) { } // BME280 in your config has a 10ms standby, so 20ms-50ms poll is ideal - vTaskDelay(pdMS_TO_TICKS(20)); + vTaskDelay(pdMS_TO_TICKS(50)); } } diff --git a/main/imu.h b/main/imu.h index 4016358..0287c45 100644 --- a/main/imu.h +++ b/main/imu.h @@ -1,10 +1,16 @@ #pragma once -#include +#include "drone_controller.h" #include +#ifdef LOW +#undef LOW +#endif +#ifdef HIGH +#undef HIGH +#endif + #include "BNO08x.hpp" -#include "drone_controller.h" struct imu_state { Vec3C accel = {0, 0, 0}; diff --git a/main/main.cpp b/main/main.cpp index 593afe8..504b360 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,21 +1,27 @@ #include "driver/gpio.h" #include "drone_comms.h" +#include "esp32-hal.h" #include "esp_log.h" #include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" #include "freertos/task.h" -#include "imu.h" #include +#include +#include #include "env_sens.h" #include "gps.h" #include "nav.h" #include "radio.h" +#include "imu.h" + static const char *TAG = "MAIN"; +void handle_packet(uint8_t *packet_addr); + extern "C" void app_main(void) { nav_mutex = xSemaphoreCreateMutex(); @@ -26,18 +32,18 @@ extern "C" void app_main(void) { static imu_state imu_state; auto _ = setup_imu(&imu_state); - // xTaskCreatePinnedToCore(radio_rx_task, // Function name - // "radio_rx", // Name for debugging - // 4096, // Stack size in bytes - // NULL, // Parameters - // 1, // Priority (higher = more urgent) - // NULL, // Task handle - // 1 // Core ID - // ); - // + xTaskCreatePinnedToCore(radio_task, // Function name + "radio_rxtx", // Name for debugging + 4096, // Stack size in bytes + NULL, // Parameters + 5, // Priority (higher = more urgent) + NULL, // Task handle + 1 // Core ID + ); + xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL); - xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 1, NULL); + xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL); ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); @@ -45,15 +51,9 @@ extern "C" void app_main(void) { Eigen::Vector3f local_vel = {0, 0, 0}; bool nav_data_ready = false; while (true) { - // if (controller_input_semaphore && - // xSemaphoreTake(controller_input_semaphore, (TickType_t)30) == pdTRUE) - // { - // inp = current_controller_input; - // xSemaphoreGive(controller_input_semaphore); - // - // ESP_LOGI(TAG, "CONT INPUT: (%f, %f), (%f,%f)", inp.lx, inp.ly, inp.rx, - // inp.ry); - // } + if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { + handle_packet(&packet_data[0]); + } if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { if (gps->gps_avaliable()) { @@ -90,3 +90,42 @@ extern "C" void app_main(void) { vTaskDelay(pdMS_TO_TICKS(1000)); } } + +void handle_packet(uint8_t *packet_addr) { + PACKET_TYPE packet_type = *((PACKET_TYPE *)packet_addr); + + if (packet_type == PACKET_TYPE::COMMAND_REQUEST) { + + packet_command_request *packet = + (packet_command_request *)(packet_addr + sizeof(PACKET_TYPE)); + PACKET_TYPE requested_type = packet->packet_requested; + std::pair resp_packet = {nullptr, 0}; + + if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) { + resp_packet = create_packet_pooled( + PACKET_TYPE::INFO_DRONE_POSITION, + packet_info_drone_position{ + {nav_filter.position.x(), nav_filter.position.y(), + nav_filter.position.z()}, + {nav_filter.velocity.x(), nav_filter.velocity.y(), + nav_filter.velocity.z()}, + {0.0, 0.0, 0.0}}); + } + + if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) { + + if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)20) == pdTRUE) { + + resp_packet = create_packet_pooled( + PACKET_TYPE::INFO_DRONE_STATUS, + packet_info_drone_status{ + {gps->origin_long, gps->origin_lat}, millis(), 0}); + xSemaphoreGive(gps_mutex); + } + } + + if (resp_packet.first != nullptr) { + xQueueSend(packet_tx_queue, resp_packet.first, portMAX_DELAY); + } + } +} diff --git a/main/radio.cpp b/main/radio.cpp index fd93b94..5e06a12 100644 --- a/main/radio.cpp +++ b/main/radio.cpp @@ -27,6 +27,12 @@ static const char *TAG = "RADIO_TASK"; #define PACKET_QUEUE_CAP 10 +static bool is_pending_switch = false; +static uint32_t target_bitrate = 0; +static uint32_t switch_at_ms = 0; +static uint32_t rollback_at_ms = 0; +static bool confirmed_at_new_rate = false; + void radio_task(void *pvParameters) { ESP_LOGI(TAG, "Radio Task Started on Core %d", xPortGetCoreID()); packet_rx_queue = xQueueCreate(PACKET_QUEUE_CAP, MAX_PACKET_SIZE); @@ -49,6 +55,7 @@ void radio_task(void *pvParameters) { if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) { radio.setHighPower(true); + radio.setCustomBitrate(DEFAULT_COMMS_BITRATE); ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10)); } else { ESP_LOGE(TAG, "Radio Init FAILED! Restarting."); @@ -56,26 +63,36 @@ void radio_task(void *pvParameters) { } while (1) { + uint32_t now = millis(); + if (radio.receiveDone()) { - // ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID, - // radio.RSSI, - // radio.DATALEN); + + // If we receive ANY valid packet while in probation, confirm the switch + // (Bit-rate switching) + if (is_pending_switch && now > switch_at_ms) { + confirmed_at_new_rate = true; + ESP_LOGI(TAG, "New bitrate confirmed by valid packet."); + } + + ESP_LOGD(TAG, "Packet [ID:%d] RSSI:%d LEN:%d", radio.SENDERID, radio.RSSI, + radio.DATALEN); memset(packet_data, '\0', sizeof(packet_data)); memcpy(packet_data, radio.DATA, radio.DATALEN); PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]); - if (packet_type == CONTROLLER_INPUT) { + if (packet_type == COMMAND_CHANGE_DATARATE) { + packet_command_datarate *cmd = + (packet_command_datarate *)(&packet_data[0] + sizeof(PACKET_TYPE)); + target_bitrate = cmd->target_bitrate; - packet_controller_input *inp = - (packet_controller_input *)(&packet_data[0] + sizeof(PACKET_TYPE)); - - if (xSemaphoreTake(controller_input_semaphore, (TickType_t)50) == - pdTRUE) { - current_controller_input = *inp; - xSemaphoreGive(controller_input_semaphore); - } + switch_at_ms = now + cmd->ms_delay; + rollback_at_ms = now + cmd->ms_rollback; + is_pending_switch = true; + confirmed_at_new_rate = false; + ESP_LOGI(TAG, "Datarate change requested: %d. Switching in 100ms...", + target_bitrate); } else { xQueueSend(packet_rx_queue, &packet_data[0], portMAX_DELAY); } @@ -84,11 +101,36 @@ void radio_task(void *pvParameters) { radio.sendACK(); } } + + // Send packets that were queued up for sending if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { PACKET_TYPE packet_type = *((PACKET_TYPE *)&packet_data[0]); radio.send(GROUNDID, &packet_data[0], get_packet_size(packet_type)); } + // --- STATE MACHINE FOR BITRATE SWITCHING --- + + // 1. Execute the Switch + if (is_pending_switch && now >= switch_at_ms && !confirmed_at_new_rate) { + // We only want to trigger the register write once + radio.setCustomBitrate(target_bitrate); + switch_at_ms = 0xFFFFFFFF; // Prevent re-triggering + } + + // 2. The Rollback (The Fail-safe) + if (is_pending_switch && !confirmed_at_new_rate && now > rollback_at_ms) { + ESP_LOGE(TAG, + "ROLLBACK: No confirmation at new rate. Reverting to default."); + radio.setCustomBitrate(DEFAULT_COMMS_BITRATE); + is_pending_switch = false; + } + + // 3. Clear pending flag once confirmed + if (confirmed_at_new_rate) { + is_pending_switch = false; + confirmed_at_new_rate = false; + } + vTaskDelay(pdMS_TO_TICKS(10)); } }