Rev 2139 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#include <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
#include "output.h"
#include "flight.h"
#include "attitude.h"
#include "timer2.h"
// #define COARSERESOLUTION 1
#ifdef COARSERESOLUTION
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f))
#define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/32000*0.5f + 0.5f))
#define SERVO_ABS_LIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f))
//#define SCALE_FACTOR 4
#define CS2 ((1<<CS21)|(1<<CS20))
#else
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/8000.0f * 1.5f + 0.5f))
#define SERVO_NORMAL_LIMIT ((int16_t)(F_CPU/8000.0f * 0.5f + 0.5f))
#define SERVO_ABS_LIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f))
//#define SCALE_FACTOR 16
#define CS2 (1<<CS21)
#endif
#define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128)
volatile uint16_t servoValues[MAX_SERVOS];
//volatile uint8_t recalculateServoTimes = 0;
//volatile uint16_t previousManualValues[2];
#define HEF4017R_ON PORTD |= (1<<PORTD3)
#define HEF4017R_OFF PORTD &= ~(1<<PORTD3)
/*****************************************************
* Initialize Timer 2
*****************************************************/
void timer2_init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
// set PD7 as output of the 4017 clk
DDRB |= (1 << DDB3);
PORTB &= ~(1 << PORTB3); // set PD7 to low
// oc2b DDRD |= (1 << DDD4); // set PC6 as output (Reset for HEF4017)
DDRD |= (1 << DDD3); // set PC6 as output (Reset for HEF4017)
HEF4017R_ON; // reset
// Timer/Counter 2 Control Register A
// Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0)
// PD3: Output OCR2 match, (Bits: COM2B1 = 1, COM2B0 = 0)
// PB3: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
// ardu TCCR2A &= ~((1 << COM2B0) | (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM22));
// ardu TCCR2A |= (1 << COM2B1) | (1 << WGM21);
TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22));
TCCR2A |= (1 << COM2A1) | (1 << WGM21);
// Timer/Counter 2 Control Register B
// Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz
// The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms
TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22));
TCCR2B |= CS2;
// Initialize the Timer/Counter 2 Register
TCNT2 = 0;
// Initialize the Output Compare Register A used for signal generation on port PD7.
OCR2A = 255;
// Timer/Counter 2 Interrupt Mask Register
// Enable timer output compare match A Interrupt only
TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2));
TIMSK2 |= (1 << OCIE2A);
//for (uint8_t axis=0; axis<2; axis++)
// previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
for (uint8_t i=0; i<MAX_SERVOS; i++)
servoValues[i] = NEUTRAL_PULSELENGTH;
SREG = sreg;
}
/*****************************************************
* Control (camera gimbal etc.) servos
* Makes no sense as long as we have no acc. reference.
*****************************************************/
/*
int16_t calculateStabilizedServoAxis(uint8_t axis) {
int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about
// With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000.
// That is a divisor of about 1<<14. Same conclusion as H&I.
value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor;
value = value >> 8;
if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
return -value;
return value;
}
// With constant-speed limitation.
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
int16_t diff = manualValue - previousManualValues[axis];
uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed;
if (diff > maxSpeed) diff = maxSpeed;
else if (diff < -maxSpeed) diff = -maxSpeed;
manualValue = previousManualValues[axis] + diff;
previousManualValues[axis] = manualValue;
return manualValue;
}
*/
/*
// add stabilization and manual, apply soft position limits.
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally)
int16_t featuredServoValue(uint8_t axis) {
int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
value += calculateStabilizedServoAxis(axis);
int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR;
if (value < limit) value = limit;
limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR;
if (value > limit) value = limit;
value -= (128 * SCALE_FACTOR);
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
else if (value > SERVOLIMIT) value = SERVOLIMIT;
// Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
return value + NEUTRAL_PULSELENGTH;
}
*/
void calculateControlServoValues(void) {
int16_t value;
for (uint8_t axis=0; axis<4; axis++) {
value = controlServos[axis];
// Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal.
int16_t min = (int16_t)staticParams.servos[axis].minValue * (SERVO_NORMAL_LIMIT >> 6);
int16_t max = (int16_t)staticParams.servos[axis].maxValue * (SERVO_NORMAL_LIMIT >> 6);
if (value < -min) value = -min;
else if (value > max) value = max;
if (value < -SERVO_ABS_LIMIT) value = -SERVO_ABS_LIMIT;
else if (value > SERVO_ABS_LIMIT) value = SERVO_ABS_LIMIT;
debugOut.analog[24+axis] = value;
servoValues[axis] = value + NEUTRAL_PULSELENGTH;
}
}
/*
void calculateFeaturedServoValues(void) {
int16_t value;
uint8_t axis;
// Save the computation cost of computing a new value before the old one is used.
if (!recalculateServoTimes) return;
for (axis= MAX_CONTROL_SERVOS; axis<MAX_CONTROL_SERVOS+2; axis++) {
value = featuredServoValue(axis-MAX_CONTROL_SERVOS);
servoValues[axis] = value;
}
for (axis=MAX_CONTROL_SERVOS+2; axis<MAX_SERVOS; axis++) {
value = 128 * SCALE_FACTOR;
servoValues[axis] = value;
}
recalculateServoTimes = 0;
}
*/
uint8_t finalServoMap[] = {1,0,0,2,4,5,6,7};
ISR(TIMER2_COMPA_vect) {
static uint16_t remainingPulseTime;
static uint8_t servoIndex = 0;
static uint16_t sumOfPulseTimes = 0;
if (!remainingPulseTime) {
// Pulse is over, and the next pulse has already just started. Calculate length of next pulse.
if (servoIndex < staticParams.servoCount) {
// There are more signals to output.
sumOfPulseTimes += (remainingPulseTime = servoValues[finalServoMap[servoIndex]]);
servoIndex++;
} else {
// There are no more signals. Reset the counter and make this pulse cover the missing frame time.
remainingPulseTime = FRAMELENGTH - sumOfPulseTimes;
sumOfPulseTimes = servoIndex = 0;
//recalculateServoTimes = 1;
HEF4017R_ON;
}
}
// Schedule the next OCR2A event. The counter is already reset at this time.
if (remainingPulseTime > 256+128) {
// Set output to reset to zero at next OCR match. It does not really matter when the output is set low again,
// as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
OCR2A = 255;
TCCR2A &= ~(1<<COM2A0);
remainingPulseTime-=256;
} else if (remainingPulseTime > 256) {
// Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard
// way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt
// condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
remainingPulseTime-=128;
OCR2A=127;
} else {
// Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low
TCCR2A |= (1<<COM2A0);
OCR2A = remainingPulseTime-1;
remainingPulseTime=0;
HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0.
}
}