| 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) { |