Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Typograhical and Whitespace fixes #1

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Typograhical and Whitespace fixes #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
aed1ebf
extraneous whitespace linting
ksatirli Jul 30, 2024
a486bf4
extraneous whitespace linting
ksatirli Jul 30, 2024
bbac349
typograhical fixes
ksatirli Jul 30, 2024
9148591
corrects mimetype
ksatirli Jul 30, 2024
8fa8bd5
more typo fixes
ksatirli Jul 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions General_Driver/AK09918.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
#define AK09918_RSV1 0x02 // Reserved 1
#define AK09918_RSV2 0x03 // Reserved 2
#define AK09918_ST1 0x10 // DataStatus 1
#define AK09918_HXL 0x11 // X-axis data
#define AK09918_HXL 0x11 // X-axis data
#define AK09918_HXH 0x12
#define AK09918_HYL 0x13 // Y-axis data
#define AK09918_HYH 0x14
Expand All @@ -66,7 +66,7 @@
#define AK09918_DRDY_BIT 0x01 // Data Ready

// #define AK09918_MEASURE_PERIOD 9 // Must not be changed
// AK09918 has following seven operation modes:
// AK09918 has the following seven operation modes:
// (1) Power-down mode: AK09918 doesn't measure
// (2) Single measurement mode: measure when you call any getData() function
// (3) Continuous measurement mode 1: 10Hz, measure 10 times per second,
Expand All @@ -93,7 +93,6 @@ enum AK09918_err_type_t {
AK09918_ERR_OVERFLOW = 5, // sensor overflow, means |x|+|y|+|z| >= 4912uT
AK09918_ERR_WRITE_FAILED = 6, // fail to write
AK09918_ERR_READ_FAILED = 7, // fail to read

};

typedef struct imu_st_sensor_data_tag
Expand Down
12 changes: 6 additions & 6 deletions General_Driver/General_Driver.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ StaticJsonDocument<512> jsonInfoHttp;
#include <Adafruit_Sensor.h>


// functions for barrery info.
// functions for battery info.
#include "battery_ctrl.h"

// functions for oled.
Expand Down Expand Up @@ -152,7 +152,7 @@ void setup() {
oled_update();
if(InfoPrint == 1){Serial.println("Power up the servos.");}
delay(500);

// init servo ctrl functions.
screenLine_2 = screenLine_3;
screenLine_3 = "ServoCtrl init UART2TTL...";
Expand Down Expand Up @@ -238,7 +238,7 @@ void loop() {
case 2: moduleType_Gimbal();break;
}

// recv esp-now json cmd.
// receive esp-now json cmd.
if(runNewJsonCmd) {
jsonCmdReceiveHandler();
jsonCmdReceive.clear();
Expand All @@ -248,11 +248,11 @@ void loop() {
getLeftSpeed();

LeftPidControllerCompute();

getRightSpeed();

RightPidControllerCompute();

oledInfoUpdate();

updateIMUData();
Expand Down
36 changes: 18 additions & 18 deletions General_Driver/IMU.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ double declination_shenzhen = -3.22;
#define Ki 1.0f // integral gain governs rate of convergence of gyroscope biases

float angles[3];
float q0, q1, q2, q3;
float q0, q1, q2, q3;

void imuInit()
{
Expand Down Expand Up @@ -84,12 +84,12 @@ void imuDataGet(EulerAngles *pstAngles,
MotionVal[8]=pstMagnRawData->s16Z;

imuAHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
(float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
(float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);

pstAngles->pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
pstAngles->roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
pstAngles->yaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;
pstAngles->yaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;

pstGyroRawData->X = gyro[0];
pstGyroRawData->Y = gyro[1];
Expand All @@ -99,10 +99,10 @@ void imuDataGet(EulerAngles *pstAngles,
pstAccelRawData->Y = acc[1];
pstAccelRawData->Z = acc[2];

return;
return;
}

void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float norm;
float hx, hy, hz, bx, bz;
Expand All @@ -117,34 +117,34 @@ void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, f
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float q3q3 = q3 * q3;

norm = invSqrt(ax * ax + ay * ay + az * az);
norm = invSqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;

norm = invSqrt(mx * mx + my * my + mz * mz);
norm = invSqrt(mx * mx + my * my + mz * mz);
mx = mx * norm;
my = my * norm;
mz = mz * norm;

// compute reference direction of flux
hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = hz;
bz = hz;

// estimated direction of gravity and flux (v and w)
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
Expand All @@ -154,7 +154,7 @@ void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, f
if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
{
exInt = exInt + ex * Ki * halfT;
eyInt = eyInt + ey * Ki * halfT;
eyInt = eyInt + ey * Ki * halfT;
ezInt = ezInt + ez * Ki * halfT;

gx = gx + Kp * ex + exInt;
Expand All @@ -165,7 +165,7 @@ void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, f
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 * norm;
Expand All @@ -174,16 +174,16 @@ void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, f
q3 = q3 * norm;
}

float invSqrt(float x)
float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;

long i = *(long*)&y; //get bits for floating value
i = 0x5f3759df - (i >> 1); //gives initial guss you
y = *(float*)&i; //convert bits back to float
y = y * (1.5f - (halfx * y * y)); //newtop step, repeating increases accuracy

return y;
}

Expand All @@ -197,7 +197,7 @@ void calibrateMagn(void)
temp[0] = x;
temp[1] = y;
temp[2] = z;

Serial.printf("rotate z axis 180 degrees and it will read all axises offset value after 4 seconds\n");
delay(4000);
Serial.printf("start read all axises offset value\n");
Expand Down
Loading