Subversion Repositories FlightCtrl

Rev

Rev 1910 | Rev 2102 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1910 Rev 2099
Line 1... Line 1...
1 
#include <avr/io.h>
 1 
#include <avr/io.h>
2 
#include <avr/interrupt.h>
 2 
#include <avr/interrupt.h>
3 
#include "timer2.h"
 - 
 
4 
#include "eeprom.h"
 3 
#include "eeprom.h"
5 
#include "uart0.h"
 - 
 
6 
#include "rc.h"
 4 
#include "rc.h"
7 
#include "attitude.h"
 5 
#include "attitude.h"
Line 8... Line -...
8 
 
 - 
 
9 
#define HEF4017_RESET_HIGH    PORTC |=  (1<<PORTC6)
 6 
 
Line -... Line 7...
- 
 
 7 
#define COARSERESOLUTION 1
- 
 
 8 
 
10 
#define HEF4017_RESET_LOW     PORTC &= ~(1<<PORTC6)
 9 
#ifdef COARSERESOLUTION
- 
 
 10 
#define NEUTRAL_PULSELENGTH 938
- 
 
 11 
#define STABILIZATION_LOG_DIVIDER 6
11 
 
 12 
#define SERVOLIMIT 500
- 
 
 13 
#define SCALE_FACTOR 4
- 
 
 14 
#define CS2 ((1<<CS21)|(1<<CS20))
- 
 
 15 
 
- 
 
 16 
#else
- 
 
 17 
#define NEUTRAL_PULSELENGTH 3750
- 
 
 18 
#define STABILIZATION_LOG_DIVIDER 4
- 
 
 19 
#define SERVOLIMIT 2000
- 
 
 20 
#define SCALE_FACTOR 16
- 
 
 21 
#define CS2 (1<<CS21)
- 
 
 22 
#endif
- 
 
 23 
 
- 
 
 24 
#define MAX_SERVOS 8
- 
 
 25 
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128)
Line -... Line 26...
- 
 
 26 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
- 
 
 27 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
12 
#define HEF4017R_ON PORTC |=  (1<<PORTC6)
 28 
 
- 
 
 29 
//volatile uint8_t servoActive = 0;
- 
 
 30 
volatile uint8_t recalculateServoTimes = 0;
- 
 
 31 
volatile uint16_t servoValues[MAX_SERVOS];
- 
 
 32 
volatile uint16_t previousManualValues[2];
Line 13... Line 33...
13 
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
 33 
 
14 
 
 34 
#define HEF4017R_ON     PORTC |=  (1<<PORTC6)
15 
OutputData_t outputs[MAX_OUTPUTS];
 35 
#define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
16 
 
 36 
 
17 
/*****************************************************
 37 
/*****************************************************
18 
 *              Initialize Timer 2                  
 38 
 *              Initialize Timer 2
19 
 *****************************************************/
 39 
 *****************************************************/
20 
void timer2_init(void) {
 40 
void timer2_init(void) {
21 
  uint8_t sreg = SREG;
 41 
    uint8_t sreg = SREG;
22 
 
 42 
 
23 
  // disable all interrupts before reconfiguration
 43 
    // disable all interrupts before reconfiguration
24 
  cli();
 44 
    cli();
25 
 
 45 
 
26 
  // set PD7 as output of the PWM for pitch servo
 46 
    // set PD7 as output of the PWM for pitch servo
27 
  DDRD |= (1 << DDD7);
 - 
 
28 
  PORTD &= ~(1 << PORTD7); // set PD7 to low
 47 
    DDRD |= (1 << DDD7);
29 
 
 48 
    PORTD &= ~(1 << PORTD7); // set PD7 to low
30 
  DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017)
 49 
 
31 
  //PORTC &= ~(1<<PORTC6);      // set PC6 to low
 - 
 
32 
  HEF4017_RESET_HIGH; // enable reset
 50 
    DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017)
33 
 
 51 
    HEF4017R_ON; // enable reset
34 
  // Timer/Counter 2 Control Register A
 52 
 
35 
 
 53 
    // Timer/Counter 2 Control Register A
36 
  // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0)
 54 
    // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0)
37 
  // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
 55 
    // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0)
38 
  // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
 56 
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
39 
  TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0));
 57 
    TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22));
40 
  TCCR2A |= (1 << WGM21);
 58 
    TCCR2A |= (1 << COM2A1) | (1 << WGM21);
41 
 
 59 
 
42 
  // Timer/Counter 2 Control Register B
 60 
    // Timer/Counter 2 Control Register B
43 
 
 61 
 
44 
  // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz
 - 
 
