Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2118 → Rev 2119

/branches/dongfang_FC_fixedwing/analog.c
7,6 → 7,7
#include "attitude.h"
#include "printf_P.h"
#include "isqrt.h"
#include "sensors.h"
 
// for Delay functions
#include "timer0.h"
404,14 → 405,14
}
 
void analog_calibrate(void) {
#define OFFSET_CYCLES 64
#define OFFSET_CYCLES 250
uint8_t i, axis;
int32_t offsets[4] = { 0, 0, 0, 0};
int32_t offsets[4] = { 0, 0, 0, 0 };
gyro_calibrate();
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for (i = 0; i < OFFSET_CYCLES; i++) {
delay_ms_with_adc_measurement(10, 1);
delay_ms_with_adc_measurement(2, 1);
for (axis = PITCH; axis <= YAW; axis++) {
offsets[axis] += rawGyroValue(axis);
}
419,7 → 420,7
}
for (axis = PITCH; axis <= YAW; axis++) {
gyroOffset.offsets[axis] = (offsets[axis] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
gyroOffset.offsets[axis] = (offsets[axis] /*+ OFFSET_CYCLES/2 */) / OFFSET_CYCLES;
int16_t min = (512-200) * GYRO_OVERSAMPLING;
int16_t max = (512+200) * GYRO_OVERSAMPLING;
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
426,7 → 427,7
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
}
 
airpressureOffset = (offsets[3] + OFFSET_CYCLES / 2) / OFFSET_CYCLES;
airpressureOffset = (offsets[3] /* + OFFSET_CYCLES/2 */) / OFFSET_CYCLES;
int16_t min = 200;
int16_t max = 1024-200;
if(airpressureOffset < min || airpressureOffset > max)
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.c
89,7 → 89,7
 
// Servos
staticParams.servoCount = 7;
staticParams.servoManualMaxSpeed = 10;
staticParams.gimbalServoMaxManualSpeed = 10;
for (uint8_t i=0; i<2; i++) {
staticParams.servoConfigurations[i].manualControl = 128;
staticParams.servoConfigurations[i].stabilizationFactor = 0;
99,7 → 99,7
}
// Battery warning and emergency flight
staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S
staticParams.emergencyThrottle = 35;
staticParams.emergencyFlightDuration = 30;
 
/branches/dongfang_FC_fixedwing/arduino_atmega328/configuration.h
152,7 → 152,7
 
uint8_t IFactor;
 
uint8_t batteryVoltageWarning;
uint8_t batteryWarningVoltage;
uint8_t emergencyThrottle;
uint8_t emergencyFlightDuration;
 
/branches/dongfang_FC_fixedwing/arduino_atmega328/main.c
135,7 → 135,7
if (checkDelay(timer)) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.batteryVoltageWarning && UBat > UBAT_AT_5V) {
} else if (UBat < staticParams.batteryWarningVoltage && UBat > UBAT_AT_5V) {
beepBatteryAlarm();
}
 
/branches/dongfang_FC_fixedwing/configuration.c
4,6 → 4,7
#include "configuration.h"
#include "rc.h"
#include "output.h"
#include "sensors.h"
#include "flight.h"
 
int16_t variables[VARIABLE_COUNT];
36,8 → 37,8
{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, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255},
{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),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},
};
137,30 → 138,30
 
// Servos
staticParams.servoCount = 7;
staticParams.servoManualMaxSpeed = 10;
staticParams.servosReverse = CONTROL_SERVO_REVERSE_ELEVATOR | CONTROL_SERVO_REVERSE_RUDDER;
 
