WebSVN - FlightCtrl - Diff - Rev 2057 and 2058 - /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 invKi;
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 _invKi, uint8_t _gyroPFactor,
    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
  invKi = _invKi;
  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;
 
  debugOut.analog[29] = headingError / 100;
 
  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.
    }
 
    // 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] * invKi) >> 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 2057	Rev 2058
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 invKi;	29	uint8_t invKi;
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 _invKi, uint8_t _gyroPFactor,	56	void flight_setParameters(uint8_t _invKi, uint8_t _gyroPFactor,
57	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {	57	uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
58	invKi = _invKi;	58	invKi = _invKi;
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.
-		73	// TODO: What a disgrace, change it.
-		74	MKFlags \|= MKFLAG_CALIBRATE;
-		75
72	HC_setGround();	76	HC_setGround();
73	#ifdef USE_MK3MAG	77	#ifdef USE_MK3MAG
74	attitude_resetHeadingToMagnetic();	78	attitude_resetHeadingToMagnetic();
75	compass_setTakeoffHeading(heading);	79	compass_setTakeoffHeading(heading);
76	#endif	80	#endif
77	}	81	}
78		82
79	/************************************************************************/	83	/************************************************************************/
80	/* Main Flight Control */	84	/* Main Flight Control */
81	/************************************************************************/	85	/************************************************************************/
82	void flight_control(void) {	86	void flight_control(void) {
83	int16_t tmp_int;	87	int16_t tmp_int;
84	// Mixer Fractions that are combined for Motor Control	88	// Mixer Fractions that are combined for Motor Control
85	int16_t yawTerm, throttleTerm, term[2];	89	int16_t yawTerm, throttleTerm, term[2];
86		90
87	// PID controller variables	91	// PID controller variables
88	int16_t PDPart;	92	int16_t PDPart;
89	static int8_t debugDataTimer = 1;	93	static int8_t debugDataTimer = 1;
90		94
91	// High resolution motor values for smoothing of PID motor outputs	95	// High resolution motor values for smoothing of PID motor outputs
92	static int16_t motorFilters[MAX_MOTORS];	96	static int16_t motorFilters[MAX_MOTORS];
93		97
94	uint8_t i, axis;	98	uint8_t i, axis;
95		99
96	throttleTerm = controls[CONTROL_THROTTLE];	100	throttleTerm = controls[CONTROL_THROTTLE];
97		101
98	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {	102	if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
99	// increment flight-time counter until overflow.	103	// increment flight-time counter until overflow.
100	if (isFlying != 0xFFFF)	104	if (isFlying != 0xFFFF)
101	isFlying++;	105	isFlying++;
102	}	106	}
103	/*	107	/*
104	* 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.
105	* 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
106	* or flip when taking off.	110	* or flip when taking off.
107	*/	111	*/
108	if (isFlying < 256) {	112	if (isFlying < 256) {
109	flight_setGround();	113	flight_setGround();
110	if (isFlying == 250)	114	if (isFlying == 250)
111	flight_takeOff();	115	flight_takeOff();
112	}	116	}
113		117
114	// 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.
115	if (throttleTerm < staticParams.minThrottle + 10)	119	if (throttleTerm < staticParams.minThrottle + 10)
116	throttleTerm = staticParams.minThrottle + 10;	120	throttleTerm = staticParams.minThrottle + 10;
117	else if (throttleTerm > staticParams.maxThrottle - 20)	121	else if (throttleTerm > staticParams.maxThrottle - 20)
118	throttleTerm = (staticParams.maxThrottle - 20);	122	throttleTerm = (staticParams.maxThrottle - 20);
119		123
120	// 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?
121	throttleTerm *= CONTROL_SCALING;	125	throttleTerm *= CONTROL_SCALING;
122	// TODO: We dont need to repeat this for every iteration!	-
123		126
124	// end part 1: 750-800 usec.	127	// end part 1: 750-800 usec.
125	// start part 3: 350 - 400 usec.	128	// start part 3: 350 - 400 usec.
126	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)	129	#define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW)
127	// 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.
128	headingError -= controls[CONTROL_YAW];	131	headingError -= controls[CONTROL_YAW];
-		132
129	if (headingError < -YAW_I_LIMIT)	133	if (headingError < -YAW_I_LIMIT)
130	headingError = -YAW_I_LIMIT;	134	headingError = -YAW_I_LIMIT;
131	if (headingError > YAW_I_LIMIT)	135	else if (headingError > YAW_I_LIMIT)
132	headingError = YAW_I_LIMIT;	136	headingError = YAW_I_LIMIT;
-		137
-		138	debugOut.analog[29] = headingError / 100;
133		139
134	PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);	140	PDPart = (int32_t) (headingError * yawIFactor) / (GYRO_DEG_FACTOR_YAW << 4);
135	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.	141	// Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on.
136	PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);	142	PDPart += (int32_t) (yawRate * yawPFactor) / (GYRO_DEG_FACTOR_YAW >> 5);
137		143
138	// Lets not limit P and D.	144	// Lets not limit P and D.
139	// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);	145	// CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
140		146
141	/*	147	/*
142	* Compose yaw term.	148	* Compose yaw term.
143	* The yaw term is limited: Absolute value is max. = the throttle term / 2.	149	* The yaw term is limited: Absolute value is max. = the throttle term / 2.
144	* However, at low throttle the yaw term is limited to a fixed value,	150	* However, at low throttle the yaw term is limited to a fixed value,
145	* and at high throttle it is limited by the throttle reserve (the difference	151	* and at high throttle it is limited by the throttle reserve (the difference
146	* between current throttle and maximum throttle).	152	* between current throttle and maximum throttle).
147	*/	153	*/
148	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value	154	#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value
149	yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;	155	yawTerm = PDPart - controls[CONTROL_YAW] * CONTROL_SCALING;
150	// Limit yawTerm	156	// Limit yawTerm
151	debugOut.digital[0] &= ~DEBUG_CLIP;	157	debugOut.digital[0] &= ~DEBUG_CLIP;
152	if (throttleTerm > MIN_YAWGAS) {	158	if (throttleTerm > MIN_YAWGAS) {
153	if (yawTerm < -throttleTerm / 2) {	159	if (yawTerm < -throttleTerm / 2) {
154	debugOut.digital[0] \|= DEBUG_CLIP;	160	debugOut.digital[0] \|= DEBUG_CLIP;
155	yawTerm = -throttleTerm / 2;	161	yawTerm = -throttleTerm / 2;
156	} else if (yawTerm > throttleTerm / 2) {	162	} else if (yawTerm > throttleTerm / 2) {
157	debugOut.digital[0] \|= DEBUG_CLIP;	163	debugOut.digital[0] \|= DEBUG_CLIP;
158	yawTerm = throttleTerm / 2;	164	yawTerm = throttleTerm / 2;
159	}	165	}
160	} else {	166	} else {
161	if (yawTerm < -MIN_YAWGAS / 2) {	167	if (yawTerm < -MIN_YAWGAS / 2) {
162	debugOut.digital[0] \|= DEBUG_CLIP;	168	debugOut.digital[0] \|= DEBUG_CLIP;
163	yawTerm = -MIN_YAWGAS / 2;	169	yawTerm = -MIN_YAWGAS / 2;
164	} else if (yawTerm > MIN_YAWGAS / 2) {	170	} else if (yawTerm > MIN_YAWGAS / 2) {
165	debugOut.digital[0] \|= DEBUG_CLIP;	171	debugOut.digital[0] \|= DEBUG_CLIP;
166	yawTerm = MIN_YAWGAS / 2;	172	yawTerm = MIN_YAWGAS / 2;
167	}	173	}
168	}	174	}
169		175
170	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;	176	tmp_int = staticParams.maxThrottle * CONTROL_SCALING;
171		177
172	if (yawTerm < -(tmp_int - throttleTerm)) {	178	if (yawTerm < -(tmp_int - throttleTerm)) {
173	yawTerm = -(tmp_int - throttleTerm);	179	yawTerm = -(tmp_int - throttleTerm);
174	debugOut.digital[0] \|= DEBUG_CLIP;	180	debugOut.digital[0] \|= DEBUG_CLIP;
175	} else if (yawTerm > (tmp_int - throttleTerm)) {	181	} else if (yawTerm > (tmp_int - throttleTerm)) {
176	yawTerm = (tmp_int - throttleTerm);	182	yawTerm = (tmp_int - throttleTerm);
177	debugOut.digital[0] \|= DEBUG_CLIP;	183	debugOut.digital[0] \|= DEBUG_CLIP;
178	}	184	}
179		185
180	debugOut.digital[1] &= ~DEBUG_CLIP;	186	debugOut.digital[1] &= ~DEBUG_CLIP;
181		187
182	tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);	188	tmp_int = ((uint16_t)dynamicParams.dynamicStability * ((uint16_t)throttleTerm + (abs(yawTerm) >> 1)) >> 6);
183	//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;	189	//tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
184		190
185	/************************************************************************/	191	/************************************************************************/
186	/* Calculate control feedback from angle (gyro integral) */	192	/* Calculate control feedback from angle (gyro integral) */
187	/* and angular velocity (gyro signal) */	193	/* and angular velocity (gyro signal) */
188	/************************************************************************/	194	/************************************************************************/
189	// The P-part is the P of the PID controller. That's the angle integrals (not rates).	195	// The P-part is the P of the PID controller. That's the angle integrals (not rates).
190	for (axis = PITCH; axis <= ROLL; axis++) {	196	for (axis = PITCH; axis <= ROLL; axis++) {
191	PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);	197	PDPart = (int32_t) rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 4);
192	PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;	-
193	// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.	198	// In acc. mode the I part is summed only from the attitude (IFaktor) angle minus stick.
194	// In HH mode, the I part is summed from P and D of gyros minus stick.	199	// In HH mode, the I part is summed from P and D of gyros minus stick.
195	if (gyroIFactor) {	200	if (gyroIFactor) {
196	int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 3);	201	int16_t iDiff = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2);
-		202	if (axis == 0) debugOut.analog[28] = iDiff;
197	PDPart += iDiff;	203	PDPart += iDiff;
198	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.	204	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.
199	} else {	205	} else {
200	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.	206	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.
201	}	207	}
202		208
203	// With normal Ki, limit I parts to +/- 205 (of about 1024)	209	// With normal Ki, limit I parts to +/- 205 (of about 1024)
204	if (IPart[axis] < -64000) {	210	if (IPart[axis] < -64000) {
205	IPart[axis] = -64000;	211	IPart[axis] = -64000;
206	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	212	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
207	} else if (IPart[axis] > 64000) {	213	} else if (IPart[axis] > 64000) {
208	IPart[axis] = 64000;	214	IPart[axis] = 64000;
209	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;	215	debugOut.digital[1] \|= DEBUG_FLIGHTCLIP;
210	}	216	}
-		217
-		218	PDPart += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
211		219
212	term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);	220	term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * invKi) >> 14);
213	term[axis] += (dynamicParams.levelCorrection[axis] - 128);	221	term[axis] += (dynamicParams.levelCorrection[axis] - 128);
214		222
215	/*	223	/*
216	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).	224	* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
217	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity	225	* The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
218	* (max. pitch or roll term is the throttle value).	226	* (max. pitch or roll term is the throttle value).
219	* OOPS: Is not applied at all.	227	* OOPS: Is not applied at all.
220	* TODO: Why a growing function of yaw?	228	* TODO: Why a growing function of yaw?
221	*/	229	*/
222	if (term[axis] < -tmp_int) {	230	if (term[axis] < -tmp_int) {
223	debugOut.digital[1] \|= DEBUG_CLIP;	231	debugOut.digital[1] \|= DEBUG_CLIP;
224	term[axis] = -tmp_int;	232	term[axis] = -tmp_int;
225	} else if (term[axis] > tmp_int) {	233	} else if (term[axis] > tmp_int) {
226	debugOut.digital[1] \|= DEBUG_CLIP;	234	debugOut.digital[1] \|= DEBUG_CLIP;
227	term[axis] = tmp_int;	235	term[axis] = tmp_int;
228	}	236	}
229	}	237	}
230		238
231	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	239	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
232	// Universal Mixer	240	// Universal Mixer
233	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].	241	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
234	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	242	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
235		243
236	if (!(--debugDataTimer)) {	244	if (!(--debugDataTimer)) {
237	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	245	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
238	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg	246	debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
239	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg	247	debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL / 10); // in 0.1 deg
240	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;	248	debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW;
241		249
242	debugOut.analog[3] = rate_ATT[PITCH];	250	debugOut.analog[3] = rate_ATT[PITCH];
243	debugOut.analog[4] = rate_ATT[ROLL];	251	debugOut.analog[4] = rate_ATT[ROLL];
244	debugOut.analog[5] = yawRate;	252	debugOut.analog[5] = yawRate;
245	}	253	}
246		254
247	debugOut.analog[8] = yawTerm;	255	debugOut.analog[8] = yawTerm;
248	debugOut.analog[9] = throttleTerm;	256	debugOut.analog[9] = throttleTerm;
249		257
250	debugOut.analog[16] = gyroActivity;	258	//debugOut.analog[16] = gyroActivity;
251		259
252	for (i = 0; i < MAX_MOTORS; i++) {	260	for (i = 0; i < MAX_MOTORS; i++) {
253	int32_t tmp;	261	int32_t tmp;
254	uint8_t throttle;	262	uint8_t throttle;
255		263
256	tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];	264	tmp = (int32_t) throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE];
257	tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];	265	tmp += (int32_t) term[PITCH] * mixerMatrix.motor[i][MIX_PITCH];
258	tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];	266	tmp += (int32_t) term[ROLL] * mixerMatrix.motor[i][MIX_ROLL];
259	tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];	267	tmp += (int32_t) yawTerm * mixerMatrix.motor[i][MIX_YAW];
260	tmp = tmp >> 6;	268	tmp = tmp >> 6;
261	motorFilters[i] = motorFilter(tmp, motorFilters[i]);	269	motorFilters[i] = motorFilter(tmp, motorFilters[i]);
262	// Now we scale back down to a 0..255 range.	270	// Now we scale back down to a 0..255 range.
263	tmp = motorFilters[i] / MOTOR_SCALING;	271	tmp = motorFilters[i] / MOTOR_SCALING;
264		272
265	// So this was the THIRD time a throttle was limited. But should the limitation	273	// So this was the THIRD time a throttle was limited. But should the limitation
266	// apply to the common throttle signal (the one used for setting the "power" of	274	// apply to the common throttle signal (the one used for setting the "power" of
267	// all motors together) or should it limit the throttle set for each motor,	275	// all motors together) or should it limit the throttle set for each motor,
268	// including mix components of pitch, roll and yaw? I think only the common	276	// including mix components of pitch, roll and yaw? I think only the common
269	// throttle should be limited.	277	// throttle should be limited.
270	// --> WRONG. This caused motors to stall completely in tight maneuvers.	278	// --> WRONG. This caused motors to stall completely in tight maneuvers.
271	// Apply to individual signals instead.	279	// Apply to individual signals instead.
272	CHECK_MIN_MAX(tmp, 1, 255);	280	CHECK_MIN_MAX(tmp, 1, 255);
273	throttle = tmp;	281	throttle = tmp;
274		282
275	if (i < 4)	283	if (i < 4)
276	debugOut.analog[10 + i] = throttle;	284	debugOut.analog[10 + i] = throttle;
277		285
278	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {	286	if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) {
279	motor[i].throttle = throttle;	287	motor[i].throttle = throttle;
280	} else if (motorTestActive) {	288	} else if (motorTestActive) {
281	motor[i].throttle = motorTest[i];	289	motor[i].throttle = motorTest[i];
282	} else {	290	} else {
283	motor[i].throttle = 0;	291	motor[i].throttle = 0;
284	}	292	}
285	}	293	}
286		294
287	I2C_Start(TWI_STATE_MOTOR_TX);	295	I2C_Start(TWI_STATE_MOTOR_TX);
288	}	296	}
289		297

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 2067 - Rev 2057 → 2058