diff --git a/components/drone_comms b/components/drone_comms index bb38f95..24803f3 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit bb38f95e0de4868b2f84b3fa8ab3025f3496e37b +Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20 diff --git a/main/drone.cpp b/main/drone.cpp index 517ada8..7db37ad 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -18,7 +18,9 @@ #include "nav.h" #include "packet_handler.h" #include "sens_fus.h" -#include "soc/gpio_num.h" #include < cstdint> +#include "soc/gpio_num.h" +#include +#include #include #include #include @@ -65,11 +67,12 @@ dcont::ControllerConfig default_config() { config.max_torque = 0.5f; // Nm float mixer[4][3] = { - // roll, pitch, yaw - {-1.0, -1.0, -1.0}, // Front Left - {1.0, 1.0, -1.0}, // Rear Right + // x, y, z + + {-1.0, -1.0, -1.0}, // Rear Right {-1.0, 1.0, 1.0}, // Rear Left {1.0, -1.0, 1.0}, // Front Right + {1.0, 1.0, -1.0}, // Front Left }; for (int i = 0; i < 4; i++) { @@ -90,13 +93,16 @@ void drone_controller_task(void *params) { while (true) { drone_cont->update(); - vTaskDelay(pdMS_TO_TICKS(wait_ms)); + vTaskDelay(pdMS_TO_TICKS(1)); } } const gpio_num_t motor_pins[4] = {GPIO_NUM_46, GPIO_NUM_16, GPIO_NUM_14, GPIO_NUM_15}; +// const gpio_num_t motor_pins[4] = {GPIO_NUM_15, GPIO_NUM_14, GPIO_NUM_46, +// GPIO_NUM_16}; + DShotRMT *motors[4]; void motor_throttles_task(void *params) { motor_throttles = (float *)malloc(sizeof(float) * 4); @@ -118,7 +124,8 @@ void motor_throttles_task(void *params) { while (true) { for (int i = 0; i < 4; i++) { - float throttle = motor_throttles[i] * 100.0f * 0.7f; + float throttle = + std::clamp(motor_throttles[i], 0.0f, 1.0f) * 100.0f * 0.3f; if (atomic_load(&killswitch_active)) { throttle = 0.0; } diff --git a/main/drone.h b/main/drone.h index a9b7371..19cfc60 100644 --- a/main/drone.h +++ b/main/drone.h @@ -1,5 +1,6 @@ #pragma once +#include "esp_log.h" #include "nav.h" #include "esp32-hal.h" @@ -142,8 +143,11 @@ struct drone_cont_state { } void update_throttles() { - memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, - sizeof(float) * 4); + auto thrt_var = dcont::get_throttles(drone_controller); + auto thrt = thrt_var.values; + memcpy(motor_throttles, thrt, sizeof(float) * 4); + // ESP_LOGI("CONT", "thr: %f, %f, %f, %f", thrt[0], thrt[1], thrt[2], + // thrt[3]); ESP_LOGE("CONT", "UPDATE THROTTLES"); } void update_input() { @@ -153,9 +157,9 @@ struct drone_cont_state { switch (atomic_load(&this->current_input_mode)) { case INPUT_TYPE::ACRO: { if (xSemaphoreTake(controller_input_semaphore, 10)) { - if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) { - current_controller_input = {0, 0, 0, 0}; - } + // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) { + // current_controller_input = {0, 0, 0, 0}; + // } cont_input = current_controller_input; xSemaphoreGive(controller_input_semaphore); diff --git a/main/imu.cpp b/main/imu.cpp index e998609..624a932 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -89,6 +89,7 @@ BNO08x *setup_imu() { auto cal_gyro = imu->rpt.cal_gyro.get(); local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z}; + // ESP_LOGI("ROT", "angvel_z: %f", cal_gyro.z); } if (imu->rpt.linear_accelerometer.has_new_data()) { diff --git a/main/main.cpp b/main/main.cpp index 1ddea9d..c9f0b96 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -9,8 +9,7 @@ #include "freertos/projdefs.h" #include "freertos/task.h" #include -#include -#include +#include #include #include #include "env_sens.h" @@ -50,14 +49,14 @@ extern "C" void app_main(void) { xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); - // xTaskCreatePinnedToCore(drone_controller_task, // Function name - // "drone_controller_task", // Name for debugging - // 1024 * 32, // Stack size in bytes - // NULL, // Parameters - // 20, // Priority (higher = more urgent) - // NULL, // Task handle - // 1 // Core ID - // ); + xTaskCreatePinnedToCore(drone_controller_task, // Function name + "drone_controller_task", // Name for debugging + 1024 * 32, // Stack size in bytes + NULL, // Parameters + 20, // Priority (higher = more urgent) + NULL, // Task handle + 1 // Core ID + ); xTaskCreatePinnedToCore(motor_throttles_task, // Function name "motor_throttles_task", // Name for debugging @@ -68,6 +67,25 @@ extern "C" void app_main(void) { 1 // Core ID ); + // vTaskDelay(5000); + // for (int i = 0; i < 4; i++) { + // motor_throttles[i] = 10.0; + // vTaskDelay(2000); + // motor_throttles[i] = 0.0; + // } + + xTaskCreate( + [](void *pvParameters) { + while (true) { + while (packet_rx_queue && + xQueueReceive(packet_rx_queue, &packet_data[0], 20)) { + handle_packet(&packet_data[0]); + } + vTaskDelay(1); + } + }, + "lambda_recv_task", 8192, NULL, 5, NULL); + setup_imu(); servo_init(); ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); @@ -83,10 +101,6 @@ extern "C" void app_main(void) { bool released = false; while (true) { - while (packet_rx_queue && - xQueueReceive(packet_rx_queue, &packet_data[0], 20)) { - handle_packet(&packet_data[0]); - } if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) { send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION); diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 431a070..21cd02e 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -113,8 +113,7 @@ void send_packet_getter(PACKET_TYPE requested_type) { .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()}, - .angvel = {imu_state_var.angvel.x(), imu_state_var.angvel.y(), - imu_state_var.angvel.z()}}); + }); } if (requested_type == PACKET_TYPE::INFO_DRONE_STATUS) {