drone task inputs. added drone.h and drone.cpp
This commit is contained in:
franchioping
parent
3f4c7e66cd
commit
b315af0f66
|
|
@ -1 +1 @@
|
|||
Subproject commit 9d8fccc2e3981746a077b47b57a88701d9f3f502
|
||||
Subproject commit 11a55db27127a10a99439cde27a04489bb7318d2
|
||||
|
|
@ -0,0 +1,99 @@
|
|||
#include "drone.h"
|
||||
|
||||
#include "drone_controller.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 "sens_fus.h"
|
||||
#include <cstdint>
|
||||
|
||||
#define CONTROLLER_TASK_FREQUENCY 400.0;
|
||||
|
||||
dcont::ControllerConfig default_config() {
|
||||
dcont::ControllerConfig config;
|
||||
|
||||
// 1. Configure the PID Stack
|
||||
// Note: kp, ki, kd are arrays of 3 [roll, pitch, yaw]
|
||||
|
||||
// Position Loop (Position -> Velocity)
|
||||
config.stack.position_pid = {
|
||||
{1.0f, 1.0f, 1.0f}, // kp
|
||||
{0.0f, 0.0f, 0.0f}, // ki
|
||||
{0.0f, 0.0f, 0.0f}, // kd
|
||||
25.0f // frequency (Hz)
|
||||
};
|
||||
|
||||
// Velocity Loop (Velocity -> Acceleration/Tilt)
|
||||
config.stack.linvel_pid = {
|
||||
{1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, 50.0f};
|
||||
|
||||
// Rotation Loop (Angle -> Angular Rate)
|
||||
config.stack.rotation_pid = {
|
||||
{4.0f, 4.0f, 4.0f}, {1.0f, 1.0f, 1.0f}, {0.0f, 0.0f, 0.0f}, 200.0f};
|
||||
|
||||
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
|
||||
config.stack.rate_pid = {{0.2f, 0.2f, 2.0f},
|
||||
{0.05f, 0.05f, 0.05f},
|
||||
{0.003f, 0.003f, 0.001f},
|
||||
600.0f};
|
||||
|
||||
// 2. Set Constraints
|
||||
config.stack.max_rate = 3.14f; // ~180 degrees/s
|
||||
config.stack.max_linvel = 10.0f; // 10 m/s
|
||||
|
||||
// 3. Physical Drone Properties
|
||||
config.mass = 0.350f; // kg
|
||||
config.max_thrust = 2.6f; // Newtons
|
||||
config.max_torque = 0.5f; // Nm
|
||||
|
||||
// roll, pitch, yaw for each motor
|
||||
float mixer[4][3] = {
|
||||
{1.0f, -1.0f, 1.0f}, // Front Right
|
||||
{-1.0f, -1.0f, -1.0f}, // Front Left
|
||||
{-1.0f, 1.0f, 1.0f}, // Rear Left
|
||||
{1.0f, 1.0f, -1.0f} // Rear Right
|
||||
};
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
config.motor_map[i][j] = mixer[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
return config;
|
||||
}
|
||||
|
||||
void setup_drone() { drone_controller = dcont::create(default_config()); }
|
||||
|
||||
void drone_controller_task(void *params) {
|
||||
setup_drone();
|
||||
uint8_t wait_ms = 1000.0 / CONTROLLER_TASK_FREQUENCY;
|
||||
|
||||
imu_state imu_state_local;
|
||||
Eigen::Vector3f position_local = Eigen::Vector3f::Zero();
|
||||
Eigen::Vector3f velocity_local = Eigen::Vector3f::Zero();
|
||||
|
||||
while (true) {
|
||||
if (xSemaphoreTake(imu_state_mutex, 1)) {
|
||||
imu_state_local = imu_state_var;
|
||||
xSemaphoreGive(imu_state_mutex);
|
||||
}
|
||||
if (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);
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(wait_ms));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
#pragma once
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "drone_comms.h"
|
||||
#include "drone_controller.h"
|
||||
|
||||
#ifdef PS
|
||||
#undef PS
|
||||
#endif
|
||||
|
||||
#ifdef F
|
||||
#undef F
|
||||
#endif
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
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;
|
||||
|
||||
void drone_controller_task(void *params);
|
||||
|
|
@ -68,7 +68,8 @@ void setup_imu() {
|
|||
|
||||
float dt =
|
||||
((float)(current_time - local_state->last_time)) / 1000000.0f;
|
||||
Vec3C accel_global = apply_rot(&local_state->accel, &local_state->rot);
|
||||
dcont::Vec3C accel_global =
|
||||
dcont::apply_rot(&local_state->accel, &local_state->rot);
|
||||
|
||||
if (xSemaphoreTake(sens_fus_mutex, (TickType_t)2) == pdTRUE) {
|
||||
sens_fus.predict(dt, Eigen::Vector3f(accel_global.x, accel_global.y,
|
||||
|
|
|
|||
|
|
@ -25,8 +25,8 @@
|
|||
#include <Eigen/Dense>
|
||||
|
||||
struct imu_state {
|
||||
Vec3C accel = {0, 0, 0};
|
||||
QuatC rot = {0, 0, 0, 1};
|
||||
dcont::Vec3C accel = {0, 0, 0};
|
||||
dcont::QuatC rot = {0, 0, 0, 1};
|
||||
int64_t last_time = -1;
|
||||
Eigen::Vector3f angvel;
|
||||
Eigen::Vector3f rot_euler;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,7 @@
|
|||
|
||||
#include "Eigen/Core"
|
||||
#include "driver/gpio.h"
|
||||
#include "drone.h"
|
||||
#include "drone_comms.h"
|
||||
#include "esp32-hal.h"
|
||||
#include "esp_log.h"
|
||||
|
|
@ -10,6 +12,7 @@
|
|||
#include <Arduino.h>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
|
||||
#include "env_sens.h"
|
||||
#include "gps.h"
|
||||
|
|
@ -30,8 +33,7 @@ extern "C" void app_main(void) {
|
|||
gpio_install_isr_service(0);
|
||||
Serial.begin(115200);
|
||||
|
||||
static imu_state imu_state;
|
||||
auto _ = setup_imu(&imu_state);
|
||||
setup_imu();
|
||||
|
||||
xTaskCreatePinnedToCore(radio_task, // Function name
|
||||
"radio_rxtx", // Name for debugging
|
||||
|
|
@ -42,6 +44,15 @@ extern "C" void app_main(void) {
|
|||
0 // 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
|
||||
);
|
||||
|
||||
xTaskCreate(env_sens::baro_poll_task, "baro_poll", 8192, NULL, 1, NULL);
|
||||
|
||||
xTaskCreate(gps_poll_task, "gps_poll", 8192, NULL, 5, NULL);
|
||||
|
|
@ -52,25 +63,28 @@ extern "C" void app_main(void) {
|
|||
Eigen::Vector3f local_vel = {0, 0, 0};
|
||||
bool nav_data_ready = false;
|
||||
while (true) {
|
||||
if (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
|
||||
while (xQueueReceive(packet_tx_queue, &packet_data[0], 1)) {
|
||||
handle_packet(&packet_data[0]);
|
||||
}
|
||||
|
||||
std::optional<Eigen::Vector3f> coords;
|
||||
float lat, lon, alt;
|
||||
if (gps_mutex && xSemaphoreTake(gps_mutex, (TickType_t)10) == pdTRUE) {
|
||||
if (gps->gps_avaliable()) {
|
||||
ESP_LOGI(TAG, "loc -> lat: %f, long: %f, height: %f",
|
||||
gps->gps->latitudeDegrees, gps->gps->longitudeDegrees,
|
||||
gps->gps->altitude);
|
||||
|
||||
auto D_pos_cond = gps->get_coordinates();
|
||||
if (D_pos_cond.has_value()) {
|
||||
auto D_pos = D_pos_cond.value();
|
||||
|
||||
ESP_LOGI(TAG, " -> D(pos): (%f, %f, %f)", D_pos[0], D_pos[1],
|
||||
D_pos[2]);
|
||||
}
|
||||
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]);
|
||||
}
|
||||
}
|
||||
env_sens::dbg_sens();
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +0,0 @@
|
|||
#include "nav.h"
|
||||
Loading…
Reference in New Issue