/branches/dongfang_FC_fixedwing/dsl.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/invenSense.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/ADXRS610_FC2.0.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/twimaster.c |
---|
File deleted |
/branches/dongfang_FC_fixedwing/twimaster.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/ENC-03_FC1.3.c |
---|
File deleted |
/branches/dongfang_FC_fixedwing/ENC-03_FC1.3.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/main.h |
---|
File deleted |
/branches/dongfang_FC_fixedwing/dsl.c |
---|
File deleted |
/branches/dongfang_FC_fixedwing/.cproject |
---|
3,8 → 3,8 |
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> |
<storageModule moduleId="org.eclipse.cdt.core.settings"> |
<cconfiguration id="0.1994915586"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1994915586" moduleId="org.eclipse.cdt.core.settings" name="Default"> |
<cconfiguration id="0.142097617"> |
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.142097617" moduleId="org.eclipse.cdt.core.settings" name="Default"> |
<externalSettings/> |
<extensions> |
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> |
16,197 → 16,38 |
</extensions> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<configuration buildProperties="" description="" id="0.1994915586" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg"> |
<folderInfo id="0.1994915586." name="/" resourcePath=""> |
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.474514268" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain"> |
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.474514268.1219089576" name=""/> |
<builder id="org.eclipse.cdt.build.core.settings.default.builder.666345243" incrementalBuildTarget="program" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1506385658" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.940726630" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1773142576" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<configuration buildProperties="" description="" id="0.142097617" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg"> |
<folderInfo id="0.142097617." name="/" resourcePath=""> |
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.648869171" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain"> |
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.648869171.1366251437" name=""/> |
<builder id="org.eclipse.cdt.build.core.settings.default.builder.1198454805" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1748574311" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.580525788" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.209091053" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
<tool id="org.eclipse.cdt.build.core.settings.holder.968560296" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.157126537" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.297906706" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.2065654697" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
<tool id="org.eclipse.cdt.build.core.settings.holder.216474989" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.188943235" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
<tool id="org.eclipse.cdt.build.core.settings.holder.187554179" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder"> |
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1912300390" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/> |
</tool> |
</toolChain> |
</folderInfo> |
</configuration> |
</storageModule> |
<storageModule moduleId="scannerConfiguration"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="makefileGenerator"> |
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<scannerConfigBuildInfo instanceId="0.1994915586"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="makefileGenerator"> |
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
</storageModule> |
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> |
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> |
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> |
</cconfiguration> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<project id="dongfang_FC_fixedwing.null.1262997060" name="dongfang_FC_fixedwing"/> |
<project id="dongfang_FC_fixedwing.forward.null.1616337749" name="dongfang_FC_fixedwing.forward"/> |
</storageModule> |
<storageModule moduleId="scannerConfiguration"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<scannerConfigBuildInfo instanceId="0.142097617"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
</scannerConfigBuildInfo> |
<scannerConfigBuildInfo instanceId="0.1994915586"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
</scannerConfigBuildInfo> |
</storageModule> |
</cproject> |
/branches/dongfang_FC_fixedwing/.project |
---|
51,7 → 51,7 |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.fullBuildTarget</key> |
<value>program</value> |
<value>all</value> |
</dictionary> |
<dictionary> |
<key>org.eclipse.cdt.make.core.stopOnError</key> |
/branches/dongfang_FC_fixedwing/ADXRS610_FC2.0.c |
---|
1,69 → 1,19 |
#include "ADXRS610_FC2.0.h" |
#include "configuration.h" |
const uint8_t GYRO_QUADRANT = 3; |
//const uint8_t ACC_QUADRANT = 0; |
const uint8_t YAW_GYRO_REVERSED = 0; |
const uint8_t PR_GYROS_ORIENTATION_REVERSED = 0; |
const uint8_t Z_ACC_REVERSED = 0; |
void gyro_calibrate(void) { |
// Nothing to calibrate. |
} |
void gyro_setDefaults(void) { |
staticParams.DriftComp = 0; |
staticParams.GyroAccFactor = 1; |
staticParams.GyroAccTrim = 1; |
void gyro_init(void) { |
// No amplifiers, no DAC. |
} |
/* |
normal 1rev 2rev |
1 0 -1 0 1 0 |
0 1 0 1 0 -1 |
45 |
1 -1 -1 -1 1 1 |
1 1 -1 1 1 -1 |
90 |
0 -1 0 -1 0 1 |
1 0 -1 0 1 0 |
135 |
-1 -1 1 -1 -1 1 |
1 -1 -1 -1 1 1 |
180 |
-1 0 1 0 -1 0 |
0 -1 0 -1 0 1 |
225 |
-1 1 1 1 -1 -1 |
-1 -1 1 -1 -1 1 |
270 |
0 1 0 1 0 -1 |
-1 0 1 0 -1 0 |
315 |
1 1 -1 1 1 -1 |
-1 1 1 1 -1 -1 |
0,0: 1,1,0,-1,-1,-1,0,1 paddp = tab[n] |
0,1: 0,-1,-1,-1,0,1,1,1 paddr = tab[n+2] |
1,0: 0,1,1,1,0,-1,-1,-1 raddp = tab[n+6] |
1,1: 1,1,0,-1,-1,-1,0,1 raddr = tab[n] |
reverse: |
0,0: -1,-1,0,1,1,1,0,-1 paddp = tab[n+4] |
0,1: 0,-1,-1,-1,0,1,1,1 (same)= tab[n+2] |
1,0: 0,-1,-1,-1,0,1,1,1 raddp = tab[n+2] |
1,1: 1,1,0,-1,-1,-1,0,1 (same)= tab[n] |
= 1rev[n-4] = 1rev[n+4] |
*/ |
void gyro_setDefaultParameters(void) { |
IMUConfig.accQuadrant = 4; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_XY; |
staticParams.gyroD = 5; |
IMUConfig.driftCompDivider = 1; |
IMUConfig.driftCompLimit = 3; |
IMUConfig.zerothOrderCorrection = 1; |
} |
/branches/dongfang_FC_fixedwing/analog.c |
---|
7,7 → 7,6 |
#include "attitude.h" |
#include "sensors.h" |
#include "printf_P.h" |
#include "mk3mag.h" |
// for Delay functions |
#include "timer0.h" |
32,8 → 31,8 |
* the offsets with the DAC. |
*/ |
volatile uint16_t sensorInputs[8]; |
int16_t acc[3]; |
int16_t filteredAcc[3] = { 0,0,0 }; |
//int16_t acc[3]; |
//int16_t filteredAcc[3] = { 0,0,0 }; |
/* |
* These 4 exported variables are zero-offset. The "PID" ones are used |
40,17 → 39,12 |
* in the attitude control as rotation rates. The "ATT" ones are for |
* integration to angles. |
*/ |
int16_t gyro_PID[2]; |
int16_t gyro_ATT[2]; |
int16_t gyroD[2]; |
int16_t gyro_PID[3]; |
int16_t gyro_ATT[3]; |
int16_t gyroD[3]; |
int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH]; |
uint8_t gyroDWindowIdx = 0; |
int16_t yawGyro; |
int16_t magneticHeading; |
int32_t groundPressure; |
int16_t dHeight; |
uint32_t gyroActivity; |
/* |
90,14 → 84,14 |
* 7: pp=-1,pr=1, rp=1, rr=1 // +315 degrees with pitch reversed |
*/ |
void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) { |
void rotate(int16_t* result, uint8_t quadrant, uint8_t reversePR, uint8_t reverseYaw) { |
static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1}; |
// Pitch to Pitch part |
int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
int8_t xx = reversePR ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant]; |
// Roll to Pitch part |
int8_t xy = rotationTab[(quadrant+2)%8]; |
// Pitch to Roll part |
int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
int8_t yx = reversePR ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8]; |
// Roll to Roll part |
int8_t yy = rotationTab[quadrant]; |
113,27 → 107,19 |
result[0] = (result[0]*11) >> 4; |
result[1] = (result[1]*11) >> 4; |
} |
if (reverseYaw) |
result[3] =-result[3]; |
} |
/* |
* Air pressure |
* Airspeed |
*/ |
volatile uint8_t rangewidth = 105; |
// Direct from sensor, irrespective of range. |
// volatile uint16_t rawAirPressure; |
// Value of 2 samples, with range. |
uint16_t simpleAirPressure; |
// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered. |
int32_t filteredAirPressure; |
// int32_t filteredAirPressure; |
#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 32 |
//int32_t lastFilteredAirPressure; |
int16_t dAirPressureWindow[MAX_D_AIRPRESSURE_WINDOW_LENGTH]; |
uint8_t dWindowPtr = 0; |
#define MAX_AIRPRESSURE_WINDOW_LENGTH 32 |
int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH]; |
int32_t windowedAirPressure; |
145,6 → 131,7 |
// The number of samples summed into airPressureSum so far. |
uint8_t pressureMeasurementCount; |
/* |
* Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt. |
* That is divided by 3 below, for a final 10.34 per volt. |
161,7 → 148,6 |
* Experiment: Measuring vibration-induced sensor noise. |
*/ |
uint16_t gyroNoisePeak[3]; |
uint16_t accNoisePeak[3]; |
volatile uint8_t adState; |
volatile uint8_t adChannel; |
189,19 → 175,27 |
*/ |
const uint8_t channelsForStates[] PROGMEM = { |
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, |
AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc. |
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro |
AD_AIRPRESSURE, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_UBAT, |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW, |
AD_ACC_PITCH, // at 12, finish pitch axis acc. |
AD_ACC_ROLL, // at 13, finish roll axis acc. |
AD_AIRPRESSURE, // at 14, finish air pressure. |
AD_AIRPRESSURE, |
AD_GYRO_PITCH, // at 15, finish pitch gyro |
AD_GYRO_ROLL, // at 16, finish roll gyro |
AD_UBAT // at 17, measure battery. |
AD_GYRO_PITCH, |
AD_GYRO_ROLL, |
AD_GYRO_YAW |
}; |
// Feature removed. Could be reintroduced later - but should work for all gyro types then. |
244,9 → 238,11 |
return sensorInputs[AD_GYRO_PITCH-axis]; |
} |
/* |
uint16_t rawAccValue(uint8_t axis) { |
return sensorInputs[AD_ACC_PITCH-axis]; |
} |
*/ |
void measureNoise(const int16_t sensor, |
volatile uint16_t* const noiseMeasurement, const uint8_t damping) { |
266,9 → 262,7 |
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type. |
*/ |
uint16_t getSimplePressure(int advalue) { |
uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue; |
result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10; |
return result; |
return advalue; |
} |
void startAnalogConversionCycle(void) { |
305,6 → 299,7 |
} |
} |
/* |
void measureGyroActivity(int16_t newValue) { |
gyroActivity += (uint32_t)((int32_t)newValue * newValue); |
} |
318,20 → 313,14 |
gyroActivity >>= GADAMPING; |
} |
} |
/* |
void dampenGyroActivity(void) { |
if (gyroActivity >= 2000) { |
gyroActivity -= 2000; |
} |
} |
*/ |
void analog_updateGyros(void) { |
// for various filters... |
int16_t tempOffsetGyro[2], tempGyro; |
int16_t tempOffsetGyro[3], tempGyro; |
debugOut.digital[0] &= ~DEBUG_SENSORLIMIT; |
for (uint8_t axis=0; axis<2; axis++) { |
for (uint8_t axis=0; axis<3; axis++) { |
tempGyro = rawGyroValue(axis); |
/* |
* Process the gyro data for the PID controller. |
340,23 → 329,23 |
// gyro with a wider range, and helps counter saturation at full control. |
if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) { |
if (tempGyro < SENSOR_MIN_PITCHROLL) { |
if (tempGyro < SENSOR_MIN) { |
debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT; |
} else if (tempGyro > SENSOR_MAX_PITCHROLL) { |
} else if (tempGyro > SENSOR_MAX) { |
debugOut.digital[0] |= DEBUG_SENSORLIMIT; |
tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL; |
tempGyro = (tempGyro - SENSOR_MAX) * EXTRAPOLATION_SLOPE + SENSOR_MAX; |
} |
} |
// 2) Apply sign and offset, scale before filtering. |
tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]); |
} |
// 2.1: Transform axes. |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW); |
for (uint8_t axis=0; axis<2; axis++) { |
for (uint8_t axis=0; axis<3; axis++) { |
// 3) Filter. |
tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant; |
374,42 → 363,23 |
gyro_PID[axis] = tempOffsetGyro[axis]; |
// Prepare tempOffsetGyro for next calculation below... |
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL; |
tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]); |
} |
/* |
* Now process the data for attitude angles. |
*/ |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR); |
rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW); |
dampenGyroActivity(); |
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
// dampenGyroActivity(); |
gyro_ATT[PITCH] = tempOffsetGyro[PITCH]; |
gyro_ATT[ROLL] = tempOffsetGyro[ROLL]; |
/* |
measureGyroActivity(tempOffsetGyro[PITCH]); |
measureGyroActivity(tempOffsetGyro[ROLL]); |
*/ |
measureGyroActivity(gyroD[PITCH]); |
measureGyroActivity(gyroD[ROLL]); |
// We measure activity of yaw by plain gyro value and not d/dt, because: |
// - There is no drift correction anyway |
// - Effect of steady circular flight would vanish (it should have effect). |
// int16_t diff = yawGyro; |
// Yaw gyro. |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW) |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
// diff -= yawGyro; |
// gyroD[YAW] -= gyroDWindow[YAW][gyroDWindowIdx]; |
// gyroD[YAW] += diff; |
// gyroDWindow[YAW][gyroDWindowIdx] = diff; |
// gyroActivity += (uint32_t)(abs(yawGyro)* IMUConfig.yawRateFactor); |
/* |
measureGyroActivity(gyroD[PITCH]); |
measureGyroActivity(gyroD[ROLL]); |
measureGyroActivity(yawGyro); |
*/ |
if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) { |
gyroDWindowIdx = 0; |
416,124 → 386,9 |
} |
} |
void analog_updateAccelerometers(void) { |
// Pitch and roll axis accelerations. |
for (uint8_t axis=0; axis<2; axis++) { |
acc[axis] = rawAccValue(axis) - accOffset.offsets[axis]; |
} |
rotate(acc, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_XY); |
for(uint8_t axis=0; axis<3; axis++) { |
filteredAcc[axis] = (filteredAcc[axis] * (IMUConfig.accFilterConstant - 1) + acc[axis]) / IMUConfig.accFilterConstant; |
measureNoise(acc[axis], &accNoisePeak[axis], 1); |
} |
// Z acc. |
if (IMUConfig.imuReversedFlags & 8) |
acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z]; |
else |
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z]; |
// debugOut.analog[29] = acc[Z]; |
} |
void analog_updateAirPressure(void) { |
static uint16_t pressureAutorangingWait = 25; |
uint16_t rawAirPressure; |
int16_t newrange; |
// air pressure |
if (pressureAutorangingWait) { |
//A range switch was done recently. Wait for steadying. |
pressureAutorangingWait--; |
} else { |
rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
if (rawAirPressure < MIN_RAWPRESSURE) { |
// value is too low, so decrease voltage on the op amp minus input, making the value higher. |
newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1; |
if (newrange > MIN_RANGES_EXTRAPOLATION) { |
pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 + |
OCR0A = newrange; |
} else { |
if (OCR0A) { |
OCR0A--; |
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
} |
} |
} else if (rawAirPressure > MAX_RAWPRESSURE) { |
// value is too high, so increase voltage on the op amp minus input, making the value lower. |
// If near the end, make a limited increase |
newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1; |
if (newrange < MAX_RANGES_EXTRAPOLATION) { |
pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR; |
OCR0A = newrange; |
} else { |
if (OCR0A < 254) { |
OCR0A++; |
pressureAutorangingWait = AUTORANGE_WAIT_FACTOR; |
} |
} |
} |
// Even if the sample is off-range, use it. |
simpleAirPressure = getSimplePressure(rawAirPressure); |
debugOut.analog[6] = rawAirPressure; |
debugOut.analog[7] = simpleAirPressure; |
if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near lower end of range. If the measurement saturates, the |
// copter may climb uncontrolledly... Simulate a drastic reduction in pressure. |
debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth |
+ (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION |
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
} else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) { |
// Danger: pressure near upper end of range. If the measurement saturates, the |
// copter may descend uncontrolledly... Simulate a drastic increase in pressure. |
debugOut.digital[1] |= DEBUG_SENSORLIMIT; |
airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth |
+ (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION |
* rangewidth) * PRESSURE_EXTRAPOLATION_COEFF; |
} else { |
// normal case. |
// If AIRPRESSURE_OVERSAMPLING is an odd number we only want to add half the double sample. |
// The 2 cases above (end of range) are ignored for this. |
debugOut.digital[1] &= ~DEBUG_SENSORLIMIT; |
airPressureSum += simpleAirPressure; |
} |
// 2 samples were added. |
pressureMeasurementCount += 2; |
// Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...) |
if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) { |
// The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half. |
airPressureSum += simpleAirPressure >> 2; |
uint32_t lastFilteredAirPressure = filteredAirPressure; |
if (!staticParams.airpressureWindowLength) { |
filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1) |
+ airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant; |
} else { |
// use windowed. |
windowedAirPressure += simpleAirPressure; |
windowedAirPressure -= airPressureWindow[windowPtr]; |
airPressureWindow[windowPtr++] = simpleAirPressure; |
if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0; |
filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength; |
} |
// positive diff of pressure |
int16_t diff = filteredAirPressure - lastFilteredAirPressure; |
// is a negative diff of height. |
dHeight -= diff; |
// remove old sample (fifo) from window. |
dHeight += dAirPressureWindow[dWindowPtr]; |
dAirPressureWindow[dWindowPtr++] = diff; |
if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0; |
pressureMeasurementCount = airPressureSum = 0; |
} |
} |
uint16_t rawAirPressure = sensorInputs[AD_AIRPRESSURE]; |
simpleAirPressure = rawAirPressure; |
} |
void analog_updateBatteryVoltage(void) { |
544,12 → 399,13 |
void analog_update(void) { |
analog_updateGyros(); |
analog_updateAccelerometers(); |
// analog_updateAccelerometers(); |
analog_updateAirPressure(); |
analog_updateBatteryVoltage(); |
#ifdef USE_MK3MAG |
magneticHeading = volatileMagneticHeading; |
#endif |
} |
void analog_setNeutral() { |
557,20 → 413,21 |
if (gyroOffset_readFromEEProm()) { |
printf("gyro offsets invalid%s",recal); |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL; |
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW; |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING; |
gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING; |
} |
/* |
if (accOffset_readFromEEProm()) { |
printf("acc. meter offsets invalid%s",recal); |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY; |
accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z; |
} |
*/ |
// Noise is relative to offset. So, reset noise measurements when changing offsets. |
for (uint8_t i=PITCH; i<=ROLL; i++) { |
for (uint8_t i=PITCH; i<=YAW; i++) { |
gyroNoisePeak[i] = 0; |
accNoisePeak[i] = 0; |
gyroD[i] = 0; |
for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) { |
gyroDWindow[i][j] = 0; |
600,8 → 457,8 |
for (axis = PITCH; axis <= YAW; axis++) { |
gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES; |
int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL; |
int16_t min = (512-200) * GYRO_OVERSAMPLING; |
int16_t max = (512+200) * GYRO_OVERSAMPLING; |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) |
versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis; |
} |
609,68 → 466,3 |
gyroOffset_writeToEEProm(); |
startAnalogConversionCycle(); |
} |
/* |
* Find acc. offsets for a neutral reading, and write them to EEPROM. |
* Does not (!} update the local variables. This must be done with a |
* call to analog_calibrate() - this always (?) is done by the caller |
* anyway. There would be nothing wrong with updating the variables |
* directly from here, though. |
*/ |
void analog_calibrateAcc(void) { |
#define ACC_OFFSET_CYCLES 32 |
uint8_t i, axis; |
int32_t offsets[3] = { 0, 0, 0 }; |
for (i = 0; i < ACC_OFFSET_CYCLES; i++) { |
delay_ms_with_adc_measurement(10, 1); |
for (axis = PITCH; axis <= YAW; axis++) { |
offsets[axis] += rawAccValue(axis); |
} |
} |
for (axis = PITCH; axis <= YAW; axis++) { |
accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES; |
int16_t min,max; |
if (axis==Z) { |
if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (256-200) * ACC_OVERSAMPLING_Z; |
max = (256+200) * ACC_OVERSAMPLING_Z; |
} else { |
// TODO: This assumes a sensitivity of +/- 2g. |
min = (768-200) * ACC_OVERSAMPLING_Z; |
max = (768+200) * ACC_OVERSAMPLING_Z; |
} |
} else { |
min = (512-200) * ACC_OVERSAMPLING_XY; |
max = (512+200) * ACC_OVERSAMPLING_XY; |
} |
if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) { |
versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis; |
} |
} |
accOffset_writeToEEProm(); |
startAnalogConversionCycle(); |
} |
void analog_setGround() { |
groundPressure = filteredAirPressure; |
} |
int32_t analog_getHeight(void) { |
return groundPressure - filteredAirPressure; |
} |
int16_t analog_getDHeight(void) { |
/* |
int16_t result = 0; |
for (int i=0; i<staticParams.airpressureDWindowLength; i++) { |
result -= dAirPressureWindow[i]; // minus pressure is plus height. |
} |
// dHeight = -dPressure, so here it is the old pressure minus the current, not opposite. |
return result; |
*/ |
return dHeight; |
} |
/branches/dongfang_FC_fixedwing/analog.h |
---|
22,7 → 22,6 |
to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or |
roll. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here. |
*/ |
#define GYRO_FACTOR_PITCHROLL 1 |
/* |
GYRO_HW_FACTOR is the relation between rotation rate and ADCValue: |
59,17 → 58,15 |
* respectively. This is = the number of occurences of each channel in the |
* channelsForStates array in analog.c. |
*/ |
#define GYRO_OVERSAMPLING_PITCHROLL 4 |
#define GYRO_OVERSAMPLING_YAW 2 |
#define GYRO_OVERSAMPLING 4 |
#define ACC_OVERSAMPLING_XY 2 |
#define ACC_OVERSAMPLING_Z 1 |
//#define ACC_OVERSAMPLING_XY 2 |
//#define ACC_OVERSAMPLING_Z 1 |
/* |
* The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate. |
*/ |
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL) |
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW) |
#define GYRO_RATE_FACTOR (GYRO_HW_FACTOR * GYRO_OVERSAMPLING) |
/* |
* The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor. |
80,13 → 77,13 |
* Gyro saturation prevention. |
*/ |
// How far from the end of its range a gyro is considered near-saturated. |
#define SENSOR_MIN_PITCHROLL 32 |
#define SENSOR_MIN 32 |
// Other end of the range (calculated) |
#define SENSOR_MAX_PITCHROLL (GYRO_OVERSAMPLING_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL) |
#define SENSOR_MAX (GYRO_OVERSAMPLING * 1023 - SENSOR_MIN) |
// Max. boost to add "virtually" to gyro signal at total saturation. |
#define EXTRAPOLATION_LIMIT 2500 |
// Slope of the boost (calculated) |
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL) |
#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN) |
/* |
* This value is subtracted from the gyro noise measurement in each iteration, |
97,7 → 94,7 |
#define PITCH 0 |
#define ROLL 1 |
#define YAW 2 |
#define Z 2 |
//#define Z 2 |
/* |
* The values that this module outputs |
* These first 2 exported arrays are zero-offset. The "PID" ones are used |
107,11 → 104,11 |
* in filtering, and when a gyro becomes near saturated. |
* Maybe this distinction is not really necessary. |
*/ |
extern int16_t gyro_PID[2]; |
extern int16_t gyro_ATT[2]; |
extern int16_t gyro_PID[3]; |
extern int16_t gyro_ATT[3]; |
#define GYRO_D_WINDOW_LENGTH 8 |
extern int16_t gyroD[3]; |
extern int16_t yawGyro; |
extern int16_t UBat; |
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
118,7 → 115,7 |
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
extern sensorOffset_t gyroOffset; |
extern sensorOffset_t accOffset; |
//extern sensorOffset_t accOffset; |
extern sensorOffset_t gyroAmplifierOffset; |
/* |
129,8 → 126,8 |
/* |
* The acceleration values that this module outputs. They are zero based. |
*/ |
extern int16_t acc[3]; |
extern int16_t filteredAcc[3]; |
//extern int16_t acc[3]; |
//extern int16_t filteredAcc[3]; |
// extern volatile int32_t stronglyFilteredAcc[3]; |
/* |
233,7 → 230,7 |
/* |
* Zero-offset accelerometers and write the calibration data to EEPROM. |
*/ |
void analog_calibrateAcc(void); |
//void analog_calibrateAcc(void); |
void analog_setGround(void); |
/branches/dongfang_FC_fixedwing/attitude.c |
---|
1,12 → 1,10 |
/************************************************************************/ |
/* Flight Attitude */ |
/************************************************************************/ |
#include <stdlib.h> |
#include <avr/io.h> |
#include <stdlib.h> |
#include "attitude.h" |
#include "dongfangMath.h" |
#include "commands.h" |
// For scope debugging only! |
#include "rc.h" |
20,9 → 18,6 |
// Some calculations are performed depending on some stick related things. |
#include "controlMixer.h" |
// For Servo_On / Off |
// #include "timer2.h" |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
/* |
33,36 → 28,22 |
* The variables are overwritten at each attitude calculation invocation - the values |
* are not preserved or reused. |
*/ |
int16_t rate_ATT[2], yawRate; |
int16_t rate_ATT[3]; |
// With different (less) filtering |
int16_t rate_PID[2]; |
int16_t rate_PID[3]; |
int16_t differential[3]; |
/* |
* Gyro integrals. These are the rotation angles of the airframe compared to the |
* horizontal plane, yaw relative to yaw at start. Not really used for anything else |
* than diagnostics. |
* horizontal plane, yaw relative to yaw at start. |
*/ |
int32_t angle[3]; |
int32_t attitude[3]; |
/* |
* Error integrals. Stick is always positive. Gyro is configurable positive or negative. |
* These represent the deviation of the attitude angle from the desired on each axis. |
*/ |
int32_t error[3]; |
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
int16_t correctionSum[2] = { 0, 0 }; |
// For NaviCTRL use. |
int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
/* |
* Experiment: Compensating for dynamic-induced gyro biasing. |
*/ |
int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
int16_t driftComp[3] = { 0, 0, 0 }; |
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
// int16_t dynamicCalCount; |
76,18 → 57,14 |
* constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
* it is hardly worth the trouble. |
************************************************************************/ |
/* |
int32_t getAngleEstimateFromAcc(uint8_t axis) { |
return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
} |
*/ |
void setStaticAttitudeAngles(void) { |
#ifdef ATTITUDE_USE_ACC_SENSORS |
angle[PITCH] = getAngleEstimateFromAcc(PITCH); |
angle[ROLL] = getAngleEstimateFromAcc(ROLL); |
#else |
angle[PITCH] = angle[ROLL] = 0; |
#endif |
attitude[PITCH] = attitude[ROLL] = 0; |
} |
/************************************************************************ |
94,20 → 71,11 |
* Neutral Readings |
************************************************************************/ |
void attitude_setNeutral(void) { |
// Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
driftComp[PITCH] = driftComp[ROLL]; |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
// Calibrate hardware. |
analog_setNeutral(); |
// Calibrate hardware. |
analog_calibrate(); |
// reset gyro integrals to acc guessing |
setStaticAttitudeAngles(); |
// Inititialize YawGyroIntegral value with current compass heading |
angle[YAW] = 0; |
// Servo_On(); //enable servo output |
} |
/************************************************************************ |
119,22 → 87,13 |
void getAnalogData(void) { |
uint8_t axis; |
for (axis = PITCH; axis <= ROLL; axis++) { |
rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
analog_update(); |
for (axis = PITCH; axis <= YAW; axis++) { |
rate_PID[axis] = gyro_PID[axis]; |
rate_ATT[axis] = gyro_ATT[axis]; |
differential[axis] = gyroD[axis]; |
averageAcc[axis] += acc[axis]; |
} |
differential[YAW] = gyroD[YAW]; |
averageAccCount++; |
yawRate = yawGyro + driftCompYaw; |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
analogDataReady = 0; |
analog_start(); |
} |
void integrate(void) { |
141,160 → 100,25 |
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
uint8_t axis; |
// TODO: Multiply on a factor on control. Wont work without... |
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) { |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & 1 ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.servoDirections & 2 ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.servoDirections & 4 ? yawRate : -yawRate); |
angle[PITCH] += rate_ATT[PITCH]; |
angle[ROLL] += rate_ATT[ROLL]; |
angle[YAW] += yawRate; |
} else { |
error[PITCH] += control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? rate_ATT[PITCH] : -rate_ATT[PITCH]); |
error[ROLL] += control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? rate_ATT[ROLL] : -rate_ATT[ROLL]); |
error[YAW] += control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? yawRate : -yawRate); |
angle[PITCH] += rate_ATT[PITCH]; |
angle[ROLL] += rate_ATT[ROLL]; |
angle[YAW] += yawRate; |
} |
// TODO: Configurable. |
#define ERRORLIMIT 1000 |
for (axis=PITCH; axis<=YAW; axis++) { |
if (error[axis] > ERRORLIMIT) { |
error[axis] = ERRORLIMIT; |
} else if (angle[axis] <= -ERRORLIMIT) { |
angle[axis] = -ERRORLIMIT; |
} |
} |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit heading proportional to 0 deg to 360 deg |
*/ |
/* |
* Pitch axis integration and range boundary wrap. |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
angle[axis] += rate_ATT[axis]; |
if (angle[axis] > PITCHROLLOVER180) { |
angle[axis] -= PITCHROLLOVER360; |
} else if (angle[axis] <= -PITCHROLLOVER180) { |
angle[axis] += PITCHROLLOVER360; |
} |
} |
/* |
* Yaw |
* Calculate yaw gyro integral (~ to rotation angle) |
* Limit yawGyroHeading proportional to 0 deg to 360 deg |
*/ |
if (angle[YAW] >= YAWOVER360) { |
angle[YAW] -= YAWOVER360; // 360 deg. wrap |
} else if (angle[YAW] < 0) { |
angle[YAW] += YAWOVER360; |
for (axis = PITCH; axis <= YAW; axis++) { |
attitude[axis] += rate_ATT[axis]; |
if (attitude[axis] > OVER180) { |
attitude[axis] -= OVER360; |
} else if (attitude[axis] <= -OVER180) { |
attitude[axis] += OVER360; |
} |
} |
/************************************************************************ |
* A kind of 0'th order integral correction, that corrects the integrals |
* directly. This is the "gyroAccFactor" stuff in the original code. |
* There is (there) also a drift compensation |
* - it corrects the differential of the integral = the gyro offsets. |
* That should only be necessary with drifty gyros like ENC-03. |
************************************************************************/ |
void correctIntegralsByAcc0thOrder(void) { |
// TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
// are less than ....., or reintroduce Kalman. |
// Well actually the Z axis acc. check is not so silly. |
uint8_t axis; |
int32_t temp; |
if (acc[Z] >= -staticParams.zerothOrderGyroCorrectionZAccLimit && acc[Z] |
<= dynamicParams.UserParams[7]) { |
DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
uint8_t permilleAcc = staticParams.zerothOrderGyroCorrectionFactorx1000; |
uint8_t debugFullWeight = 1; |
int32_t accDerived; |
if ((control[YAW] < -64) || (control[YAW] > 64)) { // reduce further if yaw stick is active |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
permilleAcc /= 2; |
debugFullWeight = 0; |
} |
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. |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
accDerived = getAngleEstimateFromAcc(axis); |
// DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
// 1000 * the correction amount that will be added to the gyro angle in next line. |
temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
+ (int32_t) permilleAcc * accDerived) / 1000L; |
correctionSum[axis] += angle[axis] - temp; |
} |
} else { |
DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
// DebugOut.Analog[9] = 0; |
// DebugOut.Analog[10] = 0; |
DebugOut.Analog[16] = 0; |
DebugOut.Analog[17] = 0; |
// experiment: Kill drift compensation updates when not flying smooth. |
correctionSum[PITCH] = correctionSum[ROLL] = 0; |
} |
} |
/************************************************************************ |
* This is an attempt to correct not the error in the angle integrals |
* (that happens in correctIntegralsByAcc0thOrder above) but rather the |
* cause of it: Gyro drift, vibration and rounding errors. |
* All the corrections made in correctIntegralsByAcc0thOrder over |
* DRIFTCORRECTION_TIME cycles are summed up. This number is |
* then divided by DRIFTCORRECTION_TIME to get the approx. |
* correction that should have been applied to each iteration to fix |
* the error. This is then added to the dynamic offsets. |
************************************************************************/ |
// 2 times / sec. = 488/2 |
#define DRIFTCORRECTION_TIME 256L |
void driftCorrection(void) { |
static int16_t timer = DRIFTCORRECTION_TIME; |
int16_t deltaCorrection; |
int16_t round; |
uint8_t axis; |
if (!--timer) { |
timer = DRIFTCORRECTION_TIME; |
for (axis = PITCH; axis <= ROLL; axis++) { |
// Take the sum of corrections applied, add it to delta |
if (correctionSum[axis] >=0) |
round = DRIFTCORRECTION_TIME / 2; |
else |
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.secondOrderGyroCorrectionDivisor; |
CHECK_MIN_MAX(driftComp[axis], -staticParams.secondOrderGyroCorrectionLimit, staticParams.secondOrderGyroCorrectionLimit); |
// DebugOut.Analog[11 + axis] = correctionSum[axis]; |
DebugOut.Analog[16 + axis] = correctionSum[axis]; |
DebugOut.Analog[28 + axis] = driftComp[axis]; |
correctionSum[axis] = 0; |
} |
} |
} |
/************************************************************************ |
* Main procedure. |
************************************************************************/ |
302,12 → 126,7 |
getAnalogData(); |
integrate(); |
DebugOut.Analog[3] = rate_PID[PITCH]; |
DebugOut.Analog[4] = rate_PID[ROLL]; |
DebugOut.Analog[5] = yawRate; |
#ifdef ATTITUDE_USE_ACC_SENSORS |
correctIntegralsByAcc0thOrder(); |
driftCorrection(); |
#endif |
// We are done reading variables from the analog module. |
// Interrupt-driven sensor reading may restart. |
startAnalogConversionCycle(); |
} |
/branches/dongfang_FC_fixedwing/attitude.h |
---|
16,7 → 16,7 |
* If you have no acc. sensor or do not want to use it, remove this define. This will cause the |
* acc. sensor to be ignored at attitude calibration. |
*/ |
#define ATTITUDE_USE_ACC_SENSORS yes |
//#define ATTITUDE_USE_ACC_SENSORS yes |
/* |
* The frequency at which numerical integration takes place. 488 in original code. |
24,16 → 24,6 |
#define INTEGRATION_FREQUENCY 488 |
/* |
* Gyro readings are divided by this before being used in attitude control. This will scale them |
* to match the scale of the stick control etc. variables. This is just a rough non-precision |
* scaling - the below definitions make precise conversion factors. |
* Will be about 4 for InvenSense, 8 for FC1.3 and 8 for ADXRS610. |
* The number 1250 is derived from the original code: It has about 225000 = 1250 * 180 for 180 degrees. |
*/ |
#define HIRES_GYRO_INTEGRATION_FACTOR 1 |
// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250) |
/* |
* Constant for deriving an attitude angle from acceleration measurement. |
* |
* The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef. |
62,8 → 52,7 |
* HIRES_GYRO_INTEGRATION_FACTOR (why???) before integration. |
* The value of this expression should be about 1250 (by setting HIRES_GYRO_INTEGRATION_FACTOR to something suitable). |
*/ |
#define GYRO_DEG_FACTOR_PITCHROLL (GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR) |
#define GYRO_DEG_FACTOR_YAW (GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION) |
#define GYRO_DEG_FACTOR (GYRO_RATE_FACTOR * INTEGRATION_FREQUENCY * GYRO_CORRECTION) |
/* |
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value |
71,25 → 60,37 |
* value for the same (small) angle. |
* The value is about 200. |
*/ |
#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
//#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR)) |
#define PITCHROLLOVER180 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 180) |
#define PITCHROLLOVER360 ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 360) |
#define YAWOVER360 ((int32_t)GYRO_DEG_FACTOR_YAW * 360) |
#define OVER180 ((int32_t)GYRO_DEG_FACTOR * 180) |
#define OVER360 ((int32_t)GYRO_DEG_FACTOR * 360) |
/* |
* Rotation rates |
*/ |
extern int16_t rate_PID[2], rate_ATT[2], yawRate; |
extern int16_t rate_PID[3], rate_ATT[3]; |
extern int16_t differential[3]; |
/* |
* Attitudes calculated by numerical integration of gyro rates |
*/ |
extern int32_t angle[3]; |
extern int32_t error[3]; |
extern int32_t attitude[3]; |
// extern volatile int32_t ReadingIntegralTop; // calculated in analog.c |
/* |
* Compass navigation |
*/ |
// extern int16_t compassHeading; |
// extern int16_t compassCourse; |
// extern int16_t compassOffCourse; |
// extern uint8_t compassCalState; |
// extern int32_t yawGyroHeading; |
// extern int16_t yawGyroHeadingInDeg; |
// extern uint8_t updateCompassCourse; |
// extern uint16_t ignoreCompassTimer; |
/* |
* Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements, |
* to help canceling out drift and vibration noise effects. The dynamic offsets themselves |
* can be updated in flight by different ways, for example: |
99,12 → 100,12 |
* - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that |
* - Invent your own... |
*/ |
extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
// extern int16_t dynamicOffset[2], dynamicOffsetYaw; |
/* |
* For NaviCtrl use. |
*/ |
extern int16_t averageAcc[2], averageAccCount; |
// extern int16_t averageAcc[2], averageAccCount; |
/* |
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor). |
/branches/dongfang_FC_fixedwing/commands.c |
---|
5,56 → 5,42 |
#include "eeprom.h" |
#include "attitude.h" |
#include "output.h" |
#include "rc.h" |
#ifdef USE_MK3MAG |
// TODO: Kick that all outa here! |
uint8_t compassCalState = 0; |
#endif |
void commands_handleCommands(void) { |
/* |
* Get the current command (start/stop motors, calibrate), if any. |
*/ |
uint8_t command = controlMixer_getCommand(); |
uint8_t repeated = controlMixer_isCommandRepeated(); |
/* |
* Get the current command (start/stop motors, calibrate), if any. |
*/ |
uint8_t command = controlMixer_getCommand(); |
uint8_t repeated = controlMixer_isCommandRepeated(); |
uint8_t argument = controlMixer_getArgument(); |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// Run gyro calibration but do not repeat it. |
GRN_OFF; |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) { |
if (command == COMMAND_GYROCAL && !repeated) { |
// Gyro calinbration, with or without selecting a new parameter-set. |
paramSet_readFromEEProm(1); |
analog_calibrateGyros(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
} |
// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough? |
// isFlying = 0; |
// check roll/pitch stick position |
// if pitch stick is top or roll stick is left or right --> change parameter setting |
// according to roll/pitch stick position |
ParamSet_ReadFromEEProm(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
} |
// save the ACC neutral setting to eeprom |
else if (command == COMMAND_ACCCAL && !repeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
GRN_OFF; |
analog_calibrateAcc(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(1); |
} |
} // end !MOTOR_RUN condition. |
if (command == COMMAND_START) { |
isFlying = 1; // TODO: Really???? |
// if (!controlMixer_isCommandRepeated()) { |
// attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors. |
MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all??? |
// } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors. |
// attitude_continueDynamicCalibration(); |
// setPointYaw = 0; |
// IPartPitch = 0; |
// IPartRoll = 0; |
// } |
} else if (command == COMMAND_STOP) { |
isFlying = 0; |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
// save the ACC neutral setting to eeprom |
/* |
else { |
if (command == COMMAND_ACCCAL && !repeated) { |
// Run gyro and acc. meter calibration but do not repeat it. |
analog_calibrateAcc(); |
attitude_setNeutral(); |
controlMixer_setNeutral(); |
beepNumber(getActiveParamSet()); |
} |
} |
*/ |
} // end !MOTOR_RUN condition. |
} |
/branches/dongfang_FC_fixedwing/commands.h |
---|
2,16 → 2,13 |
#define _COMMANDS_H |
#include <inttypes.h> |
void commands_handleCommands(void); |
/* |
* An enumeration over the start motors, stop motors, calibrate gyros |
* and calibreate acc. meters commands. |
*/ |
#define COMMAND_NONE 0 |
#define COMMAND_START 6 |
#define COMMAND_STOP 8 |
#define COMMAND_GYROCAL 2 |
#define COMMAND_ACCCAL 4 |
#define COMMAND_GYROCAL 1 |
void commands_handleCommands(void); |
#endif |
/branches/dongfang_FC_fixedwing/configuration.c |
---|
1,52 → 1,66 |
#include <util/delay.h> |
#include <avr/eeprom.h> |
#include <stddef.h> |
#include <string.h> |
#include "configuration.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "sensors.h" |
#include "rc.h" |
#include "output.h" |
#include "flight.h" |
int16_t variables[8] = {0,0,0,0,0,0,0,0}; |
int16_t variables[VARIABLE_COUNT]; |
ParamSet_t staticParams; |
channelMap_t channelMap; |
IMUConfig_t IMUConfig; |
volatile DynamicParams_t dynamicParams; |
dynamicParam_t dynamicParams; |
uint8_t CPUType = ATMEGA644; |
uint8_t BoardRelease = 13; |
uint8_t CPUType; |
uint8_t boardRelease; |
uint8_t requiredMotors; |
/************************************************************************ |
* Map the parameter to pot values |
* Replacing this code by the code below saved almost 1 kbyte. |
************************************************************************/ |
VersionInfo_t versionInfo; |
void configuration_staticToDynamic(void) { |
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.CompassYawEffect,staticParams.CompassYawEffect); |
// MK flags. TODO: Replace by enum. State machine. |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 0; |
SET_POT(dynamicParams.GyroPitchP,staticParams.GyroPitchP); |
SET_POT(dynamicParams.GyroRollP,staticParams.GyroRollP); |
SET_POT(dynamicParams.GyroYawP,staticParams.GyroYawP); |
const MMXLATION XLATIONS[] = { |
{offsetof(ParamSet_t, externalControl), offsetof(DynamicParams_t, externalControl),0,255}, |
{offsetof(ParamSet_t, servoConfigurations[0].manualControl), offsetof(DynamicParams_t, servoManualControl[0]),0,255}, |
{offsetof(ParamSet_t, servoConfigurations[1].manualControl), offsetof(DynamicParams_t, servoManualControl[1]),0,255}, |
{offsetof(ParamSet_t, outputFlash[0].timing), offsetof(DynamicParams_t, output0Timing),0,255}, |
{offsetof(ParamSet_t, outputFlash[1].timing), offsetof(DynamicParams_t, output1Timing),0,255}, |
}; |
SET_POT(dynamicParams.GyroPitchD,staticParams.GyroPitchD); |
SET_POT(dynamicParams.GyroRollD,staticParams.GyroRollD); |
SET_POT(dynamicParams.GyroYawD,staticParams.GyroYawD); |
uint8_t configuration_applyVariableToParam(uint8_t src, uint8_t min, uint8_t max) { |
uint8_t result; |
if (src>=(256-VARIABLE_COUNT)) result = variables[src-(256-VARIABLE_COUNT)]; |
else result = src; |
if (result < min) result = min; |
else if (result > max) result = max; |
return result; |
} |
for (i=0; i<sizeof(staticParams.UserParams); i++) { |
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams[i]); |
void configuration_applyVariablesToParams(void) { |
uint8_t i, src; |
uint8_t* pointerToTgt; |
for(i=0; i<sizeof(XLATIONS)/sizeof(MMXLATION); i++) { |
src = *((uint8_t*)(&staticParams) + XLATIONS[i].sourceIdx); |
pointerToTgt = (uint8_t*)(&dynamicParams) + XLATIONS[i].targetIdx; |
*pointerToTgt = configuration_applyVariableToParam(src, XLATIONS[i].min, XLATIONS[i].max); |
} |
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255); |
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255); |
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl); |
// User parameters are always variable. |
for (i=0; i<sizeof(staticParams.userParams); i++) { |
src = *((uint8_t*)(&staticParams) + offsetof(ParamSet_t, userParams) + i); |
pointerToTgt = (uint8_t*)(&dynamicParams) + offsetof(DynamicParams_t, userParams) + i; |
*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; |
//if( (UCSR1A == 0x20) && (UCSR1C == 0x06) ) CPUType = ATMEGA644P; // initial Values for 644P after reset |
return CPUType; |
void setCPUType(void) { // works only after reset or power on when the registers have default values |
if((UCSR1A == 0x20) && (UCSR1C == 0x06)) CPUType = ATMEGA644P; // initial Values for 644P after reset |
else CPUType = ATMEGA644; |
} |
/* |
59,9 → 73,7 |
* tedious to have to modify the code for how to turn on and off LEDs when deploying |
* on different HW version.... |
*/ |
uint8_t getBoardRelease(void) { |
uint8_t BoardRelease = 13; |
void setBoardRelease(void) { |
// the board release is coded via the pull up or down the 2 status LED |
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate |
71,16 → 83,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; |
87,7 → 99,88 |
} |
// set LED ports as output |
DDRB |= (1<<DDB1)|(1<<DDB0); |
RED_ON; |
RED_OFF; |
GRN_OFF; |
return BoardRelease; |
} |
void configuration_setFlightParameters() { |
// Implement: Update of stuff in flight.c |
} |
// Called after a change in configuration parameters, as a hook for modules to take over changes. |
void configuration_paramSetDidChange(void) { |
// This should be OK to do here as long as we don't change parameters during emergency flight. We don't. |
configuration_setFlightParameters(); |
// Immediately load changes to output, and also signal the paramset change. |
output_init(); |
} |
void setOtherDefaults(void) { |
// Control |
staticParams.externalControl = 0; |
staticParams.IFactor = 52; |
// Servos |
staticParams.servoCount = 7; |
staticParams.servoManualMaxSpeed = 10; |
for (uint8_t i=0; i<2; i++) { |
staticParams.servoConfigurations[i].manualControl = 128; |
staticParams.servoConfigurations[i].stabilizationFactor = 0; |
staticParams.servoConfigurations[i].minValue = 32; |
staticParams.servoConfigurations[i].maxValue = 224; |
staticParams.servoConfigurations[i].flags = 0; |
} |
// Battery warning and emergency flight |
staticParams.batteryVoltageWarning = 101; // 3.7 each for 3S |
staticParams.emergencyThrottle = 35; |
staticParams.emergencyFlightDuration = 30; |
// Outputs |
staticParams.outputFlash[0].bitmask = 1; //0b01011111; |
staticParams.outputFlash[0].timing = 15; |
staticParams.outputFlash[1].bitmask = 3; //0b11110011; |
staticParams.outputFlash[1].timing = 15; |
staticParams.outputDebugMask = 8; |
staticParams.outputFlags = OUTPUTFLAGS_FLASH_0_AT_BEEP | OUTPUTFLAGS_FLASH_1_AT_BEEP | OUTPUTFLAGS_USE_ONBOARD_LEDS; |
} |
/***************************************************/ |
/* Default Values for parameter set 1 */ |
/***************************************************/ |
void paramSet_default(uint8_t setnumber) { |
setOtherDefaults(); |
for (uint8_t i=0; i<8; i++) { |
staticParams.userParams[i] = i; |
} |
staticParams.bitConfig = |
CFG_GYRO_SATURATION_PREVENTION | CFG_COMPASS_ENABLED; |
memcpy(staticParams.name, "Default\0", 6); |
} |
void IMUConfig_default(void) { |
IMUConfig.gyroPIDFilterConstant = 1; |
IMUConfig.gyroDFilterConstant = 1; |
IMUConfig.accFilterConstant = 10; |
IMUConfig.rateTolerance = 120; |
gyro_setDefaultParameters(); |
} |
/***************************************************/ |
/* Default Values for R/C Channels */ |
/***************************************************/ |
void channelMap_default(void) { |
channelMap.channels[CH_ELEVATOR] = 1; |
channelMap.channels[CH_AILERONS] = 0; |
channelMap.channels[CH_THROTTLE] = 2; |
channelMap.channels[CH_RUDDER] = 3; |
channelMap.channels[CH_POTS + 0] = 4; |
channelMap.channels[CH_POTS + 1] = 5; |
channelMap.channels[CH_POTS + 2] = 6; |
channelMap.channels[CH_POTS + 3] = 7; |
} |
/branches/dongfang_FC_fixedwing/configuration.h |
---|
4,153 → 4,246 |
#include <inttypes.h> |
#include <avr/io.h> |
#define MAX_CHANNELS 10 |
#define MAX_MOTORS 12 |
// bitmask for VersionInfo_t.HardwareError[0] |
#define FC_ERROR0_GYRO_PITCH 0x01 |
#define FC_ERROR0_GYRO_ROLL 0x02 |
#define FC_ERROR0_GYRO_YAW 0x04 |
#define FC_ERROR0_ACC_X 0x08 |
#define FC_ERROR0_ACC_Y 0x10 |
#define FC_ERROR0_ACC_Z 0x20 |
#define FC_ERROR0_PRESSURE 0x40 |
#define FC_ERROR1_RES0 0x80 |
// bitmask for VersionInfo_t.HardwareError[1] |
#define FC_ERROR1_I2C 0x01 |
#define FC_ERROR1_BL_MISSING 0x02 |
#define FC_ERROR1_SPI_RX 0x04 |
#define FC_ERROR1_PPM 0x08 |
#define FC_ERROR1_MIXER 0x10 |
#define FC_ERROR1_RES1 0x20 |
#define FC_ERROR1_RES2 0x40 |
#define FC_ERROR1_RES3 0x80 |
typedef struct { |
/*PMM*/uint8_t HeightD; |
/* P */uint8_t MaxHeight; |
/*PMM*/uint8_t HeightP; |
/* P */uint8_t Height_ACC_Effect; |
/* P */uint8_t CompassYawEffect; |
uint8_t SWMajor; |
uint8_t SWMinor; |
uint8_t protoMajor; |
uint8_t protoMinor; |
uint8_t SWPatch; |
uint8_t hardwareErrors[5]; |
}__attribute__((packed)) VersionInfo_t; |
/* P */uint8_t GyroPitchP; |
/* P */uint8_t GyroRollP; |
/* P */uint8_t GyroYawP; |
extern VersionInfo_t versionInfo; |
/* P */uint8_t UserParams[8]; |
/* P */uint8_t ServoPitchControl; |
typedef struct { |
/*PMM*/uint8_t gyroPitchD; |
/* P */uint8_t gyroRollD; |
/* P */uint8_t gyroYawD; |
/* P */uint8_t GyroPitchD; // LoopGasLimit in tool |
/* P */uint8_t GyroRollD; // AxisCoupling1 in tool |
/* P */uint8_t GyroYawD; // AxisCoupling2 in tool |
// Control |
/* P */uint8_t externalControl; |
/* P */uint8_t ExternalControl; |
/*PMM*/uint8_t J16Timing; |
/*PMM*/uint8_t J17Timing; |
} dynamicParam_t; |
extern dynamicParam_t dynamicParams; |
// Height control |
/*PMM*/uint8_t heightP; |
/* P */uint8_t heightI; |
/*PMM*/uint8_t heightD; |
/* P */uint8_t heightSetting; |
// Output and servo |
/*PMM*/uint8_t output0Timing; |
/*PMM*/uint8_t output1Timing; |
uint8_t servoManualControl[2]; |
/* P */uint8_t userParams[8]; |
} DynamicParams_t; |
extern volatile DynamicParams_t dynamicParams; |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
uint8_t min, max; |
} MMXLATION; |
/* |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
} XLATION; |
*/ |
// 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 channels[MAX_CHANNELS]; |
} channelMap_t; |
extern channelMap_t channelMap; |
uint8_t StickElevatorP; |
uint8_t StickAileronsP; |
uint8_t StickRudderP; |
typedef struct { |
char name[12]; |
int8_t motor[MAX_MOTORS][4]; |
}__attribute__((packed)) mixerMatrix_t; |
extern mixerMatrix_t mixerMatrix; |
uint8_t PIDGyroFilter;// Value: 1-8 |
typedef struct { |
int16_t offsets[3]; |
} sensorOffset_t; |
typedef struct { |
uint8_t manualControl; |
uint8_t stabilizationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t; |
#define SERVO_STABILIZATION_REVERSE 1 |
typedef struct { |
uint8_t bitmask; |
uint8_t timing; |
} output_flash_t; |
typedef struct { |
uint8_t gyroQuadrant; |
uint8_t accQuadrant; |
uint8_t imuReversedFlags; |
uint8_t DGyroFilter; // Value: 1-8 |
uint8_t attitudeGyroFilter; // Value: 1-8 |
uint8_t gyroPIDFilterConstant; |
uint8_t gyroDWindowLength; |
uint8_t gyroDFilterConstant; |
uint8_t accFilterConstant; |
uint8_t accFilter; |
uint8_t zerothOrderCorrection; |
uint8_t rateTolerance; |
uint8_t GyroPitchP; |
uint8_t GyroRollP; |
uint8_t GyroYawP; |
uint8_t gyroActivityDamping; |
uint8_t driftCompDivider; // 1/k (Koppel_ACC_Wirkung) |
uint8_t driftCompLimit; // limit for gyrodrift compensation |
} IMUConfig_t; |
uint8_t UserParams[8]; // Value : 0-250 |
extern IMUConfig_t IMUConfig; |
uint8_t LowVoltageWarning; // Value : 0-250 |
// values above 250 representing poti1 to poti4 |
typedef struct { |
// Global bitflags |
uint8_t bitConfig; // see upper defines for bitcoding |
uint8_t servoDirections; |
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output |
// 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 GyroPitchD; |
uint8_t GyroRollD; |
uint8_t GyroYawD; |
uint8_t levelCorrection[2]; |
uint8_t zerothOrderGyroCorrectionZAccLimit; |
uint8_t zerothOrderGyroCorrectionFactorx1000; |
// Control |
uint8_t gyroPitchP; |
uint8_t gyroPitchI; |
uint8_t gyroPitchD; |
uint8_t secondOrderGyroCorrectionDivisor; |
uint8_t secondOrderGyroCorrectionLimit; |
uint8_t CompassYawEffect; // Value : 0-32 |
uint8_t gyroRollP; |
uint8_t gyroRollI; |
uint8_t gyroRollD; |
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 gyroYawP; |
uint8_t gyroYawI; |
uint8_t gyroYawD; |
uint8_t ExternalControl; // for serial Control |
} paramset_t; |
uint8_t stickIElevator; |
uint8_t stickIAilerons; |
uint8_t stickIRudder; |
#define PARAMSET_STRUCT_LEN sizeof(paramset_t) |
uint8_t externalControl; // for serial Control |
extern paramset_t staticParams; |
uint8_t IFactor; |
typedef struct { |
uint8_t Revision; |
int8_t Name[12]; |
int8_t Motor[16][4]; |
}__attribute__((packed)) MixerTable_t; |
uint8_t batteryVoltageWarning; |
uint8_t emergencyThrottle; |
uint8_t emergencyFlightDuration; |
extern MixerTable_t Mixer; |
// Servos |
uint8_t servoCount; |
uint8_t servoManualMaxSpeed; |
servo_t servoConfigurations[2]; // [PITCH, ROLL] |
// Outputs |
output_flash_t outputFlash[2]; |
uint8_t outputDebugMask; |
uint8_t outputFlags; |
// User params |
uint8_t userParams[8]; |
// Name |
char 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_RESERVE1 (1<<5) |
#define MKFLAG_RESERVE2 (1<<6) |
#define MKFLAG_RESERVE3 (1<<7) |
#define MKFLAG_MOTOR_RUN (1<<0) |
//#define MKFLAG_FLY (1<<1) |
#define MKFLAG_CALIBRATE (1<<2) |
#define MKFLAG_START (1<<3) |
#define MKFLAG_EMERGENCY_FLIGHT (1<<4) |
#define MKFLAG_LOWBAT (1<<5) |
#define MKFLAG_RESERVE2 (1<<6) |
#define MKFLAG_RESERVE3 (1<<7) |
// bit mask for staticParams.GlobalConfig |
#define CFG_HEIGHT_CONTROL (1<<0) |
#define CFG_HEIGHT_SWITCH (1<<1) |
#define CFG_HEADING_HOLD (1<<2) |
#define CFG_COMPASS_ACTIVE (1<<3) |
#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) |
// bit mask for staticParams.bitConfig |
#define CFG_SIMPLE_HEIGHT_CONTROL (1<<0) |
#define CFG_SIMPLE_HC_HOLD_SWITCH (1<<1) |
#define CFG_HEADING_HOLD (1<<2) |
#define CFG_COMPASS_ENABLED (1<<3) |
#define CFG_UNUSED (1<<4) |
#define CFG_NAVI_ENABLED (1<<5) |
#define CFG_AXIS_COUPLING_ENABLED (1<<6) |
#define CFG_GYRO_SATURATION_PREVENTION (1<<7) |
// bit mask for staticParams.ServoDirections |
#define SERVO_DIRECTION_ELEVATOR (1<<0) |
#define SERVO_DIRECTION_AILERONS (1<<1) |
#define SERVO_DIRECTION_RUDDER (1<<2) |
#define IMU_REVERSE_GYRO_PR (1<<0) |
#define IMU_REVERSE_GYRO_YAW (1<<1) |
#define IMU_REVERSE_ACC_XY (1<<2) |
#define IMU_REVERSE_ACC_Z (1<<3) |
#define ATMEGA644 0 |
#define ATMEGA644P 1 |
#define ATMEGA644 0 |
#define ATMEGA644P 1 |
#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);} |
// Not really a part of configuration, but LEDs and HW s 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_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 |
#define MIX_THROTTLE 0 |
#define MIX_PITCH 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
#define MIX_THROTTLE 0 |
#define MIX_PITCH 1 |
#define MIX_ROLL 2 |
#define MIX_YAW 3 |
#define VARIABLE_COUNT 8 |
extern volatile uint8_t MKFlags; |
extern int16_t variables[8]; // The "Poti"s. |
extern uint8_t BoardRelease; |
extern uint8_t requiredMotors; |
extern int16_t variables[VARIABLE_COUNT]; // The "Poti"s. |
extern uint8_t boardRelease; |
extern uint8_t CPUType; |
void configuration_staticToDynamic(void); |
uint8_t getCPUType(void); |
uint8_t getBoardRelease(void); |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
void IMUConfig_default(void); |
void channelMap_default(void); |
void paramSet_default(uint8_t setnumber); |
void mixerMatrix_default(void); |
void configuration_setNormalFlightParameters(void); |
void configuration_setFailsafeFlightParameters(void); |
void configuration_applyVariablesToParams(void); |
void setCPUType(void); |
void setBoardRelease(void); |
// Called after a change in configuration parameters, as a hook for modules to take over changes. |
void configuration_paramSetDidChange(void); |
#endif // _CONFIGURATION_H |
/branches/dongfang_FC_fixedwing/controlMixer.c |
---|
1,192 → 1,163 |
#include <stdlib.h> |
#include "controlMixer.h" |
#include "rc.h" |
#include "attitude.h" |
#include "externalControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "commands.h" |
#include "output.h" |
uint16_t maxControl[2] = {0, 0}; |
uint16_t controlActivity = 0; |
int16_t control[4] = {0, 0, 0, 0}; |
// int32_t controlIntegrals[4] = {0, 0, 0, 0}; |
// Internal variables for reading commands made with an R/C stick. |
uint8_t lastCommand = COMMAND_NONE; |
uint8_t lastArgument; |
uint8_t isCommandRepeated = 0; |
// MK flags. TODO: Replace by enum. State machine. |
uint16_t isFlying = 0; |
volatile uint8_t MKFlags = 0; |
/* |
* This could be expanded to take arguments from ohter sources than the RC |
* (read: Custom MK RC project) |
*/ |
uint8_t controlMixer_getArgument(void) { |
return lastArgument; |
} |
/* |
* This could be expanded to take calibrate / start / stop commands from ohter sources |
* than the R/C (read: Custom MK R/C project) |
*/ |
uint8_t controlMixer_getCommand(void) { |
return lastCommand; |
} |
uint8_t controlMixer_isCommandRepeated(void) { |
return isCommandRepeated; |
} |
void controlMixer_setNeutral() { |
//EC_setNeutral(); |
//HC_setGround(); |
} |
/* |
* Set the potientiometer values to the momentary values of the respective R/C channels. |
* No slew rate limitation. |
*/ |
void controlMixer_initVariables(void) { |
uint8_t i; |
for (i = 0; i < 8; i++) { |
variables[i] = RC_getVariable(i); |
} |
} |
/* |
* Update potentiometer values with limited slew rate. Could be made faster if desired. |
* TODO: It assumes R/C as source. Not necessarily true. |
*/ |
void controlMixer_updateVariables(void) { |
uint8_t i; |
int16_t targetvalue; |
for (i = 0; i < 8; i++) { |
targetvalue = RC_getVariable(i); |
if (targetvalue < 0) |
targetvalue = 0; |
if (variables[i] < targetvalue && variables[i] < 255) |
variables[i]++; |
else if (variables[i] > 0 && variables[i] > targetvalue) |
variables[i]--; |
} |
} |
uint8_t controlMixer_getSignalQuality(void) { |
uint8_t rcQ = RC_getSignalQuality(); |
uint8_t ecQ = EC_getSignalQuality(); |
// This needs not be the only correct solution... |
return rcQ > ecQ ? rcQ : ecQ; |
} |
void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
int16_t tmp = control[index]; |
// TODO: Scale by some factor. To be determined. |
// controlIntegrals[index] += tmp * 4; |
/* |
if (controlIntegrals[index] > PITCHROLLOVER180) { |
controlIntegrals[index] -= PITCHROLLOVER360; |
} else if (controlIntegrals[index] <= -PITCHROLLOVER180) { |
controlIntegrals[index] += PITCHROLLOVER360; |
} |
*/ |
control[index] = newValue; |
tmp -= newValue; |
tmp = tmp * tmp; |
// tmp += (newValue >= 0) ? newValue : -newValue; |
controlActivity += tmp; |
} |
#define CADAMPING 10 |
void dampenControlActivity(void) { |
int32_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. |
*/ |
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; |
RC_update(); |
// EC_update(); |
// HC_update(); |
int16_t* RC_EATR = RC_getEATR(); |
// int16_t* EC_PRTY = EC_getPRTY(); |
updateControlAndMeasureControlActivity(CONTROL_ELEVATOR, RC_EATR[CONTROL_ELEVATOR] /* + EC_PRTY[CONTROL_PITCH] */); |
updateControlAndMeasureControlActivity(CONTROL_AILERONS, RC_EATR[CONTROL_AILERONS] /* + EC_PRTY[CONTROL_ROLL] */); |
updateControlAndMeasureControlActivity(CONTROL_RUDDER, RC_EATR[CONTROL_RUDDER] /* + EC_PRTY[CONTROL_YAW] */); |
dampenControlActivity(); |
// Do we also want to have activity measurement on throttle? |
control[CONTROL_THROTTLE] = RC_EATR[CONTROL_THROTTLE]; // + EC_PRTY[CONTROL_THROTTLE]); |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
configuration_staticToDynamic(); |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
} |
// part1a end. |
/* This is not really necessary with the dead-band feature on all sticks (see rc.c) |
if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
if (controlYaw > 2) controlYaw-= 2; |
else if (controlYaw< -2) controlYaw += 2; |
else controlYaw = 0; |
} |
*/ |
/* |
* Record maxima |
*/ |
for (axis = PITCH; axis <= ROLL; axis++) { |
if (abs(control[axis]) > maxControl[axis]) { |
maxControl[axis] = abs(control[axis]); |
if (maxControl[axis] > 100) |
maxControl[axis] = 100; |
} else if (maxControl[axis]) |
maxControl[axis]--; |
} |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
// uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} /*else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} */ |
else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
} |
// TODO: Integrate into command system. |
uint8_t controlMixer_testCompassCalState(void) { |
return RC_testCompassCalState(); |
} |
#include <stdlib.h> |
#include "controlMixer.h" |
#include "rc.h" |
#include "externalControl.h" |
#include "failsafeControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "commands.h" |
#include "output.h" |
int16_t controls[4] = { 0, 0, 0, 0 }; |
// Internal variables for reading commands made with an R/C stick. |
uint8_t lastCommand = COMMAND_NONE; |
uint8_t lastArgument; |
uint8_t isCommandRepeated = 0; |
uint8_t controlMixer_didReceiveSignal = 0; |
/* |
* This could be expanded to take arguments from ohter sources than the RC |
* (read: Custom MK RC project) |
*/ |
uint8_t controlMixer_getArgument(void) { |
return lastArgument; |
} |
/* |
* This could be expanded to take calibrate / start / stop commands from ohter sources |
* than the R/C (read: Custom MK R/C project) |
*/ |
uint8_t controlMixer_getCommand(void) { |
return lastCommand; |
} |
uint8_t controlMixer_isCommandRepeated(void) { |
return isCommandRepeated; |
} |
void controlMixer_setNeutral() { |
for (uint8_t i=0; i<VARIABLE_COUNT; i++) { |
variables[i] = RC_getVariable(i); |
} |
EC_setNeutral(); |
FC_setNeutral(); // FC is FailsafeControl, not FlightCtrl. |
// This is to set the home pos in navi. |
// MKFlags |= MKFLAG_CALIBRATE; |
} |
/* |
* Update potentiometer values with limited slew rate. Could be made faster if desired. |
* TODO: It assumes R/C as source. Not necessarily true. |
*/ |
void controlMixer_updateVariables(void) { |
uint8_t i; |
int16_t targetvalue; |
for (i=0; i < VARIABLE_COUNT; i++) { |
targetvalue = RC_getVariable(i); |
if (targetvalue < 0) |
targetvalue = 0; |
if (variables[i] < targetvalue && variables[i] < 255) |
variables[i]++; |
else if (variables[i] > 0 && variables[i] > targetvalue) |
variables[i]--; |
} |
} |
uint8_t controlMixer_getSignalQuality(void) { |
uint8_t rcQ = RC_getSignalQuality(); |
uint8_t ecQ = EC_getSignalQuality(); |
// This needs not be the only correct solution... |
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; |
* / |
if (controlActivity + (uint16_t)tmp < 0x8000) |
controlActivity += tmp; |
} |
#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 |
* and whatever other controls we invented in the meantime... |
* Update variables. |
* Decode commands but do not execute them. |
*/ |
void controlMixer_periodicTask(void) { |
int16_t tempPRTY[4] = { 0, 0, 0, 0 }; |
// Decode commands. |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
: COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() |
: COMMAND_NONE; |
// Update variables ("potis"). |
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
controlMixer_didReceiveSignal = 1; |
} else { // Signal is not OK |
// Could handle switch to emergency flight here. |
// throttle is handled elsewhere. |
} |
if (rcCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == rcCommand); |
lastCommand = rcCommand; |
lastArgument = RC_getArgument(); |
} else if (ecCommand != COMMAND_NONE) { |
isCommandRepeated = (lastCommand == ecCommand); |
lastCommand = ecCommand; |
lastArgument = EC_getArgument(); |
} else { |
// Both sources have no command, or one or both are out. |
// Just set to false. There is no reason to check if the none-command was repeated anyway. |
isCommandRepeated = 0; |
lastCommand = COMMAND_NONE; |
} |
// This will init the values (not just add to them). |
RC_periodicTaskAndPRTY(tempPRTY); |
// Add external control to RC |
EC_periodicTaskAndPRTY(tempPRTY); |
FC_periodicTaskAndPRTY(tempPRTY); |
// Commit results to global variable and also measure control activity. |
controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE]; |
controls[CONTROL_ELEVATOR] = tempPRTY[CONTROL_ELEVATOR]; |
controls[CONTROL_AILERONS] = tempPRTY[CONTROL_AILERONS]; |
controls[CONTROL_RUDDER] = tempPRTY[CONTROL_RUDDER]; |
// dampenControlActivity(); |
// We can safely do this even with a bad signal - the variables will not have been updated then. |
configuration_applyVariablesToParams(); |
} |
/branches/dongfang_FC_fixedwing/controlMixer.h |
---|
23,7 → 23,7 |
#define SIGNAL_GOOD 4 |
/* |
* The EATR arrays |
* The PRTY arrays |
*/ |
#define CONTROL_ELEVATOR 0 |
#define CONTROL_AILERONS 1 |
31,6 → 31,18 |
#define CONTROL_RUDDER 3 |
/* |
* Looping flags. |
* LOOPING_UP || LOOPING_DOWN <=> LOOPING_PITCH_AXIS |
* LOOPING_LEFT || LOOPING_RIGHT <=> LOOPING_ROLL_AXIS |
*/ |
#define LOOPING_UP (1<<0) |
#define LOOPING_DOWN (1<<1) |
#define LOOPING_LEFT (1<<2) |
#define LOOPING_RIGHT (1<<3) |
#define LOOPING_PITCH_AXIS (1<<4) |
#define LOOPING_ROLL_AXIS (1<<5) |
/* |
* This is only relevant for "abstract controls" ie. all control sources have the |
* same interface. This struct of code pointers is used like an abstract class |
* definition from object-oriented languages, and all control input implementations |
59,16 → 71,12 |
/* |
* Our output. |
*/ |
extern int16_t control[4]; |
extern int32_t controlIntegrals[4]; |
extern int16_t controls[4]; |
extern uint16_t controlActivity; |
extern uint16_t maxControl[2]; |
//extern uint16_t maxControl[2]; |
extern volatile uint8_t MKFlags; |
extern uint16_t isFlying; |
void controlMixer_initVariables(void); |
void controlMixer_updateVariables(void); |
//void controlMixer_updateVariables(void); |
void controlMixer_setNeutral(void); |
75,7 → 83,7 |
/* |
* Update the exported variables. Called at every flight control cycle. |
*/ |
void controlMixer_update(void); |
void controlMixer_periodicTask(void); |
/* |
* Get the current command. See the COMMAND_.... define's |
85,6 → 93,7 |
void controlMixer_performCalibrationCommands(uint8_t command); |
uint8_t controlMixer_getSignalQuality(void); |
extern uint8_t controlMixer_didReceiveSignal; |
/* |
* The controls operate in [-1024, 1024] just about. |
111,8 → 120,5 |
* |
* Not in any of these positions: 0 |
*/ |
// void controlMixer_handleCommands(void); |
uint8_t controlMixer_getArgument(void); |
uint8_t controlMixer_isCommandRepeated(void); |
// TODO: Abstract away if possible. |
uint8_t controlMixer_testCompassCalState(void); |
/branches/dongfang_FC_fixedwing/dongfangMath.c |
---|
5,9 → 5,6 |
// For scope debugging only! |
#include "output.h" |
// Debug |
#include "uart0.h" |
const int16_t SIN_TABLE[] PROGMEM = { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.01745240643728351 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.03489949670250097 * MATH_UNIT_FACTOR + 0.5), |
97,8 → 94,8 |
(int16_t) (0.9975640502598242 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9986295347545738 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9993908270190958 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), (int16_t) (1.0 |
* MATH_UNIT_FACTOR) }; |
(int16_t) (0.9998476951563913 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (MATH_UNIT_FACTOR) + 0.5 }; |
const int16_t TAN_TABLE[] PROGMEM |
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5), |
193,43 → 190,50 |
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5), |
(int16_t) (32767) }; |
int16_t int_sin(int32_t arg) { |
int16_t sin_360(int16_t arg) { |
int8_t sign; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
argp %= 360; |
if (argp < 0) { |
argp = -argp; |
arg %= 360; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} else { |
sign = 1; |
} |
if (argp >= 90) { |
argp = 180 - argp; |
if (arg > 180) { |
arg = 360 - arg; |
sign = -sign; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]); |
if (arg > 90) { |
arg = 180 - arg; |
} |
result = pgm_read_word(&SIN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
int16_t int_cos(int32_t arg) { |
if (arg > 90L * MATH_DRG_FACTOR) |
return int_sin(arg + (90L - 360L) * MATH_DRG_FACTOR); |
return int_sin(arg + 90L * MATH_DRG_FACTOR); |
int16_t cos_360(int16_t arg) { |
return sin_360(arg + 90); |
} |
int16_t int_tan(int32_t arg) { |
int16_t tan_360(int16_t arg) { |
int8_t sign = 1; |
int16_t result; |
int16_t argp = arg / MATH_DRG_FACTOR; |
if (argp >= 90) { |
argp = 180 - argp; |
sign = -1; |
} else if (argp < -90) { |
argp += 180; |
} else if (argp < 0) { |
argp = -argp; |
sign = -1; |
if (arg < 0) { |
arg = -arg; |
sign = -1; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]); |
if (arg >= 90) { |
arg = 180 - arg; |
sign = -sign; |
} |
result = pgm_read_word(&TAN_TABLE[(uint8_t) arg]); |
return (sign == 1) ? result : -result; |
} |
void intervalWrap(int32_t *number, int32_t limit) { |
if (*number >= limit) { |
*number -= limit; // 360 deg. wrap |
} else if (*number < 0) { |
*number += limit; |
} |
} |
/branches/dongfang_FC_fixedwing/dongfangMath.h |
---|
4,7 → 4,7 |
/* |
* Angular unit scaling: Number of units per degree |
*/ |
#define MATH_DRG_FACTOR GYRO_DEG_FACTOR_PITCHROLL |
#define MATH_DRG_FACTOR GYRO_DEG_FACTOR |
/* |
* Fix-point decimal scaling: Number of units for 1 (so if sin(something) |
14,9 → 14,11 |
//#define MATH_UNIT_FACTOR 8192 |
// Changed: We want to be able to multiply 2 sines/cosines and still stay comfortably (factor 100) within 31 bits. |
// 4096 = 12 bits, square = 24 bits, 7 bits to spare. |
#define MATH_UNIT_FACTOR_LOG 12 |
#define MATH_UNIT_FACTOR (1L<<MATH_UNIT_FACTOR_LOG) |
#define LOG_MATH_UNIT_FACTOR 12 |
#define MATH_UNIT_FACTOR (1L<<LOG_MATH_UNIT_FACTOR) |
int16_t int_sin(int32_t arg); |
int16_t int_cos(int32_t arg); |
int16_t int_tan(int32_t arg); |
int16_t sin_360(int16_t arg); |
int16_t cos_360(int16_t arg); |
int16_t tan_360(int16_t arg); |
void intervalWrap(int32_t *number, int32_t limit); |
/branches/dongfang_FC_fixedwing/eeprom.c |
---|
1,246 → 1,189 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Contant Values |
// + 0-250 -> normale Values |
// + 251 -> Poti1 |
// + 252 -> Poti2 |
// + 253 -> Poti3 |
// + 254 -> Poti4 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#ifndef EEMEM |
#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/wdt.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.UserParams); i++) { |
staticParams.UserParams[i] = 0; |
} |
/* |
* While we are still using userparams for flight parameters, do set |
* some safe & meaningful default values. |
*/ |
} |
void setOtherDefaults(void) { |
/* Channel assignments were changed to the normal: |
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4 |
*/ |
staticParams.ChannelAssignment[CH_ELEVATOR] = 2; |
staticParams.ChannelAssignment[CH_AILERONS] = 1; |
staticParams.ChannelAssignment[CH_THROTTLE] = 3; |
staticParams.ChannelAssignment[CH_RUDDER] = 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.CompassYawEffect = 128; |
staticParams.GyroPitchP = 0; |
staticParams.GyroRollP = 0; |
staticParams.GyroYawP = 0; |
staticParams.GyroPitchD = 0; |
staticParams.GyroRollD = 0; |
staticParams.GyroYawD = 0; |
staticParams.StickElevatorP = 10; |
staticParams.StickAileronsP = 10; |
staticParams.StickRudderP = 10; |
staticParams.LowVoltageWarning = 105; |
staticParams.ServoRefresh = 7; |
staticParams.J16Bitmask = 95; |
staticParams.J17Bitmask = 243; |
staticParams.J16Timing = 15; |
staticParams.J17Timing = 15; |
staticParams.servoDirections = 2; |
} |
void setDefaults(void) { |
setOtherDefaults(); |
gyro_setDefaults(); |
setDefaultUserParams(); |
staticParams.J16Timing = 10; |
staticParams.J17Timing = 10; |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as byte */ |
/***************************************************/ |
uint8_t GetParamByte(uint16_t param_id) { |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
uint8_t getParamByte(uint16_t param_id) { |
return eeprom_read_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id]); |
} |
/***************************************************/ |
/* Write Parameter to EEPROM as byte */ |
/***************************************************/ |
void SetParamByte(uint16_t param_id, uint8_t value) { |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
void setParamByte(uint16_t param_id, uint8_t value) { |
eeprom_write_byte(&EEPromArray[EEPROM_ADR_PARAM_BEGIN + param_id], value); |
} |
/***************************************************/ |
/* Read Parameter from EEPROM as word */ |
/***************************************************/ |
uint16_t GetParamWord(uint16_t param_id) { |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN |
+ param_id]); |
/* |
uint16_t getParamWord(uint16_t param_id) { |
return eeprom_read_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAM_BEGIN |
+ param_id]); |
} |
*/ |
/***************************************************/ |
/* 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); |
} |
*/ |
uint16_t CRC16(uint8_t* data, uint16_t length) { |
uint16_t crc = 0; |
for (uint16_t i=0; i<length; i++) { |
crc = (uint8_t)(crc >> 8) | (crc << 8); |
crc ^= data[i]; |
crc ^= (uint8_t)(crc & 0xff) >> 4; |
crc ^= (crc << 8) << 4; |
crc ^= ((crc & 0xff) << 4) << 1; |
} |
return crc; |
} |
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data. |
// length is the length of the pure data not including checksum and revision number. |
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t offset, uint16_t length) { |
uint16_t CRC = CRC16(data, length); |
eeprom_write_byte(&EEPromArray[offset], CRC&0xff); |
eeprom_write_byte(&EEPromArray[offset+1], CRC>>8); |
eeprom_write_byte(&EEPromArray[offset+2], revisionNumber); |
eeprom_write_block(data, &EEPromArray[offset+3], length); |
} |
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data. |
// length is the length of the pure data not including checksum and revision number. |
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t offset, uint16_t length) { |
uint16_t CRCRead = eeprom_read_byte(&EEPromArray[offset]) | (eeprom_read_byte(&EEPromArray[offset+1])<<8); |
uint8_t revisionNumberRead = eeprom_read_byte(&EEPromArray[offset+2]); |
eeprom_read_block(target, &EEPromArray[offset+3], length); |
uint16_t CRCCalculated = CRC16(target, length); |
uint8_t CRCError = (CRCRead != CRCCalculated); |
uint8_t revisionMismatch = (revisionNumber != revisionNumberRead); |
if (CRCError && revisionMismatch) printf("\n\rEEPROM CRC error and revision mismatch; "); |
else if (CRCError) printf("\n\rEEPROM CRC error; "); |
else if (revisionMismatch) printf("\n\rEEPROM revision mismatch; "); |
return (CRCError || revisionMismatch); |
} |
/***************************************************/ |
/* Read Parameter Set from EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_ReadFromEEProm() { |
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], PARAMSET_STRUCT_LEN); |
output_init(); |
// setnumber [1..5] |
uint8_t paramSet_readFromEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
uint8_t result = readChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
configuration_paramSetDidChange(); |
return result; |
} |
/***************************************************/ |
/* Write Parameter Set to EEPROM */ |
/***************************************************/ |
// number [1..5] |
void ParamSet_WriteToEEProm() { |
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0], |
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN], 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 |
// set this parameter set to active set |
output_init(); |
void paramSet_writeToEEProm(uint8_t setnumber) { |
uint16_t offset = EEPROM_ADR_PARAMSET_BEGIN + (setnumber-1)*(sizeof(ParamSet_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD); |
writeChecksummedBlock(EEPARAM_REVISION, (uint8_t*)&staticParams, offset, sizeof(ParamSet_t)); |
// set this parameter set to active set |
} |
/***************************************************/ |
/* 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); |
} |
return (setnumber); |
void paramSet_readOrDefault() { |
// parameter version check |
if (paramSet_readFromEEProm(1)) { |
// if version check faild |
printf("\n\rwriting default parameter sets"); |
for (uint8_t i=5; i>0; i--) { |
paramSet_default(i); |
paramSet_writeToEEProm(i); |
} |
// default-Setting is parameter set 1 |
paramSet_readFromEEProm(1); |
// For some strange reason, the read will have no effect. |
// Lets reset... |
// wdt_enable(WDTO_500MS); |
} |
printf("\n\r\rUsing Parameter Set %d", 1); |
} |
/***************************************************/ |
/* Read MixerTable from EEPROM */ |
/* Read IMU Config from EEPROM */ |
/***************************************************/ |
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 IMUConfig_readFromEEprom(void) { |
return readChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
/***************************************************/ |
/* Write Mixer Table to EEPROM */ |
/* Write IMU Config 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 IMUConfig_writeToEEprom(void) { |
writeChecksummedBlock(IMUCONFIG_REVISION, (uint8_t*)&IMUConfig, EEPROM_ADR_IMU_CONFIG, sizeof(IMUConfig_t)); |
} |
void IMUConfig_readOrDefault(void) { |
if(IMUConfig_readFromEEprom()) { |
printf("\n\rwriting default IMU config"); |
IMUConfig_default(); |
IMUConfig_writeToEEprom(); |
} |
} |
/***************************************************/ |
/* Default Values for Mixer Table */ |
/* ChannelMap */ |
/***************************************************/ |
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; |
} |
// 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); |
void channelMap_writeToEEProm(void) { |
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
void channelMap_readOrDefault(void) { |
if (readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t))) { |
printf("\n\rwriting default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
wdt_enable(WDTO_500MS); |
} |
} |
/***************************************************/ |
/* Initialize EEPROM Parameter Sets */ |
/* Sensor offsets */ |
/***************************************************/ |
void ParamSet_Init(void) { |
uint8_t Channel_Backup = 1, j; |
// parameter version check |
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) { |
// if version check faild |
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; |
} |
// fill all 5 parameter settings |
uint8_t gyroAmplifierOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
setDefaults(); // Fill staticParams Structure to default parameter set 1 (Sport) |
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]); |
} |
} |
ParamSet_WriteToEEProm(); |
// update version info |
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION); |
} |
// read active parameter set to staticParams stucture |
ParamSet_ReadFromEEProm(); |
void gyroAmplifierOffset_writeToEEProm(void) { |
return writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroAmplifierOffset, EEPROM_ADR_GYROAMPLIFIER, sizeof(sensorOffset_t)); |
} |
uint8_t gyroOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
void gyroOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&gyroOffset, EEPROM_ADR_GYROOFFSET, sizeof(sensorOffset_t)); |
} |
/* |
uint8_t accOffset_readFromEEProm(void) { |
return readChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
void accOffset_writeToEEProm(void) { |
writeChecksummedBlock(SENSOROFFSET_REVISION, (uint8_t*)&accOffset, EEPROM_ADR_ACCOFFSET, sizeof(sensorOffset_t)); |
} |
*/ |
/branches/dongfang_FC_fixedwing/eeprom.h |
---|
3,33 → 3,51 |
#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 sizeof(paramset_t) // word |
#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) |
#define EEPROM_CHECKSUMMED_BLOCK_OVERHEAD 3 |
extern void ParamSet_Init(void); |
extern void ParamSet_ReadFromEEProm(void); |
extern void ParamSet_WriteToEEProm(void); |
#define PID_ACTIVE_SET 0 // byte |
//#define EEPROM_ADR_ACCOFFSET 1 |
//#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
extern uint8_t MixerTable_ReadFromEEProm(void); |
extern uint8_t MixerTable_WriteToEEProm(void); |
#define EEPROM_ADR_ACCOFFSET 16 |
#define EEPROM_ADR_GYROOFFSET 32 |
#define EEPROM_ADR_GYROAMPLIFIER 48 |
#define EEPROM_ADR_CHANNELMAP 64 |
#define EEPROM_ADR_IMU_CONFIG 100 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
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); |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 4 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
#define IMUCONFIG_REVISION 0 |
extern uint8_t getActiveParamSet(void); |
void paramSet_readOrDefault(void); |
void channelMap_readOrDefault(void); |
void IMUConfig_readOrDefault(void); |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
void paramSet_writeToEEProm(uint8_t setnumber); |
void channelMap_writeToEEProm(void); |
uint8_t gyroOffset_readFromEEProm(void); |
void gyroOffset_writeToEEProm(void); |
//uint8_t accOffset_readFromEEProm(void); |
//void accOffset_writeToEEProm(void); |
uint8_t getParamByte(uint16_t param_id); |
void setParamByte(uint16_t param_id, uint8_t value); |
// We have only a single param set here. |
// uint8_t getActiveParamSet(void); |
// void setActiveParamSet(uint8_t setnumber); |
#endif //_EEPROM_H |
/branches/dongfang_FC_fixedwing/externalControl.c |
---|
1,67 → 1,60 |
#include "externalcontrol.h" |
#include "externalControl.h" |
#include "configuration.h" |
#include "controlMixer.h" |
ExternalControl_t externalControl; |
uint8_t externalControlActive; |
uint8_t externalControlActive = 0; |
// int16_t EC_PRTY[4]; |
// TODO: Who is going to call this |
int16_t EC_EATR[4]; |
// TODO: Who is going to call this |
void EC_setNeutral(void) { |
// if necessary. From main.c. |
externalControl.config = 0; |
externalControl.pitch = 0; |
externalControl.roll = 0; |
externalControl.yaw = 0; |
externalControl.throttle = 0; |
// if necessary. From main.c. |
externalControl.config = 0; |
externalControl.pitch = 0; |
externalControl.roll = 0; |
externalControl.yaw = 0; |
externalControl.throttle = 0; |
// From main.c. What does it do?? |
externalControl.digital[0] = 0x55; |
// From main.c. What does it do?? |
externalControl.digital[0] = 0x55; |
} |
int16_t* EC_getEATR(void) { |
return EC_EATR; |
void EC_periodicTaskAndPRTY(int16_t* PRTY) { |
if (externalControlActive) { |
externalControlActive--; |
PRTY[CONTROL_ELEVATOR] += externalControl.pitch * 8; |
PRTY[CONTROL_AILERONS] += externalControl.roll * 8; |
PRTY[CONTROL_THROTTLE] += externalControl.throttle * 8; |
PRTY[CONTROL_RUDDER] += externalControl.yaw * 8; |
} |
} |
uint8_t EC_getArgument(void) { |
return externalControl.config; |
return externalControl.config; |
} |
uint8_t EC_getCommand(void) { |
return externalControl.free; |
return externalControl.free; |
} |
// not implemented. |
int16_t EC_getVariable(uint8_t varNum) { |
return 0; |
return 0; |
} |
void EC_update() { |
if (externalControlActive) { |
externalControlActive--; |
EC_EATR[CONTROL_ELEVATOR] = externalControl.pitch * 4; |
EC_EATR[CONTROL_AILERONS] = externalControl.roll * 4; |
EC_EATR[CONTROL_THROTTLE] = externalControl.throttle; |
EC_EATR[CONTROL_RUDDER ] = externalControl.yaw; // No stickP or similar?????? |
} else { |
EC_EATR[CONTROL_ELEVATOR] = EC_EATR[CONTROL_AILERONS] = EC_EATR[CONTROL_THROTTLE] = EC_EATR[CONTROL_RUDDER] = 0; |
} |
} |
uint8_t EC_getSignalQuality(void) { |
if (externalControlActive > 40) |
// Configured and heard from recently |
return SIGNAL_GOOD; |
if (externalControlActive) |
// 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; |
// Configured but expired. |
return SIGNAL_LOST; |
} |
/branches/dongfang_FC_fixedwing/externalControl.h |
---|
20,8 → 20,7 |
extern ExternalControl_t externalControl; |
extern uint8_t externalControlActive; |
void EC_update(void); |
int16_t* EC_getEATR(void); |
void EC_periodicTaskAndPRTY(int16_t* PRTY); |
uint8_t EC_getArgument(void); |
uint8_t EC_getCommand(void); |
int16_t EC_getVariable(uint8_t varNum); |
/branches/dongfang_FC_fixedwing/flight.c |
---|
13,47 → 13,26 |
#include "timer2.h" |
#include "attitude.h" |
#include "controlMixer.h" |
#include "commands.h" |
#ifdef USE_MK3MAG |
#include "gps.h" |
#endif |
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
/* |
* These are no longer maintained, just left at 0. The original implementation just summed the acc. |
* value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey??? |
* target-directions integrals. |
*/ |
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0; |
int32_t target[3]; |
int8_t pitchPFactor, rollPFactor, yawPFactor; |
int8_t pitchDFactor, rollDFactor, yawDFactor; |
/* |
* Error integrals. |
*/ |
int32_t error[3]; |
int32_t IPart[2] = {0,0}; |
uint8_t pFactor[3]; |
uint8_t dFactor[3]; |
uint8_t iFactor[3]; |
uint8_t reverse[3]; |
int32_t IPart[3] = { 0, 0, 0 }; |
/************************************************************************/ |
/* Filter for motor value smoothing (necessary???) */ |
/************************************************************************/ |
int16_t outputFilter(int16_t newvalue, int16_t oldvalue) { |
switch (dynamicParams.UserParams[5]) { |
case 0: |
return newvalue; |
case 1: |
return (oldvalue + newvalue) / 2; |
case 2: |
if (newvalue > oldvalue) |
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
else |
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
case 3: |
if (newvalue < oldvalue) |
return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new |
else |
return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old |
default: |
return newvalue; |
} |
} |
int16_t servos[MAX_SERVOS]; |
/************************************************************************/ |
/* Neutral Readings */ |
60,135 → 39,139 |
/************************************************************************/ |
#define CONTROL_CONFIG_SCALE 10 |
void flight_setNeutral() { |
// not really used here any more. |
controlMixer_initVariables(); |
void flight_setGround(void) { |
IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
target[PITCH] = attitude[PITCH]; |
target[ROLL] = attitude[ROLL]; |
target[YAW] = attitude[YAW]; |
} |
void setFlightParameters |
( |
uint8_t _pitchPFactor, |
uint8_t _rollPFactor, |
uint8_t _yawPFactor, |
uint8_t _pitchDFactor, |
uint8_t _rollDFactor, |
uint8_t _yawDFactor |
) { |
pitchPFactor = _pitchPFactor; |
rollPFactor = _rollPFactor; |
yawPFactor = _yawPFactor; |
void switchToFlightMode(FlightMode_t flightMode) { |
if (flightMode == FLIGHT_MODE_MANUAL) { |
pFactor[PITCH] = 0; |
pFactor[ROLL] = 0; |
pFactor[YAW] = 0; |
} else if (flightMode == FLIGHT_MODE_RATE) { |
pFactor[PITCH] = 0; //staticParams...; |
pFactor[ROLL] = 0; //staticParams...; |
pFactor[YAW] = 0; //staticParams...; |
} |
if (flightMode == FLIGHT_MODE_MANUAL || FLIGHT_MODE_RATE) { |
iFactor[PITCH] = 0; |
iFactor[ROLL] = 0; |
iFactor[YAW] = 0; |
} else if (flightMode == FLIGHT_MODE_ANGLES) { |
iFactor[PITCH] = 0; //staticParams...; |
iFactor[ROLL] = 0; //staticParams...; |
iFactor[YAW] = 0; //staticParams...; |
// When entering this mode, we want to avoid jerks from accumulated uncorrected errors. |
target[PITCH] = attitude[PITCH]; |
target[ROLL] = attitude[ROLL]; |
target[YAW] = attitude[YAW]; |
} |
pitchDFactor = _pitchDFactor; |
rollDFactor = _rollDFactor; |
yawDFactor = _yawDFactor; |
} |
dFactor[PITCH] = staticParams.gyroPitchD / CONTROL_CONFIG_SCALE; |
dFactor[ROLL] = staticParams.gyroRollD / CONTROL_CONFIG_SCALE; |
dFactor[YAW] = staticParams.gyroYawD / CONTROL_CONFIG_SCALE; |
void setNormalFlightParameters(void) { |
setFlightParameters |
( |
dynamicParams.GyroPitchP / CONTROL_CONFIG_SCALE, // 12 seems good |
dynamicParams.GyroRollP / CONTROL_CONFIG_SCALE, // 9 seems good |
dynamicParams.GyroYawP / (CONTROL_CONFIG_SCALE/2), // 24 seems too little |
dynamicParams.GyroPitchD / CONTROL_CONFIG_SCALE, |
dynamicParams.GyroRollD / CONTROL_CONFIG_SCALE, |
dynamicParams.GyroYawD / CONTROL_CONFIG_SCALE |
); |
// TODO: Set reverse also. |
} |
void setStableFlightParameters(void) { |
setFlightParameters(0, 0, 0, 0, 0, 0); |
} |
/************************************************************************/ |
/* Main Flight Control */ |
/************************************************************************/ |
void flight_control(void) { |
// Mixer Fractions that are combined for Motor Control |
int16_t yawTerm, throttleTerm, term[2]; |
int16_t throttleTerm, term[3]; |
// PID controller variables |
int16_t PDPart[2], PDPartYaw; |
int16_t PDPart[3]; |
static int8_t debugDataTimer = 1; |
// High resolution motor values for smoothing of PID motor outputs |
static int16_t outputFilters[MAX_OUTPUTS]; |
// static int16_t outputFilters[MAX_OUTPUTS]; |
uint8_t i; |
uint8_t axis; |
// Fire the main flight attitude calculation, including integration of angles. |
// We want that to kick as early as possible, not to delay new AD sampling further. |
calculateFlightAttitude(); |
controlMixer_update(); |
throttleTerm = control[CONTROL_THROTTLE]; |
// TODO: Check modern version. |
// calculateFlightAttitude(); |
// TODO: Check modern version. |
// controlMixer_update(); |
throttleTerm = controls[CONTROL_THROTTLE]; |
/************************************************************************/ |
/* RC-signal is bad */ |
/************************************************************************/ |
// These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not received or noisy |
RED_ON; |
beepRCAlarm(); |
setStableFlightParameters(); |
} else { |
commands_handleCommands(); |
setNormalFlightParameters(); |
} |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
PDPart[PITCH] = ((int32_t) rate_PID[PITCH] * pitchPFactor / |
(256L / CONTROL_SCALING)) |
+ (differential[PITCH] * (int16_t) dynamicParams.GyroPitchD) / 16; |
for (axis = PITCH; axis <= YAW; axis++) { |
if (target[axis] > OVER180) { |
target[axis] -= OVER360; |
} else if (attitude[axis] <= -OVER180) { |
attitude[axis] += OVER360; |
} |
PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor / |
(256L / CONTROL_SCALING)) |
+ (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16; |
if (reverse[axis]) |
error[axis] = attitude[axis] + target[axis]; |
else |
error[axis] = attitude[axis] - target[axis]; |
if (error[axis] > OVER180) { |
error[axis] -= OVER360; |
} else if (error[axis] <= -OVER180) { |
error[axis] += OVER360; |
} |
PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING) |
+ (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16; |
/************************************************************************/ |
/* Calculate control feedback from angle (gyro integral) */ |
/* and angular velocity (gyro signal) */ |
/************************************************************************/ |
PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
+ ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
if (reverse[axis]) |
PDPart[axis] = -PDPart[axis]; |
/************************************************************************/ |
/* Stick signals are positive and gyros are negative... */ |
/************************************************************************/ |
IPart[PITCH] = error[PITCH]; // * some factor configurable. |
IPart[ROLL] = error[ROLL]; |
// TODO: Add ipart. Or add/subtract depending, not sure. |
term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.servoDirections & SERVO_DIRECTION_ELEVATOR ? PDPart[PITCH] : -PDPart[PITCH]); |
term[ROLL] = control[CONTROL_AILERONS] + (staticParams.servoDirections & SERVO_DIRECTION_AILERONS ? PDPart[ROLL] : -PDPart[ROLL]); |
yawTerm = control[CONTROL_RUDDER] + (staticParams.servoDirections & SERVO_DIRECTION_RUDDER ? PDPartYaw : -PDPartYaw); |
int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
if (reverse[axis]) |
PDPart[axis] += anglePart; |
else |
PDPart[axis] -= anglePart; |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Universal Mixer |
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Add I parts here... these are integrated errors. |
// When an error wraps, actually its I part should be negated or something... |
DebugOut.Analog[12] = term[PITCH]; |
DebugOut.Analog[13] = term[ROLL]; |
DebugOut.Analog[14] = throttleTerm; |
DebugOut.Analog[15] = yawTerm; |
term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
} |
for (i = 0; i < MAX_OUTPUTS; i++) { |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = throttleTerm; |
debugOut.analog[15] = term[YAW]; |
for (uint8_t i = 0; i < MAX_SERVOS; i++) { |
int16_t tmp; |
if (outputTestActive) { |
outputs[i].SetPoint = outputTest[i] * 4; |
} else { |
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
switch(i) { |
case 0: tmp = term[ROLL]; break; |
case 1: tmp = term[PITCH]; break; |
case 2: tmp = throttleTerm - 310; break; |
case 3: tmp = yawTerm; break; |
default: tmp = 0; |
} |
outputFilters[i] = outputFilter(tmp, outputFilters[i]); |
// Now we scale back down to a 0..255 range. |
tmp = outputFilters[i]; |
outputs[i].SetPoint = tmp; |
if (servoTestActive) { |
servos[i] = ((int16_t) servoTest[i] - 128) * 4; |
} else { |
// Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
switch (i) { |
case 0: |
tmp = term[ROLL]; |
break; |
case 1: |
tmp = term[PITCH]; |
break; |
case 2: |
tmp = throttleTerm; |
break; |
case 3: |
tmp = term[YAW]; |
break; |
default: |
tmp = 0; |
} |
// These are all signed and in the same units as the RC stuff in rc.c. |
servos[i] = tmp; |
} |
} |
197,20 → 180,21 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = angle[YAW] / GYRO_DEG_FACTOR_YAW; |
debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[2] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
DebugOut.Analog[6] = pitchPFactor; |
DebugOut.Analog[7] = rollPFactor; |
DebugOut.Analog[8] = yawPFactor; |
DebugOut.Analog[9] = pitchDFactor; |
DebugOut.Analog[10] = rollDFactor; |
DebugOut.Analog[11] = yawDFactor; |
debugOut.analog[3] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[4] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[5] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
DebugOut.Analog[18] = (10 * error[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[19] = (10 * error[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
debugOut.analog[6] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[7] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
debugOut.analog[8] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
//DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
//debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
//debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
} |
} |
/branches/dongfang_FC_fixedwing/flight.h |
---|
11,31 → 11,18 |
#define PITCH 0 |
#define ROLL 1 |
#define YAW 2 |
/* |
* The output BLC throttles can be set in an [0..256[ interval. Some staticParams limits |
* (min, throttle, max throttle etc) are in a [0..256[ interval. |
* The calculation of motor throttle values from sensor and control information (basically |
* flight.c) take place in an [0..1024[ interval for better precision. |
* This is the conversion factor. |
* --> Replaced back again by CONTROL_SCALING. Even though the 2 things are not quite the |
* same, they are unseperable anyway. |
*/ |
#define MAX_SERVOS 8 |
// looping params |
// extern long TurnOver180Nick, TurnOver180Roll; |
extern int16_t servos[MAX_SERVOS]; |
// external control |
// extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw; |
// extern int16_t naviAccPitch, naviAccRoll, naviCntAcc; |
typedef enum { |
FLIGHT_MODE_MANUAL, |
FLIGHT_MODE_RATE, |
FLIGHT_MODE_ANGLES |
} FlightMode_t; |
// TODO: Rip em up, replace by a flight-state machine. |
// OK first step: Move to control mixer, where the state machine will be located anyway. |
// extern volatile uint8_t MKFlags; |
// extern uint16_t isFlying; |
#define HH_RANGE ((int32_t)GYRO_DEG_FACTOR_PITCHROLL * 30) |
void flight_control(void); |
void transmitMotorThrottleData(void); |
void flight_setNeutral(void); |
/branches/dongfang_FC_fixedwing/invenSense.c |
---|
1,4 → 1,3 |
#include "invenSense.h" |
#include "timer0.h" |
#include "configuration.h" |
8,8 → 7,8 |
* Configuration for my prototype board with InvenSense gyros. |
* The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll. |
*/ |
PR_GYROS_ORIENTATION_REVERSED = 0; |
// The special auto-zero feature of this gyro needs a port. |
#define AUTOZERO_PORT PORTD |
#define AUTOZERO_DDR DDRD |
#define AUTOZERO_BIT 5 |
16,8 → 15,7 |
void gyro_calibrate(void) { |
// If port not already set to output and high, do it. |
if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1 |
<< AUTOZERO_BIT))) { |
if (!(AUTOZERO_DDR & (1 << AUTOZERO_BIT)) || !(AUTOZERO_PORT & (1 << AUTOZERO_BIT))) { |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
AUTOZERO_DDR |= (1 << AUTOZERO_BIT); |
delay_ms(100); |
28,11 → 26,16 |
delay_ms(1); |
AUTOZERO_PORT |= (1 << AUTOZERO_BIT); |
// Delay_ms(10); |
delay_ms_Mess(100); |
delay_ms_with_adc_measurement(100, 0); |
} |
void gyro_setDefaults(void) { |
staticParams.zerothOrderGyroCorrectionFactorx1000 = 1; |
staticParams.secondOrderGyroCorrectionDivisor = 2; |
staticParams.secondOrderGyroCorrectionLimit = 3; |
void gyro_init(void) { |
gyro_calibrate(); |
} |
void gyro_setDefaultParameters(void) { |
IMUConfig.driftCompDivider = 2; |
IMUConfig.driftCompLimit = 5; |
IMUConfig.zerothOrderCorrection = 1; |
IMUConfig.imuReversedFlags = IMU_REVERSE_ACC_Z; |
} |
/branches/dongfang_FC_fixedwing/main.c |
---|
8,19 → 8,14 |
#include "uart0.h" |
#include "output.h" |
#include "attitude.h" |
#include "commands.h" |
#include "flight.h" |
#include "controlMixer.h" |
#include "rc.h" |
#include "analog.h" |
#include "configuration.h" |
#include "twimaster.h" |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#include "controlMixer.h" |
#include "eeprom.h" |
#include "printf_P.h" |
int16_t main(void) { |
uint16_t timer; |
29,8 → 24,8 |
cli(); |
// analyze hardware environment |
CPUType = getCPUType(); |
BoardRelease = getBoardRelease(); |
setCPUType(); |
setBoardRelease(); |
// disable watchdog |
MCUSR &= ~(1 << WDRF); |
37,31 → 32,35 |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
// PPM_in[CH_THROTTLE] = 0; |
// Why??? They are already initialized to 0. |
// stickPitch = stickRoll = stickYaw = 0; |
// This is strange: It should NOT be necessarty to do. But the call of the same, |
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
channelMap_default(); |
RED_OFF; |
// initalize modules |
output_init(); |
timer0_init(); |
usart0_Init(); |
timer2_init(); |
usart0_init(); |
//if (CPUType == ATMEGA644P);// usart1_Init(); |
RC_Init(); |
analog_init(); |
I2C_init(); |
#ifdef USE_NAVICTRL |
SPI_MasterInit(); |
#endif |
#ifdef USE_MK3MAG |
MK3MAG_Init(); |
#endif |
// Parameter Set handling |
IMUConfig_readOrDefault(); |
channelMap_readOrDefault(); |
paramSet_readOrDefault(); |
// enable interrupts global |
sei(); |
// Parameter Set handling |
ParamSet_Init(); |
printf("\n\r==================================="); |
printf("\n\rFlightControl"); |
printf("\n\rHardware: Custom"); |
printf("\n\r CPU: Atmega644"); |
if (CPUType == ATMEGA644P) |
printf("p"); |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
69,75 → 68,86 |
// Instead, while away the time by flashing the 2 outputs: |
// First J16, then J17. Makes it easier to see which is which. |
timer = setDelay(500); |
OUTPUT_SET(0,1); |
timer = setDelay(200); |
outputSet(0,1); |
GRN_OFF; |
RED_ON; |
while (!checkDelay(timer)) |
; |
OUTPUT_SET(0,0); |
timer = setDelay(500); |
while (!checkDelay(timer)) |
; |
OUTPUT_SET(1,1); |
timer = setDelay(200); |
outputSet(0,0); |
outputSet(1,1); |
RED_OFF; |
GRN_ON; |
timer = setDelay(500); |
while (!checkDelay(timer)) |
; |
timer = setDelay(500); |
timer = setDelay(200); |
while (!checkDelay(timer)) |
; |
outputSet(1,0); |
GRN_OFF; |
OUTPUT_SET(1,0); |
printf("\n\r==================================="); |
beep(2000); |
#ifdef USE_NAVICTRL |
printf("\n\rSupport for NaviCtrl"); |
#endif |
timer = setDelay(4000); |
while (!checkDelay(timer)) |
; |
#ifdef USE_DIRECT_GPS |
printf("\n\rDirect (no NaviCtrl) navigation"); |
#endif |
controlMixer_setNeutral(); |
// Cal. attitude sensors and reset integrals. |
attitude_setNeutral(); |
// Init flight parameters |
flight_setNeutral(); |
// flight_setNeutral(); |
// RED_OFF; |
beep(2000); |
beep(1000); |
timer2_init(); |
printf("\n\rControl: "); |
if (staticParams.bitConfig & CFG_HEADING_HOLD) |
printf("Heading Hold"); |
else printf("RTL Mode"); |
I2CTimeout = 5000; |
printf("\n\n\r"); |
while (1) { |
while (1) { |
if (runFlightControl) { // control interval |
runFlightControl = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
if (!analogDataReady) { |
DebugOut.Digital[0] |= DEBUG_MAINLOOP_TIMER; |
// Analog data should have been ready but is not!! |
debugOut.digital[0] |= DEBUG_MAINLOOP_TIMER; |
} else { |
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
debugOut.digital[0] &= ~DEBUG_MAINLOOP_TIMER; |
J4HIGH; |
// This is probably the correct order: |
// The attitude computation should not depend on anything from control (except maybe the estimation of control activity level) |
// The control may depend on attitude - for example, attitude control uses pitch and roll angles, compass control uses yaw angle etc. |
// Flight control uses results from both. |
calculateFlightAttitude(); |
controlMixer_periodicTask(); |
commands_handleCommands(); |
flight_control(); |
J4LOW; |
// Allow Serial Data Transmit if motors must not updated or motors are not running |
if (!runFlightControl || !(MKFlags & MKFLAG_MOTOR_RUN)) { |
usart0_TransmitTxData(); |
usart0_transmitTxData(); |
} |
usart0_ProcessRxData(); |
usart0_processRxData(); |
if (checkDelay(timer)) { |
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(); |
} |
159,6 → 169,12 |
SPI_TransmitByte(); |
} |
#endif |
if (runFlightControl) { // Time for the next iteration was up before the current finished. Signal error. |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
} else { |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
} |
} |
} |
return (1); |
/branches/dongfang_FC_fixedwing/makefile |
---|
1,6 → 1,6 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644 |
MCU = atmega644p |
F_CPU = 20000000 |
QUARZ = 20MHZ |
13,24 → 13,16 |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
NC_SPI_COMPATIBLE = 6 # SPI Protocol Version |
GYRO=ENC-03_FC1.3 |
GYRO_HW_NAME=ENC |
GYRO_HW_FACTOR=1.304f |
GYRO_PITCHROLL_CORRECTION=1.0f |
GYRO_YAW_CORRECTION=1.0f |
# Set up for cross axis gyros. Factors increased by sqrt(2). |
#GYRO=ADXRS610_FC2.0 |
#GYRO_HW_NAME=ADXR |
#GYRO_HW_FACTOR="(1.2288f * 1.4142)" |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
#GYRO_CORRECTION=1.0f |
#GYRO=invenSense |
#GYRO_HW_NAME=Isense |
#GYRO_HW_FACTOR=0.6827f |
#GYRO_PITCHROLL_CORRECTION=1.0f |
#GYRO_YAW_CORRECTION=1.0f |
GYRO=invenSense |
GYRO_HW_NAME=Isense |
GYRO_HW_FACTOR=0.6827f |
GYRO_CORRECTION=1.0f |
#------------------------------------------------------------------- |
# get SVN revision |
62,8 → 54,8 |
########################################################################################################## |
# List C source files here. (C dependencies are automatically generated.) |
SRC = main.c uart0.c timer0.c timer2.c analog.c output.c controlMixer.c |
SRC += dongfangMath.c twimaster.c rc.c attitude.c flight.c |
SRC += eeprom.c configuration.c externalControl.c commands.c $(GYRO).c |
SRC += rc.c failsafeControl.c attitude.c flight.c printf_P.c commands.c |
SRC += eeprom.c configuration.c externalControl.c $(GYRO).c |
########################################################################################################## |
104,7 → 96,7 |
#CFLAGS += -std=c99 |
CFLAGS += -std=gnu99 |
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_PITCHROLL_CORRECTION=${GYRO_PITCHROLL_CORRECTION} -DGYRO_YAW_CORRECTION=${GYRO_YAW_CORRECTION} |
CFLAGS += -DF_CPU=$(F_CPU) -DVERSION_MAJOR=$(VERSION_MAJOR) -DVERSION_MINOR=$(VERSION_MINOR) -DVERSION_PATCH=$(VERSION_PATCH) -DVERSION_SERIAL_MAJOR=$(VERSION_SERIAL_MAJOR) -DVERSION_SERIAL_MINOR=$(VERSION_SERIAL_MINOR) -DNC_SPI_COMPATIBLE=$(NC_SPI_COMPATIBLE) -DGYRO_HW_NAME=${GYRO_HW_NAME} -DGYRO_HW_FACTOR=${GYRO_HW_FACTOR} -DGYRO_CORRECTION=${GYRO_CORRECTION} |
# Optional assembler flags. |
# -Wa,...: tell GCC to pass this to the assembler. |
358,7 → 350,7 |
clean_list : |
@echo |
@echo $(MSG_CLEANING) |
# $(REMOVE) $(TARGET).hex |
$(REMOVE) $(TARGET).hex |
$(REMOVE) $(TARGET).eep |
$(REMOVE) $(TARGET).obj |
$(REMOVE) $(TARGET).cof |
/branches/dongfang_FC_fixedwing/output.c |
---|
2,28 → 2,41 |
#include "output.h" |
#include "eeprom.h" |
#include "timer0.h" |
uint8_t flashCnt[2], flashMask[2]; |
// To access the DebugOut struct. |
#include "uart0.h" |
uint8_t flashCnt[2], flashMask[2]; |
// initializes the LED control outputs J16, J17 |
DebugOut_t debugOut; |
void output_init(void) { |
// set PC2 & PC3 as output (control of J16 & J17) |
DDRC |= (1 << DDC2) | (1 << DDC3); |
OUTPUT_SET(0,0); |
OUTPUT_SET(1,0); |
outputSet(0,0); |
outputSet(1,0); |
flashCnt[0] = flashCnt[1] = 0; |
flashMask[0] = flashMask[1] = 128; |
} |
void flashingLight(uint8_t port, uint8_t timing, uint8_t bitmask, |
uint8_t manual) { |
void outputSet(uint8_t num, uint8_t state) { |
if (staticParams.outputFlags & (OUTPUTFLAGS_INVERT_0 << num)) { |
if (state) OUTPUT_LOW(num) else OUTPUT_HIGH(num); |
} else { |
if (state) OUTPUT_HIGH(num) else OUTPUT_LOW(num); |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_USE_ONBOARD_LEDS) { |
if (num) { |
if (state) GRN_ON else GRN_OFF; |
} else { |
if (state) RED_ON else RED_OFF; |
} |
} |
} |
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. |
OUTPUT_SET(port, bitmask & 128); |
// "timing" is set to "manual (a variable)" and the value is very high --> Set to the value in bitmask bit 7. |
outputSet(port, 1); |
} 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. |
OUTPUT_SET(port, !(bitmask & 128)); |
// "timing" is set to "manual" (a variable) and the value is very low --> Set to the negated value in bitmask bit 7. |
outputSet(port, 0); |
} else if (!flashCnt[port]--) { |
// rotating mask over bitmask... |
flashCnt[port] = timing - 1; |
31,44 → 44,36 |
flashMask[port] = 128; |
else |
flashMask[port] >>= 1; |
OUTPUT_SET(port, flashMask[port] & bitmask); |
outputSet(port, flashMask[port] & bitmask); |
} |
} |
void flashingLights(void) { |
void output_update(void) { |
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); |
} |
} |
void output_update(void) { |
if (!DIGITAL_DEBUG_MASK) { |
// If there is a warning beep, also flash it. |
if (beepTime) { |
if (beepTime & beepModulation) { |
OUTPUT_SET(0, 1); |
OUTPUT_SET(1, 1); |
} else { |
OUTPUT_SET(0, 0); |
OUTPUT_SET(1, 0); |
} |
}else |
flashingLights(); |
} |
else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_ON) { |
OUTPUT_SET(0, 1); |
OUTPUT_SET(1, 1); |
} else if (DIGITAL_DEBUG_MASK == DEBUG_LEDTEST_OFF) { |
OUTPUT_SET(0, 0); |
OUTPUT_SET(1, 0); |
if (staticParams.outputFlags & OUTPUTFLAGS_TEST_ON) { |
outputSet(0, 1); |
outputSet(1, 1); |
} else if (staticParams.outputFlags & OUTPUTFLAGS_TEST_OFF) { |
outputSet(0, 0); |
outputSet(1, 0); |
} else { |
OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK); |
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK); |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_0_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
flashingLight(0, 25, 0x55, 25); |
} else if (staticParams.outputDebugMask) { |
outputSet(0, debugOut.digital[0] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(0, staticParams.outputFlash[0].timing, staticParams.outputFlash[0].bitmask, dynamicParams.output0Timing); |
} |
if (staticParams.outputFlags & OUTPUTFLAGS_FLASH_1_AT_BEEP && beepModulation != BEEP_MODULATION_NONE) { |
flashingLight(1, 25, 0x55, 25); |
} else if (staticParams.outputDebugMask) { |
outputSet(1, debugOut.digital[1] & staticParams.outputDebugMask); |
} else if (!delay) { |
flashingLight(1, staticParams.outputFlash[1].timing, staticParams.outputFlash[1].bitmask, dynamicParams.output1Timing); |
} |
} |
} |
93,27 → 98,17 |
* 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; |
} |
} |
/* |
* Beep the I2C bus error signal |
*/ |
void beepI2CAlarm(void) { |
if((beepModulation == 0xFFFF) && (MKFlags & MKFLAG_MOTOR_RUN)) { |
beepTime = 10000; // 1 second |
beepModulation = 0x0080; |
} |
} |
/* |
* Beep the battery low alarm signal |
*/ |
void beepBatteryAlarm(void) { |
beepModulation = 0x0300; |
beepModulation = BEEP_MODULATION_BATTERYALARM; |
if(!beepTime) { |
beepTime = 6000; // 0.6 seconds |
} |
123,7 → 118,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_fixedwing/output.h |
---|
16,11 → 16,10 |
#define J5TOGGLE PORTD ^= (1<<PORTD3) |
// invert means: An "1" bit in digital debug data make a LOW on the output. |
#define OUTPUT_INVERT 0 |
#define DIGITAL_DEBUG_INVERT 0 |
#define OUTPUT_HIGH(num) {PORTC |= (4 << (num));} |
#define OUTPUT_LOW(num) {PORTC &= ~(4 << (num));} |
#define OUTPUT_SET(num, state) {if (OUTPUT_INVERT) { if(state) OUTPUT_LOW(num) else OUTPUT_HIGH(num)} else {if(state) OUTPUT_HIGH(num) else OUTPUT_LOW(num)}} |
#define OUTPUT_TOGGLE(num) ( {PORTC ^= (4 << (num));} |
/* |
44,32 → 43,44 |
* Digital debugs may be added as desired, and removed when the mystery |
* at hand is resolved. |
*/ |
#define DEBUG_NONE 0 |
#define DEBUG_LEDTEST_ON 1000 |
#define DEBUG_LEDTEST_OFF 1001 |
#define DEBUG_MAINLOOP_TIMER 1 |
#define DEBUG_HEIGHT_DIFF 2 |
#define DEBUG_HOVERTHROTTLE 4 |
#define DEBUG_ACC0THORDER 8 |
#define DEBUG_COMMANDREPEATED 16 |
#define DEBUG_HEIGHT_DIFF 2 |
#define DEBUG_FLIGHTCLIP 4 |
#define DEBUG_ACC0THORDER 8 |
#define DEBUG_COMPASS 16 |
#define DEBUG_PRESSURERANGE 32 |
#define DEBUG_CLIP 64 |
#define DEBUG_SENSORLIMIT 128 |
#define DEBUG_CLIP 64 |
#define DEBUG_SENSORLIMIT 128 |
#define OUTPUTFLAGS_INVERT_0 1 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option) |
#define OUTPUTFLAGS_INVERT_1 2 // Inverted: 1 means low output on atmega. Does not affect on-board LED (if used with the OUTPUTOPTIONS_USE_ONBOARD_LEDS option) |
#define OUTPUTFLAGS_FLASH_0_AT_BEEP 4 // Flash LED when beeper beeps |
#define OUTPUTFLAGS_FLASH_1_AT_BEEP 8 // Flash LED when beeper beeps |
#define OUTPUTFLAGS_USE_ONBOARD_LEDS 16 // Control on-board LEDs in addition to outputs |
#define OUTPUTFLAGS_TEST_OFF 32 // For testing: Turn off both outputs |
#define OUTPUTFLAGS_TEST_ON 64 // For testing: Turn on both outputs |
// For practical reasons put here instead of in uart0.h |
typedef struct { |
uint8_t digital[2]; |
uint16_t analog[32]; // debug values |
}__attribute__((packed)) DebugOut_t; |
extern DebugOut_t debugOut; |
/* |
* 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_NONE |
#define DIGITAL_DEBUG_MASK 0 |
void output_init(void); |
void outputSet(uint8_t num, uint8_t state); |
void output_update(void); |
void beep(uint16_t millis); |
void beepNumber(uint8_t numbeeps); |
void beepRCAlarm(void); |
void beepI2CAlarm(void); |
void beepBatteryAlarm(void); |
#endif //_output_H |
/branches/dongfang_FC_fixedwing/rc.c |
---|
3,22 → 3,17 |
#include <avr/interrupt.h> |
#include "rc.h" |
#include "uart0.h" |
#include "controlMixer.h" |
#include "configuration.h" |
#include "commands.h" |
#include "output.h" |
// The channel array is 1-based. The 0th entry is not used. |
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile uint8_t NewPpmData = 1; |
volatile int16_t RC_Quality = 0; |
int16_t RC_PRTY[4]; |
volatile uint8_t RCQuality; |
uint8_t lastRCCommand = COMMAND_NONE; |
uint8_t commandTimer = 0; |
// Useless. Just trim on the R/C instead. |
// int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
/*************************************************************** |
* 16bit timer 1 is used to decode the PPM-Signal |
***************************************************************/ |
29,20 → 24,20 |
cli(); |
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
DDRD &= ~(1 << DDD6); |
PORTD |= (1 << PORTD6); |
DDRD &= ~(1<<6); |
PORTD |= (1<<PORTD6); |
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
// set as output |
DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3); |
DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3); |
// low level |
PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3)); |
PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
// PD3 can't be used if 2nd UART is activated |
// because TXD1 is at that port |
if (CPUType != ATMEGA644P) { |
DDRD |= (1 << PORTD3); |
PORTD &= ~(1 << PORTD3); |
DDRD |= (1<<PORTD3); |
PORTD &= ~(1<<PORTD3); |
} |
// Timer/Counter1 Control Register A, B, C |
52,23 → 47,21 |
// Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
// Enable input capture noise cancler (bit: ICNC1=1) |
// Trigger on positive edge of the input capture pin (bit: ICES1=1), |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
// Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s |
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) |
| (1 << WGM11) | (1 << WGM10)); |
TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10)); |
TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
// Timer/Counter1 Interrupt Mask Register |
// Enable Input Capture Interrupt (bit: ICIE1=1) |
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
// Enable Overflow Interrupt (bit: TOIE1=0) |
TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1)); |
TIMSK1 |= (1 << ICIE1); |
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
TIMSK1 |= (1<<ICIE1); |
RC_Quality = 0; |
RCQuality = 0; |
SREG = sreg; |
} |
77,26 → 70,25 |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
/* t-Frame |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
<-----------------------------------------------------------------------> |
____ ______ _____ ________ ______ sync gap ____ |
| | | | | | | | | | | |
| | | | | | | | | | | |
___| |_| |_| |_| |_.............| |________________| |
<-----><-------><------><--------> <------> <--- |
<-----><-------><------><----------- <------> <--- |
t0 t1 t2 t4 tn t0 |
The PPM-Frame length is 22.5 ms. |
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms. |
The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms. |
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
the syncronization gap. |
*/ |
ISR(TIMER1_CAPT_vect) |
{ // typical rate of 1 ms to 2 ms |
int16_t signal = 0; |
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
int16_t signal = 0, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
111,87 → 103,96 |
//sync gap? (3.52 ms < signal < 25.6 ms) |
if ((signal > 1100) && (signal < 8000)) { |
// if a sync gap happens and there where at least 4 channels decoded before |
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array. |
if (index >= 4) { |
NewPpmData = 0; // Null means NewData for the first 4 channels |
} |
// synchronize channel index |
index = 1; |
index = 0; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels |
if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
// check for valid signal length (0.8 ms < signal < 2.1984 ms) |
// signal range is from 1.0ms/3.2us = 312.5 to 2.0ms/3.2us = 625 |
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
if ((signal > 250) && (signal < 687)) { |
// shift signal to zero symmetric range -154 to 159 |
signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
// Signal is now in the +/- 156 range (nominally). |
signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
// check for stable signal |
if (abs(signal - PPM_in[index]) < 6) { |
if (RC_Quality < 200) |
RC_Quality += 10; |
if (RCQuality < 200) |
RCQuality += 10; |
else |
RC_Quality = 200; |
RCQuality = 200; |
} |
PPM_in[index] = signal; // update channel value |
// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
//} else { |
// tmp = PPM_in[index]; |
// } |
} else |
tmp = signal; |
PPM_in[index] = tmp; // update channel value |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
// channels are usually available at the receiver anyway. |
// if(index == 5) J3HIGH; else J3LOW; |
// if(index == 6) J4HIGH; else J4LOW; |
// if(CPUType != ATMEGA644P) // not used as TXD1 |
// { |
// if(index == 7) J5HIGH; else J5LOW; |
// } |
} |
} |
} |
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
#define COMMAND_THRESHOLD 85 |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
/* |
* This must be called (as the only thing) for each control loop cycle (488 Hz). |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
void RC_update() { |
if (RC_Quality) { |
RC_Quality--; |
if (NewPpmData-- == 0) { |
RC_PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * staticParams.StickElevatorP * 2/ 10; |
RC_PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * staticParams.StickAileronsP * 2 / 10; |
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * 2 + 310; |
if (RC_PRTY[CONTROL_THROTTLE] < 0) |
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
RC_PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * staticParams.StickRudderP * 2 / 10; |
void RC_periodicTaskAndPRTY(int16_t* PRTY) { |
if (RCQuality) { |
RCQuality--; |
PRTY[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
PRTY[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
PRTY[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
uint8_t command = COMMAND_NONE; //RC_getStickCommand(); |
if (lastRCCommand == command) { |
// Keep timer from overrunning. |
if (commandTimer < COMMAND_TIMER) |
commandTimer++; |
} else { |
// There was a change. |
lastRCCommand = command; |
commandTimer = 0; |
} |
} else { // Bad signal |
RC_PRTY[CONTROL_ELEVATOR] = RC_PRTY[CONTROL_AILERONS] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_RUDDER] = 0; |
} |
} // if RCQuality is no good, we just do nothing. |
} |
/* |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
int16_t* RC_getEATR(void) { |
return RC_PRTY; |
} |
/* |
* Get other channel value |
*/ |
int16_t RC_getVariable(uint8_t varNum) { |
if (varNum < 4) |
// 0th variable is 5th channel (1-based) etc. |
return RCChannel(varNum + 4) + POT_OFFSET; |
return RCChannel(varNum + CH_POTS) + POT_OFFSET; |
/* |
* Let's just say: |
* The RC variable 4 is hardwired to channel 5 |
* The RC variable 5 is hardwired to channel 6 |
* The RC variable 6 is hardwired to channel 7 |
* The RC variable 7 is hardwired to channel 8 |
* Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
* The RC variable i is hardwired to channel i, i>=4 |
*/ |
return PPM_in[varNum + 1] + POT_OFFSET; |
return PPM_in[varNum] + POT_OFFSET; |
} |
uint8_t RC_getSignalQuality(void) { |
if (RC_Quality >= 160) |
if (RCQuality >= 160) |
return SIGNAL_GOOD; |
if (RC_Quality >= 140) |
if (RCQuality >= 140) |
return SIGNAL_OK; |
if (RC_Quality >= 120) |
if (RCQuality >= 120) |
return SIGNAL_BAD; |
return SIGNAL_LOST; |
} |
209,19 → 210,12 |
// Do nothing. |
} |
/* |
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
// In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
stickOffsetPitch = stickOffsetRoll = 0; |
} else { |
stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
} |
} |
*/ |
uint8_t RC_getCommand(void) { |
// Noy impplemented - not from RC at least. |
if (commandTimer == COMMAND_TIMER) { |
// Stick has been held long enough; command committed. |
return lastRCCommand; |
} |
// Not yet sure what the command is. |
return COMMAND_NONE; |
} |
238,21 → 232,32 |
* |
* Not in any of these positions: 0 |
*/ |
#define ARGUMENT_THRESHOLD 70 |
#define ARGUMENT_CHANNEL_VERTICAL CH_ELEVATOR |
#define ARGUMENT_CHANNEL_HORIZONTAL CH_AILERONS |
uint8_t RC_getArgument(void) { |
return 0; |
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) { |
// vertical is up |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 2; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 4; |
return 3; |
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) { |
// vertical is down |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 8; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 6; |
return 7; |
} else { |
// vertical is around center |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD) |
return 1; |
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD) |
return 5; |
return 0; |
} |
} |
uint8_t RC_testCompassCalState(void) { |
return 0; |
} |
/* |
* Abstract controls are not used at the moment. |
t_control rc_control = { |
RC_getPitch, |
RC_getRoll, |
RC_getYaw, |
RC_getThrottle, |
RC_getSignalQuality, |
RC_calibrate |
}; |
*/ |
/branches/dongfang_FC_fixedwing/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 |
13,15 → 12,15 |
extern volatile int16_t PPM_in[MAX_CHANNELS]; |
// extern volatile int16_t PPM_diff[MAX_CHANNELS]; // the differentiated RC-Signal. Should that be exported?? |
extern volatile uint8_t NewPpmData; // 0 indicates a new recieved PPM Frame |
extern volatile int16_t RC_Quality; // rc signal quality indicator (0 to 200) |
extern volatile uint8_t RCQuality; // rc signal quality indicator (0 to 200) |
// defines for lookup staticParams.ChannelAssignment |
#define CH_ELEVATOR 0 |
#define CH_AILERONS 1 |
#define CH_THROTTLE 2 |
#define CH_RUDDER 3 |
#define CH_AILERONS 1 |
#define CH_THROTTLE 2 |
#define CH_RUDDER 3 |
#define CH_POTS 4 |
#define POT_OFFSET 115 |
#define POT_OFFSET 120 |
/* |
int16_t RC_getPitch (void); |
31,12 → 30,15 |
uint8_t RC_hasNewRCData (void); |
*/ |
void RC_update(void); |
int16_t* RC_getEATR(void); |
// void RC_periodicTask(void); |
void RC_periodicTaskAndPRTY(int16_t* PRTY); |
uint8_t RC_getArgument(void); |
uint8_t RC_getCommand(void); |
int16_t RC_getVariable(uint8_t varNum); |
void RC_calibrate(void); |
uint8_t RC_getSignalQuality(void); |
uint8_t RC_getLooping(uint8_t looping); |
#ifdef USE_MK3MAG |
uint8_t RC_testCompassCalState(void); |
#endif |
#endif //_RC_H |
/branches/dongfang_FC_fixedwing/sensors.h |
---|
2,21 → 2,11 |
#define _SENSORS_H |
#include <inttypes.h> |
#include "configuration.h" |
/* |
* Whether (pitch, roll, yaw) gyros are reversed (see analog.h). |
*/ |
extern const uint8_t GYRO_QUADRANT; |
extern const uint8_t YAW_GYRO_REVERSED; |
extern const uint8_t PR_GYROS_ORIENTATION_REVERSED; |
extern sensorOffset_t gyroAmplifierOffset; |
/* |
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h). |
*/ |
//extern const uint8_t ACC_QUADRANT; |
extern const uint8_t Z_ACC_REVERSED; |
/* |
* Common procedures for all gyro types. |
* FC 1.3 hardware: Searching the DAC values that return neutral readings. |
* FC 2.0 hardware: Nothing to do. |
25,8 → 15,13 |
void gyro_calibrate(void); |
/* |
* FC 1.3: Output data in gyroAmplifierOffset to DAC. All other versions: Do nothing. |
*/ |
void gyro_init(void); |
/* |
* Set some default FC parameters, depending on gyro type: Drift correction etc. |
*/ |
void gyro_setDefaults(void); |
void gyro_setDefaultParameters(void); |
#endif |
/branches/dongfang_FC_fixedwing/timer0.c |
---|
3,9 → 3,9 |
#include <avr/interrupt.h> |
#include "eeprom.h" |
#include "analog.h" |
#include "controlMixer.h" |
// for debugging! |
#include "uart0.h" |
#include "timer0.h" |
#include "output.h" |
#ifdef USE_MK3MAG |
12,11 → 12,10 |
#include "mk3mag.h" |
#endif |
volatile uint16_t millisecondsCount = 0; |
volatile uint32_t globalMillisClock = 0; |
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; |
34,8 → 33,7 |
cli(); |
// 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 |
56,10 → 54,9 |
TCCR0A |= (1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00); |
// Timer/Counter 0 Control Register B |
// set clock divider for timer 0 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz |
// set clock divider for timer 0 to SYSCLOCK/8 = 20MHz/8 = 2.5MHz |
// i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz |
// hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
// hence the timer overflow interrupt frequency is 2.5 MHz/256 = 9.765 kHz |
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0) |
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02)); |
83,10 → 80,9 |
/*****************************************************/ |
/* Interrupt Routine of Timer 0 */ |
/*****************************************************/ |
ISR(TIMER0_OVF_vect) |
{ // 9765.625 Hz |
ISR(TIMER0_OVF_vect) { // 9765.625 Hz |
static uint8_t cnt_1ms = 1, cnt = 0; |
uint8_t beeper_On = 0; |
uint8_t beeperOn = 0; |
#ifdef USE_NAVICTRL |
if(SendSPI) SendSPI--; // if SendSPI is 0, the transmit of a byte via SPI bus to and from The Navicontrol is done |
97,12 → 93,12 |
cnt_1ms ^= 1; |
if (!cnt_1ms) { |
if (runFlightControl == 1) |
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER; |
debugOut.digital[1] |= DEBUG_MAINLOOP_TIMER; |
else |
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
debugOut.digital[1] &= ~DEBUG_MAINLOOP_TIMER; |
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz) |
} |
millisecondsCount++; // increment millisecond counter |
globalMillisClock++; // increment millisecond counter |
} |
// beeper on if duration is not over |
109,35 → 105,32 |
if (beepTime) { |
beepTime--; // decrement BeepTime |
if (beepTime & beepModulation) |
beeper_On = 1; |
beeperOn = 1; |
else |
beeper_On = 0; |
beeperOn = 0; |
} else { // beeper off if duration is over |
beeper_On = 0; |
beepModulation = 0xFFFF; |
beeperOn = 0; |
beepModulation = BEEP_MODULATION_NONE; |
} |
// if beeper is on |
if (beeper_On) { |
if (beeperOn) { |
// 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 |
} |
#ifndef USE_NAVICTRL |
#ifdef USE_MK3MAG |
// update compass value if this option is enabled in the settings |
if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
#ifdef USE_MK3MAG |
MK3MAG_Update(); // read out mk3mag pwm |
#endif |
if (staticParams.bitConfig & CFG_COMPASS_ENABLED) { |
MK3MAG_periodicTask(); // read out mk3mag pwm |
} |
#endif |
} |
144,12 → 137,12 |
// ----------------------------------------------------------------------- |
uint16_t setDelay(uint16_t t) { |
return (millisecondsCount + t - 1); |
return (globalMillisClock + t - 1); |
} |
// ----------------------------------------------------------------------- |
int8_t checkDelay(uint16_t t) { |
return (((t - millisecondsCount) & 0x8000) >> 8); // check sign bit |
return (((t - globalMillisClock) & 0x8000) >> 8); // check sign bit |
} |
// ----------------------------------------------------------------------- |
160,13 → 153,18 |
} |
// ----------------------------------------------------------------------- |
void delay_ms_Mess(uint16_t w) { |
void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) { |
uint16_t t_stop; |
t_stop = setDelay(w); |
while (!checkDelay(t_stop)) { |
if (analogDataReady) { |
analogDataReady = 0; |
analog_start(); |
} |
if (analogDataReady) { |
analog_update(); |
startAnalogConversionCycle(); |
} |
} |
if (stop) { |
// Wait for new samples to get prepared but do not restart AD conversion after that! |
// Caller MUST to that. |
while (!analogDataReady); |
} |
} |
/branches/dongfang_FC_fixedwing/timer0.h |
---|
3,9 → 3,20 |
#include <inttypes.h> |
extern volatile uint16_t millisecondsCount; |
// Normally it is 20MHz/2048 = 9765.625 Hz |
#define F_TIMER0IRQ ((float)F_CPU/2048.0) |
#define MAINLOOP_DIVIDER 10 |
// Originally it is 488 |
#define F_MAINLOOP (F_TIMER0IRQ/(2.0*MAINLOOP_DIVIDER)) |
#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 uint32_t globalMillisClock; |
extern volatile uint8_t runFlightControl; |
extern volatile uint16_t cntKompass; |
extern volatile uint16_t beepModulation; |
extern volatile uint16_t beepTime; |
#ifdef USE_NAVICTRL |
14,7 → 25,7 |
extern void timer0_init(void); |
extern void delay_ms(uint16_t w); |
extern void delay_ms_Mess(uint16_t w); |
extern void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop); |
extern uint16_t setDelay(uint16_t t); |
extern int8_t checkDelay(uint16_t t); |
/branches/dongfang_FC_fixedwing/timer2.c |
---|
1,119 → 1,202 |
#include <avr/io.h> |
#include <avr/interrupt.h> |
#include "timer2.h" |
#include "eeprom.h" |
#include "uart0.h" |
#include "rc.h" |
#include "attitude.h" |
#define HEF4017_RESET_HIGH PORTC |= (1<<PORTC6) |
#define HEF4017_RESET_LOW PORTC &= ~(1<<PORTC6) |
#define COARSERESOLUTION 1 |
#define HEF4017R_ON PORTC |= (1<<PORTC6) |
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
#ifdef COARSERESOLUTION |
#define NEUTRAL_PULSELENGTH 938 |
#define STABILIZATION_LOG_DIVIDER 6 |
#define SERVOLIMIT 500 |
#define SCALE_FACTOR 4 |
#define CS2 ((1<<CS21)|(1<<CS20)) |
OutputData_t outputs[MAX_OUTPUTS]; |
#else |
#define NEUTRAL_PULSELENGTH 3750 |
#define STABILIZATION_LOG_DIVIDER 4 |
#define SERVOLIMIT 2000 |
#define SCALE_FACTOR 16 |
#define CS2 (1<<CS21) |
#endif |
#define MAX_SERVOS 8 |
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
//volatile uint8_t servoActive = 0; |
volatile uint8_t recalculateServoTimes = 0; |
volatile uint16_t servoValues[MAX_SERVOS]; |
volatile uint16_t previousManualValues[2]; |
#define HEF4017R_ON PORTC |= (1<<PORTC6) |
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
/***************************************************** |
* Initialize Timer 2 |
* Initialize Timer 2 |
*****************************************************/ |
void timer2_init(void) { |
uint8_t sreg = SREG; |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for pitch servo |
DDRD |= (1 << DDD7); |
PORTD &= ~(1 << PORTD7); // set PD7 to low |
DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
//PORTC &= ~(1<<PORTC6); // set PC6 to low |
HEF4017_RESET_HIGH; // enable reset |
// Timer/Counter 2 Control Register A |
// Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
// PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0)); |
TCCR2A |= (1 << WGM21); |
// Timer/Counter 2 Control Register B |
// Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
// The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
// divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS22)); |
TCCR2B |= (1 << CS21) | (1 << CS20); |
// Initialize the Timer/Counter 2 Register |
TCNT2 = 0; |
// Initialize the Output Compare Register A used for signal generation on port PD7. |
OCR2A = 255; |
TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0 |
// Timer/Counter 2 Interrupt Mask Register |
// Enable timer output compare match A Interrupt only |
TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
TIMSK2 |= (1 << OCIE2A); |
uint8_t sreg = SREG; |
SREG = sreg; |
// disable all interrupts before reconfiguration |
cli(); |
// set PD7 as output of the PWM for pitch servo |
DDRD |= (1 << DDD7); |
PORTD &= ~(1 << PORTD7); // set PD7 to low |
DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
HEF4017R_ON; // enable reset |
// Timer/Counter 2 Control Register A |
// Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
// PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
// PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
// Timer/Counter 2 Control Register B |
// Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
// The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
// hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
TCCR2B |= CS2; |
// Initialize the Timer/Counter 2 Register |
TCNT2 = 0; |
// Initialize the Output Compare Register A used for signal generation on port PD7. |
OCR2A = 255; |
// Timer/Counter 2 Interrupt Mask Register |
// Enable timer output compare match A Interrupt only |
TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
TIMSK2 |= (1 << OCIE2A); |
for (uint8_t axis=0; axis<2; axis++) |
previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
SREG = sreg; |
} |
/* |
void servo_On(void) { |
servoActive = 1; |
} |
void servo_Off(void) { |
servoActive = 0; |
HEF4017R_ON; // enable reset |
} |
*/ |
/***************************************************** |
* Control Servo Position |
* Control Servo Position |
*****************************************************/ |
const uint8_t SERVO_REMAPPING[MAX_OUTPUTS] = {0,0,1,2,3,4,5,6}; |
#define NEUTRAL_PULSELENGTH 937 |
#define SERVOLIMIT 500 |
#define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.ServoRefresh + 128) |
#define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
#define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
/*typedef struct { |
uint8_t manualControl; |
uint8_t compensationFactor; |
uint8_t minValue; |
uint8_t maxValue; |
uint8_t flags; |
} servo_t;*/ |
int16_t calculateStabilizedServoAxis(uint8_t axis) { |
int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
// With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
// That is a divisor of about 1<<14. Same conclusion as H&I. |
value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
value = value >> 8; |
if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
return -value; |
return value; |
} |
// With constant-speed limitation. |
uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
int16_t diff = manualValue - previousManualValues[axis]; |
uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
if (diff > maxSpeed) diff = maxSpeed; |
else if (diff < -maxSpeed) diff = -maxSpeed; |
manualValue = previousManualValues[axis] + diff; |
previousManualValues[axis] = manualValue; |
return manualValue; |
} |
// add stabilization and manual, apply soft position limits. |
// All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
int16_t featuredServoValue(uint8_t axis) { |
int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
value += calculateStabilizedServoAxis(axis); |
int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
if (value < limit) value = limit; |
limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
if (value > limit) value = limit; |
return value; |
} |
uint16_t servoValue(uint8_t axis) { |
int16_t value; |
if (axis<2) value = featuredServoValue(axis); |
else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
// Shift out of the [0..255*SCALE_FACTOR] space |
value -= (128 * SCALE_FACTOR); |
if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
else if (value > SERVOLIMIT) value = SERVOLIMIT; |
// Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
return value + NEUTRAL_PULSELENGTH; |
} |
void calculateServoValues(void) { |
if (!recalculateServoTimes) return; |
for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
servoValues[axis] = servoValue(axis); |
} |
recalculateServoTimes = 0; |
} |
ISR(TIMER2_COMPA_vect) { |
static uint16_t remainingPulseTime = 0; |
static uint16_t remainingPulseTime; |
static uint8_t servoIndex = 0; |
static uint16_t sumOfPulseTimes = 0; |
if (!remainingPulseTime) { |
// Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
if (servoIndex < staticParams.ServoRefresh) { |
if (servoIndex < staticParams.servoCount) { |
// There are more signals to output. |
remainingPulseTime = NEUTRAL_PULSELENGTH + outputs[SERVO_REMAPPING[servoIndex]].SetPoint; |
if (remainingPulseTime < MIN_PULSELENGTH) remainingPulseTime = MIN_PULSELENGTH; |
else if (remainingPulseTime > MAX_PULSELENGTH) remainingPulseTime = MAX_PULSELENGTH; |
sumOfPulseTimes += remainingPulseTime; |
sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
servoIndex++; |
} else { |
// There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
sumOfPulseTimes = servoIndex = 0; |
HEF4017_RESET_HIGH; |
recalculateServoTimes = 1; |
HEF4017R_ON; |
} |
} |
// Schedule the next OCR2A event. The counter is already reset at this time. |
uint8_t delta; |
if (remainingPulseTime > 255+128) { |
delta = 255; |
// Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
if (remainingPulseTime > 256+128) { |
// Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
// as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
OCR2A = 255; |
TCCR2A &= ~(1<<COM2A0); |
} else if (remainingPulseTime > 255) { |
// Remaining pulse lengths in the range [256..256+something small] might cause trouble if handled the standard |
// way, which is in chunks of 255. The remainder would be very small, possibly causing an interrupt on interrupt |
remainingPulseTime-=256; |
} else if (remainingPulseTime > 256) { |
// Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
// way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
// condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
delta = 128; |
remainingPulseTime-=128; |
OCR2A=127; |
} else { |
delta = remainingPulseTime; |
// Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
TCCR2A |= (1<<COM2A0); |
HEF4017_RESET_LOW; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
OCR2A = remainingPulseTime-1; |
remainingPulseTime=0; |
HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
} |
OCR2A = delta; |
remainingPulseTime -= delta; |
} |
/branches/dongfang_FC_fixedwing/timer2.h |
---|
3,14 → 3,9 |
#include <inttypes.h> |
extern volatile int16_t ServoNickValue; |
extern volatile int16_t ServoRollValue; |
void timer2_init(void); |
typedef struct { |
uint16_t SetPoint; // written by attitude controller |
}__attribute__((packed)) OutputData_t; |
#define MAX_OUTPUTS 8 |
extern OutputData_t outputs[MAX_OUTPUTS]; |
#endif //_TIMER2_H |
/branches/dongfang_FC_fixedwing/uart0.c |
---|
12,12 → 12,8 |
#include "externalControl.h" |
#include "output.h" |
#include "attitude.h" |
#include "commands.h" |
#ifdef USE_MK3MAG |
#include "mk3mag.h" |
#endif |
#define FC_ADDRESS 1 |
#define NC_ADDRESS 2 |
#define MK3MAG_ADDRESS 3 |
24,47 → 20,62 |
#define FALSE 0 |
#define TRUE 1 |
//int8_t test __attribute__ ((section (".noinit"))); |
uint8_t request_VerInfo = FALSE; |
uint8_t request_ExternalControl = FALSE; |
uint8_t request_Display = FALSE; |
uint8_t request_Display1 = FALSE; |
uint8_t request_DebugData = FALSE; |
uint8_t request_Data3D = FALSE; |
uint8_t request_DebugLabel = 255; |
int8_t displayBuff[DISPLAYBUFFSIZE]; |
uint8_t dispPtr = 0; |
uint8_t requestedDebugLabel = 255; |
uint8_t request_verInfo = FALSE; |
uint8_t request_externalControl = FALSE; |
uint8_t request_debugData = FALSE; |
uint8_t request_data3D = FALSE; |
uint8_t request_PPMChannels = FALSE; |
uint8_t request_OutputTest = FALSE; |
uint8_t request_servoTest = FALSE; |
uint8_t request_variables = FALSE; |
uint8_t request_OSD = FALSE; |
uint8_t DisplayLine = 0; |
/* |
#define request_verInfo (1<<0) |
#define request_externalControl (1<<1) |
#define request_display (1<<3) |
#define request_display1 (1<<4) |
#define request_debugData (1<<5) |
#define request_data3D (1<<6) |
#define request_PPMChannels (1<<7) |
#define request_motorTest (1<<8) |
#define request_variables (1<<9) |
#define request_OSD (1<<10) |
*/ |
//uint16_t request = 0; |
volatile uint8_t txd_buffer[TXD_BUFFER_LEN]; |
volatile uint8_t rxd_buffer_locked = FALSE; |
volatile uint8_t rxd_buffer[RXD_BUFFER_LEN]; |
volatile uint8_t txd_complete = TRUE; |
volatile uint8_t ReceivedBytes = 0; |
volatile uint8_t receivedBytes = 0; |
volatile uint8_t *pRxData = 0; |
volatile uint8_t RxDataLen = 0; |
volatile uint8_t rxDataLen = 0; |
uint8_t outputTestActive = 0; |
uint8_t outputTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
uint8_t ConfirmFrame; |
uint8_t servoTestActive = 0; |
uint8_t servoTest[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
uint8_t confirmFrame; |
typedef struct { |
int16_t Heading; |
int16_t heading; |
}__attribute__((packed)) Heading_t; |
DebugOut_t DebugOut; |
Data3D_t Data3D; |
UART_VersionInfo_t UART_VersionInfo; |
Data3D_t data3D; |
uint16_t DebugData_Timer; |
uint16_t Data3D_Timer; |
uint16_t DebugData_Interval = 500; // in 1ms |
uint16_t Data3D_Interval = 0; // in 1ms |
uint16_t debugData_timer; |
uint16_t data3D_timer; |
uint16_t OSD_timer; |
uint16_t debugData_interval = 0; // in 1ms |
uint16_t data3D_interval = 0; // in 1ms |
uint16_t OSD_interval = 0; |
#ifdef USE_MK3MAG |
int16_t Compass_Timer; |
#ifdef USE_DIRECT_GPS |
int16_t toMk3MagTimer; |
#endif |
// keep lables in flash to save 512 bytes of sram space |
73,40 → 84,41 |
"AnglePitch ", //0 |
"AngleRoll ", |
"AngleYaw ", |
"GyroPitch(PID) ", |
"GyroRoll(PID) ", |
"GyroPitch ", |
"GyroRoll ", |
"GyroYaw ", //5 |
"GyroFactorPitch ", |
"GyroFactorRoll ", |
"GyroFactorYaw ", |
"GyroDPitch ", |
"GyroDRoll ",//10 |
"GyroDYaw ", |
"Pitch Term ", |
"Roll Term ", |
"Throttle Term ", |
"Yaw Term ", //15 |
"0th O Corr pitch", |
"0th O Corr roll ", |
"ControlIntePitch", |
"ControlInteRoll ", |
"UBat ", //20 |
"hoverThrottle ", |
"IPartPitch ", // OK |
"IPartRoll ", |
"S3 (THROTTLE) ", |
"S4 (RUDDER) ", // OK //25 |
"ControlYaw ", |
"Airpress. Range ", // OK |
"DriftCompPitch ", |
"DriftCompRoll ", |
"AirpressFiltered", // MISSING //30 |
"AirpressADC " }; |
"PitchTerm ", |
"RollTerm ", |
"ThrottleTerm ", |
"YawTerm ", |
"heightP ", //10 |
"heightI ", |
"heightD ", |
"gyroActivity ", |
"ca ", |
"GActivityDivider", //15 |
"NaviMode ", |
"NaviStatus ", |
"NaviStickP ", |
"NaviStickR ", |
"control act wghd", //20 |
"acc vector wghd ", |
"Height[dm] ", |
"dHeight ", |
"acc vector ", |
"EFT ", //25 |
"naviPitch ", |
"naviRoll ", |
"tolerance ", |
"Gyro Act Cont. ", |
"GPS altitude ", //30 |
"GPS vert accura " |
}; |
/****************************************************************/ |
/* Initialization of the USART0 */ |
/****************************************************************/ |
void usart0_Init(void) { |
void usart0_init(void) { |
uint8_t sreg = SREG; |
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1); |
161,22 → 173,26 |
UCSR0B |= (1 << TXCIE0); |
// initialize the debug timer |
DebugData_Timer = setDelay(DebugData_Interval); |
debugData_timer = setDelay(debugData_interval); |
// unlock rxd_buffer |
rxd_buffer_locked = FALSE; |
pRxData = 0; |
RxDataLen = 0; |
rxDataLen = 0; |
// no bytes to send |
txd_complete = TRUE; |
UART_VersionInfo.SWMajor = VERSION_MAJOR; |
UART_VersionInfo.SWMinor = VERSION_MINOR; |
UART_VersionInfo.SWPatch = VERSION_PATCH; |
UART_VersionInfo.ProtoMajor = VERSION_SERIAL_MAJOR; |
UART_VersionInfo.ProtoMinor = VERSION_SERIAL_MINOR; |
#ifdef USE_DIRECT_GPS |
toMk3MagTimer = setDelay(220); |
#endif |
versionInfo.SWMajor = VERSION_MAJOR; |
versionInfo.SWMinor = VERSION_MINOR; |
versionInfo.SWPatch = VERSION_PATCH; |
versionInfo.protoMajor = VERSION_SERIAL_MAJOR; |
versionInfo.protoMinor = VERSION_SERIAL_MINOR; |
// restore global interrupt flags |
SREG = sreg; |
} |
184,8 → 200,7 |
/****************************************************************/ |
/* USART0 transmitter ISR */ |
/****************************************************************/ |
ISR(USART0_TX_vect) |
{ |
ISR(USART0_TX_vect) { |
static uint16_t ptr_txd_buffer = 0; |
uint8_t tmp_tx; |
if (!txd_complete) { // transmission not completed |
206,11 → 221,10 |
/****************************************************************/ |
/* USART0 receiver ISR */ |
/****************************************************************/ |
ISR(USART0_RX_vect) |
{ |
static uint16_t crc; |
ISR(USART0_RX_vect) { |
static uint16_t checksum; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t checksum1, checksum2; |
uint8_t c; |
c = UDR0; // catch the received byte |
221,33 → 235,27 |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
checksum = c; // init checksum |
} |
#if 0 |
else if (ptr_rxd_buffer == 1) { // handle address |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
} |
#endif |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
if (c != '\r') { // no termination character |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
checksum += c; // update checksum |
} else { // termination character was received |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer - 2]; |
crc -= rxd_buffer[ptr_rxd_buffer - 1]; |
checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
checksum %= 4096; |
checksum1 = '=' + checksum / 64; |
checksum2 = '=' + checksum % 64; |
// compare checksum to transmitted checksum bytes |
if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 |
if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
== rxd_buffer[ptr_rxd_buffer - 1])) { |
// checksum valid |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
ReceivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
receivedBytes = ptr_rxd_buffer + 1;// store number of received bytes |
rxd_buffer_locked = TRUE; // lock the rxd buffer |
// if 2nd byte is an 'R' enable watchdog that will result in an reset |
if (rxd_buffer[2] == 'R') { |
265,14 → 273,14 |
} |
// -------------------------------------------------------------------------- |
void AddCRC(uint16_t datalen) { |
uint16_t tmpCRC = 0, i; |
void addChecksum(uint16_t datalen) { |
uint16_t tmpchecksum = 0, i; |
for (i = 0; i < datalen; i++) { |
tmpCRC += txd_buffer[i]; |
tmpchecksum += txd_buffer[i]; |
} |
tmpCRC %= 4096; |
txd_buffer[i++] = '=' + tmpCRC / 64; |
txd_buffer[i++] = '=' + tmpCRC % 64; |
tmpchecksum %= 4096; |
txd_buffer[i++] = '=' + (tmpchecksum >> 6); |
txd_buffer[i++] = '=' + (tmpchecksum & 0x3F); |
txd_buffer[i++] = '\r'; |
txd_complete = FALSE; |
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
280,9 → 288,9 |
// -------------------------------------------------------------------------- |
// application example: |
// SendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
// sendOutData('A', FC_ADDRESS, 2, (uint8_t *)&request_DebugLabel, sizeof(request_DebugLabel), label, 16); |
/* |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t txd_bufferIndex = 0; |
uint8_t *currentBuffer; |
325,11 → 333,11 |
txd_buffer[txd_bufferIndex] = 0; |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
Addchecksum(pt); // add checksum after data block and initates the transmission |
} |
*/ |
void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
void sendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) { // uint8_t *pdata, uint8_t len, ... |
va_list ap; |
uint16_t pt = 0; |
uint8_t a, b, c; |
391,16 → 399,16 |
txd_buffer[pt++] = '=' + (c & 0x3f); |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
addChecksum(pt); // add checksum after data block and initates the transmission |
} |
// -------------------------------------------------------------------------- |
void Decode64(void) { |
void decode64(void) { |
uint8_t a, b, c, d; |
uint8_t x, y, z; |
uint8_t ptrIn = 3; |
uint8_t ptrOut = 3; |
uint8_t len = ReceivedBytes - 6; |
uint8_t len = receivedBytes - 6; |
while (len) { |
a = rxd_buffer[ptrIn++] - '='; |
427,66 → 435,72 |
break; |
} |
pRxData = &rxd_buffer[3]; |
RxDataLen = ptrOut - 3; |
rxDataLen = ptrOut - 3; |
} |
// -------------------------------------------------------------------------- |
void usart0_ProcessRxData(void) { |
// We control the outputTestActive var from here: Count it down. |
if (outputTestActive) |
outputTestActive--; |
void usart0_processRxData(void) { |
// We control the servoTestActive var from here: Count it down. |
if (servoTestActive) |
servoTestActive--; |
// if data in the rxd buffer are not locked immediately return |
if (!rxd_buffer_locked) |
return; |
uint8_t tempchar1, tempchar2; |
Decode64(); // decode data block in rxd_buffer |
uint8_t tempchar[3]; |
decode64(); // decode data block in rxd_buffer |
switch (rxd_buffer[1] - 'a') { |
case FC_ADDRESS: |
switch (rxd_buffer[2]) { |
#ifdef USE_MK3MAG |
#ifdef USE_DIRECT_GPS |
case 'K':// compass value |
compassHeading = ((Heading_t *)pRxData)->Heading; |
// What is the point of this - the compass will overwrite this soon? |
magneticHeading = ((Heading_t *)pRxData)->heading; |
// compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
break; |
#endif |
case 't': // motor test |
if (RxDataLen > 20) { |
memcpy(&outputTest[0], (uint8_t*) pRxData, sizeof(outputTest)); |
if (rxDataLen > 20) { |
memcpy(&servoTest[0], (uint8_t*) pRxData, sizeof(servoTest)); |
} else { |
memcpy(&outputTest[0], (uint8_t*) pRxData, 4); |
memcpy(&servoTest[0], (uint8_t*) pRxData, 4); |
} |
outputTestActive = 255; |
servoTestActive = 255; |
externalControlActive = 255; |
break; |
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)); |
break; |
case 'm':// "Set Mixer Table |
if (pRxData[0] == EEMIXER_REVISION) { |
memcpy(&Mixer, (uint8_t*) pRxData, sizeof(Mixer)); |
MixerTable_WriteToEEProm(); |
while (!txd_complete) |
; // wait for previous frame to be sent |
tempchar1 = 1; |
} else { |
tempchar1 = 0; |
} |
SendOutData('M', FC_ADDRESS, 1, &tempchar1, 1); |
break; |
case 'p': // get PPM channels |
request_PPMChannels = TRUE; |
break; |
case 'i':// Read IMU configuration |
tempchar[0] = IMUCONFIG_REVISION; |
tempchar[1] = sizeof(IMUConfig); |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('I', FC_ADDRESS, 2, &tempchar, 2, (uint8_t *) &IMUConfig, sizeof(IMUConfig)); |
break; |
case 'j':// Save IMU configuration |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
{ |
if ((pRxData[0] == IMUCONFIG_REVISION) && (pRxData[1] == sizeof(IMUConfig))) { |
memcpy(&IMUConfig, (uint8_t*) &pRxData[2], sizeof(IMUConfig)); |
IMUConfig_writeToEEprom(); |
tempchar[0] = 1; //indicate ok data |
} else { |
tempchar[0] = 0; //indicate bad data |
} |
while (!txd_complete) |
; // wait for previous frame to be sent |
sendOutData('J', FC_ADDRESS, 1, &tempchar, 1); |
} |
break; |
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) |
494,31 → 508,33 |
else if (pRxData[0] > 5) |
pRxData[0] = 5; // limit to 5 |
// load requested parameter set |
ParamSet_ReadFromEEProm(); |
tempchar1 = pRxData[0]; |
tempchar2 = EEPARAM_REVISION; |
paramSet_readFromEEProm(pRxData[0]); |
tempchar[0] = pRxData[0]; |
tempchar[1] = EEPARAM_REVISION; |
tempchar[2] = sizeof(staticParams); |
while (!txd_complete) |
; // wait for previous frame to be sent |
SendOutData('Q', FC_ADDRESS, 3, &tempchar1, sizeof(tempchar1), |
&tempchar2, sizeof(tempchar2), (uint8_t *) &staticParams, |
sizeof(staticParams)); |
sendOutData('Q', FC_ADDRESS, 2, &tempchar, 3, (uint8_t *) &staticParams, sizeof(staticParams)); |
break; |
case 's': // save settings |
if (!(MKFlags & MKFLAG_MOTOR_RUN)) // save settings only if motors are off |
{ |
if ((1 <= pRxData[0]) && (pRxData[0] <= 5) && (pRxData[1] |
== EEPARAM_REVISION)) // check for setting to be in range and version of settings |
if ((pRxData[1] == EEPARAM_REVISION) && (pRxData[2] == sizeof(staticParams))) // check for setting to be in range and version of settings |
{ |
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams)); |
ParamSet_WriteToEEProm(); |
beepNumber(1); |
memcpy(&staticParams, (uint8_t*) &pRxData[3], sizeof(staticParams)); |
paramSet_writeToEEProm(1); |
configuration_paramSetDidChange(); |
tempchar[0] = 1; |
beepNumber(tempchar[0]); |
} else { |
tempchar1 = 0; //indicate bad data |
tempchar[0] = 0; //indicate bad data |
} |
while (!txd_complete) |
; // wait for previous frame to be sent |
SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1)); |
sendOutData('S', FC_ADDRESS, 1, &tempchar, 1); |
} |
break; |
530,30 → 546,25 |
default: // any Slave Address |
switch (rxd_buffer[2]) { |
case 'a':// request for labels of the analog debug outputs |
request_DebugLabel = pRxData[0]; |
if (request_DebugLabel > 31) |
request_DebugLabel = 31; |
externalControlActive = 255; |
requestedDebugLabel = pRxData[0]; |
if (requestedDebugLabel > 31) |
requestedDebugLabel = 31; |
break; |
case 'b': // submit extern control |
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl)); |
ConfirmFrame = externalControl.frame; |
confirmFrame = externalControl.frame; |
externalControlActive = 255; |
break; |
case 'h':// request for display columns |
externalControlActive = 255; |
request_Display = TRUE; |
break; |
case 'l':// request for display columns |
externalControlActive = 255; |
request_Display1 = TRUE; |
break; |
case 'o':// request for OSD data (FC style) |
OSD_interval = (uint16_t) pRxData[0] * 10; |
if (OSD_interval > 0) |
request_OSD = TRUE; |
break; |
case 'v': // request for version and board release |
request_VerInfo = TRUE; |
request_verInfo = TRUE; |
break; |
case 'x': |
560,20 → 571,20 |
request_variables = TRUE; |
break; |
case 'g':// get external control data |
request_ExternalControl = TRUE; |
case 'g':// get external control data |
request_externalControl = TRUE; |
break; |
case 'd': // request for the debug data |
DebugData_Interval = (uint16_t) pRxData[0] * 10; |
if (DebugData_Interval > 0) |
request_DebugData = TRUE; |
debugData_interval = (uint16_t) pRxData[0] * 10; |
if (debugData_interval > 0) |
request_debugData = TRUE; |
break; |
case 'c': // request for the 3D data |
Data3D_Interval = (uint16_t) pRxData[0] * 10; |
if (Data3D_Interval > 0) |
request_Data3D = TRUE; |
data3D_interval = (uint16_t) pRxData[0] * 10; |
if (data3D_interval > 0) |
request_data3D = TRUE; |
break; |
default: |
584,12 → 595,12 |
} |
// unlock the rxd buffer after processing |
pRxData = 0; |
RxDataLen = 0; |
rxDataLen = 0; |
rxd_buffer_locked = FALSE; |
} |
/************************************************************************/ |
/* Routine für die Serielle Ausgabe */ |
/* Routine f�r die Serielle Ausgabe */ |
/************************************************************************/ |
int16_t uart_putchar(int8_t c) { |
if (c == '\n') |
602,97 → 613,74 |
} |
//--------------------------------------------------------------------------------------------- |
void usart0_TransmitTxData(void) { |
void usart0_transmitTxData(void) { |
if (!txd_complete) |
return; |
if (request_VerInfo && txd_complete) { |
SendOutData('V', FC_ADDRESS, 1, (uint8_t *) &UART_VersionInfo, |
sizeof(UART_VersionInfo)); |
request_VerInfo = FALSE; |
if (request_verInfo && txd_complete) { |
sendOutData('V', FC_ADDRESS, 1, (uint8_t *) &versionInfo, sizeof(versionInfo)); |
request_verInfo = FALSE; |
} |
if (request_Display && txd_complete) { |
//LCD_PrintMenu(); |
SendOutData('H', FC_ADDRESS, 0); |
DisplayLine++; |
if (DisplayLine >= 4) |
DisplayLine = 0; |
request_Display = FALSE; |
} |
if (request_Display1 && txd_complete) { |
// LCD_PrintMenu(); |
SendOutData('L', FC_ADDRESS, 0); |
request_Display1 = FALSE; |
} |
if (request_DebugLabel != 0xFF) { // Texte für die Analogdaten |
if (requestedDebugLabel != 0xFF && txd_complete) { // Texte f�r die Analogdaten |
uint8_t label[16]; // local sram buffer |
memcpy_P(label, ANALOG_LABEL[request_DebugLabel], 16); // read lable from flash to sram buffer |
SendOutData('A', FC_ADDRESS, 2, (uint8_t *) &request_DebugLabel, |
sizeof(request_DebugLabel), label, 16); |
request_DebugLabel = 0xFF; |
memcpy_P(label, ANALOG_LABEL[requestedDebugLabel], 16); // read lable from flash to sram buffer |
sendOutData('A', FC_ADDRESS, 2, (uint8_t *) &requestedDebugLabel, |
sizeof(requestedDebugLabel), label, 16); |
requestedDebugLabel = 0xFF; |
} |
if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen |
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, |
sizeof(ConfirmFrame)); |
ConfirmFrame = 0; |
if (confirmFrame && txd_complete) { // Datensatz ohne checksum best�tigen |
sendOutData('B', FC_ADDRESS, 1, (uint8_t*) &confirmFrame, sizeof(confirmFrame)); |
confirmFrame = 0; |
} |
if (((DebugData_Interval && checkDelay(DebugData_Timer)) || request_DebugData) |
if (((debugData_interval && checkDelay(debugData_timer)) || request_debugData) |
&& txd_complete) { |
SendOutData('D', FC_ADDRESS, 1, (uint8_t *) &DebugOut, sizeof(DebugOut)); |
DebugData_Timer = setDelay(DebugData_Interval); |
request_DebugData = FALSE; |
sendOutData('D', FC_ADDRESS, 1, (uint8_t *) &debugOut, sizeof(debugOut)); |
debugData_timer = setDelay(debugData_interval); |
request_debugData = FALSE; |
} |
if (((Data3D_Interval && checkDelay(Data3D_Timer)) || request_Data3D) |
&& txd_complete) { |
SendOutData('C', FC_ADDRESS, 1, (uint8_t *) &Data3D, sizeof(Data3D)); |
Data3D.AngleNick = (int16_t) ((10 * angle[PITCH]) |
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.AngleRoll = (int16_t) ((10 * angle[ROLL]) |
/ GYRO_DEG_FACTOR_PITCHROLL); // convert to multiple of 0.1° |
Data3D.Heading = (int16_t) ((10 * angle[YAW]) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1° |
Data3D_Timer = setDelay(Data3D_Interval); |
request_Data3D = FALSE; |
if (((data3D_interval && checkDelay(data3D_timer)) || request_data3D) && txd_complete) { |
sendOutData('C', FC_ADDRESS, 1, (uint8_t *) &data3D, sizeof(data3D)); |
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D_timer = setDelay(data3D_interval); |
request_data3D = FALSE; |
} |
if (request_ExternalControl && txd_complete) { |
SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
if (request_externalControl && txd_complete) { |
sendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl, |
sizeof(externalControl)); |
request_ExternalControl = FALSE; |
request_externalControl = FALSE; |
} |
#ifdef USE_MK3MAG |
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.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 |
if(compassCalState > 4) compassCalState = 0; |
Compass_Timer = setDelay(99); |
if (request_servoTest && txd_complete) { |
sendOutData('T', FC_ADDRESS, 0); |
request_servoTest = FALSE; |
} |
#endif |
if (request_OutputTest && txd_complete) { |
SendOutData('T', FC_ADDRESS, 0); |
request_OutputTest = FALSE; |
} |
if (request_PPMChannels && txd_complete) { |
SendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in)); |
sendOutData('P', FC_ADDRESS, 1, (uint8_t *) &PPM_in, sizeof(PPM_in)); |
request_PPMChannels = FALSE; |
} |
if (request_variables && txd_complete) { |
SendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
sendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables)); |
request_variables = FALSE; |
} |
if (((OSD_interval && checkDelay(OSD_timer)) || request_OSD) && txd_complete) { |
int32_t height = 0; |
data3D.anglePitch = (int16_t) (attitude[PITCH] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
data3D.angleRoll = (int16_t) (attitude[ROLL] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
// TODO: To 0..359 interval. |
data3D.heading = (int16_t) (attitude[YAW] / (GYRO_DEG_FACTOR/10)); // convert to multiple of 0.1 deg |
sendOutData('O', FC_ADDRESS, 3, (uint8_t*)&data3D, sizeof(data3D), (uint8_t*)&height, sizeof(height), (uint8_t*)UBat, sizeof(UBat)); |
OSD_timer = setDelay(OSD_interval); |
request_OSD = FALSE; |
} |
} |
/branches/dongfang_FC_fixedwing/uart0.h |
---|
1,9 → 1,8 |
#ifndef _UART0_H |
#define _UART0_H |
// must be at least 4('#'+Addr+'CmdID'+'\r')+ (80 * 4)/3 = 111 bytes |
#define TXD_BUFFER_LEN 150 |
#define RXD_BUFFER_LEN 150 |
#define TXD_BUFFER_LEN 180 |
#define RXD_BUFFER_LEN 180 |
#include <inttypes.h> |
10,37 → 9,32 |
//Baud rate of the USART |
#define USART0_BAUD 57600 |
extern void usart0_Init(void); |
extern void usart0_TransmitTxData(void); |
extern void usart0_ProcessRxData(void); |
#define DISPLAYBUFFSIZE 80 |
extern int8_t displayBuff[DISPLAYBUFFSIZE]; |
extern uint8_t dispPtr; |
extern void usart0_init(void); |
extern void usart0_transmitTxData(void); |
extern void usart0_processRxData(void); |
extern int16_t uart_putchar(int8_t c); |
extern uint8_t RemotePollDisplayLine; |
// extern uint8_t remotePollDisplayLine; |
extern uint8_t outputTestActive; |
extern uint8_t outputTest[16]; |
extern uint8_t servoTestActive; |
extern uint8_t servoTest[16]; |
typedef struct { |
uint8_t Digital[2]; |
uint16_t Analog[32]; // Debugvalues |
}__attribute__((packed)) DebugOut_t; |
extern DebugOut_t DebugOut; |
typedef struct { |
int16_t AngleNick; // in 0.1 deg |
int16_t AngleRoll; // in 0.1 deg |
int16_t Heading; // in 0.1 deg |
uint8_t reserve[8]; |
int16_t anglePitch; // in 0.1 deg |
int16_t angleRoll; // in 0.1 deg |
int16_t heading; // in 0.1 deg |
uint8_t reserved[8]; |
}__attribute__((packed)) Data3D_t; |
typedef struct { |
uint8_t SWMajor; |
uint8_t SWMinor; |
uint8_t ProtoMajor; |
uint8_t ProtoMinor; |
uint8_t SWPatch; |
uint8_t Reserved[5]; |
}__attribute__((packed)) UART_VersionInfo_t; |
Data3D_t attitude; |
//GPS_INFO_t GPSInfo; |
int32_t airpressureHeight; |
int16_t batteryVoltage; |
}__attribute__((packed)) OSDData_t; |
#endif //_UART0_H |