File content as of revision 1:cb791d277230:

/* PulsePosition Library for Teensy 3.1
 * High resolution input and output of PPM encoded signals
 * http://www.pjrc.com/teensy/td_libs_PulsePosition.html
 * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
 *
 * Development of this library was funded by PJRC.COM, LLC by sales of Teensy
 * boards.  Please support PJRC's efforts to develop open source software by
 * purchasing Teensy or other PJRC products.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */


#include "mbed.h"
#include "PeripheralPins.h"
#include "PulsePosition.h"

#define F_BUS 60000000
// Timing parameters, in microseconds.


// The shortest time allowed between any 2 rising edges.  This should be at
// least double TX_PULSE_WIDTH.
#define TX_MINIMUM_SIGNAL   300.0

// The longest time allowed between any 2 rising edges for a normal signal.
#define TX_MAXIMUM_SIGNAL  2500.0

// The default signal to send if nothing has been written.
#define TX_DEFAULT_SIGNAL  1500.0

// When transmitting with a single pin, the minimum space signal that marks
// the end of a frame.  Single wire receivers recognize the end of a frame
// by looking for a gap longer than the maximum data size.  When viewing the
// waveform on an oscilloscope, set the trigger "holdoff" time to slightly
// less than TX_MINIMUM_SPACE, for the most reliable display.  This parameter
// is not used when transmitting with 2 pins.
#define TX_MINIMUM_SPACE   5000.0

// The minimum total frame size.  Some servo motors or other devices may not
// work with pulses the repeat more often than 50 Hz.  To allow transmission
// as fast as possible, set this to the same as TX_MINIMUM_SIGNAL.
#define TX_MINIMUM_FRAME  20000.0

// The length of all transmitted pulses.  This must be longer than the worst
// case interrupt latency, which depends on how long any other library may
// disable interrupts.  This must also be no more than half TX_MINIMUM_SIGNAL.
// Most libraries disable interrupts for no more than a few microseconds.
// The OneWire library is a notable exception, so this may need to be lengthened
// if a library that imposes unusual interrupt latency is in use.
#define TX_PULSE_WIDTH      100.0

// When receiving, any time between rising edges longer than this will be
// treated as the end-of-frame marker.
#define RX_MINIMUM_SPACE   3500.0


// convert from microseconds to I/O clock ticks

#define CLOCKS_PER_MICROSECOND ((double)F_BUS / 1000000.0)

#define TX_MINIMUM_SIGNAL_CLOCKS  (uint32_t)(TX_MINIMUM_SIGNAL * CLOCKS_PER_MICROSECOND)
#define TX_MAXIMUM_SIGNAL_CLOCKS  (uint32_t)(TX_MAXIMUM_SIGNAL * CLOCKS_PER_MICROSECOND)
#define TX_DEFAULT_SIGNAL_CLOCKS  (uint32_t)(TX_DEFAULT_SIGNAL * CLOCKS_PER_MICROSECOND)
#define TX_MINIMUM_SPACE_CLOCKS   (uint32_t)(TX_MINIMUM_SPACE * CLOCKS_PER_MICROSECOND)
#define TX_MINIMUM_FRAME_CLOCKS   (uint32_t)(TX_MINIMUM_FRAME * CLOCKS_PER_MICROSECOND)
#define TX_PULSE_WIDTH_CLOCKS     (uint32_t)(TX_PULSE_WIDTH * CLOCKS_PER_MICROSECOND)
#define RX_MINIMUM_SPACE_CLOCKS   (uint32_t)(RX_MINIMUM_SPACE * CLOCKS_PER_MICROSECOND)


#define FTM0_SC_VALUE (FTM_SC_TOIE_MASK | FTM_SC_CLKS(1) | FTM_SC_PS(0))

#define CSC_CHANGE(reg, val)         ((reg)->csc = (val))
#define CSC_INTACK(reg, val)         ((reg)->csc = (val))
#define CSC_CHANGE_INTACK(reg, val)  ((reg)->csc = (val))
#define FRAME_PIN_SET()              *framePinReg = 1
#define FRAME_PIN_CLEAR()            *framePinReg = 0


uint8_t PulsePositionOutput::channelmask = 0;
PulsePositionOutput * PulsePositionOutput::list[8];

