WebSVN - FlightCtrl - Diff - Rev 1952 and 1955 - /branches/dongfang_FC



#include "sensors.h"

 
// for Delay functions

#include "timer0.h"
 

// For debugOut
#include "uart0.h"

 
// For reading and writing acc. meter offsets.

#include "eeprom.h"
 

  // for various filters...
  int16_t tempOffsetGyro, tempGyro;

 
  for (uint8_t axis=0; axis<2; axis++) {
 
    tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis];
    /*
     * Process the gyro data for the PID controller.
     */
    // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a

    //    gyro with a wider range, and helps counter saturation at full control.
   
    if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
      if (tempGyro < SENSOR_MIN_PITCHROLL) {
        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
        tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
        tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
          + SENSOR_MAX_PITCHROLL;
      } else {
        debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;

      }
    }

  rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
  if (GYRO_REVERSED[YAW])
    yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
  else
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
 
  debugOut.analog[3] = gyro_ATT[PITCH];
  debugOut.analog[4] = gyro_ATT[ROLL];
  debugOut.analog[5] = yawGyro;
}

 
void analog_updateAccelerometers(void) {
  // Pitch and roll axis accelerations.

 
  /*
    stronglyFilteredAcc[Z] =
    (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
  */
 
  debugOut.analog[6] = acc[PITCH];
  debugOut.analog[7] = acc[ROLL];

}
 
void analog_updateAirPressure(void) {
  static uint16_t pressureAutorangingWait = 25;
  uint16_t rawAirPressure;
  int16_t newrange;
  // air pressure
  if (pressureAutorangingWait) {
    //A range switch was done recently. Wait for steadying.
    pressureAutorangingWait--;
    debugOut.analog[27] = (uint16_t) OCR0A;
    debugOut.analog[31] = simpleAirPressure;
  } else {
    rawAirPressure = sensorInputs[AD_AIRPRESSURE];
    if (rawAirPressure < MIN_RAWPRESSURE) {

      }
    }

   
    // Even if the sample is off-range, use it.
    simpleAirPressure = getSimplePressure(rawAirPressure);
    debugOut.analog[27] = (uint16_t) OCR0A;

    debugOut.analog[31] = simpleAirPressure;
   
    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
      // Danger: pressure near lower end of range. If the measurement saturates, the
      // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
      airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
      // Danger: pressure near upper end of range. If the measurement saturates, the
      // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
      airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
        + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
    } 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_SUMMATION_FACTOR - 1)
        airPressureSum += simpleAirPressure / 2;
      else

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

