communication issues when using bi directional in radio

components/DShotRMT

@@ -1 +1 @@
-Subproject commit 4f731edaead51a644270d2bf183a8a43cae33454
+Subproject commit 86d682313234556720dc63ad53aace47ab3d190c

components/RFM69_LowPowerLab

@@ -1 +1 @@
-Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b
+Subproject commit 9b083db90723c4911ac79edb6c1aa35c4c127cb8

components/drone_comms

@@ -1 +1 @@
-Subproject commit d58740e83368c9f2cc2b641ff729d04e7033d007
+Subproject commit bb38f95e0de4868b2f84b3fa8ab3025f3496e37b

components/drone_controller

@@ -1 +1 @@
-Subproject commit ac127154714cf9ce781757d7203651fed214b5ff
+Subproject commit 3504bc479075bbc363697edc4123978e4342c823

components/esp32_Adafruit_GPS

@@ -1 +1 @@
-Subproject commit 3df2e79d866d1a46fe79f9094a8647a3fd8c5c40
+Subproject commit 06ee1eea5a5073337591edc713a755306cea1331

main/drone.cpp

@@ -18,8 +18,7 @@
 #include "nav.h"
 #include "packet_handler.h"
 #include "sens_fus.h"
-#include "soc/gpio_num.h"
+#include "soc/gpio_num.h" #include < cstdint>
-#include <cstdint>
 #include <cstring>
 #include <optional>
 #include <stdlib.h>
@@ -103,7 +102,7 @@ void motor_throttles_task(void *params) {
   motor_throttles = (float *)malloc(sizeof(float) * 4);
 
   for (int i = 0; i < 4; i++) {
-    motor_throttles[i] = 0;
+    motor_throttles[i] = 0.02;
     motors[i] = new DShotRMT(motor_pins[i], DSHOT300, false);
     motors[i]->begin();
   }
@@ -123,7 +122,7 @@ void motor_throttles_task(void *params) {
       if (atomic_load(&killswitch_active)) {
         throttle = 0.0;
       }
-      motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f * 0.7f);
+      motors[i]->sendThrottlePercent(throttle);
     }
     vTaskDelay(2);
   }

main/main.cpp

@@ -3,8 +3,7 @@
 #include "drone.h"
 #include "drone_comms.h"
 #include "drone_controller.h"
-#include "esp32-hal.h"
+#include "esp32-hal.h" #include "esp_log.h"
-#include "esp_log.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/idf_additions.h"
 #include "freertos/projdefs.h"
@@ -21,10 +20,11 @@
 #include "packet_handler.h"
 #include "radio.h"
 #include "sens_fus.h"
+#include "servo.h"
 
 static const char *TAG = "MAIN";
 
