Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1926 → Rev 1927

/branches/dongfang_FC_fixedwing/analog.c
439,10 → 439,10
int32_t deltaOffsets[3] = { 0, 0, 0 };
 
// Set the filters... to be removed again, once some good settings are found.
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & (0x7 & (1<<0))) + 1;
GYROS_PID_FILTER = (staticParams.sensorFilterSettings & (0x7 & (1<<0))) + 1;
GYROS_ATT_FILTER = 1;
GYROS_D_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<4))) + 1;
ACC_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<6))) + 1;
GYROS_D_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<4))) + 1;
ACC_FILTER = (staticParams.sensorFilterSettings & (0x3 & (1<<6))) + 1;
 
gyro_calibrate();
 
/branches/dongfang_FC_fixedwing/attitude.c
44,7 → 44,7
* horizontal plane, yaw relative to yaw at start. Not really used for anything else
* than diagnostics.
*/
int32_t angle[2];
int32_t angle[3];
 
/*
* Error integrals. Stick is always positive. Gyro is configurable positive or negative.
52,21 → 52,6
*/
int32_t error[3];
 
// Yaw angle and compass stuff.
// This is updated/written from MM3. Negative angle indicates invalid data.
int16_t compassHeading = -1;
 
// This is NOT updated from MM3. Negative angle indicates invalid data.
int16_t compassCourse = -1;
 
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
// Not necessary. Never read anywhere.
// int16_t compassOffCourse = 0;
 
uint8_t updateCompassCourse = 0;
uint8_t compassCalState = 0;
uint16_t ignoreCompassTimer = 500;
 
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
 
int16_t correctionSum[2] = { 0, 0 };
119,11 → 104,8
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
 
// update compass course to current heading
compassCourse = compassHeading;
 
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
angle[YAW] = 0;
 
// Servo_On(); //enable servo output
}
159,43 → 141,35
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
uint8_t axis;
 
// TODO: Multiply on a factor. Wont work without...
error[PITCH] += control[CONTROL_ELEVATOR];
error[ROLL] += control[CONTROL_AILERONS];
error[YAW] += control[CONTROL_RUDDER];
// TODO: Multiply on a factor on control. Wont work without...
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
} else {
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
 
angle[PITCH] += rate_ATT[PITCH];
angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
} else {
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]);
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
error[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
angle[PITCH] += rate_ATT[PITCH];
angle[ROLL] += control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]);
angle[YAW] += control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? yawRate : -yawRate);
}
 
for (axis=PITCH; axis<=YAW; axis++) {
// TODO: Configurable.
#define ERRORLIMIT 1000
for (axis=PITCH; axis<=YAW; axis++) {
if (error[axis] > ERRORLIMIT) {
error[axis] = ERRORLIMIT;
} else if (angle[axis] <= -ERRORLIMIT) {
angle[axis] = -ERRORLIMIT;
}
}
}
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
 
if (yawGyroHeading >= YAWOVER360) {
yawGyroHeading -= YAWOVER360; // 360 deg. wrap
} else if (yawGyroHeading < 0) {
yawGyroHeading += YAWOVER360;
}
 
/*
* Pitch axis integration and range boundary wrap.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
206,6 → 180,18
angle[axis] += PITCHROLLOVER360;
}
}
 
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
if (angle[YAW] >= YAWOVER360) {
angle[YAW] -= YAWOVER360; // 360 deg. wrap
} else if (angle[YAW] < 0) {
angle[YAW] += YAWOVER360;
}
}
 
/************************************************************************
221,7 → 207,7
// Well actually the Z axis acc. check is not so silly.
uint8_t axis;
int32_t temp;
if (acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
if (acc[Z] >= -staticParams.accCorrectionZAccLimit && acc[Z]
<= dynamicParams.UserParams[7]) {
DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
 
/branches/dongfang_FC_fixedwing/attitude.h
86,22 → 86,10
/*
* Attitudes calculated by numerical integration of gyro rates
*/
extern int32_t angle[2];
extern int32_t angle[3];
extern int32_t error[3];
 
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c
 
/*
* Compass navigation
*/
extern int16_t compassHeading;
extern int16_t compassCourse;
// extern int16_t compassOffCourse;
extern uint8_t compassCalState;
extern int32_t yawGyroHeading;
extern uint8_t updateCompassCourse;
extern uint16_t ignoreCompassTimer;
 
/*
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves
* can be updated in flight by different ways, for example:
/branches/dongfang_FC_fixedwing/commands.c
12,7 → 12,6
*/
uint8_t command = controlMixer_getCommand();
uint8_t repeated = controlMixer_isCommandRepeated();
uint8_t argument = controlMixer_getArgument();
 
