WebSVN - FlightCtrl - Diff - Rev 2088 and 2089 - /branches/dongfang_FC


#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "heightControl.h"
#include "attitudeControl.h"
#include "externalControl.h"
#include "compassControl.h"
#include "failsafeControl.h"
#include "naviControl.h"
#include "configuration.h"
#include "attitude.h"
#include "commands.h"
#include "output.h"
 
// uint16_t maxControl[2] = { 0, 0 };
// uint16_t controlActivity = 0;
int16_t controls[4] = { 0, 0, 0, 0 };
 
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand = COMMAND_NONE;
uint8_t lastArgument;
 
uint8_t isCommandRepeated = 0;
uint8_t controlMixer_didReceiveSignal = 0;
 
/*
 * This could be expanded to take arguments from ohter sources than the RC
 * (read: Custom MK RC project)
 */
uint8_t controlMixer_getArgument(void) {
  return lastArgument;
}
 
/*
 * This could be expanded to take calibrate / start / stop commands from ohter sources
 * than the R/C (read: Custom MK R/C project)
 */
uint8_t controlMixer_getCommand(void) {
  return lastCommand;
}
 
uint8_t controlMixer_isCommandRepeated(void) {
  return isCommandRepeated;
}
 
void controlMixer_setNeutral() {
  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
    variables[i] = RC_getVariable(i);
  }
  EC_setNeutral();
  HC_setGround();
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
 
  // This is to set the home pos in navi.
  // MKFlags |= MKFLAG_CALIBRATE;
}
 
/*
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
 * TODO: It assumes R/C as source. Not necessarily true.
 */
void controlMixer_updateVariables(void) {
  uint8_t i;
  int16_t targetvalue;
  for (i=0; i < VARIABLE_COUNT; i++) {
    targetvalue = RC_getVariable(i);
    if (targetvalue < 0)
      targetvalue = 0;
    if (variables[i] < targetvalue && variables[i] < 255)
      variables[i]++;
    else if (variables[i] > 0 && variables[i] > targetvalue)
      variables[i]--;
  }
}
 
uint8_t controlMixer_getSignalQuality(void) {
  uint8_t rcQ = RC_getSignalQuality();
  uint8_t ecQ = EC_getSignalQuality();
 
  // This needs not be the only correct solution...
  return rcQ > ecQ ? rcQ : ecQ;
}
 
/*
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
  int16_t tmp = controls[index];
  controls[index] = newValue;
 
  tmp -= newValue;
  tmp /= 2;
  tmp = tmp * tmp;
  // tmp += (newValue >= 0) ? newValue : -newValue;
 
  / *
  if (controlActivity + (uint16_t)tmp >= controlActivity)
    controlActivity += tmp;
  else controlActivity = 0xffff;
  * /
  if (controlActivity + (uint16_t)tmp < 0x8000)
    controlActivity += tmp;
}
 
#define CADAMPING 10
void dampenControlActivity(void) {
  uint32_t tmp = controlActivity;
  tmp *= ((1<<CADAMPING)-1);
  tmp >>= CADAMPING;
  controlActivity = tmp;
}
*/
 
/*
 * Update the variables indicating stick position from the sum of R/C, GPS and external control
 * and whatever other controls we invented in the meantime...
 * Update variables.
 * Decode commands but do not execute them.
 */
 
void controlMixer_periodicTask(void) {
  int16_t tempPRTY[4] = { 0, 0, 0, 0 };
 
  // Decode commands.
  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
    : COMMAND_NONE;
 
  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
    : COMMAND_NONE;
 
  // Update variables ("potis").
  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
    controlMixer_updateVariables();
    controlMixer_didReceiveSignal = 1;
  } else { // Signal is not OK
    // Could handle switch to emergency flight here.
    // throttle is handled elsewhere.
  }
 
  if (rcCommand != COMMAND_NONE) {
    isCommandRepeated = (lastCommand == rcCommand);
    lastCommand = rcCommand;
    lastArgument = RC_getArgument();
  } else if (ecCommand != COMMAND_NONE) {
    isCommandRepeated = (lastCommand == ecCommand);
    lastCommand = ecCommand;
    lastArgument = EC_getArgument();
  } else {
    // Both sources have no command, or one or both are out.
    // Just set to false. There is no reason to check if the none-command was repeated anyway.
    isCommandRepeated = 0;
    lastCommand = COMMAND_NONE;
  }
 
  // This will init the values (not just add to them).
  RC_periodicTaskAndPRTY(tempPRTY);
 
  // Add external control to RC
  EC_periodicTaskAndPRTY(tempPRTY);
 
#ifdef USE_DIRECT_GPS
  if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
    navigation_periodicTaskAndPRTY(tempPRTY);
