From cadc1422819879155954acaa84284a51b0441f6e Mon Sep 17 00:00:00 2001 From: bkleiner Date: Thu, 21 Dec 2023 22:04:43 +0100 Subject: [PATCH] sixaxis: use matrix to transform gyro/accel --- src/flight/sixaxis.c | 120 +++++++++++++++++++++++-------------------- 1 file changed, 65 insertions(+), 55 deletions(-) diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index a526dc72f..b57944d13 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -62,6 +62,60 @@ void sixaxis_init() { } } +static void sixaxis_compute_matrix(float mat[3][3]) { + vec3_t rot = {.roll = 0, .pitch = 0, .yaw = 0}; + + if (profile.motor.gyro_orientation & GYRO_ROTATE_90_CW) { + rot.yaw += 90.0f * DEGTORAD; + } + if (profile.motor.gyro_orientation & GYRO_ROTATE_90_CCW) { + rot.yaw -= 90.0f * DEGTORAD; + } + if (profile.motor.gyro_orientation & GYRO_ROTATE_45_CW) { + rot.yaw += 45.0f * DEGTORAD; + } + if (profile.motor.gyro_orientation & GYRO_ROTATE_45_CCW) { + rot.yaw -= 45.0f * DEGTORAD; + } + if (profile.motor.gyro_orientation & GYRO_ROTATE_180) { + rot.yaw += 180.0f * DEGTORAD; + } + if (profile.motor.gyro_orientation & GYRO_FLIP_180) { + rot.pitch += 180.0f * DEGTORAD; + rot.yaw += 180.0f * DEGTORAD; + } + + const float cosx = fastcos(rot.roll); + const float sinx = fastsin(rot.roll); + const float cosy = fastcos(rot.pitch); + const float siny = fastsin(rot.pitch); + const float cosz = fastcos(rot.yaw); + const float sinz = fastsin(rot.yaw); + + const float coszcosx = cosz * cosx; + const float sinzcosx = sinz * cosx; + const float coszsinx = sinx * cosz; + const float sinzsinx = sinx * sinz; + + mat[0][0] = cosz * cosy; + mat[0][1] = -cosy * sinz; + mat[0][2] = siny; + mat[1][0] = sinzcosx + (coszsinx * siny); + mat[1][1] = coszcosx - (sinzsinx * siny); + mat[1][2] = -sinx * cosy; + mat[2][0] = (sinzsinx) - (coszcosx * siny); + mat[2][1] = (coszsinx) + (sinzcosx * siny); + mat[2][2] = cosy * cosx; +} + +static vec3_t sixaxis_apply_matrix(float mat[3][3], vec3_t v) { + return (vec3_t){ + .roll = (mat[0][0] * v.roll + mat[1][0] * v.pitch + mat[2][0] * v.yaw), + .pitch = (mat[0][1] * v.roll + mat[1][1] * v.pitch + mat[2][1] * v.yaw), + .yaw = (mat[0][2] * v.roll + mat[1][2] * v.pitch + mat[2][2] * v.yaw), + }; +} + void sixaxis_read() { const gyro_data_t data = gyro_spi_read(); @@ -76,63 +130,19 @@ void sixaxis_read() { state.gyro_temp = data.temp; - float temp = 0; - - if (profile.motor.gyro_orientation & GYRO_ROTATE_90_CW) { - temp = state.accel_raw.pitch; - state.accel_raw.pitch = -state.accel_raw.roll; - state.accel_raw.roll = temp; - - temp = state.gyro_raw.pitch; - state.gyro_raw.pitch = -state.gyro_raw.roll; - state.gyro_raw.roll = temp; - } - - if (profile.motor.gyro_orientation & GYRO_ROTATE_45_CCW) { - temp = state.accel_raw.pitch; - state.accel_raw.pitch = state.accel_raw.roll * INVSQRT2 + state.accel_raw.pitch * INVSQRT2; - state.accel_raw.roll = state.accel_raw.roll * INVSQRT2 - temp * INVSQRT2; - - temp = state.gyro_raw.pitch; - state.gyro_raw.pitch = state.gyro_raw.roll * INVSQRT2 + state.gyro_raw.pitch * INVSQRT2; - state.gyro_raw.roll = state.gyro_raw.roll * INVSQRT2 - temp * INVSQRT2; - } - - if (profile.motor.gyro_orientation & GYRO_ROTATE_45_CW) { - temp = state.accel_raw.roll; - state.accel_raw.roll = state.accel_raw.pitch * INVSQRT2 + state.accel_raw.roll * INVSQRT2; - state.accel_raw.pitch = state.accel_raw.pitch * INVSQRT2 - temp * INVSQRT2; - - temp = state.gyro_raw.roll; - state.gyro_raw.roll = state.gyro_raw.pitch * INVSQRT2 + state.gyro_raw.roll * INVSQRT2; - state.gyro_raw.pitch = state.gyro_raw.pitch * INVSQRT2 - temp * INVSQRT2; - } - - if (profile.motor.gyro_orientation & GYRO_ROTATE_90_CCW) { - temp = state.accel_raw.pitch; - state.accel_raw.pitch = state.accel_raw.roll; - state.accel_raw.roll = -temp; - - temp = state.gyro_raw.pitch; - state.gyro_raw.pitch = state.gyro_raw.roll; - state.gyro_raw.roll = -temp; - } - - if (profile.motor.gyro_orientation & GYRO_ROTATE_180) { - state.accel_raw.pitch = -state.accel_raw.pitch; - state.accel_raw.roll = -state.accel_raw.roll; - - state.gyro_raw.pitch = -state.gyro_raw.pitch; - state.gyro_raw.roll = -state.gyro_raw.roll; + static float mat[3][3] = { + {1.0f, 0.0f, 0.0f}, + {0.0f, 1.0f, 0.0f}, + {0.0f, 0.0f, 1.0f}, + }; + static uint8_t last_gyro_orientation = GYRO_ROTATE_NONE; + if (last_gyro_orientation != profile.motor.gyro_orientation) { + sixaxis_compute_matrix(mat); + last_gyro_orientation = profile.motor.gyro_orientation; } - if (profile.motor.gyro_orientation & GYRO_FLIP_180) { - state.accel_raw.pitch = -state.accel_raw.pitch; - state.accel_raw.yaw = -state.accel_raw.yaw; - - state.gyro_raw.pitch = -state.gyro_raw.pitch; - state.gyro_raw.yaw = -state.gyro_raw.yaw; - } + state.accel_raw = sixaxis_apply_matrix(mat, state.accel_raw); + state.gyro_raw = sixaxis_apply_matrix(mat, state.gyro_raw); filter_coeff(profile.filter.gyro[0].type, &filter[0], profile.filter.gyro[0].cutoff_freq); filter_coeff(profile.filter.gyro[1].type, &filter[1], profile.filter.gyro[1].cutoff_freq);