46,6 → 46,14 |
uint8_t gyroDWindowIdx = 0; |
|
/* |
* Airspeed |
*/ |
int16_t airpressure; |
uint16_t airspeedVelocity = 0; |
//int16_t airpressureWindow[AIRPRESSURE_WINDOW_LENGTH]; |
//uint8_t airpressureWindowIdx = 0; |
|
/* |
* Offset values. These are the raw gyro and acc. meter sums when the copter is |
* standing still. They are used for adjusting the gyro and acc. meter values |
* to be centered on zero. |
109,17 → 117,11 |
} |
|
/* |
* Airspeed |
*/ |
uint16_t simpleAirPressure; |
|
/* |
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
* That is divided by 3 below, for a final 10.34 per volt. |
* So the initial value of 100 is for 9.7 volts. |
*/ |
uint16_t UBat = 100; |
uint16_t airspeedVelocity = 0; |
|
/* |
* Control and status. |
268,22 → 270,6 |
} |
} |
|
/* |
void measureGyroActivity(int16_t newValue) { |
gyroActivity += (uint32_t)((int32_t)newValue * newValue); |
} |
|
#define GADAMPING 6 |
void dampenGyroActivity(void) { |
static uint8_t cnt = 0; |
if (++cnt >= IMUConfig.gyroActivityDamping) { |
cnt = 0; |
gyroActivity *= (uint32_t)((1L<<GADAMPING)-1); |
gyroActivity >>= GADAMPING; |
} |
} |
*/ |
|
void analog_updateGyros(void) { |
// for various filters... |
int16_t tempOffsetGyro[3], tempGyro; |
352,14 → 338,30 |
} |
|
// probably wanna aim at 1/10 m/s/unit. |
#define LOG_AIRSPEED_FACTOR 2 |
#define LOG_AIRSPEED_FACTOR 0 |
|
void analog_updateAirspeed(void) { |
uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
int16_t temp = rawAirPressure - airpressureOffset; |
if (temp<0) temp = 0; |
simpleAirPressure = temp; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(simpleAirPressure)) >> LOG_AIRSPEED_FACTOR; |
uint16_t rawAirpressure = sensorInputs[AD_AIRPRESSURE]; |
int16_t temp = airpressureOffset - rawAirpressure; |
//airpressure -= airpressureWindow[airpressureWindowIdx]; |
//airpressure += temp; |
//airpressureWindow[airpressureWindowIdx] = temp; |
//airpressureWindowIdx++; |
//if (airpressureWindowIdx == AIRPRESSURE_WINDOW_LENGTH) { |
// airpressureWindowIdx = 0; |
//} |
|
#define AIRPRESSURE_FILTER 16 |
airpressure = ((int32_t)airpressure * (AIRPRESSURE_FILTER-1) + (AIRPRESSURE_FILTER/2) + temp) / AIRPRESSURE_FILTER; |
|
uint16_t p2 = (airpressure<0) ? 0 : airpressure; |
airspeedVelocity = (staticParams.airspeedCorrection * isqrt16(p2)) >> LOG_AIRSPEED_FACTOR; |
|
debugOut.analog[17] = airpressure; |
debugOut.analog[18] = airpressureOffset; |
debugOut.analog[19] = airspeedVelocity; |
|
isFlying = 0; //(airspeedVelocity >= staticParams.isFlyingThreshold); |
} |
|
void analog_updateBatteryVoltage(void) { |