WebSVN - FlightCtrl - Diff - Rev 2095 and 2160 - /branches/dongfang_FC


#include <stdlib.h>
#include <avr/io.h>
#include <stdlib.h>
 
#include "attitude.h"
#include "dongfangMath.h"
#include "commands.h"
 
// For scope debugging only!
#include "rc.h"
 
// where our main data flow comes from.
#include "analog.h"
 
#include "configuration.h"
#include "output.h"
 
// Some calculations are performed depending on some stick related things.
#include "controlMixer.h"
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
 * Gyro readings, as read from the analog module. It would have been nice to flow
 * them around between the different calculations as a struct or array (doing
 * things functionally without side effects) but this is shorter and probably
 * faster too.
 * The variables are overwritten at each attitude calculation invocation - the values
 * are not preserved or reused.
 */
int16_t rate_ATT[2], yawRate;
 
// With different (less) filtering
int16_t rate_PID[2];
int16_t differential[2];
 
/*
 * Gyro readings, after performing "axis coupling" - that is, the transfomation
 * of rotation rates from the airframe-local coordinate system to a ground-fixed
 * coordinate system. If axis copling is disabled, the gyro readings will be
 * copied into these directly.
 * These are global for the same pragmatic reason as with the gyro readings.
 * The variables are overwritten at each attitude calculation invocation - the values
 * are not preserved or reused.
 */
int16_t ACRate[2], ACYawRate;
 
/*
 * Gyro integrals. These are the rotation angles of the airframe compared to the
 * horizontal plane, yaw relative to yaw at start.
 */
int32_t attitude[2];
 
//int readingHeight = 0;
 
// Yaw angle and compass stuff.
int32_t headingError;
 
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
// Not necessary. Never read anywhere.
// int16_t compassOffCourse = 0;
 
uint16_t ignoreCompassTimer = 0;// 500;
 
int32_t heading; // Yaw Gyro Integral supported by compass
int16_t yawGyroDrift;
 
int16_t correctionSum[2] = { 0, 0 };
 
// For NaviCTRL use.
int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;
 
/*
 * Experiment: Compensating for dynamic-induced gyro biasing.
 */
int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
// uint16_t accVector;
 
// uint32_t gyroActivity;
 
/************************************************************************
 * Set inclination angles from the acc. sensor data.                    
 * If acc. sensors are not used, set to zero.                          
 * TODO: One could use inverse sine to calculate the angles more        
 * accurately, but since: 1) the angles are rather small at times when
 * it makes sense to set the integrals (standing on ground, or flying at  
 * constant speed, and 2) at small angles a, sin(a) ~= constant * a,    
 * it is hardly worth the trouble.                                      
 ************************************************************************/
 
int32_t getAngleEstimateFromAcc(uint8_t axis) {
  //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;
  // return 342L * filteredAcc[axis];
}
 
void setStaticAttitudeAngles(void) {
#ifdef ATTITUDE_USE_ACC_SENSORS
  attitude[PITCH] = getAngleEstimateFromAcc(PITCH);
  attitude[ROLL] = getAngleEstimateFromAcc(ROLL);
#else
  attitude[PITCH] = attitude[ROLL] = 0;
#endif
}
 
/************************************************************************
 * Neutral Readings                                                    
 ************************************************************************/
void attitude_setNeutral(void) {
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
  // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
 
  driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
 
  // Calibrate hardware.
  analog_setNeutral();
 
  // reset gyro integrals to acc guessing
  setStaticAttitudeAngles();
 
#ifdef USE_MK3MAG
  attitude_resetHeadingToMagnetic();
#endif
  // Servo_On(); //enable servo output
}
 
/************************************************************************
 * Get sensor data from the analog module, and release the ADC          
 * TODO: Ultimately, the analog module could do this (instead of dumping
 * the values into variables).
 * The rate variable end up in a range of about [-1024, 1023].
 *************************************************************************/
void getAnalogData(void) {
  uint8_t axis;
 
  analog_update();
 
  for (axis = PITCH; axis <= ROLL; axis++) {
    rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
    rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
    differential[axis] = gyroD[axis];
    averageAcc[axis] += acc[axis];
  }
 
  averageAccCount++;
  yawRate = yawGyro + driftCompYaw;
}
 
/*
 * This is the standard flight-style coordinate system transformation
 * (from airframe-local axes to a ground-based system). For some reason
 * the MK uses a left-hand coordinate system. The tranformation has been
 * changed accordingly.
 */
void trigAxisCoupling(void) {
  int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
  int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
 
  int16_t cospitch = cos_360(pitchAngleInDegrees);
  int16_t cosroll = cos_360(rollAngleInDegrees);
  int16_t sinroll = sin_360(rollAngleInDegrees);
 
  ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll
      - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR);
 
  ACRate[ROLL] = rate_ATT[ROLL]
      + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
          >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees))
          >> LOG_MATH_UNIT_FACTOR);
 
  ACYawRate =
      ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
          / cospitch;
}
 
