Subversion Repositories FlightCtrl

Rev

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

Rev 2019 Rev 2020
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 
 
 - 
 
137 
//void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {}
 - 
 
138 
 
 136 
 
139 
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
 137 
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
140 
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
 138 
  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
141 
  // Pitch to Pitch part
 139 
  // Pitch to Pitch part
142 
  int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
 140 
  int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
143 
  // Roll to Pitch part
 141 
  // Roll to Pitch part
144 
  int8_t xy = rotationTab[(quadrant+2)%8];
 142 
  int8_t xy = rotationTab[(quadrant+2)%8];
145 
  // Pitch to Roll part
 143 
  // Pitch to Roll part
146 
  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
 144 
  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
Line 362... Line 360...
362 
    // 2) Apply sign and offset, scale before filtering.
 360 
    // 2) Apply sign and offset, scale before filtering.
363 
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
 361 
    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
364 
  }
 362 
  }
Line 365... Line 363...
365 
 
 363 
 
366 
  // 2.1: Transform axes.
 364 
  // 2.1: Transform axes.
Line 367... Line 365...
367 
  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
 365 
  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
368 
 
 366 
 
369 
  for (uint8_t axis=0; axis<2; axis++) {
 367 
  for (uint8_t axis=0; axis<2; axis++) {
Line 384... Line 382...
384 
  }
 382 
  }
Line 385... Line 383...
385 
 
 383 
 
386 
  /*
 384 
  /*
387 
   * Now process the data for attitude angles.
 385 
   * Now process the data for attitude angles.
388 
   */
 386 
   */
Line 389... Line 387...
389 
   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
 387 
   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_GYRO_PR);
390 
 
 388 
 
Line 391... Line 389...
391 
   gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
 389 
   gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
Line 400... Line 398...
400 
  // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
 398 
  // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
401 
  // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
 399 
  // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
402 
  // gyro_ATT[ROLL]  = (gyro_ATT[ROLL]  * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL])  / staticParams.attitudeGyroFilter;
 400 
  // gyro_ATT[ROLL]  = (gyro_ATT[ROLL]  * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL])  / staticParams.attitudeGyroFilter;
Line 403... Line 401...
403 
 
 401 
 
404 
  // Yaw gyro.
 402 
  // Yaw gyro.
405 
  if (staticParams.imuReversedFlags & 2)
 403 
  if (staticParams.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
406 
    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
 404 
    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
407 
  else
 405 
  else
408 
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
 406 
    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
Line 412... Line 410...
412 
  // Pitch and roll axis accelerations.
 410 
  // Pitch and roll axis accelerations.
413 
  for (uint8_t axis=0; axis<2; axis++) {
 411 
  for (uint8_t axis=0; axis<2; axis++) {
414 
    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
 412 
    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
415 
  }
 413 
  }
Line 416... Line 414...
416 
 
 414 
 
Line 417... Line 415...
417 
  rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4);
 415 
  rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & IMU_REVERSE_ACC_XY);
418 
 
 416 
 
419 
  for(uint8_t axis=0; axis<3; axis++) {
 417 
  for(uint8_t axis=0; axis<3; axis++) {
420 
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
 418 
    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
Line 597... Line 595...
597 
 
 595 
 
598 
  for (axis = PITCH; axis <= YAW; axis++) {
 596 
  for (axis = PITCH; axis <= YAW; axis++) {
599 
    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
 597 
    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
600 
    int16_t min,max;
 598 
    int16_t min,max;
601 
    if (axis==Z) {
 599 
    if (axis==Z) {
602 
        if (staticParams.imuReversedFlags & 8) {
 600 
        if (staticParams.imuReversedFlags & IMU_REVERSE_ACC_Z) {
603 
        // TODO: This assumes a sensitivity of +/- 2g.
 601 
        // TODO: This assumes a sensitivity of +/- 2g.
604 
                min = (256-200) * ACC_OVERSAMPLING_Z;
 602 
                min = (256-200) * ACC_OVERSAMPLING_Z;
605 
                        max = (256+200) * ACC_OVERSAMPLING_Z;
 603 
                        max = (256+200) * ACC_OVERSAMPLING_Z;
606 
        } else {
 604 
        } else {