#endif
 
  // Add compass control (could also have been before navi, they are independent)
  CC_periodicTaskAndPRTY(tempPRTY);
 
  FC_periodicTaskAndPRTY(tempPRTY);
 
  // This is temporary. There might be some emergency height control also.
  if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
    // Add height control (could also have been before navi and/or compass, they are independent)
    HC_periodicTaskAndPRTY(tempPRTY);
 
    // Add attitude control (could also have been before navi and/or compass, they are independent)
    AC_getPRTY(tempPRTY);
  }
 
  // Commit results to global variable and also measure control activity.
  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
  controls[CONTROL_PITCH] = tempPRTY[CONTROL_PITCH];
  controls[CONTROL_ROLL] = tempPRTY[CONTROL_ROLL];
  controls[CONTROL_YAW] = tempPRTY[CONTROL_YAW];
  // dampenControlActivity();
 
  // We can safely do this even with a bad signal - the variables will not have been updated then.
  configuration_applyVariablesToParams();
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 }
 

Rev 2088	Rev 2089
1	#include <stdlib.h>	1	#include <stdlib.h>
2	#include "controlMixer.h"	2	#include "controlMixer.h"
3	#include "rc.h"	3	#include "rc.h"
4	#include "heightControl.h"	4	#include "heightControl.h"
5	#include "attitudeControl.h"	5	#include "attitudeControl.h"
6	#include "externalControl.h"	6	#include "externalControl.h"
7	#include "compassControl.h"	7	#include "compassControl.h"
8	#include "failsafeControl.h"	8	#include "failsafeControl.h"
9	#include "naviControl.h"	9	#include "naviControl.h"
10	#include "configuration.h"	10	#include "configuration.h"
11	#include "attitude.h"	11	#include "attitude.h"
12	#include "commands.h"	12	#include "commands.h"
13	#include "output.h"	13	#include "output.h"
14		14
15	// uint16_t maxControl[2] = { 0, 0 };	15	// uint16_t maxControl[2] = { 0, 0 };
16	uint16_t controlActivity = 0;	16	// uint16_t controlActivity = 0;
17	int16_t controls[4] = { 0, 0, 0, 0 };	17	int16_t controls[4] = { 0, 0, 0, 0 };
18		18
19	// Internal variables for reading commands made with an R/C stick.	19	// Internal variables for reading commands made with an R/C stick.
20	uint8_t lastCommand = COMMAND_NONE;	20	uint8_t lastCommand = COMMAND_NONE;
21	uint8_t lastArgument;	21	uint8_t lastArgument;
22		22
23	uint8_t isCommandRepeated = 0;	23	uint8_t isCommandRepeated = 0;
24	uint8_t controlMixer_didReceiveSignal = 0;	24	uint8_t controlMixer_didReceiveSignal = 0;
25		25
26	/*	26	/*
27	* This could be expanded to take arguments from ohter sources than the RC	27	* This could be expanded to take arguments from ohter sources than the RC
28	* (read: Custom MK RC project)	28	* (read: Custom MK RC project)
29	*/	29	*/
30	uint8_t controlMixer_getArgument(void) {	30	uint8_t controlMixer_getArgument(void) {
31	return lastArgument;	31	return lastArgument;
32	}	32	}
33		33
34	/*	34	/*
35	* This could be expanded to take calibrate / start / stop commands from ohter sources	35	* This could be expanded to take calibrate / start / stop commands from ohter sources
36	* than the R/C (read: Custom MK R/C project)	36	* than the R/C (read: Custom MK R/C project)
37	*/	37	*/
38	uint8_t controlMixer_getCommand(void) {	38	uint8_t controlMixer_getCommand(void) {
39	return lastCommand;	39	return lastCommand;
40	}	40	}
41		41
42	uint8_t controlMixer_isCommandRepeated(void) {	42	uint8_t controlMixer_isCommandRepeated(void) {
43	return isCommandRepeated;	43	return isCommandRepeated;
44	}	44	}
45		45
46	void controlMixer_setNeutral() {	46	void controlMixer_setNeutral() {
47	for (uint8_t i=0; i<VARIABLE_COUNT; i++) {	47	for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
48	variables[i] = RC_getVariable(i);	48	variables[i] = RC_getVariable(i);
49	}	49	}
50	EC_setNeutral();	50	EC_setNeutral();
51	HC_setGround();	51	HC_setGround();
52	FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.	52	FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.
53		53
54	// This is to set the home pos in navi.	54	// This is to set the home pos in navi.
55	// MKFlags \|= MKFLAG_CALIBRATE;	55	// MKFlags \|= MKFLAG_CALIBRATE;
56	}	56	}
57		57
58	/*	58	/*
59	* Update potentiometer values with limited slew rate. Could be made faster if desired.	59	* Update potentiometer values with limited slew rate. Could be made faster if desired.
60	* TODO: It assumes R/C as source. Not necessarily true.	60	* TODO: It assumes R/C as source. Not necessarily true.
61	*/	61	*/
62	void controlMixer_updateVariables(void) {	62	void controlMixer_updateVariables(void) {
63	uint8_t i;	63	uint8_t i;
64	int16_t targetvalue;	64	int16_t targetvalue;
65	for (i=0; i < VARIABLE_COUNT; i++) {	65	for (i=0; i < VARIABLE_COUNT; i++) {
66	targetvalue = RC_getVariable(i);	66	targetvalue = RC_getVariable(i);
67	if (targetvalue < 0)	67	if (targetvalue < 0)
68	targetvalue = 0;	68	targetvalue = 0;
69	if (variables[i] < targetvalue && variables[i] < 255)	69	if (variables[i] < targetvalue && variables[i] < 255)
70	variables[i]++;	70	variables[i]++;
71	else if (variables[i] > 0 && variables[i] > targetvalue)	71	else if (variables[i] > 0 && variables[i] > targetvalue)
72	variables[i]--;	72	variables[i]--;
73	}	73	}
74	}	74	}
75		75
76	uint8_t controlMixer_getSignalQuality(void) {	76	uint8_t controlMixer_getSignalQuality(void) {
77	uint8_t rcQ = RC_getSignalQuality();	77	uint8_t rcQ = RC_getSignalQuality();
78	uint8_t ecQ = EC_getSignalQuality();	78	uint8_t ecQ = EC_getSignalQuality();
79		79
80	// This needs not be the only correct solution...	80	// This needs not be the only correct solution...
81	return rcQ > ecQ ? rcQ : ecQ;	81	return rcQ > ecQ ? rcQ : ecQ;
82	}	82	}
-		83
83		84	/*
84	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {	85	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
85	int16_t tmp = controls[index];	86	int16_t tmp = controls[index];
86	controls[index] = newValue;	87	controls[index] = newValue;
87		88
88	tmp -= newValue;	89	tmp -= newValue;
89	tmp /= 2;	90	tmp /= 2;
90	tmp = tmp * tmp;	91	tmp = tmp * tmp;
91	// tmp += (newValue >= 0) ? newValue : -newValue;	92	// tmp += (newValue >= 0) ? newValue : -newValue;
-		93
92	/*	94	/ *
93	if (controlActivity + (uint16_t)tmp >= controlActivity)	95	if (controlActivity + (uint16_t)tmp >= controlActivity)
94	controlActivity += tmp;	96	controlActivity += tmp;
95	else controlActivity = 0xffff;	97	else controlActivity = 0xffff;
96	*/	98	* /
97	if (controlActivity + (uint16_t)tmp < 0x8000)	99	if (controlActivity + (uint16_t)tmp < 0x8000)
98	controlActivity += tmp;	100	controlActivity += tmp;
99	}	101	}
100		102
101	#define CADAMPING 10	103	#define CADAMPING 10
102	void dampenControlActivity(void) {	104	void dampenControlActivity(void) {
103	uint32_t tmp = controlActivity;	105	uint32_t tmp = controlActivity;
104	tmp *= ((1<<CADAMPING)-1);	106	tmp *= ((1<<CADAMPING)-1);
105	tmp >>= CADAMPING;	107	tmp >>= CADAMPING;
106	controlActivity = tmp;	108	controlActivity = tmp;
107	}	109	}
-		110	*/
108		111
109	/*	112	/*
110	* Update the variables indicating stick position from the sum of R/C, GPS and external control	113	* Update the variables indicating stick position from the sum of R/C, GPS and external control
111	* and whatever other controls we invented in the meantime...	114	* and whatever other controls we invented in the meantime...
112	* Update variables.	115	* Update variables.
113	* Decode commands but do not execute them.	116	* Decode commands but do not execute them.
114	*/	117	*/
115		118
116	void controlMixer_periodicTask(void) {	119	void controlMixer_periodicTask(void) {
117	int16_t tempPRTY[4] = { 0, 0, 0, 0 };	120	int16_t tempPRTY[4] = { 0, 0, 0, 0 };
118		121
119	// Decode commands.	122	// Decode commands.
120	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()	123	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
121	: COMMAND_NONE;	124	: COMMAND_NONE;
122		125
123	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()	126	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
124	: COMMAND_NONE;	127	: COMMAND_NONE;
125		128
126	// Update variables ("potis").	129	// Update variables ("potis").
127	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {	130	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
128	controlMixer_updateVariables();	131	controlMixer_updateVariables();
129	controlMixer_didReceiveSignal = 1;	132	controlMixer_didReceiveSignal = 1;
130	} else { // Signal is not OK	133	} else { // Signal is not OK
131	// Could handle switch to emergency flight here.	134	// Could handle switch to emergency flight here.
132	// throttle is handled elsewhere.	135	// throttle is handled elsewhere.
133	}	136	}
134		137
135	if (rcCommand != COMMAND_NONE) {	138	if (rcCommand != COMMAND_NONE) {
136	isCommandRepeated = (lastCommand == rcCommand);	139	isCommandRepeated = (lastCommand == rcCommand);
137	lastCommand = rcCommand;	140	lastCommand = rcCommand;
138	lastArgument = RC_getArgument();	141	lastArgument = RC_getArgument();
139	} else if (ecCommand != COMMAND_NONE) {	142	} else if (ecCommand != COMMAND_NONE) {
140	isCommandRepeated = (lastCommand == ecCommand);	143	isCommandRepeated = (lastCommand == ecCommand);
141	lastCommand = ecCommand;	144	lastCommand = ecCommand;
142	lastArgument = EC_getArgument();	145	lastArgument = EC_getArgument();
143	} else {	146	} else {
144	// Both sources have no command, or one or both are out.	147	// Both sources have no command, or one or both are out.
145	// Just set to false. There is no reason to check if the none-command was repeated anyway.	148	// Just set to false. There is no reason to check if the none-command was repeated anyway.
146	isCommandRepeated = 0;	149	isCommandRepeated = 0;
147	lastCommand = COMMAND_NONE;	150	lastCommand = COMMAND_NONE;
148	}	151	}
149		152
150	// This will init the values (not just add to them).	153	// This will init the values (not just add to them).
151	RC_periodicTaskAndPRTY(tempPRTY);	154	RC_periodicTaskAndPRTY(tempPRTY);
152		155
153	// Add external control to RC	156	// Add external control to RC
154	EC_periodicTaskAndPRTY(tempPRTY);	157	EC_periodicTaskAndPRTY(tempPRTY);
155		158
156	#ifdef USE_DIRECT_GPS	159	#ifdef USE_DIRECT_GPS
157	if (staticParams.bitConfig & (CFG_NAVI_ENABLED))	160	if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
158	navigation_periodicTaskAndPRTY(tempPRTY);	161	navigation_periodicTaskAndPRTY(tempPRTY);
159	#endif	162	#endif
160		163
161	// Add compass control (could also have been before navi, they are independent)	164	// Add compass control (could also have been before navi, they are independent)
162	CC_periodicTaskAndPRTY(tempPRTY);	165	CC_periodicTaskAndPRTY(tempPRTY);
163		166
164	FC_periodicTaskAndPRTY(tempPRTY);	167	FC_periodicTaskAndPRTY(tempPRTY);
165		168
166	// This is temporary. There might be some emergency height control also.	169	// This is temporary. There might be some emergency height control also.
167	if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {	170	if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
168	// Add height control (could also have been before navi and/or compass, they are independent)	171	// Add height control (could also have been before navi and/or compass, they are independent)
169	HC_periodicTaskAndPRTY(tempPRTY);	172	HC_periodicTaskAndPRTY(tempPRTY);
170		173
171	// Add attitude control (could also have been before navi and/or compass, they are independent)	174	// Add attitude control (could also have been before navi and/or compass, they are independent)
172	AC_getPRTY(tempPRTY);	175	AC_getPRTY(tempPRTY);
173	}	176	}
174		177
175	// Commit results to global variable and also measure control activity.	178	// Commit results to global variable and also measure control activity.
176	controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];	179	controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
177	updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);	180	controls[CONTROL_PITCH] = tempPRTY[CONTROL_PITCH];
178	updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);	181	controls[CONTROL_ROLL] = tempPRTY[CONTROL_ROLL];
179	updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);	182	controls[CONTROL_YAW] = tempPRTY[CONTROL_YAW];
180	dampenControlActivity();	183	// dampenControlActivity();
181		184
182	// We can safely do this even with a bad signal - the variables will not have been updated then.	185	// We can safely do this even with a bad signal - the variables will not have been updated then.
183	configuration_applyVariablesToParams();	186	configuration_applyVariablesToParams();
184		-
185	// part1a end.	-
186		-
187	/* This is not really necessary with the dead-band feature on all sticks (see rc.c)	-
188	if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE \| CFG_GPS_ACTIVE)) {	-
189	if (controlYaw > 2) controlYaw-= 2;	-
190	else if (controlYaw< -2) controlYaw += 2;	-
191	else controlYaw = 0;	-
192	}	-
193	*/	-
194		-
195	/*	-
196	* Record maxima. Predecessor of the control activity stuff.	-
197	for (axis = PITCH; axis <= ROLL; axis++) {	-
198	if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {	-
199	maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;	-
200	if (maxControl[axis] > 100)	-
201	maxControl[axis] = 100;	-
202	} else if (maxControl[axis])	-
203	maxControl[axis]--;	-
204	}	-
205	*/	-
206	}	187	}
207		188

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c @ 1925 - Rev 2088 → 2089