diff --git a/components/drone_comms b/components/drone_comms index 350c761..ac25a39 160000 --- a/components/drone_comms +++ b/components/drone_comms @@ -1 +1 @@ -Subproject commit 350c761b9fe11f551e5c8ddf11e84252d2f496b4 +Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2 diff --git a/main/drone.cpp b/main/drone.cpp index 11fe2ba..0a53682 100644 --- a/main/drone.cpp +++ b/main/drone.cpp @@ -2,6 +2,7 @@ #include "DShotRMT.h" #include "Eigen/Core" +#include "Eigen/Geometry" #include "driver/rmt_tx.h" #include "drone_comms.h" #include "drone_controller.h" @@ -81,80 +82,14 @@ dcont::ControllerConfig default_config() { return 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 -} - constexpr uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY; void drone_controller_task(void *params) { - setup_drone(); - - imu_state imu_state_local; - Eigen::Vector3f position_local = Eigen::Vector3f::Zero(); - Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero(); + drone_cont = new drone_cont_state; + drone_cont->init(); while (true) { - if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) { - imu_state_local = imu_state_var; - xSemaphoreGive(imu_state_mutex); - } - if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) { - position_local = sens_fus.position; - velocity_local = sens_fus.velocity; - xSemaphoreGive(sens_fus_mutex); - } - - dcont::set_cur_time(drone_controller, millis() / 1000.0f); - dcont::set_cur_angvel(drone_controller, v3f_to_v3c(imu_state_local.angvel)); - dcont::set_cur_linvel(drone_controller, v3f_to_v3c(velocity_local)); - 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)) { - if (millis() - time_last_controller > 1) { - current_controller_input = {0, 0, 0, 0}; - } - 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{.joystick = {.throttle_input = cont_input.ly, - .roll_input = cont_input.rx, - .yaw_input = cont_input.lx, - .pitch_input = cont_input.ry}, - .acceleration = {0.0, 0.0, 0.0}, - .rotation = {0.0, 0.0, 0.0}, - .velocity = {0.0, 0.0, 0.0}, - .position = {coords.x(), coords.y(), coords.z()}, - .mode = current_input_mode}); - } - - // memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, - // sizeof(motor_throttles)); + drone_cont->update(); vTaskDelay(pdMS_TO_TICKS(wait_ms)); } @@ -184,7 +119,12 @@ void motor_throttles_task(void *params) { while (true) { for (int i = 0; i < 4; i++) { - motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f); + float throttle = motor_throttles[i] * 100.0f * 0.7f; + if (atomic_load(&killswitch_active)) { + throttle = 0.0; + } + // motors[i]->sendThrottlePercent(motor_throttles[i] * 100.0f * 0.7f); + motors[i]->sendThrottlePercent(10.0f); } vTaskDelay(2); } diff --git a/main/drone.h b/main/drone.h index f368f1d..9c5c1e1 100644 --- a/main/drone.h +++ b/main/drone.h @@ -1,7 +1,10 @@ #pragma once #include "esp32-hal.h" +#include +#include #include +#include #ifdef PS #undef PS #endif @@ -15,17 +18,204 @@ #include "drone_comms.h" #include "drone_controller.h" +#include "packet_handler.h" + +#include "imu.h" +#include "nav.h" +#include "packet_handler.h" +#include "sens_fus.h" + +#define CONNECTION_LOST_THRESHOLD 200 + void setup_drone(); inline dcont::Vec3C v3f_to_v3c(Eigen::Vector3f v) { return {v[0], v[1], v[2]}; } -inline dcont::StackedController *drone_controller = nullptr; -inline dcont::ModeInput current_input_mode = dcont::ModeInput::Acro; - void drone_controller_task(void *params); void motor_throttles_task(void *params); // 3 4 2 1 -inline float *motor_throttles; -inline bool killswitch_active = false; +inline float *motor_throttles = nullptr; +inline std::atomic_bool killswitch_active = false; + +dcont::ControllerConfig default_config(); + +struct drone_cont_state { + bool angvel_stablilized; + bool fall_vel_stabilized; + + Eigen::Vector3f pos; + Eigen::Vector3f vel; + Eigen::Quaternionf rot; + Eigen::Vector3f angvel; + + dcont::StackedController *drone_controller; + atomic_uint_fast8_t current_input_mode = INPUT_TYPE::ACRO; + + void init() { this->drone_controller = dcont::create(default_config()); } + + void drone_cont_stabilize() { + // 1. ANGLE VELOCITY STABILIZATION + // Kill the spin first to ensure sensors/control loops can work accurately. + if (!this->angvel_stablilized) { + dcont::set_input(drone_controller, + dcont::Input{.joystick = {.throttle_input = 0.0, + .roll_input = 0.0, + .yaw_input = 0.0, + .pitch_input = 0.0}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {0.0, 0.0, 0.0}, + .mode = dcont::ModeInput::Acro}); + if (this->angvel.norm() < 1.0) { + this->angvel_stablilized = true; + } + } + + // 2. FALL VELOCITY STABILIZATION (WITH PROP WASH AVOIDANCE) + // Instead of climbing straight up, we move at an angle. + if (!this->fall_vel_stabilized) { + dcont::set_input(drone_controller, + dcont::Input{.joystick = {.throttle_input = 1.0, + .roll_input = 0.0, + .yaw_input = 0.0, + .pitch_input = 0.0}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.05 * M_PI, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {0.0, 0.0, 0.0}, + .mode = dcont::ModeInput::Rotation}); + + if (this->vel.z() - 1.0 >= 0) { + this->fall_vel_stabilized = true; + } + } + + dcont::set_input(drone_controller, + dcont::Input{.joystick = {.throttle_input = 0.0, + .roll_input = 0.0, + .yaw_input = 0.0, + .pitch_input = 0.0}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {0.0, 0.0, 0.0}, + .mode = dcont::ModeInput::Velocity}); + } + + inline bool stabilization_done() { + return this->fall_vel_stabilized && this->angvel_stablilized; + } + + void fetch_sens() { + static imu_state imu_state_local; + if (imu_state_mutex && xSemaphoreTake(imu_state_mutex, 1)) { + imu_state_local = imu_state_var; + xSemaphoreGive(imu_state_mutex); + this->angvel = imu_state_local.angvel; + } + if (sens_fus_mutex && xSemaphoreTake(sens_fus_mutex, 1)) { + this->pos = sens_fus.position; + this->vel = sens_fus.velocity; + xSemaphoreGive(sens_fus_mutex); + } + + this->rot = imu_state_var.rot; + } + + void update_controller_sens() { + + dcont::set_cur_time(drone_controller, millis() / 1000.0f); + dcont::set_cur_angvel(drone_controller, v3f_to_v3c(this->angvel)); + dcont::set_cur_linvel(drone_controller, v3f_to_v3c(this->vel)); + dcont::set_cur_pos(drone_controller, v3f_to_v3c(this->pos)); + + dcont::set_cur_rot(drone_controller, dcont::QuatC{.i = this->rot.x(), + .j = this->rot.y(), + .k = this->rot.z(), + .w = this->rot.w()}); + } + + void update_throttles() { + memcpy(dcont::get_throttles(drone_controller).values, motor_throttles, + sizeof(float) * 4); + } + + void update_input() { + + packet_controller_input cont_input; + + 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}; + } + cont_input = current_controller_input; + + xSemaphoreGive(controller_input_semaphore); + + dcont::set_input( + drone_controller, + dcont::Input{.joystick = {.throttle_input = cont_input.ly, + .roll_input = cont_input.rx, + .yaw_input = cont_input.lx, + .pitch_input = cont_input.ry}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {0.0, 0.0, 0.0}, + .mode = dcont::ModeInput::Acro}); + } else { + drone_cont_stabilize(); + } + } break; + + case INPUT_TYPE::AUTO_NAV: { + if (!stabilization_done()) { + drone_cont_stabilize(); + } else if (xSemaphoreTake(nav_mutex, 10)) { + waypoint wayp = nav_man.get_current_waypoint(); + + xSemaphoreGive(nav_mutex); + if (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{.joystick = {.throttle_input = 0.0, + .roll_input = 0.0, + .yaw_input = 0.0, + .pitch_input = 0.0}, + .acceleration = {0.0, 0.0, 0.0}, + .rotation = {0.0, 0.0, 0.0}, + .velocity = {0.0, 0.0, 0.0}, + .position = {coords.x(), coords.y(), coords.z()}, + .mode = dcont::ModeInput::Position}); + } + } else { + drone_cont_stabilize(); + } + break; + } + default: + case INPUT_TYPE::STABILIZE_FALL: { + drone_cont_stabilize(); + break; + } + } + } + void update() { + this->fetch_sens(); + this->update_controller_sens(); + this->update_input(); + this->update_throttles(); + } +}; + +inline drone_cont_state *drone_cont = nullptr; diff --git a/main/env_sens.cpp b/main/env_sens.cpp index eae2bf8..94de0f5 100644 --- a/main/env_sens.cpp +++ b/main/env_sens.cpp @@ -1,11 +1,13 @@ #include "env_sens.h" +#include "nav.h" #include "sens_fus.h" #include "esp_log.h" #include #include #include +#include #include "freertos/idf_additions.h" @@ -113,6 +115,9 @@ void baro_poll_task(void *_) { if (dt > 0.001f) { // Prevent division by zero float current_alt = env_sens::get_altitude(); + if (current_alt == INFINITY) { + continue; + } filtered_alt = (alt_lpf * current_alt) + (1.0f - alt_lpf) * filtered_alt; diff --git a/main/gps.cpp b/main/gps.cpp index 3d3fad9..8519d49 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -52,6 +52,6 @@ void gps_poll_task(void *_) { } } - vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling + vTaskDelay(pdMS_TO_TICKS(5)); // 10Hz polling } } diff --git a/main/gps.h b/main/gps.h index 7b1a6d5..d7195ea 100644 --- a/main/gps.h +++ b/main/gps.h @@ -13,8 +13,7 @@ #include "HardwareSerial.h" #include "esp_log.h" -#include -#include +#include #include #include const float TO_RAD = M_PI / 180.0f; @@ -91,10 +90,12 @@ struct GPS { } void poll() { - char c = this->gps->read(); + for (int i = 0; i < 50; i++) { + char _ = this->gps->read(); + } if (this->gps->newNMEAreceived()) { char *line = this->gps->lastNMEA(); - ESP_LOGI("GPS", "NMEA LINE: %s", line); + // ESP_LOGI("GPS", "NMEA LINE: %s", line); this->gps->parse(line); } } diff --git a/main/imu.cpp b/main/imu.cpp index 8603dd7..06e21be 100644 --- a/main/imu.cpp +++ b/main/imu.cpp @@ -1,5 +1,7 @@ #include "imu.h" +#include "Eigen/Geometry" #include "Esp.h" +#include "drone_controller.h" #ifdef PS #undef PS @@ -28,7 +30,7 @@ static const char *TAG = "IMU"; #define IMU_MISO GPIO_NUM_5 // SDA #define IMU_SCLK GPIO_NUM_6 // SCL -void setup_imu() { +BNO08x *setup_imu() { imu_state *local_state = new imu_state; imu_state_mutex = xSemaphoreCreateMutex(); @@ -46,28 +48,39 @@ void setup_imu() { imu->dynamic_calibration_autosave_enable(); imu->dynamic_calibration_enable(BNO08xCalSel::all); - imu->rpt.rv_game.enable(2500UL); + // imu->rpt.rv_game.enable(2500UL); + imu->rpt.rv.enable(2500UL); + // imu->rpt.cal_magnetometer.enable(10000UL); imu->rpt.linear_accelerometer.enable(2500UL); + // imu->rpt.accelerometer.enable(10000UL); imu->rpt.cal_gyro.enable(2500UL); imu->register_cb([imu, local_state]() { - ESP_LOGI("IMU", "CALLBACK"); + // ESP_LOGI("IMU", "CALLBACK"); bool needs_updating = false; if (sens_fus_mutex == NULL || imu_state_mutex == NULL) return; - if (imu->rpt.rv_game.has_new_data()) { + if (imu->rpt.rv.has_new_data()) { needs_updating = true; - auto sens_rot = imu->rpt.rv_game.get_quat(); - auto sens_euler = imu->rpt.rv_game.get_euler(); - local_state->rot = {sens_rot.i, sens_rot.j, sens_rot.k, - sens_rot.real}; // FIXME: WRONG ROTATION - local_state->rot_euler = {sens_euler.x, -sens_euler.y, -sens_euler.z}; - ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f", - local_state->rot_euler.x(), local_state->rot_euler.y(), - local_state->rot_euler.z()); + auto sens_rot = imu->rpt.rv.get_quat(); + auto sens_euler = imu->rpt.rv.get_euler(); + local_state->rot = + Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k); + + // Eigen::Quaternionf q_global_yaw( + // Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ())); + // local_state->rot = q_global_yaw * local_state->rot; + + local_state->rot.normalize(); + + // {.i = sens_rot.i, .j = sens_rot.j, .k = sens_rot.k, .w = + // sens_rot.real}; local_state->rot_euler = {sens_euler.x, sens_euler.y, + // sens_euler.z}; ESP_LOGI("IMU", "rot: roll %f, pitch %f, yaw %f", + // local_state->rot_euler.x(), local_state->rot_euler.y(), + // local_state->rot_euler.z()); } if (imu->rpt.cal_gyro.has_new_data()) { @@ -75,23 +88,36 @@ void setup_imu() { needs_updating = true; auto cal_gyro = imu->rpt.cal_gyro.get(); - local_state->angvel = {cal_gyro.x, -cal_gyro.y, -cal_gyro.z}; + local_state->angvel = {cal_gyro.x, cal_gyro.y, cal_gyro.z}; } if (imu->rpt.linear_accelerometer.has_new_data()) { needs_updating = true; - auto sens_accel = imu->rpt.linear_accelerometer.get(); - local_state->accel = {sens_accel.x, -sens_accel.y, -sens_accel.z}; + auto sens_accel_lin = imu->rpt.linear_accelerometer.get(); + local_state->lin_accel = {sens_accel_lin.x, sens_accel_lin.y, + sens_accel_lin.z}; + + // ESP_LOGI("IMU-ac", "lin_accel %f,%f,%f", sens_accel_lin.x, + // sens_accel_lin.y, sens_accel_lin.z); + int64_t current_time = esp_timer_get_time(); if (local_state->last_time != -1) { float dt = ((float)(current_time - local_state->last_time)) / 1000000.0f; + + dcont::QuatC quatc{.i = local_state->rot.x(), + .j = local_state->rot.y(), + .k = local_state->rot.z(), + .w = local_state->rot.w()}; + dcont::Vec3C accel_global = - dcont::apply_rot(&local_state->accel, &local_state->rot); + dcont::apply_rot(&local_state->lin_accel, &quatc); + + local_state->lin_accel_global = accel_global; if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) { @@ -120,4 +146,5 @@ void setup_imu() { }); ESP_LOGI(TAG, "IMU Setup Success."); + return imu; } diff --git a/main/imu.h b/main/imu.h index 52b457b..e8e3793 100644 --- a/main/imu.h +++ b/main/imu.h @@ -25,14 +25,16 @@ #include struct imu_state { - dcont::Vec3C accel = {0, 0, 0}; - dcont::QuatC rot = {0, 0, 0, 1}; + dcont::Vec3C lin_accel = {0, 0, 0}; + dcont::Vec3C lin_accel_global = {0, 0, 0}; + // dcont::Vec3C accel = {0, 0, 0}; + Eigen::Quaternionf rot = {0, 0, 0, 1}; int64_t last_time = -1; Eigen::Vector3f angvel; Eigen::Vector3f rot_euler; }; -void setup_imu(); +BNO08x *setup_imu(); inline SemaphoreHandle_t imu_state_mutex = NULL; inline imu_state imu_state_var; diff --git a/main/main.cpp b/main/main.cpp index b51290e..c3c8a8a 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -1,12 +1,13 @@ #include "driver/gpio.h" -#include "drone.h" -#include "drone_comms.h" +#include "drone.h" #include "drone_comms.h" +#include "drone_controller.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 #include #include @@ -20,6 +21,8 @@ static const char *TAG = "MAIN"; +#define TIME_RELEASE_QUEUE_TO_ACTIVATION 1000 + extern "C" void app_main(void) { sens_fus_mutex = xSemaphoreCreateMutex(); @@ -30,20 +33,19 @@ extern "C" void app_main(void) { gpio_install_isr_service(0); Serial.begin(115200); - // xTaskCreatePinnedToCore(radio_task, // Function name - // "radio_rxtx", // Name for debugging - // 4096, // Stack size in bytes - // NULL, // Parameters - // 5, // Priority (higher = more urgent) - // NULL, // Task handle - // 0 // Core ID - // ); - // - // xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, - // 1, - // NULL, 0); - // - // xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); + xTaskCreatePinnedToCore(radio_task, // Function name + "radio_rxtx", // Name for debugging + 4096, // Stack size in bytes + NULL, // Parameters + 5, // Priority (higher = more urgent) + NULL, // Task handle + 0 // Core ID + ); + + xTaskCreatePinnedToCore(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 5, + NULL, 0); + + xTaskCreatePinnedToCore(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL, 0); // xTaskCreatePinnedToCore(drone_controller_task, // Function name // "drone_controller_task", // Name for debugging @@ -63,7 +65,7 @@ extern "C" void app_main(void) { // 1 // Core ID // ); - // setup_imu(); + setup_imu(); ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); Eigen::Vector3f local_pos = {0, 0, 0}; @@ -71,20 +73,49 @@ extern "C" void app_main(void) { bool nav_data_ready = false; uint64_t last_print_time = 0; - uint64_t last_broadcast_time = 0; + uint64_t last_position_broadcast_time = 0; + uint64_t last_status_broadcast_time = 0; + uint64_t time_activation_queue = 0; + bool released = false; while (true) { - while (packet_tx_queue && - xQueueReceive(packet_tx_queue, &packet_data[0], 1)) { + while (packet_rx_queue && + xQueueReceive(packet_rx_queue, &packet_data[0], 1)) { handle_packet(&packet_data[0]); } - if (millis() > last_broadcast_time + 100) { + if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) { send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION); - last_broadcast_time = millis(); + last_position_broadcast_time = millis(); + } + // auto vel = sens_fus.velocity; + // ESP_LOGI("GPSvsIMU", "gps_vel: %f, imu_vel: %f", + // gps->velocity().value().norm(), + // Eigen::Vector3f(vel.x(), vel.y(), 0.0).norm()); + + if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) { + send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS); + last_status_broadcast_time = millis(); } - if (millis() > last_print_time + 5000) { + if (imu_state_var.lin_accel_global.z < -8.0 && !released) { + released = true; + time_activation_queue = millis(); + } + + if (released && + 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); + } + + // Logging + if (millis() > last_print_time + 1000) { last_print_time = millis(); std::optional coords; @@ -135,12 +166,20 @@ extern "C" void app_main(void) { local_vel[2]); } - ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0], - motor_throttles[1], motor_throttles[2], motor_throttles[3]); + if (motor_throttles != nullptr) { - ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)", - current_controller_input.lx, current_controller_input.ly, - current_controller_input.rx, current_controller_input.ry); + ESP_LOGI(TAG, "Throttles: (%f, %f, %f, %f)", motor_throttles[0], + motor_throttles[1], motor_throttles[2], motor_throttles[3]); + } + + if (time_last_controller - millis() < CONNECTION_LOST_THRESHOLD) { + + ESP_LOGI(TAG, "Controller: (%f, %f), (%f, %f)", + current_controller_input.lx, current_controller_input.ly, + current_controller_input.rx, current_controller_input.ry); + } + // ESP_LOGI(TAG, "ROT: (%f, %f, %f)", imu_state_var.rot_euler.x(), + // imu_state_var.rot_euler.y(), imu_state_var.rot_euler.z()); } vTaskDelay(pdMS_TO_TICKS(50)); diff --git a/main/packet_handler.cpp b/main/packet_handler.cpp index 597a97d..fd0c55d 100644 --- a/main/packet_handler.cpp +++ b/main/packet_handler.cpp @@ -42,26 +42,20 @@ void handle_packet(uint8_t *packet_addr) { 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); + } 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; - } + atomic_store(&drone_cont->current_input_mode, packet->input_type); + } else if (packet_type == PACKET_TYPE::CONTROLLER_INPUT) { packet_controller_input *packet = (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE)); @@ -104,16 +98,19 @@ void send_packet_getter(PACKET_TYPE requested_type) { std::pair resp_packet = {nullptr, 0}; if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) { - // TODO: send pitch, roll, yaw + + Eigen::Vector3f local_vel = imu_state_var.rot.inverse() * sens_fus.velocity; + resp_packet = create_packet_pooled( PACKET_TYPE::INFO_DRONE_POSITION, packet_info_drone_position{ .trans = {sens_fus.position.x(), sens_fus.position.y(), sens_fus.position.z()}, - .vel = {sens_fus.velocity.x(), sens_fus.velocity.y(), - sens_fus.velocity.z()}, - .rot = {imu_state_var.rot_euler.x(), imu_state_var.rot_euler.y(), - imu_state_var.rot_euler.z()}, + .accel = {imu_state_var.lin_accel.x, imu_state_var.lin_accel.y, + imu_state_var.lin_accel.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()}, .pressure = env_sens::get_pressure(), .temperature = env_sens::get_temperature()}); } @@ -126,7 +123,11 @@ void send_packet_getter(PACKET_TYPE requested_type) { resp_packet = create_packet_pooled( PACKET_TYPE::INFO_DRONE_STATUS, packet_info_drone_status{ - {gps->origin_long, gps->origin_lat}, millis(), 0}); + .origin = {gps->origin_long, gps->origin_lat}, + .time_since_boot = millis(), + .unix_timestamp_millis = 0, + .gps_fix = gps->gps_avaliable()}); + ESP_LOGI("STATUS", "Status sent"); xSemaphoreGive(gps_mutex); } } diff --git a/main/sens_fus.h b/main/sens_fus.h index 514030b..ab8e808 100644 --- a/main/sens_fus.h +++ b/main/sens_fus.h @@ -37,20 +37,20 @@ struct sens_fus_compl { * so at t=tau, were 63% of the way there * at t=3*tau, were 95% of the way there */ - Eigen::Vector3f tau_gps_pos = {20.0f, 20.0f, INFINITY}; - Eigen::Vector3f tau_gps_vel = {40.0f, 40.0f, INFINITY}; + Eigen::Vector3f tau_gps_pos = {2.0f, 2.0f, INFINITY}; + Eigen::Vector3f tau_gps_vel = {INFINITY, INFINITY, INFINITY}; - Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 15.0f}; - Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 30.0f}; + Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 10.0f}; + Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f}; - float tau_yaw = 20.0f; + float tau_yaw = 10.0f; // Yaw remains a scalar Eigen::Matrix3f yaw_rotation_matrix = Eigen::Matrix3f::Identity().eval(); void update_yaw_matrix() { - yaw_rotation_matrix = - Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) - .toRotationMatrix(); + // yaw_rotation_matrix = + // Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) + // .toRotationMatrix(); } void predict(float dt, Eigen::Vector3f accel) { @@ -77,24 +77,23 @@ struct sens_fus_compl { alpha_pos.array() * gps_pos.array(); // 2. Yaw Correction (only if moving > 1.0 m/s) - if (gps_vel.norm() > 1.0f) { + if (gps_vel.norm() > 0.2f) { Eigen::Vector3f delta_v_imu = this->velocity - this->velocity_last_gps_measurment; Eigen::Vector3f delta_v_gps = this->velocity - gps_vel; - if (abs(delta_v_gps.norm() - delta_v_imu.norm()) < 1.0 * dt) { - float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu); + float yaw_delta = getYawDifference(delta_v_gps, delta_v_imu); - this->yaw_offset += yaw_delta * alpha_yaw; + this->yaw_offset += yaw_delta * alpha_yaw; + + this->yaw_offset = + std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); + this->update_yaw_matrix(); - this->yaw_offset = - std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset)); - this->update_yaw_matrix(); - } this->velocity = (Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() + alpha_vel.array() * gps_vel.array(); - } else if (this->velocity.norm() > 1.0) { + } else if (this->velocity.norm() > 0.2f) { this->velocity *= 1 - (0.90 * dt); }