1,53 → 1,3 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + 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 example: 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 <avr/io.h> |
#include <avr/interrupt.h> |
58,12 → 8,10 |
#include "configuration.h" |
#include "commands.h" |
|
// The channel array is now 0-based. |
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile int16_t PPM_diff[MAX_CHANNELS]; |
volatile uint8_t newPPMData = 1; |
volatile uint8_t RCQuality; |
int16_t RC_PRTY[4]; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
|
140,8 → 88,7 |
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
the syncronization gap. |
*/ |
ISR(TIMER1_CAPT_vect) |
{ // typical rate of 1 ms to 2 ms |
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
157,12 → 104,6 |
|
//sync gap? (3.52 ms < signal < 25.6 ms) |
if ((signal > 1100) && (signal < 8000)) { |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if (index >= 3) { |
newPPMData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 0; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
237,30 → 178,23 |
} |
|
/* |
* This must be called (as the only thing) for each control loop cycle (488 Hz). |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
void RC_periodicTask() { |
void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
int16_t tmp1, tmp2; |
if (RCQuality) { |
RCQuality--; |
if (newPPMData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
+ RCDiff(CH_ROLL) * staticParams.stickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
* staticParams.stickThrottleD + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) |
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) |
/ 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
RC_PRTY[CONTROL_YAW] = tmp2; |
} |
PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP + RCDiff(CH_PITCH) * staticParams.stickD; |
PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP + RCDiff(CH_ROLL) * staticParams.stickD; |
PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * staticParams.stickThrottleD + 120; |
if (PRTY[CONTROL_THROTTLE] < 0) PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
PRTY[CONTROL_YAW] = tmp2; |
|
uint8_t command = RC_getStickCommand(); |
|
if (lastRCCommand == command) { |
// Keep timer from overrunning. |
if (commandTimer < COMMAND_TIMER) |
270,20 → 204,11 |
lastRCCommand = command; |
commandTimer = 0; |
} |
} else { // Bad signal |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
} |
debugOut.analog[18] = RCQuality; |
} |
|
/* |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
int16_t* RC_getPRTY(void) { |
return RC_PRTY; |
} |
|
/* |
* Get other channel value |
*/ |
int16_t RC_getVariable(uint8_t varNum) { |
384,53 → 309,16 |
} |
|
/* |
uint8_t RC_getLooping(uint8_t looping) { |
// static uint8_t looping = 0; |
|
if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
& CFG_LOOP_LEFT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
} else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
} |
|
if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_RIGHT) { |
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
} else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
} |
|
if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_UP) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
} else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
< staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
} |
|
if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
&& staticParams.BitConfig & CFG_LOOP_DOWN) { |
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
} else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
} |
|
return looping; |
} |
*/ |
|
* For each time the stick is pulled, returns true. |
*/ |
uint8_t RC_testCompassCalState(void) { |
static uint8_t stick = 1; |
static uint8_t stickPulled = 1; |
// if pitch is centered or top set stick to zero |
if (RCChannel(CH_PITCH) > -20) |
stick = 0; |
stickPulled = 0; |
// if pitch is down trigger to next cal state |
if ((RCChannel(CH_PITCH) < -70) && !stick) { |
stick = 1; |
if ((RCChannel(CH_PITCH) < -70) && !stickPulled) { |
stickPulled = 1; |
return 1; |
} |
return 0; |