// 480 usec with axis coupling - almost no time without.
void integrate(void) {
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
  uint8_t axis;
 
  if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) {
    trigAxisCoupling();
  } else {
    ACRate[PITCH] = rate_ATT[PITCH];
    ACRate[ROLL] = rate_ATT[ROLL];
    ACYawRate = yawRate;
  }
 
  /*
   * Yaw
   * Calculate yaw gyro integral (~ to rotation angle)
   * Limit heading proportional to 0 deg to 360 deg
   */
  heading += ACYawRate;
  intervalWrap(&heading, YAWOVER360);
  headingError += ACYawRate;
 
  /*
   * Pitch axis integration and range boundary wrap.
   */
  for (axis = PITCH; axis <= ROLL; axis++) {
    attitude[axis] += ACRate[axis];
    if (attitude[axis] > PITCHROLLOVER180) {
      attitude[axis] -= PITCHROLLOVER360;
    } else if (attitude[axis] <= -PITCHROLLOVER180) {
      attitude[axis] += PITCHROLLOVER360;
    }
  }
}
 
/************************************************************************
 * A kind of 0'th order integral correction, that corrects the integrals
 * directly. This is the "gyroAccFactor" stuff in the original code.
 * There is (there) also a drift compensation
 * - it corrects the differential of the integral = the gyro offsets.
 * That should only be necessary with drifty gyros like ENC-03.
 ************************************************************************/
#define LOG_DIVIDER 12
#define DIVIDER (1L << LOG_DIVIDER)
void correctIntegralsByAcc0thOrder(void) {
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
  // are less than ....., or reintroduce Kalman.
  // Well actually the Z axis acc. check is not so silly.
  uint8_t axis;
  int32_t temp;
 
 
 
 
 
  debugOut.analog[12] = IMUConfig.zerothOrderCorrection;
 
  uint16_t ca = gyroActivity >> 14;
  uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
  debugOut.analog[15] = gyroActivityWeighted;
 
  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
 
  uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;
 
  debugOut.analog[28] = IMUConfig.rateTolerance;
 
  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 
  if (gyroActivityWeighted < 8) {
    debugOut.digital[0] |= DEBUG_ACC0THORDER;
  }
  if (gyroActivityWeighted <= 2) {
    debugOut.digital[1] |= DEBUG_ACC0THORDER;
  }
 
  /*
   * Add to each sum: The amount by which the angle is changed just below.
   */
  for (axis = PITCH; axis <= ROLL; axis++) {
    int32_t accDerived = getAngleEstimateFromAcc(axis);
    //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
    // 1000 * the correction amount that will be added to the gyro angle in next line.
    temp = attitude[axis];
    attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER;
    correctionSum[axis] += attitude[axis] - temp;
  }
}
 
/************************************************************************
 * This is an attempt to correct not the error in the angle integrals
 * (that happens in correctIntegralsByAcc0thOrder above) but rather the
 * cause of it: Gyro drift, vibration and rounding errors.
 * All the corrections made in correctIntegralsByAcc0thOrder over
 * DRIFTCORRECTION_TIME cycles are summed up. This number is
 * then divided by DRIFTCORRECTION_TIME to get the approx.
 * correction that should have been applied to each iteration to fix
 * the error. This is then added to the dynamic offsets.
 ************************************************************************/
// 2 times / sec. = 488/2
#define DRIFTCORRECTION_TIME 256L
void driftCorrection(void) {
  static int16_t timer = DRIFTCORRECTION_TIME;
  int16_t deltaCorrection;
  int16_t round;
  uint8_t axis;
 
  if (!--timer) {
    timer = DRIFTCORRECTION_TIME;
    for (axis = PITCH; axis <= ROLL; axis++) {
      // Take the sum of corrections applied, add it to delta
      if (correctionSum[axis] >= 0)
        round = DRIFTCORRECTION_TIME / 2;
      else
        round = -DRIFTCORRECTION_TIME / 2;
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
      driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;
      CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
      debugOut.analog[6 + axis] = correctionSum[axis];
      debugOut.analog[13 + axis] = driftComp[axis];
      correctionSum[axis] = 0;
    }
  }
}
 
/*
void calculateAccVector(void) {
  int16_t temp;
  temp = filteredAcc[0] >> 3;
  accVector = temp * temp;
  temp = filteredAcc[1] >> 3;
  accVector += temp * temp;
  temp = filteredAcc[2] >> 3;
  accVector += temp * temp;
}
*/
 
#ifdef USE_MK3MAG
void attitude_resetHeadingToMagnetic(void) {
  if (commands_isCalibratingCompass())
    return;
 
  // Compass is off, skip.
  if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED))
      return;
 
  // Compass is invalid, skip.
  if (magneticHeading < 0)
    return;
 
  heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
  //targetHeading = heading;
  headingError = 0;
}
 
