Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

working acro

This commit is contained in:
franchioping 2026-04-19 18:46:42 +01:00
parent 9ba3094eeb
commit f5dbec4b63
6 changed files with 53 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
components/drone_comms

@ -1 +1 @@
Subproject commit bb38f95e0de4868b2f84b3fa8ab3025f3496e37b
Subproject commit 24803f37ed9a5824682f7e83535c43392db21c20

19
main/drone.cpp
View File

@ -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 <algorithm>
#include <cstdint>
#include <cstring>
#include <optional>
#include <stdlib.h>
@ -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;
}

14
main/drone.h
View File

@ -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);

1
main/imu.cpp
View File

@ -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()) {

42
main/main.cpp
View File

@ -9,8 +9,7 @@
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include <atomic>
#include <cmath>
#include <cstdint>
#include <cmath> #include <cstdint>
#include <optional>
#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);

3
main/packet_handler.cpp
View File

@ -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) {