PulsePositionOutput::PulsePositionOutput(void)
{
    pulse_width[0] = TX_MINIMUM_FRAME_CLOCKS;
    for (int i=1; i <= PULSEPOSITION_MAXCHANNELS; i++) {
        pulse_width[i] = TX_DEFAULT_SIGNAL_CLOCKS;
    }
    cscSet = 0b01011100;
    cscClear = 0b01011000;
}

PulsePositionOutput::PulsePositionOutput(int polarity)
{
    pulse_width[0] = TX_MINIMUM_FRAME_CLOCKS;
    for (int i=1; i <= PULSEPOSITION_MAXCHANNELS; i++) {
        pulse_width[i] = TX_DEFAULT_SIGNAL_CLOCKS;
    }
    if (polarity == FALLING) {
        cscSet = 0b01011000;
        cscClear = 0b01011100;
    } else {
        cscSet = 0b01011100;
        cscClear = 0b01011000;
    }
}

bool PulsePositionOutput::begin(PinName txPin)
{
    return begin(txPin, (PinName)255);  // thd ?
}

bool PulsePositionOutput::begin(PinName txPin, PinName framePin)
{
    uint32_t channel;
    volatile void *reg;

    SIM_SCGC6 |= (1 << SIM_SCGC6_FTM0_SHIFT);  // enable FMT0
    if (FTM0_MOD != 0xFFFF || (FTM0_SC & 0x7F) != FTM0_SC_VALUE) {
        FTM0_SC = 0;
        FTM0_CNT = 0;
        FTM0_MOD = 0xFFFF;
        FTM0_SC = FTM0_SC_VALUE;
        FTM0_MODE = 0;
    }

    switch (txPin) {
      case PTC3: channel = 2; reg = &FTM0_C2SC; break;
      case PTC4: channel = 3; reg = &FTM0_C3SC; break;
      case PTA0: channel = 5; reg = &FTM0_C5SC; break;
      case PTC2: channel = 1; reg = &FTM0_C1SC; break;
      case PTA1: channel = 6; reg = &FTM0_C6SC; break;
      case PTA2: channel = 7; reg = &FTM0_C7SC; break;
      default:
        return false;
    }
    if (framePin != (PinName)255)    framePinReg = new DigitalOut(framePin);
    else framePinReg = NULL;
    state = 0;
    current_channel = 0;
    total_channels = 0;
    ftm = (struct ftm_channel_struct *)reg;
    ftm->cv = 200;
    CSC_CHANGE(ftm, cscSet); // set on compare match & interrupt
    list[channel] = this;
    channelmask |= (1<<channel);
    SIM_SCGC5 |= (1 << SIM_SCGC5_PORTC_SHIFT);  // power PORTC
    uint32_t port = txPin >> GPIO_PORT_SHIFT;
    uint32_t pin = (txPin & 0x1F) ;
    uint32_t alt= pinmap_function(txPin, PinMap_PWM);
    ((PORT_Type *)(PORTA_BASE + 0x1000 * port))->PCR[pin] = PORT_PCR_MUX(alt) | PORT_PCR_DSE_MASK | PORT_PCR_SRE_MASK;
    NVIC_SetPriority(FTM0_IRQn, 32);
    NVIC_EnableIRQ(FTM0_IRQn);
    return true;
}

bool PulsePositionOutput::write(uint8_t channel, float microseconds)
{
    uint32_t i, sum, space, clocks, num_channels;

    if (channel < 1 || channel > PULSEPOSITION_MAXCHANNELS) return false;
    if (microseconds < TX_MINIMUM_SIGNAL || microseconds > TX_MAXIMUM_SIGNAL) return false;
    clocks = microseconds * CLOCKS_PER_MICROSECOND;
    num_channels = total_channels;
    if (channel > num_channels) num_channels = channel;
    sum = clocks;
    for (i=1; i < channel; i++) sum += pulse_width[i];
    for (i=channel+1; i <= num_channels; i++) sum += pulse_width[i];
    if (sum < TX_MINIMUM_FRAME_CLOCKS - TX_MINIMUM_SPACE_CLOCKS) {
        space = TX_MINIMUM_FRAME_CLOCKS - sum;
    } else {
        if (framePinReg) {
            space = TX_PULSE_WIDTH_CLOCKS;
        } else {
            space = TX_MINIMUM_SPACE_CLOCKS;
        }
    }
    __disable_irq();
    pulse_width[0] = space;
    pulse_width[channel] = clocks;
    total_channels = num_channels;
    __enable_irq();
    return true;
}

