From ead80f33614f085ebbafbbb592c30f8101ffcc41 Mon Sep 17 00:00:00 2001 From: franchioping Date: Sun, 19 Apr 2026 15:50:02 +0100 Subject: [PATCH] communication issues when using bi directional in radio --- components/DShotRMT | 2 +- components/RFM69_LowPowerLab | 2 +- components/drone_comms | 2 +- components/drone_controller | 2 +- components/esp32_Adafruit_GPS | 2 +- main/drone.cpp | 7 +++--- main/main.cpp | 40 ++++++++++++++++++++--------------- main/packet_handler.cpp | 13 +++++++----- main/radio.cpp | 5 +++-- main/servo.cpp | 13 ++---------- main/servo.h | 11 ++++++++++ 11 files changed, 55 insertions(+), 44 deletions(-) diff --git a/components/DShotRMT b/components/DShotRMT index 4f731ed..86d6823 160000 --- a/components/DShotRMT +++ b/components/DShotRMT @@ -1 +1 @@ -Subproject commit 4f731edaead51a644270d2bf183a8a43cae33454 +Subproject commit 86d682313234556720dc63ad53aace47ab3d190c diff --git a/components/RFM69_LowPowerLab b/components/RFM69_LowPowerLab index 034caf2..9b083db 160000 --- a/components/RFM69_LowPowerLab +++ b/components/RFM69_LowPowerLab @@ -1 +1 @@ -Subproject commit 034caf2f11a15c67a8a1985b9a02ac3d5026671b +Subproject commit 9b083db90723c4911ac79edb6c1aa35c4c127cb8 diff --git a/components/drone_comms b/components/drone_comms index d58740e..bb38f95 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit d58740e83368c9f2cc2b641ff729d04e7033d007 +Subproject commit bb38f95e0de4868b2f84b3fa8ab3025f3496e37b diff --git a/components/drone_controller b/components/drone_controller index ac12715..3504bc4 160000 --- a/components/drone_controller +++ b/components/drone_controller @@ -1 +1 @@ -Subproject commit ac127154714cf9ce781757d7203651fed214b5ff +Subproject commit 3504bc479075bbc363697edc4123978e4342c823 diff --git a/components/esp32_Adafruit_GPS b/components/esp32_Adafruit_GPS index 3df2e79..06ee1ee 160000 --- a/components/esp32_Adafruit_GPS +++ b/components/esp32_Adafruit_GPS @@ -1 +1 @@ -Subproject commit 3df2e79d866d1a46fe79f9094a8647a3fd8c5c40 +Subproject commit 06ee1eea5a5073337591edc713a755306cea1331 diff --git a/main/drone.cpp b/main/drone.cpp index c10b3cd..517ada8 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -18,8 +18,7 @@ #include "nav.h" #include "packet_handler.h" #include "sens_fus.h" -#include "soc/gpio_num.h" -#include +#include "soc/gpio_num.h" #include < cstdint> #include #include #include @@ -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); } diff --git a/main/main.cpp b/main/main.cpp index 99b40fa..1ddea9d 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -3,8 +3,7 @@ #include "drone.h" #include "drone_comms.h" #include "drone_controller.h" -#include "esp32-hal.h" -#include "esp_log.h" +#include "esp32-hal.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 - // "motor_throttles_task", // Name for debugging - // 1024 * 4, // Stack size in bytes - // NULL, // Parameters - // 24, // Priority (higher = more urgent) - // NULL, // Task handle - // 1 // Core ID - // ); + xTaskCreatePinnedToCore(motor_throttles_task, // Function name + "motor_throttles_task", // Name for debugging + 1024 * 4, // Stack size in bytes + NULL, // Parameters + 24, // Priority (higher = more urgent) + NULL, // Task handle + 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); - std::atomic_store(&killswitch_active, false); - // std::atomic_store(&drone_cont->current_input_mode, - // INPUT_TYPE::AUTO_NAV); + servo_set(SERVO_OPTIONS::UP); + + xTaskCreate( + [](void *pvParameters) { + vTaskDelay(pdMS_TO_TICKS(700)); + std::atomic_store(&killswitch_active, false); + }, + "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)); } } diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 39aff4d..a006a76 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -13,6 +13,7 @@ #include "portmacro.h" #include "radio.h" #include "sens_fus.h" +#include #include #include @@ -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) { diff --git a/main/radio.cpp b/main/radio.cpp index ac59eeb..da94342 100644 --- a/main/radio.cpp +++ b/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)); } } diff --git a/main/servo.cpp b/main/servo.cpp index 21b747d..9738462 100644 --- a/main/servo.cpp +++ b/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 - -#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 { - UP, - DOWN, - OFF, -}; - -void set_servo(SERVO_OPTIONS opt) { +void servo_set(SERVO_OPTIONS opt) { switch (opt) { case UP: ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, us_to_duty(DUTY_CYCLE_UP)); diff --git a/main/servo.h b/main/servo.h index e69de29..9c6914b 100644 --- a/main/servo.h +++ b/main/servo.h @@ -0,0 +1,11 @@ +#pragma once + +enum SERVO_OPTIONS { + UP, + DOWN, + OFF, +}; + +void servo_init(); + +void servo_set(SERVO_OPTIONS opt);