Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2101 → Rev 2102

/branches/dongfang_FC_fixedwing/timer2.c
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;
}