/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]; |