WebSVN - FlightCtrl - Diff - Rev 2085 and 2086 - /branches/dongfang_FC


#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include "flight.h"
#include "output.h"
#include "uart0.h"
 
// Necessary for external control and motor test
#include "twimaster.h"
#include "attitude.h"
#include "controlMixer.h"
#include "commands.h"
#include "heightControl.h"
 
#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "compassControl.h"
#endif
 
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 
/*
 * These are no longer maintained, just left at 0. The original implementation just summed the acc.
 * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
 */
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
uint8_t ki;
int32_t IPart[2];
 
/************************************************************************/
/*  Filter for motor value smoothing (necessary???)                     */
/************************************************************************/
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
  switch (staticParams.motorSmoothing) {
  case 0:
    return newvalue;
  case 1:
    return (oldvalue + newvalue) / 2;
  case 2:
    if (newvalue > oldvalue)
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
    else
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
  case 3:
    if (newvalue < oldvalue)
      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
    else
      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
  default:
    return newvalue;
  }
}
 
void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
  ki = _ki;
  gyroPFactor = _gyroPFactor;
  gyroIFactor = _gyroIFactor;
  yawPFactor = _yawPFactor;
  yawIFactor = _yawIFactor;
}
 
void flight_setGround() {
  // Just reset all I terms.
  IPart[PITCH] = IPart[ROLL] = 0;
  headingError = 0;
}
 
void flight_takeOff() {
  // This is for GPS module to mark home position.
  // TODO: What a disgrace, change it.
  MKFlags |= MKFLAG_CALIBRATE;
 
  HC_setGround();
#ifdef USE_MK3MAG
  attitude_resetHeadingToMagnetic();
  compass_setTakeoffHeading(heading);
#endif
}
 
/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {
  int16_t tmp_int;
  // Mixer Fractions that are combined for Motor Control
  int16_t yawTerm, throttleTerm, term[2];
 
  // PID controller variables
  int16_t PDPart;
  static int8_t debugDataTimer = 1;
 
  // High resolution motor values for smoothing of PID motor outputs
  static int16_t motorFilters[MAX_MOTORS];
 
  uint8_t i, axis;
 
  throttleTerm = controls[CONTROL_THROTTLE];
 
  if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
    // increment flight-time counter until overflow.
    if (isFlying != 0xFFFF)
      isFlying++;
  }
  /*
   * When standing on the ground, do not apply I controls and zero the yaw stick.
   * Probably to avoid integration effects that will cause the copter to spin
   * or flip when taking off.
   */
  if (isFlying < 256) {
    flight_setGround();
    if (isFlying == 250)
      flight_takeOff();
  }
 
  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
  if (throttleTerm < staticParams.minThrottle + 10)
    throttleTerm = staticParams.minThrottle + 10;
  else if (throttleTerm > staticParams.maxThrottle - 20)
    throttleTerm = (staticParams.maxThrottle - 20);
 
  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
  throttleTerm *= CONTROL_SCALING;
 
// end part 1: 750-800 usec.
// start part 3: 350 - 400 usec.
#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
// This is where control affects the target heading. It also (later) directly controls yaw.
  headingError -= controls[CONTROL_YAW];
 
  if (headingError < -YAW_I_LIMIT)
    headingError = -YAW_I_LIMIT;
  else if (headingError > YAW_I_LIMIT)
    headingError = YAW_I_LIMIT;
 
  PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
  PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
 
  // Lets not limit P and D.
// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
 
  /*
   * Compose yaw term.
   * The yaw term is limited: Absolute value is max. = the throttle term / 2.
   * However, at low throttle the yaw term is limited to a fixed value,
   * and at high throttle it is limited by the throttle reserve (the difference
   * between current throttle and maximum throttle).
   */
#define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
  yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
