englebert
/
ExpressFC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Added angle and gyro readings

master
Englebert 3 years ago
parent
95ee61f22c
commit
dc11297c68
3 changed files with 56 additions and 13 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      src/gyro.cpp
  2. 5
      src/gyro.h
  3. 14
      src/main.cpp

50
src/gyro.cpp
View File

@ -140,7 +140,7 @@ void GYRO::acc_gyro_init(void) {
write_register(ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
write_register(CONFIG, 0x07); // 7:0 6:0 5:0 4:0 3:0 2:1 1:1 0:1
write_register(CONFIG, 0x07); // 7:0 6:0 5:0 4:0 3:0 2:1 1:1 0:1
delay(15);
write_register(SMPLRT_DIV, 0x00);
@ -148,7 +148,7 @@ void GYRO::acc_gyro_init(void) {
// Data ready interrupt configuratio
// write_register(INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
write_register(INT_PIN_CFG, 0x12); // INT_ANYRD_2CLEAR, BYPASS_EN ~ 0001 0010
write_register(INT_PIN_CFG, 0x12); // INT_ANYRD_2CLEAR, BYPASS_EN ~ 0001 0010
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
@ -190,9 +190,9 @@ void GYRO::calculate_imu_error(void) {
GyZ = read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L);
// Sum all readings
GyroErrorX = GyroErrorX + (GyX / 131.0);
GyroErrorY = GyroErrorY + (GyY / 131.0);
GyroErrorZ = GyroErrorZ + (GyZ / 131.0);
GyroErrorX = GyroErrorX + (GyX / 16.4);
GyroErrorY = GyroErrorY + (GyY / 16.4);
GyroErrorZ = GyroErrorZ + (GyZ / 16.4);
c++;
}
@ -211,6 +211,43 @@ void GYRO::calculate_imu_error(void) {
}
void GYRO::read_accelerometer(void) {
AcX = read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L);
AcY = read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L);
AcZ = read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L);
// Calculating Roll and Pitch from the accelerometer data
accAngleX = (atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~(0.58) See the calculate_IMU_error()custom function for more details
accAngleY = (atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~(-1.58)
}
void GYRO::read_gyro(void) {
// Read Gyro data
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
GyX = (read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L)) / 16.4; // For a 2000deg/s range we have to divide first the raw value by 16.4, according to the datasheet
GyY = (read_register(GYRO_YOUT_H) << 8 | read_register(GYRO_YOUT_L)) / 16.4;
GyZ = (read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L)) / 16.4;
// Correct the outputs with the calculated error values
// GyroX = GyroX + 0.56; // GyroErrorX ~(-0.56)
// GyroY = GyroY - 2; // GyroErrorY ~(2)
// GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8)
// Currently the raw values are in degrees per seconds, deg/s, so we need to multiply by sendonds (s) to get the angle in degrees
gyroAngleX = GyX * elapsedTime; // deg/s * s = deg
gyroAngleY = GyY * elapsedTime;
yaw = GyZ * elapsedTime;
// Complementary filter - combine acceleromter and gyro angle values
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
}
void GYRO::read_all(void) {
AcX = read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L);
AcY = read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L);
@ -223,7 +260,6 @@ void GYRO::read_all(void) {
GyX = read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L);
GyY = read_register(GYRO_YOUT_H) << 8 | read_register(GYRO_YOUT_L);
GyZ = read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L);
temperatureG = read_register(TEMP_OUT_L);
@ -315,7 +351,7 @@ void GYRO::calculate_angles(void) {
// Initialization
for(axis = 0; axis < 3; axis++) {
// valid as long as LPF_FACTOR is less than 15
imu.accSmooth[axis] = LPFAcc[axis]>>ACC_LPF_FACTOR;
imu.accSmooth[axis] = LPFAcc[axis] >> ACC_LPF_FACTOR;
LPFAcc[axis] += imu.accADC[axis] - imu.accSmooth[axis];
// used to calculate later the magnitude of acc vector

5
src/gyro.h
View File

@ -217,6 +217,8 @@ class GYRO {
uint8_t read_register(uint8_t addr);
bool write_register(uint8_t addr, uint8_t data);
void read_all(void);
void read_accelerometer(void);
void read_gyro(void);
void rotateV32(t_int32_t_vector *v, int16_t* delta);
float InvSqrt(float x);
int16_t _atan2(int32_t y, int32_t x);
@ -229,7 +231,8 @@ class GYRO {
float roll, pitch, yaw;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float accAngleX, accAngleY;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY;
float elapsedTime = 0, currentTime = 0, previousTime = 0;
private:
};

14
src/main.cpp
View File

@ -612,8 +612,9 @@ void taskMeasurements(void *pvParameters) {
sprintf(buffer_gyro, "Gyro: %d %d %d %d %d %d Cycle: %d Bat: %.2fv Temp: %.2fC Alt: %dcm T: %d Y: %d P: %d R: %d", AcX, AcY, AcZ, GyX, GyY, GyZ, cycleTime, battery.volt, baro.temperature, alt.EstAlt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL]);
#else
sprintf(buffer_gyro,
"Gyro: %d %d %d %d %d %d Temperature: %d Motor: %d Cycle: %d RC: %d Bat: %.2fv T: %d Y: %d P: %d R: %d FS: %d\nAccAngleX: %.4f, AccAngleY: %.4f",
AcX, AcY, AcZ, GyX, GyY, GyZ, temperatureG, motors.counts, cycleTime, rc_counts, battery.volt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL], failsafeCnt, gyro.accAngleX, gyro.accAngleY);
"Gyro: %d %d %d %d %d %d Temperature: %d Motor: %d Cycle: %d RC: %d Bat: %.2fv T: %d Y: %d P: %d R: %d FS: %d\nAccAngleX: %.4f, AccAngleY: %.4f, GyroAngleX: %.4f, GyroAngleY: %.4f, Roll: %.4f, Pitch: %.4f, Yaw: %.4f",
AcX, AcY, AcZ, GyX, GyY, GyZ, temperatureG, motors.counts, cycleTime, rc_counts, battery.volt, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL], failsafeCnt, gyro.accAngleX, gyro.accAngleY,
gyro.gyroAngleX, gyro.gyroAngleY, gyro.roll, gyro.pitch, gyro.yaw);
#endif
// Reset counter
@ -1085,18 +1086,21 @@ void compute_imu(void) {
///// #endif
// Reading ACC and Gyro values
gyro.read_all();
// gyro.read_all();
gyro.read_accelerometer();
gyro.read_gyro();
// IMU.cpp at multiwii....
ACC_ORIENTATION(AcX, AcY, AcZ);
acc_common();
// Calcualtions
gyro.calculate_angles_beta();
GYRO_ORIENTATION(GyX, GyY, GyZ); // Range +/- 8192; +/- 2000 deg/sec?
gyro_common();
// Calcualtions
gyro.calculate_angles();
for(axis = 0; axis < 3; axis++)
gyroADCinter[axis] = imu.gyroADC[axis];

Write Preview
Loading…
Cancel
Save