diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp index 2e47d7cdc2..472358c9e3 100644 --- a/libraries/APM_RC/APM_RC_APM2.cpp +++ b/libraries/APM_RC/APM_RC_APM2.cpp @@ -1,34 +1,34 @@ /* - APM_RC_APM2.cpp - Radio Control Library for Ardupilot Mega 2.0. 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_APM2.cpp - Radio Control Library for Ardupilot Mega 2.0. 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_APM2.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,38 +36,38 @@ volatile uint16_t APM_RC_APM2::_PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,240 volatile uint8_t APM_RC_APM2::_radio_status=0; /**************************************************** - Input Capture Interrupt ICP5 => PPM signal read - ****************************************************/ +* Input Capture Interrupt ICP5 => PPM signal read +****************************************************/ void APM_RC_APM2::_timer5_capt_cb(void) { - static uint16_t prev_icr; - static uint8_t frame_idx; - uint16_t icr; - uint16_t pwidth; + static uint16_t prev_icr; + static uint8_t frame_idx; + uint16_t icr; + uint16_t pwidth; - icr = ICR5; - // Calculate pulse width assuming timer overflow TOP = 40000 - if ( icr < prev_icr ) { - pwidth = ( icr + 40000 ) - prev_icr; - } else { - pwidth = icr - prev_icr; - } - - // Was it a sync pulse? If so, reset frame. - if ( pwidth > 8000 ) { - frame_idx=0; - } else { - // Save pulse into _PWM_RAW array. - if ( frame_idx < NUM_CHANNELS ) { - _PWM_RAW[ frame_idx++ ] = pwidth; - // If this is the last pulse in a frame, set _radio_status. - if (frame_idx >= NUM_CHANNELS) { - _radio_status = 1; - } + icr = ICR5; + // Calculate pulse width assuming timer overflow TOP = 40000 + if ( icr < prev_icr ) { + pwidth = ( icr + 40000 ) - prev_icr; + } else { + pwidth = icr - prev_icr; } - } - // Save icr for next call. - prev_icr = icr; + + // Was it a sync pulse? If so, reset frame. + if ( pwidth > 8000 ) { + frame_idx=0; + } else { + // Save pulse into _PWM_RAW array. + if ( frame_idx < NUM_CHANNELS ) { + _PWM_RAW[ frame_idx++ ] = pwidth; + // If this is the last pulse in a frame, set _radio_status. + if (frame_idx >= NUM_CHANNELS) { + _radio_status = 1; + } + } + } + // Save icr for next call. + prev_icr = icr; } @@ -80,74 +80,74 @@ APM_RC_APM2::APM_RC_APM2() // Public Methods ////////////////////////////////////////////////////////////// void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg ) { - // --------------------- TIMER1: OUT1 and OUT2 ----------------------- - pinMode(12,OUTPUT); // OUT1 (PB6/OC1B) - pinMode(11,OUTPUT); // OUT2 (PB5/OC1A) + // --------------------- TIMER1: OUT1 and OUT2 ----------------------- + pinMode(12,OUTPUT); // OUT1 (PB6/OC1B) + pinMode(11,OUTPUT); // OUT2 (PB5/OC1A) - // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1. - // CS11: prescale by 8 => 0.5us tick - TCCR1A =((1< 50hz freq - OCR1A = 0xFFFF; // Init OCR registers to nil output signal - OCR1B = 0xFFFF; + // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1. + // CS11: prescale by 8 => 0.5us tick + TCCR1A =((1< 50hz freq + OCR1A = 0xFFFF; // Init OCR registers to nil output signal + OCR1B = 0xFFFF; - // --------------- TIMER4: OUT3, OUT4, and OUT5 --------------------- - pinMode(8,OUTPUT); // OUT3 (PH5/OC4C) - pinMode(7,OUTPUT); // OUT4 (PH4/OC4B) - pinMode(6,OUTPUT); // OUT5 (PH3/OC4A) + // --------------- TIMER4: OUT3, OUT4, and OUT5 --------------------- + pinMode(8,OUTPUT); // OUT3 (PH5/OC4C) + pinMode(7,OUTPUT); // OUT4 (PH4/OC4B) + pinMode(6,OUTPUT); // OUT5 (PH3/OC4A) - // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4. - // CS41: prescale by 8 => 0.5us tick - TCCR4A =((1< 50hz freq + // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4. + // CS41: prescale by 8 => 0.5us tick + TCCR4A =((1< 50hz freq - //--------------- TIMER3: OUT6, OUT7, and OUT8 ---------------------- - pinMode(3,OUTPUT); // OUT6 (PE5/OC3C) - pinMode(2,OUTPUT); // OUT7 (PE4/OC3B) - pinMode(5,OUTPUT); // OUT8 (PE3/OC3A) + //--------------- TIMER3: OUT6, OUT7, and OUT8 ---------------------- + pinMode(3,OUTPUT); // OUT6 (PE5/OC3C) + pinMode(2,OUTPUT); // OUT7 (PE4/OC3B) + pinMode(5,OUTPUT); // OUT8 (PE3/OC3A) - // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3 - // CS31: prescale by 8 => 0.5us tick - TCCR3A =((1< 50hz freq + // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3 + // CS31: prescale by 8 => 0.5us tick + TCCR3A =((1< 50hz freq - //--------------- TIMER5: PPM INPUT, OUT10, and OUT11 --------------- - // Init PPM input on Timer 5 - pinMode(48, INPUT); // PPM Input (PL1/ICP5) - pinMode(45, OUTPUT); // OUT10 (PL4/OC5B) - pinMode(44, OUTPUT); // OUT11 (PL5/OC5C) + //--------------- TIMER5: PPM INPUT, OUT10, and OUT11 --------------- + // Init PPM input on Timer 5 + pinMode(48, INPUT); // PPM Input (PL1/ICP5) + pinMode(45, OUTPUT); // OUT10 (PL4/OC5B) + pinMode(44, OUTPUT); // OUT11 (PL5/OC5C) - // WGM: 1 1 1 1. Fast PWM, TOP is OCR5A - // COM all disabled. - // CS51: prescale by 8 => 0.5us tick - // ICES5: Input Capture on rising edge - TCCR5A =((1< 50hz freq. The input capture routine - // assumes this 40000 for TOP. + // WGM: 1 1 1 1. Fast PWM, TOP is OCR5A + // COM all disabled. + // CS51: prescale by 8 => 0.5us tick + // ICES5: Input Capture on rising edge + TCCR5A =((1< 50hz freq. The input capture routine + // assumes this 40000 for TOP. - isr_reg->register_signal( ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb ); - // Enable Input Capture interrupt - TIMSK5 |= (1<register_signal( ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb ); + // Enable Input Capture interrupt + TIMSK5 |= (1<>1; + uint16_t pwm=0; + switch(ch) { + case 0: pwm=OCR1B; break; // out1 + case 1: pwm=OCR1A; break; // out2 + case 2: pwm=OCR4C; break; // out3 + case 3: pwm=OCR4B; break; // out4 + case 4: pwm=OCR4A; break; // out5 + case 5: pwm=OCR3C; break; // out6 + case 6: pwm=OCR3B; break; // out7 + case 7: pwm=OCR3A; break; // out8 + case 9: pwm=OCR5B; break; // out10 + case 10: pwm=OCR5C; break; // out11 + } + return pwm>>1; } void APM_RC_APM2::enable_out(uint8_t ch) { - switch(ch) { + switch(ch) { case 0: TCCR1A |= (1<>= 1; + // we need to block interrupts during the read of a 16 bit + // value + cli(); + result = _PWM_RAW[ch]; + sei(); + // 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; } unsigned char APM_RC_APM2::GetState(void) { - return(_radio_status); + return(_radio_status); } // InstantPWM is not implemented! -void APM_RC_APM2::Force_Out(void) { } -void APM_RC_APM2::Force_Out0_Out1(void) { } -void APM_RC_APM2::Force_Out2_Out3(void) { } -void APM_RC_APM2::Force_Out6_Out7(void) { } +void APM_RC_APM2::Force_Out(void) { +} +void APM_RC_APM2::Force_Out0_Out1(void) { +} +void APM_RC_APM2::Force_Out2_Out3(void) { +} +void APM_RC_APM2::Force_Out6_Out7(void) { +} /* ---------------- OUTPUT SPEED CONTROL ------------------ */ void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz) { - uint16_t icr = _map_speed(speed_hz); + uint16_t icr = _map_speed(speed_hz); - if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) { - ICR1 = icr; - } + if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) { + ICR1 = icr; + } - if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) { - ICR4 = icr; - } + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) { + ICR4 = icr; + } - if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) { - ICR3 = icr; - } + if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) { + ICR3 = icr; + } } // allow HIL override of RC values @@ -269,28 +273,28 @@ void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz) // A value of 0 means no override, use the real RC values bool APM_RC_APM2::setHIL(int16_t v[NUM_CHANNELS]) { - uint8_t sum = 0; - for (unsigned char i=0; i