// Limit yawTerm
  debugOut.digital[0] &= ~DEBUG_CLIP;
  if (throttleTerm > MIN_YAWGAS) {
    if (yawTerm < -throttleTerm / 2) {
      debugOut.digital[0] |= DEBUG_CLIP;
      yawTerm = -throttleTerm / 2;
    } else if (yawTerm > throttleTerm / 2) {
      debugOut.digital[0] |= DEBUG_CLIP;
      yawTerm = throttleTerm / 2;
    }
  } else {
    if (yawTerm < -MIN_YAWGAS / 2) {
      debugOut.digital[0] |= DEBUG_CLIP;
      yawTerm = -MIN_YAWGAS / 2;
    } else if (yawTerm > MIN_YAWGAS / 2) {
      debugOut.digital[0] |= DEBUG_CLIP;
      yawTerm = MIN_YAWGAS / 2;
    }
  }
 
  tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
 
  if (yawTerm < -(tmp_int - throttleTerm)) {
    yawTerm = -(tmp_int - throttleTerm);
    debugOut.digital[0] |= DEBUG_CLIP;
  } else if (yawTerm > (tmp_int - throttleTerm)) {
    yawTerm = (tmp_int - throttleTerm);
    debugOut.digital[0] |= DEBUG_CLIP;
  }
 
  debugOut.digital[1] &= ~DEBUG_CLIP;
 
  tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
  //tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
 
  /************************************************************************/
  /* Calculate control feedback from angle (gyro integral)                */
  /* and angular velocity (gyro signal)                                   */
  /************************************************************************/
  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
  for (axis = PITCH; axis <= ROLL; axis++) {
    PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
    // In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
    // In HH mode, the I part is summed from P and D of gyros minus stick.
    if (gyroIFactor) {
      int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
      if (axis == 0) debugOut.analog[28] = iDiff;
      PDPart += iDiff;
      IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
    } else {
      IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
    }
 
    // So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
    // attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
    // We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
    // That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
    // So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
 
    // With normal Ki, limit I parts to +/- 205 (of about 1024)
    if (IPart[axis] < -64000) {
      IPart[axis] = -64000;
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
    } else if (IPart[axis] > 64000) {
      IPart[axis] = 64000;
      debugOut.digital[1] |= DEBUG_FLIGHTCLIP;
    }
 
    PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
 
    term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);
 
    /*
     * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
     * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
     * (max. pitch or roll term is the throttle value).
     * OOPS: Is not applied at all.
     * TODO: Why a growing function of yaw?
     */
    if (term[axis] < -tmp_int) {
      debugOut.digital[1] |= DEBUG_CLIP;
      term[axis] = -tmp_int;
    } else if (term[axis] > tmp_int) {
      debugOut.digital[1] |= DEBUG_CLIP;
      term[axis] = tmp_int;
    }
  }
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Universal Mixer
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
  if (!(--debugDataTimer)) {
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
    debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
    debugOut.analog[1] = attitude[ROLL]  / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
    debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
 
    debugOut.analog[3] = rate_ATT[PITCH];
    debugOut.analog[4] = rate_ATT[ROLL];
    debugOut.analog[5] = yawRate;
  }
 
  debugOut.analog[8] = yawTerm;
  debugOut.analog[9] = throttleTerm;
 
  //debugOut.analog[16] = gyroActivity;
 
  for (i = 0; i < MAX_MOTORS; i++) {
    int32_t tmp;
    uint8_t throttle;
 
    tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
    tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
    tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
    tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];
    tmp = tmp >> 6;
    motorFilters[i] = motorFilter(tmp, motorFilters[i]);
    // Now we scale back down to a 0..255 range.
    tmp = motorFilters[i] / MOTOR_SCALING;
 
    // So this was the THIRD time a throttle was limited. But should the limitation
    // apply to the common throttle signal (the one used for setting the "power" of
    // all motors together) or should it limit the throttle set for each motor,
    // including mix components of pitch, roll and yaw? I think only the common
    // throttle should be limited.
    // --> WRONG. This caused motors to stall completely in tight maneuvers.
    // Apply to individual signals instead.
    CHECK_MIN_MAX(tmp, 1, 255);
    throttle = tmp;
 
    /*
    if (i < 4)
      debugOut.analog[10 + i] = throttle;
      */
 
    if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
      motor[i].throttle = throttle;
    } else if (motorTestActive) {
      motor[i].throttle = motorTest[i];
    } else {
      motor[i].throttle = 0;
    }
  }
 
  I2C_Start(TWI_STATE_MOTOR_TX);
}
 