-#define TIME_RELEASE_QUEUE_TO_ACTIVATION 1000
+#define TIME_RELEASE_QUEUE_TO_ACTIVATION 500
 
 extern "C" void app_main(void) {
 
@@ -59,16 +59,17 @@ extern "C" void app_main(void) {
   //                         1     // Core ID
   // );
 
-  // xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
+  xTaskCreatePinnedToCore(motor_throttles_task,   // Function name
-  //                         "motor_throttles_task", // Name for debugging
+                          "motor_throttles_task", // Name for debugging
-  //                         1024 * 4,               // Stack size in bytes
+                          1024 * 4,               // Stack size in bytes
-  //                         NULL,                   // Parameters
+                          NULL,                   // Parameters
-  //                         24,   // Priority (higher = more urgent)
+                          24,   // Priority (higher = more urgent)
-  //                         NULL, // Task handle
+                          NULL, // Task handle
-  //                         1     // Core ID
+                          1     // Core ID
-  // );
+  );
 
   setup_imu();
+  servo_init();
   ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
 
   Eigen::Vector3f local_pos = {0, 0, 0};
@@ -83,7 +84,7 @@ extern "C" void app_main(void) {
 
   while (true) {
     while (packet_rx_queue &&
-           xQueueReceive(packet_rx_queue, &packet_data[0], 1)) {
+           xQueueReceive(packet_rx_queue, &packet_data[0], 20)) {
       handle_packet(&packet_data[0]);
     }
 
@@ -124,15 +125,20 @@ extern "C" void app_main(void) {
       time_activation_queue = millis();
     }
 
-    if (released &&
+    if (released && drone_cont &&
         std::atomic_load(&drone_cont->current_input_mode) ==
             INPUT_TYPE::AUTO_NAV &&
         millis() - time_activation_queue > TIME_RELEASE_QUEUE_TO_ACTIVATION) {
 
       dcont::reset_pid_states(drone_cont->drone_controller);
+      servo_set(SERVO_OPTIONS::UP);
+
+      xTaskCreate(
+          [](void *pvParameters) {
+            vTaskDelay(pdMS_TO_TICKS(700));
             std::atomic_store(&killswitch_active, false);
-      // std::atomic_store(&drone_cont->current_input_mode,
+          },
-      // INPUT_TYPE::AUTO_NAV);
+          "lambda_task", 2048, NULL, 5, NULL);
     }
 
     // Logging
@@ -203,6 +209,6 @@ extern "C" void app_main(void) {
       //          imu_state_var.rot_euler.y(), imu_state_var.rot_euler.z());
     }
 
-    vTaskDelay(pdMS_TO_TICKS(50));
+    vTaskDelay(pdMS_TO_TICKS(5));
   }
 }

main/packet_handler.cpp

@@ -13,6 +13,7 @@
 #include "portmacro.h"
 #include "radio.h"
 #include "sens_fus.h"
+#include <atomic>
 #include <cstdint>
 #include <sys/unistd.h>
 
@@ -49,7 +50,10 @@ void handle_packet(uint8_t *packet_addr) {
   } else if (packet_type == PACKET_TYPE::KILLSWITCH_TOGGLE) {
     packet_killswitch_toggle *packet =
         (packet_killswitch_toggle *)(packet_addr + sizeof(PACKET_TYPE));
-    killswitch_active = packet->killswitch_active;
+    std::atomic_store(&killswitch_active, packet->killswitch_active);
+    ESP_LOGI("RADIO_RX", "KILLSWITCH PAACKET");
+    ESP_LOGE("switch_set", "killswitch set to: %d, packet: %d",
+             atomic_load(&killswitch_active), packet->killswitch_active);
 
   } else if (packet_type == PACKET_TYPE::MODE_INPUT) {
     packet_mode_input *packet =
@@ -106,9 +110,6 @@ void send_packet_getter(PACKET_TYPE requested_type) {
         packet_info_drone_position{
             .trans = {sens_fus.position.x(), sens_fus.position.y(),
                       sens_fus.position.z()},
-            .accel = {imu_state_var.lin_accel_global.x,
-                      imu_state_var.lin_accel_global.y,
-                      imu_state_var.lin_accel_global.z},
             .vel = {local_vel.x(), local_vel.y(), local_vel.z()},
             .rot = {imu_state_var.rot.w(), imu_state_var.rot.x(),
                     imu_state_var.rot.y(), imu_state_var.rot.z()},
@@ -147,14 +148,16 @@ void send_packet_getter(PACKET_TYPE requested_type) {
     uint8_t index = requested_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0;
 
     Eigen::Vector3f coords;
+    bool land = false;
     if (xSemaphoreTake(nav_mutex, portMAX_DELAY)) {
       coords = nav_man.waypoints[index].coords;
+      land = nav_man.waypoints[index].landing;
       xSemaphoreGive(nav_mutex);
     }
 
     resp_packet = create_packet_pooled(
         requested_type,
-        packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}});
+        packet_drone_nav_waypoint{{coords[0], coords[1], coords[2]}, land});
   }
 
   if (resp_packet.first != nullptr) {

main/radio.cpp

@@ -61,7 +61,7 @@ void radio_task(void *pvParameters) {
   if (radio.initialize(FREQUENCY, NODEID, NETWORKID)) {
     radio.setPowerLevel(31);
     radio.setHighPower(true);
-    radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
+    // radio.setCustomBitrate(DEFAULT_COMMS_BITRATE);
     ESP_LOGI(TAG, "Radio Initialized. Version: 0x%02X", radio.readReg(0x10));
     radio.readAllRegsCompact();
 
@@ -102,6 +102,7 @@ void radio_task(void *pvParameters) {
         ESP_LOGI(TAG, "Datarate change requested: %d. Switching in 100ms...",
                  target_bitrate);
       } else {
+        ESP_LOGI(TAG, "RECVD PACKET");
         xQueueSend(packet_rx_queue, &packet_data[0], portMAX_DELAY);
       }
 
@@ -139,6 +140,6 @@ void radio_task(void *pvParameters) {
       confirmed_at_new_rate = false;
     }
 
-    vTaskDelay(pdMS_TO_TICKS(10));
+    vTaskDelay(pdMS_TO_TICKS(1));
   }
 }

main/servo.cpp

@@ -1,11 +1,8 @@
+#include "servo.h"
 
 #include "driver/ledc.h"
-#include "esp_log_level.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
-#include <stdio.h>
-
-#include "esp_log.h"
 
 #define SERVO_PIN 7
 #define LEDC_TIMER LEDC_TIMER_0
@@ -39,13 +36,7 @@ void servo_init() {
 #define DUTY_CYCLE_DOWN 2000
 #define DUTY_CYCLE_UP 850
 
-enum SERVO_OPTIONS {
+void servo_set(SERVO_OPTIONS opt) {
-  UP,
-  DOWN,
-  OFF,
-};
-
-void set_servo(SERVO_OPTIONS opt) {
   switch (opt) {
   case UP:
     ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_UP));

main/servo.h

@@ -0,0 +1,11 @@
+#pragma once
+
+enum SERVO_OPTIONS {
+  UP,
+  DOWN,
+  OFF,
+};
+
+void servo_init();
+
+void servo_set(SERVO_OPTIONS opt);