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

test rollback after

This commit is contained in:
franchioping 2026-04-19 20:51:00 +01:00
parent f5dbec4b63
commit 8bf1a711b2
4 changed files with 34 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
main/drone.cpp
View File

@ -68,6 +68,7 @@ dcont::ControllerConfig default_config() {
float mixer[4][3] = { float mixer[4][3] = {
// x, y, z // x, y, z
//
{-1.0, -1.0, -1.0}, // Rear Right {-1.0, -1.0, -1.0}, // Rear Right
{-1.0, 1.0, 1.0}, // Rear Left {-1.0, 1.0, 1.0}, // Rear Left
@ -122,6 +123,15 @@ void motor_throttles_task(void *params) {
vTaskDelay(2); vTaskDelay(2);
} }
unsigned long armTime_sec = millis();
while (millis() - armTime_sec < 1000) {
for (int i = 0; i < 4; i++) {
motors[i]->sendThrottlePercent(20 * (millis() - armTime_sec) /
armTime_sec);
}
vTaskDelay(2);
}
while (true) { while (true) {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
float throttle = float throttle =

16
main/drone.h
View File

@ -152,7 +152,7 @@ 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: {
@ -160,21 +160,21 @@ struct drone_cont_state {
// if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) { // if (millis() - time_last_controller > CONNECTION_LOST_THRESHOLD) {
// current_controller_input = {0, 0, 0, 0}; // current_controller_input = {0, 0, 0, 0};
// } // }
cont_input = current_controller_input; c = current_controller_input;
xSemaphoreGive(controller_input_semaphore); xSemaphoreGive(controller_input_semaphore);
dcont::set_input( dcont::set_input(
drone_controller, drone_controller,
dcont::Input{.joystick = {.throttle_input = cont_input.ly, dcont::Input{.joystick = {.throttle_input = c.ly,
.roll_input = cont_input.rx, .roll_input = c.rx,
.yaw_input = cont_input.lx, .yaw_input = c.lx,
.pitch_input = cont_input.ry}, .pitch_input = c.ry},
.acceleration = {0.0, 0.0, 0.0}, .acceleration = {0.0, 0.0, 0.0},
.rotation = {0.0, 0.0, 0.0}, .rotation = {c.ry * 3, c.rx * 3, c.lx * 3},
.velocity = {0.0, 0.0, 0.0}, .velocity = {0.0, 0.0, 0.0},
.position = {0.0, 0.0, 0.0}, .position = {0.0, 0.0, 0.0},
.mode = dcont::ModeInput::Acro}); .mode = dcont::ModeInput::Rotation});
} else { } else {
drone_cont_stabilize(); drone_cont_stabilize();
} }

9
main/imu.cpp
View File

@ -70,11 +70,15 @@ BNO08x *setup_imu() {
local_state->rot = local_state->rot =
Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k); Eigen::Quaternionf(sens_rot.real, sens_rot.i, sens_rot.j, sens_rot.k);
// ESP_LOGI("ROT", "(%f, %f, %f, %f)", sens_rot.real, sens_rot.i,
// sens_rot.j,
// sens_rot.k);
// Eigen::Quaternionf q_global_yaw( // Eigen::Quaternionf q_global_yaw(
// Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ())); // Eigen::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
// local_state->rot = q_global_yaw * local_state->rot; // local_state->rot = q_global_yaw * local_state->rot;
local_state->rot.normalize(); // local_state->rot.normalize();
// {.i = sens_rot.i, .j = sens_rot.j, .k = sens_rot.k, .w = // {.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_rot.real}; local_state->rot_euler = {sens_euler.x, sens_euler.y,
@ -89,7 +93,6 @@ BNO08x *setup_imu() {
auto cal_gyro = imu->rpt.cal_gyro.get(); 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};
// ESP_LOGI("ROT", "angvel_z: %f", cal_gyro.z);
} }
if (imu->rpt.linear_accelerometer.has_new_data()) { if (imu->rpt.linear_accelerometer.has_new_data()) {
@ -142,6 +145,8 @@ BNO08x *setup_imu() {
if (xSemaphoreTake(imu_state_mutex, 0)) { if (xSemaphoreTake(imu_state_mutex, 0)) {
imu_state_var = *local_state; imu_state_var = *local_state;
xSemaphoreGive(imu_state_mutex); xSemaphoreGive(imu_state_mutex);
} else {
ESP_LOGE(TAG, "FAILED TO GET IMU STATE MUTEX");
} }
} }
}); });

12
main/packet_handler.cpp
View File

@ -103,16 +103,22 @@ void send_packet_getter(PACKET_TYPE requested_type) {
if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) { if (requested_type == PACKET_TYPE::INFO_DRONE_POSITION) {
Eigen::Vector3f local_vel = imu_state_var.rot.inverse() * sens_fus.velocity; imu_state state;
if (xSemaphoreTake(imu_state_mutex, 20)) {
state = imu_state_var;
xSemaphoreGive(imu_state_mutex);
}
Eigen::Vector3f local_vel = state.rot.inverse() * sens_fus.velocity;
ESP_LOGE("Radio tx", "[f,%f,%f,%f]", state.rot.w(), state.rot.x(),
state.rot.y(), state.rot.z());
resp_packet = create_packet_pooled( resp_packet = create_packet_pooled(
PACKET_TYPE::INFO_DRONE_POSITION, PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{ packet_info_drone_position{
.trans = {sens_fus.position.x(), sens_fus.position.y(), .trans = {sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()}, sens_fus.position.z()},
.vel = {local_vel.x(), local_vel.y(), local_vel.z()}, .vel = {local_vel.x(), local_vel.y(), local_vel.z()},
.rot = {imu_state_var.rot.w(), imu_state_var.rot.x(), .rot = {state.rot.w(), state.rot.x(), state.rot.y(), state.rot.z()},
imu_state_var.rot.y(), imu_state_var.rot.z()},
}); });
} }