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

Fixed gyro + acc. Able to see the motor1,2,3 and 4 in the configurator page

master
Englebert 3 years ago
parent
dc11297c68
commit
d1d2c5c461
8 changed files with 227 additions and 57 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. BIN
      configurator/gzip/index.html.gz
  2. 31
      configurator/src/index.html
  3. 2
      src/config.h
  4. 178
      src/gyro.cpp
  5. 4
      src/gyro.h
  6. 57
      src/main.cpp
  7. 2
      src/main.h
  8. 10
      src/web.cpp

BIN
configurator/gzip/index.html.gz
View File

31
configurator/src/index.html
View File

@ -75,6 +75,25 @@
<div id="rc_roll" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
</div>
<div class="p-3 bd-highlight">
<h3 class="mt-5">Motor Output</h3>
Motor 1:
<div class="progress">
<div id="motor1" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
Motor 2:
<div class="progress">
<div id="motor2" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
Motor 3:
<div class="progress">
<div id="motor3" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
Motor 4:
<div class="progress">
<div id="motor4" class="progress-bar" role="progressbar" style="width: 50%;" aria-valuenow="50" aria-valuemin="0" aria-valuemax="100">1500</div>
</div>
</div>
<div class="p-3 bd-highlight">
<h3 class="mt-5">RC Switches & Flags</h3>
<button id="rc_aux1" type="button" class="btn btn-outline-dark" disabled>AUX1</button>
@ -277,9 +296,21 @@
(feeds[31] == 1) ? $("#land_complete").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#land_complete").removeClass("btn btn-success").addClass("btn btn-outline-dark");
(feeds[32] == 1) ? $("#land_in_progress").removeClass("btn btn-outline-dark").addClass("btn btn-success") : $("#land_in_progress").removeClass("btn btn-success").addClass("btn btn-outline-dark");
profile_set(feeds[33]);
motor_set(feeds[40], feeds[41], feeds[42], feeds[43]);
});
}
// Display current motor speed
function motor_set(m1, m2, m3, m4) {
$('#motor1').css('width',calc_rc_percent(m1)+'%');
$('#motor1').html(m1);
$('#motor2').css('width',calc_rc_percent(m2)+'%');
$('#motor2').html(m2);
$('#motor3').css('width',calc_rc_percent(m3)+'%');
$('#motor3').html(m3);
$('#motor4').css('width',calc_rc_percent(m4)+'%');
$('#motor4').html(m4);
}
// Display profile based on data feed
function profile_set(profile_id) {

2
src/config.h
View File

@ -51,7 +51,7 @@
* 1 = evolved oldschool algorithm (similar to v2.2)
* 2 = new experimental algorithm from Alex Khoroshko - unsupported - http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671&start=10#p37387
*/
#define PID_CONTROLLER 2
#define PID_CONTROLLER 1
#define YAW_DIRECTION 1
//#define YAW_DIRECTION -1 // if you want to reverse the yaw correction direction

178
src/gyro.cpp
View File

@ -136,9 +136,13 @@ void GYRO::acc_gyro_init(void) {
write_register(GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED);
delay(15);
// write_register(GYRO_CONFIG, INV_FSR_250DPS << 3 | FCB_DISABLED);
// delay(15);
write_register(ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
//write_register(ACCEL_CONFIG, INV_FSR_4G << 3);
//delay(15);
write_register(CONFIG, 0x07); // 7:0 6:0 5:0 4:0 3:0 2:1 1:1 0:1
delay(15);
@ -168,9 +172,12 @@ void GYRO::calculate_imu_error(void) {
// Note that we should place the IMU flat in order to get the proper values, so that we then can the correct values
// Read accelerometer values 200 times
while(c < 200) {
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);
AcX = (read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L)) / 2048.0;
AcY = (read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L)) / 2048.0;
AcZ = (read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L)) / 2048.0;
// AcX = (read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L)) >> 11;
// AcY = (read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L)) >> 11;
// AcZ = (read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L)) >> 11;
AccErrorX = AccErrorX + ((atan((AcY) / sqrt(pow((AcX), 2) + pow((AcZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AcX) / sqrt(pow((AcY), 2) + pow((AcZ), 2))) * 180 / PI));
@ -183,24 +190,27 @@ void GYRO::calculate_imu_error(void) {
AccErrorY = AccErrorY / 200;
c = 0;
// Read gyro values 200 times
// Read gyro values 1000 times
while(c < 200) {
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);
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));
// Sum all readings
GyroErrorX = GyroErrorX + (GyX / 16.4);
GyroErrorY = GyroErrorY + (GyY / 16.4);
GyroErrorZ = GyroErrorZ + (GyZ / 16.4);
// GyroErrorX = GyroErrorX + (GyX / 16.4);
// GyroErrorY = GyroErrorY + (GyY / 16.4);
// GyroErrorZ = GyroErrorZ + (GyZ / 16.4);
GyroErrorX += GyX;
GyroErrorY += GyY;
GyroErrorZ += GyZ;
c++;
}
// Divide the sum by 200 to get the error value
GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;
// Divide the sum by 1000 to get the error value
GyroErrorX /= 200;
GyroErrorY /= 200;
GyroErrorZ /= 200;
#ifdef DEBUG_MODE
sprintf(debug_str,
@ -212,39 +222,103 @@ 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);
// For a range of +-16g, we need to divide the raw values by 2048, according to the datasheet
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));
// AcX = (read_register(ACCEL_XOUT_H) << 8 | read_register(ACCEL_XOUT_L)) >> 11;
// AcY = (read_register(ACCEL_YOUT_H) << 8 | read_register(ACCEL_YOUT_L)) >> 11;
// AcZ = (read_register(ACCEL_ZOUT_H) << 8 | read_register(ACCEL_ZOUT_L)) >> 11;
// 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)
// 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;
GyX = (read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L)); // 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));
GyZ = (read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L));
// GyX = (read_register(GYRO_XOUT_H) << 8 | read_register(GYRO_XOUT_L)) >> 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)) >> 4;
// GyZ = (read_register(GYRO_ZOUT_H) << 8 | read_register(GYRO_ZOUT_L)) >> 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)
/// GyX = GyX + 0.56; // GyroErrorX ~(-0.56)
/// GyY = GyY - 2; // GyroErrorY ~(2)
/// GyZ = GyZ + 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;
/// 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;
//// roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
//// pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;
}
void GYRO::calc(void) {
// Subtract the offset values from the raw gyro values
GyX -= GyroErrorX;
GyY -= GyroErrorY;
GyZ -= GyroErrorZ;
// Gyro angle calculations. Note: 0.0000611 = 1 /(250Hz x 65.5) <--- Example
// For this we are using: _+/-2000 dps with 16.4, So it will be:
// 0.0000304878 = 1 / (2000Hz x 16.4) and the nearest is 0.0000305
// Calculate the traveled pitch angle and add this to the angle_pitch variable
pitch += GyX * 0.0000305;
roll += GyY * 0.0000305;
yaw += GyZ * 0.0000305;
// Calculate the traveled roll angle and add this to the angle_roll variable
// 0.000000532 = 0.0000305 * (3.142(PI) / 180degr) The Arduino sin function is in radians
// If the IMU has yawed transfer the roll angle to the pitch angle
pitch += roll * sin(GyZ * 0.000000532);
// If the IMU has yawed transfer the pitch angle to the roll angle
roll -= pitch * sin(GyZ * 0.000000532);
// Accelerometer angle calculations
// Calculate the total accelerometer vector
acc_total_vector = sqrt((AcX * AcX) + (AcY * AcY) + (AcZ * AcZ));
// 57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
// Calculate the pitch angle
pitch_acc = asin((float) AcY / acc_total_vector) * 57.296;
// Calculating the roll angle
roll_acc = asin((float) AcX / acc_total_vector) * -57.296;
// Accelerometer calibration value for pitch
// pitch_acc -= 0.0;
// Accelerometer calibration value for roll
// roll_acc -= 0.0;
if(set_gyro_angles) {
// IMU has been running
// Correct the drift of the gyro pitch angle with the accelerometer pitch angle
pitch = pitch * 0.9996 + pitch_acc * 0.0004;
// Correct the drift of the gyro roll angle with the accelerometer roll angle
roll = roll * 0.9996 + roll_acc * 0.0004;
} else {
// IMU just started
pitch = pitch_acc;
roll = roll_acc;
set_gyro_angles = true;
}
// To dampen the pitch and roll angles complementary filter is used
// Take 90% of the output pitch value and add 10% of the raw pitch value
pitch_dmp = pitch_dmp * 0.9 + pitch * 0.1;
roll_dmp = roll_dmp * 0.9 + roll * 0.1;
}
@ -325,6 +399,9 @@ void GYRO::calculate_angles_beta(void) {
// Old multiwii code uses void getEstimatedAltitude();
void GYRO::calculate_angles(void) {
//// calc(); return;
uint8_t axis;
int32_t accMag = 0;
float scale;
@ -340,13 +417,37 @@ void GYRO::calculate_angles(void) {
float invG; // 1/|G|
static int16_t accZoffset = 0;
int32_t accZ_tmp = 0;
static uint16_t previousT;
uint16_t currentT = micros();
// Calculate position based on gyro and accelerometers data
calc();
if((int32_t) yaw > ACCZ_25deg) {
flags.SMALL_ANGLES_25 = 1;
} else {
flags.SMALL_ANGLES_25 = 0;
}
att.angle[ROLL] = roll;
att.angle[PITCH] = pitch;
att.heading = yaw;
/// MAY NEED TO FINE TUNE!!! ALERT !!!
#if defined(THROTTLE_ANGLE_CORRECTION)
cosZ = (yaw * 100) / ACC_1G; // cos(angleZ) * 100
throttleAngleCorrection = THROTTLE_ANGLE_CORRECTION * constrain(100 - cosZ, 0, 100) >> 3; // 16 bit ok: 200*150 = 30000
#endif
/*******************
//// static uint16_t previousT;
//// uint16_t currentT = micros();
// unit: radian per bit, scaled by 2^16 for further multiplication
// with a delta time of 3000 us, and GYRO scale of most gyros, scale = a little bit less than 1
scale = (currentT - previousT) * (GYRO_SCALE * 65536);
previousT = currentT;
//// scale = (currentT - previousT) * (GYRO_SCALE * 65536);
//// previousT = currentT;
// Initialization
for(axis = 0; axis < 3; axis++) {
@ -414,6 +515,7 @@ void GYRO::calculate_angles(void) {
accZoffset += accZ;
}
accZ -= accZoffset >> 3;
***********/
}

4
src/gyro.h
View File

@ -225,11 +225,13 @@ class GYRO {
void calculate_angles(void);
void calculate_angles_beta(void);
void calculate_imu_error(void);
void calc(void);
int16_t deltaGyroAngle16[3];
int16_t accZ = 0;
float roll, pitch, yaw;
bool set_gyro_angles = false;
float roll, pitch, yaw, roll_acc, pitch_acc, yaw_acc, roll_dmp, pitch_dmp, acc_total_vector;
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY;
float elapsedTime = 0, currentTime = 0, previousTime = 0;

57
src/main.cpp
View File

@ -54,6 +54,7 @@ int16_t PTerm = 0, ITerm = 0, DTerm, PTermACC, ITermACC;
int16_t lastGyro[2] = {0, 0};
int16_t errorAngleI[2] = {0, 0};
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
// float AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
bool baro_busy = false, gyro_busy = false, rx_busy = false, osd_busy = false;
#if PID_CONTROLLER == 1
@ -70,6 +71,7 @@ int16_t AngleRateTmp, RateError;
uint32_t rcTime = 0;
uint32_t currentTime = 0;
uint32_t previousTime = 0;
uint32_t computeIMUTime = 0;
int16_t initialThrottleHold;
int16_t rc;
@ -356,7 +358,7 @@ void setup() {
xTaskCreatePinnedToCore(
taskMeasurements,
"TaskMeasurements", // Name of the process
2048, // This stack size can be checked & adjusted by reading the Stack HighWater
4096, // This stack size can be checked & adjusted by reading the Stack HighWater
NULL,
6, // Priority with 3 (configMAX_PRIORITIES - 1) being the highest and 0 being the lowest
NULL,
@ -406,6 +408,8 @@ void setup() {
PRO_CPU
);
****/
Serial.println("SETUP DONE.");
}
@ -593,7 +597,7 @@ void taskMeasurements(void *pvParameters) {
(void) pvParameters;
char buffer_gyro[256];
bool firsttime = true;
for(;;) {
// Reading more to get precesions
// gyro.get_readings();
@ -603,6 +607,10 @@ void taskMeasurements(void *pvParameters) {
// rx.get_readings();
// osd.display(); (Maybe only update it every 100mS?)
if(firsttime) {
firsttime = false;
vTaskDelay(1000);
}
// Just stop for a milisecond...
vTaskDelay(1000);
#if USE_BARO
@ -611,10 +619,23 @@ 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, baro.altitude, rcData[THROTTLE], rcData[YAW], rcData[PITCH], rcData[ROLL]);
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, 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: %.1f %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",
imu.accADC[ROLL] << 16, imu.accADC[PITCH], imu.accADC[YAW], imu.gyroADC[ROLL], imu.gyroADC[PITCH], imu.gyroADC[YAW], 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);
*/
sprintf(buffer_gyro,
"Gyro: %d %d %d %d %d %d M1: %d M2: %d M3: %d M4: %d Pitch: %d Roll: %d Yaw: %d Cycle: %d RC: %d Bat: %.2f",
AcX, AcY, AcZ, GyX, GyY, GyZ, motor[0], motor[1], motor[2], motor[3], imu.gyroData[PITCH], imu.gyroData[ROLL], imu.gyroData[YAW], cycleTime, rc_counts, battery.volt);
///// AcX, AcY, AcZ, GyX, GyY, GyZ, motor[0], motor[1], motor[2], motor[3], gyro.pitch, gyro.roll, gyro.yaw, cycleTime, rc_counts, battery.volt);
/*
sprintf(
buffer_gyro,
"Motors 1: %d, 2: %d, 3: %d, 4: %d, Batt: %.2f",
motor[0], motor[1], motor[2], motor[3], battery.volt
);
*/
#endif
// Reset counter
@ -695,8 +716,13 @@ void run_tasks(void) {
cycleTime = currentTime - previousTime;
previousTime = currentTime;
// Compute IMU
// Compute IMU if more than 500uS ~ 2000dps
// if((uint32_t)(currentTime - computeIMUTime) > 800) {
compute_imu();
// computeIMUTime = micros();
//} else {
// return;
// }
// Experimental FlightModes
#if defined(ACROTRAINER_MODE)
@ -1089,6 +1115,8 @@ void compute_imu(void) {
// gyro.read_all();
gyro.read_accelerometer();
gyro.read_gyro();
///// gyro.calc();
// IMU.cpp at multiwii....
ACC_ORIENTATION(AcX, AcY, AcZ);
@ -1104,28 +1132,31 @@ void compute_imu(void) {
for(axis = 0; axis < 3; axis++)
gyroADCinter[axis] = imu.gyroADC[axis];
timeInterleave=micros();
timeInterleave = micros();
annexCode();
uint8_t t = 0;
while((int16_t)(micros() - timeInterleave) < 650)
while((uint32_t)(micros() - timeInterleave) < 650)
t = 1; // empirical, interleaving delay between 2 consecutive reads
#ifdef LCD_TELEMETRY
if(!t) annex650_overrun_count++;
#endif
gyro.read_gyro();
GYRO_ORIENTATION(GyX, GyY, GyZ); // Range +/- 8192; +/- 2000 deg/sec?
gyro_common();
gyro.calculate_angles();
for(axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = imu.gyroADC[axis] + gyroADCinter[axis];
gyroADCinter[axis] += imu.gyroADC[axis];
// empirical, we take a weighted value of the current and the previous values
imu.gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] >> 1;
if(!USE_ACC)
imu.accADC[axis] = 0;
// empirical, we take a weighted value of the current and the previous values
imu.gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
gyroADCprevious[axis] = gyroADCinter[axis] >> 1;
if(!USE_ACC)
imu.accADC[axis] = 0;
}
#if defined(GYRO_SMOOTHING)

2
src/main.h
View File

@ -511,6 +511,7 @@ extern conf_t conf;
extern uint16_t failsafeCnt;
extern imu_t imu;
extern int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
// extern float AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
extern uint8_t temperatureG;
extern int32_t baroPressure;
extern int16_t baroTemperature;
@ -531,7 +532,6 @@ extern uint16_t cycleTime; // This is the number in micro second to
extern uint32_t rc_counts;
extern alt_t alt;
extern att_t att;
extern imu_t imu;
#ifdef LOG_PERMANENT
extern plog_t plog;
#endif

10
src/web.cpp
View File

@ -369,10 +369,14 @@ void WEB::begin(void) {
String(global_conf.magZero[ROLL]) + "," +
String(global_conf.magZero[PITCH]) + "," +
String(global_conf.magZero[YAW]) + "," +
String(motor[0]) + "," +
String(motor[1]) + "," +
String(motor[2]) + "," +
String(motor[3]) + "," +
// Gyro, Cycle, RX Rate, Battery, Motor Refreshing Rate
String(AcX) + "," +
String(AcY) + "," +
String(AcZ) + "," +
String(gyro.roll) + "," +
String(gyro.pitch) + "," +
String(gyro.yaw) + "," +
String(GyX) + "," +
String(GyY) + "," +
String(GyZ) + "," +

Write Preview
Loading…
Cancel
Save