45 
  // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us
 62 
    // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz
46 
  // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms
 63 
    // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us
47 
 
 64 
    // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms
48 
  // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1)
 65 
 
49 
  TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS22));
 66 
    TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22));
50 
  TCCR2B |= (1 << CS21) | (1 << CS20);
 67 
    TCCR2B |= CS2;
51 
 
 68 
 
52 
  // Initialize the Timer/Counter 2 Register
 69 
    // Initialize the Timer/Counter 2 Register
53 
  TCNT2 = 0;
 - 
 
54 
 
 70 
    TCNT2 = 0;
55 
  // Initialize the Output Compare Register A used for signal generation on port PD7.
 71 
 
56 
  OCR2A = 255;
 72 
    // Initialize the Output Compare Register A used for signal generation on port PD7.
57 
  TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0
 73 
    OCR2A = 255;
58 
 
 74 
 
- 
 
 75 
    // Timer/Counter 2 Interrupt Mask Register
- 
 
 76 
    // Enable timer output compare match A Interrupt only
- 
 
 77 
    TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
Line 59... Line 78...
59 
  // Timer/Counter 2 Interrupt Mask Register
 78 
    TIMSK2 |= (1 << OCIE2A);
60 
  // Enable timer output compare match A Interrupt only
 79 
 
Line -... Line 80...
- 
 
 80 
    for (uint8_t axis=0; axis<2; axis++)
- 
 
 81 
      previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
- 
 
 82 
 
- 
 
 83 
    SREG = sreg;
- 
 
 84 
}
- 
 
 85 
 
- 
 
 86 
/*
- 
 
 87 
void servo_On(void) {
- 
 
 88 
    servoActive = 1;
- 
 
 89 
}
61 
  TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
 90 
void servo_Off(void) {
62 
  TIMSK2 |= (1 << OCIE2A);
 91 
    servoActive = 0;
63 
 
 92 
    HEF4017R_ON; // enable reset
- 
 
 93 
}
- 
 
 94 
*/
- 
 
 95 
 
- 
 
 96 
/*****************************************************
- 
 
 97 
 * Control Servo Position
- 
 
 98 
 *****************************************************/
- 
 
 99 
 
- 
 
 100 
/*typedef struct {
- 
 
 101 
  uint8_t manualControl;
- 
 
 102 
  uint8_t compensationFactor;
- 
 
 103 
  uint8_t minValue;
- 
 
 104 
  uint8_t maxValue;
64 
  SREG = sreg;
 105 
  uint8_t flags;
- 
 
 106 
} servo_t;*/
- 
 
 107 
 
- 
 
 108 
int16_t calculateStabilizedServoAxis(uint8_t axis) {
- 
 
 109 
  int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
- 
 
 110 
  // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
- 
 
 111 
  // That is a divisor of about 1<<14. Same conclusion as H&I.
- 
 
 112 
  value *= staticParams.servoConfigurations[axis].stabilizationFactor;
65 
}
 113 
  value = value >> 8;
- 
 
 114 
  if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
- 
 
 115 
    return -value;
- 
 
 116 
  return value;
- 
 
 117 
}
- 
 
 118 
 
- 
 
 119 
// With constant-speed limitation.
- 
 
 120 
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
66 
 
 121 
  int16_t diff = manualValue - previousManualValues[axis];
- 
 
 122 
  uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
- 
 
 123 
  if (diff > maxSpeed) diff = maxSpeed;
- 
 
 124 
  else if (diff < -maxSpeed) diff = -maxSpeed;
- 
 
 125 
  manualValue = previousManualValues[axis] + diff;
- 
 
 126 
  previousManualValues[axis] = manualValue;
- 
 
 127 
  return manualValue;
- 
 
 128 
}
- 
 
 129 
 
- 
 
 130 
// add stabilization and manual, apply soft position limits.
- 
 
 131 
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
- 
 
 132 
int16_t featuredServoValue(uint8_t axis) {
- 
 
 133 
  int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR);
- 
 
 134 
  value += calculateStabilizedServoAxis(axis);
- 
 
 135 
  int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
- 
 
 136 
  if (value < limit) value = limit;
- 
 
 137 
  limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
- 
 
 138 
  if (value > limit) value = limit;
67 
/*****************************************************
 139 
  return value;
- 
 
 140 
}
- 
 
 141 
 
- 
 
 142 
uint16_t servoValue(uint8_t axis) {
68 
 * Control Servo Position              
 143 
  int16_t value;
69 
 *****************************************************/
 144 
  if (axis<2) value = featuredServoValue(axis);
- 
 
 145 
  else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8.