staticParams.gimbalServoMaxManualSpeed = 10;
for (uint8_t i=0; i<2; i++) {
staticParams.servoConfigurations[i].manualControl = 128;
staticParams.servoConfigurations[i].stabilizationFactor = 0;
staticParams.servoConfigurations[i].minValue = 32;
staticParams.servoConfigurations[i].maxValue = 224;
staticParams.servoConfigurations[i].flags = 0;
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.batteryVoltageWarning = 101; // 3.7 each for 3S
staticParams.emergencyThrottle = 35;
staticParams.emergencyFlightDuration = 30;
staticParams.batteryWarningVoltage = 101; // 3.7 each for 3S
 
for (uint8_t i=0; i<3; i++) {
staticParams.gyroPID[i].P = 80;
staticParams.gyroPID[i].I = 80;
staticParams.gyroPID[i].D = 40;
staticParams.gyroPID[i].iMax = 45;
staticParams.gyroPID[i].iMax = 30;
}
 
staticParams.stickIElevator = 80;
staticParams.stickIAilerons = 120;
staticParams.stickIRudder = 40;
staticParams.stickIElevator = 60;
staticParams.stickIAilerons = 80;
staticParams.stickIRudder = 25;
 
// Outputs
staticParams.outputFlash[0].bitmask = 1; //0b01011111;
193,7 → 194,7
IMUConfig.gyroDFilterConstant = 1;
IMUConfig.rateTolerance = 120;
IMUConfig.gyroDWindowLength = 3;
IMUConfig.gyroQuadrant = 0;
IMUConfig.gyroQuadrant = 2;
IMUConfig.imuReversedFlags = 0;
gyro_setDefaultParameters();
}
202,7 → 203,9
/* Default Values for R/C Channels */
/***************************************************/
void channelMap_default(void) {
channelMap.trim = 0;
channelMap.RCPolarity = 1;
channelMap.trim = 192;
channelMap.variableOffset = 128;
channelMap.channels[CH_ELEVATOR] = 1;
channelMap.channels[CH_AILERONS] = 0;
channelMap.channels[CH_THROTTLE] = 2;
/branches/dongfang_FC_fixedwing/configuration.h
92,7 → 92,7
/*PMM*/uint8_t output0Timing;
/*PMM*/uint8_t output1Timing;
 
uint8_t servoManualControl[2];
uint8_t gimbalServoManualControl[2];
 
/* P */uint8_t userParams[8];
} DynamicParams_t;
111,8 → 111,9
*/
 
typedef struct {
uint8_t trim;
uint8_t variableOffset;
uint8_t RCPolarity; // 1=positive, 0=negative. Use positive with Futaba receiver, negative with FrSky.
uint8_t trim;
uint8_t variableOffset;
uint8_t channels[MAX_CHANNELS];
} ChannelMap_t;
extern ChannelMap_t channelMap;
129,7 → 130,7
uint8_t flags;
} Servo_t;
 
//#define SERVO_STABILIZATION_REVERSE 1
#define SERVO_STABILIZATION_REVERSE 1
 
typedef struct {
uint8_t bitmask;
160,7 → 161,7
// Servos
 
uint8_t servoCount;
uint8_t gimbalServosReverse;
uint8_t servosReverse;
 
uint8_t controlServoMinValue;
uint8_t controlServoMaxValue;
/branches/dongfang_FC_fixedwing/flight.c
45,9 → 45,9
 
void flight_updateFlightParametersToFlightMode(void) {
debugOut.analog[16] = currentFlightMode;
reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS;
reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER;
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!
64,7 → 64,7
 
// Normal at airspeed = 10.
uint8_t calcAirspeedPID(uint8_t pid) {
if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) {
if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
return pid;
}
 
85,6 → 85,11
}
}
 
#define LOG_STICK_SCALE 8
#define LOG_P_SCALE 6
#define LOG_I_SCALE 10
#define LOG_D_SCALE 6
 
/************************************************************************/
/* Main Flight Control */
/************************************************************************/
107,9 → 112,9
term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
 
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7;
target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7;
target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7;
target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
 
for (axis = PITCH; axis <= YAW; axis++) {
if (target[axis] > OVER180) {
119,32 → 124,25
}
 
if (reverse[axis])
error[axis] = attitude[axis] + target[axis];
else
error[axis] = attitude[axis] - target[axis];
error[axis] = target[axis] - attitude[axis];
else
error[axis] = attitude[axis] - target[axis];
 
if (error[axis] > maxError[axis]) {
error[axis] = maxError[axis];
error[axis] = maxError[axis];
} else if (error[axis] < -maxError[axis]) {
error[axis] =- maxError[axis];
error[axis] =- maxError[axis];
} else {
// update I parts here for angles mode. Ĩ parts in rate mode is something different.
}
 
#define LOG_P_SCALE 6
#define LOG_I_SCALE 6
#define LOG_D_SCALE 4
 
/************************************************************************/
/* Calculate control feedback from angle (gyro integral) */
/* and angular velocity (gyro signal) */
/************************************************************************/
if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode
== FLIGHT_MODE_RATE) {
PDPart[axis] = (((int32_t) gyro_PID[axis]
* (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)
+ ((gyroD[axis] * (int16_t) airspeedPID[axis].D)
>> LOG_D_SCALE);
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 {
152,9 → 150,7
}
 
if (currentFlightMode == FLIGHT_MODE_ANGLES) {
int16_t anglePart = (int32_t)(
error[axis] * (int32_t) airspeedPID[axis].I)
>> LOG_I_SCALE;
int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
if (reverse[axis])
PDPart[axis] += anglePart;
else
162,7 → 158,7
}
 
// Add I parts here... these are integrated errors.
term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
}
 
debugOut.analog[12] = term[PITCH];
173,7 → 169,7
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
} else {
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder.
switch (i) {
/branches/dongfang_FC_fixedwing/main.c
142,7 → 142,7
if (UBat <= UBAT_AT_5V) {
// Do nothing. The voltage on the input side of the regulator is <5V;
// we must be running off USB power. Keep it quiet.
} else if (UBat < staticParams.batteryVoltageWarning) {
} else if (UBat < staticParams.batteryWarningVoltage) {
beepBatteryAlarm();
}
 
/branches/dongfang_FC_fixedwing/rc.c
47,16 → 47,22
 
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1)
// Set clock source to SYSCLK/8 (bit: CS12=0, CS11=1, CS10=0)
// Enable input capture noise cancler (bit: ICNC1=1)
// Trigger on positive edge of the input capture pin (bit: ICES1=1),
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10));
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12));
TCCR1B |= (1 << CS11) | (1 << ICES1) | (1 << ICNC1);
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B));
TCCR1A &= ~((1<<COM1A1)| (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0) | (1<<WGM11) | (1<<WGM10));
TCCR1B &= ~((1<<WGM13) | (1<<WGM12) | (1<<CS12));
TCCR1B |= (1<<CS11) | (1<<ICNC1);
TCCR1C &= ~((1<<FOC1A) | (1<<FOC1B));
 
