/branches/dongfang_FC_rewrite/analog_fastirq.c |
---|
File deleted |
/branches/dongfang_FC_rewrite/gyro.h |
---|
File deleted |
/branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c |
---|
8,8 → 8,8 |
} |
void gyro_setDefaults(void) { |
staticParams.GyroD = 5; |
staticParams.DriftComp = 3; |
staticParams.GyroAccFactor = 1; |
staticParams.GyroAccTrim = 5; |
staticParams.gyroD = 5; |
staticParams.driftCompDivider = 1; |
staticParams.driftCompLimit = 3; |
staticParams.zerothOrderCorrection = 1; |
} |
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
56,7 → 56,6 |
} |
} |
startAnalogConversionCycle(); |
delay_ms_Mess(i < 10 ? 10 : 2); |
} |
delay_ms_Mess(70); |
63,8 → 62,8 |
} |
void gyro_setDefaults(void) { |
staticParams.GyroD = 3; |
staticParams.DriftComp = 200; |
staticParams.GyroAccFactor = 25; |
staticParams.GyroAccTrim = 1; |
staticParams.gyroD = 3; |
staticParams.driftCompDivider = 1; |
staticParams.driftCompLimit = 200; |
staticParams.zerothOrderCorrection = 25; |
} |
/branches/dongfang_FC_rewrite/analog.c |
---|
100,20 → 100,21 |
* standing still. They are used for adjusting the gyro and acc. meter values |
* to be centered on zero. |
*/ |
/* |
volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 |
* GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW }; |
volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512 |
* ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z }; |
*/ |
sensorOffset_t gyroOffset; |
sensorOffset_t accOffset; |
/* |
* This allows some experimentation with the gyro filters. |
* Should be replaced by #define's later on... |
*/ |
volatile uint8_t GYROS_PID_FILTER; |
volatile uint8_t GYROS_ATT_FILTER; |
volatile uint8_t GYROS_D_FILTER; |
volatile uint8_t ACC_FILTER; |
/* |
* Air pressure |
245,6 → 246,7 |
} |
void startAnalogConversionCycle(void) { |
analogDataReady = 0; |
// Stop the sampling. Cycle is over. |
for (uint8_t i = 0; i < 8; i++) { |
sensorInputs[i] = 0; |
289,7 → 291,7 |
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a |
// gyro with a wider range, and helps counter saturation at full control. |
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) { |
if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) { |
if (tempGyro < SENSOR_MIN_PITCHROLL) { |
debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
304,19 → 306,19 |
// 2) Apply sign and offset, scale before filtering. |
if (GYRO_REVERSED[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 3) Scale and filter. |
tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro) / GYROS_PID_FILTER; |
tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant; |
// 4) Measure noise. |
measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING); |
// 5) Differential measurement. |
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER; |
gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro - gyro_PID[axis])) / staticParams.gyroDFilterConstant; |
// 6) Done. |
gyro_PID[axis] = tempOffsetGyro; |
328,21 → 330,21 |
// 1) Apply sign and offset, scale before filtering. |
if (GYRO_REVERSED[axis]) { |
tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL; |
} else { |
tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
} |
// 2) Filter. |
gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro) / GYROS_ATT_FILTER; |
gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant; |
} |
// Yaw gyro. |
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW]; |
if (GYRO_REVERSED[YAW]) |
yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW]; |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW]; |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
debugOut.analog[3] = gyro_ATT[PITCH]; |
debugOut.analog[4] = gyro_ATT[ROLL]; |
353,11 → 355,11 |
// Pitch and roll axis accelerations. |
for (uint8_t axis=0; axis<2; axis++) { |
if (ACC_REVERSED[axis]) |
acc[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis]; |
else |
acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis]; |
acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis]; |
filteredAcc[axis] = (filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER; |
filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant; |
/* |
stronglyFilteredAcc[PITCH] = |
369,17 → 371,14 |
// Z acc. |
if (ACC_REVERSED[Z]) |
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z]; |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z]; |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
/* |
stronglyFilteredAcc[Z] = |
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100; |
*/ |
debugOut.analog[6] = acc[PITCH]; |
debugOut.analog[7] = acc[ROLL]; |
} |
void analog_updateAirPressure(void) { |
479,13 → 478,6 |
#define GYRO_OFFSET_CYCLES 32 |
uint8_t i, axis; |
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] & 0b00000011) + 1; |
GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1; |
GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1; |
ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1; |
gyro_calibrate(); |
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!) |
497,16 → 489,17 |
} |
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
// DebugOut.Analog[20 + axis] = gyroOffset[axis]; |
gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
debugOut.analog[6+axis] = gyroOffset.offsets[axis]; |
} |
// Noise is relativ to offset. So, reset noise measurements when changing offsets. |
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0; |
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH); |
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL); |
accOffset[Z] = GetParamWord(PID_ACC_Z); |
accOffset_readFromEEProm(); |
// accOffset[PITCH] = getParamWord(PID_ACC_PITCH); |
// accOffset[ROLL] = getParamWord(PID_ACC_ROLL); |
// accOffset[Z] = getParamWord(PID_ACC_Z); |
// Rough estimate. Hmm no nothing happens at calibration anyway. |
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2); |
539,13 → 532,14 |
for (axis = PITCH; axis <= YAW; axis++) { |
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) |
/ ACC_OFFSET_CYCLES; |
accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta; |
} |
// Save ACC neutral settings to eeprom |
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
SetParamWord(PID_ACC_Z, accOffset[Z]); |
// setParamWord(PID_ACC_PITCH, accOffset[PITCH]); |
// setParamWord(PID_ACC_ROLL, accOffset[ROLL]); |
// setParamWord(PID_ACC_Z, accOffset[Z]); |
accOffset_writeToEEProm(); |
// Noise is relative to offset. So, reset noise measurements when |
// changing offsets. |
554,31 → 548,4 |
// Setting offset values has an influence in the analog.c ISR |
// Therefore run measurement for 100ms to achive stable readings |
delay_ms_Mess(100); |
// Set the feedback so that air pressure ends up in the middle of the range. |
// (raw pressure high --> OCR0A also high...) |
/* |
OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1; |
delay_ms_Mess(1000); |
pressureDiff = 0; |
// DebugOut.Analog[16] = rawAirPressure; |
#define PRESSURE_CAL_CYCLE_COUNT 5 |
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) { |
savedRawAirPressure = rawAirPressure; |
OCR0A+=2; |
delay_ms_Mess(500); |
// raw pressure will decrease. |
pressureDiff += (savedRawAirPressure - rawAirPressure); |
savedRawAirPressure = rawAirPressure; |
OCR0A-=2; |
delay_ms_Mess(500); |
// raw pressure will increase. |
pressureDiff += (rawAirPressure - savedRawAirPressure); |
} |
rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2); |
DebugOut.Analog[27] = rangewidth; |
*/ |
} |
/branches/dongfang_FC_rewrite/analog.h |
---|
188,6 → 188,15 |
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
typedef struct { |
uint8_t checksum; |
uint8_t revisionNumber; |
int16_t offsets[3]; |
} sensorOffset_t; |
extern sensorOffset_t gyroOffset; |
extern sensorOffset_t accOffset; |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
*/ |
/branches/dongfang_FC_rewrite/attitude.c |
---|
173,7 → 173,7 |
************************************************************************/ |
void attitude_setNeutral(void) { |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
217,7 → 217,6 |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
startAnalogConversionCycle(); |
} |
249,7 → 248,7 |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
if (/*!looping && */ (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
trigAxisCoupling(); |
} else { |
ACRate[PITCH] = rate_ATT[PITCH]; |
300,10 → 299,8 |
debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
<= dynamicParams.UserParams[7]) { |
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) { |
uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
int32_t accDerived; |
/* |
376,8 → 373,8 |
round = -DRIFTCORRECTION_TIME / 2; |
deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
// Add the delta to the compensation. So positive delta means, gyro should have higher value. |
driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
// DebugOut.Analog[16 + axis] = correctionSum[axis]; |
debugOut.analog[28 + axis] = driftComp[axis]; |
444,7 → 441,7 |
w = (w * dynamicParams.CompassYawEffect) / 32; |
w = dynamicParams.CompassYawEffect - w; |
*/ |
w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) |
w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
/ 32; |
// As readable formula: |
460,7 → 457,7 |
% 360) - 180; |
v = (r * w) / v; // align to compass course |
// limit yaw rate |
w = 3 * dynamicParams.CompassYawEffect; |
w = 3 * dynamicParams.compassYawEffect; |
if (v > w) |
v = w; |
else if (v < -w) |
/branches/dongfang_FC_rewrite/attitudeControl.c |
---|
15,7 → 15,7 |
// With static MINPROJECTION: 220 usec. |
uint16_t AC_getThrottle(uint16_t throttle) { |
int32_t projection; |
uint8_t effect = dynamicParams.UserParams[2]; // Userparam 3 |
uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
int16_t deltaThrottle, y; |
projection = (int32_t) int_cos(angle[PITCH]) * (int32_t) int_cos(angle[ROLL]); |
/branches/dongfang_FC_rewrite/commands.c |
---|
82,12 → 82,12 |
// A valid parameter-set (1..5) was chosen - use it. |
setActiveParamSet(argument); |
} |
ParamSet_ReadFromEEProm(getActiveParamSet()); |
paramSet_readFromEEProm(getActiveParamSet()); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} else if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE |
} else if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE |
| CFG_GPS_ACTIVE) && argument == 7) { |
// If right stick is centered and down |
// TODO: Out of here! State machine instead. |
/branches/dongfang_FC_rewrite/configuration.c |
---|
49,16 → 49,20 |
// + POSSIBILITY OF SUCH DAMAGE. |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <stddef.h> |
#include <string.h> |
#include "configuration.h" |
#include "eeprom.h" |
#include "sensors.h" |
#include "rc.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}; |
paramset_t staticParams; |
channelMap_t channelMap; |
mixerMatrix_t mixerMatrix; |
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; |
uint8_t boardRelease = 13; |
uint8_t requiredMotors; |
/************************************************************************ |
* Map the parameter to pot values |
* Replacing this code by the code below saved almost 1 kbyte. |
68,76 → 72,43 |
uint8_t i; |
#define SET_POT_MM(b,a,min,max) {if (a<255) {if (a>=251) b=variables[a-251]; else b=a;} if(b<=min) b=min; else if(b>=max) b=max;} |
#define SET_POT(b,a) { if (a<255) {if (a>=251) b=variables[a-251]; else b=a;}} |
SET_POT(dynamicParams.MaxHeight,staticParams.MaxHeight); |
SET_POT_MM(dynamicParams.HeightD,staticParams.HeightD,0,100); |
SET_POT_MM(dynamicParams.HeightP,staticParams.HeightP,0,100); |
SET_POT(dynamicParams.Height_ACC_Effect,staticParams.Height_ACC_Effect); |
SET_POT(dynamicParams.CompassYawEffect,staticParams.CompassYawEffect); |
SET_POT_MM(dynamicParams.GyroP,staticParams.GyroP,0,255); |
SET_POT(dynamicParams.GyroI,staticParams.GyroI); |
SET_POT(dynamicParams.GyroD,staticParams.GyroD); |
SET_POT_MM(dynamicParams.gyroP, staticParams.gyroP, 5, 200); |
SET_POT(dynamicParams.gyroI, staticParams.gyroI); |
SET_POT(dynamicParams.gyroD, staticParams.gyroD); |
SET_POT(dynamicParams.IFactor,staticParams.IFactor); |
for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]); |
SET_POT(dynamicParams.yawIFactor, staticParams.yawIFactor); |
SET_POT(dynamicParams.compassYawEffect,staticParams.compassYawEffect); |
SET_POT(dynamicParams.externalControl, staticParams.externalControl); |
SET_POT(dynamicParams.axisCoupling1, staticParams.axisCoupling1); |
SET_POT(dynamicParams.axisCoupling2, staticParams.axisCoupling2); |
SET_POT(dynamicParams.axisCouplingYawCorrection, staticParams.axisCouplingYawCorrection); |
SET_POT(dynamicParams.dynamicStability,staticParams.dynamicStability); |
SET_POT(dynamicParams.maxControlActivityForAcc,staticParams.maxControlActivityForAcc); |
SET_POT_MM(dynamicParams.heightP, staticParams.heightP,0,100); |
SET_POT_MM(dynamicParams.heightD, staticParams.heightD,0,100); |
SET_POT(dynamicParams.heightSetting,staticParams.heightSetting); |
SET_POT(dynamicParams.heightACCEffect,staticParams.heightACCEffect); |
SET_POT(dynamicParams.attitudeControl,staticParams.attitudeControl); |
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(dynamicParams.ServoPitchControl,staticParams.ServoPitchControl); |
SET_POT(dynamicParams.LoopGasLimit,staticParams.LoopGasLimit); |
SET_POT(dynamicParams.AxisCoupling1,staticParams.AxisCoupling1); |
SET_POT(dynamicParams.AxisCoupling2,staticParams.AxisCoupling2); |
SET_POT(dynamicParams.AxisCouplingYawCorrection,staticParams.AxisCouplingYawCorrection); |
SET_POT(dynamicParams.DynamicStability,staticParams.DynamicStability); |
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255); |
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255); |
#if defined (USE_NAVICTRL) |
SET_POT(dynamicParams.NaviGpsModeControl,staticParams.NaviGpsModeControl); |
SET_POT(dynamicParams.NaviGpsGain,staticParams.NaviGpsGain); |
SET_POT(dynamicParams.NaviGpsP,staticParams.NaviGpsP); |
SET_POT(dynamicParams.NaviGpsI,staticParams.NaviGpsI); |
SET_POT(dynamicParams.NaviGpsD,staticParams.NaviGpsD); |
SET_POT(dynamicParams.NaviGpsACC,staticParams.NaviGpsACC); |
SET_POT_MM(dynamicParams.NaviOperatingRadius,staticParams.NaviOperatingRadius,10, 255); |
SET_POT(dynamicParams.NaviWindCorrection,staticParams.NaviWindCorrection); |
SET_POT(dynamicParams.NaviSpeedCompensation,staticParams.NaviSpeedCompensation); |
#endif |
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl); |
SET_POT(dynamicParams.servoManualControl[0], staticParams.servoConfigurations[0].manualControl); |
SET_POT(dynamicParams.servoManualControl[1], staticParams.servoConfigurations[1].manualControl); |
SET_POT_MM(dynamicParams.output0Timing, staticParams.outputFlash[0].timing,1,255); |
SET_POT_MM(dynamicParams.output1Timing, staticParams.outputFlash[1].timing,1,255); |
} |
const XLATION XLATIONS[] = { |
{offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)}, |
{offsetof(paramset_t, Height_ACC_Effect), offsetof(dynamicParam_t, Height_ACC_Effect)}, |
{offsetof(paramset_t, CompassYawEffect), offsetof(dynamicParam_t, CompassYawEffect)}, |
{offsetof(paramset_t, GyroI), offsetof(dynamicParam_t, GyroI)}, |
{offsetof(paramset_t, GyroD), offsetof(dynamicParam_t, GyroD)}, |
{offsetof(paramset_t, IFactor), offsetof(dynamicParam_t, IFactor)}, |
{offsetof(paramset_t, ServoPitchControl), offsetof(dynamicParam_t, ServoPitchControl)}, |
{offsetof(paramset_t, LoopGasLimit), offsetof(dynamicParam_t, LoopGasLimit)}, |
{offsetof(paramset_t, AxisCoupling1), offsetof(dynamicParam_t, AxisCoupling1)}, |
{offsetof(paramset_t, AxisCoupling2), offsetof(dynamicParam_t, AxisCoupling2)}, |
{offsetof(paramset_t, AxisCouplingYawCorrection), offsetof(dynamicParam_t, AxisCouplingYawCorrection)}, |
{offsetof(paramset_t, DynamicStability), offsetof(dynamicParam_t, DynamicStability)}, |
{offsetof(paramset_t, NaviGpsModeControl), |
offsetof(dynamicParam_t, NaviGpsModeControl)}, |
{offsetof(paramset_t, NaviGpsGain), offsetof(dynamicParam_t, NaviGpsGain)}, |
{offsetof(paramset_t, NaviGpsP), offsetof(dynamicParam_t, NaviGpsP)}, |
{offsetof(paramset_t, NaviGpsI), offsetof(dynamicParam_t, NaviGpsI)}, |
{offsetof(paramset_t, NaviGpsD), offsetof(dynamicParam_t, NaviGpsD)}, |
{offsetof(paramset_t, NaviGpsACC), offsetof(dynamicParam_t, NaviGpsACC)}, |
{offsetof(paramset_t, NaviWindCorrection), offsetof(dynamicParam_t, NaviWindCorrection)}, |
{offsetof(paramset_t, NaviSpeedCompensation), offsetof(dynamicParam_t, NaviSpeedCompensation)}, |
{offsetof(paramset_t, ExternalControl), offsetof(dynamicParam_t, ExternalControl)} |
// {offsetof(paramset_t, MaxHeight), offsetof(dynamicParam_t, MaxHeight)}, |
}; |
const MMXLATION MMXLATIONS[] = { |
{offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
{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}, |
{offsetof(paramset_t, NaviOperatingRadius), offsetof(dynamicParam_t, NaviOperatingRadius),10,255} |
// {offsetof(paramset_t, HeightD), offsetof(dynamicParam_t, HeightD),0,100}, |
}; |
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
168,22 → 139,14 |
} |
} |
for (i=0; i<sizeof(staticParams.UserParams1); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams1) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + i); |
for (i=0; i<sizeof(staticParams.userParams); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, userParams) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, userParams) + i); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
for (i=0; i<sizeof(staticParams.UserParams2); i++) { |
src = *((uint8_t*)(&staticParams + offsetof(paramset_t, UserParams2) + i)); |
pointerToTgt = (uint8_t*)(&dynamicParams + offsetof(dynamicParam_t, UserParams) + sizeof(staticParams.UserParams1) + i); |
if (src < 255) { |
*pointerToTgt = configuration_applyVariableToParam(src, 0, 255); |
} |
} |
} |
uint8_t getCPUType(void) { // works only after reset or power on when the registers have default values |
uint8_t CPUType = ATMEGA644; |
203,7 → 166,7 |
*/ |
uint8_t getBoardRelease(void) { |
uint8_t BoardRelease = 13; |
uint8_t boardRelease = 13; |
// the board release is coded via the pull up or down the 2 status LED |
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
213,16 → 176,16 |
switch( PINB & ((1<<PINB1)|(1<<PINB0))) { |
case 0x00: |
BoardRelease = 10; // 1.0 |
boardRelease = 10; // 1.0 |
break; |
case 0x01: |
BoardRelease = 11; // 1.1 or 1.2 |
boardRelease = 11; // 1.1 or 1.2 |
break; |
case 0x02: |
BoardRelease = 20; // 2.0 |
boardRelease = 20; // 2.0 |
break; |
case 0x03: |
BoardRelease = 13; // 1.3 |
boardRelease = 13; // 1.3 |
break; |
default: |
break; |
231,5 → 194,125 |
DDRB |= (1<<DDB1)|(1<<DDB0); |
RED_ON; |
GRN_OFF; |
return BoardRelease; |
return boardRelease; |
} |
void setOtherDefaults(void) { |
// Height Control |
staticParams.heightP = 10; |
staticParams.heightD = 30; |
staticParams.heightSetting = 251; |
staticParams.heightMaxThrottleChange = 10; |
staticParams.heightSlewRate = 4; |
staticParams.heightACCEffect = 30; |
// Control |
staticParams.stickP = 8; |
staticParams.stickD = 12; |
staticParams.stickYawP = 12; |
staticParams.stickThrottleD = 12; |
staticParams.minThrottle = 8; |
staticParams.maxThrottle = 230; |
staticParams.externalControl = 0; |
// IMU |
staticParams.gyroPIDFilterConstant = 1; |
staticParams.gyroATTFilterConstant = 1; |
staticParams.gyroDFilterConstant = 1; |
staticParams.accFilterConstant = 4; |
staticParams.gyroP = 60; |
staticParams.gyroI = 80; |
staticParams.gyroD = 4; |
// set by gyro code. |
// staticParams.zerothOrderCorrection = |
// staticParams.driftCompDivider = |
// staticParams.driftCompLimit = |
staticParams.axisCoupling1 = 90; |
staticParams.axisCoupling2 = 67; |
staticParams.axisCouplingYawCorrection = 0; |
staticParams.dynamicStability = 50; |
staticParams.IFactor = 32; |
staticParams.compassYawEffect = 128; |
// Servos |
for (uint8_t i=0; i<2; i++) { |
staticParams.servoConfigurations[i].manualControl = 128; |
staticParams.servoConfigurations[i].compensationFactor = 0; |
staticParams.servoConfigurations[i].minValue = 64; |
staticParams.servoConfigurations[i].maxValue = 192; |
staticParams.servoConfigurations[i].flags = 0; |
} |
staticParams.servoCount = 4; |
// Battery warning and emergency flight |
staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S |
staticParams.emergencyThrottle = 35; |
staticParams.emergencyFlightDuration = 30; |
// Outputs |
staticParams.outputFlash[0].bitmask = 0b01011111; |
staticParams.outputFlash[0].timing = 15; |
staticParams.outputFlash[1].bitmask = 0b11110011; |
staticParams.outputFlash[1].timing = 15; |
staticParams.outputFlags = 0; |
} |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void paramSet_default(uint8_t setnumber) { |
setOtherDefaults(); |
gyro_setDefaults(); |
for (uint8_t i=0; i<8; i++) { |
staticParams.userParams[i] = 0; |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION | |
CFG_HEADING_HOLD; |
staticParams.bitConfig2 = 0; |
memcpy(staticParams.name, "Default\0", 6); |
} |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
void mixerMatrix_default(void) { // Quadro |
uint8_t i; |
// mixerMatric.revision = EEMIXER_REVISION; |
// clear mixer table (but preset throttle) |
for (i = 0; i < 16; i++) { |
mixerMatrix.motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
mixerMatrix.motor[i][MIX_PITCH] = 0; |
mixerMatrix.motor[i][MIX_ROLL] = 0; |
mixerMatrix.motor[i][MIX_YAW] = 0; |
} |
// default = Quadro |
mixerMatrix.motor[0][MIX_PITCH] = +64; |
mixerMatrix.motor[0][MIX_YAW] = +64; |
mixerMatrix.motor[1][MIX_PITCH] = -64; |
mixerMatrix.motor[1][MIX_YAW] = +64; |
mixerMatrix.motor[2][MIX_ROLL] = -64; |
mixerMatrix.motor[2][MIX_YAW] = -64; |
mixerMatrix.motor[3][MIX_ROLL] = +64; |
mixerMatrix.motor[3][MIX_YAW] = -64; |
memcpy(mixerMatrix.name, "Quadro\0", 7); |
} |
void channelMap_default(void) { |
channelMap.channels[CH_PITCH] = 2; |
channelMap.channels[CH_ROLL] = 1; |
channelMap.channels[CH_THROTTLE] = 3; |
channelMap.channels[CH_YAW] = 4; |
channelMap.channels[CH_POTS + 0] = 5; |
channelMap.channels[CH_POTS + 1] = 6; |
channelMap.channels[CH_POTS + 2] = 7; |
channelMap.channels[CH_POTS + 3] = 8; |
} |
/branches/dongfang_FC_rewrite/configuration.h |
---|
4,40 → 4,46 |
#include <inttypes.h> |
#include <avr/io.h> |
#define MAX_CHANNELS 10 |
#define MAX_MOTORS 12 |
typedef struct { |
/*PMM*/uint8_t HeightD; |
/* P */uint8_t MaxHeight; |
/*PMM*/uint8_t HeightP; |
/* P */uint8_t Height_ACC_Effect; |
/* P */uint8_t CompassYawEffect; |
/* P */uint8_t GyroD; |
/*PMM*/uint8_t GyroP; |
/* P */uint8_t GyroI; |
/* Never used */uint8_t StickYawP; |
// IMU |
/*PMM*/uint8_t gyroP; |
/* P */uint8_t gyroD; |
/* P */uint8_t gyroI; |
/* P */uint8_t IFactor; |
/* P */uint8_t UserParams[8]; |
/* P */uint8_t ServoPitchControl; |
/* P */uint8_t LoopGasLimit; |
/* P */uint8_t AxisCoupling1; |
/* P */uint8_t AxisCoupling2; |
/* P */uint8_t AxisCouplingYawCorrection; |
/* P */uint8_t DynamicStability; |
/* P */uint8_t ExternalControl; |
/*PMM*/uint8_t J16Timing; |
/*PMM*/uint8_t J17Timing; |
/* P */uint8_t NaviGpsModeControl; |
/* P */uint8_t NaviGpsGain; |
/* P */uint8_t NaviGpsP; |
/* P */uint8_t NaviGpsI; |
/* P */uint8_t NaviGpsD; |
/* P */uint8_t NaviGpsACC; |
/*PMM*/uint8_t NaviOperatingRadius; |
/* P */uint8_t NaviWindCorrection; |
/* P */uint8_t NaviSpeedCompensation; |
int8_t KalmanK; |
int8_t KalmanMaxDrift; |
int8_t KalmanMaxFusion; |
uint8_t yawIFactor; |
/* P */uint8_t compassYawEffect; |
// Control |
/* P */uint8_t externalControl; |
/* P */uint8_t axisCoupling1; |
/* P */uint8_t axisCoupling2; |
/* P */uint8_t axisCouplingYawCorrection; |
/* P */uint8_t dynamicStability; |
uint8_t maxControlActivityForAcc; |
// Height control |
/*PMM*/uint8_t heightP; |
/*PMM*/uint8_t heightD; |
/* P */uint8_t heightSetting; |
/* P */uint8_t heightACCEffect; |
uint8_t attitudeControl; |
// The rest... |
/* P */uint8_t userParams[8]; |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t servoManualControl[2]; |
uint8_t motorSmoothing; |
} dynamicParam_t; |
extern dynamicParam_t dynamicParams; |
typedef struct { |
49,109 → 55,120 |
uint8_t sourceIdx, targetIdx; |
} XLATION; |
typedef struct { |
uint8_t checksum; |
uint8_t revisionNumber; |
uint8_t channels[MAX_CHANNELS]; |
} channelMap_t; |
extern channelMap_t channelMap; |
typedef struct { |
uint8_t checksum; |
uint8_t revision; |
int8_t name[12]; |
int8_t motor[MAX_MOTORS][4]; |
}__attribute__((packed)) mixerMatrix_t; |
extern mixerMatrix_t mixerMatrix; |
typedef struct { |
uint8_t manualControl; |
uint8_t compensationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t; |
typedef struct { |
uint8_t bitmask; |
uint8_t timing; |
} output_flash_t; |
// values above 250 representing poti1 to poti4 |
typedef struct { |
uint8_t ChannelAssignment[8]; // see upper defines for details |
uint8_t GlobalConfig; // see upper defines for bitcoding |
uint8_t HeightMinGas; // Value : 0-100 |
uint8_t HeightD; // Value : 0-250 |
uint8_t MaxHeight; // Value : 0-32 |
uint8_t HeightP; // Value : 0-32 |
uint8_t Height_Gain; // Value : 0-50 |
uint8_t Height_ACC_Effect; // Value : 0-250 |
uint8_t StickP; // Value : 1-6 |
uint8_t StickD; // Value : 0-64 |
uint8_t StickYawP; // Value : 1-20 |
uint8_t MinThrottle; // Value : 0-32 |
uint8_t MaxThrottle; // Value : 33-250 |
uint8_t GyroAccFactor; // Value : 1-64 |
uint8_t CompassYawEffect; // Value : 0-32 |
uint8_t GyroP; // Value : 10-250 |
uint8_t GyroI; // Value : 0-250 |
uint8_t GyroD; // Value : 0-250 |
uint8_t LowVoltageWarning; // Value : 0-250 |
uint8_t EmergencyGas; // Value : 0-250 //Gaswert bei Empüngsverlust |
uint8_t EmergencyGasDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
uint8_t Unused0; // |
// Global bitflags |
uint8_t bitConfig; // see upper defines for bitcoding |
uint8_t bitConfig2; // see upper defines for bitcoding |
// Height Control |
uint8_t heightP; // Value : 0-32 |
uint8_t heightD; // Value : 0-250 |
uint8_t heightSetting; // Value : 0-32 |
uint8_t heightMaxThrottleChange; // Value : 0-100 |
uint8_t heightSlewRate; // Value : 0-50 |
uint8_t heightACCEffect; // Value : 0-250 |
// Attitude Control |
uint8_t attitudeControl; |
// Control |
uint8_t stickP; // Value : 1-6 |
uint8_t stickD; // Value : 0-64 |
uint8_t stickYawP; // Value : 1-20 |
uint8_t stickThrottleD; |
uint8_t minThrottle; // Value : 0-32 |
uint8_t maxThrottle; // Value : 33-250 |
uint8_t externalControl; // for serial Control |
uint8_t maxControlActivityForAcc; |
// IMU |
uint8_t gyroPIDFilterConstant; |
uint8_t gyroATTFilterConstant; |
uint8_t gyroDFilterConstant; |
uint8_t accFilterConstant; |
uint8_t gyroP; // Value : 10-250 |
uint8_t gyroI; // Value : 0-250 |
uint8_t gyroD; // Value : 0-250 |
uint8_t zerothOrderCorrection; // Value : 1-64 |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
uint8_t axisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
uint8_t axisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t axisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t dynamicStability; // PID limit for Attitude controller |
uint8_t IFactor; // Value : 0-250 |
uint8_t UserParams1[4]; // Value : 0-250 |
/* |
uint8_t UserParam2; // Value : 0-250 |
uint8_t UserParam3; // Value : 0-250 |
uint8_t UserParam4; // Value : 0-250 |
*/ |
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 |
uint8_t LoopThreshold; // Value: 0-250 Schwelle für Stickausschlag |
uint8_t LoopHysteresis; // Value: 0-250 Hysterese für Stickausschlag |
uint8_t AxisCoupling1; // Value: 0-250 Faktor, mit dem Yaw die Achsen Roll und Nick koppelt (NickRollMitkopplung) |
uint8_t AxisCoupling2; // Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t AxisCouplingYawCorrection;// Value: 0-250 Faktor, mit dem Nick und Roll verkoppelt werden |
uint8_t unused0; |
uint8_t unused1; |
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung) |
uint8_t DriftComp; // limit for gyrodrift compensation |
uint8_t DynamicStability; // PID limit for Attitude controller |
uint8_t UserParams2[4]; // Value : 0-250 |
/* |
uint8_t UserParam6; // Value : 0-250 |
uint8_t UserParam7; // Value : 0-250 |
uint8_t UserParam8; // 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 NaviGpsModeControl; // Parameters for the Naviboard |
uint8_t NaviGpsGain; // overall gain for GPS-PID controller |
uint8_t NaviGpsP; // P gain for GPS-PID controller |
uint8_t NaviGpsI; // I gain for GPS-PID controller |
uint8_t NaviGpsD; // D gain for GPS-PID controller |
uint8_t NaviGpsPLimit; // P limit for GPS-PID controller |
uint8_t NaviGpsILimit; // I limit for GPS-PID controller |
uint8_t NaviGpsDLimit; // D limit for GPS-PID controller |
uint8_t NaviGpsACC; // ACC gain for GPS-PID controller |
uint8_t NaviGpsMinSat; // number of sattelites neccesary for GPS functions |
uint8_t NaviStickThreshold; // activation threshild for detection of manual stick movements |
uint8_t NaviWindCorrection; // streng of wind course correction |
uint8_t NaviSpeedCompensation; // D gain fefore position hold login |
uint8_t NaviOperatingRadius; // Radius limit in m around start position for GPS flights |
uint8_t NaviAngleLimitation; // limitation of attitude angle controlled by the gps algorithm |
uint8_t NaviPHLoginTime; // position hold logintimeout |
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; |
uint8_t yawIFactor; |
uint8_t compassYawEffect; // Value : 0-32 |
#define PARAMSET_STRUCT_LEN sizeof(paramset_t) |
// Servos |
servo_t servoConfigurations[2]; // [PITCH, ROLL] |
uint8_t servoCount; |
extern paramset_t staticParams; |
// Battery warning and emergency flight |
uint8_t batteryVoltageWarning; // Value : 0-250 |
uint8_t emergencyThrottle; // Value : 0-250 //Gaswert bei Empüngsverlust /* |
uint8_t emergencyFlightDuration; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen |
typedef struct { |
uint8_t Revision; |
int8_t Name[12]; |
int8_t Motor[16][4]; |
}__attribute__((packed)) MixerTable_t; |
// Outputs |
output_flash_t outputFlash[2]; |
uint8_t outputDebugMask; |
// bit0: Test all OFF |
// bit1: Test all ON |
// bit2: flash when beeping |
uint8_t outputFlags; |
extern MixerTable_t Mixer; |
// User params |
uint8_t userParams[8]; // Value : 0-250 |
// Name |
uint8_t name[12]; |
} paramset_t; |
extern paramset_t staticParams; |
// MKFlags |
#define MKFLAG_MOTOR_RUN (1<<0) |
#define MKFLAG_FLY (1<<1) |
#define MKFLAG_CALIBRATE (1<<2) |
#define MKFLAG_START (1<<3) |
#define MKFLAG_EMERGENCY_LANDING (1<<4) |
#define MKFLAG_EMERGENCY_FLIGHT (1<<4) |
#define MKFLAG_RESERVE1 (1<<5) |
#define MKFLAG_RESERVE2 (1<<6) |
#define MKFLAG_RESERVE3 (1<<7) |
// bit mask for staticParams.GlobalConfig |
// bit mask for staticParams.bitConfig |
#define CFG_HEIGHT_CONTROL (1<<0) |
#define CFG_HEIGHT_SWITCH (1<<1) |
#define CFG_HEADING_HOLD (1<<2) |
159,9 → 176,9 |
#define CFG_COMPASS_FIX (1<<4) |
#define CFG_GPS_ACTIVE (1<<5) |
#define CFG_AXIS_COUPLING_ACTIVE (1<<6) |
#define CFG_ROTARY_RATE_LIMITER (1<<7) |
#define CFG_GYRO_SATURATION_PREVENTION (1<<7) |
// bit mask for staticParams.BitConfig |
// bit mask for staticParams.bitConfig2 |
#define CFG_LOOP_UP (1<<0) |
#define CFG_LOOP_DOWN (1<<1) |
#define CFG_LOOP_LEFT (1<<2) |
173,11 → 190,11 |
#define SYSCLK F_CPU |
// Not really a part of configuration, but LEDs and HW version test are the same. |
#define RED_OFF {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
#define RED_ON {if((BoardRelease == 10) || (BoardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);} |
#define RED_OFF {if((boardRelease == 10) || (boardRelease == 20)) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);} |
#define RED_ON {if((boardRelease == 10) || (boardRelease == 20)) PORTB |= (1<<PORTB0); else PORTB &=~(1<<PORTB0);} |
#define RED_FLASH PORTB ^= (1<<PORTB0) |
#define GRN_OFF {if(BoardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);} |
#define GRN_ON {if(BoardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);} |
#define GRN_OFF {if(boardRelease < 12) PORTB &=~(1<<PORTB1); else PORTB |= (1<<PORTB1);} |
#define GRN_ON {if(boardRelease < 12) PORTB |= (1<<PORTB1); else PORTB &=~(1<<PORTB1);} |
#define GRN_FLASH PORTB ^= (1<<PORTB1) |
// Mixer table |
187,10 → 204,15 |
#define MIX_YAW 3 |
extern volatile uint8_t MKFlags; |
extern uint8_t requiredMotors; |
extern int16_t variables[8]; // The "Poti"s. |
extern uint8_t BoardRelease; |
extern uint8_t boardRelease; |
extern uint8_t CPUType; |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void mixerMatrix_default(void); |
void configuration_applyVariablesToParams(void); |
uint8_t getCPUType(void); |
uint8_t getBoardRelease(void); |
/branches/dongfang_FC_rewrite/controlMixer.c |
---|
63,8 → 63,7 |
// 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; |
//uint8_t looping = 0; |
// Internal variables for reading commands made with an R/C stick. |
uint8_t lastCommand = COMMAND_NONE; |
192,11 → 191,11 |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
configuration_applyVariablesToParams(); |
looping = RC_getLooping(looping); |
//looping = RC_getLooping(looping); |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
looping = 0; |
// looping = 0; |
} |
// part1a end. |
/branches/dongfang_FC_rewrite/controlMixer.h |
---|
75,7 → 75,7 |
//extern int16_t controlYaw, controlThrottle; |
extern uint16_t controlActivity; |
//extern uint16_t maxControl[2]; |
extern uint8_t looping; |
//extern uint8_t looping; |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
63,164 → 63,18 |
#define EEMEM __attribute__ ((section (".eeprom"))) |
#endif |
#include <avr/eeprom.h> |
#include <string.h> |
#include "eeprom.h" |
#include "printf_P.h" |
#include "output.h" |
// TODO: Get rid of these. They have nothing to do with eeprom. |
#include "flight.h" |
#include "rc.h" |
#include "sensors.h" |
#include <avr/eeprom.h> |
// byte array in eeprom |
uint8_t EEPromArray[E2END + 1] EEMEM; |
paramset_t staticParams; |
MixerTable_t Mixer; |
/* |
* Default for your own experiments here, so you don't have to reset them |
* from MK-Tool all the time. |
*/ |
void setDefaultUserParams(void) { |
uint8_t i; |
for (i = 0; i < sizeof(staticParams.UserParams1); i++) { |
staticParams.UserParams1[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.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 |
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles. |
} |
void setOtherDefaults(void) { |
/* Channel assignments were changed to the normal: |
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4 |
*/ |
staticParams.ChannelAssignment[CH_PITCH] = 2; |
staticParams.ChannelAssignment[CH_ROLL] = 1; |
staticParams.ChannelAssignment[CH_THROTTLE] = 3; |
staticParams.ChannelAssignment[CH_YAW] = 4; |
staticParams.ChannelAssignment[CH_POTS + 0] = 5; |
staticParams.ChannelAssignment[CH_POTS + 1] = 6; |
staticParams.ChannelAssignment[CH_POTS + 2] = 7; |
staticParams.ChannelAssignment[CH_POTS + 3] = 8; |
staticParams.GlobalConfig = /* CFG_AXIS_COUPLING_ACTIVE | */ CFG_HEADING_HOLD; // CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE;//CFG_HEIGHT_CONTROL | CFG_HEIGHT_SWITCH | CFG_COMPASS_FIX; |
staticParams.HeightMinGas = 30; |
staticParams.MaxHeight = 251; |
staticParams.HeightP = 10; |
staticParams.HeightD = 30; |
staticParams.Height_ACC_Effect = 30; |
staticParams.Height_Gain = 4; |
staticParams.StickP = 8; |
staticParams.StickD = 12; |
staticParams.StickYawP = 12; |
staticParams.MinThrottle = 8; |
staticParams.MaxThrottle = 230; |
staticParams.CompassYawEffect = 128; |
staticParams.GyroP = 80; |
staticParams.GyroI = 100; |
staticParams.LowVoltageWarning = 101; // 3.7 each for 3S |
staticParams.EmergencyGas = 35; |
staticParams.EmergencyGasDuration = 30; |
staticParams.Unused0 = 0; |
staticParams.IFactor = 32; |
staticParams.ServoPitchControl = 100; |
staticParams.ServoPitchComp = 40; |
staticParams.ServoPitchCompInvert = 0; |
staticParams.ServoPitchMin = 50; |
staticParams.ServoPitchMax = 150; |
staticParams.ServoRefresh = 5; |
staticParams.LoopGasLimit = 50; |
staticParams.LoopThreshold = 90; |
staticParams.LoopHysteresis = 50; |
staticParams.BitConfig = 0; |
staticParams.AxisCoupling1 = 90; |
staticParams.AxisCoupling2 = 67; |
staticParams.AxisCouplingYawCorrection = 0; |
staticParams.DynamicStability = 50; |
staticParams.J16Bitmask = 95; |
staticParams.J17Bitmask = 243; |
staticParams.J16Timing = 15; |
staticParams.J17Timing = 15; |
staticParams.NaviGpsModeControl = 253; |
staticParams.NaviGpsGain = 100; |
staticParams.NaviGpsP = 90; |
staticParams.NaviGpsI = 90; |
staticParams.NaviGpsD = 90; |
staticParams.NaviGpsPLimit = 75; |
staticParams.NaviGpsILimit = 75; |
staticParams.NaviGpsDLimit = 75; |
staticParams.NaviGpsACC = 0; |
staticParams.NaviGpsMinSat = 6; |
staticParams.NaviStickThreshold = 8; |
staticParams.NaviWindCorrection = 90; |
staticParams.NaviSpeedCompensation = 30; |
staticParams.NaviOperatingRadius = 100; |
staticParams.NaviAngleLimitation = 100; |
staticParams.NaviPHLoginTime = 4; |
} |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void ParamSet_DefaultSet1(void) { // sport |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.GlobalConfig = CFG_ROTARY_RATE_LIMITER |
/* | CFG_AXIS_COUPLING_ACTIVE*/; |
staticParams.EmergencyGasDuration = 200; |
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.GlobalConfig = CFG_ROTARY_RATE_LIMITER |
/* | CFG_AXIS_COUPLING_ACTIVE*/ ; |
staticParams.Height_Gain = 3; |
staticParams.EmergencyGasDuration = 200; |
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.GlobalConfig = CFG_ROTARY_RATE_LIMITER |
/* | CFG_AXIS_COUPLING_ACTIVE */; |
staticParams.Height_Gain = 3; |
staticParams.EmergencyGasDuration = 200; |
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) { |
uint8_t getParamByte(uint16_t param_id) { |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
227,7 → 81,7 |
/***************************************************/ |
/* Write Parameter to EEPROM as byte */ |
/***************************************************/ |
void SetParamByte(uint16_t param_id, uint8_t value) { |
void setParamByte(uint16_t param_id, uint8_t value) { |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
234,7 → 88,7 |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
uint16_t GetParamWord(uint16_t param_id) { |
uint16_t getParamWord(uint16_t param_id) { |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN |
+ param_id]); |
} |
242,22 → 96,39 |
/***************************************************/ |
/* Write Parameter to EEPROM as word */ |
/***************************************************/ |
void SetParamWord(uint16_t param_id, uint16_t value) { |
eeprom_write_word( |
(uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
void setParamWord(uint16_t param_id, uint16_t value) { |
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
uint8_t calculateChecksum(uint8_t* data, uint16_t length) { |
uint8_t result = 0; |
for (uint16_t i=0; i<length; i++) { |
result += data[i]; |
} |
return result; |
} |
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t length, uint16_t offset) { |
uint8_t checksum = calculateChecksum(data+1, length-1); |
data[0] = checksum; |
data[1] = revisionNumber; |
eeprom_write_block(data, &EEPromArray[offset], length); |
} |
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t length, uint16_t offset) { |
eeprom_read_block(target, &EEPromArray[offset], length); |
uint8_t checksum = calculateChecksum(target+1, length-1); |
return (checksum != target[0] || revisionNumber != target[1]); |
} |
/***************************************************/ |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_ReadFromEEProm(uint8_t setnumber) { |
if ((1 > setnumber) || (setnumber > 5)) |
setnumber = 3; |
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber |
- 1)], PARAMSET_STRUCT_LEN); |
output_init(); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t); |
output_init(); // what's that doing here?? |
return readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset); |
} |
/***************************************************/ |
264,159 → 135,104 |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_WriteToEEProm(uint8_t setnumber) { |
if (setnumber > 5) |
setnumber = 5; |
if (setnumber < 1) |
return; |
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber |
- 1)], 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 |
void paramSet_writeToEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*sizeof(paramset_t); |
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, sizeof(paramset_t), offset); |
// set this parameter set to active set |
setActiveParamSet(setnumber); |
output_init(); |
output_init(); // what's that doing here?? |
} |
/***************************************************/ |
/* Get active parameter set */ |
/***************************************************/ |
uint8_t getActiveParamSet(void) { |
uint8_t setnumber; |
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]); |
if (setnumber > 5) { |
setnumber = 3; |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
void paramSet_readOrDefault() { |
uint8_t setnumber = getActiveParamSet(); |
// parameter version check |
if (setnumber<1 ||setnumber>5 || paramSet_readFromEEProm(setnumber)) { |
// if version check faild |
printf("\n\rInit Parameter in EEPROM"); |
for (uint8_t i=6; i>0; i--) { |
paramSet_default(i); // Fill staticParams Structure to default parameter set 1 (Sport) |
paramSet_writeToEEProm(i); |
} |
return (setnumber); |
// default-Setting is parameter set 3 |
setActiveParamSet(1); |
} |
/***************************************************/ |
/* 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); |
printf("\n\rUsing Parameter Set %d", getActiveParamSet()); |
} |
/***************************************************/ |
/* Read MixerTable from EEPROM */ |
/* MixerTable */ |
/***************************************************/ |
uint8_t MixerTable_ReadFromEEProm(void) { |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE]) |
== EEMIXER_REVISION) { |
eeprom_read_block((uint8_t *) &Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], |
sizeof(Mixer)); |
return 1; |
} else |
return 0; |
uint8_t mixerMatrix_readFromEEProm(void) { |
return readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t)); |
} |
/***************************************************/ |
/* Write Mixer Table to EEPROM */ |
/***************************************************/ |
uint8_t MixerTable_WriteToEEProm(void) { |
if (Mixer.Revision == EEMIXER_REVISION) { |
eeprom_write_block((uint8_t *) &Mixer, |
&EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
return 1; |
} else |
return 0; |
void mixerMatrix_writeToEEProm(void) { |
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t)); |
} |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/***************************************************/ |
void MixerTable_Default(void) { // Quadro |
uint8_t i; |
Mixer.Revision = EEMIXER_REVISION; |
// clear mixer table (but preset throttle) |
for (i = 0; i < 16; i++) { |
Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0; |
Mixer.Motor[i][MIX_PITCH] = 0; |
Mixer.Motor[i][MIX_ROLL] = 0; |
Mixer.Motor[i][MIX_YAW] = 0; |
void mixerMatrix_readOrDefault(void) { |
// load mixer table |
if (mixerMatrix_readFromEEProm()) { |
printf("\n\rGenerating default mixerMatrix"); |
mixerMatrix_default(); // Quadro |
mixerMatrix_writeToEEProm(); |
} |
// default = Quadro |
Mixer.Motor[0][MIX_PITCH] = +64; |
Mixer.Motor[0][MIX_YAW] = +64; |
Mixer.Motor[1][MIX_PITCH] = -64; |
Mixer.Motor[1][MIX_YAW] = +64; |
Mixer.Motor[2][MIX_ROLL] = -64; |
Mixer.Motor[2][MIX_YAW] = -64; |
Mixer.Motor[3][MIX_ROLL] = +64; |
Mixer.Motor[3][MIX_YAW] = -64; |
memcpy(Mixer.Name, "Quadro\0", 7); |
// determine motornumber |
requiredMotors = 0; |
for (uint8_t i=0; i<MAX_MOTORS; i++) { |
if (mixerMatrix.motor[i][MIX_THROTTLE]) |
requiredMotors++; |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors); |
printf("\n\r=============================="); |
} |
/***************************************************/ |
/* Initialize EEPROM Parameter Sets */ |
/* ChannelMap */ |
/***************************************************/ |
void ParamSet_Init(void) { |
uint8_t Channel_Backup = 1, i, j; |
// parameter version check |
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) { |
// if version check faild |
printf("\n\rInit Parameter in EEPROM"); |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_MIXER_TABLE], 0xFF); // reset also mixer table |
// check if channel mapping backup is valid |
for (j = 0; j < 4 && Channel_Backup; j++) { |
if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_CHANNELS + 0]) >= 12) |
Channel_Backup = 0; |
uint8_t channelMap_readFromEEProm(void) { |
return readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
// 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; |
void channelMap_writeToEEProm(void) { |
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
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]); |
void channelMap_readOrDefault(void) { |
if (!channelMap_readFromEEProm()) { |
printf("\n\rGenerating default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
} |
} |
ParamSet_WriteToEEProm(i); |
uint8_t accOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
// default-Setting is parameter set 3 |
setActiveParamSet(1); |
// update version info |
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
void accOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
// read active parameter set to staticParams stucture |
ParamSet_ReadFromEEProm(getActiveParamSet()); |
printf("\n\rUsing Parameter Set %d", getActiveParamSet()); |
// load mixer table |
if (!MixerTable_ReadFromEEProm()) { |
printf("\n\rGenerating default Mixer Table"); |
MixerTable_Default(); // Quadro |
MixerTable_WriteToEEProm(); |
/***************************************************/ |
/* Get active parameter set */ |
/***************************************************/ |
uint8_t getActiveParamSet(void) { |
uint8_t setnumber; |
setnumber = eeprom_read_byte(&EEPromArray[PID_ACTIVE_SET]); |
if (setnumber > 4) { |
setActiveParamSet(setnumber = 0); |
} |
// determine motornumber |
RequiredMotors = 0; |
for (i = 0; i < 16; i++) { |
if (Mixer.Motor[i][MIX_THROTTLE] > 0) |
RequiredMotors++; |
return setnumber; |
} |
printf("\n\rMixer-Config: '%s' (%u Motors)",Mixer.Name, RequiredMotors); |
printf("\n\r=============================="); |
/***************************************************/ |
/* Set active parameter set */ |
/***************************************************/ |
void setActiveParamSet(uint8_t setnumber) { |
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber); |
} |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
3,33 → 3,53 |
#include <inttypes.h> |
#include "configuration.h" |
#include "analog.h" |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define PID_PARAM_REVISION 1 // byte |
#define PID_ACTIVE_SET 2 // byte |
#define PID_PRESSURE_OFFSET 3 // byte |
#define PID_ACC_PITCH 4 // word |
#define PID_ACC_ROLL 6 // word |
#define PID_ACC_Z 8 // word |
#define EEPROM_ADR_CHANNELS 80 // 8 bytes |
#define EEPROM_ADR_PARAMSET_LENGTH 98 // word |
//#define PID_ACC_PITCH 4 // word |
//#define PID_ACC_ROLL 6 // word |
//#define PID_ACC_Z 8 // word |
#define EEPROM_ADR_ACCOFFSET 20 |
#define EEPROM_ADR_GYROOFFSET 30 |
#define EEPROM_ADR_CHANNELMAP 80 |
#define EEPROM_ADR_PARAMSET_BEGIN 100 |
#define EEPROM_ADR_MIXER_TABLE 1000 // 1000 - 1076 |
#define EEPARAM_REVISION 75 // is count up, if paramater stucture has changed (compatibility) |
#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); |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 0 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
extern uint8_t MixerTable_ReadFromEEProm(void); |
extern uint8_t MixerTable_WriteToEEProm(void); |
extern void paramSet_readOrDefault(void); |
extern void channelMap_readOrDefault(void); |
extern void mixerMatrix_readOrDefault(void); |
extern uint8_t GetParamByte(uint16_t param_id); |
extern void SetParamByte(uint16_t param_id, uint8_t value); |
extern uint16_t GetParamWord(uint16_t param_id); |
extern void SetParamWord(uint16_t param_id, uint16_t value); |
extern uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
extern void paramSet_writeToEEProm(uint8_t setnumber); |
extern uint8_t channelMap_readFromEEProm(void); |
extern void channelMap_writeToEEProm(void); |
extern uint8_t mixerMatrix_eeadFromEEProm(void); |
extern void mixerMatrix_writeToEEProm(void); |
extern uint8_t accOffset_readFromEEProm(void); |
extern void accOffset_writeToEEProm(void); |
extern uint8_t gyroOffset_readFromEEProm(void); |
extern void gyroOffset_writeToEEProm(void); |
extern uint8_t getParamByte(uint16_t param_id); |
extern void setParamByte(uint16_t param_id, uint8_t value); |
extern uint16_t getParamWord(uint16_t param_id); |
extern void setParamWord(uint16_t param_id, uint16_t value); |
extern uint8_t getActiveParamSet(void); |
extern void setActiveParamSet(uint8_t setnumber); |
/branches/dongfang_FC_rewrite/externalControl.c |
---|
40,8 → 40,8 |
void EC_update() { |
if (externalControlActive) { |
externalControlActive--; |
EC_PRTY[CONTROL_PITCH] = externalControl.pitch * (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.StickP; |
EC_PRTY[CONTROL_PITCH] = externalControl.pitch * (int16_t) staticParams.stickP; |
EC_PRTY[CONTROL_ROLL] = externalControl.roll * (int16_t) staticParams.stickP; |
EC_PRTY[CONTROL_THROTTLE] = externalControl.throttle; |
EC_PRTY[CONTROL_YAW] = externalControl.yaw; // No stickP or similar?????? |
} else { |
58,7 → 58,7 |
// Configured and heard from |
return SIGNAL_OK; |
if (!(externalControl.config & 0x01 && dynamicParams.ExternalControl > 128)) |
if (!(externalControl.config & 0x01 && dynamicParams.externalControl > 128)) |
// External control is not even configured. |
return NO_SIGNAL; |
/branches/dongfang_FC_rewrite/flight.c |
---|
61,10 → 61,6 |
// Necessary for external control and motor test |
#include "uart0.h" |
// for scope debugging |
// #include "rc.h" |
#include "twimaster.h" |
#include "attitude.h" |
#include "controlMixer.h" |
86,13 → 82,12 |
// Some integral weight constant... |
uint16_t Ki = 10300 / 33; |
uint8_t RequiredMotors = 0; |
/************************************************************************/ |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
switch (dynamicParams.UserParams[5]) { |
switch (dynamicParams.motorSmoothing) { |
case 0: |
return newvalue; |
case 1: |
118,9 → 113,11 |
void flight_setNeutral() { |
MKFlags |= MKFLAG_CALIBRATE; |
// not really used here any more. |
/* |
dynamicParams.KalmanK = -1; |
dynamicParams.KalmanMaxDrift = 0; |
dynamicParams.KalmanMaxFusion = 32; |
*/ |
controlMixer_initVariables(); |
} |
136,10 → 133,10 |
void setNormalFlightParameters(void) { |
setFlightParameters( |
dynamicParams.IFactor, |
dynamicParams.GyroP, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP, |
dynamicParams.UserParams[6] |
dynamicParams.gyroP, |
staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
dynamicParams.gyroP, |
dynamicParams.yawIFactor |
); |
} |
179,10 → 176,10 |
throttleTerm = controls[CONTROL_THROTTLE]; |
// This check removed. Is done on a per-motor basis, after output matrix multiplication. |
if (throttleTerm < staticParams.MinThrottle + 10) |
throttleTerm = staticParams.MinThrottle + 10; |
else if (throttleTerm > staticParams.MaxThrottle - 20) |
throttleTerm = (staticParams.MaxThrottle - 20); |
if (throttleTerm < staticParams.minThrottle + 10) |
throttleTerm = staticParams.minThrottle + 10; |
else if (throttleTerm > staticParams.maxThrottle - 20) |
throttleTerm = (staticParams.maxThrottle - 20); |
/************************************************************************/ |
/* RC-signal is bad */ |
199,8 → 196,8 |
emergencyFlightTime--; |
if (isFlying > 256) { |
// We're probably still flying. Descend slowly. |
throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle |
MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing |
setStableFlightParameters(); |
} else { |
MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
207,15 → 204,15 |
} |
} else { |
// end emergency flight (just cut the motors???) |
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT); |
} |
} else { |
// signal is acceptable |
if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
// Reset emergency landing control variables. |
MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
// The time is in whole seconds. |
emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
} |
// If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
246,34 → 243,20 |
} |
commands_handleCommands(); |
// if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
setNormalFlightParameters(); |
// } |
} // end else (not bad signal case) |
// end part1a: 750-800 usec. |
/* |
* Looping the H&I way basically is just a matter of turning off attitude angle measurement |
* by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
* This is the throttle part. |
*/ |
if (looping) { |
if (throttleTerm > staticParams.LoopGasLimit) |
throttleTerm = staticParams.LoopGasLimit; |
} |
/************************************************************************/ |
/* Yawing */ |
/************************************************************************/ |
if (abs(controls[CONTROL_YAW]) > 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)) { |
if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
updateCompassCourse = 1; |
} |
} |
// yawControlRate = controlYaw; |
// 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. |
285,7 → 268,7 |
/************************************************************************/ |
/* Compass is currently not supported. */ |
/************************************************************************/ |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
updateCompass(); |
} |
310,11 → 293,7 |
// The P-part is the P of the PID controller. That's the angle integrals (not rates). |
for (axis = PITCH; axis <= ROLL; axis++) { |
if (looping & ((1 << 4) << axis)) { |
PPart[axis] = 0; |
} else { // TODO: Where do the 44000 come from??? |
PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
} |
/* |
* Now blend in the D-part - proportional to the Differential of the integral = the rate. |
321,9 → 300,8 |
* Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
* where pfactor is in [0..1]. |
*/ |
PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
* gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
* (int16_t) dynamicParams.GyroD) / 16; |
PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
* (int16_t) dynamicParams.gyroD) / 16; |
CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
} |
343,7 → 321,7 |
// if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
isFlying = 1; // keep within lift off condition |
throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of |
} |
// Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
381,7 → 359,7 |
} |
// FIXME: Throttle may exceed maxThrottle (there is no check no more). |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
tmp_int = staticParams.maxThrottle * CONTROL_SCALING; |
if (yawTerm < -(tmp_int - throttleTerm)) { |
yawTerm = -(tmp_int - throttleTerm); |
debugOut.digital[0] |= DEBUG_CLIP; |
407,7 → 385,7 |
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 |
tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
* (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
// TODO: From which planet comes the 16000? |
443,10 → 421,10 |
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 = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE]; |
tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH]; |
tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL]; |
tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW]; |
tmp = tmp >> 6; |
motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
// Now we scale back down to a 0..255 range. |
464,7 → 442,7 |
if (i < 4) debugOut.analog[22 + i] = throttle; |
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) { |
motor[i].SetPoint = throttle; |
} else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
486,6 → 464,6 |
debugOut.analog[16] = gyroPFactor; |
debugOut.analog[17] = gyroIFactor; |
debugOut.analog[18] = dynamicParams.GyroD; |
debugOut.analog[18] = dynamicParams.gyroD; |
} |
} |
/branches/dongfang_FC_rewrite/flight.h |
---|
22,8 → 22,6 |
* same, they are unseperable anyway. |
*/ |
extern uint8_t RequiredMotors; |
// looping params |
// extern long TurnOver180Nick, TurnOver180Roll; |
/branches/dongfang_FC_rewrite/heightControl.c |
---|
64,10 → 64,10 |
if (height > maxHeight) |
maxHeight = height; |
if (staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) { |
if (staticParams.bitConfig & CFG_HEIGHT_SWITCH) { |
// If switch is activated in config, the MaxHeight parameter is a switch value: ON in both ends of the range; OFF in the middle. |
// DebugOut.Digital[0] |= DEBUG_HEIGHT_SWITCH; |
if (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40) { |
if (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40) { |
// Switch is ON |
if (setHeightLatch <= LATCH_TIME) { |
if (setHeightLatch == LATCH_TIME) { |
86,15 → 86,15 |
} else { |
// Switch is not activated; take the "max-height" as the target height. |
// DebugOut.Digital[0] &= ~DEBUG_HEIGHT_SWITCH; |
targetHeight = (uint16_t) dynamicParams.MaxHeight * 100; //getHeight() + 10 * 100; |
targetHeight = (uint16_t) dynamicParams.heightSetting * 100; //getHeight() + 10 * 100; |
} |
if (++heightRampingTimer == INTEGRATION_FREQUENCY / 10) { |
heightRampingTimer = 0; |
if (rampedTargetHeight + staticParams.Height_Gain <= targetHeight) { |
rampedTargetHeight += staticParams.Height_Gain; |
} else if (rampedTargetHeight - staticParams.Height_Gain >= targetHeight) { |
rampedTargetHeight -= staticParams.Height_Gain; |
if (rampedTargetHeight + staticParams.heightSlewRate <= targetHeight) { |
rampedTargetHeight += staticParams.heightSlewRate; |
} else if (rampedTargetHeight - staticParams.heightSlewRate >= targetHeight) { |
rampedTargetHeight -= staticParams.heightSlewRate; |
} |
} |
138,20 → 138,20 |
else if (iHeight < 0) { if (DEBUGINTEGRAL) DebugOut.Digital[1] = 1;} |
*/ |
int16_t dThrottle = heightError * staticParams.HeightP / 1000 |
int16_t dThrottle = heightError * staticParams.heightP / 1000 |
/*+ iHeight / 10000L * staticParams.Height_ACC_Effect */- dHeight |
* staticParams.HeightD; |
* staticParams.heightD; |
// the "minGas" is now a limit for how much up / down the throttle can be varied |
if (dThrottle > staticParams.HeightMinGas) |
dThrottle = staticParams.HeightMinGas; |
else if (dThrottle < -staticParams.HeightMinGas) |
dThrottle = -staticParams.HeightMinGas; |
if (dThrottle > staticParams.heightMaxThrottleChange) |
dThrottle = staticParams.heightMaxThrottleChange; |
else if (dThrottle < -staticParams.heightMaxThrottleChange) |
dThrottle = -staticParams.heightMaxThrottleChange; |
// TODO: Eliminate repetition. |
if (staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) { |
if (!(staticParams.GlobalConfig & CFG_HEIGHT_SWITCH) |
|| (dynamicParams.MaxHeight < 40 || dynamicParams.MaxHeight > 255 - 40)) { |
if (staticParams.bitConfig & CFG_HEIGHT_CONTROL) { |
if (!(staticParams.bitConfig & CFG_HEIGHT_SWITCH) |
|| (dynamicParams.heightSetting < 40 || dynamicParams.heightSetting > 255 - 40)) { |
// If switch is not in use --> Just apply height control. |
// If switch is in use --> only apply height control when switch is also ON. |
throttle += dThrottle; |
/branches/dongfang_FC_rewrite/invenSense.c |
---|
33,8 → 33,8 |
} |
void gyro_setDefaults(void) { |
staticParams.GyroD = 3; |
staticParams.GyroAccFactor = 1; |
staticParams.DriftComp = 3; |
staticParams.GyroAccTrim = 2; |
staticParams.gyroD = 3; |
staticParams.driftCompDivider = 2; |
staticParams.driftCompLimit = 3; |
staticParams.zerothOrderCorrection = 2; |
} |
/branches/dongfang_FC_rewrite/main.c |
---|
85,7 → 85,7 |
// analyze hardware environment |
CPUType = getCPUType(); |
BoardRelease = getBoardRelease(); |
boardRelease = getBoardRelease(); |
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
128,7 → 128,8 |
printf("\n\r==================================="); |
// Parameter Set handling |
ParamSet_Init(); |
channelMap_readOrDefault(); |
paramSet_readOrDefault(); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
160,17 → 161,6 |
printf("\n\r==================================="); |
/* |
if(staticParams.GlobalConfig & CFG_HEIGHT_CONTROL) |
{ |
printf("\n\rCalibrating air pressure sensor.."); |
timer = SetDelay(1000); |
SearchAirPressureOffset(); |
while (!CheckDelay(timer)); |
printf("OK\n\r"); |
} |
*/ |
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
#ifdef USE_RC_DSL |
205,7 → 195,7 |
beep(2000); |
printf("\n\rControl: "); |
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) |
if (staticParams.bitConfig & CFG_HEADING_HOLD) |
printf("HeadingHold"); |
else printf("Neutral (ACC-Mode)"); |
248,7 → 238,7 |
if (UBat <= UBAT_AT_5V) { |
// Do nothing. The voltage on the input side of the regulator is <5V; |
// we must be running off USB power. Keep it quiet. |
} else if (UBat < staticParams.LowVoltageWarning) { |
} else if (UBat < staticParams.batteryVoltageWarning) { |
beepBatteryAlarm(); |
} |
/branches/dongfang_FC_rewrite/menu.c |
---|
126,9 → 126,9 |
case 0:// Version Info Menu Item |
LCD_printfxy(0,0,"+ MikroKopter +") |
; |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",BoardRelease/10,BoardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a') |
LCD_printfxy(0,1,"HW:V%d.%d SW:%d.%d%c",boardRelease/10,boardRelease%10,VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH+'a') |
; |
LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), Mixer.Name) |
LCD_printfxy(0,2,"Setting: %d %s", getActiveParamSet(), mixerMatrix.name) |
; |
if (I2CTimeout < 6) { |
LCD_printfxy(0,3,"I2C Error!!!"); |
164,10 → 164,10 |
LCD_printfxy(0,3,"C7:%4i C8:%4i ",PPM_in[7],PPM_in[8]); |
break; |
case 4:// Remote Control Mapping Menu Item |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[staticParams.ChannelAssignment[CH_PITCH]],PPM_in[staticParams.ChannelAssignment[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[staticParams.ChannelAssignment[CH_THROTTLE]],PPM_in[staticParams.ChannelAssignment[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS]],PPM_in[staticParams.ChannelAssignment[CH_POTS+1]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[staticParams.ChannelAssignment[CH_POTS+2]],PPM_in[staticParams.ChannelAssignment[CH_POTS+3]]); |
LCD_printfxy(0,0,"Ni:%4i Ro:%4i ",PPM_in[channelMap.channels[CH_PITCH]], PPM_in[channelMap.channels[CH_ROLL]]); |
LCD_printfxy(0,1,"Gs:%4i Ya:%4i ",PPM_in[channelMap.channels[CH_THROTTLE]],PPM_in[channelMap.channels[CH_YAW]]); |
LCD_printfxy(0,2,"P1:%4i P2:%4i ",PPM_in[channelMap.channels[CH_POTS]], PPM_in[channelMap.channels[CH_POTS+1]]); |
LCD_printfxy(0,3,"P3:%4i P4:%4i ",PPM_in[channelMap.channels[CH_POTS+2]], PPM_in[channelMap.channels[CH_POTS+3]]); |
break; |
/* |
case 5:// Gyro Sensor Menu Item |
/branches/dongfang_FC_rewrite/output.c |
---|
69,10 → 69,10 |
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, |
uint8_t manual) { |
if (timing > 250 && manual > 230) { |
// "timing" is set to "manual" and the value is very high --> Set to the value in bitmask bit 7. |
// "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
OUTPUT_SET(port, bitmask & 128); |
} else if (timing > 250 && manual < 10) { |
// "timing" is set to "manual" and the value is very low --> Set to the negated value in bitmask bit 7. |
// "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
OUTPUT_SET(port, !(bitmask & 128)); |
} else if (!flashCnt[port]--) { |
// rotating mask over bitmask... |
89,37 → 89,29 |
static int8_t delay = 0; |
if (!delay--) { // 10 ms intervals |
delay = 4; |
if (beepModulation != BEEP_MODULATION_NONE) { |
if (staticParams.outputFlags & 4 && 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); |
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
} |
} |
} |
void output_update(void) { |
if (!DIGITAL_DEBUG_MASK) { |
flashingLights(); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
if (staticParams.outputFlags & 1) { |
OUTPUT_SET(0, 1); |
OUTPUT_SET(1, 1); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) { |
} else if (staticParams.outputFlags & 2) { |
OUTPUT_SET(0, 0); |
OUTPUT_SET(1, 0); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_0) { |
OUTPUT_SET(0, 1); |
OUTPUT_SET(1, 0); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_1) { |
OUTPUT_SET(0, 0); |
OUTPUT_SET(1, 1); |
} else if (!staticParams.outputDebugMask) { |
flashingLights(); |
} else { |
OUTPUT_SET(0, debugOut.digital[0] & DIGITAL_DEBUG_MASK); |
OUTPUT_SET(1, debugOut.digital[1] & DIGITAL_DEBUG_MASK); |
OUTPUT_SET(0, debugOut.digital[0] & staticParams.outputDebugMask); |
OUTPUT_SET(1, debugOut.digital[1] & staticParams.outputDebugMask); |
} |
} |
/branches/dongfang_FC_rewrite/output.h |
---|
70,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_ACC0THORDER |
#define DIGITAL_DEBUG_MASK 0 |
void output_init(void); |
void output_update(void); |
/branches/dongfang_FC_rewrite/rc.c |
---|
214,8 → 214,8 |
} |
} |
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
#define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
#define RCDiff(dimension) PPM_diff[channelMap.channels[dimension]] |
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
249,19 → 249,19 |
if (RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP |
+ RCDiff(CH_PITCH) * staticParams.StickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP |
+ RCDiff(CH_ROLL) * staticParams.StickD; |
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
+ RCDiff(CH_PITCH) * staticParams.stickD; |
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
+ RCDiff(CH_ROLL) * staticParams.stickD; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
* dynamicParams.UserParams[3] + 120; |
* staticParams.stickThrottleD + 120; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) |
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
// exponential stick sensitivity in yawing rate |
tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1)) |
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1)) |
/ 512L; // expo y = ax + bx^2 |
tmp2 += (staticParams.StickYawP * tmp1) >> 2; |
tmp2 += (staticParams.stickYawP * tmp1) >> 2; |
RC_PRTY[CONTROL_YAW] = tmp2; |
} |
uint8_t command = RC_getStickCommand(); |
392,6 → 392,7 |
} |
} |
/* |
uint8_t RC_getLooping(uint8_t looping) { |
// static uint8_t looping = 0; |
429,6 → 430,7 |
return looping; |
} |
*/ |
uint8_t RC_testCompassCalState(void) { |
static uint8_t stick = 1; |
/branches/dongfang_FC_rewrite/rc.h |
---|
2,9 → 2,8 |
#define _RC_H |
#include <inttypes.h> |
#include "configuration.h" |
#define MAX_CHANNELS 10 |
// Number of cycles a command must be repeated before commit. |
#define COMMAND_TIMER 200 |
/branches/dongfang_FC_rewrite/timer0.c |
---|
86,7 → 86,7 |
// Configure speaker port as output. |
if (BoardRelease == 10) { // Speaker at PD2 |
if (boardRelease == 10) { // Speaker at PD2 |
DDRD |= (1 << DDD2); |
PORTD &= ~(1 << PORTD2); |
} else { // Speaker at PC7 |
160,7 → 160,8 |
if (beepTime) { |
beepTime--; // decrement BeepTime |
if (beepTime & beepModulation) |
beeper_On = 1; |
// beeper_On = 1; |
beeper_On = 0; // shut up in dev!!! |
else |
beeper_On = 0; |
} else { // beeper off if duration is over |
171,13 → 172,13 |
// if beeper is on |
if (beeper_On) { |
// set speaker port to high. |
if (BoardRelease == 10) |
if (boardRelease == 10) |
PORTD |= (1 << PORTD2); // Speaker at PD2 |
else |
PORTC |= (1 << PORTC7); // Speaker at PC7 |
} else { // beeper is off |
// set speaker port to low |
if (BoardRelease == 10) |
if (boardRelease == 10) |
PORTD &= ~(1 << PORTD2);// Speaker at PD2 |
else |
PORTC &= ~(1 << PORTC7);// Speaker at PC7 |
185,7 → 186,7 |
#ifndef USE_NAVICTRL |
// update compass value if this option is enabled in the settings |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_MK3MAG |
MK3MAG_Update(); // read out mk3mag pwm |
#endif |
216,7 → 217,7 |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
if (analogDataReady) { |
analogDataReady = 0; |
analog_update(); |
startAnalogConversionCycle(); |
} |
} |
/branches/dongfang_FC_rewrite/twimaster.c |
---|
198,8 → 198,7 |
// Master Transmit |
case 0: // TWI_STATE_MOTOR_TX |
// skip motor if not used in mixer |
while ((Mixer.Motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write |
< MAX_MOTORS)) |
while ((mixerMatrix.motor[motor_write][MIX_THROTTLE] <= 0) && (motor_write < MAX_MOTORS)) |
motor_write++; |
if (motor_write >= MAX_MOTORS) { // writing finished, read now |
motor_write = 0; |
312,7 → 311,7 |
} |
for (i = 0; i < MAX_MOTORS; i++) { |
if (!motor[i].Present && Mixer.Motor[i][MIX_THROTTLE] > 0) |
if (!motor[i].Present && mixerMatrix.motor[i][MIX_THROTTLE] > 0) |
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i + 1); |
motor[i].Error = 0; |
} |
/branches/dongfang_FC_rewrite/twimaster.h |
---|
2,6 → 2,7 |
#define _I2C_MASTER_H |
#include <inttypes.h> |
#include "configuration.h" |
#define TWI_STATE_MOTOR_TX 0 |
#define TWI_STATE_MOTOR_RX 3 |
21,7 → 22,6 |
uint8_t MaxPWM; // read back from BL |
}__attribute__((packed)) MotorData_t; |
#define MAX_MOTORS 12 |
extern MotorData_t motor[MAX_MOTORS]; |
extern volatile uint16_t I2CTimeout; |
/branches/dongfang_FC_rewrite/uart0.c |
---|
129,8 → 129,8 |
"GyroPitch(PID) ", |
"GyroRoll(PID) ", |
"GyroYaw ", //5 |
"AccPitch (raw) ", |
"AccRoll (raw) ", |
"OffsPitch ", |
"OffsRoll ", |
"AttitudeControl ", |
"AccPitch (angle)", |
"AccRoll (angle) ", //10 |
521,13 → 521,13 |
case 'n':// "Get Mixer Table |
while (!txd_complete) |
; // wait for previous frame to be sent |
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &Mixer, sizeof(Mixer)); |
SendOutData('N', FC_ADDRESS, 1, (uint8_t *) &mixerMatrix, sizeof(mixerMatrix)); |
break; |
case 'm':// "Set Mixer Table |
if (pRxData[0] == EEMIXER_REVISION) { |
memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer)); |
MixerTable_WriteToEEProm(); |
memcpy(&mixerMatrix, (uint8_t*) pRxData, sizeof(mixerMatrix)); |
mixerMatrix_writeToEEProm(); |
while (!txd_complete) |
; // wait for previous frame to be sent |
tempchar1 = 1; |
543,7 → 543,7 |
case 'q':// request settings |
if (pRxData[0] == 0xFF) { |
pRxData[0] = GetParamByte(PID_ACTIVE_SET); |
pRxData[0] = getParamByte(PID_ACTIVE_SET); |
} |
// limit settings range |
if (pRxData[0] < 1) |
551,7 → 551,7 |
else if (pRxData[0] > 5) |
pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
ParamSet_ReadFromEEProm(pRxData[0]); |
paramSet_readFromEEProm(pRxData[0]); |
tempchar1 = pRxData[0]; |
tempchar2 = EEPARAM_REVISION; |
while (!txd_complete) |
568,7 → 568,7 |
== 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]); |
paramSet_writeToEEProm(pRxData[0]); |
tempchar1 = getActiveParamSet(); |
beepNumber(tempchar1); |
} else { |
734,8 → 734,8 |
if((checkDelay(Compass_Timer)) && txd_complete) { |
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
ToMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
ToMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
ToMk3Mag.CalState = compassCalState; |
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
// the last state is 5 and should be send only once to avoid multiple flash writing |