- 
 
 146 
  // Shift out of the [0..255*SCALE_FACTOR] space
- 
 
 147 
  value -= (128 * SCALE_FACTOR);
- 
 
 148 
  if (value < -SERVOLIMIT) value = -SERVOLIMIT;
- 
 
 149 
  else if (value > SERVOLIMIT) value = SERVOLIMIT;
- 
 
 150 
  // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
- 
 
 151 
  return value + NEUTRAL_PULSELENGTH;
- 
 
 152 
}
- 
 
 153 
 
- 
 
 154 
void calculateServoValues(void) {
Line 70... Line 155...
70 
const uint8_t SERVO_REMAPPING[MAX_OUTPUTS] = {0,0,1,2,3,4,5,6};
 155 
  if (!recalculateServoTimes) return;
71 
#define NEUTRAL_PULSELENGTH 937
 156 
  for (uint8_t axis=0; axis<MAX_SERVOS; axis++) {
72 
#define SERVOLIMIT 500
 157 
    servoValues[axis] = servoValue(axis);
73 
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.ServoRefresh + 128)
 158 
  }
Line 74... Line 159...
74 
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
 159 
  recalculateServoTimes = 0;
75 
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
 160 
}
76 
 
 161 
 
77 
ISR(TIMER2_COMPA_vect) {
 162 
ISR(TIMER2_COMPA_vect) {
78 
  static uint16_t remainingPulseTime = 0;
 - 
 
79 
  static uint8_t servoIndex = 0;
 - 
 
80 
  static uint16_t sumOfPulseTimes = 0;
 - 
 
81 
 
 163 
  static uint16_t remainingPulseTime;
82 
  if (!remainingPulseTime) {
 164 
  static uint8_t servoIndex = 0;
83 
    // Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
 165 
  static uint16_t sumOfPulseTimes = 0;
84 
    if (servoIndex < staticParams.ServoRefresh) {
 166 
 
85 
      // There are more signals to output.
 167 
  if (!remainingPulseTime) {
86 
      remainingPulseTime = NEUTRAL_PULSELENGTH + outputs[SERVO_REMAPPING[servoIndex]].SetPoint;
 168 
    // Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
- 
 
 169 
    if (servoIndex < staticParams.servoCount) {
87 
      if (remainingPulseTime < MIN_PULSELENGTH) remainingPulseTime = MIN_PULSELENGTH;
 170 
      // There are more signals to output.
88 
      else if (remainingPulseTime > MAX_PULSELENGTH) remainingPulseTime = MAX_PULSELENGTH;
 171 
      sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]);
89 
      sumOfPulseTimes += remainingPulseTime;
 172 
      servoIndex++;
Line 90... Line 173...
90 
      servoIndex++;
 173 
    } else {
91 
    } else {
 - 
 
92 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
 174 
      // There are no more signals. Reset the counter and make this pulse cover the missing frame time.
93 
      remainingPulseTime = FRAMELEN - sumOfPulseTimes;
 - 
 
94 
      sumOfPulseTimes = servoIndex = 0;
 175 
      remainingPulseTime = FRAMELEN - sumOfPulseTimes;
95 
      HEF4017_RESET_HIGH;
 176 
      sumOfPulseTimes = servoIndex = 0;
- 
 
 177 
      recalculateServoTimes = 1;
96 
    }
 178 
      HEF4017R_ON;
- 
 
 179 
    }
97 
  }
 180 
  }
98 
 
 181 
 
99 
  // Schedule the next OCR2A event. The counter is already reset at this time.
 182 
  // Schedule the next OCR2A event. The counter is already reset at this time.
100 
  uint8_t delta;
 183 
  if (remainingPulseTime > 256+128) {
- 
 
 184 
    // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
101 
  if (remainingPulseTime > 255+128) {
 185 
    // as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
102 
    delta = 255;
 186 
    OCR2A = 255;
103 
    // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
- 
 
104 
    // as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
 187 
    TCCR2A &= ~(1<<COM2A0);
105 
    TCCR2A &= ~(1<<COM2A0);
 188 
    remainingPulseTime-=256;
- 
 
 189 
  } else if (remainingPulseTime > 256) {
- 
 
 190 
    // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
106 
  } else if (remainingPulseTime > 255) {
 191 
    // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
107 
    // Remaining pulse lengths in the range [256..256+something small] might cause trouble if handled the standard
192 
    // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
108 
    // way, which is in chunks of 255. The remainder would be very small, possibly causing an interrupt on interrupt
 - 
 
109 
    // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
 - 
 
110 
    delta = 128;
 193 
    remainingPulseTime-=128;