drone comms update. add change bitrate custom function to radio. change drone comms. implement change datarate

components/RFM69_LowPowerLab

@@ -1 +1 @@
-Subproject commit 240306de6ebe5ef16ab474f8862366f96b54a711
+Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b

components/drone_comms

@@ -1 +1 @@
-Subproject commit 1a1fccca83a4a24670715efa4b532d8a7c4e51ca
+Subproject commit d7debd6a59298850dd7371a90ed331175b64ab12

components/drone_controller

@@ -1 +1 @@
-Subproject commit 17f62e4827fb94694799f091ab93c09628effef8
+Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502

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));
   }
 }
 

main/imu.h

@@ -1,10 +1,16 @@
 #pragma once
 
-#include <cmath>
+#include "drone_controller.h"
 #include <cstdint>
 
+#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};

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 <Arduino.h>
+#include <cstddef>
+#include <cstdint>
 
 #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
+  xTaskCreatePinnedToCore(radio_task,   // Function name
-  //                         "radio_rx",    // Name for debugging
+                          "radio_rxtx", // Name for debugging
-  //                         4096,          // Stack size in bytes
+                          4096,         // Stack size in bytes
-  //                         NULL,          // Parameters
+                          NULL,         // Parameters
-  //                         1,             // Priority (higher = more urgent)
+                          5,            // Priority (higher = more urgent)
-  //                         NULL,          // Task handle
+                          NULL,         // Task handle
-  //                         1              // Core ID
+                          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 &&
+    if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
-    //     xSemaphoreTake(controller_input_semaphore, (TickType_t)30) == pdTRUE)
+      handle_packet(&packet_data[0]);
-    //     {
+    }
-    //   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 (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<uint8_t *, size_t> 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);
+    }
+  }
+}

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,
+      // If we receive ANY valid packet while in probation, confirm the switch
-      //          radio.DATALEN);
+      // (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 =
+        switch_at_ms = now + cmd->ms_delay;
-            (packet_controller_input *)(&packet_data[0] + sizeof(PACKET_TYPE));
+        rollback_at_ms = now + cmd->ms_rollback;
-
+        is_pending_switch = true;
-        if (xSemaphoreTake(controller_input_semaphore, (TickType_t)50) ==
+        confirmed_at_new_rate = false;
-            pdTRUE) {
-          current_controller_input = *inp;
-          xSemaphoreGive(controller_input_semaphore);
-        }
 
+        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));
   }
 }