WebSVN - FlightCtrl - Diff - Rev 1634 and 1645 - /branches/dongfang_FC


// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "externalControl.h"
#include "configuration.h"
#include "attitude.h"
#include "eeprom.h"
#include "flight.h"
 
/*
 * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
 */
#define COMMAND_TIMER 200
 
uint16_t maxControl[2] = {0,0};
int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0;
uint8_t looping = 0;
 
// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand  = COMMAND_NONE;
uint8_t commandTimer = 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 RC_getArgument();
}
 
/*
 * 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) {
  // If the same command was made COMMAND_TIMER times, it's stable.
  if (commandTimer >= COMMAND_TIMER) {
    return lastCommand;
  }
  return COMMAND_NONE;
}
 
uint8_t controlMixer_isCommandRepeated(void) {
  return commandTimer > COMMAND_TIMER ? 1 : 0;
}
 
void controlMixer_setNeutral(uint8_t calibrate) {
  if (calibrate)
    RC_calibrate();
  EC_setNeutral();
}
 
/*
 * Set the potientiometer values to the momentary values of the respective R/C channels.
 * No slew rate limitation.
 */
void controlMixer_initVariables(void) {
  uint8_t i;
  for (i=0; i<8; i++) {
    variables[i] = RC_getVariable(i);
  }
}
 
/*
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
 */
void controlMixer_updateVariables(void) {
  uint8_t i;
  int16_t targetvalue;
  for (i=0; i<8; 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;
}
 
/*
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
 */
void controlMixer_update(void) {
  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
  // TODO: If no signal --> zero.
  uint8_t axis;
  RC_update();
  EC_update();
 
  int16_t* RC_PRTY = RC_getPRTY();
  int16_t* EC_PRTY = EC_getPRTY();
 
  control[PITCH]   = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
  control[ROLL]    = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
  controlThrottle  = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
 
 
 
 
 
  if (RC_getSignalQuality() >= SIGNAL_GOOD) {
    controlMixer_updateVariables();
    configuration_applyVariablesToParams();
    looping = RC_getLooping(looping);
  } else { // Signal is not OK
    // Could handle switch to emergency flight here.
    // throttle is handled elsewhere.
    looping = 0;
  }
 
  DebugOut.Analog[18] = (int16_t)looping;
 
  // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
  // TODO: Correct, for changed range (stick gain + expo is now applied already)
  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
   */
  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]--;
 
 
 
  }
 
 
  // Here we could blend in other sources.
  uint8_t commandNow = RC_getCommand();
 
  if (commandNow != lastCommand) {
    lastCommand = commandNow;
    commandTimer = 0;
  } else if (commandTimer < COMMAND_TIMER + 1)
    commandTimer++;
}
 
/*
void setCompassCalState(void) {
  static uint8_t stick = 1;
  // if pitch is centered or top set stick to zero
  if(RCChannel(CH_PITCH) > -20) stick = 0;
  // if pitch is down trigger to next cal state
  if((RCChannel(CH_PITCH) < -70) && !stick) {
    stick = 1;
    compassCalState++;
    if(compassCalState < 5) beepNumber(compassCalState);
    else beep(1000);
  }
}
*/
 

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c @ 2018 - Rev 1634 → 1645

