diff --git a/components/drone_controller b/components/drone_controller index 11a55db..ac12715 160000 --- a/components/drone_controller +++ b/components/drone_controller @@ -1 +1 @@ -Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2 +Subproject commit ac127154714cf9ce781757d7203651fed214b5ff diff --git a/main/drone.cpp b/main/drone.cpp index db0ae89..c3eb8e8 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -1,17 +1,26 @@ #include "drone.h" +#include "DShotRMT.h" +#include "Eigen/Core" +#include "drone_comms.h" #include "drone_controller.h" +#include "dshot_definitions.h" #include "esp32-hal.h" #include "freertos/FreeRTOS.h" #include "freertos/idf_additions.h" #include "freertos/projdefs.h" #include "freertos/task.h" #include "imu.h" +#include "nav.h" +#include "packet_handler.h" #include "sens_fus.h" +#include "soc/gpio_num.h" #include +#include +#include -#define CONTROLLER_TASK_FREQUENCY 400.0; +#define CONTROLLER_TASK_FREQUENCY 1000.0; dcont::ControllerConfig default_config() { dcont::ControllerConfig config; @@ -39,7 +48,7 @@ dcont::ControllerConfig default_config() { config.stack.rate_pid = {{0.2f, 0.2f, 2.0f}, {0.05f, 0.05f, 0.05f}, {0.003f, 0.003f, 0.001f}, - 600.0f}; + 1000.0f}; // 2. Set Constraints config.stack.max_rate = 3.14f; // ~180 degrees/s @@ -69,6 +78,13 @@ dcont::ControllerConfig default_config() { void setup_drone() { drone_controller = dcont::create(default_config()); } +void drone_cont_stabilize() { + // TODO: Implement stabilization. if |angvel| > something, we should make it 0 + // first, then if falling fast, pull rotation to the side, then when falling + // is stabilized, turn slowly up until pointing up. let velocity dissipate + // afterwards +} + void drone_controller_task(void *params) { setup_drone(); uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY; @@ -94,6 +110,69 @@ void drone_controller_task(void *params) { dcont::set_cur_pos(drone_controller, v3f_to_v3c(position_local)); dcont::set_cur_rot(drone_controller, imu_state_local.rot); + packet_controller_input cont_input; + if (current_input_mode == dcont::ModeInput::Acro && + xSemaphoreTake(controller_input_semaphore, 10)) { + cont_input = current_controller_input; + + xSemaphoreGive(controller_input_semaphore); + } + + waypoint wayp; + if (current_input_mode == dcont::ModeInput::Position && + xSemaphoreTake(nav_mutex, 10)) { + wayp = nav_man.get_current_waypoint(); + + xSemaphoreGive(nav_mutex); + } + if (current_input_mode == dcont::ModeInput::Position && + wayp.coords_in_axis == std::nullopt) { + drone_cont_stabilize(); + } else { + auto coords = wayp.coords_in_axis.value_or(Eigen::Vector3f::Zero()); + + dcont::set_input(drone_controller, + dcont::Input{{cont_input.ly, cont_input.lx, + cont_input.rx, cont_input.ry}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {0.0, 0.0, 0.0}, + {coords.x(), coords.y(), coords.z()}, + current_input_mode}); + } + + memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, + sizeof(motor_throttles)); + vTaskDelay(pdMS_TO_TICKS(wait_ms)); } } + +const gpio_num_t motor_pins[4] = {GPIO_NUM_NC, GPIO_NUM_NC, GPIO_NUM_NC, + GPIO_NUM_NC}; + +void motor_throttles_task(void *params) { + DShotRMT *motors[4]; + for (int i = 0; i < 4; i++) { + motors[i] = new DShotRMT(motor_pins[i], DSHOT150, false); + } + + // ARM + unsigned long armTime = millis(); + while (millis() - armTime < 4000) { + for (int i = 0; i < 4; i++) { + motors[i]->sendThrottlePercent(0); + } + } + + while (true) { + for (int i = 0; i < 4; i++) { + if (!killswitch_active) { + motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f); + } else { + motors[i]->sendThrottlePercent(0.0f); + } + } + vTaskDelay(1); + } +} diff --git a/main/drone.h b/main/drone.h index 12a6e33..82ca379 100644 --- a/main/drone.h +++ b/main/drone.h @@ -22,3 +22,8 @@ inline dcont::StackedController *drone_controller = nullptr; inline dcont::ModeInput current_input_mode; void drone_controller_task(void *params); + +void motor_throttles_task(void *params); + +inline float motor_throttles[4]; +inline bool killswitch_active; diff --git a/main/gps.h b/main/gps.h index f487b37..5ce8b7c 100644 --- a/main/gps.h +++ b/main/gps.h @@ -19,7 +19,6 @@ const float TO_RAD = M_PI / 180.0f; const float KNOTS_TO_M_SEC = 0.5144444f; - const float earth_radius = 6371000.0f; struct GPS { @@ -73,8 +72,8 @@ struct GPS { } std::optional get_coordinates() { - if (this->gps->fix == false || this->gps->latitudeDegrees == 0.0 || - this->gps->longitudeDegrees != 0.0) { + if (this->gps->fix == false || (this->gps->latitudeDegrees == 0.0 && + this->gps->longitudeDegrees == 0.0)) { return std::nullopt; } float latitude = this->gps->latitudeDegrees; diff --git a/main/imu.cpp b/main/imu.cpp index e40bbc2..799ef91 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -33,7 +33,7 @@ void setup_imu() { imu->rpt.rv_game.enable(2500UL); imu->rpt.linear_accelerometer.enable(2500UL); - imu->rpt.cal_gyro.enable(2500UL); + imu->rpt.cal_gyro.enable(1000UL); imu->register_cb([imu, local_state]() { bool needs_updating = false; diff --git a/main/main.cpp b/main/main.cpp index 4d7206a..5d697b7 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -44,6 +44,10 @@ extern "C" void app_main(void) { 0 // Core ID ); + xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL); + + xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL); + xTaskCreatePinnedToCore(drone_controller_task, // Function name "drone_controller_task", // Name for debugging 1024 * 32, // Stack size in bytes @@ -53,56 +57,66 @@ extern "C" void app_main(void) { 1 // Core ID ); - xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL); - - xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL); + xTaskCreatePinnedToCore(motor_throttles_task, // Function name + "motor_throttles_task", // Name for debugging + 1024 * 4, // Stack size in bytes + NULL, // Parameters + 20, // Priority (higher = more urgent) + NULL, // Task handle + 1 // Core ID + ); ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); Eigen::Vector3f local_pos = {0, 0, 0}; Eigen::Vector3f local_vel = {0, 0, 0}; bool nav_data_ready = false; + + uint64_t last_print_time = 0; while (true) { while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { handle_packet(&packet_data[0]); } - std::optional coords; - float lat, lon, alt; - if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { - if (gps->gps_avaliable()) { - coords = gps->get_coordinates(); - lat = gps->gps->latitudeDegrees; - lon = gps->gps->longitudeDegrees; - alt = gps->gps->altitude; + if (millis() > last_print_time + 1000) { + + std::optional coords; + float lat, lon, alt; + if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) { + if (gps->gps_avaliable()) { + coords = gps->get_coordinates(); + lat = gps->gps->latitudeDegrees; + lon = gps->gps->longitudeDegrees; + alt = gps->gps->altitude; + } + xSemaphoreGive(gps_mutex); + + if (coords.has_value()) { + auto D_pos = coords.value(); + + ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", lat, lon, alt); + ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1], + D_pos[2]); + } } - xSemaphoreGive(gps_mutex); + env_sens::dbg_sens(); - if (coords.has_value()) { - auto D_pos = coords.value(); + if (sens_fus_mutex && + xSemaphoreTake(sens_fus_mutex, pdMS_TO_TICKS(10)) == pdTRUE) { + local_pos = sens_fus.position; + local_vel = sens_fus.velocity; + nav_data_ready = true; + xSemaphoreGive(sens_fus_mutex); + } - ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f", lat, lon, alt); - ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1], - D_pos[2]); + if (nav_data_ready) { + ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1], + local_pos[2]); + ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1], + local_vel[2]); } } - env_sens::dbg_sens(); - if (sens_fus_mutex && - xSemaphoreTake(sens_fus_mutex, pdMS_TO_TICKS(10)) == pdTRUE) { - local_pos = sens_fus.position; - local_vel = sens_fus.velocity; - nav_data_ready = true; - xSemaphoreGive(sens_fus_mutex); - } - - if (nav_data_ready) { - ESP_LOGI(TAG, "nav(pos): (%f, %f, %f)", local_pos[0], local_pos[1], - local_pos[2]); - ESP_LOGI(TAG, "nav(vel): (%f, %f, %f)", local_vel[0], local_vel[1], - local_vel[2]); - } - - vTaskDelay(pdMS_TO_TICKS(1000)); + vTaskDelay(pdMS_TO_TICKS(10)); } } diff --git a/main/nav.h b/main/nav.h index 1c3f49d..3852e15 100644 --- a/main/nav.h +++ b/main/nav.h @@ -2,7 +2,10 @@ #include "Eigen/Core" #include "freertos/idf_additions.h" +#include "gps.h" +#include #include +#include #ifdef PS #undef PS @@ -17,8 +20,9 @@ #define WAYPOINT_COUNT 8 struct waypoint { - Eigen::Vector3f coords; // lon, lat, alt - bool active; // active or to be skipped + Eigen::Vector3f coords; // lat, lon, alt + std::optional coords_in_axis = std::nullopt; + bool active = false; // active or to be skipped }; struct drone_nav { @@ -40,6 +44,16 @@ struct drone_nav { } return mask; } + + waypoint get_current_waypoint() { + waypoint wayp = this->waypoints[this->current_waypoint]; + if (!wayp.coords_in_axis.has_value()) { + auto axis = + gps->waypoint_to_xyz(wayp.coords[0], wayp.coords[1], wayp.coords[2]); + wayp.coords_in_axis = axis; + } + return wayp; + } }; inline SemaphoreHandle_t nav_mutex = NULL; diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index fd95e92..231346d 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -1,7 +1,10 @@ #include "packet_handler.h" #include "Eigen/Core" +#include "drone.h" #include "drone_comms.h" +#include "drone_controller.h" +#include "esp_log.h" #include "freertos/idf_additions.h" #include "gps.h" #include "nav.h" @@ -32,13 +35,38 @@ void handle_packet(uint8_t *packet_addr) { } // NAV SETTERS - if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 && - packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) { + else if (packet_type >= PACKET_TYPE::DRONE_NAV_WAYPOINT_0 && + packet_type <= PACKET_TYPE::DRONE_NAV_WAYPOINT_7) { uint8_t index = packet_type - PACKET_TYPE::DRONE_NAV_WAYPOINT_0; handle_waypoint_update(packet_addr, index); - } - if (packet_type == PACKET_TYPE::DRONE_NAV) { + } else if (packet_type == PACKET_TYPE::DRONE_NAV) { handle_nav_update(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; + } else if (packet_type == PACKET_TYPE::MODE_INPUT) { + packet_mode_input *packet = + (packet_mode_input *)(packet_addr + sizeof(PACKET_TYPE)); + switch (packet->input_type) { + case INPUT_TYPE::ACRO: + current_input_mode = dcont::ModeInput::Acro; + break; + case INPUT_TYPE::LEVEL: + case INPUT_TYPE::STABILIZE_FALL: + ESP_LOGE("PACKET_HANDLER", + "INPUT TYPES NOT IMPLEMENTED. DEFAULTING TO POSITION (AUTONAV)"); + case INPUT_TYPE::AUTO_NAV: + current_input_mode = dcont::ModeInput::Position; + } + } else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) { + packet_controller_input *packet = + (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE)); + + if (xSemaphoreTake(controller_input_semaphore, 10)) { + current_controller_input = *packet; + xSemaphoreGive(controller_input_semaphore); + } } } diff --git a/main/packet_handler.h b/main/packet_handler.h index e22d46a..1192fb4 100644 --- a/main/packet_handler.h +++ b/main/packet_handler.h @@ -2,4 +2,10 @@ #include +#include "drone_comms.h" +#include "freertos/idf_additions.h" + void handle_packet(uint8_t *packet_addr); + +inline SemaphoreHandle_t controller_input_semaphore = NULL; +inline packet_controller_input current_controller_input; diff --git a/main/radio.h b/main/radio.h index dcb058c..4849aa8 100644 --- a/main/radio.h +++ b/main/radio.h @@ -5,7 +5,5 @@ inline QueueHandle_t packet_rx_queue = NULL; inline QueueHandle_t packet_tx_queue = NULL; -inline SemaphoreHandle_t controller_input_semaphore = NULL; -inline packet_controller_input current_controller_input; void radio_task(void *pvParameters);