/branches/dongfang_FC_fixedwing/analog.c |
---|
8,6 → 8,7 |
#include "printf_P.h" |
#include "isqrt.h" |
#include "sensors.h" |
#include "configuration.h" |
// for Delay functions |
#include "timer0.h" |
295,7 → 296,7 |
} |
// 2) Apply sign and offset, scale before filtering. |
tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]); |
tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis] + IMUConfig.gyroCalibrationTweak[axis]); |
} |
// 2.1: Transform axes. |
319,7 → 320,7 |
gyro_PID[axis] = tempOffsetGyro[axis]; |
// Prepare tempOffsetGyro for next calculation below... |
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]); |
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis] + IMUConfig.gyroCalibrationTweak[axis]); |
} |
/* |
375,9 → 376,6 |
// analog_updateAccelerometers(); |
analog_updateAirspeed(); |
analog_updateBatteryVoltage(); |
#ifdef USE_MK3MAG |
magneticHeading = volatileMagneticHeading; |
#endif |
} |
void analog_setNeutral() { |
385,8 → 383,9 |
if (gyroOffset_readFromEEProm()) { |
printf("gyro offsets invalid%s",recal); |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING; |
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING; |
gyroOffset.offsets[PITCH] = 0x0721-3; |
gyroOffset.offsets[ROLL] = 0x0721-15; |
gyroOffset.offsets[YAW] = 0x0CD9+12; // these are practical values from my gyros :) |
} |
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
/branches/dongfang_FC_fixedwing/arduino_atmega328/attitude.c |
---|
80,10 → 80,7 |
* The rate variable end up in a range of about [-1024, 1023]. |
*************************************************************************/ |
void getAnalogData(void) { |
uint8_t axis; |
analog_update(); |
for (axis = PITCH; axis <= YAW; axis++) { |
} |
} |
void integrate(void) { |
/branches/dongfang_FC_fixedwing/arduino_atmega328/rc.c |
---|
179,12 → 179,6 |
void RC_periodicTaskAndPRYT(int16_t* PRYT) { |
if (RCQuality) { |
RCQuality--; |
debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
debugOut.analog[21] = RCChannel(CH_AILERONS); |
debugOut.analog[22] = RCChannel(CH_RUDDER); |
debugOut.analog[23] = RCChannel(CH_THROTTLE); |
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
197,6 → 197,9 |
IMUConfig.gyroDWindowLength = 3; |
IMUConfig.gyroQuadrant = 2; |
IMUConfig.imuReversedFlags = 0; |
IMUConfig.gyroCalibrationTweak[0] = |
IMUConfig.gyroCalibrationTweak[1] = |
IMUConfig.gyroCalibrationTweak[2] = 0; |
gyro_setDefaultParameters(); |
} |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
41,6 → 41,8 |
uint8_t gyroActivityDamping; |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
int8_t gyroCalibrationTweak[3]; |
} IMUConfig_t; |
extern IMUConfig_t IMUConfig; |
/branches/dongfang_FC_fixedwing/eeprom.h |
---|
28,7 → 28,7 |
#define EEPARAM_REVISION 100 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
#define IMUCONFIG_REVISION 0 |
#define IMUCONFIG_REVISION 1 |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
/branches/dongfang_FC_fixedwing/main.c |
---|
19,6 → 19,7 |
int16_t main(void) { |
uint16_t timer; |
static uint8_t profileTimer; |
// disable interrupts global |
cli(); |
115,6 → 116,7 |
while (1) { |
if (runFlightControl) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
if (!analogDataReady) { |
// Analog data should have been ready but is not!! |
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
156,7 → 158,7 |
#ifdef DO_PROFILE |
stopProfileTimer(FLIGHT); |
#endif |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !isFlying) { |
usart0_transmitTxData(); |
174,26 → 176,18 |
beepBatteryAlarm(); |
} |
} |
calculateFeaturedServoValues(); |
timer = setDelay(20); // every 20 ms |
} |
output_update(); |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
} |
calculateFeaturedServoValues(); |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
#ifdef DO_PROFILE |
static uint8_t profileTimer; |
if (profileTimer++ == 0) { |
debugProfileTimers(24); |
} |
#endif |
} |
return (1); |
} |
/branches/dongfang_FC_fixedwing/timer0.c |
---|
9,8 → 9,9 |
#include "output.h" |
#ifdef DO_PROFILE |
uint32_t profileTimers[NUM_PROFILE_TIMERS]; |
uint32_t runningProfileTimers[NUM_PROFILE_TIMERS]; |
volatile uint32_t global10kHzClock = 0; |
volatile int32_t profileTimers[NUM_PROFILE_TIMERS]; |
volatile int32_t runningProfileTimers[NUM_PROFILE_TIMERS]; |
#endif |
volatile uint32_t globalMillisClock = 0; |
87,7 → 88,11 |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t beeperOn = 0; |
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz) |
#ifdef DO_PROFILE |
global10kHzClock++; |
#endif |
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz) |
cnt = 9; |
cnt_1ms ^= 1; |
if (!cnt_1ms) { |
166,20 → 171,20 |
#ifdef DO_PROFILE |
void startProfileTimer(uint8_t timer) { |
runningProfileTimers[timer] = globalMillisClock; |
runningProfileTimers[timer] = global10kHzClock++; |
} |
void stopProfileTimer(uint8_t timer) { |
int32_t t = globalMillisClock - runningProfileTimers[timer]; |
int32_t t = global10kHzClock++ - runningProfileTimers[timer]; |
profileTimers[timer] += t; |
} |
void debugProfileTimers(uint8_t index) { |
for (uint8_t i=0; i<NUM_PROFILE_TIMERS; i++) { |
uint16_t tenths = profileTimers[i] / 10000L; |
uint16_t tenths = profileTimers[i] / 1000L; |
debugOut.analog[i+index] = tenths; |
} |
uint16_t tenths = globalMillisClock / 10000L; |
uint16_t tenths = global10kHzClock / 1000L; |
debugOut.analog[index + NUM_PROFILE_TIMERS] = tenths; |
} |
#endif; |
/branches/dongfang_FC_fixedwing/timer2.c |
---|
137,10 → 137,6 |
else if (value > SERVOLIMIT) value = SERVOLIMIT; |
servoValues[axis] = value + NEUTRAL_PULSELENGTH; |
} |
debugOut.analog[24] = servoValues[0]; |
debugOut.analog[25] = servoValues[1]; |
debugOut.analog[26] = servoValues[2]; |
debugOut.analog[27] = servoValues[3]; |
} |
void calculateFeaturedServoValues(void) { |