174,6 → 174,33 |
|
|
/************************************************************************/ |
/* Update Debug Outputs */ |
/************************************************************************/ |
void UpdateDebugValues(void) |
{ |
DebugOut.Analog[0] = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[1] = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[2] = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[3] = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[4] = GyroYaw; |
DebugOut.Analog[5] = ReadingHeight/5; |
DebugOut.Analog[6] = AdValueAccTop;//(ReadingIntegralTop / 512);//AdValueAccZ; |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[18] = ReadingVario; |
DebugOut.Analog[19] = CompassCalState; |
DebugOut.Analog[20] = ServoNickValue; |
DebugOut.Analog[22] = Capacity.ActualCurrent; |
DebugOut.Analog[23] = Capacity.UsedCapacity; |
DebugOut.Analog[29] = NCSerialDataOkay; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
} |
|
|
/************************************************************************/ |
/* Filter for motor value smoothing */ |
/************************************************************************/ |
int16_t MotorSmoothing(int16_t newvalue, int16_t oldvalue) |
663,7 → 690,6 |
static int32_t CorrectionNick, CorrectionRoll; |
static uint16_t RcLostTimer; |
static uint8_t delay_neutral = 0, delay_startmotors = 0, delay_stopmotors = 0; |
static int8_t TimerDebugOut = 0; |
static uint16_t UpdateCompassCourse = 0; |
// high resolution motor values for smoothing of PID motor outputs |
static int16_t MotorValue[MAX_MOTORS]; |
1397,33 → 1423,6 |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// DebugOutputs |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(!TimerDebugOut--) |
{ |
TimerDebugOut = 24; // update debug outputs every 25*2ms = 50 ms (20Hz) |
DebugOut.Analog[0] = (10 * IntegralGyroNick) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[1] = (10 * IntegralGyroRoll) / GYRO_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[2] = (10 * AccNick) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[3] = (10 * AccRoll) / ACC_DEG_FACTOR; // in 0.1 deg |
DebugOut.Analog[4] = GyroYaw; |
DebugOut.Analog[5] = ReadingHeight/5; |
DebugOut.Analog[6] = AdValueAccTop;//(ReadingIntegralTop / 512);//AdValueAccZ; |
DebugOut.Analog[8] = CompassHeading; |
DebugOut.Analog[9] = UBat; |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[18] = ReadingVario; |
DebugOut.Analog[19] = CompassCalState; |
DebugOut.Analog[20] = ServoNickValue; |
DebugOut.Analog[22] = Capacity.ActualCurrent; |
DebugOut.Analog[23] = Capacity.UsedCapacity; |
DebugOut.Analog[29] = NCSerialDataOkay; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
} |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// calculate control feedback from angle (gyro integral) and agular velocity (gyro signal) |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|