Subversion Repositories FlightCtrl

Rev

Rev 2102 | Rev 2104 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2102 Rev 2103
Line 41... Line 41...
41 
 * integration to angles.
 41 
 * integration to angles.
42 
 */
 42 
 */
43 
int16_t gyro_PID[3];
 43 
int16_t gyro_PID[3];
44 
int16_t gyro_ATT[3];
 44 
int16_t gyro_ATT[3];
45 
int16_t gyroD[3];
 45 
int16_t gyroD[3];
46 
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
 46 
int16_t gyroDWindow[3][GYRO_D_WINDOW_LENGTH];
47 
uint8_t gyroDWindowIdx = 0;
 47 
uint8_t gyroDWindowIdx = 0;
48 
//int16_t dHeight;
 48 
//int16_t dHeight;
49 
//uint32_t gyroActivity;
 49 
//uint32_t gyroActivity;
Line 50... Line 50...
50 
 
 50 
 
Line 117... Line 117...
117 
 * Airspeed
 117 
 * Airspeed
118 
 */
 118 
 */
119 
uint16_t simpleAirPressure;
 119 
uint16_t simpleAirPressure;
120 
uint16_t airspeedVelocity;
 120 
uint16_t airspeedVelocity;
Line 121... Line -...
121 
 
 - 
 
122 
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.
 - 
 
123 
// int32_t filteredAirPressure;
 - 
 
124 
 
 - 
 
125 
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32
 - 
 
126 
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];
 - 
 
127 
int32_t windowedAirPressure;
 - 
 
128 
uint8_t windowPtr = 0;
 - 
 
129 
 
 - 
 
130 
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
 - 
 
131 
int32_t airPressureSum;
 - 
 
132 
 
 - 
 
133 
// The number of samples summed into airPressureSum so far.
 - 
 
134 
uint8_t pressureMeasurementCount;
 - 
 
135 
 
 - 
 
136 
 
 121 
 
137 
/*
 122 
/*
138 
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
 123 
 * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
139 
 * That is divided by 3 below, for a final 10.34 per volt.
 124 
 * That is divided by 3 below, for a final 10.34 per volt.
140 
 * So the initial value of 100 is for 9.7 volts.
 125 
 * So the initial value of 100 is for 9.7 volts.
Line 223... Line 208...
223 
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
 208 
        ADCSRA = (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
224 
        //Set ADC Control and Status Register B
 209 
        //Set ADC Control and Status Register B
225 
        //Trigger Source to Free Running Mode
 210 
        //Trigger Source to Free Running Mode
226 
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
 211 
        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
Line 227... Line -...
227 
 
 - 
 
228 
        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
 - 
 
229 
          airPressureWindow[i] = 0;
 - 
 
230 
        }
 - 
 
231 
    windowedAirPressure = 0;
 - 
 
232 
 
 212 
 
Line 233... Line 213...
233 
        startAnalogConversionCycle();
 213 
        startAnalogConversionCycle();
234 
 
 214 
 
235 
        // restore global interrupt flags
 215 
        // restore global interrupt flags
Line 320... Line 300...
320 
void analog_updateGyros(void) {
 300 
void analog_updateGyros(void) {
321 
  // for various filters...
 301 
  // for various filters...
322 
  int16_t tempOffsetGyro[3], tempGyro;
 302 
  int16_t tempOffsetGyro[3], tempGyro;
Line 323... Line 303...
323 
 
 303 
 
- 
 
 304 
  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
324 
  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
 305 
 
325 
  for (uint8_t axis=0; axis<3; axis++) {
 306 
  for (uint8_t axis=0; axis<3; axis++) {
326 
    tempGyro = rawGyroValue(axis);
 307 
    tempGyro = rawGyroValue(axis);
327 
    /*
 308 
    /*
328 
     * Process the gyro data for the PID controller.
 309 
     * Process the gyro data for the PID controller.
Line 374... Line 355...
374 
  rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW);
 355 
  rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW);
Line 375... Line 356...
375 
 
 356 
 
376 
  // dampenGyroActivity();
 357 
  // dampenGyroActivity();
377 
  gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
 358 
  gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
- 
 
 359 
  gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
Line 378... Line 360...
378 
  gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
 360 
  gyro_ATT[YAW] = tempOffsetGyro[YAW];
379 
 
 361 
 
380 
  if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
 362 
  if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
381 
      gyroDWindowIdx = 0;
 363 
      gyroDWindowIdx = 0;
Line 400... Line 382...
400 
  analog_updateAirPressure();
 382 
  analog_updateAirPressure();
401 
  analog_updateBatteryVoltage();
 383 
  analog_updateBatteryVoltage();
402 
#ifdef USE_MK3MAG
 384 
#ifdef USE_MK3MAG
403 
  magneticHeading = volatileMagneticHeading;
 385 
  magneticHeading = volatileMagneticHeading;
404 
#endif
 386 
#endif
405 
 
 - 
 
406 
}
 387 
}
Line 407... Line 388...
407 
 
 388 
 
408 
void analog_setNeutral() {
 389 
void analog_setNeutral() {
Line 428... Line 409...
428 
 
 409 
 
429 
  // gyroActivity = 0;
 410 
  // gyroActivity = 0;
Line 430... Line 411...
430 
}
 411 
}
431 
 
 412 
 
432 
void analog_calibrateGyros(void) {
 413 
void analog_calibrateGyros(void) {
433 
#define GYRO_OFFSET_CYCLES 32
 414 
#define GYRO_OFFSET_CYCLES 64
434 
  uint8_t i, axis;
 415 
  uint8_t i, axis;
Line 435... Line 416...
435 
  int32_t offsets[3] = { 0, 0, 0 };
 416 
  int32_t offsets[3] = { 0, 0, 0 };