| 0,0 → 1,351 |
|---|
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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 <avr/io.h> |
| #include <avr/interrupt.h> |
| #include "fc.h" |
| #include "eeprom.h" |
| #include "uart0.h" |
| #include "main.h" |
| #include "rc.h" |
| |
| volatile int16_t ServoNickValue = 0; |
| volatile int16_t ServoRollValue = 0; |
| volatile uint8_t ServoActive = 0; |
| |
| #define HEF4017R_ON PORTC |= (1<<PORTC6) |
| #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
| |
| |
| /*****************************************************/ |
| /* Initialize Timer 2 */ |
| /*****************************************************/ |
| // The timer 2 is used to generate the PWM at PD7 (J7) |
| // to control a camera servo for nick compensation. |
| void TIMER2_Init(void) |
| { |
| uint8_t sreg = SREG; |
| |
| // disable all interrupts before reconfiguration |
| cli(); |
| |
| // set PD7 as output of the PWM for nick servo |
| DDRD |= (1<<DDD7); |
| PORTD &= ~(1<<PORTD7); // set PD7 to low |
| |
| DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
| //PORTC &= ~(1<<PORTC6); // set PC6 to low |
| HEF4017R_ON; // enable reset |
| |
| // Timer/Counter 2 Control Register A |
| |
| // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1) |
| // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
| // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
| TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
| TCCR2A |= (1<<WGM21)|(1<<WGM20); |
| |
| // Timer/Counter 2 Control Register B |
| |
| // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
| // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
| // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
| |
| // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
| TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22)); |
| TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22); |
| |
| // Initialize the Timer/Counter 2 Register |
| TCNT2 = 0; |
| |
| // Initialize the Output Compare Register A used for PWM generation on port PD7. |
| OCR2A = 255; |
| TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0 |
| |
| // Timer/Counter 2 Interrupt Mask Register |
| // Enable timer output compare match A Interrupt only |
| TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
| TIMSK2 |= (1<<OCIE2A); |
| |
| SREG = sreg; |
| } |
| |
| |
| void Servo_On(void) |
| { |
| ServoActive = 1; |
| } |
| |
| void Servo_Off(void) |
| { |
| ServoActive = 0; |
| HEF4017R_ON; // enable reset |
| } |
| |
| /*****************************************************/ |
| /* Control Servo Position */ |
| /*****************************************************/ |
| |
| ISR(TIMER2_COMPA_vect) |
| { |
| |
| // frame len 22.5 ms = 14063 * 1.6 us |
| // stop pulse: 0.3 ms = 188 * 1.6 us |
| // min servo pulse: 0.6 ms = 375 * 1.6 us |
| // max servo pulse: 2.4 ms = 1500 * 1.6 us |
| // resolution: 1500 - 375 = 1125 steps |
| |
| #define IRS_RUNTIME 127 |
| #define PPM_STOPPULSE 188 |
| //#define PPM_FRAMELEN 14063 |
| #define PPM_FRAMELEN (1757 * ParamSet.ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
| #define MINSERVOPULSE 375 |
| #define MAXSERVOPULSE 1500 |
| #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
| |
| static uint8_t PulseOutput = 0; |
| static uint16_t RemainingPulse = 0; |
| static uint16_t ServoFrameTime = 0; |
| static uint8_t ServoIndex = 0; |
| |
| #define MULTIPLYER 4 |
| static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
| static int16_t ServoRollOffset = (255 / 2) * MULTIPLYER; // initial value near center position |
| |
| if(BoardRelease < 20) |
| { |
| //--------------------------- |
| // Nick servo state machine |
| //--------------------------- |
| if(!PulseOutput) // pulse output complete |
| { |
| if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
| { |
| TCCR2A &= ~(1<<COM2A0);// make a high pulse |
| RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| |
| ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoNickCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| |
| RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| |
| ServoNickValue /= MULTIPLYER; |
| DebugOut.Analog[20] = ServoNickValue; |
| |
| // range servo pulse width |
| if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
| else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
| // accumulate time for correct update rate |
| ServoFrameTime = RemainingPulse; |
| } |
| else // we had a high pulse |
| { |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
| } |
| // set pulse output active |
| PulseOutput = 1; |
| } |
| } // EOF Nick servo state machine |
| else |
| { |
| //----------------------------------------------------- |
| // PPM state machine, onboard demultiplexed by HEF4017 |
| //----------------------------------------------------- |
| if(!PulseOutput) // pulse output complete |
| { |
| if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
| { |
| TCCR2A &= ~(1<<COM2A0);// make a high pulse |
| |
| if(ServoIndex == 0) // if we are at the sync gap |
| { |
| RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
| ServoFrameTime = 0; // reset servo frame time |
| HEF4017R_ON; // enable HEF4017 reset |
| } |
| else // servo channels |
| { |
| RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| switch(ServoIndex) // map servo channels |
| { |
| case 1: // Nick Compensation Servo |
| ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| if(ParamSet.ServoNickCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| else |
| { // non inverting movement of servo |
| ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| } |
| // limit servo value to its parameter range definition |
| if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| |
| RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| |
| ServoNickValue /= MULTIPLYER; |
| DebugOut.Analog[20] = ServoNickValue; |
| break; |
| |
| |
| case 2: // Roll Compensation Servo |
| ServoRollOffset = (ServoRollOffset * 3 + (int16_t)80 * MULTIPLYER) / 4; // lowpass offset |
| ServoRollValue = ServoRollOffset; // offset (Range from 0 to 255 * 3 = 765) |
| //if(ParamSet.ServoRollCompInvert & 0x01) |
| { // inverting movement of servo |
| ServoRollValue += (int16_t)( ( (int32_t) 50 * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
| } |
| /* else |
| { // non inverting movement of servo |
| ServoRollValue -= (int16_t)( ( (int32_t) 40 * MULTIPLYER * (IntegralGyroRoll / 128L ) ) / (256L) ); |
| } |
| */ // limit servo value to its parameter range definition |
| if(ServoRollValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| } |
| else |
| if(ServoRollValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| { |
| ServoRollValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
| } |
| RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoRollValue to center position |
| ServoRollValue /= MULTIPLYER; |
| //DebugOut.Analog[20] = ServoRollValue; |
| break; |
| |
| default: // other servo channels |
| RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
| break; |
| } |
| // range servo pulse width |
| if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
| else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
| // substract stop pulse width |
| RemainingPulse -= PPM_STOPPULSE; |
| // accumulate time for correct sync gap |
| ServoFrameTime += RemainingPulse; |
| } |
| } |
| else // we had a high pulse |
| { |
| TCCR2A |= (1<<COM2A0); // make a low pulse |
| // set pulsewidth to stop pulse width |
| RemainingPulse = PPM_STOPPULSE; |
| // accumulate time for correct sync gap |
| ServoFrameTime += RemainingPulse; |
| if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset |
| ServoIndex++; // change to next servo channel |
| if(ServoIndex > ParamSet.ServoRefresh) ServoIndex = 0; // reset to the sync gap |
| } |
| // set pulse output active |
| PulseOutput = 1; |
| } |
| } // EOF PPM state machine |
| |
| // General pulse output generator |
| if(RemainingPulse > (255 + IRS_RUNTIME)) |
| { |
| OCR2A = 255; |
| RemainingPulse -= 255; |
| } |
| else |
| { |
| if(RemainingPulse > 255) // this is the 2nd last part |
| { |
| if((RemainingPulse - 255) < IRS_RUNTIME) |
| { |
| OCR2A = 255 - IRS_RUNTIME; |
| RemainingPulse -= 255 - IRS_RUNTIME; |
| |
| } |
| else // last part > ISR_RUNTIME |
| { |
| OCR2A = 255; |
| RemainingPulse -= 255; |
| } |
| } |
| else // this is the last part |
| { |
| OCR2A = RemainingPulse; |
| RemainingPulse = 0; |
| PulseOutput = 0; // trigger to stop pulse |
| } |
| } // EOF general pulse output generator |
| |
| } |