if (channelMap.RCPolarity) {
TCCR1B |= (1<<ICES1);
} else {
TCCR1B &= ~(1<<ICES1);
}
 
// Timer/Counter1 Interrupt Mask Register
// Enable Input Capture Interrupt (bit: ICIE1=1)
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
232,6 → 238,6
// Do nothing.
}
 
int16_t RC_getZeroThrottle() {
int16_t RC_getZeroThrottle(void) {
return TIME (-0.5);
}
/branches/dongfang_FC_fixedwing/rc.h
29,7 → 29,7
int16_t RC_getVariable(uint8_t varNum);
void RC_calibrate(void);
uint8_t RC_getSignalQuality(void);
uint8_t RC_getLooping(uint8_t looping);
int16_t RC_getZeroThrottle(void);
#ifdef USE_MK3MAG
uint8_t RC_testCompassCalState(void);
#endif
/branches/dongfang_FC_fixedwing/timer2.c
81,7 → 81,7
TIMSK2 |= (1 << OCIE2A);
 
for (uint8_t axis=0; axis<2; axis++)
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR;
previousManualValues[axis] = dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR;
 
SREG = sreg;
}
93,9 → 93,9
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.
// That is a divisor of about 1<<14. Same conclusion as H&I.
value *= staticParams.servoConfigurations[axis].stabilizationFactor;
value *= staticParams.gimbalServoConfigurations[axis].stabilizationFactor;
value = value >> 8;
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
if (staticParams.gimbalServoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE)
return -value;
return value;
}
103,7 → 103,7
// With constant-speed limitation.
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) {
int16_t diff = manualValue - previousManualValues[axis];
uint8_t maxSpeed = staticParams.servoManualMaxSpeed;
uint8_t maxSpeed = staticParams.gimbalServoMaxManualSpeed;
if (diff > maxSpeed) diff = maxSpeed;
else if (diff < -maxSpeed) diff = -maxSpeed;
manualValue = previousManualValues[axis] + diff;
114,11 → 114,11
// 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) {
int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR);
int16_t value = calculateManualServoAxis(axis, dynamicParams.gimbalServoManualControl[axis] * SCALE_FACTOR);
value += calculateStabilizedServoAxis(axis);
int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR;
int16_t limit = staticParams.gimbalServoConfigurations[axis].minValue * SCALE_FACTOR;
if (value < limit) value = limit;
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR;
limit = staticParams.gimbalServoConfigurations[axis].maxValue * SCALE_FACTOR;
if (value > limit) value = limit;
value -= (128 * SCALE_FACTOR);
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
129,13 → 129,12
 
void calculateControlServoValues(void) {
int16_t value;
int16_t minLimit = staticParams.controlServoMinValue * SCALE_FACTOR;
int16_t maxLimit = staticParams.controlServoMaxValue * SCALE_FACTOR;
//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];
value *= 2;
if (value < minLimit) value = minLimit;
else if (value > maxLimit) value = maxLimit;
if (value < -SERVOLIMIT) value = -SERVOLIMIT;
else if (value > SERVOLIMIT) value = SERVOLIMIT;
servoValues[axis] = value + NEUTRAL_PULSELENGTH;
}
debugOut.analog[24] = servoValues[0];