| 134,9 → 134,8 |
|---|
| * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
| */ |
| |
| void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
| //void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
| |
| /* |
| void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
| static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
| // Pitch to Pitch part |
| 149,7 → 148,7 |
|---|
| int8_t yy = rotationTab[quadrant]; |
| |
| int16_t xIn = result[0]; |
| result[0] = xx*result[0] + xy*result[1]; |
| result[0] = xx*xIn + xy*result[1]; |
| result[1] = yx*xIn + yy*result[1]; |
| |
| if (quadrant & 1) { |
| 161,7 → 160,7 |
|---|
| result[1] = (result[1]*11) >> 4; |
| } |
| } |
| */ |
| |
| /* |
| * Air pressure |
| */ |
| 173,7 → 172,7 |
|---|
| // Value of 2 samples, with range. |
| uint16_t simpleAirPressure; |
| |
| // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
| // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
| int32_t filteredAirPressure; |
| |
| // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples. |
| 492,7 → 491,7 |
|---|
| // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
| // The 2 cases above (end of range) are ignored for this. |
| debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
| if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
| if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1) |
| airPressureSum += simpleAirPressure / 2; |
| else |
| airPressureSum += simpleAirPressure; |
| 500,7 → 499,7 |
|---|
| |
| // 2 samples were added. |
| pressureMeasurementCount += 2; |
| if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) { |
| if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) { |
| filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
| + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
| pressureMeasurementCount = airPressureSum = 0; |
| 513,7 → 512,6 |
|---|
| // This is divided by 3 --> 10.34 counts per volt. |
| UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
| debugOut.analog[11] = UBat; |
| debugOut.analog[21] = sensorInputs[AD_UBAT]; |
| } |
| |
| void analog_update(void) { |
| 528,14 → 526,14 |
|---|
| |
| if (gyroOffset_readFromEEProm()) { |
| printf("gyro offsets invalid%s",recal); |
| gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
| gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
| gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL; |
| gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW; |
| } |
| |
| if (accOffset_readFromEEProm()) { |
| printf("acc. meter offsets invalid%s",recal); |
| accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
| accOffset.offsets[Z] = 717 * ACC_SUMMATION_FACTOR_Z; |
| accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
| accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
| } |
| |
| // Noise is relative to offset. So, reset noise measurements when changing offsets. |
| 547,7 → 545,7 |
|---|
| delay_ms_with_adc_measurement(100, 0); |
| |
| // Rough estimate. Hmm no nothing happens at calibration anyway. |
| // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
| // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2); |
| // pressureMeasurementCount = 0; |
| } |
| |
| 568,8 → 566,8 |
|---|
| for (axis = PITCH; axis <= YAW; axis++) { |
| gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
| |
| int16_t min = (512-200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
| int16_t max = (512+200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
| int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
| int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
| if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
| versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
| } |
| 603,16 → 601,16 |
|---|
| if (axis==Z) { |
| if (staticParams.imuReversedFlags & 8) { |
| // TODO: This assumes a sensitivity of +/- 2g. |
| min = (256-200) * ACC_SUMMATION_FACTOR_Z; |
| max = (256+200) * ACC_SUMMATION_FACTOR_Z; |
| min = (256-200) * ACC_OVERSAMPLING_Z; |
| max = (256+200) * ACC_OVERSAMPLING_Z; |
| } else { |
| // TODO: This assumes a sensitivity of +/- 2g. |
| min = (768-200) * ACC_SUMMATION_FACTOR_Z; |
| max = (768+200) * ACC_SUMMATION_FACTOR_Z; |
| min = (768-200) * ACC_OVERSAMPLING_Z; |
| max = (768+200) * ACC_OVERSAMPLING_Z; |
| } |
| } else { |
| min = (512-200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
| max = (512+200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
| min = (512-200) * ACC_OVERSAMPLING_XY; |
| max = (512+200) * ACC_OVERSAMPLING_XY; |
| } |
| if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
| versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |