From 36346fd86bed351e0ba25a9c249d20558969997a Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Sun, 13 Nov 2011 13:57:42 +1100 Subject: [PATCH] purple: rework APM_RC library for purple hardware this splits the APM_RC class into instances for purple and APM1, and adds example sketches for both --- libraries/APM_RC/APM_RC.h | 37 +- .../APM_RC/{APM_RC.cpp => APM_RC_APM1.cpp} | 477 +++++++++--------- libraries/APM_RC/APM_RC_APM1.h | 36 ++ libraries/APM_RC/APM_RC_Purple.cpp | 224 ++++++++ libraries/APM_RC/APM_RC_Purple.h | 36 ++ .../APM1_radio.pde} | 11 +- .../{APM_radio => APM1_radio}/Makefile | 2 +- .../APM_RC/examples/Purple_radio/Makefile | 2 + .../examples/Purple_radio/Purple_radio.pde | 37 ++ 9 files changed, 599 insertions(+), 263 deletions(-) rename libraries/APM_RC/{APM_RC.cpp => APM_RC_APM1.cpp} (82%) create mode 100644 libraries/APM_RC/APM_RC_APM1.h create mode 100644 libraries/APM_RC/APM_RC_Purple.cpp create mode 100644 libraries/APM_RC/APM_RC_Purple.h rename libraries/APM_RC/examples/{APM_radio/APM_radio.pde => APM1_radio/APM1_radio.pde} (75%) rename libraries/APM_RC/examples/{APM_radio => APM1_radio}/Makefile (69%) create mode 100644 libraries/APM_RC/examples/Purple_radio/Makefile create mode 100644 libraries/APM_RC/examples/Purple_radio/Purple_radio.pde diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 93f6b69f08..65fb36425f 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -1,9 +1,7 @@ -#ifndef APM_RC_h -#define APM_RC_h +#ifndef __APM_RC_H__ +#define __APM_RC_H__ -#define NUM_CHANNELS 8 -#define MIN_PULSEWIDTH 900 -#define MAX_PULSEWIDTH 2100 +#include // Radio channels // Note channels are from 0! @@ -15,30 +13,23 @@ #define CH_6 5 #define CH_7 6 #define CH_8 7 -#define CH_10 9 //PB5 -#define CH_11 10 //PE3 +#define CH_10 9 +#define CH_11 10 -#include +#define NUM_CHANNELS 8 class APM_RC_Class { - private: public: - APM_RC_Class(); - void Init(); - void OutputCh(uint8_t ch, uint16_t pwm); - uint16_t InputCh(uint8_t ch); - uint8_t GetState(); - void Force_Out0_Out1(void); - void Force_Out2_Out3(void); - void Force_Out6_Out7(void); - bool setHIL(int16_t v[NUM_CHANNELS]); - void clearOverride(void); - - private: - int16_t _HIL_override[NUM_CHANNELS]; + APM_RC_Class() {} + virtual void OutputCh(uint8_t ch, uint16_t pwm) = 0; + virtual uint16_t InputCh(uint8_t ch) = 0; + virtual uint8_t GetState() = 0; + virtual void clearOverride(void) = 0; + virtual void Force_Out() = 0; }; -extern APM_RC_Class APM_RC; +#include "APM_RC_APM1.h" +#include "APM_RC_Purple.h" #endif diff --git a/libraries/APM_RC/APM_RC.cpp b/libraries/APM_RC/APM_RC_APM1.cpp similarity index 82% rename from libraries/APM_RC/APM_RC.cpp rename to libraries/APM_RC/APM_RC_APM1.cpp index db81cd1852..5b82afc3bb 100644 --- a/libraries/APM_RC/APM_RC.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -1,236 +1,241 @@ -/* - APM_RC.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.h" - -#include -#include "WProgram.h" - -#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) -# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. -#else - -// Variable definition for Input Capture interrupt -//volatile uint16_t ICR4_old; -//volatile uint8_t PPM_Counter=0; -volatile uint16_t PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,2400,2400,2400,2400}; -volatile uint8_t radio_status=0; - -/**************************************************** - Input Capture Interrupt ICP4 => PPM signal read - ****************************************************/ -ISR(TIMER4_CAPT_vect) -{ - 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; -} - - -// Constructors //////////////////////////////////////////////////////////////// - -APM_RC_Class::APM_RC_Class() -{ -} - -// Public Methods ////////////////////////////////////////////////////////////// -void APM_RC_Class::Init(void) -{ - // 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; // Because timer runs at 0.5us we need to do value/2 - - // 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_Class::GetState(void) -{ - return(radio_status); -} - -// InstantPWM implementation -// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use -void APM_RC_Class::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 -} -// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use -void APM_RC_Class::Force_Out2_Out3(void) -{ - if (TCNT1>5000) - TCNT1=39990; -} -// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use -void APM_RC_Class::Force_Out6_Out7(void) -{ - if (TCNT3>5000) - TCNT3=39990; -} - -// allow HIL override of RC values -// A value of -1 means no change -// A value of 0 means no override, use the real RC values -bool APM_RC_Class::setHIL(int16_t v[NUM_CHANNELS]) -{ - uint8_t sum = 0; - for (uint8_t i=0; i New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ +#include "APM_RC_APM1.h" + +#include +#include "WProgram.h" + +#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) +# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. +#else + +// Variable definition for Input Capture interrupt +volatile uint16_t APM_RC_APM1::_PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,2400,2400,2400,2400}; +volatile uint8_t APM_RC_APM1::_radio_status=0; + +/**************************************************** + 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; + } + } + } + ICR4_old = Pulse; +} + + +// Constructors //////////////////////////////////////////////////////////////// + +APM_RC_APM1::APM_RC_APM1() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +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<>= 1; // Because timer runs at 0.5us we need to do value/2 + + // 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); +} + + +// InstantPWM implementation +void APM_RC_APM1::Force_Out(void) +{ + Force_Out0_Out1(); + Force_Out2_Out3(); + Force_Out6_Out7(); +} +// 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 +} +// 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; +} +// 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; +} + +// allow HIL override of RC values +// A value of -1 means no change +// 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 New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ +#include "APM_RC_Purple.h" + +#include "WProgram.h" + +#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__) +# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target. +#else + +// Variable definition for Input Capture interrupt +volatile uint16_t APM_RC_Purple::_PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,2400,2400,2400,2400}; +volatile uint8_t APM_RC_Purple::_radio_status=0; + +/**************************************************** + Input Capture Interrupt ICP5 => PPM signal read + ****************************************************/ +void APM_RC_Purple::_timer5_capt_cb(void) +{ + 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; + } + } + } + // Save icr for next call. + prev_icr = icr; +} + + +// Constructors //////////////////////////////////////////////////////////////// + +APM_RC_Purple::APM_RC_Purple() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void APM_RC_Purple::Init( Arduino_Mega_ISR_Registry * isr_reg ) +{ + // --------------------- 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. + // COM1A and COM1B enabled, set to low level on match. + // CS11: prescale by 8 => 0.5us tick + TCCR1A =((1< 50hz freq + + OutputCh(1, 1100); + OutputCh(2, 1100); + + // --------------- 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. + // COM4A, 4B, 4C enabled, set to low level on match. + // CS41: prescale by 8 => 0.5us tick + TCCR4A =((1< 50hz freq + + OutputCh(3, 1100); + OutputCh(4, 1100); + OutputCh(5, 1100); + + //--------------- 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 + // COM3A, 3B, 3C enabled, set to low level on match + // CS31: prescale by 8 => 0.5us tick + TCCR3A =((1< 50hz freq + + OutputCh(6, 1100); + OutputCh(7, 1100); + OutputCh(8, 1100); + + //--------------- TIMER5: PPM INPUT --------------------------------- + // Init PPM input on Timer 5 + pinMode(48, INPUT); // PPM Input (PL1/ICP5) + + // 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<>1; // Because timer runs at 0.5us we need to do value/2 + result2 = _PWM_RAW[ch]>>1; + if (result != result2) + result = _PWM_RAW[ch]>>1; // if the results are different we make a third reading (this should be fine) + + // 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_Purple::GetState(void) +{ + return(_radio_status); +} + +// InstantPWM is not implemented! + +void APM_RC_Purple::Force_Out(void) { } +void APM_RC_Purple::Force_Out0_Out1(void) { } +void APM_RC_Purple::Force_Out2_Out3(void) { } +void APM_RC_Purple::Force_Out6_Out7(void) { } + +// allow HIL override of RC values +// A value of -1 means no change +// A value of 0 means no override, use the real RC values +bool APM_RC_Purple::setHIL(int16_t v[NUM_CHANNELS]) +{ + uint8_t sum = 0; + for (unsigned char i=0; i #include // ArduPilot Mega RC Library +Arduino_Mega_ISR_Registry isr_registry; +APM_RC_APM1 APM_RC; + void setup() { - APM_RC.Init(); // APM Radio initialization + isr_registry.init(); + APM_RC.Init(&isr_registry); // APM Radio initialization - Serial.begin(38400); + Serial.begin(115200); Serial.println("ArduPilot Mega RC library test"); delay(1000); } @@ -29,4 +34,4 @@ void loop() } Serial.println(); } -} \ No newline at end of file +} diff --git a/libraries/APM_RC/examples/APM_radio/Makefile b/libraries/APM_RC/examples/APM1_radio/Makefile similarity index 69% rename from libraries/APM_RC/examples/APM_radio/Makefile rename to libraries/APM_RC/examples/APM1_radio/Makefile index 85b4d8856b..a5d5e7918b 100644 --- a/libraries/APM_RC/examples/APM_radio/Makefile +++ b/libraries/APM_RC/examples/APM1_radio/Makefile @@ -1,2 +1,2 @@ -BOARD = mega +BOARD = mega2560 include ../../../AP_Common/Arduino.mk diff --git a/libraries/APM_RC/examples/Purple_radio/Makefile b/libraries/APM_RC/examples/Purple_radio/Makefile new file mode 100644 index 0000000000..a5d5e7918b --- /dev/null +++ b/libraries/APM_RC/examples/Purple_radio/Makefile @@ -0,0 +1,2 @@ +BOARD = mega2560 +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APM_RC/examples/Purple_radio/Purple_radio.pde b/libraries/APM_RC/examples/Purple_radio/Purple_radio.pde new file mode 100644 index 0000000000..be1871526e --- /dev/null +++ b/libraries/APM_RC/examples/Purple_radio/Purple_radio.pde @@ -0,0 +1,37 @@ +/* + Example of APM_RC library. + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include +#include // ArduPilot Mega RC Library + +Arduino_Mega_ISR_Registry isr_registry; +APM_RC_Purple APM_RC; + +void setup() +{ + isr_registry.init(); + APM_RC.Init(&isr_registry); // APM Radio initialization + + Serial.begin(115200); + Serial.println("ArduPilot Mega RC library test"); + delay(1000); +} + +void loop() +{ + // New radio frame? (we could use also if((millis()- timer) > 20) + if (APM_RC.GetState() == 1){ + Serial.print("CH:"); + for(int i = 0; i < 8; i++){ + Serial.print(APM_RC.InputCh(i)); // Print channel values + Serial.print(","); + APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos + } + Serial.println(); + } +}