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)
config.stack.rotation_pid = {
.kp = {30.0f, 40.0f, 1.0f},
.ki = {2.0f, 2.0f, 0.2f},
.kp = {0.05f, 0.05f, 1.0f},
.ki = {0.0f, 0.0f, 0.2f},
.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,
};
// Rate Loop (Angular Rate -> Torque) - The "Inner" Loop
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},
.kd = {0.00f, 0.00f, 0.0f},
.integral_cap = {1.0f, 1.0f, 1.0f},

21
main/drone.h
View File

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

14
main/main.cpp
View File

@ -99,7 +99,7 @@ extern "C" void app_main(void) {
);
servo_init();
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_vel = {0, 0, 0};
@ -112,24 +112,12 @@ extern "C" void app_main(void) {
bool released = 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) {
if (millis() > last_position_broadcast_time + 200 && packet_tx_queue) {
send_packet_getter(PACKET_TYPE::INFO_DRONE_POSITION);
// ESP_LOGI("RADIO_TX_INVOK", "INFO DRONE POSITION");
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);

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));
if (xSemaphoreTake(controller_input_semaphore, 10)) {
// current_controller_input = *packet;
current_controller_input = *packet;
time_last_controller = millis();
xSemaphoreGive(controller_input_semaphore);
}