void correctHeadingToMagnetic(void) {
  int32_t error;
 
  if (commands_isCalibratingCompass()) {
    //debugOut.analog[30] = -1;
    return;
  }
 
  // Compass is off, skip.
  // Naaah this is assumed.
  // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
  //     return;
 
  // Compass is invalid, skip.
  if (magneticHeading < 0) {
    //debugOut.analog[30] = -2;
    return;
  }
 
  // Spinning fast, skip
  if (abs(yawRate) > 128) {
    // debugOut.analog[30] = -3;
    return;
  }
 
  // Otherwise invalidated, skip
  if (ignoreCompassTimer) {
    ignoreCompassTimer--;
    //debugOut.analog[30] = -4;
    return;
  }
 
  //debugOut.analog[30] = magneticHeading;
 
  // TODO: Find computational cost of this.
  error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading);
  if (error <= -YAWOVER180) error += YAWOVER360;
  else if (error > YAWOVER180) error -= YAWOVER360;
 
  // We only correct errors larger than the resolution of the compass, or else we would keep rounding the
  // better resolution of the gyros to the worse resolution of the compass all the time.
  // The correction should really only serve to compensate for gyro drift.
  if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
 
  int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
  //debugOut.analog[30] = correction;
 
  debugOut.digital[0] &= ~DEBUG_COMPASS;
  debugOut.digital[1] &= ~DEBUG_COMPASS;
 
  if (correction > 0) {
          debugOut.digital[0] ^= DEBUG_COMPASS;
  } else if (correction < 0) {
          debugOut.digital[1] ^= DEBUG_COMPASS;
  }
 
  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
  // and to the heading error (the angle of yaw that the copter is off the set heading).
  heading += correction;
  headingError += correction;
  intervalWrap(&heading, YAWOVER360);
 
  // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all
  // when the compass corrects the heading - it only corrects numbers!) we want to add:
  // This will however cause drift to remain uncorrected!
  // headingError += correction;
  //debugOut.analog[29] = 0;
}
#endif
 
/************************************************************************
 * Main procedure.
 ************************************************************************/
void calculateFlightAttitude(void) {
  getAnalogData();
  // calculateAccVector();
  integrate();
 
#ifdef ATTITUDE_USE_ACC_SENSORS
  correctIntegralsByAcc0thOrder();
  driftCorrection();
#endif
 
  // We are done reading variables from the analog module.
  // Interrupt-driven sensor reading may restart.
  startAnalogConversionCycle();
 
#ifdef USE_MK3MAG
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
    correctHeadingToMagnetic();
  }
#endif
}
 

