/elec/quadcopter

//

// motor.cc

//

#include "motors.h"

#include "config.h"

#include <Arduino.h>

#include <limits.h>

void Motors::setup()

{

	// setup outputs

	for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {

		pinMode( pin, OUTPUT );

		digitalWrite( pin, LOW );

	}	

}

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

{

	// does it look like now has overflowed (and wrapped)?

	if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )

		return now + ( ULONG_MAX - then );

	// else, calculate duration

	else

		return now - then;

}

void Motors::update_channels( int channels[] )

{

	static int event = NUM_CHANNELS * 2;

	unsigned long now = micros();

	static unsigned long frame_start = now - FRAME_DURATION;

	static unsigned long next_event_at;

	if( now >= next_event_at )

	{

		// action event

		digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );

		// move to next event

		if( ++event >= NUM_CHANNELS * 2 ) {

			event = 0;

			frame_start += FRAME_DURATION;

		}

		// calculate the time that the next event will occur

		next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;

		if( event & 1 )

			next_event_at += MIN_PULSE_WIDTH + channels[ event / 2 ] *

				( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;

//		Serial.print( event / 2 );

//		Serial.print( ( event & 1 )? 'v' : '^' );

//		Serial.print( " " );

//		Serial.println( next_event_at );

		if( event == 1 ) {

			unsigned long width = MIN_PULSE_WIDTH +

				(unsigned long)channels[ event / 2 ] *

				( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;

			Serial.print( channels[ event / 2 ] );

			Serial.print( " " );

			Serial.println( width );

		}

	}

}