WebSVN - FlightCtrl - Diff - Rev 2102 and 2103 - /branches/dongfang_FC


#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "externalControl.h"
#include "failsafeControl.h"
#include "configuration.h"
#include "attitude.h"
#include "commands.h"
#include "output.h"
 
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();
  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.
}
 
/*
 * 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 tempPRYT[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_periodicTaskAndPRYT(tempPRYT);
 
  // Add external control to RC
  EC_periodicTaskAndPRYT(tempPRYT);
 
  FC_periodicTaskAndPRYT(tempPRYT);
 
  // Commit results to global variable and also measure control activity.
  controls[CONTROL_THROTTLE] = tempPRYT[CONTROL_THROTTLE];
  controls[CONTROL_ELEVATOR] = tempPRYT[CONTROL_ELEVATOR];
  controls[CONTROL_AILERONS] = tempPRYT[CONTROL_AILERONS];
  controls[CONTROL_RUDDER] =   tempPRYT[CONTROL_RUDDER];
  // dampenControlActivity();
 
  // We can safely do this even with a bad signal - the variables will not have been updated then.
  configuration_applyVariablesToParams();
 }
 

Rev 2102	Rev 2103
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 "externalControl.h"	4	#include "externalControl.h"
5	#include "failsafeControl.h"	5	#include "failsafeControl.h"
6	#include "configuration.h"	6	#include "configuration.h"
7	#include "attitude.h"	7	#include "attitude.h"
8	#include "commands.h"	8	#include "commands.h"
9	#include "output.h"	9	#include "output.h"
10		10
11	int16_t controls[4] = { 0, 0, 0, 0 };	11	int16_t controls[4] = { 0, 0, 0, 0 };
12		12
13	// Internal variables for reading commands made with an R/C stick.	13	// Internal variables for reading commands made with an R/C stick.
14	uint8_t lastCommand = COMMAND_NONE;	14	uint8_t lastCommand = COMMAND_NONE;
15	uint8_t lastArgument;	15	uint8_t lastArgument;
16		16
17	uint8_t isCommandRepeated = 0;	17	uint8_t isCommandRepeated = 0;
18	uint8_t controlMixer_didReceiveSignal = 0;	18	uint8_t controlMixer_didReceiveSignal = 0;
19		19
20	/*	20	/*
21	* This could be expanded to take arguments from ohter sources than the RC	21	* This could be expanded to take arguments from ohter sources than the RC
22	* (read: Custom MK RC project)	22	* (read: Custom MK RC project)
23	*/	23	*/
24	uint8_t controlMixer_getArgument(void) {	24	uint8_t controlMixer_getArgument(void) {
25	return lastArgument;	25	return lastArgument;
26	}	26	}
27		27
28	/*	28	/*
29	* This could be expanded to take calibrate / start / stop commands from ohter sources	29	* This could be expanded to take calibrate / start / stop commands from ohter sources
30	* than the R/C (read: Custom MK R/C project)	30	* than the R/C (read: Custom MK R/C project)
31	*/	31	*/
32	uint8_t controlMixer_getCommand(void) {	32	uint8_t controlMixer_getCommand(void) {
33	return lastCommand;	33	return lastCommand;
34	}	34	}
35		35
36	uint8_t controlMixer_isCommandRepeated(void) {	36	uint8_t controlMixer_isCommandRepeated(void) {
37	return isCommandRepeated;	37	return isCommandRepeated;
38	}	38	}
39		39
40	void controlMixer_setNeutral() {	40	void controlMixer_setNeutral() {
41	for (uint8_t i=0; i<VARIABLE_COUNT; i++) {	41	for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
42	variables[i] = RC_getVariable(i);	42	variables[i] = RC_getVariable(i);
43	}	43	}
44	EC_setNeutral();	44	EC_setNeutral();
45	FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.	45	FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl.
46	}	46	}
47		47
48	/*	48	/*
49	* Update potentiometer values with limited slew rate. Could be made faster if desired.	49	* Update potentiometer values with limited slew rate. Could be made faster if desired.
50	* TODO: It assumes R/C as source. Not necessarily true.	50	* TODO: It assumes R/C as source. Not necessarily true.
51	*/	51	*/
52	void controlMixer_updateVariables(void) {	52	void controlMixer_updateVariables(void) {
53	uint8_t i;	53	uint8_t i;
54	int16_t targetvalue;	54	int16_t targetvalue;
55	for (i=0; i < VARIABLE_COUNT; i++) {	55	for (i=0; i < VARIABLE_COUNT; i++) {
56	targetvalue = RC_getVariable(i);	56	targetvalue = RC_getVariable(i);
57	if (targetvalue < 0)	57	if (targetvalue < 0)
58	targetvalue = 0;	58	targetvalue = 0;
59	if (variables[i] < targetvalue && variables[i] < 255)	59	if (variables[i] < targetvalue && variables[i] < 255)
60	variables[i]++;	60	variables[i]++;
61	else if (variables[i] > 0 && variables[i] > targetvalue)	61	else if (variables[i] > 0 && variables[i] > targetvalue)
62	variables[i]--;	62	variables[i]--;
63	}	63	}
64	}	64	}
65		65
66	uint8_t controlMixer_getSignalQuality(void) {	66	uint8_t controlMixer_getSignalQuality(void) {
67	uint8_t rcQ = RC_getSignalQuality();	67	uint8_t rcQ = RC_getSignalQuality();
68	uint8_t ecQ = EC_getSignalQuality();	68	uint8_t ecQ = EC_getSignalQuality();
69		69
70	// This needs not be the only correct solution...	70	// This needs not be the only correct solution...
71	return rcQ > ecQ ? rcQ : ecQ;	71	return rcQ > ecQ ? rcQ : ecQ;
72	}	72	}
73		73
74	/*	74	/*
75	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {	75	void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
76	int16_t tmp = controls[index];	76	int16_t tmp = controls[index];
77	controls[index] = newValue;	77	controls[index] = newValue;
78		78
79	tmp -= newValue;	79	tmp -= newValue;
80	tmp /= 2;	80	tmp /= 2;
81	tmp = tmp * tmp;	81	tmp = tmp * tmp;
82	// tmp += (newValue >= 0) ? newValue : -newValue;	82	// tmp += (newValue >= 0) ? newValue : -newValue;
83		83
84	/ *	84	/ *
85	if (controlActivity + (uint16_t)tmp >= controlActivity)	85	if (controlActivity + (uint16_t)tmp >= controlActivity)
86	controlActivity += tmp;	86	controlActivity += tmp;
87	else controlActivity = 0xffff;	87	else controlActivity = 0xffff;
88	* /	88	* /
89	if (controlActivity + (uint16_t)tmp < 0x8000)	89	if (controlActivity + (uint16_t)tmp < 0x8000)
90	controlActivity += tmp;	90	controlActivity += tmp;
91	}	91	}
92		92
93	#define CADAMPING 10	93	#define CADAMPING 10
94	void dampenControlActivity(void) {	94	void dampenControlActivity(void) {
95	uint32_t tmp = controlActivity;	95	uint32_t tmp = controlActivity;
96	tmp *= ((1<<CADAMPING)-1);	96	tmp *= ((1<<CADAMPING)-1);
97	tmp >>= CADAMPING;	97	tmp >>= CADAMPING;
98	controlActivity = tmp;	98	controlActivity = tmp;
99	}	99	}
100	*/	100	*/
101		101
102	/*	102	/*
103	* Update the variables indicating stick position from the sum of R/C, GPS and external control	103	* Update the variables indicating stick position from the sum of R/C, GPS and external control
104	* and whatever other controls we invented in the meantime...	104	* and whatever other controls we invented in the meantime...
105	* Update variables.	105	* Update variables.
106	* Decode commands but do not execute them.	106	* Decode commands but do not execute them.
107	*/	107	*/
108		108
109	void controlMixer_periodicTask(void) {	109	void controlMixer_periodicTask(void) {
110	int16_t tempPRTY[4] = { 0, 0, 0, 0 };	110	int16_t tempPRYT[4] = { 0, 0, 0, 0 };
111		111
112	// Decode commands.	112	// Decode commands.
113	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;	113	uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;
114		114
115	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;	115	uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;
116		116
117	// Update variables ("potis").	117	// Update variables ("potis").
118	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {	118	if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
119	controlMixer_updateVariables();	119	controlMixer_updateVariables();
120	controlMixer_didReceiveSignal = 1;	120	controlMixer_didReceiveSignal = 1;
121	} else { // Signal is not OK	121	} else { // Signal is not OK
122	// Could handle switch to emergency flight here.	122	// Could handle switch to emergency flight here.
123	// throttle is handled elsewhere.	123	// throttle is handled elsewhere.
124	}	124	}
125		125
126	if (rcCommand != COMMAND_NONE) {	126	if (rcCommand != COMMAND_NONE) {
127	isCommandRepeated = (lastCommand == rcCommand);	127	isCommandRepeated = (lastCommand == rcCommand);
128	lastCommand = rcCommand;	128	lastCommand = rcCommand;
129	lastArgument = RC_getArgument();	129	lastArgument = RC_getArgument();
130	} else if (ecCommand != COMMAND_NONE) {	130	} else if (ecCommand != COMMAND_NONE) {
131	isCommandRepeated = (lastCommand == ecCommand);	131	isCommandRepeated = (lastCommand == ecCommand);
132	lastCommand = ecCommand;	132	lastCommand = ecCommand;
133	lastArgument = EC_getArgument();	133	lastArgument = EC_getArgument();
134	} else {	134	} else {
135	// Both sources have no command, or one or both are out.	135	// Both sources have no command, or one or both are out.
136	// Just set to false. There is no reason to check if the none-command was repeated anyway.	136	// Just set to false. There is no reason to check if the none-command was repeated anyway.
137	isCommandRepeated = 0;	137	isCommandRepeated = 0;
138	lastCommand = COMMAND_NONE;	138	lastCommand = COMMAND_NONE;
139	}	139	}
140		140
141	// This will init the values (not just add to them).	141	// This will init the values (not just add to them).
142	RC_periodicTaskAndPRTY(tempPRTY);	142	RC_periodicTaskAndPRYT(tempPRYT);
143		143
144	// Add external control to RC	144	// Add external control to RC
145	EC_periodicTaskAndPRTY(tempPRTY);	145	EC_periodicTaskAndPRYT(tempPRYT);
146		146
147	FC_periodicTaskAndPRTY(tempPRTY);	147	FC_periodicTaskAndPRYT(tempPRYT);
148		148
149	// Commit results to global variable and also measure control activity.	149	// Commit results to global variable and also measure control activity.
150	controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];	150	controls[CONTROL_THROTTLE] = tempPRYT[CONTROL_THROTTLE];
151	controls[CONTROL_ELEVATOR] = tempPRTY[CONTROL_ELEVATOR];	151	controls[CONTROL_ELEVATOR] = tempPRYT[CONTROL_ELEVATOR];
152	controls[CONTROL_AILERONS] = tempPRTY[CONTROL_AILERONS];	152	controls[CONTROL_AILERONS] = tempPRYT[CONTROL_AILERONS];
153	controls[CONTROL_RUDDER] = tempPRTY[CONTROL_RUDDER];	153	controls[CONTROL_RUDDER] = tempPRYT[CONTROL_RUDDER];
154	// dampenControlActivity();	154	// dampenControlActivity();
155		155
156	// We can safely do this even with a bad signal - the variables will not have been updated then.	156	// We can safely do this even with a bad signal - the variables will not have been updated then.
157	configuration_applyVariablesToParams();	157	configuration_applyVariablesToParams();
158	}	158	}
159		159

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/controlMixer.c @ 2076 - Rev 2102 → 2103