Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2141 → Rev 2142

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