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

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c - Rev 2055 → 2058