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; |
} |
} |