void PulsePositionOutput::isr(void)
{
  //  printf("out isr %d %d\n",state,ftm->cv);
    FTM0_MODE = 0;
    if (state == 0) {
        // pin was just set high, schedule it to go low
        ftm->cv += TX_PULSE_WIDTH_CLOCKS;
        CSC_CHANGE_INTACK(ftm, cscClear); // clear on compare match & interrupt
        state = 1;
    } else {
        // pin just went low
        uint32_t width, channel;
        if (state == 1) {
            channel = current_channel;
            if (channel == 0) {
                total_channels_buffer = total_channels;
                for (uint32_t i=0; i <= total_channels_buffer; i++) {
                    pulse_buffer[i] = pulse_width[i];
                }
            }
            width = pulse_buffer[channel] - TX_PULSE_WIDTH_CLOCKS;
            if (++channel > total_channels_buffer) {
                channel = 0;
            }
            if (framePinReg) {
                //if (channel == 0) {
                if (channel == 1) {
                    FRAME_PIN_SET();
                } else {
                    FRAME_PIN_CLEAR();
                }
            }
            current_channel = channel;
        } else {
            width = pulse_remaining;
        }
        if (width <= 60000) {
            ftm->cv += width;
            CSC_CHANGE_INTACK(ftm, cscSet); // set on compare match & interrupt
            state = 0;
        } else {
            ftm->cv += 58000;
            CSC_INTACK(ftm, cscClear); // clear on compare match & interrupt
            pulse_remaining = width - 58000;
            state = 2;
        }
    }
}



void ftm0_isr(void) {
    if (FTM0_SC & 0x80) {
        FTM0_SC = FTM0_SC_VALUE;
        PulsePositionInput::overflow_count++;
        PulsePositionInput::overflow_inc = true;
    }
    // TODO: this could be efficient by reading FTM0_STATUS
    uint8_t maskin = PulsePositionInput::channelmask;
    if ((maskin & 0x01) && (FTM0_C0SC & 0x80)) PulsePositionInput::list[0]->isr();
    if ((maskin & 0x02) && (FTM0_C1SC & 0x80)) PulsePositionInput::list[1]->isr();
    if ((maskin & 0x04) && (FTM0_C2SC & 0x80)) PulsePositionInput::list[2]->isr();
    if ((maskin & 0x08) && (FTM0_C3SC & 0x80)) PulsePositionInput::list[3]->isr();
    if ((maskin & 0x10) && (FTM0_C4SC & 0x80)) PulsePositionInput::list[4]->isr();
    if ((maskin & 0x20) && (FTM0_C5SC & 0x80)) PulsePositionInput::list[5]->isr();
    if ((maskin & 0x40) && (FTM0_C6SC & 0x80)) PulsePositionInput::list[6]->isr();
    if ((maskin & 0x80) && (FTM0_C7SC & 0x80)) PulsePositionInput::list[7]->isr();

    uint8_t maskout = PulsePositionOutput::channelmask;
    if ((maskout & 0x01) && (FTM0_C0SC & 0x80)) PulsePositionOutput::list[0]->isr();
    if ((maskout & 0x02) && (FTM0_C1SC & 0x80)) PulsePositionOutput::list[1]->isr();
    if ((maskout & 0x04) && (FTM0_C2SC & 0x80)) PulsePositionOutput::list[2]->isr();
    if ((maskout & 0x08) && (FTM0_C3SC & 0x80)) PulsePositionOutput::list[3]->isr();
    if ((maskout & 0x10) && (FTM0_C4SC & 0x80)) PulsePositionOutput::list[4]->isr();
    if ((maskout & 0x20) && (FTM0_C5SC & 0x80)) PulsePositionOutput::list[5]->isr();
    if ((maskout & 0x40) && (FTM0_C6SC & 0x80)) PulsePositionOutput::list[6]->isr();
    if ((maskout & 0x80) && (FTM0_C7SC & 0x80)) PulsePositionOutput::list[7]->isr();
    PulsePositionInput::overflow_inc = false;

}
uint32_t ticks;
extern "C" void FTM0_IRQHandler() {
    ticks++;
    ftm0_isr();
}
// some explanation regarding this C to C++ trickery can be found here:
// http://forum.pjrc.com/threads/25278-Low-Power-with-Event-based-software-architecture-brainstorm?p=43496&viewfull=1#post43496

