• MikroKopter.de
• Wiki
• SHOP
• Datenschutz
• Impressum

FlightCtrlBL-CtrlMK3MagNaviCtrlProjects

Català-Valencià – Catalan中文 – Chinese (Simplified)中文 – Chinese (Traditional)Česky – CzechDansk – DanishNederlands – DutchEnglish – EnglishSuomi – FinnishFrançais – FrenchDeutsch – Germanעברית – Hebrewहिंदी – HindiMagyar – HungarianBahasa Indonesia – IndonesianItaliano – Italian日本語 – Japanese한국어 – KoreanМакедонски – Macedonianमराठी – MarathiNorsk – NorwegianPolski – PolishPortuguês – PortuguesePortuguês – Portuguese (Brazil)Русский – RussianSlovenčina – SlovakSlovenščina – SlovenianEspañol – SpanishSvenska – SwedishTürkçe – TurkishУкраїнська – UkrainianOëzbekcha – Uzbek

Compare Revisions

• This comparison shows the changes necessary to convert path

  → /branches/dongfang_FC_fixedwing @ 2096

  → /branches/dongfang_FC_fixedwing @ 2099

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 2096 → Rev 2099

/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 &quot;${plugin_state_location}/${specs_file}&quot;'" 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 &quot;${plugin_state_location}/specs.cpp&quot;'" 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 &quot;${plugin_state_location}/specs.c&quot;'" 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 &quot;${plugin_state_location}/${specs_file}&quot;'" 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 &quot;${plugin_state_location}/specs.cpp&quot;'" 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 &quot;${plugin_state_location}/specs.c&quot;'" 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