Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2135 → Rev 2142

/branches/dongfang_FC_fixedwing/timer2.c
10,26 → 10,24
 
#ifdef COARSERESOLUTION
#define NEUTRAL_PULSELENGTH ((int16_t)(F_CPU/32000*1.5f + 0.5f))
#define STABILIZATION_LOG_DIVIDER 6
#define SERVOLIMIT ((int16_t)(F_CPU/32000*0.8f + 0.5f))
#define SCALE_FACTOR 4
#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 STABILIZATION_LOG_DIVIDER 4
#define SERVOLIMIT ((int16_t)(F_CPU/8000.0f * 0.8f + 0.5f))
#define SCALE_FACTOR 16
#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 + SERVOLIMIT) * (uint16_t)staticParams.servoCount + 128)
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT)
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT)
#define FRAMELENGTH ((uint16_t)(NEUTRAL_PULSELENGTH + SERVO_ABS_LIMIT) * (uint16_t)staticParams.servoCount + 128)
 
volatile uint8_t recalculateServoTimes = 0;
volatile uint16_t servoValues[MAX_SERVOS];
volatile uint16_t previousManualValues[2];
//volatile uint8_t recalculateServoTimes = 0;
//volatile uint16_t previousManualValues[2];
 
#define HEF4017R_ON PORTC |= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
77,8 → 75,8
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;
}
86,6 → 84,7
/*****************************************************
* Control (camera gimbal etc.) servos
*****************************************************/
/*
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.
107,7 → 106,9
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) {
123,19 → 124,28
// Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space.
return value + NEUTRAL_PULSELENGTH;
}
*/
 
void calculateControlServoValues(void) {
int16_t value;
//int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR;
//int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR;
for (uint8_t axis=0; axis<4; axis++) {
value = controlServos[axis];
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
else if (value > SERVOLIMIT) value = SERVOLIMIT;
 
// Apply configurable limits. These are signed: +-128 is twice the normal +- 0.5 ms limit and +- 64 is normal.
int16_t min = (staticParams.servos[axis].minValue * SERVO_NORMAL_LIMIT) >> 6;
int16_t max = (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;
 
servoValues[axis] = value + NEUTRAL_PULSELENGTH;
}
}
 
/*
void calculateFeaturedServoValues(void) {
int16_t value;
uint8_t axis;
154,6 → 164,7
 
recalculateServoTimes = 0;
}
*/
 
ISR(TIMER2_COMPA_vect) {
static uint16_t remainingPulseTime;
170,7 → 181,7
// 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;
//recalculateServoTimes = 1;
HEF4017R_ON;
}
}