/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], yawAngleDiff; |
int32_t angle[2]; |
/* |
* Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
68,7 → 68,6 |
uint16_t ignoreCompassTimer = 500; |
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t yawGyroDrift; |
int16_t correctionSum[2] = { 0, 0 }; |
111,7 → 110,7 |
************************************************************************/ |
void attitude_setNeutral(void) { |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
driftComp[PITCH] = driftComp[ROLL]; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
// Calibrate hardware. |
119,7 → 118,6 |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
yawAngleDiff = 0; |
// update compass course to current heading |
compassCourse = compassHeading; |
161,18 → 159,19 |
// 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]; |
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += (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); |
} else { |
error[PITCH] += (staticParams.ControlSigns & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += (staticParams.ControlSigns & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += (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); |
} |
for (axis=PITCH; axis<=YAW; axis++) { |
189,7 → 188,6 |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
yawGyroHeading += ACYawRate; |
yawAngleDiff += yawRate; |
if (yawGyroHeading >= YAWOVER360) { |
yawGyroHeading -= YAWOVER360; // 360 deg. wrap |
/branches/dongfang_FC_fixedwing/attitude.h |
---|
86,7 → 86,7 |
/* |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t angle[2], yawAngleDiff; |
extern int32_t angle[2]; |
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
98,7 → 98,6 |
// extern int16_t compassOffCourse; |
extern uint8_t compassCalState; |
extern int32_t yawGyroHeading; |
extern int16_t yawGyroHeadingInDeg; |
extern uint8_t updateCompassCourse; |
extern uint16_t ignoreCompassTimer; |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
33,12 → 33,9 |
SET_POT(dynamicParams.GyroYawD,staticParams.GyroYawD); |
SET_POT(dynamicParams.IFactor,staticParams.IFactor); |
for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]); |
for (i=0; i<sizeof(staticParams.UserParams); i++) { |
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams[i]); |
} |
for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[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 |
---|
16,7 → 16,6 |
/* P */uint8_t GyroRollP; |
/* P */uint8_t GyroYawP; |
/* P */uint8_t IFactor; |
/* P */uint8_t UserParams[8]; |
/* P */uint8_t ServoPitchControl; |
68,8 → 67,6 |
uint8_t StickAileronsP; // StickD in tool. |
uint8_t StickRudderP; // StickYawP in tool. |
uint8_t Unused3; |
uint8_t Unused4; |
uint8_t GyroAccFactor; // Value : 1-64 |
uint8_t CompassYawEffect; // Value : 0-32 |
77,44 → 74,29 |
uint8_t GyroRollP; // GyroI in tool |
uint8_t GyroYawP; // GyroD in tool |
uint8_t UserParams[8]; // Value : 0-250 |
uint8_t LowVoltageWarning; // Value : 0-250 |
uint8_t Unused5; // Value : 0-250 //Gaswert bei Empüngsverlust |
uint8_t Unused6; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
uint8_t Unused7; // |
uint8_t IFactor; // Value : 0-250 |
uint8_t UserParams1[4]; // Value : 0-250 |
uint8_t ControlSigns; |
uint8_t Unused9; |
uint8_t Unused10; |
uint8_t Unused11; // Value : 0-250 // Anschlag |
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output |
uint8_t Unused16; |
uint8_t Unused17; |
uint8_t Unused18; |
uint8_t GyroPitchD; // LoopGasLimit in tool |
uint8_t GyroRollD; // loopThreshold in tool |
uint8_t GyroYawD; // loopHysteresis in tool |
uint8_t Unused19; |
uint8_t Unused20; |
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
uint8_t DriftComp; // limit for gyrodrift compensation |
uint8_t Unused21; // PID limit for Attitude controller |
uint8_t UserParams2[4]; // Value : 0-250 |
uint8_t J16Bitmask; // for the J16 Output |
uint8_t J16Timing; // for the J16 Output |
uint8_t J17Bitmask; // for the J17 Output |
uint8_t J17Timing; // for the J17 Output |
uint8_t ExternalControl; // for serial Control |
uint8_t BitConfig; // see upper defines for bitcoding |
uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen |
uint8_t Reserved[4]; |
int8_t Name[12]; |
} paramset_t; |
#define PARAMSET_STRUCT_LEN sizeof(paramset_t) |
/branches/dongfang_FC_fixedwing/controlMixer.c |
---|
96,7 → 96,7 |
} else if (controlIntegrals[index] <= -PITCHROLLOVER180) { |
controlIntegrals[index] += PITCHROLLOVER360; |
} |
*/212e |
*/ |
control[index] = newValue; |
tmp -= newValue; |