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

before shitty test

This commit is contained in:
franchioping 2026-04-20 22:24:16 +01:00
parent a7691360b7
commit adb7c41f77
4 changed files with 18 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
main/drone.cpp
View File

@ -50,16 +50,16 @@ dcont::ControllerConfig default_config() {
// Rotation Loop (Rotation/Accel -> Angular Rate) // Rotation Loop (Rotation/Accel -> Angular Rate)
config.stack.rotation_pid = { config.stack.rotation_pid = {
.kp = {30.0f, 40.0f, 1.0f}, .kp = {0.05f, 0.05f, 1.0f},
.ki = {2.0f, 2.0f, 0.2f}, .ki = {0.0f, 0.0f, 0.2f},
.kd = {0.0f, 0.0f, 0.0f}, .kd = {0.0f, 0.0f, 0.0f},
.integral_cap = {10.0f, 10.0f, 40.0f}, .integral_cap = {10.0f, 1.0f, 2.0f},
.frequency = 200.0f, .frequency = 200.0f,
}; };
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop // Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
config.stack.rate_pid = { config.stack.rate_pid = {
.kp = {0.0f, 0.0f, 2.0f}, .kp = {0.05f, 0.05f, 2.0f},
.ki = {0.00f, 0.00f, 0.0f}, .ki = {0.00f, 0.00f, 0.0f},
.kd = {0.00f, 0.00f, 0.0f}, .kd = {0.00f, 0.00f, 0.0f},
.integral_cap = {1.0f, 1.0f, 1.0f}, .integral_cap = {1.0f, 1.0f, 1.0f},

27
main/drone.h
View File

@ -153,32 +153,29 @@ struct drone_cont_state {
void update_input() { void update_input() {
packet_controller_input cont_input; packet_controller_input c;
switch (atomic_load(&this->current_input_mode)) { switch (atomic_load(&this->current_input_mode)) {
case INPUT_TYPE::ACRO: { case INPUT_TYPE::ACRO: {
if (controller_input_semaphore && if (controller_input_semaphore &&
xSemaphoreTake(controller_input_semaphore, 10)) { xSemaphoreTake(controller_input_semaphore, 10)) {
cont_input = current_controller_input; c = current_controller_input;
xSemaphoreGive(controller_input_semaphore); xSemaphoreGive(controller_input_semaphore);
auto inp = auto inp = dcont::Input{.joystick = {.throttle_input = c.ly,
dcont::Input{.joystick = {.throttle_input = 0.5, .roll_input = c.rx,
.roll_input = cont_input.rx, .yaw_input = c.lx,
.yaw_input = cont_input.lx, .pitch_input = c.ry},
.pitch_input = cont_input.ry}, .acceleration = {0.0, 0.0, 0.0},
.acceleration = {0.0, 0.0, 0.0}, .rotation = {-c.ry, c.rx, c.lx},
.rotation = {0.0, 0.0, 0.0}, .velocity = {0.0, 0.0, 0.0},
.velocity = {2 * cont_input.rx, 2 * cont_input.ry, .position = {0.0, 0.0, 0.0},
2 * cont_input.ly}, .mode = dcont::ModeInput::Rotation};
.position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Rotation};
dcont::set_input(drone_controller, inp); dcont::set_input(drone_controller, inp);
// ESP_LOGI("TEST", "(%f,%f), (%f,%f)", cont_input.lx, cont_input.ly, // ESP_LOGI("TEST", "(%f,%f), (%f,%f)", c.lx, c.ly, c.rx, c.ry);
// cont_input.rx, cont_input.ry);
} else { } else {
ESP_LOGE("TAG", "ERRR"); ESP_LOGE("TAG", "ERRR");

14
main/main.cpp
View File

@ -99,7 +99,7 @@ extern "C" void app_main(void) {
); );
servo_init(); servo_init();
ESP_LOGI("MAIN", "All tasks spawned. Main loop free."); ESP_LOGI("MAIN", "All tasks spawned. Main loop free.");
ESP_LOGI("MAIN", "FLASHED"); ESP_LOGI("MAIN", "FLASHED2");
Eigen::Vector3f local_pos = {0, 0, 0}; Eigen::Vector3f local_pos = {0, 0, 0};
Eigen::Vector3f local_vel = {0, 0, 0}; Eigen::Vector3f local_vel = {0, 0, 0};
@ -112,24 +112,12 @@ extern "C" void app_main(void) {
bool released = false; bool released = false;
bool active = false; bool active = false;
while (true) {
vTaskDelay(pdMS_TO_TICKS(2000));
current_controller_input.lx = 0;
vTaskDelay(pdMS_TO_TICKS(2000));
current_controller_input.lx = M_PI / 2.0;
}
while (true) { while (true) {
if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) { if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION); send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
// ESP_LOGI("RADIO_TX_INVOK", "INFO DRONE POSITION");
last_position_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) { if (millis() > last_status_broadcast_time + 500 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS); send_packet_getter(PACKET_TYPE::INFO_DRONE_STATUS);

2
main/packet_handler.cpp
View File

@ -65,7 +65,7 @@ void handle_packet(uint8_t *packet_addr) {
(packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE)); (packet_controller_input *)(packet_addr + sizeof(PACKET_TYPE));
if (xSemaphoreTake(controller_input_semaphore, 10)) { if (xSemaphoreTake(controller_input_semaphore, 10)) {
// current_controller_input = *packet; current_controller_input = *packet;
time_last_controller = millis(); time_last_controller = millis();
xSemaphoreGive(controller_input_semaphore); xSemaphoreGive(controller_input_semaphore);
} }