/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
16,7 → 16,7 |
uint8_t i, axis, factor, numberOfAxesInRange = 0; |
uint16_t timeout; |
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0; |
timeout = SetDelay(2000); |
timeout = setDelay(2000); |
for (i = 140; i != 0; i--) { |
// If all 3 axis are in range, shorten the remaining number of iterations. |
50,7 → 50,7 |
// Wait for I2C to finish transmission. |
while (twi_state) { |
// Did it take too long? |
if (CheckDelay(timeout)) { |
if (checkDelay(timeout)) { |
printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl"); |
break; |
} |
58,9 → 58,9 |
analog_start(); |
Delay_ms_Mess(i < 10 ? 10 : 2); |
delay_ms_Mess(i < 10 ? 10 : 2); |
} |
Delay_ms_Mess(70); |
delay_ms_Mess(70); |
} |
void gyro_setDefaults(void) { |
/branches/dongfang_FC_rewrite/attitude.c |
---|
295,29 → 295,30 |
// Well actually the Z axis acc. check is not so silly. |
uint8_t axis; |
int32_t temp; |
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
<= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
uint8_t debugFullWeight = 1; |
int32_t accDerived; |
/* |
if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands |
permilleAcc /= 2; |
debugFullWeight = 0; |
*/ |
if (controlActivity > 10000) { // reduce effect during stick commands |
permilleAcc /= 4; |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
if (controlActivity > 20000) { // reduce effect during stick commands |
permilleAcc /= 4; |
DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
} |
} |
if (debugFullWeight) |
DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
else |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
/* |
* Add to each sum: The amount by which the angle is changed just below. |
*/ |
332,8 → 333,6 |
correctionSum[axis] += angle[axis] - temp; |
} |
} else { |
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
DebugOut.Analog[9] = 0; |
DebugOut.Analog[10] = 0; |
452,7 → 451,8 |
if (w >= 0) { // maxAttitudeAngle < 32 |
if (!ignoreCompassTimer) { |
v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
/*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/ |
v = 64 + controlActivity / 100; |
// yawGyroHeading - compassCourse on a -180..179 degree interval. |
r |
= ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
/branches/dongfang_FC_rewrite/configuration.c |
---|
133,7 → 133,7 |
const MMXLATION MMXLATIONS[] = { |
{offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
{offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightD),0,150}, |
{offsetof(paramset_t, HeightP), offsetof(dynamicParam_t, HeightP),0,150}, |
{offsetof(paramset_t, GyroP), offsetof(dynamicParam_t, GyroP),0,255}, |
{offsetof(paramset_t, J16Timing), offsetof(dynamicParam_t, J16Timing),1,255}, |
{offsetof(paramset_t, J17Timing), offsetof(dynamicParam_t, J17Timing),1,255}, |
/branches/dongfang_FC_rewrite/configuration.h |
---|
80,9 → 80,9 |
uint8_t UserParam3; // Value : 0-250 |
uint8_t UserParam4; // Value : 0-250 |
*/ |
uint8_t ServoPitchControl; // Value : 0-250 // Stellung des Servos |
uint8_t ServoPitchComp; // Value : 0-250 // Einfluss Gyro/Servo |
uint8_t ServoPitchMin; // Value : 0-250 // Anschlag |
uint8_t ServoPitchControl; |
uint8_t ServoPitchComp; |
uint8_t ServoPitchMin; |
uint8_t ServoPitchMax; // Value : 0-250 // Anschlag |
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output |
uint8_t LoopGasLimit; // Value: 0-250 max. Gas wührend Looping |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
60,9 → 60,10 |
#include "commands.h" |
#include "output.h" |
uint16_t maxControl[2] = { 0, 0 }; |
int16_t control[2] = { 0, 0 }; |
int16_t controlYaw = 0, controlThrottle = 0; |
// uint16_t maxControl[2] = { 0, 0 }; |
uint16_t controlActivity = 0; |
int16_t controls[4] = { 0, 0, 0, 0 }; |
//int16_t controlYaw = 0, controlThrottle = 0; |
uint8_t looping = 0; |
// Internal variables for reading commands made with an R/C stick. |
136,6 → 137,26 |
return rcQ > ecQ ? rcQ : ecQ; |
} |
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
int16_t tmp = controls[index]; |
controls[index] = newValue; |
tmp -= newValue; |
tmp /= 2; |
tmp = tmp * tmp; |
// tmp += (newValue >= 0) ? newValue : -newValue; |
if (controlActivity + (uint16_t)tmp >= controlActivity) |
controlActivity += tmp; |
else controlActivity = 0xffff; |
} |
#define CADAMPING 10 |
void dampenControlActivity(void) { |
uint32_t tmp = controlActivity; |
tmp *= ((1<<CADAMPING)-1); |
tmp >>= CADAMPING; |
controlActivity = tmp; |
} |
/* |
* Update the variables indicating stick position from the sum of R/C, GPS and external control. |
*/ |
142,7 → 163,6 |
void controlMixer_update(void) { |
// calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
// TODO: If no signal --> zero. |
uint8_t axis; |
int16_t tempThrottle; |
// takes almost no time... |
154,20 → 174,21 |
// takes about 80 usec. |
HC_update(); |
int16_t* RC_PRTY = RC_getPRTY(); |
int16_t* EC_PRTY = EC_getPRTY(); |
int16_t* RC_PRTY = RC_getPRTY(); |
int16_t* EC_PRTY = EC_getPRTY(); |
control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
dampenControlActivity(); |
DebugOut.Analog[14] = controlActivity/10; |
tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
controlThrottle = AC_getThrottle(tempThrottle); |
controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
// controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
DebugOut.Analog[12] = control[PITCH]; |
DebugOut.Analog[13] = control[ROLL]; |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
configuration_applyVariablesToParams(); |
190,7 → 211,6 |
/* |
* Record maxima |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
199,6 → 219,7 |
} else if (maxControl[axis]) |
maxControl[axis]--; |
} |
*/ |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
: COMMAND_NONE; |
/branches/dongfang_FC_rewrite/controlMixer.h |
---|
71,9 → 71,10 |
/* |
* Our output. |
*/ |
extern int16_t control[2]; |
extern int16_t controlYaw, controlThrottle; |
extern uint16_t maxControl[2]; |
extern int16_t controls[4]; |
//extern int16_t controlYaw, controlThrottle; |
extern uint16_t controlActivity; |
//extern uint16_t maxControl[2]; |
extern uint8_t looping; |
extern volatile uint8_t MKFlags; |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
95,7 → 95,7 |
* While we are still using userparams for flight parameters, do set |
* some safe & meaningful default values. |
*/ |
staticParams.UserParams1[3] = 8; // Throttle stick D=8 |
staticParams.UserParams1[3] = 12; // Throttle stick D=12 |
staticParams.UserParams2[0] = 0b11010101; // All gyro filter constants 2; acc. 4 |
staticParams.UserParams2[1] = 2; // H&I motor smoothing. |
staticParams.UserParams2[2] = 120; // Yaw I factor |
129,7 → 129,7 |
staticParams.CompassYawEffect = 128; |
staticParams.GyroP = 80; |
staticParams.GyroI = 100; |
staticParams.LowVoltageWarning = 95; |
staticParams.LowVoltageWarning = 101; // 3.7 each for 3S |
staticParams.EmergencyGas = 35; |
staticParams.EmergencyGasDuration = 30; |
staticParams.Unused0 = 0; |
/branches/dongfang_FC_rewrite/flight.c |
---|
172,7 → 172,7 |
// We want that to kick as early as possible, not to delay new AD sampling further. |
calculateFlightAttitude(); |
controlMixer_update(); |
throttleTerm = controlThrottle; |
throttleTerm = controls[CONTROL_THROTTLE]; |
// This check removed. Is done on a per-motor basis, after output matrix multiplication. |
if (throttleTerm < staticParams.MinThrottle + 10) |
261,7 → 261,7 |
/************************************************************************/ |
/* Yawing */ |
/************************************************************************/ |
if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated |
if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated |
ignoreCompassTimer = 1000; |
if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
updateCompassCourse = 1; |
273,7 → 273,7 |
// Trim drift of yawAngleDiff with controlYaw. |
// TODO: We want NO feedback of control related stuff to the attitude related stuff. |
// This seems to be used as: Difference desired <--> real heading. |
yawAngleDiff -= controlYaw; |
yawAngleDiff -= controls[CONTROL_YAW]; |
// limit the effect |
CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
353,7 → 353,7 |
* between current throttle and maximum throttle). |
*/ |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING; |
yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
// Limit yawTerm |
DebugOut.Digital[0] &= ~DEBUG_CLIP; |
if (throttleTerm > MIN_YAWGAS) { |
396,11 → 396,11 |
// Integration mode: Integrate (angle - stick) = the difference between angle and stick pos. |
// That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time. |
// TODO: Find out why this seems to be proportional to stick position - not integrating it at all. |
IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos. |
IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos. |
} else { |
// "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
// To keep up with a full stick PDPart should be about 156... |
IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
} |
tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
409,7 → 409,7 |
// TODO: From which planet comes the 16000? |
CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
// Add (P, D) parts minus stick pos. to the scaled-down I part. |
term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch |
term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
/* |
* Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
432,37 → 432,41 |
DebugOut.Analog[12] = term[PITCH]; |
DebugOut.Analog[13] = term[ROLL]; |
DebugOut.Analog[14] = yawTerm; |
//DebugOut.Analog[14] = yawTerm; |
DebugOut.Analog[15] = throttleTerm; |
for (i = 0; i < MAX_MOTORS; i++) { |
int32_t tmp; |
uint8_t throttle; |
tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
tmp = motorFilters[i] / MOTOR_SCALING; |
// So this was the THIRD time a throttle was limited. But should the limitation |
// apply to the common throttle signal (the one used for setting the "power" of |
// all motors together) or should it limit the throttle set for each motor, |
// including mix components of pitch, roll and yaw? I think only the common |
// throttle should be limited. |
// --> WRONG. This caused motors to stall completely in tight maneuvers. |
// Apply to individual signals instead. |
CHECK_MIN_MAX(tmp, 1, 255); |
throttle = tmp; |
if (i < 4) DebugOut.Analog[22 + i] = throttle; |
if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
tmp = motorFilters[i] / MOTOR_SCALING; |
// So this was the THIRD time a throttle was limited. But should the limitation |
// apply to the common throttle signal (the one used for setting the "power" of |
// all motors together) or should it limit the throttle set for each motor, |
// including mix components of pitch, roll and yaw? I think only the common |
// throttle should be limited. |
// --> WRONG. This caused motors to stall completely in tight maneuvers. |
// Apply to individual signals instead. |
CHECK_MIN_MAX(tmp, 1, 255); |
motor[i].SetPoint = tmp; |
motor[i].SetPoint = throttle; |
} else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
} else { |
motor[i].SetPoint = 0; |
} |
if (i < 4) |
DebugOut.Analog[22 + i] = motor[i].SetPoint; |
} |
I2C_Start(TWI_STATE_MOTOR_TX); |
/branches/dongfang_FC_rewrite/output.c |
---|
89,17 → 89,23 |
static int8_t delay = 0; |
if (!delay--) { // 10 ms intervals |
delay = 4; |
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, |
dynamicParams.J16Timing); |
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, |
dynamicParams.J17Timing); |
if (beepModulation != BEEP_MODULATION_NONE) { |
// alarm |
flashingLight(0, 25, 0x55, 25); |
flashingLight(1, 25, 0xAA, 25); |
} else { |
flashingLight(0, staticParams.J16Timing, staticParams.J16Bitmask, |
dynamicParams.J16Timing); |
flashingLight(1, staticParams.J17Timing, staticParams.J17Bitmask, |
dynamicParams.J17Timing); |
} |
} |
} |
void output_update(void) { |
if (!DIGITAL_DEBUG_MASK) |
if (!DIGITAL_DEBUG_MASK) { |
flashingLights(); |
else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
OUTPUT_SET(0, 1); |
OUTPUT_SET(1, 1); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) { |
138,9 → 144,9 |
* Beep the R/C alarm signal |
*/ |
void beepRCAlarm(void) { |
if(beepModulation == 0xFFFF) { // If not already beeping an alarm signal (?) |
if(beepModulation == BEEP_MODULATION_NONE) { // If not already beeping an alarm signal (?) |
beepTime = 15000; // 1.5 seconds |
beepModulation = 0x0C00; |
beepModulation = BEEP_MODULATION_RCALARM; |
} |
} |
148,9 → 154,9 |
* Beep the I2C bus error signal |
*/ |
void beepI2CAlarm(void) { |
if((beepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
if((beepModulation == BEEP_MODULATION_NONE) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
beepTime = 10000; // 1 second |
beepModulation = 0x0080; |
beepModulation = BEEP_MODULATION_I2CALARM; |
} |
} |
158,7 → 164,7 |
* Beep the battery low alarm signal |
*/ |
void beepBatteryAlarm(void) { |
beepModulation = 0x0300; |
beepModulation = BEEP_MODULATION_BATTERYALARM; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
168,7 → 174,7 |
* Beep the EEPROM checksum alarm |
*/ |
void beepEEPROMAlarm(void) { |
beepModulation = 0x0007; |
beepModulation = BEEP_MODULATION_EEPROMALARM; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
/branches/dongfang_FC_rewrite/output.h |
---|
45,9 → 45,16 |
* at hand is resolved. |
*/ |
// Both on |
#define DEBUG_LEDTEST_ON 1000 |
// Both off |
#define DEBUG_LEDTEST_OFF 1001 |
// #0 on |
#define DEBUG_LEDTEST_0 1002 |
// #1 on |
#define DEBUG_LEDTEST_1 1003 |
#define DEBUG_MAINLOOP_TIMER 1 |
63,7 → 70,7 |
* Set to 0 for using outputs as the usual flashing lights. |
* Set to one of the DEBUG_... defines h for using the outputs as debug lights. |
*/ |
#define DIGITAL_DEBUG_MASK DEBUG_CLIP |
#define DIGITAL_DEBUG_MASK DEBUG_ACC0THORDER |
void output_init(void); |
void output_update(void); |
/branches/dongfang_FC_rewrite/timer0.c |
---|
54,6 → 54,7 |
#include "eeprom.h" |
#include "analog.h" |
#include "timer0.h" |
// for debugging! |
#include "uart0.h" |
#include "output.h" |
66,7 → 67,7 |
volatile uint8_t runFlightControl = 0; |
volatile uint16_t cntKompass = 0; |
volatile uint16_t beepTime = 0; |
volatile uint16_t beepModulation = 0xFFFF; |
volatile uint16_t beepModulation = BEEP_MODULATION_NONE; |
#ifdef USE_NAVICTRL |
volatile uint8_t SendSPI = 0; |
164,7 → 165,7 |
beeper_On = 0; |
} else { // beeper off if duration is over |
beeper_On = 0; |
beepModulation = 0xFFFF; |
beepModulation = BEEP_MODULATION_NONE; |
} |
// if beeper is on |
/branches/dongfang_FC_rewrite/timer0.h |
---|
3,6 → 3,12 |
#include <inttypes.h> |
#define BEEP_MODULATION_NONE 0xFFFF |
#define BEEP_MODULATION_RCALARM 0x0C00 |
#define BEEP_MODULATION_I2CALARM 0x0080 |
#define BEEP_MODULATION_BATTERYALARM 0x0300 |
#define BEEP_MODULATION_EEPROMALARM 0x0007 |
extern volatile uint16_t millisecondsCount; |
extern volatile uint8_t runFlightControl; |
extern volatile uint16_t cntKompass; |
/branches/dongfang_FC_rewrite/uart0.c |
---|
137,8 → 137,8 |
"UBat ", |
"Pitch Term ", |
"Roll Term ", |
"Yaw Term ", |
"Throttle Term ", //15 |
"controlActivity ", |
"ca debug ", //15 |
"0th O Corr pitch", |
"0th O Corr roll ", |
"unused ", |