Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2047 → Rev 2048

/branches/dongfang_FC_rewrite/rc.c
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;