Rev 1952	Rev 1955
Line 57...	Line 57...
57	#include "sensors.h"	57	#include "sensors.h"
Line 58...	Line 58...
58		58
59	// for Delay functions	59	// for Delay functions
Line 60...	Line 60...
60	#include "timer0.h"	60	#include "timer0.h"
61		61
Line 62...	Line 62...
62	// For DebugOut	62	// For debugOut
63	#include "uart0.h"	63	#include "uart0.h"
Line 64...	Line 64...
64		64
65	// For reading and writing acc. meter offsets.	65	// For reading and writing acc. meter offsets.
Line 66...	Line 66...
66	#include "eeprom.h"	66	#include "eeprom.h"
67		67
Line 281...	Line 281...
281	// for various filters...	281	// for various filters...
282	int16_t tempOffsetGyro, tempGyro;	282	int16_t tempOffsetGyro, tempGyro;
Line 283...	Line 283...
283		283
284	for (uint8_t axis=0; axis<2; axis++) {	284	for (uint8_t axis=0; axis<2; axis++) {
285	tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis];	-
286	// DebugOut.Analog[6 + 3 * axis ] = tempGyro;	285	tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis];
287	/*	286	/*
288	* Process the gyro data for the PID controller.	287	* Process the gyro data for the PID controller.
289	*/	288	*/
290	// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a	289	// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
Line 291...	Line 290...
291	// gyro with a wider range, and helps counter saturation at full control.	290	// gyro with a wider range, and helps counter saturation at full control.
292		291
293	if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {	292	if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
294	if (tempGyro < SENSOR_MIN_PITCHROLL) {	293	if (tempGyro < SENSOR_MIN_PITCHROLL) {
295	DebugOut.Digital[0] \|= DEBUG_SENSORLIMIT;	294	debugOut.digital[0] \|= DEBUG_SENSORLIMIT;
296	tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;	295	tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
297	} else if (tempGyro > SENSOR_MAX_PITCHROLL) {	296	} else if (tempGyro > SENSOR_MAX_PITCHROLL) {
298	DebugOut.Digital[0] \|= DEBUG_SENSORLIMIT;	297	debugOut.digital[0] \|= DEBUG_SENSORLIMIT;
299	tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE	298	tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
300	+ SENSOR_MAX_PITCHROLL;	299	+ SENSOR_MAX_PITCHROLL;
301	} else {	300	} else {
302	DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT;	301	debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
Line 303...	Line 302...
303	}	302	}
304	}	303	}
Line 342...	Line 341...
342	rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];	341	rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
343	if (GYRO_REVERSED[YAW])	342	if (GYRO_REVERSED[YAW])
344	yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];	343	yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
345	else	344	else
346	yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];	345	yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
-		346
-		347	debugOut.analog[3] = gyro_ATT[PITCH];
-		348	debugOut.analog[4] = gyro_ATT[ROLL];
-		349	debugOut.analog[5] = yawGyro;
347	}	350	}
Line 348...	Line 351...
348		351
349	void analog_updateAccelerometers(void) {	352	void analog_updateAccelerometers(void) {
350	// Pitch and roll axis accelerations.	353	// Pitch and roll axis accelerations.
Line 372...	Line 375...
372		375
373	/*	376	/*
374	stronglyFilteredAcc[Z] =	377	stronglyFilteredAcc[Z] =
375	(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;	378	(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
-		379	*/
-		380
-		381	debugOut.analog[6] = acc[PITCH];
376	*/	382	debugOut.analog[7] = acc[ROLL];
Line 377...	Line 383...
377	}	383	}
378		384
379	void analog_updateAirPressure(void) {	385	void analog_updateAirPressure(void) {
380	static uint16_t pressureAutorangingWait = 25;	386	static uint16_t pressureAutorangingWait = 25;
381	uint16_t rawAirPressure;	387	uint16_t rawAirPressure;
382	int16_t newrange;	388	int16_t newrange;
383	// air pressure	389	// air pressure
384	if (pressureAutorangingWait) {	390	if (pressureAutorangingWait) {
385	//A range switch was done recently. Wait for steadying.	391	//A range switch was done recently. Wait for steadying.
386	pressureAutorangingWait--;	392	pressureAutorangingWait--;
387	DebugOut.Analog[27] = (uint16_t) OCR0A;	393	debugOut.analog[27] = (uint16_t) OCR0A;
388	DebugOut.Analog[31] = simpleAirPressure;	394	debugOut.analog[31] = simpleAirPressure;
389	} else {	395	} else {
390	rawAirPressure = sensorInputs[AD_AIRPRESSURE];	396	rawAirPressure = sensorInputs[AD_AIRPRESSURE];
391	if (rawAirPressure < MIN_RAWPRESSURE) {	397	if (rawAirPressure < MIN_RAWPRESSURE) {
Line 415...	Line 421...
415	}	421	}
416	}	422	}
Line 417...	Line 423...
417		423
418	// Even if the sample is off-range, use it.	424	// Even if the sample is off-range, use it.
419	simpleAirPressure = getSimplePressure(rawAirPressure);	425	simpleAirPressure = getSimplePressure(rawAirPressure);
420	DebugOut.Analog[27] = (uint16_t) OCR0A;	426	debugOut.analog[27] = (uint16_t) OCR0A;
Line 421...	Line 427...
421	DebugOut.Analog[31] = simpleAirPressure;	427	debugOut.analog[31] = simpleAirPressure;
422		428
423	if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {	429	if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
424	// Danger: pressure near lower end of range. If the measurement saturates, the	430	// Danger: pressure near lower end of range. If the measurement saturates, the
425	// copter may climb uncontrolledly... Simulate a drastic reduction in pressure.	431	// copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
426	DebugOut.Digital[1] \|= DEBUG_SENSORLIMIT;	432	debugOut.digital[1] \|= DEBUG_SENSORLIMIT;
427	airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth	433	airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
428	+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION	434	+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
429	* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;	435	* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
430	} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {	436	} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
431	// Danger: pressure near upper end of range. If the measurement saturates, the	437	// Danger: pressure near upper end of range. If the measurement saturates, the
432	// copter may descend uncontrolledly... Simulate a drastic increase in pressure.	438	// copter may descend uncontrolledly... Simulate a drastic increase in pressure.
433	DebugOut.Digital[1] \|= DEBUG_SENSORLIMIT;	439	debugOut.digital[1] \|= DEBUG_SENSORLIMIT;
434	airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth	440	airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth
435	+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION	441	+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION
436	* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;	442	* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
437	} else {	443	} else {
438	// normal case.	444	// normal case.
439	// If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.	445	// If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.
440	// The 2 cases above (end of range) are ignored for this.	446	// The 2 cases above (end of range) are ignored for this.
441	DebugOut.Digital[1] &= ~DEBUG_SENSORLIMIT;	447	debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
442	if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1)	448	if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1)
443	airPressureSum += simpleAirPressure / 2;	449	airPressureSum += simpleAirPressure / 2;
444	else	450	else
Line 457...	Line 463...
457		463
458	void analog_updateBatteryVoltage(void) {	464	void analog_updateBatteryVoltage(void) {
459	// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).	465	// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
460	// This is divided by 3 --> 10.34 counts per volt.	466	// This is divided by 3 --> 10.34 counts per volt.
461	UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;	467	UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
462	DebugOut.Analog[11] = UBat;	468	debugOut.analog[11] = UBat;
Line 463...	Line 469...
463	}	469	}
464		470
465	void analog_update(void) {	471	void analog_update(void) {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c @ 1947 - Rev 1952 → 1955