diff --git a/Makefile b/Makefile
index ee3e58e482..7bb7effc72 100644
--- a/Makefile
+++ b/Makefile
@@ -60,6 +60,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \
drv_system.c \
drv_uart.c \
printf.c \
+ utils.c \
$(CMSIS_SRC) \
$(STDPERIPH_SRC)
diff --git a/baseflight.uvproj b/baseflight.uvproj
index a1b03bd84c..64ed25f91f 100755
--- a/baseflight.uvproj
+++ b/baseflight.uvproj
@@ -462,6 +462,11 @@
1
.\src\telemetry.c
+
+ utils.c
+ 1
+ .\src\utils.c
+
@@ -1293,6 +1298,11 @@
1
.\src\telemetry.c
+
+ utils.c
+ 1
+ .\src\utils.c
+
@@ -2068,6 +2078,11 @@
1
.\src\telemetry.c
+
+ utils.c
+ 1
+ .\src\utils.c
+
diff --git a/src/board.h b/src/board.h
index 35a107971c..ec83b5bc34 100755
--- a/src/board.h
+++ b/src/board.h
@@ -32,7 +32,6 @@
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
-#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// Chip Unique ID on F103
#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
@@ -40,11 +39,12 @@
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
typedef enum {
- SENSOR_ACC = 1 << 0,
- SENSOR_BARO = 1 << 1,
- SENSOR_MAG = 1 << 2,
- SENSOR_SONAR = 1 << 3,
- SENSOR_GPS = 1 << 4,
+ SENSOR_GYRO = 1 << 0, // always present
+ SENSOR_ACC = 1 << 1,
+ SENSOR_BARO = 1 << 2,
+ SENSOR_MAG = 1 << 3,
+ SENSOR_SONAR = 1 << 4,
+ SENSOR_GPS = 1 << 5,
} AvailableSensors;
// Type of accelerometer used/detected
@@ -98,12 +98,20 @@ typedef enum {
CW270_DEG_FLIP = 8
} sensor_align_e;
+enum {
+ GYRO_UPDATED = 0,
+ ACC_UPDATED,
+ MAG_UPDATED,
+ TEMP_UPDATED
+};
+
typedef struct sensor_data_t
{
- int16_t x;
- int16_t y;
- int16_t z;
+ int16_t gyro[3];
+ int16_t acc[3];
+ int16_t mag[3];
float temperature;
+ int updated;
} sensor_data_t;
typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype
@@ -207,6 +215,8 @@ typedef struct baro_t
#undef SOFT_I2C // enable to test software i2c
+#include "utils.h"
+
#ifdef FY90Q
// FY90Q
#include "drv_adc.h"
diff --git a/src/drv_l3g4200d.c b/src/drv_l3g4200d.c
index 9030157e08..4c8e2af62e 100644
--- a/src/drv_l3g4200d.c
+++ b/src/drv_l3g4200d.c
@@ -3,6 +3,7 @@
// L3G4200D, Standard address 0x68
#define L3G4200D_ADDRESS 0x68
#define L3G4200D_ID 0xD3
+#define L3G4200D_AUTOINCR 0x80
// Registers
#define L3G4200D_WHO_AM_I 0x0F
@@ -13,7 +14,7 @@
#define L3G4200D_CTRL_REG5 0x24
#define L3G4200D_REFERENCE 0x25
#define L3G4200D_STATUS_REG 0x27
-#define L3G4200D_GYRO_OUT 0xA8
+#define L3G4200D_GYRO_OUT 0x28
// Bits
#define L3G4200D_POWER_ON 0x0F
@@ -87,10 +88,10 @@ static void l3g4200dRead(int16_t *gyroData)
uint8_t buf[6];
int16_t data[3];
- i2cRead(L3G4200D_ADDRESS, L3G4200D_GYRO_OUT, 6, buf);
- data[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
- data[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
- data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+ i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
+ data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
+ data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+ data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
diff --git a/src/imu.c b/src/imu.c
index 12265059f3..7be47a3814 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -45,39 +45,18 @@ void imuInit(void)
void computeIMU(void)
{
uint32_t axis;
- static int16_t gyroADCprevious[3] = { 0, 0, 0 };
- int16_t gyroADCp[3];
- int16_t gyroADCinter[3];
- static uint32_t timeInterleave = 0;
static int16_t gyroYawSmooth = 0;
+ Gyro_getADC();
if (sensors(SENSOR_ACC)) {
ACC_getADC();
getEstimatedAttitude();
- }
-
- Gyro_getADC();
-
- for (axis = 0; axis < 3; axis++)
- gyroADCp[axis] = gyroADC[axis];
- timeInterleave = micros();
- annexCode();
-
- if ((micros() - timeInterleave) > 650) {
- annex650_overrun_count++;
} else {
- while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
- }
-
- Gyro_getADC();
- for (axis = 0; axis < 3; axis++) {
- gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
- // empirical, we take a weighted value of the current and the previous values
- gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis]) / 3;
- gyroADCprevious[axis] = gyroADCinter[axis] / 2;
- if (!sensors(SENSOR_ACC))
- accADC[axis] = 0;
+ accADC[X] = 0;
+ accADC[Y] = 0;
+ accADC[Z] = 0;
}
+ annexCode();
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };
@@ -95,6 +74,9 @@ void computeIMU(void)
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
+ } else {
+ for (axis = 0; axis < 3; axis++)
+ gyroData[axis] = gyroADC[axis];
}
}
@@ -118,8 +100,6 @@ void computeIMU(void)
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
-// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
-
typedef struct fp_vector {
float X;
float Y;
@@ -303,18 +283,15 @@ static void getEstimatedAttitude(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
- // baseflight calculation (no, sorry, tarducopter calculation)
- float magX = EstM.A[X];
- float magY = EstM.A[Y];
- float magZ = EstM.A[Z];
- float cr = cosf(anglerad[ROLL]);
- float sr = sinf(anglerad[ROLL]);
- float cp = cosf(anglerad[PITCH]);
- float sp = sinf(anglerad[PITCH]);
- float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
- float Yh = magY * cr - magZ * sr;
- float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
- heading = -hd;
+ // baseflight calculation by Luggi09
+ float cosineRoll = cosf(anglerad[ROLL]);
+ float sineRoll = sinf(anglerad[ROLL]);
+ float cosinePitch = cosf(anglerad[PITCH]);
+ float sinePitch = sinf(anglerad[PITCH]);
+ float Xh = EstM.A[X] * cosinePitch + EstM.A[Z] * sinePitch;
+ float Yh = EstM.A[X] * sinePitch * sineRoll + EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll * cosinePitch;
+ float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
+ heading = hd;
if (heading > 180)
heading = heading - 360;
else if (heading < -180)
diff --git a/src/mw.c b/src/mw.c
index bcea8c5c81..ce4156510e 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -11,7 +11,6 @@ uint32_t previousTime = 0;
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
int16_t headFreeModeHold;
-int16_t annex650_overrun_count = 0;
uint8_t vbat; // battery voltage in 0.1V steps
int16_t telemTemperature1; // gyro sensor temperature
@@ -841,7 +840,7 @@ void loop(void)
#endif
if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) {
- rcCommand[THROTTLE]+= throttleAngleCorrection;
+ rcCommand[THROTTLE] += throttleAngleCorrection;
}
if (sensors(SENSOR_GPS)) {
diff --git a/src/mw.h b/src/mw.h
index 5fbdb618ca..597eb0cb15 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -343,7 +343,6 @@ extern uint16_t calibratingA;
extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
-extern int16_t annex650_overrun_count;
extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;
diff --git a/src/sensors.c b/src/sensors.c
index d10642e31c..db2dcd186e 100755
--- a/src/sensors.c
+++ b/src/sensors.c
@@ -321,7 +321,6 @@ static float devStandardDeviation(stdev_t *dev)
static void GYRO_Common(void)
{
int axis;
- static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3];
static stdev_t var[3];
@@ -355,12 +354,8 @@ static void GYRO_Common(void)
}
calibratingG--;
}
- for (axis = 0; axis < 3; axis++) {
+ for (axis = 0; axis < 3; axis++)
gyroADC[axis] -= gyroZero[axis];
- //anti gyro glitch, limit the variation between two consecutive readings
- gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
- previousGyroADC[axis] = gyroADC[axis];
- }
}
void Gyro_getADC(void)
diff --git a/src/utils.c b/src/utils.c
new file mode 100644
index 0000000000..2de9f351b9
--- /dev/null
+++ b/src/utils.c
@@ -0,0 +1,12 @@
+#include "board.h"
+#include "mw.h"
+
+int constrain(int amt, int low, int high)
+{
+ if (amt < low)
+ return low;
+ else if (amt > high)
+ return high;
+ else
+ return amt;
+}
diff --git a/src/utils.h b/src/utils.h
new file mode 100644
index 0000000000..4fca60f8cf
--- /dev/null
+++ b/src/utils.h
@@ -0,0 +1,3 @@
+#pragma once
+
+int constrain(int amt, int low, int high);