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] = {
// x, y, z
//
{-1.0, -1.0, -1.0}, // Rear Right
{-1.0, 1.0, 1.0}, // Rear Left
@ -122,6 +123,15 @@ void motor_throttles_task(void *params) {
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) {
for (int i = 0; i < 4; i++) {
float throttle =

16
main/drone.h
View File

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

9
main/imu.cpp
View File

@ -70,11 +70,15 @@ BNO08x *setup_imu() {
local_state->rot =
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::AngleAxisf(-M_PI / 2.0, Eigen::Vector3f::UnitZ()));
// 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 =
// 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();
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()) {
@ -142,6 +145,8 @@ BNO08x *setup_imu() {
if (xSemaphoreTake(imu_state_mutex, 0)) {
imu_state_var = *local_state;
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) {
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(
PACKET_TYPE::INFO_DRONE_POSITION,
packet_info_drone_position{
.trans = {sens_fus.position.x(), sens_fus.position.y(),
sens_fus.position.z()},
.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()},
.rot = {state.rot.w(), state.rot.x(), state.rot.y(), state.rot.z()},
});
}