Rev 2085	Rev 2086
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include <avr/io.h>	2	#include <avr/io.h>
3	#include "eeprom.h"	3	#include "eeprom.h"
4	#include "flight.h"	4	#include "flight.h"
5	#include "output.h"	5	#include "output.h"
6	#include "uart0.h"	6	#include "uart0.h"
7		7
8	// Necessary for external control and motor test	8	// Necessary for external control and motor test
9	#include "twimaster.h"	9	#include "twimaster.h"
10	#include "attitude.h"	10	#include "attitude.h"
11	#include "controlMixer.h"	11	#include "controlMixer.h"
12	#include "commands.h"	12	#include "commands.h"
13	#include "heightControl.h"	13	#include "heightControl.h"
14		14
15	#ifdef USE_MK3MAG	15	#ifdef USE_MK3MAG
16	#include "mk3mag.h"	16	#include "mk3mag.h"
17	#include "compassControl.h"	17	#include "compassControl.h"
18	#endif	18	#endif
19		19
20	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}	20	#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
21		21
22	/*	22	/*
23	* These are no longer maintained, just left at 0. The original implementation just summed the acc.	23	* These are no longer maintained, just left at 0. The original implementation just summed the acc.
24	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???	24	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
25	*/	25	*/
26	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;	26	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
27	uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control	27	uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
28	uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control	28	uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
29	uint8_t ki;	29	uint8_t ki;
30	int32_t IPart[2];	30	int32_t IPart[2];
31		31
32	/************************************************************************/	32	/************************************************************************/
33	/* Filter for motor value smoothing (necessary???) */	33	/* Filter for motor value smoothing (necessary???) */
34	/************************************************************************/	34	/************************************************************************/
35	int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {	35	int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
36	switch (staticParams.motorSmoothing) {	36	switch (staticParams.motorSmoothing) {
37	case 0:	37	case 0:
38	return newvalue;	38	return newvalue;
39	case 1:	39	case 1:
40	return (oldvalue + newvalue) / 2;	40	return (oldvalue + newvalue) / 2;
41	case 2:	41	case 2:
42	if (newvalue > oldvalue)	42	if (newvalue > oldvalue)
43	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	43	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
44	else	44	else
45	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	45	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
46	case 3:	46	case 3:
47	if (newvalue < oldvalue)	47	if (newvalue < oldvalue)
48	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	48	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
49	else	49	else
50	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	50	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
51	default:	51	default:
52	return newvalue;	52	return newvalue;
53	}	53	}
54	}	54	}
55		55
56	void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,	56	void flight_setParameters(uint8_t _ki, uint8_t _gyroPFactor,
57	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {	57	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
58	ki = _ki;	58	ki = _ki;
59	gyroPFactor = _gyroPFactor;	59	gyroPFactor = _gyroPFactor;
60	gyroIFactor = _gyroIFactor;	60	gyroIFactor = _gyroIFactor;
61	yawPFactor = _yawPFactor;	61	yawPFactor = _yawPFactor;
62	yawIFactor = _yawIFactor;	62	yawIFactor = _yawIFactor;
63	}	63	}
64		64
65	void flight_setGround() {	65	void flight_setGround() {
66	// Just reset all I terms.	66	// Just reset all I terms.
67	IPart[PITCH] = IPart[ROLL] = 0;	67	IPart[PITCH] = IPart[ROLL] = 0;
68	headingError = 0;	68	headingError = 0;
69	}	69	}
70		70
71	void flight_takeOff() {	71	void flight_takeOff() {
72	// This is for GPS module to mark home position.	72	// This is for GPS module to mark home position.
73	// TODO: What a disgrace, change it.	73	// TODO: What a disgrace, change it.
74	MKFlags \|= MKFLAG_CALIBRATE;	74	MKFlags \|= MKFLAG_CALIBRATE;
75		75
76	HC_setGround();	76	HC_setGround();
77	#ifdef USE_MK3MAG	77	#ifdef USE_MK3MAG
78	attitude_resetHeadingToMagnetic();	78	attitude_resetHeadingToMagnetic();
79	compass_setTakeoffHeading(heading);	79	compass_setTakeoffHeading(heading);
80	#endif	80	#endif
81	}	81	}
82		82
83	/************************************************************************/	83	/************************************************************************/
84	/* Main Flight Control */	84	/* Main Flight Control */
85	/************************************************************************/	85	/************************************************************************/
86	void flight_control(void) {	86	void flight_control(void) {
87	int16_t tmp_int;	87	int16_t tmp_int;
88	// Mixer Fractions that are combined for Motor Control	88	// Mixer Fractions that are combined for Motor Control
89	int16_t yawTerm, throttleTerm, term[2];	89	int16_t yawTerm, throttleTerm, term[2];
90		90
91	// PID controller variables	91	// PID controller variables
92	int16_t PDPart;	92	int16_t PDPart;
93	static int8_t debugDataTimer = 1;	93	static int8_t debugDataTimer = 1;
94		94
95	// High resolution motor values for smoothing of PID motor outputs	95	// High resolution motor values for smoothing of PID motor outputs
96	static int16_t motorFilters[MAX_MOTORS];	96	static int16_t motorFilters[MAX_MOTORS];
97		97
98	uint8_t i, axis;	98	uint8_t i, axis;
99		99
100	throttleTerm = controls[CONTROL_THROTTLE];	100	throttleTerm = controls[CONTROL_THROTTLE];
101		101
102	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {	102	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
103	// increment flight-time counter until overflow.	103	// increment flight-time counter until overflow.
104	if (isFlying != 0xFFFF)	104	if (isFlying != 0xFFFF)
105	isFlying++;	105	isFlying++;
106	}	106	}
107	/*	107	/*
108	* When standing on the ground, do not apply I controls and zero the yaw stick.	108	* When standing on the ground, do not apply I controls and zero the yaw stick.
109	* Probably to avoid integration effects that will cause the copter to spin	109	* Probably to avoid integration effects that will cause the copter to spin
110	* or flip when taking off.	110	* or flip when taking off.
111	*/	111	*/
112	if (isFlying < 256) {	112	if (isFlying < 256) {
113	flight_setGround();	113	flight_setGround();
114	if (isFlying == 250)	114	if (isFlying == 250)
115	flight_takeOff();	115	flight_takeOff();
116	}	116	}
117		117
118	// This check removed. Is done on a per-motor basis, after output matrix multiplication.	118	// This check removed. Is done on a per-motor basis, after output matrix multiplication.
119	if (throttleTerm < staticParams.minThrottle + 10)	119	if (throttleTerm < staticParams.minThrottle + 10)
120	throttleTerm = staticParams.minThrottle + 10;	120	throttleTerm = staticParams.minThrottle + 10;
121	else if (throttleTerm > staticParams.maxThrottle - 20)	121	else if (throttleTerm > staticParams.maxThrottle - 20)
122	throttleTerm = (staticParams.maxThrottle - 20);	122	throttleTerm = (staticParams.maxThrottle - 20);
123		123
124	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?	124	// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
125	throttleTerm *= CONTROL_SCALING;	125	throttleTerm *= CONTROL_SCALING;
126		126
127	// end part 1: 750-800 usec.	127	// end part 1: 750-800 usec.
128	// start part 3: 350 - 400 usec.	128	// start part 3: 350 - 400 usec.
129	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)	129	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
130	// This is where control affects the target heading. It also (later) directly controls yaw.	130	// This is where control affects the target heading. It also (later) directly controls yaw.
131	headingError -= controls[CONTROL_YAW];	131	headingError -= controls[CONTROL_YAW];
132		132
133	if (headingError < -YAW_I_LIMIT)	133	if (headingError < -YAW_I_LIMIT)
134	headingError = -YAW_I_LIMIT;	134	headingError = -YAW_I_LIMIT;
135	else if (headingError > YAW_I_LIMIT)	135	else if (headingError > YAW_I_LIMIT)
136	headingError = YAW_I_LIMIT;	136	headingError = YAW_I_LIMIT;
137		137
138	PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);	138	PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
139	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.	139	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
140	PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);	140	PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
141		141
142	// Lets not limit P and D.	142	// Lets not limit P and D.
143	// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);	143	// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
144		144
145	/*	145	/*
146	* Compose yaw term.	146	* Compose yaw term.
147	* The yaw term is limited: Absolute value is max. = the throttle term / 2.	147	* The yaw term is limited: Absolute value is max. = the throttle term / 2.
148	* However, at low throttle the yaw term is limited to a fixed value,	148	* However, at low throttle the yaw term is limited to a fixed value,
149	* and at high throttle it is limited by the throttle reserve (the difference	149	* and at high throttle it is limited by the throttle reserve (the difference
150	* between current throttle and maximum throttle).	150	* between current throttle and maximum throttle).
151	*/	151	*/
152	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value	152	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
153	yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;	153	yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
154	// Limit yawTerm	154	// Limit yawTerm
155	debugOut.digital[0] &= ~DEBUG_CLIP;	155	debugOut.digital[0] &= ~DEBUG_CLIP;
156	if (throttleTerm > MIN_YAWGAS) {	156	if (throttleTerm > MIN_YAWGAS) {
157	if (yawTerm < -throttleTerm / 2) {	157	if (yawTerm < -throttleTerm / 2) {
158	debugOut.digital[0] \|= DEBUG_CLIP;	158	debugOut.digital[0] \|= DEBUG_CLIP;
159	yawTerm = -throttleTerm / 2;	159	yawTerm = -throttleTerm / 2;
160	} else if (yawTerm > throttleTerm / 2) {	160	} else if (yawTerm > throttleTerm / 2) {
161	debugOut.digital[0] \|= DEBUG_CLIP;	161	debugOut.digital[0] \|= DEBUG_CLIP;
162	yawTerm = throttleTerm / 2;	162	yawTerm = throttleTerm / 2;
163	}	163	}
164	} else {	164	} else {
165	if (yawTerm < -MIN_YAWGAS / 2) {	165	if (yawTerm < -MIN_YAWGAS / 2) {
166	debugOut.digital[0] \|= DEBUG_CLIP;	166	debugOut.digital[0] \|= DEBUG_CLIP;
167	yawTerm = -MIN_YAWGAS / 2;	167	yawTerm = -MIN_YAWGAS / 2;
168	} else if (yawTerm > MIN_YAWGAS / 2) {	168	} else if (yawTerm > MIN_YAWGAS / 2) {
169	debugOut.digital[0] \|= DEBUG_CLIP;	169	debugOut.digital[0] \|= DEBUG_CLIP;
170	yawTerm = MIN_YAWGAS / 2;	170	yawTerm = MIN_YAWGAS / 2;
171	}	171	}
172	}	172	}
173		173
174	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;	174	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
175		175
176	if (yawTerm < -(tmp_int - throttleTerm)) {	176	if (yawTerm < -(tmp_int - throttleTerm)) {
177	yawTerm = -(tmp_int - throttleTerm);	177	yawTerm = -(tmp_int - throttleTerm);
178	debugOut.digital[0] \|= DEBUG_CLIP;	178	debugOut.digital[0] \|= DEBUG_CLIP;
179	} else if (yawTerm > (tmp_int - throttleTerm)) {	179	} else if (yawTerm > (tmp_int - throttleTerm)) {
180	yawTerm = (tmp_int - throttleTerm);	180	yawTerm = (tmp_int - throttleTerm);
181	debugOut.digital[0] \|= DEBUG_CLIP;	181	debugOut.digital[0] \|= DEBUG_CLIP;
182	}	182	}
183		183
184	debugOut.digital[1] &= ~DEBUG_CLIP;	184	debugOut.digital[1] &= ~DEBUG_CLIP;
185		185
186	tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);	186	tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
187	//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;	187	//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
188		188
189	/************************************************************************/	189	/************************************************************************/
190	/* Calculate control feedback from angle (gyro integral) */	190	/* Calculate control feedback from angle (gyro integral) */
191	/* and angular velocity (gyro signal) */	191	/* and angular velocity (gyro signal) */
192	/************************************************************************/	192	/************************************************************************/
193	// The P-part is the P of the PID controller. That's the angle integrals (not rates).	193	// The P-part is the P of the PID controller. That's the angle integrals (not rates).
194	for (axis = PITCH; axis <= ROLL; axis++) {	194	for (axis = PITCH; axis <= ROLL; axis++) {
195	PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);	195	PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
196	// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.	196	// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
197	// In HH mode, the I part is summed from P and D of gyros minus stick.	197	// In HH mode, the I part is summed from P and D of gyros minus stick.
198	if (gyroIFactor) {	198	if (gyroIFactor) {
199	int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);	199	int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
200	if (axis == 0) debugOut.analog[28] = iDiff;	200	if (axis == 0) debugOut.analog[28] = iDiff;
201	PDPart += iDiff;	201	PDPart += iDiff;
202	IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.	202	IPart[axis] += iDiff - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
203	} else {	203	} else {
204	IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.	204	IPart[axis] += PDPart - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
205	}	205	}
-		206
-		207	// So (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4) +
-		208	// attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
-		209	// We can ignore the rate: attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2) - controls[axis]
-		210	// That is: attitudeAngle[degrees] * gyroIFactor/4 - controls[axis]
-		211	// So attitude attained at attitudeAngle[degrees] * gyroIFactor/4 == controls[axis]
206		212
207	// With normal Ki, limit I parts to +/- 205 (of about 1024)	213	// With normal Ki, limit I parts to +/- 205 (of about 1024)
208	if (IPart[axis] < -64000) {	214	if (IPart[axis] < -64000) {
209	IPart[axis] = -64000;	215	IPart[axis] = -64000;
210	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	216	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
211	} else if (IPart[axis] > 64000) {	217	} else if (IPart[axis] > 64000) {
212	IPart[axis] = 64000;	218	IPart[axis] = 64000;
213	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	219	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
214	}	220	}
215		221
216	PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;	222	PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
217		223
218	term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);	224	term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
219	term[axis] += (dynamicParams.levelCorrection[axis] - 128);	225	term[axis] += (dynamicParams.levelCorrection[axis] - 128);
220		226
221	/*	227	/*
222	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).	228	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
223	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity	229	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
224	* (max. pitch or roll term is the throttle value).	230	* (max. pitch or roll term is the throttle value).
225	* OOPS: Is not applied at all.	231	* OOPS: Is not applied at all.
226	* TODO: Why a growing function of yaw?	232	* TODO: Why a growing function of yaw?
227	*/	233	*/
228	if (term[axis] < -tmp_int) {	234	if (term[axis] < -tmp_int) {
229	debugOut.digital[1] \|= DEBUG_CLIP;	235	debugOut.digital[1] \|= DEBUG_CLIP;
230	term[axis] = -tmp_int;	236	term[axis] = -tmp_int;
231	} else if (term[axis] > tmp_int) {	237	} else if (term[axis] > tmp_int) {
232	debugOut.digital[1] \|= DEBUG_CLIP;	238	debugOut.digital[1] \|= DEBUG_CLIP;
233	term[axis] = tmp_int;	239	term[axis] = tmp_int;
234	}	240	}
235	}	241	}
236		242
237	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	243	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
238	// Universal Mixer	244	// Universal Mixer
239	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].	245	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
240	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	246	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
241		247
242	if (!(--debugDataTimer)) {	248	if (!(--debugDataTimer)) {
243	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	249	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
244	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg	250	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
245	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg	251	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
246	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;	252	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
247		253
248	debugOut.analog[3] = rate_ATT[PITCH];	254	debugOut.analog[3] = rate_ATT[PITCH];
249	debugOut.analog[4] = rate_ATT[ROLL];	255	debugOut.analog[4] = rate_ATT[ROLL];
250	debugOut.analog[5] = yawRate;	256	debugOut.analog[5] = yawRate;
251	}	257	}
252		258
253	debugOut.analog[8] = yawTerm;	259	debugOut.analog[8] = yawTerm;
254	debugOut.analog[9] = throttleTerm;	260	debugOut.analog[9] = throttleTerm;
255		261
256	//debugOut.analog[16] = gyroActivity;	262	//debugOut.analog[16] = gyroActivity;
257		263
258	for (i = 0; i < MAX_MOTORS; i++) {	264	for (i = 0; i < MAX_MOTORS; i++) {
259	int32_t tmp;	265	int32_t tmp;
260	uint8_t throttle;	266	uint8_t throttle;
261		267
262	tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];	268	tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
263	tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];	269	tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
264	tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];	270	tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
265	tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];	271	tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];
266	tmp = tmp >> 6;	272	tmp = tmp >> 6;
267	motorFilters[i] = motorFilter(tmp, motorFilters[i]);	273	motorFilters[i] = motorFilter(tmp, motorFilters[i]);
268	// Now we scale back down to a 0..255 range.	274	// Now we scale back down to a 0..255 range.
269	tmp = motorFilters[i] / MOTOR_SCALING;	275	tmp = motorFilters[i] / MOTOR_SCALING;
270		276
271	// So this was the THIRD time a throttle was limited. But should the limitation	277	// So this was the THIRD time a throttle was limited. But should the limitation
272	// apply to the common throttle signal (the one used for setting the "power" of	278	// apply to the common throttle signal (the one used for setting the "power" of
273	// all motors together) or should it limit the throttle set for each motor,	279	// all motors together) or should it limit the throttle set for each motor,
274	// including mix components of pitch, roll and yaw? I think only the common	280	// including mix components of pitch, roll and yaw? I think only the common
275	// throttle should be limited.	281	// throttle should be limited.
276	// --> WRONG. This caused motors to stall completely in tight maneuvers.	282	// --> WRONG. This caused motors to stall completely in tight maneuvers.
277	// Apply to individual signals instead.	283	// Apply to individual signals instead.
278	CHECK_MIN_MAX(tmp, 1, 255);	284	CHECK_MIN_MAX(tmp, 1, 255);
279	throttle = tmp;	285	throttle = tmp;
280		286
281	/*	287	/*
282	if (i < 4)	288	if (i < 4)
283	debugOut.analog[10 + i] = throttle;	289	debugOut.analog[10 + i] = throttle;
284	*/	290	*/
285		291
286	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {	292	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
287	motor[i].throttle = throttle;	293	motor[i].throttle = throttle;
288	} else if (motorTestActive) {	294	} else if (motorTestActive) {
289	motor[i].throttle = motorTest[i];	295	motor[i].throttle = motorTest[i];
290	} else {	296	} else {
291	motor[i].throttle = 0;	297	motor[i].throttle = 0;
292	}	298	}
293	}	299	}
294		300
295	I2C_Start(TWI_STATE_MOTOR_TX);	301	I2C_Start(TWI_STATE_MOTOR_TX);
296	}	302	}
297		303

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c - Rev 2085 → 2086