if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
if (command == COMMAND_GYROCAL && !repeated) {
25,29 → 24,15
// if pitch stick is top or roll stick is left or right --> change parameter setting
// according to roll/pitch stick position
 
if (argument < 6) {
// Gyro calinbration, with or without selecting a new parameter-set.
if (argument > 0 && argument < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(argument);
}
ParamSet_ReadFromEEProm(getActiveParamSet());
ParamSet_ReadFromEEProm();
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
} else if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE
| CFG_GPS_ACTIVE) && argument == 7) {
// If right stick is centered and down
// TODO: Out of here! State machine instead.
compassCalState = 1;
beep(1000);
}
beepNumber(1);
}
 
// save the ACC neutral setting to eeprom
else {
if (command == COMMAND_ACCCAL && !repeated) {
else if (command == COMMAND_ACCCAL && !repeated) {
// Run gyro and acc. meter calibration but do not repeat it.
GRN_OFF;
analog_calibrateAcc();
54,8 → 39,7
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
}
beepNumber(1);
}
} // end !MOTOR_RUN condition.
if (command == COMMAND_START) {
/branches/dongfang_FC_fixedwing/configuration.c
6,7 → 6,7
#include "uart0.h"
 
int16_t variables[8] = {0,0,0,0,0,0,0,0};
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
dynamicParam_t dynamicParams;// = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
uint8_t CPUType = ATMEGA644;
uint8_t BoardRelease = 13;
 
32,10 → 32,10
SET_POT(dynamicParams.GyroRollD,staticParams.GyroRollD);
SET_POT(dynamicParams.GyroYawD,staticParams.GyroYawD);
 
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
for (i=0; i<sizeof(staticParams.UserParams); i++) {
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams[i]);
}
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
 
/branches/dongfang_FC_fixedwing/configuration.h
95,6 → 95,9
 
uint8_t ExternalControl; // for serial Control
uint8_t BitConfig; // see upper defines for bitcoding
 
uint8_t accCorrectionZAccLimit;
uint8_t sensorFilterSettings;
uint8_t Reserved[4];
} paramset_t;
/branches/dongfang_FC_fixedwing/eeprom.c
32,20 → 32,13
*/
void setDefaultUserParams(void) {
uint8_t i;
for (i = 0; i < sizeof(staticParams.UserParams1); i++) {
staticParams.UserParams1[i] = 0;
for (i = 0; i < sizeof(staticParams.UserParams); i++) {
staticParams.UserParams[i] = 0;
}
for (i = 0; i < sizeof(staticParams.UserParams2); i++) {
staticParams.UserParams2[i] = 0;
}
/*
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams2[0] = 0xd5; //0b11010101; // All gyro filter constants 2; acc. 4
staticParams.UserParams2[1] = 0; // H&I motor smoothing.
staticParams.UserParams2[2] = 120; // Yaw I factor
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles.
}
 
void setOtherDefaults(void) {
82,7 → 75,6
staticParams.StickRudderP = 10;
 
staticParams.LowVoltageWarning = 105;
staticParams.IFactor = 32;
staticParams.ServoRefresh = 7;
staticParams.BitConfig = 0;
staticParams.J16Bitmask = 95;
89,49 → 81,18
staticParams.J17Bitmask = 243;
staticParams.J16Timing = 15;
staticParams.J17Timing = 15;
 
staticParams.ControlSigns = 2;
}
 
/***************************************************/
/* Default Values for parameter set 1 */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
void setDefaults(void) {
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.J16Timing = 10;
staticParams.J17Timing = 10;
memcpy(staticParams.Name, "Sport\0", 6);
}
 
/***************************************************/
/* Default Values for parameter set 2 */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.Height_Gain = 3;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
}
 
/***************************************************/
/* Default Values for parameter set 3 */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.Height_Gain = 3;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
}
 
