Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) Holger Buss, Ingo Busker

// + Nur für den privaten Gebrauch

// + www.MikroKopter.com

// + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + 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 <avr/interrupt.h>

#include "rc.h"

#include "eeprom.h"

#include "main.h"

#include "fc.h"

#include "uart0.h"

//#define ACT_S3D_SUMSIGNAL

volatile uint8_t RC_Channels;   // number of received channels

volatile int16_t PPM_in[MAX_CHANNELS]; //PPM24 supports 12 channels per frame

volatile int16_t PPM_diff[MAX_CHANNELS];

volatile uint8_t NewPpmData = 1;

volatile uint8_t RC_Quality = 0;

volatile uint8_t RC_RSSI = 0; // Received Signal Strength Indication

/***************************************************************/

/*  16bit timer 1 is used to decode the PPM-Signal            */

/***************************************************************/

void RC_Init (void)

{

        uint8_t sreg = SREG;

        // disable all interrupts before reconfiguration

        cli();

        // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1

        DDRD &= ~(1<<DDD6);

        PORTD |= (1<<PORTD6);

        // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)

        // set as output

        DDRD |= (1<<DDD5)|(1<<DDD4);

        // low level

        PORTD &= ~((1<<PORTD5)|(1<<PORTD4));

        // PD3 can't be used if 2nd UART is activated

        // because TXD1 is at that port

        if(CPUType != ATMEGA644P)

        {

                DDRD |= (1<<PORTD3);

                PORTD &= ~(1<<PORTD3);

        }

        // Timer/Counter1 Control Register A, B, C

        // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)

        // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)

        TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10));

#ifndef ACT_S3D_SUMSIGNAL

        // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)

        // Enable input capture noise cancler (bit: ICNC1=1)

        // Trigger on positive edge of the input capture pin (bit: ICES1=1),

        // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs

        // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.

        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12));

        TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1);

#else

        // Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=0)

        // Enable input capture noise cancler (bit: ICNC1=1)

        // Trigger on positive edge of the input capture pin (bit: ICES1=1),

        // Therefore the counter incremets at a clock of 20 MHz/8 = 2.5 MHz or 0.4µs

    // The longest period is 0xFFFF / 2.5 MHz = 0.026214 s.

        TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12)|(1<<CS10));

        TCCR1B |= (1<<CS11)|(1<<ICES1)|(1<<ICNC1);

#endif

        TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B));

        // Timer/Counter1 Interrupt Mask Register

        // Enable Input Capture Interrupt (bit: ICIE1=1)

        // Disable Output Compare A & B Match Interrupts and Overflow Interrupt (bit: OCIE1B=0, OICIE1A=0, TOIE1=0)

        TIMSK1 &= ~((1<<OCIE1B)|(1<<OCIE1A)|(1<<TOIE1));

    PPM_INPUT_ON; //enabled by default

    SREG = sreg;

}

/********************************************************************/

/*         Every time a positive edge is detected at PD6            */

/********************************************************************/

/*                               t-Frame

       <----------------------------------------------------------------------->

         ____   ______   _____   ________                ___   ________________   ____

        |    | |      | |     | |        |              |   | |                | |

        |    | |      | |     | |        |              |   | |    sync gap    | |

     ___|    |_|      |_|     |_|        |_............_|   |_|                |_|

        <-----><-------><------><-------->              <----->                   <---

          t0       t1      t2       t4                     tn                       t0

The PPM-Frame length is 22.5 ms.

Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.

The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.

The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.

The minimum duration of all channels at minimum value is  8 * 1 ms = 8 ms.

The maximum duration of all channels at maximum value is  8 * 2 ms = 16 ms.

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.

*/

#ifndef ACT_S3D_SUMSIGNAL

ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms

