Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use hypot function

Browse Source
This commit is contained in:
Gold856
2024-06-05 17:12:53 -04:00
parent 3a1315ae0a
commit 9c933f88a9
4 changed files with 12 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View File

@@ -670,12 +670,10 @@ void ADIS16448_IMU::Acquire() {
// Calculate alpha for use with the complementary filter
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX = atan2f(
-accel_x_si,
sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
double accelAngleY = atan2f(
accel_y_si,
sqrtf((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
double accelAngleX =
atan2f(-accel_x_si, std::hypotf(accel_y_si, -accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(-accel_x_si, -accel_z_si));
// Calculate complementary filter
if (m_first_run) {
compAngleX = accelAngleX;

10
wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View File

@@ -698,12 +698,10 @@ void ADIS16470_IMU::Acquire() {
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX = atan2f(
accel_x_si,
sqrtf((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
double accelAngleY = atan2f(
accel_y_si,
sqrtf((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
double accelAngleX =
atan2f(accel_x_si, std::hypotf(accel_y_si, accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(accel_x_si, accel_z_si));
if (m_first_run) {
compAngleX = accelAngleX;
compAngleY = accelAngleY;

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

@@ -760,14 +760,8 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Calculate alpha for use with the complementary filter
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
Math.atan2(
-accel_x_si,
Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
double accelAngleY =
Math.atan2(
accel_y_si,
Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
double accelAngleX = Math.atan2(-accel_x_si, Math.hypot(accel_y_si, -accel_z_si));
double accelAngleY = Math.atan2(accel_y_si, Math.hypot(-accel_x_si, -accel_z_si));
// Calculate complementary filter
if (m_first_run) {
compAngleX = accelAngleX;

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

@@ -747,12 +747,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
Math.atan2(
accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
double accelAngleY =
Math.atan2(
accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
double accelAngleX = Math.atan2(accel_x_si, Math.hypot(accel_y_si, accel_z_si));
double accelAngleY = Math.atan2(accel_y_si, Math.hypot(accel_x_si, accel_z_si));
if (m_first_run) {
compAngleX = accelAngleX;
compAngleY = accelAngleY;