diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index 2bc8a20cf4..1d6d1f8124 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -1,34 +1,34 @@ /* - APM_RC_APM1.cpp - Radio Control Library for Ardupilot Mega. Arduino - Code by Jordi Muņoz and Jose Julio. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - RC Input : PPM signal on IC4 pin - RC Output : 11 Servo outputs (standard 20ms frame) - - Methods: - Init() : Initialization of interrupts an Timers - OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 - InputCh(ch) : Read a channel input value. ch=0..7 - GetState() : Returns the state of the input. 1 => New radio frame to process - Automatically resets when we call InputCh to read channels - -*/ + * APM_RC_APM1.cpp - Radio Control Library for Ardupilot Mega. Arduino + * Code by Jordi Muņoz and Jose Julio. DIYDrones.com + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * RC Input : PPM signal on IC4 pin + * RC Output : 11 Servo outputs (standard 20ms frame) + * + * Methods: + * Init() : Initialization of interrupts an Timers + * OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10 + * InputCh(ch) : Read a channel input value. ch=0..7 + * GetState() : Returns the state of the input. 1 => New radio frame to process + * Automatically resets when we call InputCh to read channels + * + */ #include "APM_RC_APM1.h" #include #if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" + #include "Arduino.h" #else - #include "WProgram.h" + #include "WProgram.h" #endif #if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) -# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. + # error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. #else // Variable definition for Input Capture interrupt @@ -36,37 +36,37 @@ volatile uint16_t APM_RC_APM1::_PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,240 volatile uint8_t APM_RC_APM1::_radio_status=0; /**************************************************** - Input Capture Interrupt ICP4 => PPM signal read - ****************************************************/ +* Input Capture Interrupt ICP4 => PPM signal read +****************************************************/ void APM_RC_APM1::_timer4_capt_cb(void) { - static uint16_t ICR4_old; - static uint8_t PPM_Counter=0; - - uint16_t Pulse; - uint16_t Pulse_Width; - - Pulse=ICR4; - if (Pulse8000) { // SYNC pulse? - PPM_Counter=0; - } - else { - if (PPM_Counter < NUM_CHANNELS) { // Valid pulse channel? - _PWM_RAW[PPM_Counter++]=Pulse_Width; // Saving pulse. - - if (PPM_Counter >= NUM_CHANNELS) { - _radio_status = 1; - } + static uint16_t ICR4_old; + static uint8_t PPM_Counter=0; + + uint16_t Pulse; + uint16_t Pulse_Width; + + Pulse=ICR4; + if (Pulse8000) { // SYNC pulse? + PPM_Counter=0; + } + else { + if (PPM_Counter < NUM_CHANNELS) { // Valid pulse channel? + _PWM_RAW[PPM_Counter++]=Pulse_Width; // Saving pulse. + + if (PPM_Counter >= NUM_CHANNELS) { + _radio_status = 1; + } + } + } + ICR4_old = Pulse; } @@ -80,69 +80,69 @@ APM_RC_APM1::APM_RC_APM1() void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg ) { - isr_reg->register_signal(ISR_REGISTRY_TIMER4_CAPT, _timer4_capt_cb ); - - // Init PWM Timer 1 - pinMode(11,OUTPUT); //OUT9 (PB5/OC1A) - pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) - pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) - - //Remember the registers not declared here remains zero by default... - TCCR1A =((1<register_signal(ISR_REGISTRY_TIMER4_CAPT, _timer4_capt_cb ); + + // Init PWM Timer 1 + pinMode(11,OUTPUT); //OUT9 (PB5/OC1A) + pinMode(12,OUTPUT); //OUT2 (PB6/OC1B) + pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1<>1; + uint16_t pwm=0; + switch(ch) { + case 0: pwm=OCR5B; break; //ch1 + case 1: pwm=OCR5C; break; //ch2 + case 2: pwm=OCR1B; break; //ch3 + case 3: pwm=OCR1C; break; //ch4 + case 4: pwm=OCR4C; break; //ch5 + case 5: pwm=OCR4B; break; //ch6 + case 6: pwm=OCR3C; break; //ch7 + case 7: pwm=OCR3B; break; //ch8 + case 8: pwm=OCR5A; break; //ch9, PL3 + case 9: pwm=OCR1A; break; //ch10, PB5 + case 10: pwm=OCR3A; break; //ch11, PE3 + } + return pwm>>1; } void APM_RC_APM1::enable_out(uint8_t ch) { - switch(ch){ + switch(ch) { case 0: TCCR5A |= (1<>= 1; + // Because timer runs at 0.5us we need to do value/2 + result >>= 1; - // Limit values to a valid range - result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); - _radio_status = 0; // Radio channel read - return result; + // Limit values to a valid range + result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH); + _radio_status = 0; // Radio channel read + return result; } uint8_t APM_RC_APM1::GetState(void) { - return(_radio_status); + return(_radio_status); } @@ -248,51 +248,51 @@ void APM_RC_APM1::Force_Out(void) // This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use void APM_RC_APM1::Force_Out0_Out1(void) { - if (TCNT5>5000) // We take care that there are not a pulse in the output - TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 + if (TCNT5>5000) // We take care that there are not a pulse in the output + TCNT5=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 } // This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use void APM_RC_APM1::Force_Out2_Out3(void) { - if (TCNT1>5000) - TCNT1=39990; + if (TCNT1>5000) + TCNT1=39990; } // This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use void APM_RC_APM1::Force_Out6_Out7(void) { - if (TCNT3>5000) - TCNT3=39990; + if (TCNT3>5000) + TCNT3=39990; } /* --------------------- OUTPUT SPEED CONTROL --------------------- */ void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz) { - uint16_t icr = _map_speed(speed_hz); - - if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) { - ICR1 = icr; - } - - if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) { - ICR5 = icr; - } - -#if 0 - if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0) { - /* These channels intentionally left blank: - * Can't change output speed of ch5 (OCR4B) and ch6 (OCR4C). - * Timer 4 period controlled by OCR4A, and used for input - * capture on ICR4. - * If the period of Timer 4 must be changed, the input capture - * code will have to be adjusted as well - */ - } -#endif + uint16_t icr = _map_speed(speed_hz); - if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) { - ICR3 = icr; - } + if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) { + ICR1 = icr; + } + + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) { + ICR5 = icr; + } + + #if 0 + if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0) { + /* These channels intentionally left blank: + * Can't change output speed of ch5 (OCR4B) and ch6 (OCR4C). + * Timer 4 period controlled by OCR4A, and used for input + * capture on ICR4. + * If the period of Timer 4 must be changed, the input capture + * code will have to be adjusted as well + */ + } + #endif + + if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) { + ICR3 = icr; + } } @@ -301,28 +301,28 @@ void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz) // A value of 0 means no override, use the real RC values bool APM_RC_APM1::setHIL(int16_t v[NUM_CHANNELS]) { - uint8_t sum = 0; - for (uint8_t i=0; i