WebSVN - FlightCtrl - Diff - Rev 2067 and 2073 - /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;
 
  debugOut.analog[31] = rcCommand;
 
  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);
 
  debugOut.analog[16] = tempPRTY[CONTROL_PITCH];
  debugOut.analog[17] = tempPRTY[CONTROL_ROLL];
  debugOut.analog[18] = tempPRTY[CONTROL_THROTTLE];
  debugOut.analog[19] = tempPRTY[CONTROL_YAW];
 
  // Add external control to RC
  EC_periodicTaskAndPRTY(tempPRTY);
 
#ifdef USE_DIRECT_GPS
  if (staticParams.bitConfig & (CFG_COMPASS_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];
  updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
  updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
  updateControlAndMeasureControlActivity(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();
 
  // part1a end.
 
  /* This is not really necessary with the dead-band feature on all sticks (see rc.c)
     if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
     if (controlYaw > 2) controlYaw-= 2;
     else if (controlYaw< -2) controlYaw += 2;
     else controlYaw = 0;
     }
  */
 
  /*
   * Record maxima. Predecessor of the control activity stuff.
   for (axis = PITCH; axis <= ROLL; axis++) {
   if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
   maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
   if (maxControl[axis] > 100)
   maxControl[axis] = 100;
   } else if (maxControl[axis])
   maxControl[axis]--;
   }
  */
 }
 

Rev 2067	Rev 2073
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		-
75	debugOut.analog[31] = variables[0];	-
76	}	74	}
77		75
78	uint8_t controlMixer_getSignalQuality(void) {	76	uint8_t controlMixer_getSignalQuality(void) {
79	uint8_t rcQ = RC_getSignalQuality();	77	uint8_t rcQ = RC_getSignalQuality();
80	uint8_t ecQ = EC_getSignalQuality();	78	uint8_t ecQ = EC_getSignalQuality();
81		79
82	// This needs not be the only correct solution...	80	// This needs not be the only correct solution...
83	return rcQ > ecQ ? rcQ : ecQ;	81	return rcQ > ecQ ? rcQ : ecQ;
84	}	82	}
85		83
86	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {	84	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
87	int16_t tmp = controls[index];	85	int16_t tmp = controls[index];
88	controls[index] = newValue;	86	controls[index] = newValue;
89		87
90	tmp -= newValue;	88	tmp -= newValue;
91	tmp /= 2;	89	tmp /= 2;
92	tmp = tmp * tmp;	90	tmp = tmp * tmp;
93	// tmp += (newValue >= 0) ? newValue : -newValue;	91	// tmp += (newValue >= 0) ? newValue : -newValue;
94	/*	92	/*
95	if (controlActivity + (uint16_t)tmp >= controlActivity)	93	if (controlActivity + (uint16_t)tmp >= controlActivity)
96	controlActivity += tmp;	94	controlActivity += tmp;
97	else controlActivity = 0xffff;	95	else controlActivity = 0xffff;
98	*/	96	*/
99	if (controlActivity + (uint16_t)tmp < 0x8000)	97	if (controlActivity + (uint16_t)tmp < 0x8000)
100	controlActivity += tmp;	98	controlActivity += tmp;
101	}	99	}
102		100
103	#define CADAMPING 10	101	#define CADAMPING 10
104	void dampenControlActivity(void) {	102	void dampenControlActivity(void) {
105	uint32_t tmp = controlActivity;	103	uint32_t tmp = controlActivity;
106	tmp *= ((1<<CADAMPING)-1);	104	tmp *= ((1<<CADAMPING)-1);
107	tmp >>= CADAMPING;	105	tmp >>= CADAMPING;
108	controlActivity = tmp;	106	controlActivity = tmp;
109	}	107	}
110		108
111	/*	109	/*
112	* Update the variables indicating stick position from the sum of R/C, GPS and external control	110	* Update the variables indicating stick position from the sum of R/C, GPS and external control
113	* and whatever other controls we invented in the meantime...	111	* and whatever other controls we invented in the meantime...
114	* Update variables.	112	* Update variables.
115	* Decode commands but do not execute them.	113	* Decode commands but do not execute them.
116	*/	114	*/
117		115
118	void controlMixer_periodicTask(void) {	116	void controlMixer_periodicTask(void) {
119	int16_t tempPRTY[4] = { 0, 0, 0, 0 };	117	int16_t tempPRTY[4] = { 0, 0, 0, 0 };
120		118
121	// Decode commands.	119	// Decode commands.
122	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()	120	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
123	: COMMAND_NONE;	121	: COMMAND_NONE;
-		122
-		123	debugOut.analog[31] = rcCommand;
-		124
124	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()	125	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
125	: COMMAND_NONE;	126	: COMMAND_NONE;
126		127
127	// Update variables ("potis").	128	// Update variables ("potis").
128	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {	129	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
129	controlMixer_updateVariables();	130	controlMixer_updateVariables();
130	controlMixer_didReceiveSignal = 1;	131	controlMixer_didReceiveSignal = 1;
131	} else { // Signal is not OK	132	} else { // Signal is not OK
132	// Could handle switch to emergency flight here.	133	// Could handle switch to emergency flight here.
133	// throttle is handled elsewhere.	134	// throttle is handled elsewhere.
134	}	135	}
135		136
136	if (rcCommand != COMMAND_NONE) {	137	if (rcCommand != COMMAND_NONE) {
137	isCommandRepeated = (lastCommand == rcCommand);	138	isCommandRepeated = (lastCommand == rcCommand);
138	lastCommand = rcCommand;	139	lastCommand = rcCommand;
139	lastArgument = RC_getArgument();	140	lastArgument = RC_getArgument();
140	} else if (ecCommand != COMMAND_NONE) {	141	} else if (ecCommand != COMMAND_NONE) {
141	isCommandRepeated = (lastCommand == ecCommand);	142	isCommandRepeated = (lastCommand == ecCommand);
142	lastCommand = ecCommand;	143	lastCommand = ecCommand;
143	lastArgument = EC_getArgument();	144	lastArgument = EC_getArgument();
144	} else {	145	} else {
145	// Both sources have no command, or one or both are out.	146	// Both sources have no command, or one or both are out.
146	// Just set to false. There is no reason to check if the none-command was repeated anyway.	147	// Just set to false. There is no reason to check if the none-command was repeated anyway.
147	isCommandRepeated = 0;	148	isCommandRepeated = 0;
148	lastCommand = COMMAND_NONE;	149	lastCommand = COMMAND_NONE;
149	}	150	}
150		151
151	// This will init the values (not just add to them).	152	// This will init the values (not just add to them).
152	RC_periodicTaskAndPRTY(tempPRTY);	153	RC_periodicTaskAndPRTY(tempPRTY);
153		154
154	debugOut.analog[16] = tempPRTY[CONTROL_PITCH];	155	debugOut.analog[16] = tempPRTY[CONTROL_PITCH];
155	debugOut.analog[17] = tempPRTY[CONTROL_ROLL];	156	debugOut.analog[17] = tempPRTY[CONTROL_ROLL];
156	debugOut.analog[18] = tempPRTY[CONTROL_THROTTLE];	157	debugOut.analog[18] = tempPRTY[CONTROL_THROTTLE];
157	debugOut.analog[19] = tempPRTY[CONTROL_YAW];	158	debugOut.analog[19] = tempPRTY[CONTROL_YAW];
158		159
159	// Add external control to RC	160	// Add external control to RC
160	EC_periodicTaskAndPRTY(tempPRTY);	161	EC_periodicTaskAndPRTY(tempPRTY);
161		162
162	#ifdef USE_DIRECT_GPS	163	#ifdef USE_DIRECT_GPS
163	if (staticParams.bitConfig & (CFG_COMPASS_ENABLED))	164	if (staticParams.bitConfig & (CFG_COMPASS_ENABLED))
164	navigation_periodicTaskAndPRTY(tempPRTY);	165	navigation_periodicTaskAndPRTY(tempPRTY);
165	#endif	166	#endif
166		167
167	// Add compass control (could also have been before navi, they are independent)	168	// Add compass control (could also have been before navi, they are independent)
168	CC_periodicTaskAndPRTY(tempPRTY);	169	CC_periodicTaskAndPRTY(tempPRTY);
169		170
170	FC_periodicTaskAndPRTY(tempPRTY);	171	FC_periodicTaskAndPRTY(tempPRTY);
171		172
172	// This is temporary. There might be some emergency height control also.	173	// This is temporary. There might be some emergency height control also.
173	if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {	174	if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
174	// Add height control (could also have been before navi and/or compass, they are independent)	175	// Add height control (could also have been before navi and/or compass, they are independent)
175	HC_periodicTaskAndPRTY(tempPRTY);	176	HC_periodicTaskAndPRTY(tempPRTY);
176		177
177	// Add attitude control (could also have been before navi and/or compass, they are independent)	178	// Add attitude control (could also have been before navi and/or compass, they are independent)
178	AC_getPRTY(tempPRTY);	179	AC_getPRTY(tempPRTY);
179	}	180	}
180		181
181	// Commit results to global variable and also measure control activity.	182	// Commit results to global variable and also measure control activity.
182	controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];	183	controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];
183	updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);	184	updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
184	updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);	185	updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
185	updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);	186	updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
186	dampenControlActivity();	187	dampenControlActivity();
187	//debugOut.analog[17] = controlActivity/10;	-
188		188
189	// We can safely do this even with a bad signal - the variables will not have been updated then.	189	// We can safely do this even with a bad signal - the variables will not have been updated then.
190	configuration_applyVariablesToParams();	190	configuration_applyVariablesToParams();
191		191
192	// part1a end.	192	// part1a end.
193		193
194	/* This is not really necessary with the dead-band feature on all sticks (see rc.c)	194	/* This is not really necessary with the dead-band feature on all sticks (see rc.c)
195	if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE \| CFG_GPS_ACTIVE)) {	195	if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE \| CFG_GPS_ACTIVE)) {
196	if (controlYaw > 2) controlYaw-= 2;	196	if (controlYaw > 2) controlYaw-= 2;
197	else if (controlYaw< -2) controlYaw += 2;	197	else if (controlYaw< -2) controlYaw += 2;
198	else controlYaw = 0;	198	else controlYaw = 0;
199	}	199	}
200	*/	200	*/
201		201
202	/*	202	/*
203	* Record maxima. Predecessor of the control activity stuff.	203	* Record maxima. Predecessor of the control activity stuff.
204	for (axis = PITCH; axis <= ROLL; axis++) {	204	for (axis = PITCH; axis <= ROLL; axis++) {
205	if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {	205	if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
206	maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;	206	maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
207	if (maxControl[axis] > 100)	207	if (maxControl[axis] > 100)
208	maxControl[axis] = 100;	208	maxControl[axis] = 100;
209	} else if (maxControl[axis])	209	} else if (maxControl[axis])
210	maxControl[axis]--;	210	maxControl[axis]--;
211	}	211	}
212	*/	212	*/
213	}	213	}
214		214

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c - Rev 2067 → 2073