WebSVN - FlightCtrl - Diff - Rev 1927 and 2025 - /branches/dongfang_FC


#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include "flight.h"
#include "output.h"
 
// Necessary for external control and motor test
#include "uart0.h"
 
// for scope debugging
// #include "rc.h"
 
#include "timer2.h"
#include "attitude.h"
#include "controlMixer.h"
#include "commands.h"
#ifdef USE_MK3MAG
#include "gps.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;
 
int8_t pitchPFactor, rollPFactor, yawPFactor;
int8_t pitchDFactor, rollDFactor, yawDFactor;
 
int32_t IPart[2] = {0,0};
 
/************************************************************************/
/*  Filter for motor value smoothing (necessary???)                     */
/************************************************************************/
int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {
  switch (dynamicParams.UserParams[5]) {
  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;
  }
}
 
/************************************************************************/
/*  Neutral Readings                                                    */
/************************************************************************/
#define CONTROL_CONFIG_SCALE 10
 
void flight_setNeutral() {
 
  // not really used here any more.
  controlMixer_initVariables();
}
 
void setFlightParameters
(
 uint8_t _pitchPFactor,
 uint8_t _rollPFactor,
 uint8_t _yawPFactor,
 
 uint8_t _pitchDFactor,
 uint8_t _rollDFactor,
 uint8_t _yawDFactor
 ) {
  pitchPFactor = _pitchPFactor;
  rollPFactor = _rollPFactor;
  yawPFactor = _yawPFactor;
 
  pitchDFactor = _pitchDFactor;
  rollDFactor = _rollDFactor;
  yawDFactor = _yawDFactor;
}
 
void setNormalFlightParameters(void) {
  setFlightParameters
    (
     dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE,     // 12 seems good
     dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE,      // 9 seems good
     dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2),   // 24 seems too little
 
     dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,
     dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,
     dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE
     );
}
 
void setStableFlightParameters(void) {
  setFlightParameters(0, 0, 0, 0, 0, 0);
}
 
/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {
  // Mixer Fractions that are combined for Motor Control
  int16_t yawTerm, throttleTerm, term[2];
   
  // PID controller variables
  int16_t PDPart[2], PDPartYaw;
 
  static int8_t debugDataTimer = 1;
 
  // High resolution motor values for smoothing of PID motor outputs
  static int16_t outputFilters[MAX_OUTPUTS];
 
  uint8_t i;
 
  // Fire the main flight attitude calculation, including integration of angles.
  // We want that to kick as early as possible, not to delay new AD sampling further.
  calculateFlightAttitude();
  controlMixer_update();
  throttleTerm = control[CONTROL_THROTTLE];
 
  /************************************************************************/
  /* RC-signal is bad                                                     */
  /************************************************************************/
 
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy
    RED_ON;
    beepRCAlarm();
    setStableFlightParameters();
  } else {
    commands_handleCommands();
    setNormalFlightParameters();
  }
   
  /************************************************************************/
  /* Calculate control feedback from angle (gyro integral)                */
  /* and angular velocity (gyro signal)                                   */
  /************************************************************************/
  PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /
                  (256L / CONTROL_SCALING))
  + (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;
 
  PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
                  (256L / CONTROL_SCALING))
  + (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
 
  PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
  + (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
 
  /************************************************************************/
  /* Stick signals are positive and gyros are negative...                 */
  /************************************************************************/
  IPart[PITCH] = error[PITCH]; // * some factor configurable.
  IPart[ROLL] = error[ROLL];
    // TODO: Add ipart. Or add/subtract depending, not sure.
  term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]);
  term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]);
  yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw);
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Universal Mixer
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
  DebugOut.Analog[12] = term[PITCH];
  DebugOut.Analog[13] = term[ROLL];
  DebugOut.Analog[14] = throttleTerm;
  DebugOut.Analog[15] = yawTerm;
 
  for (i = 0; i < MAX_OUTPUTS; i++) {
    int16_t tmp;
      if (outputTestActive) {
          outputs[i].SetPoint = outputTest[i] * 4;
      } else {
        // Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
        switch(i) {
        case 0: tmp = term[ROLL]; break;
        case 1: tmp = term[PITCH]; break;
        case 2: tmp = throttleTerm - 310; break;
        case 3: tmp = yawTerm; break;
        default: tmp = 0;
        }
      outputFilters[i] = outputFilter(tmp, outputFilters[i]);
      // Now we scale back down to a 0..255 range.
      tmp = outputFilters[i];
      outputs[i].SetPoint = tmp;
    }
  }
 
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Debugging
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  if (!(--debugDataTimer)) {
    debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
    DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
    DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
      DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW;
 
    DebugOut.Analog[6] = pitchPFactor;
    DebugOut.Analog[7] = rollPFactor;
    DebugOut.Analog[8] = yawPFactor;
    DebugOut.Analog[9] = pitchDFactor;
    DebugOut.Analog[10] = rollDFactor;
    DebugOut.Analog[11] = yawDFactor;
 
    DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
    DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
    DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
    DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
  }
}
 