{

    int16_t signal = 0, tmp;

    uint8_t i;

        static uint8_t index = 1;

        static uint16_t oldICR1 = 0;

        // 16bit Input Capture Register ICR1 contains the timer value TCNT1

        // at the time the edge was detected

        // calculate the time delay to the previous event time which is stored in oldICR1

        // calculatiing the difference of the two uint16_t and converting the result to an int16_t

        // implicit handles a timer overflow 65535 -> 0 the right way.

        signal = (uint16_t) ICR1 - oldICR1;

        oldICR1 = ICR1;

        if(ParamSet.Config2 & CFG2_SENSITIVE_RC)

        {

                static int16_t old_ppm_in[MAX_CHANNELS];

                static int16_t ppm_in[MAX_CHANNELS];

                static int16_t ppm_diff[MAX_CHANNELS];

                static uint8_t okay_cnt = 0;

                //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

                        // and the number of channes decoded since the last sync gap matches the expectation

                        // then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.

                        index--; // no next channel because the sync gap was detected

                        if(index >= 4 && index == RC_Channels)

                        {

                                if(okay_cnt > 10) // at least 10 frames in line ok

                                {

                                        for(i=0; i < MAX_CHANNELS; i++)

                                        {

                                                if(okay_cnt > 30) // at least 30 frames ok

                                                {

                                                        old_ppm_in[i] = PPM_in[i]; // backup data

                                                }

                                                PPM_in[i] = ppm_in[i];

                                                PPM_diff[i] = ppm_diff[i];

                                        }

                                        NewPpmData = 0;  // Null means NewData for at least the first 4 channels

                                }

                                if(okay_cnt < 255) okay_cnt++;

                        } // eof if(index >= 4 && index == RC_Channels)

                        else

                        {

                                if(okay_cnt > 100) okay_cnt = 10;

                                else okay_cnt = 0;

                                RED_ON;

                        }

                        // store max channels transmitted

                        if(!(MKFlags & MKFLAG_MOTOR_RUN)) RC_Channels = index;

                        // reset channel index

                        index = 1;

                }

                else // assuming within the PPM frame

                {

                        if(index < MAX_CHANNELS)

                        {

                                // check for valid signal length (0.8 ms < signal < 2.1984 ms)

                                // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625

                                if((signal > 250) && (signal < 687))

                                {

                                        // shift signal to zero symmetric range  -154 to 159

                                        signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)

                                        // check for stable signal

                                        if(abs(signal - ppm_in[index]) < 6)

                                        {

                                                if(okay_cnt > 25) RC_Quality +=10;

                                                else if(okay_cnt > 10) RC_Quality += 2;

                                                if(RC_Quality > 200) RC_Quality = 200;

                                        }

                                        // calculate exponential history for signal

                                        tmp = (3 * (ppm_in[index]) + signal) / 4;

                                        if(tmp > signal+1) tmp--; else

                                        if(tmp < signal-1) tmp++;

                                        // calculate signal difference on good signal level

                                        if(RC_Quality >= 190)  ppm_diff[index] = ((tmp - ppm_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction

                                        else ppm_diff[index] = 0;

                                        ppm_in[index] = tmp; // update channel value

                                }

                                else // invalid signal lenght

                                {

                                        RED_ON;

                                }

                                // demux sum signal for channels 5 to 7 to J3, J4, J5

                                if(index == 5) J3HIGH; else J3LOW;

                                if(index == 6) J4HIGH; else J4LOW;

                                if(CPUType != ATMEGA644P) // not used as TXD1

                                {

                                        if(index == 7) J5HIGH; else J5LOW;

                                }

                                index++; // next channel

                        }

                        else // (index >= MAX_CHANNELS)

                        {

                                RED_ON;

                                if(index == MAX_CHANNELS)

                                {

                                        for(i = 0; i < MAX_CHANNELS; i++)

                                        {

                                                PPM_in[i] = old_ppm_in[i]; // restore old valid values

                                                PPM_diff[i] = 0;

                                        }

                                        index++;

                                }

                        } // eof (index >= MAX_CHANNELS)

                } // eof within the PPM frame

        } // eof sensitive rc

        else // old more tolerant version

        {

                //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.

                        index--; // no next channel because the sync gap was detected

                        RC_Channels = index;

                        if(index >= 4)

                        {

                                NewPpmData = 0;  // Null means NewData for the first 4 channels

                        }

                        // synchronize channel index

                        index = 1;

                }

                else // within the PPM frame

                {

                        if(index < MAX_CHANNELS)

                        {

                                // check for valid signal length (0.8 ms < signal < 2.1984 ms)

                                // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625

                                if((signal > 250) && (signal < 687))

                                {

                                        // shift signal to zero symmetric range  -154 to 159

                                        signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)

                                        // check for stable signal

                                        if(abs(signal - PPM_in[index]) < 6)

                                        {

                                                if(RC_Quality < 200) RC_Quality +=10;

                                                else RC_Quality = 200;

                                        }

                                        // calculate exponential history for signal

                                        tmp = (3 * (PPM_in[index]) + signal) / 4;

                                        if(tmp > signal+1) tmp--; else

                                        if(tmp < signal-1) tmp++;

                                        // calculate signal difference on good signal level

                                        if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction

                                        else PPM_diff[index] = 0;

                                        PPM_in[index] = tmp; // update channel value

                                }

                                index++; // next channel

                                // demux sum signal for channels 5 to 7 to J3, J4, J5

                                if(index == 5) J3HIGH; else J3LOW;

                                if(index == 6) J4HIGH; else J4LOW;

                                if(CPUType != ATMEGA644P) // not used as TXD1

                                {

                                        if(index == 7) J5HIGH; else J5LOW;

                                }

                        } // eof (index < MAX_CHANNELS)

                } // eof within the PPM frame

        } // eof old more tolerant version

}

