/branches/dongfang_FC_fixedwing/analog.h |
---|
105,7 → 105,7 |
extern int16_t gyro_ATT[3]; |
extern int16_t gyroD[3]; |
#define GYRO_D_WINDOW_LENGTH 8 |
#define GYRO_D_WINDOW_LENGTH 16 |
extern uint16_t UBat; |
extern uint16_t airspeedVelocity; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/analog.c |
---|
190,11 → 190,11 |
int16_t rawGyroValue(uint8_t axis) { |
switch(axis) { |
case PITCH: |
return ITG3200SensorInputs[3]; |
return ITG3200SensorInputs[1]; |
case ROLL: |
return ITG3200SensorInputs[1]; |
return ITG3200SensorInputs[2]; |
case YAW: |
return ITG3200SensorInputs[2]; |
return ITG3200SensorInputs[3]; |
} |
return 0; |
} |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c |
---|
85,22 → 85,27 |
staticParams.IFactor = 52; |
staticParams.airspeedCorrection = 1; |
staticParams.isFlyingThreshold = 100; |
staticParams.isFlyingThreshold = 10; |
// Servos |
staticParams.servoCount = 7; |
staticParams.servoCount = 6; |
staticParams.servos[CONTROL_ELEVATOR].reverse = 0; |
staticParams.servos[CONTROL_AILERONS].reverse = 1; |
staticParams.servos[CONTROL_AILERONS].reverse = 0; // 1 for extra. |
staticParams.servos[CONTROL_RUDDER].reverse = 0; |
for (uint8_t i=0; i<4; i++) { |
staticParams.servos[i].minValue = 80; |
staticParams.servos[i].maxValue = 80; |
} |
// Battery warning and emergency flight |
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
for (uint8_t i=0; i<3; i++) { |
staticParams.gyroPID[i].P = 40; |
staticParams.gyroPID[i].I = 40; |
staticParams.gyroPID[i].D = 40; |
staticParams.gyroPID[i].iMax = 60; |
staticParams.gyroPID[i].I = 20; |
staticParams.gyroPID[i].D = 20; |
staticParams.gyroPID[i].iMax = 30; // 60 for extra. |
} |
staticParams.stickIElevator = 40; |
113,7 → 118,7 |
staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
staticParams.outputFlash[1].timing = 15; |
staticParams.outputDebugMask = 8; |
staticParams.outputDebugMask = 0; |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
} |
139,7 → 144,7 |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroDWindowLength = 8; |
IMUConfig.gyroQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_GYRO_PR; |
IMUConfig.imuReversedFlags = 0; |
} |
/***************************************************/ |
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h |
---|
153,7 → 153,7 |
// Servos |
uint8_t servoCount; |
Servo_t servos[3]; |
Servo_t servos[4]; |
// Outputs |
output_flash_t outputFlash[2]; |
/branches/dongfang_FC_fixedwing/arduino_atmega328/controlMixer.c |
---|
8,6 → 8,7 |
#include "commands.h" |
#include "output.h" |
// The -1024 is an arbrtrary, very low value that will shut off the motor. This is default until the control mixer runs for the first time. |
int16_t controls[4] = { 0, 0, 0, -1024 }; |
// Internal variables for reading commands made with an R/C stick. |
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c |
---|
103,7 → 103,7 |
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) { |
signal -= (TIME(1.5) - 128 + channelMap.HWTrim); |
if (abs(signal - PPM_in[channel]) < TIME(0.05)) { |
// With 7 channels and 50 frames/sec, weget 350 channel values/sec. |
// With 7 channels and 50 frames/sec, we get 350 channel values/sec. |
if (RCQuality < 200) |
RCQuality += 2; |
} |
138,6 → 138,7 |
int16_t channel = RCChannel(CH_THROTTLE); |
if (channel <= -TIME(0.55)) { |
debugOut.analog[17] = 1; |
int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL); |
if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal. |
lastRCCommand = COMMAND_GYROCAL; |
144,6 → 145,7 |
else |
lastRCCommand = COMMAND_RCCAL; |
} else { |
debugOut.analog[17] = 0; |
lastRCCommand = COMMAND_NONE; |
} |
return lastRCCommand; |
204,7 → 206,7 |
} |
int16_t RC_getZeroThrottle(void) { |
return TIME (0.95f); |
return TIME (1.0f); |
} |
void RC_setZeroTrim(void) { |
/branches/dongfang_FC_fixedwing/arduino_atmega328/timer2.c |
---|
139,15 → 139,17 |
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 = (staticParams.servos[axis].minValue * SERVO_NORMAL_LIMIT) >> 6; |
int16_t max = (staticParams.servos[axis].maxValue * SERVO_NORMAL_LIMIT) >> 6; |
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; |
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; |
} |
} |
173,6 → 175,8 |
} |
*/ |
uint8_t finalServoMap[] = {1,0,0,2,4,5,6,7}; |
ISR(TIMER2_COMPA_vect) { |
static uint16_t remainingPulseTime; |
static uint8_t servoIndex = 0; |
182,7 → 186,7 |
// 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[servoIndex]); |
sumOfPulseTimes += (remainingPulseTime = servoValues[finalServoMap[servoIndex]]); |
servoIndex++; |
} else { |
// There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
2,6 → 2,7 |
#include <stddef.h> |
#include <string.h> |
#include "configuration.h" |
#include "controlMixer.h" |
#include "rc.h" |
#include "output.h" |
#include "sensors.h" |
38,8 → 39,6 |
{offsetof(ParamSet_t, gyroPID[2].I), offsetof(DynamicParams_t, gyroPID[2].I),0,255}, |
{offsetof(ParamSet_t, gyroPID[2].D), offsetof(DynamicParams_t, gyroPID[2].D),0,255}, |
{offsetof(ParamSet_t, gimbalServoConfigurations[0].manualControl), offsetof(DynamicParams_t, gimbalServoManualControl[0]),0,255}, |
{offsetof(ParamSet_t, gimbalServoConfigurations[1].manualControl), offsetof(DynamicParams_t, gimbalServoManualControl[1]),0,255}, |
{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
}; |
134,22 → 133,15 |
staticParams.externalControl = 0; |
staticParams.IFactor = 52; |
staticParams.airspeedCorrection = 100; |
staticParams.isFlyingThreshold = 100; |
staticParams.airspeedCorrection = 1; |
staticParams.isFlyingThreshold = 10; |
// Servos |
staticParams.servoCount = 7; |
staticParams.servosReverse = CONTROL_SERVO_REVERSE_ELEVATOR | CONTROL_SERVO_REVERSE_RUDDER; |
staticParams.servos[CONTROL_ELEVATOR].reverse = 1; |
staticParams.servos[CONTROL_AILERONS].reverse = 0; |
staticParams.servos[CONTROL_RUDDER].reverse = 1; |
staticParams.gimbalServoMaxManualSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
staticParams.gimbalServoConfigurations[i].manualControl = 128; |
staticParams.gimbalServoConfigurations[i].stabilizationFactor = 0; |
staticParams.gimbalServoConfigurations[i].minValue = 32; |
staticParams.gimbalServoConfigurations[i].maxValue = 224; |
staticParams.gimbalServoConfigurations[i].flags = 0; |
} |
// Battery warning and emergency flight |
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S |
194,7 → 186,7 |
IMUConfig.gyroPIDFilterConstant = 8; |
IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.rateTolerance = 120; |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroDWindowLength = 8; |
IMUConfig.gyroQuadrant = 2; |
IMUConfig.imuReversedFlags = 0; |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
65,10 → 65,6 |
FLIGHT_MODE_ANGLES |
} FlightMode_t; |
#define CONTROL_SERVO_REVERSE_ELEVATOR 1 |
#define CONTROL_SERVO_REVERSE_AILERONS 2 |
#define CONTROL_SERVO_REVERSE_RUDDER 4 |
typedef struct { |
uint8_t SWMajor; |
uint8_t SWMinor; |
91,8 → 87,6 |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t gimbalServoManualControl[2]; |
/* P */uint8_t userParams[8]; |
} DynamicParams_t; |
103,13 → 97,7 |
uint8_t min, max; |
} MMXLATION; |
/* |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
} XLATION; |
*/ |
typedef struct { |
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky. |
uint8_t HWTrim; |
uint8_t variableOffset; |
129,11 → 117,9 |
} sensorOffset_t; |
typedef struct { |
uint8_t manualControl; |
uint8_t stabilizationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
uint8_t reverse; |
int8_t minValue; |
int8_t maxValue; |
} Servo_t; |
#define SERVO_STABILIZATION_REVERSE 1 |
167,14 → 153,8 |
// Servos |
uint8_t servoCount; |
uint8_t servosReverse; |
Servo_t servos[3]; |
uint8_t controlServoMinValue; |
uint8_t controlServoMaxValue; |
uint8_t gimbalServoMaxManualSpeed; |
Servo_t gimbalServoConfigurations[2]; // [PITCH, ROLL] |
// Outputs |
output_flash_t outputFlash[2]; |
uint8_t outputDebugMask; |
/branches/dongfang_FC_fixedwing/flight.c |
---|
23,7 → 23,6 |
* Error integrals. |
*/ |
uint8_t reverse[3]; |
int32_t maxError[3]; |
int32_t IPart[3] = { 0, 0, 0 }; |
PID_t airspeedPID[3]; |
44,10 → 43,6 |
void flight_updateFlightParametersToFlightMode(void) { |
debugOut.analog[16] = currentFlightMode; |
reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
// At a switch to angles, we want to kill errors first. |
// This should be triggered only once per mode change! |
85,11 → 80,6 |
} |
} |
#define LOG_STICK_SCALE 8 |
#define LOG_P_SCALE 6 |
#define LOG_I_SCALE 10 |
#define LOG_D_SCALE 6 |
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
115,13 → 105,13 |
int32_t tmp; |
tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp; |
if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp; |
tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp; |
if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp; |
tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp; |
if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp; |
for (axis = PITCH; axis <= YAW; axis++) { |
if (target[axis] > OVER180) { |
192,8 → 182,6 |
if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
+ (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
//if (reverse[axis]) |
// PDPart[axis] = -PDPart[axis]; |
} else { |
PDPart[axis] = 0; |
} |
204,17 → 192,12 |
} |
// Add I parts here... these are integrated errors. |
if (reverse[axis]) |
if (staticParams.servos[axis].reverse) |
term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
else |
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
} |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
debugOut.analog[15] = term[THROTTLE]; |
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
int16_t tmp; |
if (servoTestActive) { |
264,5 → 247,6 |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = term[YAW]; |
debugOut.analog[15] = term[THROTTLE]; |
} |
} |
/branches/dongfang_FC_fixedwing/flight.h |
---|
14,6 → 14,11 |
#define YAW 2 |
#define THROTTLE 3 |
#define LOG_STICK_SCALE 8 |
#define LOG_P_SCALE 6 |
#define LOG_I_SCALE 10 |
#define LOG_D_SCALE 6 |
#define NUM_CONTROL_SERVOS 4 |
extern int16_t controlServos[NUM_CONTROL_SERVOS]; |
/branches/dongfang_FC_fixedwing/main.c |
---|
140,7 → 140,7 |
beepBatteryAlarm(); |
} |
} |
calculateFeaturedServoValues(); |
//calculateFeaturedServoValues(); |
timer = setDelay(20); // every 20 ms |
} |
/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; |
} |
} |