Rev 2095	Rev 2160
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include <stdlib.h>	3	#include <stdlib.h>
4		4
5	#include "attitude.h"	5	#include "attitude.h"
6	#include "dongfangMath.h"	6	#include "dongfangMath.h"
7	#include "commands.h"	7	#include "commands.h"
8		8
9	// For scope debugging only!	9	// For scope debugging only!
10	#include "rc.h"	10	#include "rc.h"
11		11
12	// where our main data flow comes from.	12	// where our main data flow comes from.
13	#include "analog.h"	13	#include "analog.h"
14		14
15	#include "configuration.h"	15	#include "configuration.h"
16	#include "output.h"	16	#include "output.h"
17		17
18	// Some calculations are performed depending on some stick related things.	18	// Some calculations are performed depending on some stick related things.
19	#include "controlMixer.h"	19	#include "controlMixer.h"
20		20
21	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}	21	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
22		22
23	/*	23	/*
24	* Gyro readings, as read from the analog module. It would have been nice to flow	24	* Gyro readings, as read from the analog module. It would have been nice to flow
25	* them around between the different calculations as a struct or array (doing	25	* them around between the different calculations as a struct or array (doing
26	* things functionally without side effects) but this is shorter and probably	26	* things functionally without side effects) but this is shorter and probably
27	* faster too.	27	* faster too.
28	* The variables are overwritten at each attitude calculation invocation - the values	28	* The variables are overwritten at each attitude calculation invocation - the values
29	* are not preserved or reused.	29	* are not preserved or reused.
30	*/	30	*/
31	int16_t rate_ATT[2], yawRate;	31	int16_t rate_ATT[2], yawRate;
32		32
33	// With different (less) filtering	33	// With different (less) filtering
34	int16_t rate_PID[2];	34	int16_t rate_PID[2];
35	int16_t differential[2];	35	int16_t differential[2];
36		36
37	/*	37	/*
38	* Gyro readings, after performing "axis coupling" - that is, the transfomation	38	* Gyro readings, after performing "axis coupling" - that is, the transfomation
39	* of rotation rates from the airframe-local coordinate system to a ground-fixed	39	* of rotation rates from the airframe-local coordinate system to a ground-fixed
40	* coordinate system. If axis copling is disabled, the gyro readings will be	40	* coordinate system. If axis copling is disabled, the gyro readings will be
41	* copied into these directly.	41	* copied into these directly.
42	* These are global for the same pragmatic reason as with the gyro readings.	42	* These are global for the same pragmatic reason as with the gyro readings.
43	* The variables are overwritten at each attitude calculation invocation - the values	43	* The variables are overwritten at each attitude calculation invocation - the values
44	* are not preserved or reused.	44	* are not preserved or reused.
45	*/	45	*/
46	int16_t ACRate[2], ACYawRate;	46	int16_t ACRate[2], ACYawRate;
47		47
48	/*	48	/*
49	* Gyro integrals. These are the rotation angles of the airframe compared to the	49	* Gyro integrals. These are the rotation angles of the airframe compared to the
50	* horizontal plane, yaw relative to yaw at start.	50	* horizontal plane, yaw relative to yaw at start.
51	*/	51	*/
52	int32_t attitude[2];	52	int32_t attitude[2];
53		53
54	//int readingHeight = 0;	54	//int readingHeight = 0;
55		55
56	// Yaw angle and compass stuff.	56	// Yaw angle and compass stuff.
57	int32_t headingError;	57	int32_t headingError;
58		58
59	// The difference between the above 2 (heading - course) on a -180..179 degree interval.	59	// The difference between the above 2 (heading - course) on a -180..179 degree interval.
60	// Not necessary. Never read anywhere.	60	// Not necessary. Never read anywhere.
61	// int16_t compassOffCourse = 0;	61	// int16_t compassOffCourse = 0;
62		62
63	uint16_t ignoreCompassTimer = 0;// 500;	63	uint16_t ignoreCompassTimer = 0;// 500;
64		64
65	int32_t heading; // Yaw Gyro Integral supported by compass	65	int32_t heading; // Yaw Gyro Integral supported by compass
66	int16_t yawGyroDrift;	66	int16_t yawGyroDrift;
67		67
68	int16_t correctionSum[2] = { 0, 0 };	68	int16_t correctionSum[2] = { 0, 0 };
69		69
70	// For NaviCTRL use.	70	// For NaviCTRL use.
71	int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;	71	int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0;
72		72
73	/*	73	/*
74	* Experiment: Compensating for dynamic-induced gyro biasing.	74	* Experiment: Compensating for dynamic-induced gyro biasing.
75	*/	75	*/
76	int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;	76	int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;
77	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;	77	// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
78	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;	78	// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
79	// int16_t dynamicCalCount;	79	// int16_t dynamicCalCount;
80	// uint16_t accVector;	80	// uint16_t accVector;
81		81
82	// uint32_t gyroActivity;	82	// uint32_t gyroActivity;
83		83
84	/************************************************************************	84	/************************************************************************
85	* Set inclination angles from the acc. sensor data.	85	* Set inclination angles from the acc. sensor data.
86	* If acc. sensors are not used, set to zero.	86	* If acc. sensors are not used, set to zero.
87	* TODO: One could use inverse sine to calculate the angles more	87	* TODO: One could use inverse sine to calculate the angles more
88	* accurately, but since: 1) the angles are rather small at times when	88	* accurately, but since: 1) the angles are rather small at times when
89	* it makes sense to set the integrals (standing on ground, or flying at	89	* it makes sense to set the integrals (standing on ground, or flying at
90	* constant speed, and 2) at small angles a, sin(a) ~= constant * a,	90	* constant speed, and 2) at small angles a, sin(a) ~= constant * a,
91	* it is hardly worth the trouble.	91	* it is hardly worth the trouble.
92	************************************************************************/	92	************************************************************************/
93		93
94	int32_t getAngleEstimateFromAcc(uint8_t axis) {	94	int32_t getAngleEstimateFromAcc(uint8_t axis) {
95	//int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;	95	//int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L;
96	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;	96	return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm;
97	// return 342L * filteredAcc[axis];	97	// return 342L * filteredAcc[axis];
98	}	98	}
99		99
100	void setStaticAttitudeAngles(void) {	100	void setStaticAttitudeAngles(void) {
101	#ifdef ATTITUDE_USE_ACC_SENSORS	101	#ifdef ATTITUDE_USE_ACC_SENSORS
102	attitude[PITCH] = getAngleEstimateFromAcc(PITCH);	102	attitude[PITCH] = getAngleEstimateFromAcc(PITCH);
103	attitude[ROLL] = getAngleEstimateFromAcc(ROLL);	103	attitude[ROLL] = getAngleEstimateFromAcc(ROLL);
104	#else	104	#else
105	attitude[PITCH] = attitude[ROLL] = 0;	105	attitude[PITCH] = attitude[ROLL] = 0;
106	#endif	106	#endif
107	}	107	}
108		108
109	/************************************************************************	109	/************************************************************************
110	* Neutral Readings	110	* Neutral Readings
111	************************************************************************/	111	************************************************************************/
112	void attitude_setNeutral(void) {	112	void attitude_setNeutral(void) {
113	// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.	113	// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
114	// dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;	114	// dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
115		115
116	driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;	116	driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
117	correctionSum[PITCH] = correctionSum[ROLL] = 0;	117	correctionSum[PITCH] = correctionSum[ROLL] = 0;
118		118
119	// Calibrate hardware.	119	// Calibrate hardware.
120	analog_setNeutral();	120	analog_setNeutral();
121		121
122	// reset gyro integrals to acc guessing	122	// reset gyro integrals to acc guessing
123	setStaticAttitudeAngles();	123	setStaticAttitudeAngles();
124		124
125	#ifdef USE_MK3MAG	125	#ifdef USE_MK3MAG
126	attitude_resetHeadingToMagnetic();	126	attitude_resetHeadingToMagnetic();
127	#endif	127	#endif
128	// Servo_On(); //enable servo output	128	// Servo_On(); //enable servo output
129	}	129	}
130		130
131	/************************************************************************	131	/************************************************************************
132	* Get sensor data from the analog module, and release the ADC	132	* Get sensor data from the analog module, and release the ADC
133	* TODO: Ultimately, the analog module could do this (instead of dumping	133	* TODO: Ultimately, the analog module could do this (instead of dumping
134	* the values into variables).	134	* the values into variables).
135	* The rate variable end up in a range of about [-1024, 1023].	135	* The rate variable end up in a range of about [-1024, 1023].
136	*************************************************************************/	136	*************************************************************************/
137	void getAnalogData(void) {	137	void getAnalogData(void) {
138	uint8_t axis;	138	uint8_t axis;
139		139
140	analog_update();	140	analog_update();
141		141
142	for (axis = PITCH; axis <= ROLL; axis++) {	142	for (axis = PITCH; axis <= ROLL; axis++) {
143	rate_PID[axis] = gyro_PID[axis] + driftComp[axis];	143	rate_PID[axis] = gyro_PID[axis] + driftComp[axis];
144	rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];	144	rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis];
145	differential[axis] = gyroD[axis];	145	differential[axis] = gyroD[axis];
146	averageAcc[axis] += acc[axis];	146	averageAcc[axis] += acc[axis];
147	}	147	}
148		148
149	averageAccCount++;	149	averageAccCount++;
150	yawRate = yawGyro + driftCompYaw;	150	yawRate = yawGyro + driftCompYaw;
151	}	151	}
152		152
153	/*	153	/*
154	* This is the standard flight-style coordinate system transformation	154	* This is the standard flight-style coordinate system transformation
155	* (from airframe-local axes to a ground-based system). For some reason	155	* (from airframe-local axes to a ground-based system). For some reason
156	* the MK uses a left-hand coordinate system. The tranformation has been	156	* the MK uses a left-hand coordinate system. The tranformation has been
157	* changed accordingly.	157	* changed accordingly.
158	*/	158	*/
159	void trigAxisCoupling(void) {	159	void trigAxisCoupling(void) {
160	int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;	160	int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL;
161	int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;	161	int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL;
162		162
163	int16_t cospitch = cos_360(pitchAngleInDegrees);	163	int16_t cospitch = cos_360(pitchAngleInDegrees);
164	int16_t cosroll = cos_360(rollAngleInDegrees);	164	int16_t cosroll = cos_360(rollAngleInDegrees);
165	int16_t sinroll = sin_360(rollAngleInDegrees);	165	int16_t sinroll = sin_360(rollAngleInDegrees);
166		166
167	ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll	167	ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll
168	- (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR);	168	- (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR);
169		169
170	ACRate[ROLL] = rate_ATT[ROLL]	170	ACRate[ROLL] = rate_ATT[ROLL]
171	+ (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)	171	+ (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
172	>> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees))	172	>> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees))
173	>> LOG_MATH_UNIT_FACTOR);	173	>> LOG_MATH_UNIT_FACTOR);
174		174
175	ACYawRate =	175	ACYawRate =
176	((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)	176	((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll)
177	/ cospitch;	177	/ cospitch;
178	}	178	}
179		179
180	// 480 usec with axis coupling - almost no time without.	180	// 480 usec with axis coupling - almost no time without.
181	void integrate(void) {	181	void integrate(void) {
182	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.	182	// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
183	uint8_t axis;	183	uint8_t axis;
184		184
185	if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) {	185	if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) {
186	trigAxisCoupling();	186	trigAxisCoupling();
187	} else {	187	} else {
188	ACRate[PITCH] = rate_ATT[PITCH];	188	ACRate[PITCH] = rate_ATT[PITCH];
189	ACRate[ROLL] = rate_ATT[ROLL];	189	ACRate[ROLL] = rate_ATT[ROLL];
190	ACYawRate = yawRate;	190	ACYawRate = yawRate;
191	}	191	}
192		192
193	/*	193	/*
194	* Yaw	194	* Yaw
195	* Calculate yaw gyro integral (~ to rotation angle)	195	* Calculate yaw gyro integral (~ to rotation angle)
196	* Limit heading proportional to 0 deg to 360 deg	196	* Limit heading proportional to 0 deg to 360 deg
197	*/	197	*/
198	heading += ACYawRate;	198	heading += ACYawRate;
199	intervalWrap(&heading, YAWOVER360);	199	intervalWrap(&heading, YAWOVER360);
200	headingError += ACYawRate;	200	headingError += ACYawRate;
201		201
202	/*	202	/*
203	* Pitch axis integration and range boundary wrap.	203	* Pitch axis integration and range boundary wrap.
204	*/	204	*/
205	for (axis = PITCH; axis <= ROLL; axis++) {	205	for (axis = PITCH; axis <= ROLL; axis++) {
206	attitude[axis] += ACRate[axis];	206	attitude[axis] += ACRate[axis];
207	if (attitude[axis] > PITCHROLLOVER180) {	207	if (attitude[axis] > PITCHROLLOVER180) {
208	attitude[axis] -= PITCHROLLOVER360;	208	attitude[axis] -= PITCHROLLOVER360;
209	} else if (attitude[axis] <= -PITCHROLLOVER180) {	209	} else if (attitude[axis] <= -PITCHROLLOVER180) {
210	attitude[axis] += PITCHROLLOVER360;	210	attitude[axis] += PITCHROLLOVER360;
211	}	211	}
212	}	212	}
213	}	213	}
214		214
215	/************************************************************************	215	/************************************************************************
216	* A kind of 0'th order integral correction, that corrects the integrals	216	* A kind of 0'th order integral correction, that corrects the integrals
217	* directly. This is the "gyroAccFactor" stuff in the original code.	217	* directly. This is the "gyroAccFactor" stuff in the original code.
218	* There is (there) also a drift compensation	218	* There is (there) also a drift compensation
219	* - it corrects the differential of the integral = the gyro offsets.	219	* - it corrects the differential of the integral = the gyro offsets.
220	* That should only be necessary with drifty gyros like ENC-03.	220	* That should only be necessary with drifty gyros like ENC-03.
221	************************************************************************/	221	************************************************************************/
222	#define LOG_DIVIDER 12	222	#define LOG_DIVIDER 12
223	#define DIVIDER (1L << LOG_DIVIDER)	223	#define DIVIDER (1L << LOG_DIVIDER)
224	void correctIntegralsByAcc0thOrder(void) {	224	void correctIntegralsByAcc0thOrder(void) {
225	// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities	225	// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
226	// are less than ....., or reintroduce Kalman.	226	// are less than ....., or reintroduce Kalman.
227	// Well actually the Z axis acc. check is not so silly.	227	// Well actually the Z axis acc. check is not so silly.
228	uint8_t axis;	228	uint8_t axis;
229	int32_t temp;	229	int32_t temp;
230		230
231	debugOut.analog[13] = gyroActivity / 65536L;	-
232		-
233	uint16_t ca;	-
234	ca = gyroActivity >> 12;	-
-		231	debugOut.analog[12] = IMUConfig.zerothOrderCorrection;
235	debugOut.analog[14] = ca;	232
-		233	uint16_t ca = gyroActivity >> 14;
-		234	uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
236		235	debugOut.analog[15] = gyroActivityWeighted;
237	uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;	236
238	if (!gyroActivityWeighted) gyroActivityWeighted = 1;	237	if (!gyroActivityWeighted) gyroActivityWeighted = 1;
239		238
240	uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;	239	uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;
241		240
242	debugOut.analog[28] = IMUConfig.rateTolerance;	241	debugOut.analog[28] = IMUConfig.rateTolerance;
243	debugOut.analog[15] = gyroActivityWeighted;	-
244	debugOut.digital[0] &= ~DEBUG_ACC0THORDER;	242	debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
245	debugOut.digital[1] &= ~DEBUG_ACC0THORDER;	243	debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
246		244
247	if (gyroActivityWeighted < 8) {	245	if (gyroActivityWeighted < 8) {
248	debugOut.digital[0] \|= DEBUG_ACC0THORDER;	246	debugOut.digital[0] \|= DEBUG_ACC0THORDER;
249	}	247	}
250	if (gyroActivityWeighted <= 2) {	248	if (gyroActivityWeighted <= 2) {
251	debugOut.digital[1] \|= DEBUG_ACC0THORDER;	249	debugOut.digital[1] \|= DEBUG_ACC0THORDER;
252	}	250	}
253		251
254	/*	252	/*
255	* Add to each sum: The amount by which the angle is changed just below.	253	* Add to each sum: The amount by which the angle is changed just below.
256	*/	254	*/
257	for (axis = PITCH; axis <= ROLL; axis++) {	255	for (axis = PITCH; axis <= ROLL; axis++) {
258	int32_t accDerived = getAngleEstimateFromAcc(axis);	256	int32_t accDerived = getAngleEstimateFromAcc(axis);
259	//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);	257	//debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
260	// 1000 * the correction amount that will be added to the gyro angle in next line.	258	// 1000 * the correction amount that will be added to the gyro angle in next line.
261	temp = attitude[axis];	259	temp = attitude[axis];
262	attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER;	260	attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER;
263	correctionSum[axis] += attitude[axis] - temp;	261	correctionSum[axis] += attitude[axis] - temp;
264	}	262	}
265	}	263	}
266		264
267	/************************************************************************	265	/************************************************************************
268	* This is an attempt to correct not the error in the angle integrals	266	* This is an attempt to correct not the error in the angle integrals
269	* (that happens in correctIntegralsByAcc0thOrder above) but rather the	267	* (that happens in correctIntegralsByAcc0thOrder above) but rather the
270	* cause of it: Gyro drift, vibration and rounding errors.	268	* cause of it: Gyro drift, vibration and rounding errors.
271	* All the corrections made in correctIntegralsByAcc0thOrder over	269	* All the corrections made in correctIntegralsByAcc0thOrder over
272	* DRIFTCORRECTION_TIME cycles are summed up. This number is	270	* DRIFTCORRECTION_TIME cycles are summed up. This number is
273	* then divided by DRIFTCORRECTION_TIME to get the approx.	271	* then divided by DRIFTCORRECTION_TIME to get the approx.
274	* correction that should have been applied to each iteration to fix	272	* correction that should have been applied to each iteration to fix
275	* the error. This is then added to the dynamic offsets.	273	* the error. This is then added to the dynamic offsets.
276	************************************************************************/	274	************************************************************************/
277	// 2 times / sec. = 488/2	275	// 2 times / sec. = 488/2
278	#define DRIFTCORRECTION_TIME 256L	276	#define DRIFTCORRECTION_TIME 256L
279	void driftCorrection(void) {	277	void driftCorrection(void) {
280	static int16_t timer = DRIFTCORRECTION_TIME;	278	static int16_t timer = DRIFTCORRECTION_TIME;
281	int16_t deltaCorrection;	279	int16_t deltaCorrection;
282	int16_t round;	280	int16_t round;
283	uint8_t axis;	281	uint8_t axis;
284		282
285	if (!--timer) {	283	if (!--timer) {
286	timer = DRIFTCORRECTION_TIME;	284	timer = DRIFTCORRECTION_TIME;
287	for (axis = PITCH; axis <= ROLL; axis++) {	285	for (axis = PITCH; axis <= ROLL; axis++) {
288	// Take the sum of corrections applied, add it to delta	286	// Take the sum of corrections applied, add it to delta
289	if (correctionSum[axis] >= 0)	287	if (correctionSum[axis] >= 0)
290	round = DRIFTCORRECTION_TIME / 2;	288	round = DRIFTCORRECTION_TIME / 2;
291	else	289	else
292	round = -DRIFTCORRECTION_TIME / 2;	290	round = -DRIFTCORRECTION_TIME / 2;
293	deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;	291	deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
294	// Add the delta to the compensation. So positive delta means, gyro should have higher value.	292	// Add the delta to the compensation. So positive delta means, gyro should have higher value.
295	driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;	293	driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;
296	CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);	294	CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);
297	// DebugOut.Analog[11 + axis] = correctionSum[axis];	295	// DebugOut.Analog[11 + axis] = correctionSum[axis];
298	// DebugOut.Analog[16 + axis] = correctionSum[axis];	296	debugOut.analog[6 + axis] = correctionSum[axis];
299	// debugOut.analog[28 + axis] = driftComp[axis];	297	debugOut.analog[13 + axis] = driftComp[axis];
300	correctionSum[axis] = 0;	298	correctionSum[axis] = 0;
301	}	299	}
302	}	300	}
303	}	301	}
304		302
305	/*	303	/*
306	void calculateAccVector(void) {	304	void calculateAccVector(void) {
307	int16_t temp;	305	int16_t temp;
308	temp = filteredAcc[0] >> 3;	306	temp = filteredAcc[0] >> 3;
309	accVector = temp * temp;	307	accVector = temp * temp;
310	temp = filteredAcc[1] >> 3;	308	temp = filteredAcc[1] >> 3;
311	accVector += temp * temp;	309	accVector += temp * temp;
312	temp = filteredAcc[2] >> 3;	310	temp = filteredAcc[2] >> 3;
313	accVector += temp * temp;	311	accVector += temp * temp;
314	}	312	}
315	*/	313	*/
316		314
317	#ifdef USE_MK3MAG	315	#ifdef USE_MK3MAG
318	void attitude_resetHeadingToMagnetic(void) {	316	void attitude_resetHeadingToMagnetic(void) {
319	if (commands_isCalibratingCompass())	317	if (commands_isCalibratingCompass())
320	return;	318	return;
321		319
322	// Compass is off, skip.	320	// Compass is off, skip.
323	if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED))	321	if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED))
324	return;	322	return;
325		323
326	// Compass is invalid, skip.	324	// Compass is invalid, skip.
327	if (magneticHeading < 0)	325	if (magneticHeading < 0)
328	return;	326	return;
329		327
330	heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;	328	heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW;
331	//targetHeading = heading;	329	//targetHeading = heading;
332	headingError = 0;	330	headingError = 0;
333	}	331	}
334		332
335	void correctHeadingToMagnetic(void) {	333	void correctHeadingToMagnetic(void) {
336	int32_t error;	334	int32_t error;
337		335
338	if (commands_isCalibratingCompass()) {	336	if (commands_isCalibratingCompass()) {
339	//debugOut.analog[30] = -1;	337	//debugOut.analog[30] = -1;
340	return;	338	return;
341	}	339	}
342		340
343	// Compass is off, skip.	341	// Compass is off, skip.
344	// Naaah this is assumed.	342	// Naaah this is assumed.
345	// if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))	343	// if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
346	// return;	344	// return;
347		345
348	// Compass is invalid, skip.	346	// Compass is invalid, skip.
349	if (magneticHeading < 0) {	347	if (magneticHeading < 0) {
350	//debugOut.analog[30] = -2;	348	//debugOut.analog[30] = -2;
351	return;	349	return;
352	}	350	}
353		351
354	// Spinning fast, skip	352	// Spinning fast, skip
355	if (abs(yawRate) > 128) {	353	if (abs(yawRate) > 128) {
356	// debugOut.analog[30] = -3;	354	// debugOut.analog[30] = -3;
357	return;	355	return;
358	}	356	}
359		357
360	// Otherwise invalidated, skip	358	// Otherwise invalidated, skip
361	if (ignoreCompassTimer) {	359	if (ignoreCompassTimer) {
362	ignoreCompassTimer--;	360	ignoreCompassTimer--;
363	//debugOut.analog[30] = -4;	361	//debugOut.analog[30] = -4;
364	return;	362	return;
365	}	363	}
366		364
367	//debugOut.analog[30] = magneticHeading;	365	//debugOut.analog[30] = magneticHeading;
368		366
369	// TODO: Find computational cost of this.	367	// TODO: Find computational cost of this.
370	error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading);	368	error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading);
371	if (error <= -YAWOVER180) error += YAWOVER360;	369	if (error <= -YAWOVER180) error += YAWOVER360;
372	else if (error > YAWOVER180) error -= YAWOVER360;	370	else if (error > YAWOVER180) error -= YAWOVER360;
373		371
374	// We only correct errors larger than the resolution of the compass, or else we would keep rounding the	372	// We only correct errors larger than the resolution of the compass, or else we would keep rounding the
375	// better resolution of the gyros to the worse resolution of the compass all the time.	373	// better resolution of the gyros to the worse resolution of the compass all the time.
376	// The correction should really only serve to compensate for gyro drift.	374	// The correction should really only serve to compensate for gyro drift.
377	if(abs(error) < GYRO_DEG_FACTOR_YAW) return;	375	if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
378		376
379	int32_t correction = (error * staticParams.compassYawCorrection) >> 8;	377	int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
380	//debugOut.analog[30] = correction;	378	//debugOut.analog[30] = correction;
381		379
382	debugOut.digital[0] &= ~DEBUG_COMPASS;	380	debugOut.digital[0] &= ~DEBUG_COMPASS;
383	debugOut.digital[1] &= ~DEBUG_COMPASS;	381	debugOut.digital[1] &= ~DEBUG_COMPASS;
384		382
385	if (correction > 0) {	383	if (correction > 0) {
386	debugOut.digital[0] ^= DEBUG_COMPASS;	384	debugOut.digital[0] ^= DEBUG_COMPASS;
387	} else if (correction < 0) {	385	} else if (correction < 0) {
388	debugOut.digital[1] ^= DEBUG_COMPASS;	386	debugOut.digital[1] ^= DEBUG_COMPASS;
389	}	387	}
390		388
391	// The correction is added both to current heading (the direction in which the copter thinks it is pointing)	389	// The correction is added both to current heading (the direction in which the copter thinks it is pointing)
392	// and to the heading error (the angle of yaw that the copter is off the set heading).	390	// and to the heading error (the angle of yaw that the copter is off the set heading).
393	heading += correction;	391	heading += correction;
394	headingError += correction;	392	headingError += correction;
395	intervalWrap(&heading, YAWOVER360);	393	intervalWrap(&heading, YAWOVER360);
396		394
397	// If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all	395	// If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all
398	// when the compass corrects the heading - it only corrects numbers!) we want to add:	396	// when the compass corrects the heading - it only corrects numbers!) we want to add:
399	// This will however cause drift to remain uncorrected!	397	// This will however cause drift to remain uncorrected!
400	// headingError += correction;	398	// headingError += correction;
401	//debugOut.analog[29] = 0;	399	//debugOut.analog[29] = 0;
402	}	400	}
403	#endif	401	#endif
404		402
405	/************************************************************************	403	/************************************************************************
406	* Main procedure.	404	* Main procedure.
407	************************************************************************/	405	************************************************************************/
408	void calculateFlightAttitude(void) {	406	void calculateFlightAttitude(void) {
409	getAnalogData();	407	getAnalogData();
410	// calculateAccVector();	408	// calculateAccVector();
411	integrate();	409	integrate();
412		410
413	#ifdef ATTITUDE_USE_ACC_SENSORS	411	#ifdef ATTITUDE_USE_ACC_SENSORS
414	correctIntegralsByAcc0thOrder();	412	correctIntegralsByAcc0thOrder();
415	driftCorrection();	413	driftCorrection();
416	#endif	414	#endif
417		415
418	// We are done reading variables from the analog module.	416	// We are done reading variables from the analog module.
419	// Interrupt-driven sensor reading may restart.	417	// Interrupt-driven sensor reading may restart.
420	startAnalogConversionCycle();	418	startAnalogConversionCycle();
421		419
422	#ifdef USE_MK3MAG	420	#ifdef USE_MK3MAG
423	if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {	421	if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
424	correctHeadingToMagnetic();	422	correctHeadingToMagnetic();
425	}	423	}
426	#endif	424	#endif
427	}	425	}
428		426

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/attitude.c - Rev 2095 → 2160