#else // ACT_S3D_SUMSIGNAL

ISR(TIMER1_CAPT_vect) // typical rate of 1 ms to 2 ms

{

        int16_t signal = 0, tmp;

        uint8_t i;

        static uint8_t index = 1;

        static uint16_t oldICR1 = 0;

        // 16bit Input Capture Register ICR1 contains the timer value TCNT1

        // at the time the edge was detected

        // calculate the time delay to the previous event time which is stored in oldICR1

        // calculatiing the difference of the two uint16_t and converting the result to an int16_t

        // implicit handles a timer overflow 65535 -> 0 the right way.

        signal = (uint16_t) ICR1 - oldICR1;

        signal /= 2;

        oldICR1 = ICR1;

        //sync gap? (3.52 ms < signal < 25.6 ms)

        if((signal > 1100*2) && (signal < 8000*2))

        {

                // 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.

                index--; // no next channel because the sync gap was detected

                RC_Channels = index;

                if(index >= 4)

                {

                        NewPpmData = 0;  // Null means NewData for the first 4 channels

                }

                // synchronize channel index

                index = 1;

        }

        else // within the PPM frame

        {

                if(index < MAX_CHANNELS)

                {

                        // check for valid signal length (0.8 ms < signal < 2.1984 ms)

                        // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625

                        if((signal > 250) && (signal < 687*2))

                        {

                                // shift signal to zero symmetric range  -154 to 159

                                signal -= 962; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)

                                // check for stable signal

                                if(abs(signal - PPM_in[index]) < 6)

                                {

                                        if(RC_Quality < 200) RC_Quality +=10;

                                        else RC_Quality = 200;

                                }

                                // calculate exponential history for signal

                                tmp = (3 * (PPM_in[index]) + signal) / 4;

                                if(tmp > signal+1) tmp--; else

                                if(tmp < signal-1) tmp++;

                                // calculate signal difference on good signal level

                                if(RC_Quality >= 195)  PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction

                                else PPM_diff[index] = 0;

                                PPM_in[index] = tmp; // update channel value

                        }

                        index++; // next channel

                } // eof (index < MAX_CHANNELS)

        } // eof within the PPM frame

}

#endif