Rev 1927	Rev 2025
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		6
7	// Necessary for external control and motor test	7	// Necessary for external control and motor test
8	#include "uart0.h"	8	#include "uart0.h"
9		9
10	// for scope debugging	10	// for scope debugging
11	// #include "rc.h"	11	// #include "rc.h"
12		12
13	#include "timer2.h"	13	#include "timer2.h"
14	#include "attitude.h"	14	#include "attitude.h"
15	#include "controlMixer.h"	15	#include "controlMixer.h"
16	#include "commands.h"	16	#include "commands.h"
17	#ifdef USE_MK3MAG	17	#ifdef USE_MK3MAG
18	#include "gps.h"	18	#include "gps.h"
19	#endif	19	#endif
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	* These are no longer maintained, just left at 0. The original implementation just summed the acc.	24	* These are no longer maintained, just left at 0. The original implementation just summed the acc.
25	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???	25	* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
26	*/	26	*/
27	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;	27	// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
28		28
29	int8_t pitchPFactor, rollPFactor, yawPFactor;	29	int8_t pitchPFactor, rollPFactor, yawPFactor;
30	int8_t pitchDFactor, rollDFactor, yawDFactor;	30	int8_t pitchDFactor, rollDFactor, yawDFactor;
31		31
32	int32_t IPart[2] = {0,0};	32	int32_t IPart[2] = {0,0};
33		33
34	/************************************************************************/	34	/************************************************************************/
35	/* Filter for motor value smoothing (necessary???) */	35	/* Filter for motor value smoothing (necessary???) */
36	/************************************************************************/	36	/************************************************************************/
37	int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {	37	int16_t outputFilter(int16_t newvalue, int16_t oldvalue) {
38	switch (dynamicParams.UserParams[5]) {	38	switch (dynamicParams.UserParams[5]) {
39	case 0:	39	case 0:
40	return newvalue;	40	return newvalue;
41	case 1:	41	case 1:
42	return (oldvalue + newvalue) / 2;	42	return (oldvalue + newvalue) / 2;
43	case 2:	43	case 2:
44	if (newvalue > oldvalue)	44	if (newvalue > oldvalue)
45	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	45	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
46	else	46	else
47	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	47	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
48	case 3:	48	case 3:
49	if (newvalue < oldvalue)	49	if (newvalue < oldvalue)
50	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new	50	return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
51	else	51	else
52	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old	52	return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
53	default:	53	default:
54	return newvalue;	54	return newvalue;
55	}	55	}
56	}	56	}
57		57
58	/************************************************************************/	58	/************************************************************************/
59	/* Neutral Readings */	59	/* Neutral Readings */
60	/************************************************************************/	60	/************************************************************************/
61	#define CONTROL_CONFIG_SCALE 10	61	#define CONTROL_CONFIG_SCALE 10
62		62
63	void flight_setNeutral() {	63	void flight_setNeutral() {
64	MKFlags \|= MKFLAG_CALIBRATE;	-
65	// not really used here any more.	64	// not really used here any more.
66	controlMixer_initVariables();	65	controlMixer_initVariables();
67	}	66	}
68		67
69	void setFlightParameters	68	void setFlightParameters
70	(	69	(
71	uint8_t _pitchPFactor,	70	uint8_t _pitchPFactor,
72	uint8_t _rollPFactor,	71	uint8_t _rollPFactor,
73	uint8_t _yawPFactor,	72	uint8_t _yawPFactor,
74		73
75	uint8_t _pitchDFactor,	74	uint8_t _pitchDFactor,
76	uint8_t _rollDFactor,	75	uint8_t _rollDFactor,
77	uint8_t _yawDFactor	76	uint8_t _yawDFactor
78	) {	77	) {
79	pitchPFactor = _pitchPFactor;	78	pitchPFactor = _pitchPFactor;
80	rollPFactor = _rollPFactor;	79	rollPFactor = _rollPFactor;
81	yawPFactor = _yawPFactor;	80	yawPFactor = _yawPFactor;
82		81
83	pitchDFactor = _pitchDFactor;	82	pitchDFactor = _pitchDFactor;
84	rollDFactor = _rollDFactor;	83	rollDFactor = _rollDFactor;
85	yawDFactor = _yawDFactor;	84	yawDFactor = _yawDFactor;
86	}	85	}
87		86
88	void setNormalFlightParameters(void) {	87	void setNormalFlightParameters(void) {
89	setFlightParameters	88	setFlightParameters
90	(	89	(
91	dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good	90	dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good
92	dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good	91	dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good
93	dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little	92	dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little
94		93
95	dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,	94	dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE,
96	dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,	95	dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE,
97	dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE	96	dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE
98	);	97	);
99	}	98	}
100		99
101	void setStableFlightParameters(void) {	100	void setStableFlightParameters(void) {
102	setFlightParameters(0, 0, 0, 0, 0, 0);	101	setFlightParameters(0, 0, 0, 0, 0, 0);
103	}	102	}
104		103
105	/************************************************************************/	104	/************************************************************************/
106	/* Main Flight Control */	105	/* Main Flight Control */
107	/************************************************************************/	106	/************************************************************************/
108	void flight_control(void) {	107	void flight_control(void) {
109	// Mixer Fractions that are combined for Motor Control	108	// Mixer Fractions that are combined for Motor Control
110	int16_t yawTerm, throttleTerm, term[2];	109	int16_t yawTerm, throttleTerm, term[2];
111		110
112	// PID controller variables	111	// PID controller variables
113	int16_t PDPart[2], PDPartYaw;	112	int16_t PDPart[2], PDPartYaw;
114		113
115	static int8_t debugDataTimer = 1;	114	static int8_t debugDataTimer = 1;
116		115
117	// High resolution motor values for smoothing of PID motor outputs	116	// High resolution motor values for smoothing of PID motor outputs
118	static int16_t outputFilters[MAX_OUTPUTS];	117	static int16_t outputFilters[MAX_OUTPUTS];
119		118
120	uint8_t i;	119	uint8_t i;
121		120
122	// Fire the main flight attitude calculation, including integration of angles.	121	// Fire the main flight attitude calculation, including integration of angles.
123	// We want that to kick as early as possible, not to delay new AD sampling further.	122	// We want that to kick as early as possible, not to delay new AD sampling further.
124	calculateFlightAttitude();	123	calculateFlightAttitude();
125	controlMixer_update();	124	controlMixer_update();
126	throttleTerm = control[CONTROL_THROTTLE];	125	throttleTerm = control[CONTROL_THROTTLE];
127		126
128	/************************************************************************/	127	/************************************************************************/
129	/* RC-signal is bad */	128	/* RC-signal is bad */
130	/************************************************************************/	129	/************************************************************************/
131		130
132	if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy	131	if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy
133	RED_ON;	132	RED_ON;
134	beepRCAlarm();	133	beepRCAlarm();
135	setStableFlightParameters();	134	setStableFlightParameters();
136	} else {	135	} else {
137	commands_handleCommands();	136	commands_handleCommands();
138	setNormalFlightParameters();	137	setNormalFlightParameters();
139	}	138	}
140		139
141	/************************************************************************/	140	/************************************************************************/
142	/* Calculate control feedback from angle (gyro integral) */	141	/* Calculate control feedback from angle (gyro integral) */
143	/* and angular velocity (gyro signal) */	142	/* and angular velocity (gyro signal) */
144	/************************************************************************/	143	/************************************************************************/
145	PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /	144	PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor /
146	(256L / CONTROL_SCALING))	145	(256L / CONTROL_SCALING))
147	+ (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;	146	+ (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16;
148		147
149	PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /	148	PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
150	(256L / CONTROL_SCALING))	149	(256L / CONTROL_SCALING))
151	+ (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;	150	+ (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
152		151
153	PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)	152	PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
154	+ (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;	153	+ (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
155		154
156	/************************************************************************/	155	/************************************************************************/
157	/* Stick signals are positive and gyros are negative... */	156	/* Stick signals are positive and gyros are negative... */
158	/************************************************************************/	157	/************************************************************************/
159	IPart[PITCH] = error[PITCH]; // * some factor configurable.	158	IPart[PITCH] = error[PITCH]; // * some factor configurable.
160	IPart[ROLL] = error[ROLL];	159	IPart[ROLL] = error[ROLL];
161	// TODO: Add ipart. Or add/subtract depending, not sure.	160	// TODO: Add ipart. Or add/subtract depending, not sure.
162	term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]);	161	term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]);
163	term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]);	162	term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]);
164	yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw);	163	yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw);
165		164
166	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	165	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
167	// Universal Mixer	166	// Universal Mixer
168	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].	167	// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
169	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	168	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
170		169
171	DebugOut.Analog[12] = term[PITCH];	170	DebugOut.Analog[12] = term[PITCH];
172	DebugOut.Analog[13] = term[ROLL];	171	DebugOut.Analog[13] = term[ROLL];
173	DebugOut.Analog[14] = throttleTerm;	172	DebugOut.Analog[14] = throttleTerm;
174	DebugOut.Analog[15] = yawTerm;	173	DebugOut.Analog[15] = yawTerm;
175		174
176	for (i = 0; i < MAX_OUTPUTS; i++) {	175	for (i = 0; i < MAX_OUTPUTS; i++) {
177	int16_t tmp;	176	int16_t tmp;
178	if (outputTestActive) {	177	if (outputTestActive) {
179	outputs[i].SetPoint = outputTest[i] * 4;	178	outputs[i].SetPoint = outputTest[i] * 4;
180	} else {	179	} else {
181	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.	180	// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
182	switch(i) {	181	switch(i) {
183	case 0: tmp = term[ROLL]; break;	182	case 0: tmp = term[ROLL]; break;
184	case 1: tmp = term[PITCH]; break;	183	case 1: tmp = term[PITCH]; break;
185	case 2: tmp = throttleTerm - 310; break;	184	case 2: tmp = throttleTerm - 310; break;
186	case 3: tmp = yawTerm; break;	185	case 3: tmp = yawTerm; break;
187	default: tmp = 0;	186	default: tmp = 0;
188	}	187	}
189	outputFilters[i] = outputFilter(tmp, outputFilters[i]);	188	outputFilters[i] = outputFilter(tmp, outputFilters[i]);
190	// Now we scale back down to a 0..255 range.	189	// Now we scale back down to a 0..255 range.
191	tmp = outputFilters[i];	190	tmp = outputFilters[i];
192	outputs[i].SetPoint = tmp;	191	outputs[i].SetPoint = tmp;
193	}	192	}
194	}	193	}
195		194
196	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	195	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
197	// Debugging	196	// Debugging
198	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	197	// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
199	if (!(--debugDataTimer)) {	198	if (!(--debugDataTimer)) {
200	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.	199	debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
201	DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	200	DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
202	DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	201	DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
203	DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW;	202	DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW;
204		203
205	DebugOut.Analog[6] = pitchPFactor;	204	DebugOut.Analog[6] = pitchPFactor;
206	DebugOut.Analog[7] = rollPFactor;	205	DebugOut.Analog[7] = rollPFactor;
207	DebugOut.Analog[8] = yawPFactor;	206	DebugOut.Analog[8] = yawPFactor;
208	DebugOut.Analog[9] = pitchDFactor;	207	DebugOut.Analog[9] = pitchDFactor;
209	DebugOut.Analog[10] = rollDFactor;	208	DebugOut.Analog[10] = rollDFactor;
210	DebugOut.Analog[11] = yawDFactor;	209	DebugOut.Analog[11] = yawDFactor;
211		210
212	DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	211	DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
213	DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	212	DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
214	DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	213	DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
215	DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg	214	DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
216	}	215	}
217	}	216	}
218		217

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 1927 → 2025