Skip to content

Commit

Permalink
Add PWM and timer based servo signal generators
Browse files Browse the repository at this point in the history
  • Loading branch information
@BrettFolkins
BrettFolkins committed May 20, 2016
1 parent 8b8d901 commit 770a78c
Show file tree
Hide file tree
Showing 3 changed files with 263 additions and 22 deletions.
22 changes: 0 additions & 22 deletions src/APM/OutputGenerator.h
View file

This file was deleted.

186 changes: 186 additions & 0 deletions src/APM/ServoGenerator.h
View file
@@ -0,0 +1,186 @@
#pragma once
#include <util/atomic.h>

#define TIMER_NUM 1

#define EXPCAT(A,B,C) EXPANDEDCONCATENATE(A,B,C)
#define EXPANDEDCONCATENATE(A,B,C) A ## B ## C
#define TIMER_ISR(N,V) TIMER_ISR_EXP(N,V)
#define TIMER_ISR_EXP(N,V) TIMER ## N ## _ ## V ## _vect

namespace {
constexpr volatile uint8_t& TCCRA = EXPCAT(TCCR, TIMER_NUM,A);
constexpr volatile uint8_t& TCCRB = EXPCAT(TCCR, TIMER_NUM,B);
constexpr volatile uint8_t& TCCRC = EXPCAT(TCCR, TIMER_NUM,C);
constexpr volatile uint8_t& TIFR = EXPCAT(TIFR, TIMER_NUM, );
constexpr volatile uint8_t& TIMSK = EXPCAT(TIMSK,TIMER_NUM, );
constexpr volatile uint16_t& ICR = EXPCAT(ICR, TIMER_NUM, );
constexpr volatile uint16_t& TCNT = EXPCAT(TCNT, TIMER_NUM, );
constexpr volatile uint16_t& OCRA = EXPCAT(OCR, TIMER_NUM,A);
constexpr volatile uint16_t& OCRB = EXPCAT(OCR, TIMER_NUM,B);

class Output{
public:
Output(): highTime(0xffff), pinMask(0), pinReg(0) {}
uint16_t highTime;
uint8_t pinMask;
volatile uint8_t* pinReg;
bool enabled() const volatile { return pinMask != 0; }
void disable() volatile { pinMask = 0; pinReg = 0; highTime = 0xffff; }
void setPin(int p) volatile {
pinMask = digitalPinToBitMask(p);
pinReg = portOutputRegister(digitalPinToPort(p));
}
void setHigh() const volatile { *pinReg |= pinMask; }
void setLow() const volatile { *pinReg &= ~pinMask; }
};

constexpr uint8_t MAX_OUTPUTS = 8;
volatile Output output[MAX_OUTPUTS];
volatile uint8_t activeOutputs = 0;

class Action{ //lawsuit
public:
uint16_t time;
uint8_t channel;
};
Action actions[MAX_OUTPUTS+1];
Action * next = actions;
// constructor attribute makes this get run once before main
void setupActions() __attribute__ ((constructor));
void setupActions() {
actions[MAX_OUTPUTS] = {0xffff, 0};
for(uint8_t i=0; i<MAX_OUTPUTS; i++){
actions[i] = {0xffff, i};
}
}

constexpr uint8_t PRESCALAR = 8;
//works for prescalars less than or equal to 16
constexpr uint8_t TICKS_PER_MS = (F_CPU / (PRESCALAR * 1e6L));

int16_t constexpr intervalFromMicros(uint32_t us){
return us * TICKS_PER_MS;
}
}

namespace ServoGenerator{
void set(int channel, uint16_t us){
output[channel].highTime = intervalFromMicros(us);
}

void disable(int channel){
if(output[channel].enabled()){
output[channel].disable();
activeOutputs--;
}
}

bool enable(int channel, int pin){
pinMode(pin, OUTPUT);
output[channel].setPin(pin);
if(!output[channel].enabled()){
activeOutputs++;
return true;
}
return false;
}

void setup(uint16_t refreshIntervalMicroseconds){
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
// CTC, clear when TCNT == ICR, prescalar = 8
TCCRA = 0;
TCCRB = _BV(WGM13) | _BV(WGM12) | _BV(CS11);

// enable the ICF (TCNT==ICR) and OCRA (TCNT==OCRA) interrupt
TIMSK |= _BV(ICIE1);
TIMSK |= _BV(OCIE1A);

ICR = intervalFromMicros(refreshIntervalMicroseconds);
OCRA = 0xffff;

// clear the timer count and pending interrupts
TCNT = 0;
TIFR |= _BV(OCF1A);
TIFR |= _BV(ICF1);
}
}

