before fucking shit up
This commit is contained in:
franchioping
parent
eaa2c1d4ad
commit
88d605ae68
|
|
@ -1 +1 @@
|
|||
Subproject commit 350c761b9fe11f551e5c8ddf11e84252d2f496b4
|
||||
Subproject commit ac25a39777bffe724f80de5ec86bc1b0773833a2
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
200
main/drone.h
200
main/drone.h
|
|
@ -1,7 +1,10 @@
|
|||
#pragma once
|
||||
|
||||
#include "esp32-hal.h"
|
||||
#include <atomic>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <stdatomic.h>
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -1,11 +1,13 @@
|
|||
#include "env_sens.h"
|
||||
|
||||
#include "nav.h"
|
||||
#include "sens_fus.h"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include <Adafruit_BME280.h>
|
||||
#include <SPI.h>
|
||||
#include <Wire.h>
|
||||
#include <cmath>
|
||||
|
||||
#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;
|
||||
|
||||
|
|
|
|||
|
|
@ -52,6 +52,6 @@ void gps_poll_task(void *_) {
|
|||
}
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(10)); // 10Hz polling
|
||||
vTaskDelay(pdMS_TO_TICKS(5)); // 10Hz polling
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,8 +13,7 @@
|
|||
#include "HardwareSerial.h"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include <Adafruit_GPS.h>
|
||||
#include <cmath>
|
||||
#include <Adafruit_GPS.h> #include <cmath>
|
||||
#include <optional>
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
59
main/imu.cpp
59
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,14 +25,16 @@
|
|||
#include <Eigen/Dense>
|
||||
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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 <atomic>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
|
||||
|
|
@ -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<Eigen::Vector3f> 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));
|
||||
|
|
|
|||
|
|
@ -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<uint8_t *, size_t> 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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue