/elec/quadcopter

//

// receiver.cc

//

#include "receiver.h"

#include "config.h"

#include <Arduino.h>

#include <limits.h>

// channel sequence order

static const int channel_order_[] = { CHANNEL_ORDER };

// set to the time of the last pulse edges

static unsigned long new_pulse_up_[2] = { 0, 0 };

static unsigned long new_pulse_down_ = 0;

static char new_pulse_interrupt_;

// ISR to handle the PPM signals

inline void signal_handler( int interrupt, int pin )

{

	// record rising/falling edge

	if( digitalRead( pin ) )

		new_pulse_up_[ interrupt ] = micros();

	else {

		new_pulse_down_ = micros();

		// record which interrupt just had a falling edge

		new_pulse_interrupt_ = interrupt;

	}

}

static void signal_handler_0()

{

	signal_handler( 0, 2 );

}

static void signal_handler_1()

{

	signal_handler( 1, 3 );

}

void Receiver::setup()

{

	// set up an interrupts

	attachInterrupt( 0, signal_handler_0, CHANGE );

	attachInterrupt( 1, signal_handler_1, CHANGE );

	digitalWrite( 2, LOW );

	digitalWrite( 3, LOW );

}

static unsigned long calculate_duration( unsigned long then, unsigned long now )

{

	return now - then;

	if( now < then )

		return now + ( ULONG_MAX - then );

	else

		return now - then;

}

bool Receiver::read_channels( unsigned long channel_values[] )

{

	static unsigned long last_pulse_down = 0;

	static int next_channel = 0;

	// capture pulse values atomically

	noInterrupts();

	unsigned long pulse_up = new_pulse_up_[ (int)new_pulse_interrupt_ ];

	unsigned long pulse_down = new_pulse_down_;

	char new_pulse_interrupt = new_pulse_interrupt_;

	interrupts();

	// if the amount of time that has passed since the last falling edge is

	// greater than the frame gap, reset the next channel so that we can start

	// reading them again

	if( next_channel &&

		new_pulse_interrupt == 1 &&

		calculate_duration( pulse_down, micros() ) > MIN_FRAME_GAP_WIDTH )

	{

		// reset the next channel (which restarts reading them)

		next_channel = 0;

	}

	// check for a new complete pulse

	if( pulse_down != last_pulse_down )

	{

		// are there still pulses to read?

		if( next_channel < NUM_CHANNELS )

		{

			unsigned long duration =

				calculate_duration( pulse_up, pulse_down );

			// does this pulse look ok?

			if( duration >= MIN_PULSE_WIDTH &&

				duration <= MAX_PULSE_WIDTH )

			{

				// store channel value

				int channel = channel_order_[ next_channel ] - 1;

				channel_values[ channel ] = duration;

				// we got a channel

				next_channel++;

			}

			else {

				// set invalid channel number (to indicate error)

				next_channel = NUM_CHANNELS + 1;

			}

		}

		last_pulse_down = pulse_down;

	}

	// if we've read a frame, invalidate the frame (so we don't report it a

	// second time) and return true

	if( next_channel == NUM_CHANNELS ) {

		next_channel++;

		return true;

	}

	return false;

}