Rev 1634		Rev 1645
1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	1	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2	// + Copyright (c) 04.2007 Holger Buss	2	// + Copyright (c) 04.2007 Holger Buss
3	// + Nur für den privaten Gebrauch	3	// + Nur für den privaten Gebrauch
4	// + www.MikroKopter.com	4	// + www.MikroKopter.com
5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	5	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),	6	// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.	7	// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt	8	// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
9	// + bzgl. der Nutzungsbedingungen aufzunehmen.	9	// + bzgl. der Nutzungsbedingungen aufzunehmen.
10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,	10	// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
11	// + Verkauf von Luftbildaufnahmen, usw.	11	// + Verkauf von Luftbildaufnahmen, usw.
12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	12	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,	13	// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen	14	// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	15	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts	16	// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"	17	// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
18	// + eindeutig als Ursprung verlinkt werden	18	// + eindeutig als Ursprung verlinkt werden
19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	19	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion	20	// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
21	// + Benutzung auf eigene Gefahr	21	// + Benutzung auf eigene Gefahr
22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden	22	// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	23	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur	24	// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
25	// + mit unserer Zustimmung zulässig	25	// + mit unserer Zustimmung zulässig
26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	26	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen	27	// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	28	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,	29	// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
30	// + this list of conditions and the following disclaimer.	30	// + this list of conditions and the following disclaimer.
31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived	31	// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
32	// + from this software without specific prior written permission.	32	// + from this software without specific prior written permission.
33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet	33	// + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
34	// + for non-commercial use (directly or indirectly)	34	// + for non-commercial use (directly or indirectly)
35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted	35	// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
36	// + with our written permission	36	// + with our written permission
37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be	37	// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
38	// + clearly linked as origin	38	// + clearly linked as origin
39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed	39	// + * porting to systems other than hardware from www.mikrokopter.de is not allowed
40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"	40	// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE	41	// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE	42	// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE	43	// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR	44	// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF	45	// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS	46	// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)	47	// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE	48	// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
49	// + POSSIBILITY OF SUCH DAMAGE.	49	// + POSSIBILITY OF SUCH DAMAGE.
50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	50	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
51		51
52	#include <stdlib.h>	52	#include <stdlib.h>
53	#include "controlMixer.h"	53	#include "controlMixer.h"
54	#include "rc.h"	54	#include "rc.h"
55	#include "externalControl.h"	55	#include "externalControl.h"
56	#include "configuration.h"	56	#include "configuration.h"
57	#include "attitude.h"	57	#include "attitude.h"
58	#include "eeprom.h"	58	#include "eeprom.h"
59	#include "flight.h"	59	#include "flight.h"
60		60
61	/*	61	/*
62	* Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.	62	* Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
63	*/	63	*/
64	#define COMMAND_TIMER 200	64	#define COMMAND_TIMER 200
65		65
66	uint16_t maxControlPitch = 0, maxControlRoll = 0;	66	uint16_t maxControl[2] = {0,0};
67	int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0;	67	int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0;
68	uint8_t looping = 0;	68	uint8_t looping = 0;
69		69
70	// Internal variables for reading commands made with an R/C stick.	70	// Internal variables for reading commands made with an R/C stick.
71	uint8_t lastCommand = COMMAND_NONE;	71	uint8_t lastCommand = COMMAND_NONE;
72	uint8_t commandTimer = 0;	72	uint8_t commandTimer = 0;
73		73
74	/*	74	/*
75	* This could be expanded to take arguments from ohter sources than the RC	75	* This could be expanded to take arguments from ohter sources than the RC
76	* (read: Custom MK RC project)	76	* (read: Custom MK RC project)
77	*/	77	*/
78	uint8_t controlMixer_getArgument(void) {	78	uint8_t controlMixer_getArgument(void) {
79	return RC_getArgument();	79	return RC_getArgument();
80	}	80	}
81		81
82	/*	82	/*
83	* This could be expanded to take calibrate / start / stop commands from ohter sources	83	* This could be expanded to take calibrate / start / stop commands from ohter sources
84	* than the R/C (read: Custom MK R/C project)	84	* than the R/C (read: Custom MK R/C project)
85	*/	85	*/
86	uint8_t controlMixer_getCommand(void) {	86	uint8_t controlMixer_getCommand(void) {
87	// If the same command was made COMMAND_TIMER times, it's stable.	87	// If the same command was made COMMAND_TIMER times, it's stable.
88	if (commandTimer >= COMMAND_TIMER) {	88	if (commandTimer >= COMMAND_TIMER) {
89	return lastCommand;	89	return lastCommand;
90	}	90	}
91	return COMMAND_NONE;	91	return COMMAND_NONE;
92	}	92	}
93		93
94	uint8_t controlMixer_isCommandRepeated(void) {	94	uint8_t controlMixer_isCommandRepeated(void) {
95	return commandTimer > COMMAND_TIMER ? 1 : 0;	95	return commandTimer > COMMAND_TIMER ? 1 : 0;
96	}	96	}
97		97
98	void controlMixer_setNeutral(uint8_t calibrate) {	98	void controlMixer_setNeutral(uint8_t calibrate) {
99	if (calibrate)	99	if (calibrate)
100	RC_calibrate();	100	RC_calibrate();
101	EC_setNeutral();	101	EC_setNeutral();
102	}	102	}
103		103
104	/*	104	/*
105	* Set the potientiometer values to the momentary values of the respective R/C channels.	105	* Set the potientiometer values to the momentary values of the respective R/C channels.
106	* No slew rate limitation.	106	* No slew rate limitation.
107	*/	107	*/
108	void controlMixer_initVariables(void) {	108	void controlMixer_initVariables(void) {
109	uint8_t i;	109	uint8_t i;
110	for (i=0; i<8; i++) {	110	for (i=0; i<8; i++) {
111	variables[i] = RC_getVariable(i);	111	variables[i] = RC_getVariable(i);
112	}	112	}
113	}	113	}
114		114
115	/*	115	/*
116	* Update potentiometer values with limited slew rate. Could be made faster if desired.	116	* Update potentiometer values with limited slew rate. Could be made faster if desired.
117	*/	117	*/
118	void controlMixer_updateVariables(void) {	118	void controlMixer_updateVariables(void) {
119	uint8_t i;	119	uint8_t i;
120	int16_t targetvalue;	120	int16_t targetvalue;
121	for (i=0; i<8; i++) {	121	for (i=0; i<8; i++) {
122	targetvalue = RC_getVariable(i);	122	targetvalue = RC_getVariable(i);
123	if (targetvalue < 0) targetvalue = 0;	123	if (targetvalue < 0) targetvalue = 0;
124	if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;	124	if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
125	}	125	}
126	}	126	}
127		127
128	uint8_t controlMixer_getSignalQuality(void) {	128	uint8_t controlMixer_getSignalQuality(void) {
129	uint8_t rcQ = RC_getSignalQuality();	129	uint8_t rcQ = RC_getSignalQuality();
130	uint8_t ecQ = EC_getSignalQuality();	130	uint8_t ecQ = EC_getSignalQuality();
131	DebugOut.Analog[16] = rcQ;	-
132	DebugOut.Analog[17] = ecQ;	-
133	// This needs not be the only correct solution...	131	// This needs not be the only correct solution...
134	return rcQ > ecQ ? rcQ : ecQ;	132	return rcQ > ecQ ? rcQ : ecQ;
135	}	133	}
136		134
137	/*	135	/*
138	* Update the variables indicating stick position from the sum of R/C, GPS and external control.	136	* Update the variables indicating stick position from the sum of R/C, GPS and external control.
139	*/	137	*/
140	void controlMixer_update(void) {	138	void controlMixer_update(void) {
141	// calculate Stick inputs by rc channels (P) and changing of rc channels (D)	139	// calculate Stick inputs by rc channels (P) and changing of rc channels (D)
142	// TODO: If no signal --> zero.	140	// TODO: If no signal --> zero.
-		141	uint8_t axis;
143	RC_update();	142	RC_update();
144	EC_update();	143	EC_update();
145		144
146	int16_t* RC_PRTY = RC_getPRTY();	145	int16_t* RC_PRTY = RC_getPRTY();
147	int16_t* EC_PRTY = EC_getPRTY();	146	int16_t* EC_PRTY = EC_getPRTY();
148		147
149	controlPitch = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];	148	control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
150	controlRoll = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];	149	control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
151	controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];	150	controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
152	controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];	151	controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
153		-
154	DebugOut.Analog[6] = controlPitch;	-
155	DebugOut.Analog[7] = controlRoll;	-
156	DebugOut.Analog[8] = controlYaw;	-
157		152
158	if (RC_getSignalQuality() >= SIGNAL_GOOD) {	153	if (RC_getSignalQuality() >= SIGNAL_GOOD) {
159	controlMixer_updateVariables();	154	controlMixer_updateVariables();
160	configuration_applyVariablesToParams();	155	configuration_applyVariablesToParams();
161	looping = RC_getLooping(looping);	156	looping = RC_getLooping(looping);
162	} else { // Signal is not OK	157	} else { // Signal is not OK
163	// Could handle switch to emergency flight here.	158	// Could handle switch to emergency flight here.
164	// throttle is handled elsewhere.	159	// throttle is handled elsewhere.
165	looping = 0;	160	looping = 0;
166	}	161	}
167		162
168	DebugOut.Analog[18] = (int16_t)looping;	163	DebugOut.Analog[18] = (int16_t)looping;
169		164
170	// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)	165	// (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
171	// TODO: Correct, for changed range (stick gain + expo is now applied already)	166	// TODO: Correct, for changed range (stick gain + expo is now applied already)
172	if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE \| CFG_GPS_ACTIVE)) {	167	if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE \| CFG_GPS_ACTIVE)) {
173	if (controlYaw > 2) controlYaw-= 2;	168	if (controlYaw > 2) controlYaw-= 2;
174	else if (controlYaw< -2) controlYaw += 2;	169	else if (controlYaw< -2) controlYaw += 2;
175	else controlYaw = 0;	170	else controlYaw = 0;
176	}	171	}
177		172
178	/*	173	/*
179	* Record maxima	174	* Record maxima
180	*/	175	*/
-		176	for (axis=PITCH; axis<=ROLL; axis++) {
181	if(abs(controlPitch / STICK_GAIN) > maxControlPitch) {	177	if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
182	maxControlPitch = abs(controlPitch) / STICK_GAIN;	178	maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
183	if(maxControlPitch > 100) maxControlPitch = 100;	179	if(maxControl[axis] > 100) maxControl[axis] = 100;
184	} else if (maxControlPitch) maxControlPitch--;	180	} else if (maxControl[axis]) maxControl[axis]--;
185	if(abs(controlRoll / STICK_GAIN) > maxControlRoll) {	-
186	maxControlRoll = abs(controlRoll) / STICK_GAIN;	-
187	if(maxControlRoll > 100) maxControlRoll = 100;	-
188	}	181	}
189	else if (maxControlRoll) maxControlRoll--;	-
190		182
191	// Here we could blend in other sources.	183	// Here we could blend in other sources.
192	uint8_t commandNow = RC_getCommand();	184	uint8_t commandNow = RC_getCommand();
193		185
194	if (commandNow != lastCommand) {	186	if (commandNow != lastCommand) {
195	lastCommand = commandNow;	187	lastCommand = commandNow;
196	commandTimer = 0;	188	commandTimer = 0;
197	} else if (commandTimer < COMMAND_TIMER + 1)	189	} else if (commandTimer < COMMAND_TIMER + 1)
198	commandTimer++;	190	commandTimer++;
199	}	191	}
200		192
201	/*	193	/*
202	void setCompassCalState(void) {	194	void setCompassCalState(void) {
203	static uint8_t stick = 1;	195	static uint8_t stick = 1;
204	// if pitch is centered or top set stick to zero	196	// if pitch is centered or top set stick to zero
205	if(RCChannel(CH_PITCH) > -20) stick = 0;	197	if(RCChannel(CH_PITCH) > -20) stick = 0;
206	// if pitch is down trigger to next cal state	198	// if pitch is down trigger to next cal state
207	if((RCChannel(CH_PITCH) < -70) && !stick) {	199	if((RCChannel(CH_PITCH) < -70) && !stick) {
208	stick = 1;	200	stick = 1;
209	compassCalState++;	201	compassCalState++;
210	if(compassCalState < 5) beepNumber(compassCalState);	202	if(compassCalState < 5) beepNumber(compassCalState);
211	else beep(1000);	203	else beep(1000);
212	}	204	}
213	}	205	}
214	*/	206	*/
215		207