uint16_t PulsePositionInput::overflow_count = 0;
bool PulsePositionInput::overflow_inc = false;
uint8_t PulsePositionInput::channelmask = 0;
PulsePositionInput * PulsePositionInput::list[8];

PulsePositionInput::PulsePositionInput(void)
{
    cscEdge = 0b01000100;
}

PulsePositionInput::PulsePositionInput(int polarity)
{
    cscEdge = (polarity == FALLING) ? 0b01001000 : 0b01000100;
}


bool PulsePositionInput::begin(PinName pin)
{
    uint32_t channel;
    volatile void *reg;

    SIM_SCGC6 |= (1 << SIM_SCGC6_FTM0_SHIFT);  // enable FTM0
    if (FTM0_MOD != 0xFFFF || (FTM0_SC & 0x7F) != FTM0_SC_VALUE) {
        FTM0_SC = 0;
        FTM0_CNT = 0;
        FTM0_MOD = 0xFFFF;
        FTM0_SC = FTM0_SC_VALUE;
        FTM0_MODE = 0;
    }
    switch (pin) {
      case PTC3: channel = 2; reg = &FTM0_C2SC; break;
      case PTC4: channel = 3; reg = &FTM0_C3SC; break;
      case PTA0: channel = 5; reg = &FTM0_C5SC; break;
      case PTC2: channel = 1; reg = &FTM0_C1SC; break;
      case PTA1: channel = 6; reg = &FTM0_C6SC; break;
      case PTA2: channel = 7; reg = &FTM0_C7SC; break;
      default:
        return false;
    }
    prev = 0;
    write_index = 255;
    available_flag = false;
    ftm = (struct ftm_channel_struct *)reg;
    list[channel] = this;
    channelmask |= (1<<channel);
    SIM_SCGC5 |= (1 << SIM_SCGC5_PORTC_SHIFT);  // power PORTC
    uint32_t port = pin >> GPIO_PORT_SHIFT;
    uint32_t ppin = (pin & 0x1F) ;
    uint32_t alt= pinmap_function(pin, PinMap_PWM);
    ((PORT_Type *)(PORTA_BASE + 0x1000 * port))->PCR[ppin] = PORT_PCR_MUX(alt);
    CSC_CHANGE(ftm, cscEdge); // input capture & interrupt on rising edge
    NVIC_SetPriority(FTM0_IRQn, 32);
    NVIC_EnableIRQ(FTM0_IRQn);
    return true;
}

void PulsePositionInput::isr(void)
{
    uint32_t val, count;

    val = ftm->cv;
    CSC_INTACK(ftm, cscEdge); // input capture & interrupt on rising edge
    count = overflow_count;
    if (val > 0xE000 && overflow_inc) count--;
    val |= (count << 16);
    count = val - prev;
    prev = val;
     //Serial.print(val, HEX);
     //Serial.print("  ");
     //Serial.println(count);
    if (count >= RX_MINIMUM_SPACE_CLOCKS) {
        if (write_index < 255) {
            for (int i=0; i < write_index; i++) {
                pulse_buffer[i] = pulse_width[i];
            }
            total_channels = write_index;
            available_flag = true;
        }
        write_index = 0;
    } else {
        if (write_index < PULSEPOSITION_MAXCHANNELS) {
            pulse_width[write_index++] = count;
        }
    }
}

int PulsePositionInput::available(void)
{
    uint32_t total;
    bool flag;

    __disable_irq();
    flag = available_flag;
    total = total_channels;
    __enable_irq();
    if (flag) return total;
    return -1;
}

float PulsePositionInput::read(uint8_t channel)
{
    uint32_t total, index, value=0;

    if (channel == 0) return 0.0;
    index = channel - 1;
    __disable_irq();
    total = total_channels;
    if (index < total) value = pulse_buffer[index];
    if (channel >= total) available_flag = false;
    __enable_irq();
    return (float)value / (float)CLOCKS_PER_MICROSECOND;
}