From 20689a517409edc87d1abd67adccc94cc83f04a0 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Mon, 31 Oct 2022 12:57:38 -0700 Subject: [PATCH 1/5] Add DCM Ahrs --- firmware/src/src/DCMAhrs/dcmahrs.cpp | 419 +++++++++++++++++++++++++++ firmware/src/src/DCMAhrs/dcmahrs.h | 9 + 2 files changed, 428 insertions(+) create mode 100644 firmware/src/src/DCMAhrs/dcmahrs.cpp create mode 100644 firmware/src/src/DCMAhrs/dcmahrs.h diff --git a/firmware/src/src/DCMAhrs/dcmahrs.cpp b/firmware/src/src/DCMAhrs/dcmahrs.cpp new file mode 100644 index 00000000..0ab2f2ce --- /dev/null +++ b/firmware/src/src/DCMAhrs/dcmahrs.cpp @@ -0,0 +1,419 @@ +/* This Algorithm provided by Paul_BB on Rc Groups. + * Details to be filled in here! TODO + */ + +#include "dcmahrs.h" + +#include +#include + +#include "defines.h" +#include "sense.h" +#include "trackersettings.h" + +#define R2D RAD_TO_DEG // Radians to degrees +#define D2R DEG_TO_RAD // Degrees to radians + +bool DCMInitFlag = true; +float tilt, roll, pan; +float rzero[9]; +float rmat[9]; +float rup[9]; +float errorRP[3]; +float omegacorrPAcc[3]; +float omegacorrPMag[3]; +float biasGyros[3]; +float dirovergndHB[3]; +float dirovergndHMag[3]; +float errorYawground[3]; +float errorYawplane[3]; + +float omegatotal[3], theta[3]; +float MagvecEarth[3]; +float V_buff[3]; +float r_buff[9]; +float f_buff; + +//---------------------------------------------------------------------- +// Calculations and Main Channel Thread +//---------------------------------------------------------------------- + +/*============================================================================*/ +/*============================================================================*/ +/* Math routines */ +/*============================================================================*/ +/*============================================================================*/ +/*----------------------------------------------------------------------------*/ +static void MatrixMultiply(float *M1, float *M2) +/*----------------------------------------------------------------------------*/ +/* M1 = M1*M2 */ +/*----------------------------------------------------------------------------*/ +{ + /* Declarations */ + float M_buff[9]; + + for (int i = 0; i < 9; i++) M_buff[i] = M1[i]; + + M1[0] = M_buff[0] * M2[0] + M_buff[1] * M2[3] + M_buff[2] * M2[6]; + M1[3] = M_buff[3] * M2[0] + M_buff[4] * M2[3] + M_buff[5] * M2[6]; + M1[6] = M_buff[6] * M2[0] + M_buff[7] * M2[3] + M_buff[8] * M2[6]; + + M1[1] = M_buff[0] * M2[1] + M_buff[1] * M2[4] + M_buff[2] * M2[7]; + M1[4] = M_buff[3] * M2[1] + M_buff[4] * M2[4] + M_buff[5] * M2[7]; + M1[7] = M_buff[6] * M2[1] + M_buff[7] * M2[4] + M_buff[8] * M2[7]; + + M1[2] = M_buff[0] * M2[2] + M_buff[1] * M2[5] + M_buff[2] * M2[8]; + M1[5] = M_buff[3] * M2[2] + M_buff[4] * M2[5] + M_buff[5] * M2[8]; + M1[8] = M_buff[6] * M2[2] + M_buff[7] * M2[5] + M_buff[8] * M2[8]; +} + +/*----------------------------------------------------------------------------*/ +static void TransposeMatrix(float *M1, float *M2) +/*----------------------------------------------------------------------------*/ +/* M2 = Transpose M1 */ +/*----------------------------------------------------------------------------*/ +{ + M2[0] = M1[0]; + M2[1] = M1[3]; + M2[2] = M1[6]; + M2[3] = M1[1]; + M2[4] = M1[4]; + M2[5] = M1[7]; + M2[6] = M1[2]; + M2[7] = M1[5]; + M2[8] = M1[8]; +} + +/*----------------------------------------------------------------------------*/ +static void MatrixVector(float *M, float *V1, float *V2) +/*----------------------------------------------------------------------------*/ +/* V2 = M*V1 */ +/*----------------------------------------------------------------------------*/ +{ + V2[0] = M[0] * V1[0] + M[1] * V1[1] + M[2] * V1[2]; + V2[1] = M[3] * V1[0] + M[4] * V1[1] + M[5] * V1[2]; + V2[2] = M[6] * V1[0] + M[7] * V1[1] + M[8] * V1[2]; +} + +/*----------------------------------------------------------------------------*/ +static void VectorDotProduct(float *V1, float *V2, float *Scalar_Product) +/*----------------------------------------------------------------------------*/ +/* Scalar_Product = V1.V2 */ +/*----------------------------------------------------------------------------*/ +{ + *Scalar_Product = 0.; + for (int i = 0; i < 3; i++) *Scalar_Product += V1[i] * V2[i]; +} + +/*----------------------------------------------------------------------------*/ +static void VectorCross(float *V1, float *V2, float *V3) +/*----------------------------------------------------------------------------*/ +/* V3 = V1xV2 */ +/*----------------------------------------------------------------------------*/ +{ + V3[0] = V1[1] * V2[2] - V1[2] * V2[1]; + V3[1] = V1[2] * V2[0] - V1[0] * V2[2]; + V3[2] = V1[0] * V2[1] - V1[1] * V2[0]; +} + +/*----------------------------------------------------------------------------*/ +static void VectorAdd(float *V1, float *V2, float *V3) +/*----------------------------------------------------------------------------*/ +/* V3 = V1+V2 */ +/*----------------------------------------------------------------------------*/ +{ + /* Declarations */ + for (int i = 0; i < 3; i++) V3[i] = V1[i] + V2[i]; +} + +static void MatrixMultiply2(float *M1, float *M2, float *M3) +/*----------------------------------------------------------------------------*/ +/* M3 = M1*M2 */ +/*----------------------------------------------------------------------------*/ +{ + M3[0] = M1[0] * M2[0] + M1[1] * M2[3] + M1[2] * M2[6]; + M3[3] = M1[3] * M2[0] + M1[4] * M2[3] + M1[5] * M2[6]; + M3[6] = M1[6] * M2[0] + M1[7] * M2[3] + M1[8] * M2[6]; + + M3[1] = M1[0] * M2[1] + M1[1] * M2[4] + M1[2] * M2[7]; + M3[4] = M1[3] * M2[1] + M1[4] * M2[4] + M1[5] * M2[7]; + M3[7] = M1[6] * M2[1] + M1[7] * M2[4] + M1[8] * M2[7]; + + M3[2] = M1[0] * M2[2] + M1[1] * M2[5] + M1[2] * M2[8]; + M3[5] = M1[3] * M2[2] + M1[4] * M2[5] + M1[5] * M2[8]; + M3[8] = M1[6] * M2[2] + M1[7] * M2[5] + M1[8] * M2[8]; +} + +/*----------------------------------------------------------------------------*/ +void DcmAHRSInitialize(float u0[3], float u1[3], float u2[3]) +/*----------------------------------------------------------------------------*/ +/* This routine computes the initial tilt roll pan */ +/* and initializes the DCM algorithm */ +/* The Rzero matrix is set to the DCM matrix so that tilt roll pan are equal */ +/* to zero after boot */ +/*----------------------------------------------------------------------------*/ +{ + /* Declarations */ + float tilt0, roll0, pan0; + float BeX, BeY; + float Ct, St, Cr, Sr, Cp, Sp; + + /* Compute initial orientation relatively to Earth NED frame */ + /* X local Magnetic North */ + /* Y east */ + /* Z down */ + /* ========================================================= */ + /* ========================================================= */ + /* Compute tilt and roll with gravity vector */ + /* ========================================= */ + /* Read body acceleration (x y z accelerometers) */ + for (int i = 0; i < 3; i++) V_buff[i] = u1[i]; + + /* Normalize body acceleration vector */ + VectorDotProduct(V_buff, V_buff, &f_buff); + f_buff = 1. / sqrt(f_buff); + if (f_buff > 0.001) + for (int i = 0; i < 3; i++) V_buff[i] *= f_buff; + + /* Tilt and roll in radians */ + tilt0 = -asin(V_buff[0]); + roll0 = atan2(V_buff[1], V_buff[2]); + + /* Compute pan with Magnetic North vector */ + /* ====================================== */ + /* Straighten Magnetometer x and y (remove tilt and roll) */ + Ct = cos(tilt0); + St = sin(tilt0); + Cr = cos(roll0); + Sr = sin(roll0); + + BeX = Ct * u2[0] + St * Sr * u2[1] + St * Cr * u2[2]; + BeY = -Cr * u2[1] + Sr * u2[2]; + + /* pan in radians */ + /* No need to normalize because atan2 is a ratio */ + pan0 = atan2(BeY, BeX); + + /* Initialize DCM algorithm */ + /* ======================== */ + /* ======================== */ + /* Initialize DCM */ + /* ============== */ + Cp = cos(pan0); + Sp = sin(pan0); + + /* xe in body frame */ + rmat[0] = Cp * Ct; + rmat[1] = -Sp * Cr + Cp * St * Sr; + rmat[2] = Sp * Sr + Cp * St * Cr; + /* ye in body frame */ + rmat[3] = Sp * Ct; + rmat[4] = Cp * Cr + Sp * St * Sr; + rmat[5] = -Cp * Sr + Sp * St * Cr; + /* ze in body frame */ + rmat[6] = -St; + rmat[7] = Ct * Sr; + rmat[8] = Ct * Cr; + + /* Update matrix rup = identity */ + /* ============================ */ + rup[0] = 1.; + rup[4] = 1.; + rup[8] = 1.; + + /* Zero matrix */ + /* Thus at the first DcmCalculate call tilt = roll = pan = 0 */ + /* ========================================================= */ + for (int i = 0; i < 9; i++) rzero[i] = rmat[i]; + + /* tilt-roll correction */ + /* ===================== */ + for (int i = 0; i < 3; i++) omegacorrPAcc[i] = 0.; + + /* pan correction */ + /* ============== */ + for (int i = 0; i < 3; i++) omegacorrPMag[i] = 0.; + + /* Gyros bias */ + /* ========== */ + for (int i = 0; i < 3; i++) biasGyros[i] = -u0[i]; +} + +/*----------------------------------------------------------------------------*/ + +void DCMnormalization_Thread() +{ + while (1) { + rt_sleep_ms(1000); + // Use a mutex so sensor data can't be updated part way + k_mutex_lock(&sensor_mutex, K_FOREVER); + + /* Orthogonalization */ + /* ================= */ + /* ================= */ + /* (U,V) */ + VectorDotProduct(&rmat[0], &rmat[3], &f_buff); + f_buff /= 2.; + + /* U = U - 0.5*V(U,V) */ + for (int i = 0; i < 3; i++) V_buff[i] = rmat[i]; + for (int i = 0; i < 3; i++) rmat[i] -= rmat[3 + i] * f_buff; + + /* V = V - 0.5*U(U,V) */ + for (int i = 0; i < 3; i++) rmat[3 + i] -= V_buff[i] * f_buff; + + /* W = UxV */ + VectorCross(&rmat[0], &rmat[3], &rmat[6]); + + /* U scaling */ + VectorDotProduct(&rmat[0], &rmat[0], &f_buff); + f_buff = 1. / sqrt(f_buff); + for (int i = 0; i < 3; i++) rmat[i] = rmat[i] * f_buff; + + /* V scaling */ + VectorDotProduct(&rmat[3], &rmat[3], &f_buff); + f_buff = 1. / sqrt(f_buff); + for (int i = 0; i < 3; i++) rmat[3 + i] = rmat[3 + i] * f_buff; + + /* W scaling */ + VectorDotProduct(&rmat[6], &rmat[6], &f_buff); + f_buff = 1. / sqrt(f_buff); + for (int i = 0; i < 3; i++) rmat[6 + i] = rmat[6 + i] * f_buff; + + // Free Mutex Lock, Allow sensor updates + k_mutex_unlock(&sensor_mutex); + } +} + +void DcmCalculate(float u0[3], float u1[3], float u2[3], float deltat) +{ + float kp = trkset.Kp(); + float ki = trkset.Ki(); + // Don't do calulations if no accel or mag data, will result in Nan + if (fabs(u1[0] + u1[1] + u1[2]) < 0.0001) return; + if (fabs(u2[0] + u2[1] + u2[2]) < 0.0001) return; + + /* R matrix initialization */ + /* ======================= */ + if (DCMInitFlag == true) { + DcmAHRSInitialize(u0, u1, u2); + DCMInitFlag = false; + } + + /* R matrix update */ + /* =============== */ + /* =============== */ + /* Read body angular velocity (p q r gyrometers) */ + for (int i = 0; i < 3; i++) omegatotal[i] = u0[i]; + + /* Add pitch-roll and yaw corrections */ + VectorAdd(omegatotal, omegacorrPAcc, omegatotal); + VectorAdd(omegatotal, omegacorrPMag, omegatotal); + VectorAdd(omegatotal, biasGyros, omegatotal); + + /* Integrate angular velocity over the 25ms time step */ + for (int i = 0; i < 3; i++) theta[i] = omegatotal[i] * deltat; + + /* Assemble equivalent small rotation matrix (update matrix) */ + rup[1] = -theta[2]; + rup[2] = theta[1]; + rup[3] = theta[2]; + rup[5] = -theta[0]; + rup[6] = -theta[1]; + rup[7] = theta[0]; + + /* Rotate body frame */ + MatrixMultiply(rmat, rup); + + /* roll-tilt correction */ + /* ===================== */ + /* ===================== */ + /* Read body acceleration (x y z accelerometers) */ + for (int i = 0; i < 3; i++) V_buff[i] = u1[i]; + + /* Normalize body acceleration vector */ + /* Note: if f_buff close to zero then errorRP will be close to zero */ + VectorDotProduct(V_buff, V_buff, &f_buff); + f_buff = 1. / sqrt(f_buff); + if (f_buff > 0.001) + for (int i = 0; i < 3; i++) V_buff[i] *= f_buff; + + /* Compute the roll-pitch error vector: cross product of measured */ + /* earth Z vector with estimated earth vector expressed in body */ + /* frame (3rd row of rmat) */ + VectorCross(V_buff, &rmat[6], errorRP); + + /* Compute pitch-roll correction proportional term */ + for (int i = 0; i < 3; i++) omegacorrPAcc[i] = errorRP[i] * kp; + + /* Add pitch-roll error to integrator */ + for (int i = 0; i < 3; i++) biasGyros[i] += errorRP[i] * ki * deltat; + + /* pan correction */ + /* ============== */ + /* ============== */ + /* Read the magnetometer vector */ + for (int i = 0; i < 3; i++) V_buff[i] = u2[i]; + + /* Express the magnetometer vector in the Earth frame */ + MatrixVector(rmat, V_buff, MagvecEarth); + + /* Horizontal component of the magnetometer vector in the Earth frame */ + for (int i = 0; i < 2; i++) dirovergndHMag[i] = MagvecEarth[i]; + dirovergndHMag[2] = 0.; + + /* Normalization */ + f_buff = dirovergndHMag[0] * dirovergndHMag[0] + dirovergndHMag[1] * dirovergndHMag[1]; + f_buff = 1. / sqrt(f_buff); + for (int i = 0; i < 2; i++) dirovergndHMag[i] *= f_buff; + + /* Horizontal component of Earth magnetic vector */ + /* The Earth magnetic horizontal vector is the reference for yaw=0 */ + dirovergndHB[0] = 1.; + dirovergndHB[1] = 0.; + dirovergndHB[2] = 0.; + + /* Compute the yaw error vector expressed in earth frame */ + VectorCross(dirovergndHMag, dirovergndHB, errorYawground); + + /* Express the yaw error vector in the body frame */ + TransposeMatrix(rmat, r_buff); + MatrixVector(r_buff, errorYawground, errorYawplane); + + /* Compute yaw correction proportional term */ + for (int i = 0; i < 3; i++) omegacorrPMag[i] = errorYawplane[i] * kp; + + /* Update gyros bias */ + for (int i = 0; i < 3; i++) biasGyros[i] += errorYawplane[i] * ki * deltat; + + /* Transpose rzero matrix */ + TransposeMatrix(rzero, r_buff); + + /* Multiply transposed rzero with rmat */ + float rout[9]; + MatrixMultiply2(r_buff, rmat, rout); + + /* Euler angles from DCM */ + /* ===================== */ + /* atan2(rmat31,rmat11) */ + pan = atan2(rout[3], rout[0]) * RAD_TO_DEG; + + /* atan2(rmat32,rmat33) */ + tilt = -asin(rout[6]) * RAD_TO_DEG; + + /* -asin(rmat31) */ + roll = atan2(rout[7], rout[8]) * RAD_TO_DEG; +} + +void DcmAhrsResetCenter() +{ + /* Center reset */ + /* ============ */ + for (int i = 0; i < 9; i++) { + rzero[i] = rmat[i]; + } +} + +float DcmGetTilt() { return tilt; } +float DcmGetRoll() { return roll; } +float DcmGetPan() { return pan; } diff --git a/firmware/src/src/DCMAhrs/dcmahrs.h b/firmware/src/src/DCMAhrs/dcmahrs.h new file mode 100644 index 00000000..e8734d1e --- /dev/null +++ b/firmware/src/src/DCMAhrs/dcmahrs.h @@ -0,0 +1,9 @@ +#pragma once + +void DcmAHRSInitialize(float U0[3], float U1[3], float U2[3]); +void DcmAhrsResetCenter(); +void DcmCalculate(float U0[3], float U1[3], float U2[3], float deltat); +float DcmGetTilt(); +float DcmGetRoll(); +float DcmGetPan(); +void DCMnormalization_Thread(); From e1ffb88d087f51897eea9935349ce7ca393fad70 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Mon, 31 Oct 2022 13:19:47 -0700 Subject: [PATCH 2/5] Initial --- firmware/src/src/DCMAhrs/dcmahrs.cpp | 4 +- firmware/src/src/basetrackersettings.h | 32 ++-- firmware/src/src/sense.cpp | 199 ++++++++++++++----------- gui/src/basetrackersettings.h | 30 ++-- settings/settings.csv | 6 +- 5 files changed, 150 insertions(+), 121 deletions(-) diff --git a/firmware/src/src/DCMAhrs/dcmahrs.cpp b/firmware/src/src/DCMAhrs/dcmahrs.cpp index 0ab2f2ce..a5c40bb0 100644 --- a/firmware/src/src/DCMAhrs/dcmahrs.cpp +++ b/firmware/src/src/DCMAhrs/dcmahrs.cpp @@ -287,8 +287,8 @@ void DCMnormalization_Thread() void DcmCalculate(float u0[3], float u1[3], float u2[3], float deltat) { - float kp = trkset.Kp(); - float ki = trkset.Ki(); + float kp = trkset.getKp(); + float ki = trkset.getKi(); // Don't do calulations if no accel or mag data, will result in Nan if (fabs(u1[0] + u1[1] + u1[2]) < 0.0001) return; if (fabs(u2[0] + u2[1] + u2[2]) < 0.0001) return; diff --git a/firmware/src/src/basetrackersettings.h b/firmware/src/src/basetrackersettings.h index 2e98d8e4..e1208ad8 100644 --- a/firmware/src/src/basetrackersettings.h +++ b/firmware/src/src/basetrackersettings.h @@ -829,21 +829,21 @@ class BaseTrackerSettings { return false; } - // Low Pass filter For Pan - inline const uint8_t& getLpPan() {return lppan;} - bool setLpPan(uint8_t val=100) { - if(val >= 1 && val <= 100) { - lppan = val; + // Kp Value for DCM Algorithm + inline const float& getKp() {return kp;} + bool setKp(float val=1) { + if(val >= FLOAT_MIN && val <= FLOAT_MAX) { + kp = val; return true; } return false; } - // Low Pass filter For Tilt + Roll - inline const uint8_t& getLpTiltRoll() {return lptiltroll;} - bool setLpTiltRoll(uint8_t val=100) { - if(val >= 1 && val <= 100) { - lptiltroll = val; + // Ki Value for DCM Algorithm + inline const float& getKi() {return ki;} + bool setKi(float val=1) { + if(val >= FLOAT_MIN && val <= FLOAT_MAX) { + ki = val; return true; } return false; @@ -1066,8 +1066,8 @@ class BaseTrackerSettings { json["ppmframe"] = ppmframe; json["ppmsync"] = ppmsync; json["ppmchcnt"] = ppmchcnt; - json["lppan"] = lppan; - json["lptiltroll"] = lptiltroll; + json["kp"] = kp; + json["ki"] = ki; } void loadJSONSettings(DynamicJsonDocument &json) { @@ -1153,8 +1153,8 @@ class BaseTrackerSettings { v = json["ppmframe"]; if(!v.isNull()) {setPpmFrame(v);} v = json["ppmsync"]; if(!v.isNull()) {setPpmSync(v);} v = json["ppmchcnt"]; if(!v.isNull()) {setPpmChCnt(v);} - v = json["lppan"]; if(!v.isNull()) {setLpPan(v);} - v = json["lptiltroll"]; if(!v.isNull()) {setLpTiltRoll(v);} + v = json["kp"]; if(!v.isNull()) {setKp(v);} + v = json["ki"]; if(!v.isNull()) {setKi(v);} if(chresetfusion) resetFusion(); if(chpinschanged) @@ -1565,8 +1565,8 @@ class BaseTrackerSettings { uint16_t ppmframe = 22500; // PPM Frame Length (us) uint16_t ppmsync = 350; // PPM Sync Pulse Length (us) uint8_t ppmchcnt = 8; // PPM channels to output - uint8_t lppan = 100; // Low Pass filter For Pan - uint8_t lptiltroll = 100; // Low Pass filter For Tilt + Roll + float kp = 1; // Kp Value for DCM Algorithm + float ki = 1; // Ki Value for DCM Algorithm // Setting Arrays char btpairedaddress[19]; // Bluetooth Remote address to Pair With diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index 92bfd9a2..d1e9973b 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -15,15 +15,19 @@ * along with this program. If not, see . */ +#include "sense.h" + #include -#include #include +#include #include +#include "DCMAhrs/dcmahrs.h" #include "MadgwickAHRS/MadgwickAHRS.h" #include "uart_mode.h" #include "analog.h" #include "ble.h" +#include "defines.h" #include "filters.h" #include "filters/SF1eFilter.h" #include "io.h" @@ -33,8 +37,7 @@ #include "pmw.h" #include "soc_flash.h" #include "trackersettings.h" -#include "defines.h" -#include "sense.h" + #if defined(HAS_APDS9960) #include "APDS9960/APDS9960.h" @@ -61,7 +64,9 @@ static float rolloffset = 0, panoffset = 0, tiltoffset = 0; static float magxoff = 0, magyoff = 0, magzoff = 0; static float accxoff = 0, accyoff = 0, acczoff = 0; static float gyrxoff = 0, gyryoff = 0, gyrzoff = 0; -static float l_panout = 0, l_tiltout = 0, l_rollout = 0; +static float faccx = 0, faccy = 0, faccz = 0; +static float fmagx = 0, fmagy = 0, fmagz = 0; +static float fgyrx = 0, fgyry = 0, fgyrz = 0; static bool trpOutputEnabled = false; // Default to disabled T/R/P output // Input Channel Data @@ -107,7 +112,8 @@ volatile bool senseTreadRun = false; int sense_Init() { - const struct device* i2c_dev = device_get_binding("I2C_1"); // DEVICE_DT_GET(DT_NODELABEL(I2C_1)); + const struct device *i2c_dev = + device_get_binding("I2C_1"); // DEVICE_DT_GET(DT_NODELABEL(I2C_1)); if (!i2c_dev) { LOGE("Could not get device binding for I2C"); return false; @@ -179,9 +185,9 @@ void calculate_Thread() // Use a mutex so sensor data can't be updated part way k_mutex_lock(&sensor_mutex, K_FOREVER); - roll = madgwick.getPitch(); - tilt = madgwick.getRoll(); - pan = madgwick.getYaw(); + tilt = DcmGetTilt(); + roll = DcmGetRoll(); + pan = DcmGetPan(); k_mutex_unlock(&sensor_mutex); // Toggles output on and off if long pressed @@ -204,9 +210,7 @@ void calculate_Thread() // Zero button was pressed, adjust all values to zero bool butdnw = false; if (wasButtonPressed()) { - rolloffset = roll; - panoffset = pan; - tiltoffset = tilt; + DcmAhrsResetCenter(); butdnw = true; } @@ -222,24 +226,18 @@ void calculate_Thread() } // Tilt output - float tiltout = - (tilt - tiltoffset) * trkset.getTlt_Gain() * (trkset.isTiltReversed() ? -1.0 : 1.0); - float beta = (float)trkset.getLpTiltRoll() / 100; // LP Beta - filter_expAverage(&tiltout, beta, &l_tiltout); + float tiltout = tilt * trkset.getTlt_Gain() * (trkset.isTiltReversed() ? -1.0 : 1.0); uint16_t tiltout_ui = tiltout + trkset.getTlt_Cnt(); // Apply Center Offset tiltout_ui = MAX(MIN(tiltout_ui, trkset.getTlt_Max()), trkset.getTlt_Min()); // Limit Output // Roll output - float rollout = - (roll - rolloffset) * trkset.getRll_Gain() * (trkset.isRollReversed() ? -1.0 : 1.0); - filter_expAverage(&rollout, beta, &l_rollout); + float rollout = roll * trkset.getRll_Gain() * (trkset.isRollReversed() ? -1.0 : 1.0); uint16_t rollout_ui = rollout + trkset.getRll_Cnt(); // Apply Center Offset rollout_ui = MAX(MIN(rollout_ui, trkset.getRll_Max()), trkset.getRll_Min()); // Limit Output // Pan output, Normalize to +/- 180 Degrees - float panout = normalize((pan - panoffset), -180, 180) * trkset.getPan_Gain() * - (trkset.isPanReversed() ? -1.0 : 1.0); - filter_expAverage(&panout, (float)trkset.getLpPan() / 100, &l_panout); + float panout = + normalize(pan, -180, 180) * trkset.getPan_Gain() * (trkset.isPanReversed() ? -1.0 : 1.0); uint16_t panout_ui = panout + trkset.getPan_Cnt(); // Apply Center Offset panout_ui = MAX(MIN(panout_ui, trkset.getPan_Max()), trkset.getPan_Min()); // Limit Output @@ -620,6 +618,13 @@ void calculate_Thread() void sensor_Thread() { + float LPalpha, LPalphaC; + float lacc[3] = {0, 0, 0}; + float lgyr[3] = {0, 0, 0}; + float lmag[3] = {0, 0, 0}; + bool initLPfilter = true; + float lastUpdate = 0; + while (1) { if (!senseTreadRun || pauseForFlash) { rt_sleep_ms(10); @@ -628,6 +633,56 @@ void sensor_Thread() senseUsDuration = micros64(); + // Filtering + if (initLPfilter) { + for (int i = 0; i < 3; i++) { + LPalpha = exp(-(float)(SENSOR_PERIOD) / + (float)(SENSOR_LP_TIME_CST)); // low pass filter coefficient + LPalphaC = 0.5 * (1. - LPalpha); // complementary + faccx = accx; + faccy = accy; + faccz = accz; + fgyrx = gyrx; + fgyry = gyry; + fgyrz = gyrz; + fmagx = magx; + fmagy = magy; + fmagz = magz; + lacc[0] = accx; + lacc[1] = accy; + lacc[2] = accz; + lgyr[0] = gyrx; + lgyr[1] = gyry; + lgyr[2] = gyrz; + lmag[0] = magx; + lmag[1] = magy; + lmag[2] = magz; + initLPfilter = false; + } + } else { + faccx = faccx * LPalpha + LPalphaC * (accx + lacc[0]); + faccy = faccy * LPalpha + LPalphaC * (accy + lacc[1]); + faccz = faccz * LPalpha + LPalphaC * (accz + lacc[2]); + + fgyrx = fgyrx * LPalpha + LPalphaC * (gyrx + lgyr[0]); + fgyry = fgyry * LPalpha + LPalphaC * (gyry + lgyr[1]); + fgyrz = fgyrz * LPalpha + LPalphaC * (gyrz + lgyr[2]); + + fmagx = fmagx * LPalpha + LPalphaC * (magx + lmag[0]); + fmagy = fmagy * LPalpha + LPalphaC * (magy + lmag[1]); + fmagz = fmagz * LPalpha + LPalphaC * (magz + lmag[2]); + + lacc[0] = accx; + lacc[1] = accy; + lacc[2] = accz; + lgyr[0] = gyrx; + lgyr[1] = gyry; + lgyr[2] = gyrz; + lmag[0] = magx; + lmag[1] = magy; + lmag[2] = magz; + } + #if defined(HAS_APDS9960) // Reset Center on Proximity, Don't need to update this often static int sensecount = 0; @@ -669,9 +724,9 @@ void sensor_Thread() // Read the data from the sensors float tacc[3], tgyr[3], tmag[3]; - bool accValid=false; - bool gyrValid=false; - bool magValid=false; + bool accValid = false; + bool gyrValid = false; + bool magValid = false; #if defined(HAS_LSM9DS1) if (IMU.accelerationAvailable()) { @@ -691,7 +746,7 @@ void sensor_Thread() #endif #if defined(HAS_QMC5883) - if(qmc5883Read(tmag)) { + if (qmc5883Read(tmag)) { magValid = true; } #endif @@ -701,15 +756,14 @@ void sensor_Thread() short _gyro[3]; short _accel[3]; unsigned long timestamp; - if(!mpu_get_accel_reg(_accel, ×tamp)) - accValid = true; - unsigned short ascale = 1;; + if (!mpu_get_accel_reg(_accel, ×tamp)) accValid = true; + unsigned short ascale = 1; + ; mpu_get_accel_sens(&ascale); tacc[0] = (float)_accel[0] / (float)ascale; tacc[1] = (float)_accel[1] / (float)ascale; tacc[2] = (float)_accel[2] / (float)ascale; - if(!mpu_get_gyro_reg(_gyro, ×tamp)) - gyrValid = true; + if (!mpu_get_gyro_reg(_gyro, ×tamp)) gyrValid = true; float gscale = 1.0f; mpu_get_gyro_sens(&gscale); tgyr[0] = _gyro[0] / gscale; @@ -720,7 +774,7 @@ void sensor_Thread() k_mutex_lock(&sensor_mutex, K_FOREVER); // -- Accelerometer - if(accValid) { + if (accValid) { raccx = tacc[0]; raccy = tacc[1]; raccz = tacc[2]; @@ -744,7 +798,7 @@ void sensor_Thread() } // --- Gyrometer Calcs - if(gyrValid) { + if (gyrValid) { rgyrx = tgyr[0]; rgyry = tgyr[1]; rgyrz = tgyr[2]; @@ -764,7 +818,7 @@ void sensor_Thread() gyrz = tmpgyr[2]; } - if(magValid) { + if (magValid) { // --- Magnetometer Calcs rmagx = tmag[0]; rmagy = tmag[1]; @@ -790,63 +844,36 @@ void sensor_Thread() magx = tmpmag[0]; magy = tmpmag[1]; magz = tmpmag[2]; + } - // For inital orientation setup - madgsensbits |= MADGINIT_MAG; - } - - // Only do this update after the first mag and accel data have been read. - if (madgreads == 0) { - if (madgsensbits == MADGINIT_READY) { - madgsensbits = 0; - madgreads++; - aacc[0] = accx; - aacc[1] = accy; - aacc[2] = accz; - amag[0] = magx; - amag[1] = magy; - amag[2] = magz; - } + // Scale properly for DCM algorithm + // PAUL's reference frame is standard aeronautical: + // X axis is the longitudinal axis pointing ahead, + // Z axis is the vertical axis pointing downwards, + // Y axis is the lateral one, pointing in such a way that the frame is right-handed. + // PAUL's acceleration from accelerometer sign convention is opposite of used by rest of program + float u0[3], u1[3], u2[3]; + u0[0] = fgyrx * DEG_TO_RAD; + u0[1] = -fgyry * DEG_TO_RAD; + u0[2] = -fgyrz * DEG_TO_RAD; + u1[0] = -faccx; + u1[1] = faccy; + u1[2] = faccz; + u2[0] = fmagx; + u2[1] = -fmagy; + u2[2] = -fmagz; + + float now = micros(); + float deltat = ((now - lastUpdate) / 1000000.0f); + lastUpdate = now; - // Average samples - } else if (madgreads < MADGSTART_SAMPLES - 1) { - if (madgsensbits == MADGINIT_READY) { - madgsensbits = 0; - madgreads++; - aacc[0] += accx; - aacc[1] += accy; - aacc[2] += accz; - aacc[0] /= 2; - aacc[1] /= 2; - aacc[2] /= 2; - amag[0] += magx; - amag[1] += magy; - amag[2] += magz; - amag[0] /= 2; - amag[1] /= 2; - amag[2] /= 2; - } + k_mutex_lock(&sensor_mutex, K_FOREVER); + DcmCalculate(u0, u1, u2, deltat); + k_mutex_unlock(&sensor_mutex); - // Got the averaged values, apply the initial orientation. - } else if (madgreads == MADGSTART_SAMPLES - 1) { - // Pass it averaged values - madgwick.begin(aacc[0], aacc[1], aacc[2], amag[0], amag[1], amag[2]); - panoffset = pan; - madgreads = MADGSTART_SAMPLES; - } - - // Do the AHRS calculations - if (madgreads == MADGSTART_SAMPLES) { - // Period Between Samples - madgwick.update(gyrx * DEG_TO_RAD, gyry * DEG_TO_RAD, gyrz * DEG_TO_RAD, accx, accy, accz, - magx, magy, magz, madgwick.deltatUpdate()); - if (firstrun) { - panoffset = pan; - firstrun = false; - } - } + // Adjust sleep for a more accurate period + usduration = micros64() - usduration; - k_mutex_unlock(&sensor_mutex); // Adjust sleep for a more accurate period senseUsDuration = micros64() - senseUsDuration; diff --git a/gui/src/basetrackersettings.h b/gui/src/basetrackersettings.h index 624efd54..de462e57 100644 --- a/gui/src/basetrackersettings.h +++ b/gui/src/basetrackersettings.h @@ -146,8 +146,8 @@ class BaseTrackerSettings : public QObject _setting["ppmframe"] = 22500; _setting["ppmsync"] = 350; _setting["ppmchcnt"] = 8; - _setting["lppan"] = 100; - _setting["lptiltroll"] = 100; + _setting["kp"] = 1; + _setting["ki"] = 1; _setting["btpairedaddress[17]"] = QString(""); _dataItems["magx"] = false; _dataItems["magy"] = false; @@ -1103,30 +1103,32 @@ class BaseTrackerSettings : public QObject return false; } - // Low Pass filter For Pan - uint8_t getLpPan() { - return _setting["lppan"].toUInt(); + // Kp Value for DCM Algorithm + float getKp() { + return _setting["kp"].toFloat(); } - bool setLpPan(uint8_t val=100) { - if(val >= 1 && val <= 100) { - _setting["lppan"] = val; + bool setKp(float val=1) { + if(val >= FLOAT_MIN && val <= FLOAT_MAX) { + _setting["kp"] = QString::number(val,'g',); return true; } return false; } - // Low Pass filter For Tilt + Roll - uint8_t getLpTiltRoll() { - return _setting["lptiltroll"].toUInt(); + + // Ki Value for DCM Algorithm + float getKi() { + return _setting["ki"].toFloat(); } - bool setLpTiltRoll(uint8_t val=100) { - if(val >= 1 && val <= 100) { - _setting["lptiltroll"] = val; + bool setKi(float val=1) { + if(val >= FLOAT_MIN && val <= FLOAT_MAX) { + _setting["ki"] = QString::number(val,'g',); return true; } return false; } + // Bluetooth Remote address to Pair With QString getBtPairedAddress() { return _setting["btpairedaddress"].toString(); diff --git a/settings/settings.csv b/settings/settings.csv index ed8c665e..1988f3c7 100644 --- a/settings/settings.csv +++ b/settings/settings.csv @@ -179,9 +179,9 @@ u16,Setting,PpmFrame,22500,PPM_MIN_FRAME,PPM_MAX_FRAME,PPM Frame Length (us),,,, u16,Setting,PpmSync,350,100,800,PPM Sync Pulse Length (us),,,, u8,Setting,PpmChCnt,8,1,16,PPM channels to output,,,, ,,,,,,,,,, -Filter Settings,,,,,,,,,, -u8,Setting,LpPan,100,1,100,Low Pass filter For Pan,,,, -u8,Setting,LpTiltRoll,100,1,100,Low Pass filter For Tilt + Roll,,,, +IMU Setttings,,,,,,,,,, +float,Setting,Kp,1,FLOAT_MIN,FLOAT_MAX,Kp Value for DCM Algorithm,,,, +float,Setting,Ki,1,FLOAT_MIN,FLOAT_MAX,Ki Value for DCM Algorithm,,,, ,,,,,,,,,, Notes,,,,,,,,,, bool Doesn't need min/max settings,,,,,,,,,, From a1eae24c695462bf6a9a3aa66d441b2e2ae0aaa8 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Mon, 31 Oct 2022 13:24:22 -0700 Subject: [PATCH 3/5] Add define and extern --- firmware/src/src/defines.h | 5 +++-- firmware/src/src/sense.h | 7 +------ 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/firmware/src/src/defines.h b/firmware/src/src/defines.h index b1a55a67..7441bc9c 100644 --- a/firmware/src/src/defines.h +++ b/firmware/src/src/defines.h @@ -45,6 +45,9 @@ #define PWM_FREQUENCY 50 // (ms) PWM Period #define PAUSE_BEFORE_FLASH 60 // (ms) Time to pause all threads before Flash writing +// Sensors LP filter time constant +#define SENSOR_LP_TIME_CST 60000 // (us) Low pass filter time constant + // Analog Filters 1 Euro Filter #define AN_CH_CNT 4 #define AN_FILT_FREQ 150 @@ -126,8 +129,6 @@ extern volatile bool sbusTreadRun; #define DEG_TO_RAD 0.017453295199 #define RAD_TO_DEG 57.29577951308 - - // Magnetometer, Initial Orientation, Samples to average #define MADGSTART_SAMPLES 15 diff --git a/firmware/src/src/sense.h b/firmware/src/src/sense.h index 0b4b2b82..727489d8 100644 --- a/firmware/src/src/sense.h +++ b/firmware/src/src/sense.h @@ -2,12 +2,7 @@ #define APDS_HYSTERISIS 10 -// Oversample Setting -#if defined(MAHONY) || defined(MADGWICK) -const int SENSEUPDATE = 1; -#else -const int SENSEUPDATE = 6; -#endif +extern struct k_mutex sensor_mutex; int sense_Init(); void sensor_Thread(); From 606e4d69534a9e833dce60ff3c5a55971a02bb15 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Mon, 31 Oct 2022 20:36:44 -0700 Subject: [PATCH 4/5] Updates --- firmware/src/src/basetrackersettings.h | 34 ++++- firmware/src/src/defines.h | 2 +- firmware/src/src/nano33ble.cpp | 3 + firmware/src/src/sense.cpp | 143 +++++++++++------- gui/src/basetrackersettings.h | 38 ++++- gui/src/mainwindow.cpp | 12 +- gui/src/mainwindow.ui | 192 +++++++------------------ settings/settings.csv | 8 +- 8 files changed, 213 insertions(+), 219 deletions(-) diff --git a/firmware/src/src/basetrackersettings.h b/firmware/src/src/basetrackersettings.h index e1208ad8..19979854 100644 --- a/firmware/src/src/basetrackersettings.h +++ b/firmware/src/src/basetrackersettings.h @@ -829,9 +829,29 @@ class BaseTrackerSettings { return false; } + // Low Pass filter For Pan + inline const uint8_t& getLpPan() {return lppan;} + bool setLpPan(uint8_t val=100) { + if(val >= 1 && val <= 100) { + lppan = val; + return true; + } + return false; + } + + // Low Pass filter For Tilt + Roll + inline const uint8_t& getLpTiltRoll() {return lptiltroll;} + bool setLpTiltRoll(uint8_t val=100) { + if(val >= 1 && val <= 100) { + lptiltroll = val; + return true; + } + return false; + } + // Kp Value for DCM Algorithm inline const float& getKp() {return kp;} - bool setKp(float val=1) { + bool setKp(float val=0.2) { if(val >= FLOAT_MIN && val <= FLOAT_MAX) { kp = val; return true; @@ -841,7 +861,7 @@ class BaseTrackerSettings { // Ki Value for DCM Algorithm inline const float& getKi() {return ki;} - bool setKi(float val=1) { + bool setKi(float val=0.01) { if(val >= FLOAT_MIN && val <= FLOAT_MAX) { ki = val; return true; @@ -1066,6 +1086,8 @@ class BaseTrackerSettings { json["ppmframe"] = ppmframe; json["ppmsync"] = ppmsync; json["ppmchcnt"] = ppmchcnt; + json["lppan"] = lppan; + json["lptiltroll"] = lptiltroll; json["kp"] = kp; json["ki"] = ki; } @@ -1153,6 +1175,8 @@ class BaseTrackerSettings { v = json["ppmframe"]; if(!v.isNull()) {setPpmFrame(v);} v = json["ppmsync"]; if(!v.isNull()) {setPpmSync(v);} v = json["ppmchcnt"]; if(!v.isNull()) {setPpmChCnt(v);} + v = json["lppan"]; if(!v.isNull()) {setLpPan(v);} + v = json["lptiltroll"]; if(!v.isNull()) {setLpTiltRoll(v);} v = json["kp"]; if(!v.isNull()) {setKp(v);} v = json["ki"]; if(!v.isNull()) {setKi(v);} if(chresetfusion) @@ -1565,8 +1589,10 @@ class BaseTrackerSettings { uint16_t ppmframe = 22500; // PPM Frame Length (us) uint16_t ppmsync = 350; // PPM Sync Pulse Length (us) uint8_t ppmchcnt = 8; // PPM channels to output - float kp = 1; // Kp Value for DCM Algorithm - float ki = 1; // Ki Value for DCM Algorithm + uint8_t lppan = 100; // Low Pass filter For Pan + uint8_t lptiltroll = 100; // Low Pass filter For Tilt + Roll + float kp = 0.2; // Kp Value for DCM Algorithm + float ki = 0.01; // Ki Value for DCM Algorithm // Setting Arrays char btpairedaddress[19]; // Bluetooth Remote address to Pair With diff --git a/firmware/src/src/defines.h b/firmware/src/src/defines.h index 7441bc9c..e16d69d8 100644 --- a/firmware/src/src/defines.h +++ b/firmware/src/src/defines.h @@ -46,7 +46,7 @@ #define PAUSE_BEFORE_FLASH 60 // (ms) Time to pause all threads before Flash writing // Sensors LP filter time constant -#define SENSOR_LP_TIME_CST 60000 // (us) Low pass filter time constant +#define SENSOR_LP_TIME_CST 15000 // (us) Low pass filter time constant // Analog Filters 1 Euro Filter #define AN_CH_CNT 4 diff --git a/firmware/src/src/nano33ble.cpp b/firmware/src/src/nano33ble.cpp index 17c6ab49..b8009c46 100644 --- a/firmware/src/src/nano33ble.cpp +++ b/firmware/src/src/nano33ble.cpp @@ -6,6 +6,7 @@ #include #include +#include "DCMAhrs/dcmahrs.h" #include "PPMIn.h" #include "PPMOut.h" #include "uart_mode.h" @@ -74,6 +75,8 @@ K_THREAD_DEFINE(calculate_Thread_id, 4096, calculate_Thread, NULL, NULL, NULL, CALCULATE_THREAD_PRIO, K_FP_REGS, 1000); K_THREAD_DEFINE(uartTx_Thread_ID, 1024, uartTx_Thread, NULL, NULL, NULL, UARTTX_THREAD_PRIO, 0, 1000); K_THREAD_DEFINE(uartRx_Thread_ID, 1024, uartRx_Thread, NULL, NULL, NULL, UARTRX_THREAD_PRIO, 0, 1000); +K_THREAD_DEFINE(DCMnormalization_Thread_id, 1024, DCMnormalization_Thread, NULL, NULL, NULL, + CALCULATE_THREAD_PRIO, K_FP_REGS, 3000); #elif defined(RTOS_FREERTOS) #error "TODO... Add tasks for FreeRTOS" diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index d1e9973b..d7cd390f 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -38,6 +38,7 @@ #include "soc_flash.h" #include "trackersettings.h" +#define DEBUG_SENSOR_RATES #if defined(HAS_APDS9960) #include "APDS9960/APDS9960.h" @@ -633,56 +634,6 @@ void sensor_Thread() senseUsDuration = micros64(); - // Filtering - if (initLPfilter) { - for (int i = 0; i < 3; i++) { - LPalpha = exp(-(float)(SENSOR_PERIOD) / - (float)(SENSOR_LP_TIME_CST)); // low pass filter coefficient - LPalphaC = 0.5 * (1. - LPalpha); // complementary - faccx = accx; - faccy = accy; - faccz = accz; - fgyrx = gyrx; - fgyry = gyry; - fgyrz = gyrz; - fmagx = magx; - fmagy = magy; - fmagz = magz; - lacc[0] = accx; - lacc[1] = accy; - lacc[2] = accz; - lgyr[0] = gyrx; - lgyr[1] = gyry; - lgyr[2] = gyrz; - lmag[0] = magx; - lmag[1] = magy; - lmag[2] = magz; - initLPfilter = false; - } - } else { - faccx = faccx * LPalpha + LPalphaC * (accx + lacc[0]); - faccy = faccy * LPalpha + LPalphaC * (accy + lacc[1]); - faccz = faccz * LPalpha + LPalphaC * (accz + lacc[2]); - - fgyrx = fgyrx * LPalpha + LPalphaC * (gyrx + lgyr[0]); - fgyry = fgyry * LPalpha + LPalphaC * (gyry + lgyr[1]); - fgyrz = fgyrz * LPalpha + LPalphaC * (gyrz + lgyr[2]); - - fmagx = fmagx * LPalpha + LPalphaC * (magx + lmag[0]); - fmagy = fmagy * LPalpha + LPalphaC * (magy + lmag[1]); - fmagz = fmagz * LPalpha + LPalphaC * (magz + lmag[2]); - - lacc[0] = accx; - lacc[1] = accy; - lacc[2] = accz; - lgyr[0] = gyrx; - lgyr[1] = gyry; - lgyr[2] = gyrz; - lmag[0] = magx; - lmag[1] = magy; - lmag[2] = magz; - } - #if defined(HAS_APDS9960) // Reset Center on Proximity, Don't need to update this often static int sensecount = 0; @@ -834,9 +785,9 @@ void sensor_Thread() magy = rmagy - magyoff; magz = rmagz - magzoff; - magx = (magx * magsioff[0]) + (magy * magsioff[1]) + (magz * magsioff[2]); + /*magx = (magx * magsioff[0]) + (magy * magsioff[1]) + (magz * magsioff[2]); magy = (magx * magsioff[3]) + (magy * magsioff[4]) + (magz * magsioff[5]); - magz = (magx * magsioff[6]) + (magy * magsioff[7]) + (magz * magsioff[8]); + magz = (magx * magsioff[6]) + (magy * magsioff[7]) + (magz * magsioff[8]);*/ // Apply Rotation float tmpmag[3] = {magx, magy, magz}; @@ -846,7 +797,71 @@ void sensor_Thread() magz = tmpmag[2]; } - // Scale properly for DCM algorithm + // Filtering +#ifdef DO_FILTER + if (magValid && gyrValid && accValid && initLPfilter) { + LPalpha = exp(-(float)(SENSOR_PERIOD) / + (float)(SENSOR_LP_TIME_CST)); // low pass filter coefficient + LPalphaC = 0.5 * (1. - LPalpha); // complementary + faccx = accx; + faccy = accy; + faccz = accz; + fgyrx = gyrx; + fgyry = gyry; + fgyrz = gyrz; + fmagx = magx; + fmagy = magy; + fmagz = magz; + lacc[0] = accx; + lacc[1] = accy; + lacc[2] = accz; + lgyr[0] = gyrx; + lgyr[1] = gyry; + lgyr[2] = gyrz; + lmag[0] = magx; + lmag[1] = magy; + lmag[2] = magz; + initLPfilter = false; + } else if (initLPfilter == false) { + if (accValid) { + faccx = faccx * LPalpha + LPalphaC * (accx + lacc[0]); + faccy = faccy * LPalpha + LPalphaC * (accy + lacc[1]); + faccz = faccz * LPalpha + LPalphaC * (accz + lacc[2]); + lacc[0] = accx; + lacc[1] = accy; + lacc[2] = accz; + } + + if (gyrValid) { + fgyrx = fgyrx * LPalpha + LPalphaC * (gyrx + lgyr[0]); + fgyry = fgyry * LPalpha + LPalphaC * (gyry + lgyr[1]); + fgyrz = fgyrz * LPalpha + LPalphaC * (gyrz + lgyr[2]); + lgyr[0] = gyrx; + lgyr[1] = gyry; + lgyr[2] = gyrz; + } + + if (magValid) { + fmagx = fmagx * LPalpha + LPalphaC * (magx + lmag[0]); + fmagy = fmagy * LPalpha + LPalphaC * (magy + lmag[1]); + fmagz = fmagz * LPalpha + LPalphaC * (magz + lmag[2]); + lmag[0] = magx; + lmag[1] = magy; + lmag[2] = magz; + } + } +#else + faccx = accx; + faccy = accy; + faccz = accx; + fgyrx = gyrx; + fgyry = gyry; + fgyrz = gyrz; + fmagx = magx; + fmagy = magy; + fmagz = magz; +#endif + // Scale properly for DCM algorithm // PAUL's reference frame is standard aeronautical: // X axis is the longitudinal axis pointing ahead, // Z axis is the vertical axis pointing downwards, @@ -867,14 +882,32 @@ void sensor_Thread() float deltat = ((now - lastUpdate) / 1000000.0f); lastUpdate = now; - k_mutex_lock(&sensor_mutex, K_FOREVER); DcmCalculate(u0, u1, u2, deltat); k_mutex_unlock(&sensor_mutex); +#if defined(DEBUG_SENSOR_RATES) + static int mcount = 1; + static int mcounta = 1; + static int mcountg = 1; + static int mcountm = 1; + static int64_t mmic = millis64() + 1000; + if (mmic < millis64()) { // Every Second + mmic = millis64() + 1000; + LOGI("Sens Rate = %d, Acc=%d, Gyr=%d, Mag=%d", mcount, mcounta, mcountg, mcountm); + mcount = 1; + mcounta = 1; + mcountg = 1; + mcountm = 1; + } + mcount++; + if (accValid) mcounta++; + if (magValid) mcountm++; + if (gyrValid) mcountg++; +#endif + // Adjust sleep for a more accurate period usduration = micros64() - usduration; - // Adjust sleep for a more accurate period senseUsDuration = micros64() - senseUsDuration; if (SENSOR_PERIOD - senseUsDuration < diff --git a/gui/src/basetrackersettings.h b/gui/src/basetrackersettings.h index de462e57..75bf9ff3 100644 --- a/gui/src/basetrackersettings.h +++ b/gui/src/basetrackersettings.h @@ -146,8 +146,10 @@ class BaseTrackerSettings : public QObject _setting["ppmframe"] = 22500; _setting["ppmsync"] = 350; _setting["ppmchcnt"] = 8; - _setting["kp"] = 1; - _setting["ki"] = 1; + _setting["lppan"] = 100; + _setting["lptiltroll"] = 100; + _setting["kp"] = 0.2; + _setting["ki"] = 0.01; _setting["btpairedaddress[17]"] = QString(""); _dataItems["magx"] = false; _dataItems["magy"] = false; @@ -1103,13 +1105,37 @@ class BaseTrackerSettings : public QObject return false; } + // Low Pass filter For Pan + uint8_t getLpPan() { + return _setting["lppan"].toUInt(); + } + bool setLpPan(uint8_t val=100) { + if(val >= 1 && val <= 100) { + _setting["lppan"] = val; + return true; + } + return false; + } + + // Low Pass filter For Tilt + Roll + uint8_t getLpTiltRoll() { + return _setting["lptiltroll"].toUInt(); + } + bool setLpTiltRoll(uint8_t val=100) { + if(val >= 1 && val <= 100) { + _setting["lptiltroll"] = val; + return true; + } + return false; + } + // Kp Value for DCM Algorithm float getKp() { return _setting["kp"].toFloat(); } - bool setKp(float val=1) { + bool setKp(float val=0.2) { if(val >= FLOAT_MIN && val <= FLOAT_MAX) { - _setting["kp"] = QString::number(val,'g',); + _setting["kp"] = QString::number(val,'g',4); return true; } return false; @@ -1120,9 +1146,9 @@ class BaseTrackerSettings : public QObject float getKi() { return _setting["ki"].toFloat(); } - bool setKi(float val=1) { + bool setKi(float val=0.01) { if(val >= FLOAT_MIN && val <= FLOAT_MAX) { - _setting["ki"] = QString::number(val,'g',); + _setting["ki"] = QString::number(val,'g',4); return true; } return false; diff --git a/gui/src/mainwindow.cpp b/gui/src/mainwindow.cpp index fba1ae71..160ddf42 100644 --- a/gui/src/mainwindow.cpp +++ b/gui/src/mainwindow.cpp @@ -130,8 +130,6 @@ MainWindow::MainWindow(QWidget *parent) //connect(ui->chkRawData,SIGNAL(clicked(bool)),this,SLOT(setDataMode(bool))); // Spin Boxes - connect(ui->spnLPPan,SIGNAL(valueChanged(int)),this,SLOT(updateFromUI())); - connect(ui->spnLPTiltRoll,SIGNAL(valueChanged(int)),this,SLOT(updateFromUI())); connect(ui->spnLPPan2,SIGNAL(valueChanged(int)),this,SLOT(updateFromUI())); connect(ui->spnLPTiltRoll2,SIGNAL(valueChanged(int)),this,SLOT(updateFromUI())); connect(ui->spnPPMSync,SIGNAL(valueChanged(int)),this,SLOT(updateFromUI())); @@ -222,7 +220,7 @@ MainWindow::MainWindow(QWidget *parent) connect(ui->actionFirmware_Wizard,SIGNAL(triggered()),this,SLOT(uploadFirmwareWizard())); connect(ui->actionShow_Data,SIGNAL(triggered()),this,SLOT(showDiagsClicked())); connect(ui->actionShow_Serial_Transmissions,SIGNAL(triggered()),this,SLOT(showSerialDiagClicked())); - connect(ui->actionChannel_Viewer,SIGNAL(triggered()),this,SLOT(showChannelViewerClicked())); + connect(ui->actionChannel_Viewer,SIGNAL(triggered()),this,SLOT(showChannelViewerClicked())); connect(ui->actionPinout, SIGNAL(triggered()),this, SLOT(showPinView())); connect(ui->actionEraseFlash, SIGNAL(triggered()),this, SLOT(eraseFlash())); connect(ui->actionOnline_Help, SIGNAL(triggered()),this, SLOT(openHelp())); @@ -585,8 +583,6 @@ void MainWindow::updateToUI() ui->spnPPMFrameLen->setMinimum((double)TrackerSettings::PPM_MIN_FRAME / 1000.0); ui->spnPPMFrameLen->setMaximum((double)TrackerSettings::PPM_MAX_FRAME / 1000.0); - ui->spnLPTiltRoll->setValue(trkset.getLpTiltRoll()); - ui->spnLPPan->setValue(trkset.getLpPan()); ui->spnLPTiltRoll2->setValue(trkset.getLpTiltRoll()); ui->spnLPPan2->setValue(trkset.getLpPan()); ui->spnA0Gain->setValue(trkset.getAn0Gain()); @@ -743,11 +739,7 @@ void MainWindow::updateFromUI() trkset.setRll_Gain(static_cast(ui->rll_gain->value())/10.0f); // Filters - if(trkset.hardware() == "NANO33BLE" || - trkset.hardware() == "DTQSYS") { - trkset.setLpTiltRoll(ui->spnLPTiltRoll->value()); - trkset.setLpPan(ui->spnLPPan->value()); - } else if (trkset.hardware() == "BNO055") { + if (trkset.hardware() == "BNO055") { trkset.setLpTiltRoll(ui->spnLPTiltRoll2->value()); trkset.setLpPan(ui->spnLPPan2->value()); } diff --git a/gui/src/mainwindow.ui b/gui/src/mainwindow.ui index 144d260e..6af73fdd 100644 --- a/gui/src/mainwindow.ui +++ b/gui/src/mainwindow.ui @@ -532,14 +532,14 @@ false - 5 + 0 General - + Qt::Vertical @@ -552,37 +552,6 @@ - - - - - 0 - 0 - - - - - 50 - 25 - - - - - 8 - 0 - - - - % - - - 1 - - - 100 - - - @@ -672,42 +641,6 @@ - - - - <html><head/><body><p>Sets the output channel you want tilt on</p></body></html> - - - Center on Proximity Detect (BLE Sense Only) - - - - - - - - 0 - 0 - - - - Low Pass Filter on Tilt/Roll - - - - - - - - 0 - 0 - - - - Low Pass Filter on Pan - - - @@ -775,76 +708,6 @@ - - - - - 0 - 0 - - - - Board Rotation - - - - - - - - 0 - 0 - - - - - 50 - 25 - - - - - 8 - 0 - - - - % - - - 1 - - - 100 - - - - - - - - 0 - 0 - - - - Center Button - - - - - - - - 0 - 0 - - - - Send Recenter on Chan - - - @@ -937,6 +800,55 @@ + + + + + 0 + 0 + + + + Send Recenter on Chan + + + + + + + <html><head/><body><p>Sets the output channel you want tilt on</p></body></html> + + + Center on Proximity Detect (BLE Sense Only) + + + + + + + + 0 + 0 + + + + Center Button + + + + + + + + 0 + 0 + + + + Board Rotation + + + @@ -4436,8 +4348,6 @@ spnRotY spnRotZ chkResetCenterWave - spnLPTiltRoll - spnLPPan spnPPMFrameLen cmbPpmOutPin cmbBtMode diff --git a/settings/settings.csv b/settings/settings.csv index 1988f3c7..e7214adb 100644 --- a/settings/settings.csv +++ b/settings/settings.csv @@ -179,9 +179,13 @@ u16,Setting,PpmFrame,22500,PPM_MIN_FRAME,PPM_MAX_FRAME,PPM Frame Length (us),,,, u16,Setting,PpmSync,350,100,800,PPM Sync Pulse Length (us),,,, u8,Setting,PpmChCnt,8,1,16,PPM channels to output,,,, ,,,,,,,,,, +Filter Settings,,,,,,,,,, +u8,Setting,LpPan,100,1,100,Low Pass filter For Pan,,,, +u8,Setting,LpTiltRoll,100,1,100,Low Pass filter For Tilt + Roll,,,, +,,,,,,,,,, IMU Setttings,,,,,,,,,, -float,Setting,Kp,1,FLOAT_MIN,FLOAT_MAX,Kp Value for DCM Algorithm,,,, -float,Setting,Ki,1,FLOAT_MIN,FLOAT_MAX,Ki Value for DCM Algorithm,,,, +float,Setting,Kp,0.2,FLOAT_MIN,FLOAT_MAX,Kp Value for DCM Algorithm,,,4, +float,Setting,Ki,0.01,FLOAT_MIN,FLOAT_MAX,Ki Value for DCM Algorithm,,,4, ,,,,,,,,,, Notes,,,,,,,,,, bool Doesn't need min/max settings,,,,,,,,,, From 98f9ef874da7f4862e915b0bc58f40fa34f51cf7 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Thu, 3 Nov 2022 20:33:45 -0700 Subject: [PATCH 5/5] Temp, Add new code --- .../src/DCMAhrs/Board_Orientation_V0_R2.txt | 319 ++++++++++++++++++ 1 file changed, 319 insertions(+) create mode 100644 firmware/src/src/DCMAhrs/Board_Orientation_V0_R2.txt diff --git a/firmware/src/src/DCMAhrs/Board_Orientation_V0_R2.txt b/firmware/src/src/DCMAhrs/Board_Orientation_V0_R2.txt new file mode 100644 index 00000000..0ffb9b23 --- /dev/null +++ b/firmware/src/src/DCMAhrs/Board_Orientation_V0_R2.txt @@ -0,0 +1,319 @@ +/*----------------------------------------------------------------------------*/ +void Board_Orientation(float *a1, float *m1, float *a2, float *m2, float *a3, + float *m3, float *R) +/*----------------------------------------------------------------------------*/ +/* Author: Paul BIZARD 11/20/2022 */ +/* Version 1 Revision 2 */ +/*----------------------------------------------------------------------------*/ +/* This routine computes the orientation (rotation) matrix R of the head */ +/* tracker board relatively to the operator's head. The board can be attached */ +/* to the operator's headset in any possible manner */ +/* */ +/* Process: the operator takes 3 snapshots of the accelerometer ax ay az */ +/* (vector a) and magnetometer mx my mz values (vector m) */ +/* */ +/* 1st snapshot: the operator's head is level and looking straight ahead. */ +/* Related accelerometer and magnetometer vectors are a1 and m1 */ +/* */ +/* 2nd snapshot: the operator's head is tilted upward relatively to the 1st */ +/* snapshot. Tilt angle value should be +45° or more. */ +/* Related accelerometer and magnetometer vectors are a2 and m2 */ +/* */ +/* 3rd snapshot: the operator's head is kept level but panned to the left */ +/* relatively to the 1st snapshot. Pan angle value should be +45° or more. */ +/* Related accelerometer and magnetometer vectors are a3 and m3 */ +/* */ +/* The output of this routine is a rotation matrix R */ +/* x y z */ +/* 1st column is x board vector expressed in operator frame R[0] R[1] R[2] */ +/* 2nd... y R[3] R[4] R[5] */ +/* 3rd... z R[6] R[7] R[8] */ +/* */ +/* This matrix is used to rotate the accelerometer, magnetometer and */ +/* gyrometer sensors values from the board frame to the operator's frame */ +/* Example: */ +/* az acceleration as read by board in its own frame of reference */ +/* Az acceleration in operator's frame of reference */ +/* Ax = R[0]*ax + R[1]*ay + R[2]*az */ +/* Ay = R[3]*ax + R[4]*ay + R[5]*az */ +/* Az = R[6]*ax + R[7]*ay + R[8]*az */ +/* */ +/* Note: in what follows the DCM matix is the transpose of the R matrix */ +/* */ +/* Frames conventions: see Cliff Blackburn's Head Tracker video: */ +/* https://www.youtube.com/watch?v=DwtC1GqwQ4U&t=234s */ +/* x right */ +/* y forward */ +/* z up */ +/* Sign of angles: right hand rule */ +/*----------------------------------------------------------------------------*/ +{ + /* Declarations */ + float DCM1[9], DCM2[9], DCM3[9]; + float DCM_T[9], DCM_P[9]; + float f_buff1, f_buff2, M_buff[9]; + float Q_T[4], Q_P[4]; + float angle_T, angle_P; + float T[3], P[3], TpP[3], TmP[3]; + float Xo[3], Yo[3], Zo[3]; + int i; + + + /* Compute the DCM matrices related to the 3 snapshots */ + snapshotDCM(a1, m1, DCM1); + snapshotDCM(a2, m2, DCM2); + snapshotDCM(a3, m3, DCM3); + + /* Tilt procedure DCM */ + TransposeMatrix(DCM1, M_buff); + MatrixMultiply2(DCM2, M_buff, DCM_T); + + /* Pan procedure DCM */ + MatrixMultiply2(DCM3, M_buff, DCM_P); + + /* Tilt procedure quaternion */ + DCM2QUAT(DCM_T, Q_T); + + /* Pan procedure quaternion */ + DCM2QUAT(DCM_P, Q_P); + + /* Tilt and pan unit vectors expressed in board frame. */ + /* Associated tilt and pan angles could be compared to a */ + /* minimum value (e.g. 30°). TBD: Should test fail the */ + /* orientation procedure would be undertaken again */ + QUAT2AXIS(Q_T, T, &angle_T); + QUAT2AXIS(Q_P, P, &angle_P); + + /* Tilt and pan unit vectors must be orthonormalized to */ + /* eventually become the operator's frame x and z axis */ + /* expressed in the board frame of reference */ + + /* Orthogonalization */ + for (i=0; i<3; i++) + { + TpP[i] = T[i] + P[i]; + TmP[i] = T[i] - P[i]; + } + + VectorDotProduct(TpP, TpP, &f_buff1); + VectorDotProduct(TmP, TmP, &f_buff2); + f_buff1 = sqrt(f_buff1/f_buff2); + + for (i=0; i<3; i++) + { + T[i] = 0.5*(TpP[i] + TmP[i]*f_buff1); + P[i] = 0.5*(TpP[i] - TmP[i]*f_buff1); + } + + /* Normalization */ + VectorDotProduct(T, T, &f_buff1); + if (f_buff1 > 0.001) f_buff1 = 1. / sqrt(f_buff1); + for (i=0; i<3; i++) Xo[i] = T[i] * f_buff1; + + VectorDotProduct(P, P, &f_buff1); + if (f_buff1 > 0.001) f_buff1 = 1. / sqrt(f_buff1); + for (i=0; i<3; i++) Zo[i] = P[i] * f_buff1; + + /* Now compute Yo as the cross product of Zo and Xo. */ + /* So now we have the operator's reference frame expressed */ + /* in the board frame of reference */ + VectorCross(Zo, Xo, Yo); + + /* Now here is the rotation matrix we were looking for */ + /* Note that it is the inverse of a DCM matrix */ + R[0] = Xo[0]; R[1] = Xo[1]; R[2] = Xo[2]; + R[3] = Yo[0]; R[4] = Yo[1]; R[5] = Yo[2]; + R[6] = Zo[0]; R[7] = Zo[1]; R[8] = Zo[2]; +} + +/*----------------------------------------------------------------------------*/ +void snapshotDCM(float *acc, float *mag, float *DCM) +/*----------------------------------------------------------------------------*/ +/* Author: Paul BIZARD 11/20/2022 */ +/* Version 1 Revision 0 */ +/*----------------------------------------------------------------------------*/ +/* This routine computes the DCM matrix associated to the rotation of the */ +/* board with respect to the frame of reference of the Earth */ +/* It computes the DCM matrix from the accelerometer and magnetometer vectors */ +/* The DCM matrix translates from a vector expressed in the earth frame to */ +/* the same vector but expressed in the board frame */ +/* x y z */ +/* 1st column is x earth vector expressed in board frame DCM[0] DCM[1] DCM[2] */ +/* 2nd... y DCM[3] DCM[4] DCM[5] */ +/* 3rd... z DCM[6] DCM[7] DCM[8] */ +/* */ +/* Note & convention: the DCM matix is the transpose/inverse of the */ +/* associated R matrix */ +/* */ +/* Frames conventions: see Cliff Blackburn's Head Tracker video: */ +/* https://www.youtube.com/watch?v=DwtC1GqwQ4U&t=234s */ +/* Board frame of reference: */ +/* x right */ +/* y forward */ +/* z up */ +/* Earth frame of reference: */ +/* x east */ +/* y magnetic north */ +/* z up */ +/* Sign of angles: right hand rule */ +/*----------------------------------------------------------------------------*/ +{ + int i, j; + float tilt, roll, pan; + float BeX, BeY; + float Ct, St, Cr, Sr, Cp, Sp; + float f_buff; + float V_buff[3]; + + + /* Compute tilt and roll with acceleration vector */ + /* ============================================== */ + /* Normalize acceleration vector */ + VectorDotProduct(acc, acc, &f_buff); + + if (f_buff > 0.001) f_buff = 1. / sqrt(f_buff); + + for (i=0; i<3; i++) V_buff[i] = acc[i] * f_buff; + + /* Tilt and roll in radians */ + tilt = atan2(V_buff[1], V_buff[2]); + roll = -asin(V_buff[0]); + + + /* Compute pan with magnetic field vector */ + /* ====================================== */ + /* Straighten magnetic field x and y (remove tilt and roll) */ + Ct = cos(tilt); St = sin(tilt); + Cr = cos(roll); Sr = sin(roll); + + BeX = Cr*mag[0] + St*Sr*mag[1] + Sr*Ct*mag[2]; + BeY = Ct*mag[1] - St*mag[2]; + + /* Pan in radians */ + /* No need to normalize because atan2 is a ratio */ + pan = atan2(BeX, BeY); + + Cp = cos(pan); Sp = sin(pan); + + DCM[0] = Cp*Cr ; DCM[1] = Sp*Cr; DCM[2] = -Sr; + DCM[3] = Cp*Sr*St-Sp*Ct; DCM[4] = Sp*Sr*St+Cp*Ct; DCM[5] = Cr*St; + DCM[6] = Cp*Sr*Ct+Sp*St; DCM[7] = Sp*Sr*Ct-Cp*St; DCM[8] = Cr*Ct; +} + +/*----------------------------------------------------------------------------*/ +static void DCM2QUAT(float *DCM, float *Q) +/*----------------------------------------------------------------------------*/ +/* This routine computes the quaternion associated to a DCM matrix */ +/* The DCM matrix translates from a vector expressed in the earth frame to */ +/* this same vector expressed in the board frame */ +/* x y z */ +/* 1st column is x earth vector expressed in earth frame DCM[0] DCM[3] DCM[6] */ +/* 2nd... y DCM[1] DCM[4] DCM[7] */ +/* 3rd... z DCM[2] DCM[5] DCM[8] */ +/*----------------------------------------------------------------------------*/ +{ + float ST1, ST2, ST3 ,ST4; + float maxST; + + + ST1 = sqrt(1. + DCM[0] + DCM[4] + DCM[8]); + ST2 = sqrt(1. + DCM[0] - DCM[4] - DCM[8]); + ST3 = sqrt(1. - DCM[0] + DCM[4] - DCM[8]); + ST4 = sqrt(1. - DCM[0] - DCM[4] + DCM[8]); + + maxST = max(ST1, max(ST2, max(ST3, ST4))); + + if (ST1 == maxST) + { + Q[0] = 0.5*ST1; + Q[1] = 0.5*(DCM[5] - DCM[7])/ST1; + Q[2] = 0.5*(DCM[6] - DCM[2])/ST1; + Q[3] = 0.5*(DCM[1] - DCM[3])/ST1; + } + else if (ST2 == maxST) + { + Q[0] = 0.5*(DCM[5] - DCM[7])/ST2; + Q[1] = 0.5*ST2; + Q[2] = 0.5*(DCM[1] + DCM[3])/ST2; + Q[3] = 0.5*(DCM[6] + DCM[2])/ST2; + } + else if (ST3 == maxST) + { + Q[0] = 0.5*(DCM[6] - DCM[2])/ST3; + Q[1] = 0.5*(DCM[1] + DCM[3])/ST3; + Q[2] = 0.5*ST3; + Q[3] = 0.5*(DCM[5] + DCM[7])/ST3; + } + else + { + Q[0] = 0.5*(DCM[1] - DCM[3])/ST4; + Q[1] = 0.5*(DCM[6] + DCM[2])/ST4; + Q[2] = 0.5*(DCM[5] + DCM[7])/ST4; + Q[3] = 0.5*ST4; + } +} + +/*----------------------------------------------------------------------------*/ +static void QUAT2AXIS(float *Q, float *V, float *angle) +/*----------------------------------------------------------------------------*/ +{ + float f_buff = sqrt(1. - Q[0]*Q[0]); + + + V[0] = Q[1]/f_buff; + V[1] = Q[2]/f_buff; + V[2] = Q[3]/f_buff; + + *angle = 2.*atan2(f_buff, Q[0]); +} + +/*----------------------------------------------------------------------------*/ +static void VectorCross(float *V1, float *V2, float *V3) +/*----------------------------------------------------------------------------*/ +/* V3 = V1xV2 */ +/*----------------------------------------------------------------------------*/ +{ + V3[0] = V1[1]*V2[2] - V1[2]*V2[1]; + V3[1] = V1[2]*V2[0] - V1[0]*V2[2]; + V3[2] = V1[0]*V2[1] - V1[1]*V2[0]; +} + +/*----------------------------------------------------------------------------*/ +static void VectorDotProduct(float *V1, float *V2, float *Scalar_Product) +/*----------------------------------------------------------------------------*/ +/* Scalar_Product = V1.V2 */ +/*----------------------------------------------------------------------------*/ +{ + *Scalar_Product = 0.; + for (int i = 0; i < 3; i++) *Scalar_Product += V1[i] * V2[i]; +} + +/*----------------------------------------------------------------------------*/ +static void TransposeMatrix(float *M1, float *M2) +/*----------------------------------------------------------------------------*/ +/* M2 = Transpose M1 */ +/*----------------------------------------------------------------------------*/ +{ + M2[0] = M1[0]; M2[1] = M1[3]; M2[2] = M1[6]; + M2[3] = M1[1]; M2[4] = M1[4]; M2[5] = M1[7]; + M2[6] = M1[2]; M2[7] = M1[5]; M2[8] = M1[8]; +} + +/*----------------------------------------------------------------------------*/ +static void MatrixMultiply2(float *M1, float *M2, float *M3) +/*----------------------------------------------------------------------------*/ +/* M3 = M1*M2 */ +/*----------------------------------------------------------------------------*/ +{ + M3[0] = M1[0]*M2[0] + M1[1]*M2[3] + M1[2]*M2[6]; + M3[3] = M1[3]*M2[0] + M1[4]*M2[3] + M1[5]*M2[6]; + M3[6] = M1[6]*M2[0] + M1[7]*M2[3] + M1[8]*M2[6]; + + M3[1] = M1[0]*M2[1] + M1[1]*M2[4] + M1[2]*M2[7]; + M3[4] = M1[3]*M2[1] + M1[4]*M2[4] + M1[5]*M2[7]; + M3[7] = M1[6]*M2[1] + M1[7]*M2[4] + M1[8]*M2[7]; + + M3[2] = M1[0]*M2[2] + M1[1]*M2[5] + M1[2]*M2[8]; + M3[5] = M1[3]*M2[2] + M1[4]*M2[5] + M1[5]*M2[8]; + M3[8] = M1[6]*M2[2] + M1[7]*M2[5] + M1[8]*M2[8]; +} \ No newline at end of file