class Servo{
uint8_t channel;
Servo(): channel(-1) {
setup(20000);
}
Servo(uint16_t frameUs): channel(-1) {
setup(frameUs);
}
bool attach(uint8_t arduinopin){
//find open channel, try to attach
uint8_t ch = -1;
for(uint8_t i; i<MAX_OUTPUTS; i++){
if(!output[i].enabled()){
ch = i;
break;
}
}
if(ch == -1) return false;
enable(ch, arduinopin);
channel = ch;
return true;
}
void detach(){
if(channel != -1)
disable(channel);
channel = -1;
}
void write(uint8_t sig){
if(channel != -1)
set(channel, sig*10 + 600); // convert [0,180] to [600,2400]
}
void writeMicroseconds(uint16_t us){
if(channel != -1)
set(channel, us);
}
bool attached() {
return channel != -1;
}
};
}

ISR(TIMER_ISR(TIMER_NUM, COMPA)){
do {
// stops when it finds an output with OCRA=0xffff at
// the end of actions[]
output[next->channel].setLow();
next++;
OCRA = next->time;
} while (OCRA <= TCNT);
}

ISR(TIMER_ISR(TIMER_NUM, CAPT)){
//update highTime values from outputs
for(uint8_t i=0; i<MAX_OUTPUTS; i++){
actions[i].time = output[actions[i].channel].highTime;
}
//sort actions by time
for(uint8_t i=1; i<MAX_OUTPUTS; i++){
uint16_t time = actions[i].time;
uint8_t j = i;
while(j>0 && time < actions[j-1].time){
Action a = actions[j-1];
actions[j-1] = actions[j];
actions[j] = a;
j--;
}
}
//set all signals high
//this preserves order because TCNT is monotonically increasing
for(uint8_t i=0; i<activeOutputs; i++){
output[actions[i].channel].setHigh();
actions[i].time += TCNT;
}

next = actions;
OCRA = next->time;
}

77 changes: 77 additions & 0 deletions src/APM/ServoPWM.h
View file
@@ -0,0 +1,77 @@
#pragma once

namespace ServoPWM{
void setup(uint16_t frameMicroseconds){
uint16_t frameTicks = frameMicroseconds * 2;
// This code is taken from Ardupilot temporarily

// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
// CS11: prescale by 8 => 0.5us tick
TCCR1A =((1<<WGM11));
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
ICR1 = frameTicks;
OCR1A = 0xFFFF; // Init OCR registers to nil output signal
OCR1B = 0xFFFF;

// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
// CS41: prescale by 8 => 0.5us tick
TCCR4A =((1<<WGM41));
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
OCR4A = 0xFFFF; // Init OCR registers to nil output signal
OCR4B = 0xFFFF;
OCR4C = 0xFFFF;
ICR4 = frameTicks;

// WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
// CS31: prescale by 8 => 0.5us tick
TCCR3A =((1<<WGM31));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
OCR3B = 0xFFFF;
OCR3C = 0xFFFF;
ICR3 = frameTicks;

pinMode(12, OUTPUT); // CH_1 (PB6/OC1B)
pinMode(11, OUTPUT); // CH_2 (PB5/OC1A)
pinMode(8, OUTPUT); // CH_3 (PH5/OC4C)
pinMode(7, OUTPUT); // CH_4 (PH4/OC4B)
pinMode(6, OUTPUT); // CH_5 (PH3/OC4A)
pinMode(3, OUTPUT); // CH_6 (PE5/OC3C)
pinMode(2, OUTPUT); // CH_7 (PE4/OC3B)
pinMode(5, OUTPUT); // CH_8 (PE3/OC3A)

TCCR1A |= (1<<COM1B1); // CH_1 : OC1B
TCCR1A |= (1<<COM1A1); // CH_2 : OC1A
TCCR4A |= (1<<COM4C1); // CH_3 : OC4C
TCCR4A |= (1<<COM4B1); // CH_4 : OC4B
TCCR4A |= (1<<COM4A1); // CH_5 : OC4A
TCCR3A |= (1<<COM3C1); // CH_6 : OC3C
TCCR3A |= (1<<COM3B1); // CH_7 : OC3B
TCCR3A |= (1<<COM3A1); // CH_8 : OC3A
}

void set(uint8_t output, uint16_t us){
uint16_t pwm = us*2;
switch(output){
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
}
}

class APMoutput{
uint8_t pin;
APMoutput(uint8_t APMoutput): pin(APMoutput) {}
void write(uint8_t sig){
set(pin, sig*10 + 600); // convert [0,180] to [600,2400]
}
void writeMicroseconds(uint16_t us){
set(pin, us);
}
};
}

0 comments on commit 770a78c

Please sign in to comment.