// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "heightControl.h"
#include "attitudeControl.h"
#include "externalControl.h"
#include "configuration.h"
#include "attitude.h"
#include "commands.h"
#include "output.h"
uint16_t maxControl
[2] = { 0, 0 };
int16_t control
[2] = { 0, 0 };
int16_t 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 lastArgument
;
uint8_t isCommandRepeated
= 0;
// MK flags. TODO: Replace by enum. State machine.
uint16_t isFlying
= 0;
volatile uint8_t MKFlags
= 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
() {
EC_setNeutral
();
HC_setGround
();
}
/*
* 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.
* TODO: It assumes R/C as source. Not necessarily true.
*/
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
;
// takes almost no time...
RC_update
();
// takes almost no time...
EC_update
();
// takes about 80 usec.
HC_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
];
// This can be a CPU time killer if the function implementations are inefficient.
controlThrottle
= HC_getThrottle
(AC_getThrottle
(RC_PRTY
[CONTROL_THROTTLE
]
+ EC_PRTY
[CONTROL_THROTTLE
]));
controlYaw
= RC_PRTY
[CONTROL_YAW
] + EC_PRTY
[CONTROL_YAW
];
DebugOut.
Analog[12] = control
[PITCH
];
DebugOut.
Analog[13] = control
[ROLL
];
//DebugOut.Analog[26] = controlYaw;
if (controlMixer_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;
}
// part1a end.
/* This is not really necessary with the dead-gap 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
*/
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
]--;
}
uint8_t rcCommand
= (RC_getSignalQuality
() >= SIGNAL_OK
) ? RC_getCommand
()
: COMMAND_NONE
;
uint8_t ecCommand
= (EC_getSignalQuality
() >= SIGNAL_OK
) ? EC_getCommand
()
: COMMAND_NONE
;
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
;
}
if (isCommandRepeated
)
DebugOut.
Digital[0] |= DEBUG_COMMANDREPEATED
;
else
DebugOut.
Digital[0] &= ~DEBUG_COMMANDREPEATED
;
if (rcCommand
)
DebugOut.
Digital[1] |= DEBUG_COMMANDREPEATED
;
else
DebugOut.
Digital[1] &= ~DEBUG_COMMANDREPEATED
;
// part1 end.
}
// TODO: Integrate into command system.
uint8_t controlMixer_testCompassCalState
(void) {
return RC_testCompassCalState
();
}