/***************************************************/
/* Read Parameter from EEPROM as byte */
/***************************************************/
uint8_t GetParamByte(uint16_t param_id) {
165,12 → 126,9
/* Read Parameter Set from EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber) {
if ((1 > setnumber) || (setnumber > 5))
setnumber = 3;
void ParamSet_ReadFromEEProm() {
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], PARAMSET_STRUCT_LEN);
output_init();
}
 
178,20 → 136,14
/* Write Parameter Set to EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_WriteToEEProm(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
return;
void ParamSet_WriteToEEProm() {
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], PARAMSET_STRUCT_LEN);
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH],
PARAMSET_STRUCT_LEN);
eeprom_write_block(&staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init();
}
 
209,17 → 161,6
}
 
/***************************************************/
/* Set active parameter set */
/***************************************************/
void setActiveParamSet(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
setnumber = 1;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
 
/***************************************************/
/* Read MixerTable from EEPROM */
/***************************************************/
uint8_t MixerTable_ReadFromEEProm(void) {
273,7 → 214,7
/* Initialize EEPROM Parameter Sets */
/***************************************************/
void ParamSet_Init(void) {
uint8_t Channel_Backup = 1, i, j;
uint8_t Channel_Backup = 1, j;
// parameter version check
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) {
// if version check faild
284,35 → 225,21
Channel_Backup = 0;
}
// fill all 5 parameter settings
for (i = 1; i < 6; i++) {
switch (i) {
case 1:
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Kamera
break;
case 3:
ParamSet_DefaultSet3(); // Beginner
break;
default:
ParamSet_DefaultSet2(); // Kamera
break;
}
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom
// restore it
for (j = 0; j < 8; j++) {
staticParams.ChannelAssignment[j] = eeprom_read_byte(
&EEPromArray[EEPROM_ADR_CHANNELS + j]);
}
}
ParamSet_WriteToEEProm(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
 
setDefaults(); // Fill staticParams Structure to default parameter set 1 (Sport)
if (Channel_Backup) { // if we have a rc channel mapping backup in eeprom
// restore it
for (j = 0; j < 8; j++) {
staticParams.ChannelAssignment[j] = eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS + j]);
}
}
ParamSet_WriteToEEProm();
// update version info
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION);
}
// read active parameter set to staticParams stucture
ParamSet_ReadFromEEProm(getActiveParamSet());
ParamSet_ReadFromEEProm();
}
/branches/dongfang_FC_fixedwing/eeprom.h
19,8 → 19,8
#define EEMIXER_REVISION 1 // is count up, if Mixer stucture has changed (compatibility)
 
extern void ParamSet_Init(void);
extern void ParamSet_ReadFromEEProm(uint8_t setnumber);
extern void ParamSet_WriteToEEProm(uint8_t setnumber);
extern void ParamSet_ReadFromEEProm(void);
extern void ParamSet_WriteToEEProm(void);
 
extern uint8_t MixerTable_ReadFromEEProm(void);
extern uint8_t MixerTable_WriteToEEProm(void);
31,6 → 31,5
extern void SetParamWord(uint16_t param_id, uint16_t value);
 
extern uint8_t getActiveParamSet(void);
extern void setActiveParamSet(uint8_t setnumber);
 
#endif //_EEPROM_H
/branches/dongfang_FC_fixedwing/flight.c
156,23 → 156,13
/************************************************************************/
/* Stick signals are positive and gyros are negative... */
/************************************************************************/
IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH];
if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360;
else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360;
if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE;
else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE;
 
IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL];
if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360;
else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360;
if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE;
else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE;
 
IPart[PITCH] = error[PITCH]; // * some factor configurable.
IPart[ROLL] = error[ROLL];
// TODO: Add ipart. Or add/subtract depending, not sure.
term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]);
term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]);
yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw);
 
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
210,7 → 200,7
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW;
 
DebugOut.Analog[6] = pitchPFactor;
DebugOut.Analog[7] = rollPFactor;
219,8 → 209,8
DebugOut.Analog[10] = rollDFactor;
DebugOut.Analog[11] = yawDFactor;
 
DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
}
/branches/dongfang_FC_fixedwing/uart0.c
494,7 → 494,7
else if (pRxData[0] > 5)
pRxData[0] = 5; // limit to 5
// load requested parameter set
ParamSet_ReadFromEEProm(pRxData[0]);
ParamSet_ReadFromEEProm();
tempchar1 = pRxData[0];
tempchar2 = EEPARAM_REVISION;
while (!txd_complete)
511,9 → 511,8
== EEPARAM_REVISION)) // check for setting to be in range and version of settings
{
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams));
ParamSet_WriteToEEProm(pRxData[0]);
tempchar1 = getActiveParamSet();
beepNumber(tempchar1);
ParamSet_WriteToEEProm();
beepNumber(1);
} else {
tempchar1 = 0; //indicate bad data
}
656,7 → 655,7
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL])
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1°
Data3D.Heading = (int16_t) ((10 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
Data3D.Heading = (int16_t) ((10 * angle[YAW]) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
Data3D_Timer = setDelay(Data3D_Interval);
request_Data3D = FALSE;
}