Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Pat Hickey 2011-11-13 13:57:42 +11:00
parent 89d2f0f849
commit 36346fd86b
9 changed files with 599 additions and 263 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/APM_RC/APM_RC.h
View File

@ -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 <inttypes.h>
// 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 <inttypes.h>
#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

61
libraries/APM_RC/APM_RC.cpp → libraries/APM_RC/APM_RC_APM1.cpp
View File

@ -1,5 +1,5 @@
/*
APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino
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
@ -18,7 +18,7 @@
Automatically resets when we call InputCh to read channels
*/
#include "APM_RC.h"
#include "APM_RC_APM1.h"
#include <avr/interrupt.h>
#include "WProgram.h"
@ -28,15 +28,13 @@
#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;
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
****************************************************/
ISR(TIMER4_CAPT_vect)
void APM_RC_APM1::_timer4_capt_cb(void)
{
static uint16_t ICR4_old;
static uint8_t PPM_Counter=0;
@ -57,10 +55,10 @@ ISR(TIMER4_CAPT_vect)
}
else {
if (PPM_Counter < NUM_CHANNELS) { // Valid pulse channel?
PWM_RAW[PPM_Counter++]=Pulse_Width; // Saving pulse.
_PWM_RAW[PPM_Counter++]=Pulse_Width; // Saving pulse.
if (PPM_Counter >= NUM_CHANNELS) {
radio_status = 1;
_radio_status = 1;
}
}
}
@ -70,13 +68,16 @@ ISR(TIMER4_CAPT_vect)
// Constructors ////////////////////////////////////////////////////////////////
APM_RC_Class::APM_RC_Class()
APM_RC_APM1::APM_RC_APM1()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void APM_RC_Class::Init(void)
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)
@ -132,7 +133,7 @@ void APM_RC_Class::Init(void)
TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
}
void APM_RC_Class::OutputCh(uint8_t ch, uint16_t pwm)
void APM_RC_APM1::OutputCh(uint8_t ch, uint16_t pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
@ -153,7 +154,7 @@ void APM_RC_Class::OutputCh(uint8_t ch, uint16_t pwm)
}
}
uint16_t APM_RC_Class::InputCh(uint8_t ch)
uint16_t APM_RC_APM1::InputCh(uint8_t ch)
{
uint16_t result;
@ -163,38 +164,45 @@ uint16_t APM_RC_Class::InputCh(uint8_t ch)
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result = PWM_RAW[ch];
if (result != PWM_RAW[ch]) {
result = PWM_RAW[ch]; // if the results are different we make a third reading (this should be fine)
result = _PWM_RAW[ch];
if (result != _PWM_RAW[ch]) {
result = _PWM_RAW[ch]; // if the results are different we make a third reading (this should be fine)
}
result >>= 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
_radio_status=0; // Radio channel read
return(result);
}
uint8_t APM_RC_Class::GetState(void)
uint8_t APM_RC_APM1::GetState(void)
{
return(radio_status);
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_Class::Force_Out0_Out1(void)
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_Class::Force_Out2_Out3(void)
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_Class::Force_Out6_Out7(void)
void APM_RC_APM1::Force_Out6_Out7(void)
{
if (TCNT3>5000)
TCNT3=39990;
@ -203,7 +211,7 @@ void APM_RC_Class::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_Class::setHIL(int16_t v[NUM_CHANNELS])
bool APM_RC_APM1::setHIL(int16_t v[NUM_CHANNELS])
{
uint8_t sum = 0;
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
@ -214,7 +222,7 @@ bool APM_RC_Class::setHIL(int16_t v[NUM_CHANNELS])
sum++;
}
}
radio_status = 1;
_radio_status = 1;
if (sum == 0) {
return 0;
} else {
@ -222,7 +230,7 @@ bool APM_RC_Class::setHIL(int16_t v[NUM_CHANNELS])
}
}
void APM_RC_Class::clearOverride(void)
void APM_RC_APM1::clearOverride(void)
{
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
_HIL_override[i] = 0;
@ -230,7 +238,4 @@ void APM_RC_Class::clearOverride(void)
}
// make one instance for the user to use
APM_RC_Class APM_RC;
#endif // defined(ATMega1280)

36
libraries/APM_RC/APM_RC_APM1.h Normal file
View File

@ -0,0 +1,36 @@
#ifndef __APM_RC_APM1_H__
#define __APM_RC_APM1_H__
#define MIN_PULSEWIDTH 900
#define MAX_PULSEWIDTH 2100
#include "APM_RC.h"
#include "../Arduino_Mega_ISR_Registry/Arduino_Mega_ISR_Registry.h"
class APM_RC_APM1 : public APM_RC_Class
{
public:
APM_RC_APM1();
void Init( Arduino_Mega_ISR_Registry * isr_reg );
void OutputCh(uint8_t ch, uint16_t pwm);
uint16_t InputCh(uint8_t ch);
uint8_t GetState();
bool setHIL(int16_t v[NUM_CHANNELS]);
void clearOverride(void);
void Force_Out(void);
void Force_Out0_Out1(void);
void Force_Out2_Out3(void);
void Force_Out6_Out7(void);
private:
static void _timer4_capt_cb(void);
static volatile uint16_t _PWM_RAW[NUM_CHANNELS];
static volatile uint8_t _radio_status;
int16_t _HIL_override[NUM_CHANNELS];
};
#endif

224
libraries/APM_RC/APM_RC_Purple.cpp Normal file
View File

@ -0,0 +1,224 @@
/*
APM_RC_Purple.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_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<<WGM11)|(1<<COM1A1)|(1<<COM1B1));
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
ICR1 = 40000; // 0.5us tick => 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<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1));
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
ICR4 = 40000; // 0.5us tick => 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<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
ICR3 = 40000; // 0.5us tick => 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<<WGM50)|(1<<WGM51));
// Input Capture rising edge
TCCR5B = ((1<<WGM53)|(1<<WGM52)|(1<<CS51)|(1<<ICES5));
OCR5A = 40000; // 0.5us tick => 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<<ICIE5);
}
void APM_RC_Purple::OutputCh(unsigned char ch, uint16_t pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
switch(ch)
{
case 0: OCR1B=pwm; break; // out1
case 1: OCR1A=pwm; break; // out2
case 2: OCR4C=pwm; break; // out3
case 3: OCR4B=pwm; break; // out4
case 4: OCR4A=pwm; break; // out5
case 5: OCR3C=pwm; break; // out6
case 6: OCR3B=pwm; break; // out7
case 7: OCR3A=pwm; break; // out8
}
}
uint16_t APM_RC_Purple::InputCh(unsigned char ch)
{
uint16_t result;
uint16_t result2;
if (_HIL_override[ch] != 0) {
return _HIL_override[ch];
}
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result = _PWM_RAW[ch]>>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<NUM_CHANNELS; i++) {
if (v[i] != -1) {
_HIL_override[i] = v[i];
}
if (_HIL_override[i] != 0) {
sum++;
}
}
if (sum == 0) {
return 0;
} else {
return 1;
}
_radio_status = 1;
}
void APM_RC_Purple::clearOverride(void)
{
for (unsigned char i=0; i<NUM_CHANNELS; i++) {
_HIL_override[i] = 0;
}
}
#endif

36
libraries/APM_RC/APM_RC_Purple.h Normal file
View File

@ -0,0 +1,36 @@
#ifndef __APM_RC_PURPLE_H__
#define __APM_RC_PURPLE_H__
#define NUM_CHANNELS 8
#define MIN_PULSEWIDTH 900
#define MAX_PULSEWIDTH 2100
#include "APM_RC.h"
#include "../Arduino_Mega_ISR_Registry/Arduino_Mega_ISR_Registry.h"
class APM_RC_Purple : public APM_RC_Class
{
private:
public:
APM_RC_Purple();
void Init( Arduino_Mega_ISR_Registry * isr_reg );
void OutputCh(unsigned char ch, uint16_t pwm);
uint16_t InputCh(unsigned char ch);
unsigned char GetState();
bool setHIL(int16_t v[NUM_CHANNELS]);
void clearOverride(void);
void Force_Out(void);
void Force_Out0_Out1(void);
void Force_Out2_Out3(void);
void Force_Out6_Out7(void);
private:
static void _timer5_capt_cb(void);
static volatile uint16_t _PWM_RAW[NUM_CHANNELS];
static volatile uint8_t _radio_status;
int16_t _HIL_override[NUM_CHANNELS];
};
#endif

9
libraries/APM_RC/examples/APM_radio/APM_radio.pde → libraries/APM_RC/examples/APM1_radio/APM1_radio.pde
View File

@ -6,13 +6,18 @@
(Works with last PPM_encoder firmware)
*/
#include <Arduino_Mega_ISR_Registry.h>
#include <APM_RC.h> // 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);
}

2
libraries/APM_RC/examples/APM_radio/Makefile → libraries/APM_RC/examples/APM1_radio/Makefile
View File

@ -1,2 +1,2 @@
BOARD = mega
BOARD = mega2560
include ../../../AP_Common/Arduino.mk

2
libraries/APM_RC/examples/Purple_radio/Makefile Normal file
View File

@ -0,0 +1,2 @@
BOARD = mega2560
include ../../../AP_Common/Arduino.mk

37
libraries/APM_RC/examples/Purple_radio/Purple_radio.pde Normal file
View File

@ -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 <Arduino_Mega_ISR_Registry.h>
#include <APM_RC.h> // 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();
}
}