1,10 → 1,11 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "rc.h" |
#include "output.h" |
#include "flight.h" |
#include "attitude.h" |
|
#define COARSERESOLUTION 1 |
// #define COARSERESOLUTION 1 |
|
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
26,7 → 27,6 |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
|
//volatile uint8_t servoActive = 0; |
volatile uint8_t recalculateServoTimes = 0; |
volatile uint16_t servoValues[MAX_SERVOS]; |
volatile uint16_t previousManualValues[2]; |
83,28 → 83,9 |
SREG = sreg; |
} |
|
/* |
void servo_On(void) { |
servoActive = 1; |
} |
void servo_Off(void) { |
servoActive = 0; |
HEF4017R_ON; // enable reset |
} |
*/ |
|
/***************************************************** |
* Control Servo Position |
* Control (camera gimbal etc.) servos |
*****************************************************/ |
|
/*typedef struct { |
uint8_t manualControl; |
uint8_t compensationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t;*/ |
|
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. |
136,14 → 117,6 |
if (value < limit) value = limit; |
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
if (value > limit) value = limit; |
return value; |
} |
|
uint16_t servoValue(uint8_t axis) { |
int16_t value; |
if (axis<2) value = featuredServoValue(axis); |
else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
// Shift out of the [0..255*SCALE_FACTOR] space |
value -= (128 * SCALE_FACTOR); |
if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
else if (value > SERVOLIMIT) value = SERVOLIMIT; |
151,11 → 124,33 |
return value + NEUTRAL_PULSELENGTH; |
} |
|
void calculateServoValues(void) { |
void calculateControlServoValues(void) { |
int16_t value; |
for (uint8_t axis=0; axis<3; axis++) { |
value = controlServos[axis]; |
value /= 2; |
servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
} |
debugOut.analog[18] = servoValues[0]; |
debugOut.analog[19] = servoValues[2]; |
} |
|
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 (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
servoValues[axis] = servoValue(axis); |
|
for (axis=0; axis<2; axis++) { |
value = featuredServoValue(axis); |
servoValues[axis + 3] = value; |
} |
for (axis=2; axis<MAX_SERVOS; axis++) { |
value = 128 * SCALE_FACTOR; |
servoValues[axis + 3] = value; |
} |
|
recalculateServoTimes = 0; |
} |
|