Rev 2018 | Rev 2020 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2018 | Rev 2019 | ||
---|---|---|---|
Line 132... | Line 132... | ||
132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
132 | * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed |
133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
133 | * 6: pp=0, pr=1, rp=1, rr=0 // +270 degrees with pitch reversed |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
134 | * 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
135 | */ |
135 | */ |
Line 136... | Line 136... | ||
136 | 136 | ||
Line 137... | Line -... | ||
137 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
- | |
138 | 137 | //void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {} |
|
139 | /* |
138 | |
140 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
139 | void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
141 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
140 | static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
142 | // Pitch to Pitch part |
141 | // Pitch to Pitch part |
Line 147... | Line 146... | ||
147 | int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
146 | int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
148 | // Roll to Roll part |
147 | // Roll to Roll part |
149 | int8_t yy = rotationTab[quadrant]; |
148 | int8_t yy = rotationTab[quadrant]; |
Line 150... | Line 149... | ||
150 | 149 | ||
151 | int16_t xIn = result[0]; |
150 | int16_t xIn = result[0]; |
152 | result[0] = xx*result[0] + xy*result[1]; |
151 | result[0] = xx*xIn + xy*result[1]; |
Line 153... | Line 152... | ||
153 | result[1] = yx*xIn + yy*result[1]; |
152 | result[1] = yx*xIn + yy*result[1]; |
154 | |
153 | |
155 | if (quadrant & 1) { |
154 | if (quadrant & 1) { |
Line 159... | Line 158... | ||
159 | // of 2 11 bit numbers, so 12 bits. We have 4 bits left... |
158 | // of 2 11 bit numbers, so 12 bits. We have 4 bits left... |
160 | result[0] = (result[0]*11) >> 4; |
159 | result[0] = (result[0]*11) >> 4; |
161 | result[1] = (result[1]*11) >> 4; |
160 | result[1] = (result[1]*11) >> 4; |
162 | } |
161 | } |
163 | } |
162 | } |
164 | */ |
163 | |
165 | /* |
164 | /* |
166 | * Air pressure |
165 | * Air pressure |
167 | */ |
166 | */ |
168 | volatile uint8_t rangewidth = 105; |
167 | volatile uint8_t rangewidth = 105; |
Line 171... | Line 170... | ||
171 | // volatile uint16_t rawAirPressure; |
170 | // volatile uint16_t rawAirPressure; |
Line 172... | Line 171... | ||
172 | 171 | ||
173 | // Value of 2 samples, with range. |
172 | // Value of 2 samples, with range. |
Line 174... | Line 173... | ||
174 | uint16_t simpleAirPressure; |
173 | uint16_t simpleAirPressure; |
175 | 174 | ||
Line 176... | Line 175... | ||
176 | // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered. |
175 | // Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
177 | int32_t filteredAirPressure; |
176 | int32_t filteredAirPressure; |
Line 490... | Line 489... | ||
490 | } else { |
489 | } else { |
491 | // normal case. |
490 | // normal case. |
492 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
491 | // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample. |
493 | // The 2 cases above (end of range) are ignored for this. |
492 | // The 2 cases above (end of range) are ignored for this. |
494 | debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
493 | debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
495 | if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1) |
494 | if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING - 1) |
496 | airPressureSum += simpleAirPressure / 2; |
495 | airPressureSum += simpleAirPressure / 2; |
497 | else |
496 | else |
498 | airPressureSum += simpleAirPressure; |
497 | airPressureSum += simpleAirPressure; |
499 | } |
498 | } |
Line 500... | Line 499... | ||
500 | 499 | ||
501 | // 2 samples were added. |
500 | // 2 samples were added. |
502 | pressureMeasurementCount += 2; |
501 | pressureMeasurementCount += 2; |
503 | if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) { |
502 | if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) { |
504 | filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
503 | filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1) |
505 | + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
504 | + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER; |
506 | pressureMeasurementCount = airPressureSum = 0; |
505 | pressureMeasurementCount = airPressureSum = 0; |
507 | } |
506 | } |
Line 511... | Line 510... | ||
511 | void analog_updateBatteryVoltage(void) { |
510 | void analog_updateBatteryVoltage(void) { |
512 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
511 | // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v). |
513 | // This is divided by 3 --> 10.34 counts per volt. |
512 | // This is divided by 3 --> 10.34 counts per volt. |
514 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
513 | UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4; |
515 | debugOut.analog[11] = UBat; |
514 | debugOut.analog[11] = UBat; |
516 | debugOut.analog[21] = sensorInputs[AD_UBAT]; |
- | |
517 | } |
515 | } |
Line 518... | Line 516... | ||
518 | 516 | ||
519 | void analog_update(void) { |
517 | void analog_update(void) { |
520 | analog_updateGyros(); |
518 | analog_updateGyros(); |
Line 526... | Line 524... | ||
526 | void analog_setNeutral() { |
524 | void analog_setNeutral() { |
527 | gyro_init(); |
525 | gyro_init(); |
Line 528... | Line 526... | ||
528 | 526 | ||
529 | if (gyroOffset_readFromEEProm()) { |
527 | if (gyroOffset_readFromEEProm()) { |
530 | printf("gyro offsets invalid%s",recal); |
528 | printf("gyro offsets invalid%s",recal); |
531 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
529 | gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL; |
532 | gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
530 | gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW; |
Line 533... | Line 531... | ||
533 | } |
531 | } |
534 | 532 | ||
535 | if (accOffset_readFromEEProm()) { |
533 | if (accOffset_readFromEEProm()) { |
536 | printf("acc. meter offsets invalid%s",recal); |
534 | printf("acc. meter offsets invalid%s",recal); |
537 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
535 | accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
Line 538... | Line 536... | ||
538 | accOffset.offsets[Z] = 717 * ACC_SUMMATION_FACTOR_Z; |
536 | accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
539 | } |
537 | } |
540 | 538 | ||
Line 545... | Line 543... | ||
545 | // Setting offset values has an influence in the analog.c ISR |
543 | // Setting offset values has an influence in the analog.c ISR |
546 | // Therefore run measurement for 100ms to achive stable readings |
544 | // Therefore run measurement for 100ms to achive stable readings |
547 | delay_ms_with_adc_measurement(100, 0); |
545 | delay_ms_with_adc_measurement(100, 0); |
Line 548... | Line 546... | ||
548 | 546 | ||
549 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
547 | // Rough estimate. Hmm no nothing happens at calibration anyway. |
550 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
548 | // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2); |
551 | // pressureMeasurementCount = 0; |
549 | // pressureMeasurementCount = 0; |
Line 552... | Line 550... | ||
552 | } |
550 | } |
553 | 551 | ||
Line 566... | Line 564... | ||
566 | } |
564 | } |
Line 567... | Line 565... | ||
567 | 565 | ||
568 | for (axis = PITCH; axis <= YAW; axis++) { |
566 | for (axis = PITCH; axis <= YAW; axis++) { |
Line 569... | Line 567... | ||
569 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
567 | gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
570 | 568 | ||
571 | int16_t min = (512-200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
569 | int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
572 | int16_t max = (512+200) * (axis==YAW) ? GYRO_SUMMATION_FACTOR_YAW : GYRO_SUMMATION_FACTOR_PITCHROLL; |
570 | int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
573 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
571 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
Line 574... | Line 572... | ||
574 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
572 | versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
Line 601... | Line 599... | ||
601 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
599 | accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
602 | int16_t min,max; |
600 | int16_t min,max; |
603 | if (axis==Z) { |
601 | if (axis==Z) { |
604 | if (staticParams.imuReversedFlags & 8) { |
602 | if (staticParams.imuReversedFlags & 8) { |
605 | // TODO: This assumes a sensitivity of +/- 2g. |
603 | // TODO: This assumes a sensitivity of +/- 2g. |
606 | min = (256-200) * ACC_SUMMATION_FACTOR_Z; |
604 | min = (256-200) * ACC_OVERSAMPLING_Z; |
607 | max = (256+200) * ACC_SUMMATION_FACTOR_Z; |
605 | max = (256+200) * ACC_OVERSAMPLING_Z; |
608 | } else { |
606 | } else { |
609 | // TODO: This assumes a sensitivity of +/- 2g. |
607 | // TODO: This assumes a sensitivity of +/- 2g. |
610 | min = (768-200) * ACC_SUMMATION_FACTOR_Z; |
608 | min = (768-200) * ACC_OVERSAMPLING_Z; |
611 | max = (768+200) * ACC_SUMMATION_FACTOR_Z; |
609 | max = (768+200) * ACC_OVERSAMPLING_Z; |
612 | } |
610 | } |
613 | } else { |
611 | } else { |
614 | min = (512-200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
612 | min = (512-200) * ACC_OVERSAMPLING_XY; |
615 | max = (512+200) * ACC_SUMMATION_FACTOR_PITCHROLL; |
613 | max = (512+200) * ACC_OVERSAMPLING_XY; |
616 | } |
614 | } |
617 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
615 | if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
618 | versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |
616 | versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |
619 | } |
617 | } |
620 | } |
618 | } |