/elec/quadcopter

To get this branch, use:
bzr branch http://bzr.ed.am/elec/quadcopter

« back to all changes in this revision

Viewing changes to src/rc-interface/motors.cc

  • Committer: Tim Marston
  • Date: 2013-10-23 19:58:45 UTC
  • Revision ID: tim@ed.am-20131023195845-6coin3ndwkkc52np
updated ignores

Show diffs side-by-side

added added

removed removed

src/rc-interface/motors.cc
1
 
//
2
 
// motor.cc
3
 
//
4
 
 
5
 
 
6
 
#include "motors.h"
7
 
#include "config.h"
8
 
#include <Arduino.h>
9
 
#include <limits.h>
10
 
 
11
 
 
12
 
void Motors::setup()
13
 
{
14
 
        // setup outputs
15
 
        for( int pin = FIRST_PIN; pin < FIRST_PIN + NUM_CHANNELS; pin++ ) {
16
 
                pinMode( pin, OUTPUT );
17
 
                digitalWrite( pin, LOW );
18
 
        }       
19
 
}
20
 
 
21
 
 
22
 
static signed long calculate_duration( unsigned long then, unsigned long now )
23
 
{
24
 
        // does it look like now has overflowed (and wrapped)?
25
 
        if( now < then && now < ( ULONG_MAX / 2 ) && then > ( ULONG_MAX / 2 ) )
26
 
                return now + ( ULONG_MAX - then );
27
 
        
28
 
        // else, calculate duration
29
 
        else
30
 
                return now - then;
31
 
}
32
 
 
33
 
 
34
 
void Motors::update_channels( int channels[] )
35
 
{
36
 
        static int event = NUM_CHANNELS * 2;
37
 
        unsigned long now = micros();
38
 
        static unsigned long frame_start = now - FRAME_DURATION;
39
 
        static unsigned long next_event_at;
40
 
 
41
 
        if( now >= next_event_at )
42
 
        {
43
 
                // action event
44
 
                digitalWrite( FIRST_PIN + ( event / 2 ), ( event & 1 )? LOW : HIGH );
45
 
 
46
 
                // move to next event
47
 
                if( ++event >= NUM_CHANNELS * 2 ) {
48
 
                        event = 0;
49
 
                        frame_start += FRAME_DURATION;
50
 
                }
51
 
 
52
 
                // calculate the time that the next event will occur
53
 
                next_event_at = frame_start + ( event / 2 ) * CHANNEL_INTERVAL;
54
 
                if( event & 1 )
55
 
                        next_event_at += MIN_PULSE_WIDTH + channels[ event / 2 ] *
56
 
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
57
 
 
58
 
//              Serial.print( event / 2 );
59
 
//              Serial.print( ( event & 1 )? 'v' : '^' );
60
 
//              Serial.print( " " );
61
 
//              Serial.println( next_event_at );
62
 
 
63
 
                if( event == 1 ) {
64
 
                        unsigned long width = MIN_PULSE_WIDTH +
65
 
                                (unsigned long)channels[ event / 2 ] *
66
 
                                ( MAX_PULSE_WIDTH - MIN_PULSE_WIDTH ) / MAX_CHANNEL_VALUE;
67
 
                        Serial.print( channels[ event / 2 ] );
68
 
                        Serial.print( " " );
69
 
                        Serial.println( width );
70
 
                }
71
 
        }
72
 
}