WebSVN - FlightCtrl - Diff - Rev 2018 and 2019 - /branches/dongfang_FC



 * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
 * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
 * 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) {
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
  // Pitch to Pitch part

  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
  // Roll to Roll part
  int8_t yy = rotationTab[quadrant];

 
  int16_t xIn = result[0];
  result[0] = xx*xIn + xy*result[1];

  result[1] = yx*xIn + yy*result[1];
 
  if (quadrant & 1) {

        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
        result[0] = (result[0]*11) >> 4;
        result[1] = (result[1]*11) >> 4;
  }
}
 
/*
 * Air pressure
 */
volatile uint8_t rangewidth = 105;

// volatile uint16_t rawAirPressure;

 
// Value of 2 samples, with range.

uint16_t simpleAirPressure;
 

// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
int32_t filteredAirPressure;

    } else {
      // normal case.
      // 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_OVERSAMPLING - 1)
        airPressureSum += simpleAirPressure / 2;
      else
        airPressureSum += simpleAirPressure;
    }

   
    // 2 samples were added.
    pressureMeasurementCount += 2;
    if (pressureMeasurementCount >= AIRPRESSURE_OVERSAMPLING) {
      filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1)
                             + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;
      pressureMeasurementCount = airPressureSum = 0;
    }

void analog_updateBatteryVoltage(void) {
  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
  // This is divided by 3 --> 10.34 counts per volt.
  UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
  debugOut.analog[11] = UBat;
 
}

 
void analog_update(void) {
  analog_updateGyros();

void analog_setNeutral() {
  gyro_init();

 
  if (gyroOffset_readFromEEProm()) {
    printf("gyro offsets invalid%s",recal);
    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_OVERSAMPLING_XY;

    accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
  }
 

  // Setting offset values has an influence in the analog.c ISR
  // Therefore run measurement for 100ms to achive stable readings
  delay_ms_with_adc_measurement(100, 0);

 
  // Rough estimate. Hmm no nothing happens at calibration anyway.
  // airPressureSum = simpleAirPressure * (AIRPRESSURE_OVERSAMPLING/2);
  // pressureMeasurementCount = 0;

}
 

  }

 
  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_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;

    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
    int16_t min,max;
    if (axis==Z) {
        if (staticParams.imuReversedFlags & 8) {
        // TODO: This assumes a sensitivity of +/- 2g.
                min = (256-200) * ACC_OVERSAMPLING_Z;
                        max = (256+200) * ACC_OVERSAMPLING_Z;
        } else {
        // TODO: This assumes a sensitivity of +/- 2g.
                min = (768-200) * ACC_OVERSAMPLING_Z;
                        max = (768+200) * ACC_OVERSAMPLING_Z;
        }
    } else {
        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;
    }
  }

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] = xxresult[0] + xyresult[1];	151	result[0] = xxxIn + xyresult[1];
Line 153...	Line 152...
153	result[1] = yxxIn + yyresult[1];	152	result[1] = yxxIn + yyresult[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	}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c @ 2033 - Rev 2018 → 2019