• 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

  → / @ 1909

  → / @ 1910

  ↔ Reverse comparison

• Compare Path:	Rev

  With Path:	Rev

Ignore whitespace Rev 1909 → Rev 1910

/branches/dongfang_FC_fixedwing/.cproject
0,0 → 1,212
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<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">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</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"/>
</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>
<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>
</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"/>
</storageModule>
</cproject>

/branches/dongfang_FC_fixedwing/.project
0,0 → 1,79
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>dongfang_FC_fixedwing</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
<dictionary>
<key>?name?</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.append_environment</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
<value>make</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
<value>clean</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.contents</key>
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
<value>false</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
<value>program</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.stopOnError</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>

/branches/dongfang_FC_fixedwing/ADXRS610_FC2.0.c
0,0 → 1,68
#include "ADXRS610_FC2.0.h"
#include "configuration.h"
const uint8_t GYRO_QUADRANT = 3;
//const uint8_t ACC_QUADRANT = 0;
const uint8_t YAW_REVERSED = 0;
const uint8_t GYROS_REVERSED = 0;
const uint8_t Z_ACC_REVERSED = 0;
void gyro_calibrate(void) {
}
void gyro_setDefaults(void) {
staticParams.DriftComp = 0;
staticParams.GyroAccFactor = 1;
staticParams.GyroAccTrim = 0;
}
/*
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]
*/

/branches/dongfang_FC_fixedwing/ADXRS610_FC2.0.h
0,0 → 1,28
/*
#ifndef _ADXRS610_H
#define _ADXRS610_H
#include "sensors.h"
/ *
* Configuration for the ADXR610 gyros, as they are oriented and wired on the FC 2.0 ME board.
* /
#define GYRO_HW_NAME "ADXR"
#define GYRO_HW_FACTOR 1.2288f
/ *
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
* /
#define GYRO_PITCHROLL_CORRECTION 1.0f
/ *
* Same for yaw.
* /
#define GYRO_YAW_CORRECTION 1.0f
#endif
*/

/branches/dongfang_FC_fixedwing/ENC-03_FC1.3.c
0,0 → 1,71
#include "analog.h"
#include "twimaster.h"
#include "configuration.h"
#include "timer0.h"
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
#define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
const uint8_t GYROS_REVERSED = 0;
const uint8_t GYRO_QUADRANT = 0;
const uint8_t YAW_REVERSED = 1;
const uint8_t Z_ACC_REVERSED = 0;
// void gyro_init(void) {}
void gyro_calibrate(void) {
uint8_t i, axis, factor, numberOfAxesInRange = 0;
uint16_t timeout;
// GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
timeout = setDelay(2000);
for (i = 140; i != 0; i--) {
// If all 3 axis are in range, shorten the remaining number of iterations.
if (numberOfAxesInRange == 3 && i > 10)
i = 9;
numberOfAxesInRange = 0;
for (axis = PITCH; axis <= YAW; axis++) {
if (axis == YAW)
factor = GYRO_SUMMATION_FACTOR_YAW;
else
factor = GYRO_SUMMATION_FACTOR_PITCHROLL;
if (rawGyroSum[axis] < 510 * factor)
DACValues[axis]--;
else if (rawGyroSum[axis] > 515 * factor)
DACValues[axis]++;
else
numberOfAxesInRange++;
/* Gyro is defective. But do keep DAC within bounds (it's an op amp not a differential amp). */
if (DACValues[axis] < 10) {
DACValues[axis] = 10;
} else if (DACValues[axis] > 245) {
DACValues[axis] = 245;
}
}
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // initiate data transmission
// Wait for I2C to finish transmission.
while (twi_state) {
// Did it take too long?
if (checkDelay(timeout)) {
// printf("\r\n DAC or I2C Error! check I2C, 3Vref, DAC, and BL-Ctrl");
RED_ON;
break;
}
}
analog_start();
delay_ms_Mess(i < 10 ? 10 : 2);
}
delay_ms_Mess(70);
}
void gyro_setDefaults(void) {
staticParams.DriftComp = 200;
staticParams.GyroAccFactor = 25;
staticParams.GyroAccTrim = 1;
}

/branches/dongfang_FC_fixedwing/ENC-03_FC1.3.h
0,0 → 1,26
/*
#ifndef _ENC03_FC13_H
#define _ENC03_FC13_H
#include "sensors.h"
/ *
* Configuration for the ENC-03 gyros and oriented and wired on FC 1.3 (with DAC).
* /
#define GYRO_HW_NAME "ENC"
#define GYRO_HW_FACTOR 1.304f
/ *
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
* /
#define GYRO_PITCHROLL_CORRECTION 1.11f
/ *
* Same for yaw.
* /
#define GYRO_YAW_CORRECTION 1.28f
#endif
*/

/branches/dongfang_FC_fixedwing/analog.c
0,0 → 1,544
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include "analog.h"
#include "attitude.h"
#include "sensors.h"
// for Delay functions
#include "timer0.h"
// For DebugOut
#include "uart0.h"
// For reading and writing acc. meter offsets.
#include "eeprom.h"
// For DebugOut.Digital
#include "output.h"
/*
* For each A/D conversion cycle, each analog channel is sampled a number of times
* (see array channelsForStates), and the results for each channel are summed.
* Here are those for the gyros and the acc. meters. They are not zero-offset.
* They are exported in the analog.h file - but please do not use them! The only
* reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
* the offsets with the DAC.
*/
volatile int16_t rawGyroSum[3];
volatile int16_t acc[3];
volatile int16_t filteredAcc[2] = { 0,0 };
/*
* These 4 exported variables are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles.
*/
volatile int16_t gyro_PID[2];
volatile int16_t gyro_ATT[2];
volatile int16_t gyroD[3];
volatile int16_t yawGyro;
/*
* Offset values. These are the raw gyro and acc. meter sums when the copter is
* standing still. They are used for adjusting the gyro and acc. meter values
* to be centered on zero.
*/
volatile int16_t gyroOffset[3] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512
* GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW };
volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512
* ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
/*
* This allows some experimentation with the gyro filters.
* Should be replaced by #define's later on...
*/
volatile uint8_t GYROS_PID_FILTER;
volatile uint8_t GYROS_ATT_FILTER;
volatile uint8_t GYROS_D_FILTER;
volatile uint8_t ACC_FILTER;
/*
* Air pressure
*/
volatile uint8_t rangewidth = 106;
// Direct from sensor, irrespective of range.
// volatile uint16_t rawAirPressure;
// Value of 2 samples, with range.
volatile uint16_t simpleAirPressure;
// Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered.
volatile int32_t filteredAirPressure;
// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
volatile int32_t airPressureSum;
// The number of samples summed into airPressureSum so far.
volatile 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.
* So the initial value of 100 is for 9.7 volts.
*/
volatile int16_t UBat = 100;
/*
* Control and status.
*/
volatile uint16_t ADCycleCount = 0;
volatile uint8_t analogDataReady = 1;
/*
* Experiment: Measuring vibration-induced sensor noise.
*/
volatile uint16_t gyroNoisePeak[2];
volatile uint16_t accNoisePeak[2];
// ADC channels
#define AD_GYRO_YAW 0
#define AD_GYRO_ROLL 1
#define AD_GYRO_PITCH 2
#define AD_AIRPRESSURE 3
#define AD_UBAT 4
#define AD_ACC_Z 5
#define AD_ACC_ROLL 6
#define AD_ACC_PITCH 7
/*
* Table of AD converter inputs for each state.
* The number of samples summed for each channel is equal to
* the number of times the channel appears in the array.
* The max. number of samples that can be taken in 2 ms is:
* 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
* loop needs a little time between reading AD values and
* re-enabling ADC, the real limit is (how much?) lower.
* The acc. sensor is sampled even if not used - or installed
* at all. The cost is not significant.
*/
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_ACC_Z, // at 8, measure Z acc.
AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
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_GYRO_PITCH, // at 15, finish pitch gyro
AD_GYRO_ROLL, // at 16, finish roll gyro
AD_UBAT // at 17, measure battery.
};
// Feature removed. Could be reintroduced later - but should work for all gyro types then.
// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
void analog_init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
//ADC0 ... ADC7 is connected to PortA pin 0 ... 7
DDRA = 0x00;
PORTA = 0x00;
// Digital Input Disable Register 0
// Disable digital input buffer for analog adc_channel pins
DIDR0 = 0xFF;
// external reference, adjust data to the right
ADMUX &= ~((1 << REFS1) | (1 << REFS0) | (1 << ADLAR));
// set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH;
//Set ADC Control and Status Register A
//Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
ADCSRA = (0 << ADEN) | (0 << ADSC) | (0 << ADATE) | (1 << ADPS2) | (1
<< ADPS1) | (1 << ADPS0) | (0 << ADIE);
//Set ADC Control and Status Register B
//Trigger Source to Free Running Mode
ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0));
// Start AD conversion
analog_start();
// restore global interrupt flags
SREG = sreg;
}
void measureNoise(const int16_t sensor,
volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
if (sensor > (int16_t) (*noiseMeasurement)) {
*noiseMeasurement = sensor;
} else if (-sensor > (int16_t) (*noiseMeasurement)) {
*noiseMeasurement = -sensor;
} else if (*noiseMeasurement > damping) {
*noiseMeasurement -= damping;
} else {
*noiseMeasurement = 0;
}
}
/*
* Min.: 0
* Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
*/
uint16_t getSimplePressure(int advalue) {
return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
}
void transformPRGyro(int16_t* result) {
static const uint8_t tab[] = {1,1,0,0-1,-1,-1,0,1};
int8_t pp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+4)%8] : tab[GYRO_QUADRANT];
int8_t pr = tab[(GYRO_QUADRANT+2)%8];
int8_t rp = GYROS_REVERSED ? tab[(GYRO_QUADRANT+2)%8] : tab[(GYRO_QUADRANT+6)%8];
int8_t rr = tab[GYRO_QUADRANT];
int16_t temp = result[0];
result[0] = pp*result[0] + pr*result[1];
result[1] = rp*temp + rr*result[1];
}
/*****************************************************
* Interrupt Service Routine for ADC
* Runs at 312.5 kHz or 3.2 µs. When all states are
* processed the interrupt is disabled and further
* AD conversions are stopped.
*****************************************************/
ISR(ADC_vect) {
static uint8_t ad_channel = AD_GYRO_PITCH, state = 0;
static uint16_t sensorInputs[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static uint16_t pressureAutorangingWait = 25;
uint16_t rawAirPressure;
uint8_t i, axis;
int16_t newrange;
// for various filters...
int16_t tempOffsetGyro[2];
sensorInputs[ad_channel] += ADC;
/*
* Actually we don't need this "switch". We could do all the sampling into the
* sensorInputs array first, and all the processing after the last sample.
*/
switch (state++) {
case 8: // Z acc
if (Z_ACC_REVERSED)
acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
else
acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
/*
stronglyFilteredAcc[Z] =
(stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
*/
break;
case 11: // yaw gyro
rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
if (YAW_REVERSED)
tempOffsetGyro[0] = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
else
tempOffsetGyro[0] = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
gyroD[YAW] = (gyroD[YAW] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[0] - yawGyro)) / GYROS_D_FILTER;
yawGyro = tempOffsetGyro[0];
break;
case 13: // roll axis acc.
/*
for (axis=0; axis<2; axis++) {
if (ACC_REVERSED[axis])
tempSensor[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis];
else
tempSensor[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis];
}
if (AXIS_TRANSFORM) {
acc[PITCH] = tempSensor[PITCH] + tempSensor[ROLL];
acc[ROLL] = tempSensor[ROLL] - tempSensor[PITCH];
} else {
acc[PITCH] = tempSensor[PITCH];
acc[ROLL] = tempSensor[ROLL];
}
*/
// We have no sensor installed...
acc[PITCH] = acc[ROLL] = 0;
for (axis=0; axis<2; axis++) {
filteredAcc[axis] =
(filteredAcc[axis] * (ACC_FILTER - 1) + acc[axis]) / ACC_FILTER;
measureNoise(acc[axis], &accNoisePeak[axis], 1);
}
break;
case 14: // air pressure
if (pressureAutorangingWait) {
//A range switch was done recently. Wait for steadying.
pressureAutorangingWait--;
DebugOut.Analog[27] = (uint16_t) OCR0A;
DebugOut.Analog[31] = simpleAirPressure;
break;
}
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[27] = (uint16_t) OCR0A;
DebugOut.Analog[31] = 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_SUMMATION_FACTOR 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;
if (pressureMeasurementCount == AIRPRESSURE_SUMMATION_FACTOR - 1)
airPressureSum += simpleAirPressure / 2;
else
airPressureSum += simpleAirPressure;
}
// 2 samples were added.
pressureMeasurementCount += 2;
if (pressureMeasurementCount >= AIRPRESSURE_SUMMATION_FACTOR) {
filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER - 1)
+ airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;
pressureMeasurementCount = airPressureSum = 0;
}
break;
case 16: // pitch and roll gyro.
for (axis=0; axis<2; axis++) {
tempOffsetGyro[axis] = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH - axis];
// 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
// gyro with a wider range, and helps counter saturation at full control.
if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
if (tempOffsetGyro[axis] < SENSOR_MIN_PITCHROLL) {
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
tempOffsetGyro[axis] = tempOffsetGyro[axis] * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
} else if (tempOffsetGyro[axis] > SENSOR_MAX_PITCHROLL) {
DebugOut.Digital[0] |= DEBUG_SENSORLIMIT;
tempOffsetGyro[axis] = (tempOffsetGyro[axis] - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
} else {
DebugOut.Digital[0] &= ~DEBUG_SENSORLIMIT;
}
}
// 2) Apply sign and offset, scale before filtering.
tempOffsetGyro[axis] = (tempOffsetGyro[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
}
// 2.1: Transform axis if configured to.
transformPRGyro(tempOffsetGyro);
// 3) Scale and filter.
for (axis=0; axis<2; axis++) {
tempOffsetGyro[axis] = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro[axis]) / GYROS_PID_FILTER;
// 4) Measure noise.
measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
// 5) Differential measurement.
gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / GYROS_D_FILTER;
// 6) Done.
gyro_PID[axis] = tempOffsetGyro[axis];
}
/*
* Now process the data for attitude angles.
*/
for (axis=0; axis<2; axis++) {
tempOffsetGyro[axis] = (rawGyroSum[axis] - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
}
transformPRGyro(tempOffsetGyro);
// 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[PITCH]) / GYROS_ATT_FILTER;
gyro_ATT[ROLL] = (gyro_ATT[ROLL] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro[ROLL]) / GYROS_ATT_FILTER;
break;
case 17:
// Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
// This is divided by 3 --> 10.34 counts per volt.
UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
DebugOut.Analog[20] = UBat;
analogDataReady = 1; // mark
ADCycleCount++;
// Stop the sampling. Cycle is over.
state = 0;
for (i = 0; i < 8; i++) {
sensorInputs[i] = 0;
}
break;
default: {
} // do nothing.
}
// set up for next state.
ad_channel = pgm_read_byte(&channelsForStates[state]);
// ad_channel = channelsForStates[state];
// set adc muxer to next ad_channel
ADMUX = (ADMUX & 0xE0) | ad_channel;
// after full cycle stop further interrupts
if (state)
analog_start();
}
void analog_calibrate(void) {
#define GYRO_OFFSET_CYCLES 32
uint8_t i, axis;
int32_t deltaOffsets[3] = { 0, 0, 0 };
// Set the filters... to be removed again, once some good settings are found.
GYROS_PID_FILTER = (dynamicParams.UserParams[4] & (0x7 & (1<<0))) + 1;
GYROS_ATT_FILTER = 1;
GYROS_D_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<4))) + 1;
ACC_FILTER = (dynamicParams.UserParams[4] & (0x3 & (1<<6))) + 1;
gyro_calibrate();
// determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
delay_ms_Mess(20);
for (axis = PITCH; axis <= YAW; axis++) {
deltaOffsets[axis] += rawGyroSum[axis];
}
}
for (axis = PITCH; axis <= YAW; axis++) {
gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
// DebugOut.Analog[20 + axis] = gyroOffset[axis];
}
// Noise is relativ to offset. So, reset noise measurements when changing offsets.
gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
accOffset[Z] = GetParamWord(PID_ACC_Z);
// Rough estimate. Hmm no nothing happens at calibration anyway.
// airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2);
// pressureMeasurementCount = 0;
delay_ms_Mess(100);
}
/*
* 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 10
/*
uint8_t i, axis;
int32_t deltaOffset[3] = { 0, 0, 0 };
int16_t filteredDelta;
// int16_t pressureDiff, savedRawAirPressure;
for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
delay_ms_Mess(10);
for (axis = PITCH; axis <= YAW; axis++) {
deltaOffset[axis] += acc[axis];
}
}
for (axis = PITCH; axis <= YAW; axis++) {
filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
/ ACC_OFFSET_CYCLES;
accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
}
// Save ACC neutral settings to eeprom
SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
SetParamWord(PID_ACC_ROLL, accOffset[ROLL]);
SetParamWord(PID_ACC_Z, accOffset[Z]);
// Noise is relative to offset. So, reset noise measurements when
// changing offsets.
accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
// Setting offset values has an influence in the analog.c ISR
// Therefore run measurement for 100ms to achive stable readings
delay_ms_Mess(100);
*/
// Set the feedback so that air pressure ends up in the middle of the range.
// (raw pressure high --> OCR0A also high...)
/*
OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1;
delay_ms_Mess(1000);
pressureDiff = 0;
// DebugOut.Analog[16] = rawAirPressure;
#define PRESSURE_CAL_CYCLE_COUNT 5
for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) {
savedRawAirPressure = rawAirPressure;
OCR0A+=2;
delay_ms_Mess(500);
// raw pressure will decrease.
pressureDiff += (savedRawAirPressure - rawAirPressure);
savedRawAirPressure = rawAirPressure;
OCR0A-=2;
delay_ms_Mess(500);
// raw pressure will increase.
pressureDiff += (rawAirPressure - savedRawAirPressure);
}
rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2);
DebugOut.Analog[27] = rangewidth;
*/
}

/branches/dongfang_FC_fixedwing/analog.h
0,0 → 1,259
#ifndef _ANALOG_H
#define _ANALOG_H
#include <inttypes.h>
//#include "invenSense.h"
//#include "ENC-03_FC1.3.h"
//#include "ADXRS610_FC2.0.h"
/*
* How much low pass filtering to apply for gyro_PID.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
// #define GYROS_PID_FILTER 1
/*
* How much low pass filtering to apply for gyro_ATT.
* 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...
* Temporarily replaced by userparam-configurable variable.
*/
// #define GYROS_ATT_FILTER 1
// Temporarily replaced by userparam-configurable variable.
// #define ACC_FILTER 4
/*
About setting constants for different gyros:
Main parameters are positive directions and voltage/angular speed gain.
The "Positive direction" is the rotation direction around an axis where
the corresponding gyro outputs a voltage > the no-rotation voltage.
A gyro is considered, in this code, to be "forward" if its positive
direction is:
- Nose down for pitch
- Left hand side down for roll
- Clockwise seen from above for yaw.
Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and
GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.
Setting gyro gain correctly: All sensor measurements in analog.c take
place in a cycle, each cycle comprising all sensors. Some sensors are
sampled more than ones, and the results added. The pitch and roll gyros
are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74
code.
In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)
or 4 (other versions), offset to zero, low pass filtered and then assigned
to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or
roll.
So:
gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),
where V is 2 for FC1.0 and 4 for all others.
Assuming constant ADCValue, in the H&I code:
gyro = I * ADCValue.
where I is 8 for FC1.0 and 16 for all others.
The relation between rotation rate and ADCValue:
ADCValue [units] =
rotational speed [deg/s] *
gyro sensitivity [V / deg/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
or: H is the number of steps the ADC value changes with,
for a 1 deg/s change in rotational velocity:
H = ADCValue [units] / rotation rate [deg/s] =
gyro sensitivity [V / deg/s] *
amplifier gain [units] *
1024 [units] /
3V full range [V]
Examples:
FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:
H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).
FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:
H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).
My InvenSense copter has 2mV/deg/s gyros and no amplifiers:
H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
(only about half as sensitive as V1.3. But it will take about twice the
rotation rate!)
All together: gyro = I * H * rotation rate [units / (deg/s)].
*/
/*
* A factor that the raw gyro values are multiplied by,
* before being filtered and passed to the attitude module.
* A value of 1 would cause a little bit of loss of precision in the
* filtering (on the other hand the values are so noisy in flight that
* it will not really matter - but when testing on the desk it might be
* noticeable). 4 is fine for the default filtering.
* Experiment: Set it to 1.
*/
#define GYRO_FACTOR_PITCHROLL 1
/*
* How many samples are summed in one ADC loop, for pitch&roll and yaw,
* respectively. This is = the number of occurences of each channel in the
* channelsForStates array in analog.c.
*/
#define GYRO_SUMMATION_FACTOR_PITCHROLL 4
#define GYRO_SUMMATION_FACTOR_YAW 2
#define ACC_SUMMATION_FACTOR_PITCHROLL 2
#define ACC_SUMMATION_FACTOR_Z 1
/*
Integration:
The HiResXXXX values are divided by 8 (in H&I firmware) before integration.
In the Killagreg rewrite of the H&I firmware, the factor 8 is called
HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,
and care has been taken that all other constants (gyro to degree factor, and
180 degree flip-over detection limits) are corrected to it. Because the
division by the constant takes place in the flight attitude code, the
constant is there.
The control loop executes every 2ms, and for each iteration
gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].
Assuming a constant rotation rate v and a zero initial gyroIntegral
(for this explanation), we get:
gyroIntegral =
N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =
N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR
where N is the number of summations; N = t/2ms.
For one degree of rotation: t*v = 1:
gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.
This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.
Examples:
FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304
FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260
My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365
*/
/*
* The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.
* Will be about 10 or so for InvenSense, and about 33 for ADXRS610.
*/
#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)
#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)
/*
* Gyro saturation prevention.
*/
// How far from the end of its range a gyro is considered near-saturated.
#define SENSOR_MIN_PITCHROLL 32
// Other end of the range (calculated)
#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)
// 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)
/*
* This value is subtracted from the gyro noise measurement in each iteration,
* making it return towards zero.
*/
#define GYRO_NOISE_MEASUREMENT_DAMPING 5
#define PITCH 0
#define ROLL 1
#define YAW 2
#define Z 2
/*
* The values that this module outputs
* These first 2 exported arrays are zero-offset. The "PID" ones are used
* in the attitude control as rotation rates. The "ATT" ones are for
* integration to angles. For the same axis, the PID and ATT variables
* generally have about the same values. There are just some differences
* in filtering, and when a gyro becomes near saturated.
* Maybe this distinction is not really necessary.
*/
extern volatile int16_t gyro_PID[2];
extern volatile int16_t gyro_ATT[2];
extern volatile int16_t gyroD[3];
extern volatile int16_t yawGyro;
extern volatile uint16_t ADCycleCount;
extern volatile int16_t UBat;
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
/*
* This is not really for external use - but the ENC-03 gyro modules needs it.
*/
extern volatile int16_t rawGyroSum[3];
/*
* The acceleration values that this module outputs. They are zero based.
*/
extern volatile int16_t acc[3];
extern volatile int16_t filteredAcc[2];
// extern volatile int32_t stronglyFilteredAcc[3];
/*
* Diagnostics: Gyro noise level because of motor vibrations. The variables
* only really reflect the noise level when the copter stands still but with
* its motors running.
*/
extern volatile uint16_t gyroNoisePeak[2];
extern volatile uint16_t accNoisePeak[2];
/*
* Air pressure.
* The sensor has a sensitivity of 46 mV/kPa.
* An approximate p(h) formula is = p(h[m])[Pa] = p_0 - 11.95 * 10^-3 * h
* That is: dV = 46 mV * 11.95 * 10^-3 dh = 0.5497 mV / m.
* That is, with 2 * 1.024 / 3 steps per mV: 0.037 steps / m
*/
#define AIRPRESSURE_SUMMATION_FACTOR 16
#define AIRPRESSURE_FILTER 8
// Minimum A/D value before a range change is performed.
#define MIN_RAWPRESSURE (200 * 2)
// Maximum A/D value before a range change is performed.
#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)
#define MIN_RANGES_EXTRAPOLATION 15
#define MAX_RANGES_EXTRAPOLATION 240
#define PRESSURE_EXTRAPOLATION_COEFF 25L
#define AUTORANGE_WAIT_FACTOR 1
extern volatile uint16_t simpleAirPressure;
/*
* At saturation, the filteredAirPressure may actually be (simulated) negative.
*/
extern volatile int32_t filteredAirPressure;
/*
* Flag: Interrupt handler has done all A/D conversion and processing.
*/
extern volatile uint8_t analogDataReady;
void analog_init(void);
// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
/*
* "Warm" calibration: Zero-offset gyros.
*/
void analog_calibrate(void);
/*
* "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.
*/
void analog_calibrateAcc(void);
#endif //_ANALOG_H

/branches/dongfang_FC_fixedwing/attitude.c
0,0 → 1,346
/************************************************************************/
/* Flight Attitude */
/************************************************************************/
#include <stdlib.h>
#include <avr/io.h>
#include "attitude.h"
#include "dongfangMath.h"
// For scope debugging only!
#include "rc.h"
// where our main data flow comes from.
#include "analog.h"
#include "configuration.h"
#include "output.h"
// 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;}
/*
* Gyro readings, as read from the analog module. It would have been nice to flow
* them around between the different calculations as a struct or array (doing
* things functionally without side effects) but this is shorter and probably
* faster too.
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t rate_ATT[2], yawRate;
// With different (less) filtering
int16_t rate_PID[2];
int16_t differential[3];
/*
* Gyro readings, after performing "axis coupling" - that is, the transfomation
* of rotation rates from the airframe-local coordinate system to a ground-fixed
* coordinate system. If axis copling is disabled, the gyro readings will be
* copied into these directly.
* These are global for the same pragmatic reason as with the gyro readings.
* The variables are overwritten at each attitude calculation invocation - the values
* are not preserved or reused.
*/
int16_t ACRate[2], ACYawRate;
/*
* Gyro integrals. These are the rotation angles of the airframe compared to the
* horizontal plane, yaw relative to yaw at start.
*/
int32_t angle[2], yawAngleDiff;
int readingHeight = 0;
// Yaw angle and compass stuff.
// This is updated/written from MM3. Negative angle indicates invalid data.
int16_t compassHeading = -1;
// This is NOT updated from MM3. Negative angle indicates invalid data.
int16_t compassCourse = -1;
// The difference between the above 2 (heading - course) on a -180..179 degree interval.
// Not necessary. Never read anywhere.
// int16_t compassOffCourse = 0;
uint8_t updateCompassCourse = 0;
uint8_t compassCalState = 0;
uint16_t ignoreCompassTimer = 500;
int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass
int16_t yawGyroDrift;
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 savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
// int16_t dynamicCalCount;
/************************************************************************
* Set inclination angles from the acc. sensor data.
* If acc. sensors are not used, set to zero.
* TODO: One could use inverse sine to calculate the angles more
* accurately, but since: 1) the angles are rather small at times when
* it makes sense to set the integrals (standing on ground, or flying at
* 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];
}
void setStaticAttitudeAngles(void) {
#ifdef ATTITUDE_USE_ACC_SENSORS
angle[PITCH] = getAngleEstimateFromAcc(PITCH);
angle[ROLL] = getAngleEstimateFromAcc(ROLL);
#else
angle[PITCH] = angle[ROLL] = 0;
#endif
}
/************************************************************************
* 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] = yawGyroDrift = driftCompYaw = 0;
correctionSum[PITCH] = correctionSum[ROLL] = 0;
// Calibrate hardware.
analog_calibrate();
// reset gyro integrals to acc guessing
setStaticAttitudeAngles();
yawAngleDiff = 0;
// update compass course to current heading
compassCourse = compassHeading;
// Inititialize YawGyroIntegral value with current compass heading
yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW;
// Servo_On(); //enable servo output
}
/************************************************************************
* Get sensor data from the analog module, and release the ADC
* TODO: Ultimately, the analog module could do this (instead of dumping
* the values into variables).
* The rate variable end up in a range of about [-1024, 1023].
*************************************************************************/
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];
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();
}
/*
* This is the standard flight-style coordinate system transformation
* (from airframe-local axes to a ground-based system). For some reason
* the MK uses a left-hand coordinate system. The tranformation has been
* changed accordingly.
*/
void trigAxisCoupling(void) {
int16_t cospitch = int_cos(angle[PITCH]);
int16_t cosroll = int_cos(angle[ROLL]);
int16_t sinroll = int_sin(angle[ROLL]);
ACRate[PITCH] = (((int32_t)rate_ATT[PITCH] * cosroll - (int32_t)yawRate
* sinroll) >> MATH_UNIT_FACTOR_LOG);
ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
+ (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
}
// 480 usec with axis coupling - almost no time without.
void integrate(void) {
// First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
uint8_t axis;
if (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE) {
trigAxisCoupling();
} else {
ACRate[PITCH] = rate_ATT[PITCH];
ACRate[ROLL] = rate_ATT[ROLL];
ACYawRate = yawRate;
}
/*
* Yaw
* Calculate yaw gyro integral (~ to rotation angle)
* Limit yawGyroHeading proportional to 0 deg to 360 deg
*/
yawGyroHeading += ACYawRate;
yawAngleDiff += yawRate;
if (yawGyroHeading >= YAWOVER360) {
yawGyroHeading -= YAWOVER360; // 360 deg. wrap
} else if (yawGyroHeading < 0) {
yawGyroHeading += YAWOVER360;
}
/*
* Pitch axis integration and range boundary wrap.
*/
for (axis = PITCH; axis <= ROLL; axis++) {
angle[axis] += ACRate[axis];
if (angle[axis] > PITCHROLLOVER180) {
angle[axis] -= PITCHROLLOVER360;
} else if (angle[axis] <= -PITCHROLLOVER180) {
angle[axis] += PITCHROLLOVER360;
}
}
}
/************************************************************************
* 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] >= -dynamicParams.UserParams[7] && acc[Z]
<= dynamicParams.UserParams[7]) {
DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
uint8_t debugFullWeight = 1;
int32_t accDerived;
if ((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.GyroAccTrim;
CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
// DebugOut.Analog[11 + axis] = correctionSum[axis];
DebugOut.Analog[16 + axis] = correctionSum[axis];
DebugOut.Analog[28 + axis] = driftComp[axis];
correctionSum[axis] = 0;
}
}
}
/************************************************************************
* Main procedure.
************************************************************************/
void calculateFlightAttitude(void) {
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
}

/branches/dongfang_FC_fixedwing/attitude.h
0,0 → 1,140
/*********************************************************************************/
/* Attitude sense system (processing of gyro, accelerometer and altimeter data) */
/*********************************************************************************/
#ifndef _ATTITUDE_H
#define _ATTITUDE_H
#include <inttypes.h>
#include "analog.h"
// For debugging only.
#include "uart0.h"
/*
* 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
/*
* The frequency at which numerical integration takes place. 488 in original code.
*/
#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.
* 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each
* [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.
* We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:
* In this explanation it is assumed that the ADC values are 0 based, and gravity is included.
* The sine of v is the far side / the hypothenusis:
* sin v = acc / sqrt(acc^2 + acc_z^2)
* Using that v is a small angle, and the near side is about equal to the the hypothenusis:
* sin v ~= acc / acc_z
* Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,
* and sin v ~= v (small angles, in radians):
* sin v ~= acc / 410
* v / 57.3 ~= acc / 410
* v ~= acc * 57.3 / 410
* acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.
*
* Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)
*/
#define DEG_ACC_FACTOR 7
/*
* Growth of the integral per degree:
* The hiResXXXX value per deg / s * INTEGRATION_FREQUENCY samples / sec * correction / a number divided by
* 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)
/*
* This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value
* = the factor an acc. sensor should be multiplied by to get the gyro integral
* value for the same (small) angle.
* The value is about 200.
*/
#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)
/*
* Rotation rates
*/
extern int16_t rate_PID[2], rate_ATT[2], yawRate;
extern int16_t differential[3];
/*
* Attitudes calculated by numerical integration of gyro rates
*/
extern int32_t angle[2], yawAngleDiff;
// 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:
* - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool
* - Summing up how much acc. meter correction was done to the gyro integrals over the last n
* integration, and then adding the sum / n to the dynamic offset
* - 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;
/*
* For NaviCtrl use.
*/
extern int16_t averageAcc[2], averageAccCount;
/*
* Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).
* To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).
*/
void attitude_setNeutral(void);
/*
* Experiment.
*/
// void attitude_startDynamicCalibration(void);
// void attitude_continueDynamicCalibration(void);
int32_t getAngleEstimateFromAcc(uint8_t axis);
/*
* Main routine, called from the flight loop.
*/
void calculateFlightAttitude(void);
#endif //_ATTITUDE_H

/branches/dongfang_FC_fixedwing/commands.c
0,0 → 1,76
#include <stdlib.h>
#include "commands.h"
#include "controlMixer.h"
#include "flight.h"
#include "eeprom.h"
#include "attitude.h"
#include "output.h"
void commands_handleCommands(void) {
/*
* 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;
// 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
if (argument < 6) {
// Gyro calinbration, with or without selecting a new parameter-set.
if (argument > 0 && argument < 6) {
// A valid parameter-set (1..5) was chosen - use it.
setActiveParamSet(argument);
}
ParamSet_ReadFromEEProm(getActiveParamSet());
attitude_setNeutral();
flight_setNeutral();
controlMixer_setNeutral();
beepNumber(getActiveParamSet());
} else if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE
| CFG_GPS_ACTIVE) && argument == 7) {
// If right stick is centered and down
// TODO: Out of here! State machine instead.
compassCalState = 1;
beep(1000);
}
}
// 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(getActiveParamSet());
}
}
} // 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);
}
}

/branches/dongfang_FC_fixedwing/commands.h
0,0 → 1,17
#ifndef _COMMANDS_H
#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
#endif

/branches/dongfang_FC_fixedwing/configuration.c
0,0 → 1,95
#include <util/delay.h>
#include <avr/eeprom.h>
#include <stddef.h>
#include "configuration.h"
#include "eeprom.h"
#include "uart0.h"
int16_t variables[8] = {0,0,0,0,0,0,0,0};
dynamicParam_t dynamicParams = {48,251,16,58,64,8,150,150,2,10,{0,0,0,0,0,0,0,0},100,70,90,65,64,100,0,0,0};
uint8_t CPUType = ATMEGA644;
uint8_t BoardRelease = 13;
/************************************************************************
* Map the parameter to pot values
* Replacing this code by the code below saved almost 1 kbyte.
************************************************************************/
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);
SET_POT(dynamicParams.GyroPitchP,staticParams.GyroPitchP);
SET_POT(dynamicParams.GyroRollP,staticParams.GyroRollP);
SET_POT(dynamicParams.GyroYawP,staticParams.GyroYawP);
SET_POT(dynamicParams.GyroPitchD,staticParams.GyroPitchD);
SET_POT(dynamicParams.GyroRollD,staticParams.GyroRollD);
SET_POT(dynamicParams.GyroYawD,staticParams.GyroYawD);
SET_POT(dynamicParams.IFactor,staticParams.IFactor);
for (i=0; i<sizeof(staticParams.UserParams1); i++) {
SET_POT(dynamicParams.UserParams[i],staticParams.UserParams1[i]);
}
for (i=0; i<sizeof(staticParams.UserParams2); i++) {
SET_POT(dynamicParams.UserParams[i + sizeof(staticParams.UserParams1)], staticParams.UserParams2[i]);
}
SET_POT_MM(dynamicParams.J16Timing,staticParams.J16Timing,1,255);
SET_POT_MM(dynamicParams.J17Timing,staticParams.J17Timing,1,255);
SET_POT(dynamicParams.ExternalControl,staticParams.ExternalControl);
}
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;
}
/*
* Automatic detection of hardware components is not supported in this development-oriented
* FC firmware. It would go against the point of it: To enable alternative hardware
* configurations with otherwise unsupported components. Instead, one should write
* custom code + adjust constants for the new hardware, and include the relevant code
* from the makefile.
* However - we still do detect the board release. Reason: Otherwise it would be too
* 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;
// the board release is coded via the pull up or down the 2 status LED
PORTB &= ~((1 << PORTB1)|(1 << PORTB0)); // set tristate
DDRB &= ~((1 << DDB0)|(1 << DDB0)); // set port direction as input
_delay_loop_2(1000); // make some delay
switch( PINB & ((1<<PINB1)|(1<<PINB0))) {
case 0x00:
BoardRelease = 10; // 1.0
break;
case 0x01:
BoardRelease = 11; // 1.1 or 1.2
break;
case 0x02:
BoardRelease = 20; // 2.0
break;
case 0x03:
BoardRelease = 13; // 1.3
break;
default:
break;
}
// set LED ports as output
DDRB |= (1<<DDB1)|(1<<DDB0);
RED_ON;
GRN_OFF;
return BoardRelease;
}

/branches/dongfang_FC_fixedwing/configuration.h
0,0 → 1,186
#ifndef _CONFIGURATION_H
#define _CONFIGURATION_H
#include <inttypes.h>
#include <avr/io.h>
typedef struct {
/*PMM*/uint8_t HeightD;
/* P */uint8_t MaxHeight;
/*PMM*/uint8_t HeightP;
/* P */uint8_t Height_ACC_Effect;
/* P */uint8_t CompassYawEffect;
/* P */uint8_t unnused;
/* P */uint8_t GyroPitchP;
/* P */uint8_t GyroRollP;
/* P */uint8_t GyroYawP;
/* P */uint8_t IFactor;
/* P */uint8_t UserParams[8];
/* P */uint8_t ServoPitchControl;
/* P */uint8_t GyroPitchD; // LoopGasLimit in tool
/* P */uint8_t GyroRollD; // AxisCoupling1 in tool
/* P */uint8_t GyroYawD; // AxisCoupling2 in tool
/* P */uint8_t AxisCouplingYawCorrection;
/* P */uint8_t DynamicStability;
/* P */uint8_t ExternalControl;
/*PMM*/uint8_t J16Timing;
/*PMM*/uint8_t J17Timing;
/* P */uint8_t NaviGpsModeControl;
/* P */uint8_t NaviGpsGain;
/* P */uint8_t NaviGpsP;
/* P */uint8_t NaviGpsI;
/* P */uint8_t NaviGpsD;
/* P */uint8_t NaviGpsACC;
/*PMM*/uint8_t NaviOperatingRadius;
/* P */uint8_t NaviWindCorrection;
/* P */uint8_t NaviSpeedCompensation;
int8_t KalmanK;
int8_t KalmanMaxDrift;
int8_t KalmanMaxFusion;
} dynamicParam_t;
extern dynamicParam_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 StickElevatorP; // StickP in tool.
uint8_t StickAileronsP; // StickD in tool.
uint8_t StickRudderP; // StickYawP in tool.
uint8_t Unused3;
uint8_t Unused4;
uint8_t GyroAccFactor; // Value : 1-64
uint8_t CompassYawEffect; // Value : 0-32
uint8_t GyroPitchP; // GyroP in tool
uint8_t GyroRollP; // GyroI in tool
uint8_t GyroYawP; // GyroD in tool
uint8_t LowVoltageWarning; // Value : 0-250
uint8_t Unused5; // Value : 0-250 //Gaswert bei Empüngsverlust
uint8_t Unused6; // Value : 0-250 // Zeitbis auf EmergencyGas geschaltet wird, wg. Rx-Problemen
uint8_t Unused7; //
uint8_t IFactor; // Value : 0-250
uint8_t UserParams1[4]; // Value : 0-250
uint8_t ControlSigns;
uint8_t Unused9;
uint8_t Unused10;
uint8_t Unused11; // Value : 0-250 // Anschlag
uint8_t ServoRefresh; // Value: 0-250 // Refreshrate of servo pwm output
uint8_t Unused16;
uint8_t Unused17;
uint8_t Unused18;
uint8_t GyroPitchD; // LoopGasLimit in tool
uint8_t GyroRollD; // loopThreshold in tool
uint8_t GyroYawD; // loopHysteresis in tool
uint8_t Unused19;
uint8_t Unused20;
uint8_t GyroAccTrim; // 1/k (Koppel_ACC_Wirkung)
uint8_t DriftComp; // limit for gyrodrift compensation
uint8_t Unused21; // PID limit for Attitude controller
uint8_t UserParams2[4]; // Value : 0-250
uint8_t J16Bitmask; // for the J16 Output
uint8_t J16Timing; // for the J16 Output
uint8_t J17Bitmask; // for the J17 Output
uint8_t J17Timing; // for the J17 Output
uint8_t ExternalControl; // for serial Control
uint8_t BitConfig; // see upper defines for bitcoding
uint8_t ServoPitchCompInvert; // Value : 0-250 0 oder 1 // WICHTIG!!! am Ende lassen
uint8_t Reserved[4];
int8_t Name[12];
} paramset_t;
#define PARAMSET_STRUCT_LEN sizeof(paramset_t)
extern paramset_t staticParams;
typedef struct {
uint8_t Revision;
int8_t Name[12];
int8_t Motor[16][4];
}__attribute__((packed)) MixerTable_t;
extern MixerTable_t Mixer;
// 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)
// 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_LOOP_UP (1<<0)
#define CFG_LOOP_DOWN (1<<1)
#define CFG_LOOP_LEFT (1<<2)
#define CFG_LOOP_RIGHT (1<<3)
#define CFG_HEIGHT_3SWITCH (1<<4)
#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);}
#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_FLASH PORTB ^= (1<<PORTB1)
// Mixer table
#define MIX_THROTTLE 0
#define MIX_PITCH 1
#define MIX_ROLL 2
#define MIX_YAW 3
extern volatile uint8_t MKFlags;
extern int16_t variables[8]; // The "Poti"s.
extern uint8_t BoardRelease;
extern uint8_t CPUType;
void configuration_staticToDynamic(void);
uint8_t getCPUType(void);
uint8_t getBoardRelease(void);
#endif // _CONFIGURATION_H

/branches/dongfang_FC_fixedwing/controlMixer.c
0,0 → 1,190
#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();
}

/branches/dongfang_FC_fixedwing/controlMixer.h
0,0 → 1,118
#include <inttypes.h>
/*
* An attempt to define a generic control. That could be an R/C receiver, an external control
* (serial over Bluetooth, Wi232, XBee, whatever) or the NaviCtrl.
* This resembles somewhat an object-oriented class definition (except that there are no virtuals).
* The idea is that the combination of different control inputs, of the way they superimpose upon
* each other, the priorities between them and the behavior in case that one fails is simplified,
* and all in one place.
*/
/*
* Signal qualities, used to determine the availability of a control.
* NO_SIGNAL means there was never a signal. SIGNAL_LOST that there was a signal, but it was lost.
* SIGNAL_BAD is too bad for flight. This is the hysteresis range for deciding whether to engage
* or disengage emergency landing.
* SIGNAL_OK means the signal is usable for flight.
* SIGNAL_GOOD means the signal can also be used for setting parameters.
*/
#define NO_SIGNAL 0
#define SIGNAL_LOST 1
#define SIGNAL_BAD 2
#define SIGNAL_OK 3
#define SIGNAL_GOOD 4
/*
* The EATR arrays
*/
#define CONTROL_ELEVATOR 0
#define CONTROL_AILERONS 1
#define CONTROL_THROTTLE 2
#define CONTROL_RUDDER 3
/*
* 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
* will declare an instance of the stuct (=implementation of the abstract class).
*/
typedef struct {
/* Get the pitch input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getPitch)(void);
/* Get the roll input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getRoll)(void);
/* Get the yaw input in the nominal range [-STICK_RANGE, STICK_RANGE]. */
int16_t(*getYaw)(void);
/* Get the throttle input in the nominal range [0, THROTTLE_RANGE]. */
uint16_t(*getThrottle)(void);
/* Signal quality, by the above SIGNAL_... definitions. */
uint8_t (*getSignalQuality)(void);
/* Calibrate sticks to their center positions (only relevant for R/C, really) */
void (*calibrate)(void);
} t_control;
/*
* Our output.
*/
extern int16_t control[4];
extern int32_t controlIntegrals[4];
extern uint16_t controlActivity;
extern uint16_t maxControl[2];
extern volatile uint8_t MKFlags;
extern uint16_t isFlying;
void controlMixer_initVariables(void);
void controlMixer_updateVariables(void);
void controlMixer_setNeutral(void);
/*
* Update the exported variables. Called at every flight control cycle.
*/
void controlMixer_update(void);
/*
* Get the current command. See the COMMAND_.... define's
*/
uint8_t controlMixer_getCommand(void);
void controlMixer_performCalibrationCommands(uint8_t command);
uint8_t controlMixer_getSignalQuality(void);
/*
* The controls operate in [-1024, 1024] just about.
* Throttle is [0..255] just about.
*/
// Scale controls to 1 byte:
#define CONTROL_SCALING (1024/256)
// Scale throttle levels to byte:
#define MOTOR_SCALING (1024/256)
/*
* Gets the argument for the current command (a number).
*
* Stick position to argument values (for stick control):
* 2--3--4
* | | +
* 1 9 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* 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
0,0 → 1,235
#include "dongfangMath.h"
#include <inttypes.h>
#include <avr/pgmspace.h>
// 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),
(int16_t) (0.05233595624294383 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.0697564737441253 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.08715574274765817 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.10452846326765346 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.12186934340514748 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.13917310096006544 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.15643446504023087 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.17364817766693033 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.1908089953765448 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.20791169081775931 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.224951054343865 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.24192189559966773 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.25881904510252074 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.27563735581699916 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.29237170472273677 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3090169943749474 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.32556815445715664 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3420201433256687 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.35836794954530027 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.374606593415912 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3907311284892737 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.40673664307580015 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.42261826174069944 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4383711467890774 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.45399049973954675 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4694715627858908 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.48480962024633706 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.49999999999999994 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5150380749100542 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5299192642332049 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5446390350150271 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5591929034707469 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.573576436351046 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5877852522924731 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6018150231520483 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6156614753256582 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6293203910498374 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6427876096865393 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6560590289905072 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6691306063588582 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6819983600624985 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6946583704589973 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7071067811865475 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7193398003386511 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7313537016191705 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7431448254773942 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.754709580222772 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.766044443118978 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7771459614569708 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.788010753606722 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7986355100472928 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8090169943749475 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8191520442889918 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8290375725550417 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8386705679454239 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8480480961564261 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8571673007021122 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8660254037844386 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8746197071393957 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8829475928589269 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8910065241883678 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.898794046299167 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9063077870366499 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9135454576426009 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9205048534524403 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9271838545667874 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9335804264972017 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9396926207859083 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9455185755993167 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9510565162951535 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9563047559630354 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9612616959383189 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9659258262890683 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9702957262759965 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9743700647852352 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9781476007338056 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.981627183447664 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.984807753012208 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9876883405951378 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9902680687415703 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.992546151641322 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9945218953682733 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9961946980917455 * MATH_UNIT_FACTOR + 0.5),
(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) };
const int16_t TAN_TABLE[] PROGMEM
= { (int16_t) (0.0 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.017455064928217585 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.03492076949174773 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.0524077792830412 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.06992681194351041 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.08748866352592401 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.10510423526567646 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.1227845609029046 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.14054083470239145 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.15838444032453627 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.17632698070846498 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.19438030913771848 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2125565616700221 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.23086819112556312 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.24932800284318068 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2679491924311227 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.2867453857588079 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3057306814586604 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3249196962329063 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3443276132896652 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.36397023426620234 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.3838640350354158 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4040262258351568 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4244748162096047 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4452286853085361 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.4663076581549986 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.48773258856586144 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5095254494944288 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5317094316614788 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.554309051452769 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.5773502691896257 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6008606190275604 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6248693519093275 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6494075931975106 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.6745085168424267 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7002075382097097 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7265425280053608 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7535540501027942 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.7812856265067173 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.809784033195007 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8390996311772799 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.8692867378162265 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9004040442978399 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9325150861376615 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9656887748070739 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (0.9999999999999999 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.0355303137905694 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.0723687100246826 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.1106125148291928 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.1503684072210094 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.19175359259421 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.234897156535051 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.2799416321930788 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.3270448216204098 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.3763819204711734 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.4281480067421144 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.4825609685127403 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.5398649638145825 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.6003345290410504 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.6642794823505174 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.7320508075688767 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.8040477552714236 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.8807264653463318 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (1.9626105055051504 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.050303841579296 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.1445069205095586 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.2460367739042164 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.355852365823752 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.4750868534162964 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.6050890646938005 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.7474774194546216 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (2.904210877675822 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.0776835371752527 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.2708526184841404 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.4874144438409087 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (3.7320508075688776 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.010780933535842 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.331475874284157 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (4.704630109478451 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (5.144554015970307 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (5.671281819617707 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (6.313751514675041 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (7.115369722384195 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (8.144346427974593 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (9.514364454222587 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (11.430052302761348 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (14.300666256711896 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (19.08113668772816 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (28.636253282915515 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (57.289961630759876 * MATH_UNIT_FACTOR + 0.5),
(int16_t) (32767) };
int16_t int_sin(int32_t arg) {
int8_t sign;
int16_t result;
int16_t argp = arg / MATH_DRG_FACTOR;
argp %= 360;
if (argp < 0) {
argp = -argp;
sign = -1;
} else {
sign = 1;
}
if (argp >= 90) {
argp = 180 - argp;
}
result = pgm_read_word(&SIN_TABLE[(uint8_t) argp]);
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 int_tan(int32_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;
}
result = pgm_read_word(&TAN_TABLE[(uint8_t) argp]);
return (sign == 1) ? result : -result;
}

/branches/dongfang_FC_fixedwing/dongfangMath.h
0,0 → 1,22
#include<inttypes.h>
#include "attitude.h"
/*
* Angular unit scaling: Number of units per degree
*/
#define MATH_DRG_FACTOR GYRO_DEG_FACTOR_PITCHROLL
/*
* Fix-point decimal scaling: Number of units for 1 (so if sin(something)
* returns UNIT_FACTOR * 0.8, the result is to be understood as 0.8)
* a * sin(b) = (a * int_sin(b * DRG_FACTOR)) / UNIT_FACTOR
*/
//#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)
int16_t int_sin(int32_t arg);
int16_t int_cos(int32_t arg);
int16_t int_tan(int32_t arg);

/branches/dongfang_FC_fixedwing/dsl.c
0,0 → 1,222
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// This code has been derived from the implementation of Stefan Engelke.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/*
Copyright (c) 2008 Stefan Engelke <stefan@tinkerer.eu>
Permission is hereby granted, free of charge, to any person
obtaining a copy of this software and associated documentation
files (the "Software"), to deal in the Software without
restriction, including without limitation the rights to use, copy,
modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be
included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
$Id: rcdsl.c 60 2008-08-21 07:50:48Z taser $
RCDSL.H and RCDSL.C is an INOFFICIAL implementation of the
communication protocol used by DSL receivers of Act Europe.
The DSL receivers have a serial communication port to connect
two receivers in diversity mode. Each receiver is sending the
received servo signals periodically over this port. This fact
can be used to connect the receiver to the control unit of the
model via UART instead of evaluating the PPM signal.
If you have any questions, fell free to send me an e-mail.
*/
/*
Connection of DSL to SV1 of FC:
( DSL Pin1 is on side of channel 4 )
1. GND <--> pin 7 (GND)
2. TXD <--> pin 3 (RXD1 Atmega644p)
3. RXD <--> pin 4 (TXD1 Atmega644p) optional
4. 5V <--> pin 2 (5V)
Do not connect the receiver via PPM-Sumsignal output the same time.
Data are send at every 20 ms @ 38400 Baud 8-N-1
Data Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|10|CH0D1|CH0D0|CH1D1|CH1D0|CRC| ...etc
FREQALLOC = 35, 40, 72
RSSI = 0.. 255 // Received signal strength indicator
VBAT = 0...255 // supply voltage (0.0V.. 7.8V)
Servo Pair: |0x1X|CHXD1|CHXD0|CHX+1D1|CHX+1D0|CRC|
X is channel index of 1 servo value
D1D0 is servo value as u16 in range of 7373 (1ms) to 14745 (2ms)
there are 8 channels submitted, i.e 4 servo pairs
Frame examples with signel received
FFFF 1F23F079A304AD 1036012B1E6F 122AFB2AECB2 142B4D2B4404 1636872B33CE
FFFF 1F23F079A304AD 1036002B1F6F 122AFE2AEBB0 142B4B2B4406 1636872B33CE
FFFF 1F23F079A304AD 1035FF2B226E 122AFC2AEAB3 142B4E2B4304 1636882B33CD
FFFF 1F23F079A304AD 1036022B1E6E 122AFB2AEEB0 142B4A2B4506 1636872B33CE
FFFF 1F23F079A304AD 1036022B1E6E 122AFE2AEBB0 142B4B2B4406 1636882B33CD
FFFF 1F23F079A304AD 1036012B1E6F 122AFD2AEAB2 142B4E2B4403 1636862B33CF
FFFF 1F23F079A304AD 1036032B1D6E 122AFD2AEBB1 142B4C2B4504 1636862B33CF
Frame examples with no signal received
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
FFFF 1F23F000A30426
*/
#include <stdlib.h>
#include "dsl.h"
#include "rc.h"
#include "uart0.h"
uint8_t dsl_RSSI = 0;
uint8_t dsl_Battery = 0;
uint8_t dsl_Allocation = 0;
uint8_t PacketBuffer[6];
//uint8_t Jitter = 0; // same measurement as RC_Quality in rc.c
typedef union {
int16_t Servo[2];
uint8_t byte[4];
} ChannelPair_t;
ChannelPair_t ChannelPair;
// This function is called, when a new servo signal is properly received.
// Parameters: servo - servo number (0-9)
// signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal) {
int16_t tmp;
uint8_t index = channel + 1; // mk channels start with 1
//RC_Quality = (212 * (uint16_t)dsl_RSSI) / 128; // have to be scaled approx. by a factor of 1.66 to get 200 at full level
//if(RC_Quality > 255) RC_Quality = 255;
// signal from DSL-receiver is between 7373 (1ms) und 14745 (2ms).
signal -= 11059; // shift to neutral
signal /= 24; // scale to mk rc resolution
if (abs(signal-PPM_in[index]) < 6) {
if (RC_Quality < 200)
RC_Quality += 10;
else
RC_Quality = 200;
}
// calculate exponential history for signal
tmp = (3 * (PPM_in[index]) + signal) / 4;
if (tmp > signal + 1)
tmp--;
else if (tmp < signal - 1)
tmp++;
// calculate signal difference on good signal level
if (RC_Quality >= 195)
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
else
PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
if (index == 4) {
NewPpmData = 0;
}
}
// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_decode_packet(void) {
uint8_t i;
// check for header condition
if ((PacketBuffer[0] & 0xF0) == 0x10) {
if (PacketBuffer[0] == 0x1F) // separate status frame
{
dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
// ?? = PacketBuffer[2];
dsl_RSSI = PacketBuffer[3]; // Get signal quality
dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
// ?? = PacketBuffer[5];
if (dsl_RSSI == 0) {
RC_Quality = 0;
for (i = 0; i < 5; i++) {
PPM_diff[i] = 0;
PPM_in[i] = 0;
}
}
} else // probably a channel pair
{
i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates the channel pair
if (i < 10)// maximum 12 channels
{
// big to little endian
ChannelPair.byte[1] = PacketBuffer[1];
ChannelPair.byte[0] = PacketBuffer[2];
ChannelPair.byte[3] = PacketBuffer[3];
ChannelPair.byte[2] = PacketBuffer[4];
dsl_new_signal(i, ChannelPair.Servo[0]);
dsl_new_signal(i + 1, ChannelPair.Servo[1]);
}
}
} // EOF header condition
}
// this function should be called within the UART RX ISR
void dsl_parser(uint8_t c) {
static uint8_t last_c = 0;
static uint8_t crc = 0;
static uint8_t cnt = 0;
static uint8_t packet_len = 0;
// check for sync condition
if ((c == 0xFF) && (last_c == 0xFF)) {
cnt = 0; // reset byte counter
crc = 0; // reset checksum
return;
}
if (cnt == 0) // begin of a packet
{
if (c == 0x1F)
packet_len = 5; // a status packet has 5 bytes + crc
else
packet_len = 4; // a channel pair packet has 4 bytes + crc
}
if (cnt > packet_len) // packet complete, crc byte received
{
// calculate checksum
crc = ~crc;
if (crc == 0xFF)
crc = 0xFE;
// if crc matches decode the packet
if (c == crc)
dsl_decode_packet();
// handle next packet
cnt = 0;
crc = 0;
} else // collect channel data bytes
{
PacketBuffer[cnt++] = c;
crc += c;
}
// store last byte for sync check
last_c = c;
}

/branches/dongfang_FC_fixedwing/dsl.h
0,0 → 1,14
#ifndef _DSL_H
#define _DSL_H
#include <inttypes.h>
extern uint8_t dsl_RSSI; // Received signal strength indicator
extern uint8_t dsl_Battery; // Battery voltage (0-255 [0V - 8.2V])
extern uint8_t dsl_Allocation; // Frequency allocation (35,40,72)
#define USART1_BAUD 38400
// this function should be called within the UART RX ISR
extern void dsl_parser(uint8_t c);
#endif //_DSL_H

/branches/dongfang_FC_fixedwing/eeprom.c
0,0 → 1,318
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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 "output.h"
// TODO: Get rid of these. They have nothing to do with eeprom.
#include "flight.h"
#include "rc.h"
#include "sensors.h"
// byte array in eeprom
uint8_t EEPromArray[E2END + 1] EEMEM;
paramset_t staticParams;
MixerTable_t Mixer;
/*
* Default for your own experiments here, so you don't have to reset them
* from MK-Tool all the time.
*/
void setDefaultUserParams(void) {
uint8_t i;
for (i = 0; i < sizeof(staticParams.UserParams1); i++) {
staticParams.UserParams1[i] = 0;
}
for (i = 0; i < sizeof(staticParams.UserParams2); i++) {
staticParams.UserParams2[i] = 0;
}
/*
* While we are still using userparams for flight parameters, do set
* some safe & meaningful default values.
*/
staticParams.UserParams2[0] = 0xd5; //0b11010101; // All gyro filter constants 2; acc. 4
staticParams.UserParams2[1] = 0; // H&I motor smoothing.
staticParams.UserParams2[2] = 120; // Yaw I factor
staticParams.UserParams2[3] = 100; // Max Z acceleration for acc. correction of angles.
}
void setOtherDefaults(void) {
/* Channel assignments were changed to the normal:
* Aileron/roll=1, elevator/pitch=2, throttle=3, yaw/rudder=4
*/
staticParams.ChannelAssignment[CH_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.IFactor = 32;
staticParams.ServoRefresh = 7;
staticParams.BitConfig = 0;
staticParams.J16Bitmask = 95;
staticParams.J17Bitmask = 243;
staticParams.J16Timing = 15;
staticParams.J17Timing = 15;
staticParams.ControlSigns = 2;
}
/***************************************************/
/* Default Values for parameter set 1 */
/***************************************************/
void ParamSet_DefaultSet1(void) { // sport
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.J16Timing = 10;
staticParams.J17Timing = 10;
memcpy(staticParams.Name, "Sport\0", 6);
}
/***************************************************/
/* Default Values for parameter set 2 */
/***************************************************/
void ParamSet_DefaultSet2(void) { // normal
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.Height_Gain = 3;
staticParams.J16Timing = 20;
staticParams.J17Timing = 20;
memcpy(staticParams.Name, "Normal\0", 7);
}
/***************************************************/
/* Default Values for parameter set 3 */
/***************************************************/
void ParamSet_DefaultSet3(void) { // beginner
setOtherDefaults();
gyro_setDefaults();
setDefaultUserParams();
staticParams.Height_Gain = 3;
staticParams.J16Timing = 30;
staticParams.J17Timing = 30;
memcpy(staticParams.Name, "Beginner\0", 9);
}
/***************************************************/
/* Read Parameter from EEPROM as byte */
/***************************************************/
uint8_t GetParamByte(uint16_t param_id) {
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);
}
/***************************************************/
/* 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]);
}
/***************************************************/
/* 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);
}
/***************************************************/
/* Read Parameter Set from EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_ReadFromEEProm(uint8_t setnumber) {
if ((1 > setnumber) || (setnumber > 5))
setnumber = 3;
eeprom_read_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
output_init();
}
/***************************************************/
/* Write Parameter Set to EEPROM */
/***************************************************/
// number [1..5]
void ParamSet_WriteToEEProm(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
return;
eeprom_write_block((uint8_t *) &staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_PARAMSET_BEGIN + PARAMSET_STRUCT_LEN * (setnumber
- 1)], PARAMSET_STRUCT_LEN);
eeprom_write_word((uint16_t *) &EEPromArray[EEPROM_ADR_PARAMSET_LENGTH],
PARAMSET_STRUCT_LEN);
eeprom_write_block(&staticParams.ChannelAssignment[0],
&EEPromArray[EEPROM_ADR_CHANNELS], 8); // backup the first 8 bytes that is the rc channel mapping
// set this parameter set to active set
setActiveParamSet(setnumber);
output_init();
}
/***************************************************/
/* 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);
}
/***************************************************/
/* Set active parameter set */
/***************************************************/
void setActiveParamSet(uint8_t setnumber) {
if (setnumber > 5)
setnumber = 5;
if (setnumber < 1)
setnumber = 1;
eeprom_write_byte(&EEPromArray[PID_ACTIVE_SET], setnumber);
}
/***************************************************/
/* Read MixerTable 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;
}
/***************************************************/
/* Write Mixer Table to EEPROM */
/***************************************************/
uint8_t MixerTable_WriteToEEProm(void) {
if (Mixer.Revision == EEMIXER_REVISION) {
eeprom_write_block((uint8_t *) &Mixer,
&EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer));
return 1;
} else
return 0;
}
/***************************************************/
/* Default Values for Mixer Table */
/***************************************************/
void MixerTable_Default(void) { // Quadro
uint8_t i;
Mixer.Revision = EEMIXER_REVISION;
// clear mixer table (but preset throttle)
for (i = 0; i < 16; i++) {
Mixer.Motor[i][MIX_THROTTLE] = i < 4 ? 64 : 0;
Mixer.Motor[i][MIX_PITCH] = 0;
Mixer.Motor[i][MIX_ROLL] = 0;
Mixer.Motor[i][MIX_YAW] = 0;
}
// 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);
}
/***************************************************/
/* Initialize EEPROM Parameter Sets */
/***************************************************/
void ParamSet_Init(void) {
uint8_t Channel_Backup = 1, i, j;
// parameter version check
if (eeprom_read_byte(&EEPromArray[PID_PARAM_REVISION]) != EEPARAM_REVISION) {
// if version check faild
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
for (i = 1; i < 6; i++) {
switch (i) {
case 1:
ParamSet_DefaultSet1(); // Fill staticParams Structure to default parameter set 1 (Sport)
break;
case 2:
ParamSet_DefaultSet2(); // Kamera
break;
case 3:
ParamSet_DefaultSet3(); // Beginner
break;
default:
ParamSet_DefaultSet2(); // Kamera
break;
}
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(i);
}
// default-Setting is parameter set 3
setActiveParamSet(1);
// update version info
SetParamByte(PID_PARAM_REVISION, EEPARAM_REVISION);
}
// read active parameter set to staticParams stucture
ParamSet_ReadFromEEProm(getActiveParamSet());
}

/branches/dongfang_FC_fixedwing/eeprom.h
0,0 → 1,36
#ifndef _EEPROM_H
#define _EEPROM_H
#include <inttypes.h>
#include "configuration.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)
extern void ParamSet_Init(void);
extern void ParamSet_ReadFromEEProm(uint8_t setnumber);
extern void ParamSet_WriteToEEProm(uint8_t setnumber);
extern uint8_t MixerTable_ReadFromEEProm(void);
extern uint8_t MixerTable_WriteToEEProm(void);
extern uint8_t GetParamByte(uint16_t param_id);
extern void SetParamByte(uint16_t param_id, uint8_t value);
extern uint16_t GetParamWord(uint16_t param_id);
extern void SetParamWord(uint16_t param_id, uint16_t value);
extern uint8_t getActiveParamSet(void);
extern void setActiveParamSet(uint8_t setnumber);
#endif //_EEPROM_H

/branches/dongfang_FC_fixedwing/externalControl.c
0,0 → 1,67
#include "externalcontrol.h"
#include "configuration.h"
#include "controlMixer.h"
ExternalControl_t externalControl;
uint8_t externalControlActive;
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;
// From main.c. What does it do??
externalControl.digital[0] = 0x55;
}
int16_t* EC_getEATR(void) {
return EC_EATR;
}
uint8_t EC_getArgument(void) {
return externalControl.config;
}
uint8_t EC_getCommand(void) {
return externalControl.free;
}
// not implemented.
int16_t EC_getVariable(uint8_t varNum) {
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))
// External control is not even configured.
return NO_SIGNAL;
// Configured but expired.
return SIGNAL_LOST;
}

/branches/dongfang_FC_fixedwing/externalControl.h
0,0 → 1,32
// Or does this simply belong in uart0.h??
#ifndef _EXTERNALCONTROL_H
#define _EXTERNALCONTROL_H
#include<inttypes.h>
typedef struct {
uint8_t digital[2];
uint8_t remoteButtons;
int8_t pitch;
int8_t roll;
int8_t yaw;
uint8_t throttle;
int8_t height;
uint8_t free; // Let's use that for commands now.
uint8_t frame;
uint8_t config; // Let's use that for arguemnts.
}__attribute__((packed)) ExternalControl_t;
extern ExternalControl_t externalControl;
extern uint8_t externalControlActive;
void EC_update(void);
int16_t* EC_getEATR(void);
uint8_t EC_getArgument(void);
uint8_t EC_getCommand(void);
int16_t EC_getVariable(uint8_t varNum);
void EC_calibrate(void);
uint8_t EC_getSignalQuality(void);
void EC_setNeutral(void);
#endif

/branches/dongfang_FC_fixedwing/flight.c
0,0 → 1,226
#include <stdlib.h>
#include <avr/io.h>
#include "eeprom.h"
#include "flight.h"
#include "output.h"
// Necessary for external control and motor test
#include "uart0.h"
// for scope debugging
// #include "rc.h"
#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???
*/
// int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
int8_t pitchPFactor, rollPFactor, yawPFactor;
int8_t pitchDFactor, rollDFactor, yawDFactor;
int32_t IPart[2] = {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;
}
}
/************************************************************************/
/* Neutral Readings */
/************************************************************************/
#define CONTROL_CONFIG_SCALE 10
void flight_setNeutral() {
MKFlags |= MKFLAG_CALIBRATE;
// not really used here any more.
controlMixer_initVariables();
}
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;
pitchDFactor = _pitchDFactor;
rollDFactor = _rollDFactor;
yawDFactor = _yawDFactor;
}
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
);
}
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];
// PID controller variables
int16_t PDPart[2], PDPartYaw;
static int8_t debugDataTimer = 1;
// High resolution motor values for smoothing of PID motor outputs
static int16_t outputFilters[MAX_OUTPUTS];
uint8_t i;
// 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];
/************************************************************************/
/* RC-signal is bad */
/************************************************************************/
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;
PDPart[ROLL] = ((int32_t) rate_PID[ROLL] * rollPFactor /
(256L / CONTROL_SCALING))
+ (differential[ROLL] * (int16_t) dynamicParams.GyroRollD) / 16;
PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING)
+ (differential[YAW] * (int16_t) dynamicParams.GyroYawD) / 16;
IPart[PITCH] = controlIntegrals[CONTROL_ELEVATOR] - angle[PITCH];
if (IPart[PITCH] > PITCHROLLOVER180) IPart[PITCH] -= PITCHROLLOVER360;
else if (IPart[PITCH] <= -PITCHROLLOVER180) IPart[PITCH] += PITCHROLLOVER360;
if (IPart[PITCH] > HH_RANGE) IPart[PITCH] = HH_RANGE;
else if (IPart[PITCH] < -HH_RANGE) IPart[PITCH] = -HH_RANGE;
IPart[ROLL] = controlIntegrals[CONTROL_AILERONS] - angle[ROLL];
if (IPart[ROLL] > PITCHROLLOVER180) IPart[ROLL] -= PITCHROLLOVER360;
else if (IPart[ROLL] <= -PITCHROLLOVER180) IPart[ROLL] += PITCHROLLOVER360;
if (IPart[ROLL] > HH_RANGE) IPart[ROLL] = HH_RANGE;
else if (IPart[ROLL] < -HH_RANGE) IPart[ROLL] = -HH_RANGE;
term[PITCH] = control[CONTROL_ELEVATOR] + (staticParams.ControlSigns & 1 ? PDPart[PITCH] : -PDPart[PITCH]);
term[ROLL] = control[CONTROL_AILERONS] + (staticParams.ControlSigns & 2 ? PDPart[ROLL] : -PDPart[ROLL]);
yawTerm = control[CONTROL_RUDDER] + (staticParams.ControlSigns & 4 ? PDPartYaw : -PDPartYaw);
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Universal Mixer
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
DebugOut.Analog[12] = term[PITCH];
DebugOut.Analog[13] = term[ROLL];
DebugOut.Analog[14] = throttleTerm;
DebugOut.Analog[15] = yawTerm;
for (i = 0; i < MAX_OUTPUTS; 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;
}
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Debugging
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
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[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_PITCHROLL; // in 0.1 deg
DebugOut.Analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
}
}

/branches/dongfang_FC_fixedwing/flight.h
0,0 → 1,43
/*********************************************************************************/
/* Flight Control */
/*********************************************************************************/
#ifndef _FLIGHT_H
#define _FLIGHT_H
#include <inttypes.h>
#include "analog.h"
#include "configuration.h"
#define PITCH 0
#define ROLL 1
/*
* 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.
*/
// looping params
// extern long TurnOver180Nick, TurnOver180Roll;
// external control
// extern int16_t ExternStickNick, ExternStickRoll, ExternStickYaw;
// extern int16_t naviAccPitch, naviAccRoll, naviCntAcc;
// 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);
#endif //_FLIGHT_H

/branches/dongfang_FC_fixedwing/invenSense.c
0,0 → 1,40
#include "invenSense.h"
#include "timer0.h"
#include "configuration.h"
#include <avr/io.h>
/*
* 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.
*/
const uint8_t GYRO_REVERSED[3] = { 0, 0, 0 };
const uint8_t ACC_REVERSED[3] = { 0, 0, 1 };
const uint8_t AXIS_TRANSFORM = 0;
#define AUTOZERO_PORT PORTD
#define AUTOZERO_DDR DDRD
#define AUTOZERO_BIT 5
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))) {
AUTOZERO_PORT |= (1 << AUTOZERO_BIT);
AUTOZERO_DDR |= (1 << AUTOZERO_BIT);
delay_ms(100);
}
// Make a pulse on the auto-zero output line.
AUTOZERO_PORT &= ~(1 << AUTOZERO_BIT);
delay_ms(1);
AUTOZERO_PORT |= (1 << AUTOZERO_BIT);
// Delay_ms(10);
delay_ms_Mess(100);
}
void gyro_setDefaults(void) {
staticParams.GyroAccFactor = 1;
staticParams.DriftComp = 3;
staticParams.GyroAccTrim = 2;
}

/branches/dongfang_FC_fixedwing/invenSense.h
0,0 → 1,30
/*
#ifndef _INVENSENSE_H
#define _INVENSENSE_H
#include "sensors.h"
#define GYRO_HW_NAME "ISens"
/ *
* The InvenSense gyros have a lower sensitivity than for example the ADXRS610s on the FC 2.0 ME,
* but they have a wider range too.
* 2mV/deg/s gyros and no amplifiers:
* H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)
* /
#define GYRO_HW_FACTOR 0.6827f
/ *
* Correction factor - determined experimentally: Hold the copter in the hand, and turn it 90 degrees.
* If AnglePitch or AngleRoll in debug in MK-Tool changes by x degrees, multiply the value here by x/90.
* If the hardware related contants are set correctly, flight should be OK without bothering to
* make any adjustments here. It is only for luxury.
* /
#define GYRO_PITCHROLL_CORRECTION 0.93f
/ *
* Same for yaw.
* /
#define GYRO_YAW_CORRECTION 0.97f
#endif
*/

/branches/dongfang_FC_fixedwing/main.c
0,0 → 1,165
#include <avr/boot.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "timer0.h"
#include "timer2.h"
#include "uart0.h"
#include "output.h"
#include "attitude.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 "eeprom.h"
int16_t main(void) {
uint16_t timer;
// disable interrupts global
cli();
// analyze hardware environment
CPUType = getCPUType();
BoardRelease = getBoardRelease();
// disable watchdog
MCUSR &= ~(1 << WDRF);
WDTCSR |= (1 << WDCE) | (1 << WDE);
WDTCSR = 0;
// PPM_in[CH_THROTTLE] = 0;
// Why??? They are already initialized to 0.
// stickPitch = stickRoll = stickYaw = 0;
RED_OFF;
// initalize modules
output_init();
timer0_init();
usart0_Init();
RC_Init();
analog_init();
I2C_init();
#ifdef USE_NAVICTRL
SPI_MasterInit();
#endif
#ifdef USE_MK3MAG
MK3MAG_Init();
#endif
// enable interrupts global
sei();
// Parameter Set handling
ParamSet_Init();
// Wait for a short time (otherwise the RC channel check won't work below)
// timer = SetDelay(500);
// while(!CheckDelay(timer));
// 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);
GRN_OFF;
RED_ON;
while (!checkDelay(timer))
;
OUTPUT_SET(0,0);
timer = setDelay(500);
while (!checkDelay(timer))
;
OUTPUT_SET(1,1);
RED_OFF;
GRN_ON;
timer = setDelay(500);
while (!checkDelay(timer))
;
timer = setDelay(500);
while (!checkDelay(timer))
;
OUTPUT_SET(1,0);
beep(2000);
timer = setDelay(4000);
while (!checkDelay(timer))
;
controlMixer_setNeutral();
// Cal. attitude sensors and reset integrals.
attitude_setNeutral();
// Init flight parameters
flight_setNeutral();
// RED_OFF;
beep(1000);
timer2_init();
I2CTimeout = 5000;
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;
} else {
DebugOut.Digital[0] &= ~DEBUG_MAINLOOP_TIMER;
J4HIGH;
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_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) {
beepBatteryAlarm();
}
#ifdef USE_NAVICTRL
SPI_StartTransmitPacket();
SendSPI = 4;
#endif
timer = setDelay(20); // every 20 ms
}
output_update();
}
#ifdef USE_NAVICTRL
if(!SendSPI) {
// SendSPI is decremented in timer0.c with a rate of 9.765 kHz.
// within the SPI_TransmitByte() routine the value is set to 4.
// I.e. the SPI_TransmitByte() is called at a rate of 9.765 kHz/4= 2441.25 Hz,
// and therefore the time of transmission of a complete spi-packet (32 bytes) is 32*4/9.765 kHz = 13.1 ms.
SPI_TransmitByte();
}
#endif
}
}
return (1);
}

/branches/dongfang_FC_fixedwing/main.h
0,0 → 1,3
#ifndef _MAIN_H
#define _MAIN_H
#endif //_MAIN_H

/branches/dongfang_FC_fixedwing/makefile
0,0 → 1,400
#--------------------------------------------------------------------
# MCU name
MCU = atmega644p
F_CPU = 20000000
QUARZ = 20MHZ
#-------------------------------------------------------------------
VERSION_MAJOR = 0
VERSION_MINOR = 0
VERSION_PATCH = 0
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version
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=invenSense
#GYRO_HW_NAME=Isense
#GYRO_HW_FACTOR=0.6827f
#GYRO_PITCHROLL_CORRECTION=1.0f
#GYRO_YAW_CORRECTION=1.0f
#-------------------------------------------------------------------
# get SVN revision
REV=0001
#$(shell sh -c "cat .svn/entries | sed -n '4p'")
ifeq ($(MCU), atmega644)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644_$(GYRO)
endif
ifeq ($(MCU), atmega644p)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644p_$(GYRO)
endif
# Output format. (can be srec, ihex, binary)
FORMAT = ihex
# Target file name (without extension).
TARGET = Flight-Ctrl_Fixedwing_$(HEX_NAME)
# Optimization level, can be [0, 1, 2, 3, s]. 0 turns off optimization.
# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
OPT = 2
#OPT = s
##########################################################################################################
# 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
##########################################################################################################
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
# will not be considered source files but generated files (assembler
# output from the compiler), and will be deleted upon "make clean"!
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS =
# Optional compiler flags.
# -g: generate debugging information (for GDB, or for COFF conversion)
# -O*: optimization level
# -f...: tuning, see gcc manual and avr-libc documentation
# -Wall...: warning level
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create assembler listing
CFLAGS = -O$(OPT) \
-funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums \
-Wall -Wstrict-prototypes \
-DGYRO=$(GYRO) \
-Wa,-adhlns=$(<:.c=.lst) \
$(patsubst %,-I%,$(EXTRAINCDIRS))
# Set a "language standard" compiler flag.
# Unremark just one line below to set the language standard to use.
# gnu99 = C99 + GNU extensions. See GCC manual for more information.
#CFLAGS += -std=c89
#CFLAGS += -std=gnu89
#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}
# Optional assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create listing
# -gstabs: have the assembler create line number information; note that
# for use in COFF files, additional information about filenames
# and function names needs to be present in the assembler source
# files -- see avr-libc docs [FIXME: not yet described there]
ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs
# Optional linker flags.
# -Wl,...: tell GCC to pass this to linker.
# -Map: create map file
# --cref: add cross reference to map file
LDFLAGS = -Wl,-Map=$(TARGET).map,--cref
# Additional libraries
# Minimalistic printf version
#LDFLAGS += -Wl,-u,vfprintf -lprintf_min
# Floating point printf version (requires -lm below)
#LDFLAGS += -Wl,-u,vfprintf -lprintf_flt
# -lm = math library
LDFLAGS += -lm
##LDFLAGS += -T./linkerfile/avr5.x
# Programming support using avrdude. Settings and variables.
# Programming hardware: alf avr910 avrisp bascom bsd
# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500
#
# Type: avrdude -c ?
# to get a full listing.
#
#AVRDUDE_PROGRAMMER = dt006
#AVRDUDE_PROGRAMMER = stk200
#AVRDUDE_PROGRAMMER = ponyser
#AVRDUDE_PROGRAMMER = avrispv2
AVRDUDE_PROGRAMMER = usbtiny
#falls Ponyser ausgewaehlt wird, muss sich unsere avrdude-Configdatei im Bin-Verzeichnis des Compilers befinden
#AVRDUDE_PORT = com1 # programmer connected to serial device
#AVRDUDE_PORT = lpt1 # programmer connected to parallel port
AVRDUDE_PORT = usb # programmer connected to USB
#AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex $(FUSE_SETTINGS)
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
#avrdude -c avrispv2 -P usb -p m32 -U flash:w:blink.hex
AVRDUDE_FLAGS = -p $(MCU) -c $(AVRDUDE_PROGRAMMER) -B 1 -F
# Uncomment the following if you want avrdude's erase cycle counter.
# Note that this counter needs to be initialized first using -Yn,
# see avrdude manual.
#AVRDUDE_ERASE += -y
# Uncomment the following if you do /not/ wish a verification to be
# performed after programming the device.
AVRDUDE_FLAGS += -V
# Increase verbosity level. Please use this when submitting bug
# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude>
# to submit bug reports.
#AVRDUDE_FLAGS += -v -v
# ---------------------------------------------------------------------------
# Define directories, if needed.
DIRAVR = c:/winavr
DIRAVRBIN = $(DIRAVR)/bin
DIRAVRUTILS = $(DIRAVR)/utils/bin
DIRINC = .
DIRLIB = $(DIRAVR)/avr/lib
# Define programs and commands.
SHELL = sh
CC = avr-gcc
OBJCOPY = avr-objcopy
OBJDUMP = avr-objdump
SIZE = avr-size
# Programming support using avrdude.
AVRDUDE = avrdude
REMOVE = rm -f
COPY = cp
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
ELFSIZE = $(SIZE) -A $(TARGET).elf
# Define Messages
# English
MSG_ERRORS_NONE = Errors: none
MSG_BEGIN = -------- begin --------
MSG_END = -------- end --------
MSG_SIZE_BEFORE = Size before:
MSG_SIZE_AFTER = Size after:
MSG_COFF = Converting to AVR COFF:
MSG_EXTENDED_COFF = Converting to AVR Extended COFF:
MSG_FLASH = Creating load file for Flash:
MSG_EEPROM = Creating load file for EEPROM:
MSG_EXTENDED_LISTING = Creating Extended Listing:
MSG_SYMBOL_TABLE = Creating Symbol Table:
MSG_LINKING = Linking:
MSG_COMPILING = Compiling:
MSG_ASSEMBLING = Assembling:
MSG_CLEANING = Cleaning project:
# Define all object files.
OBJ = $(SRC:.c=.o) $(ASRC:.S=.o)
# Define all listing files.
LST = $(ASRC:.S=.lst) $(SRC:.c=.lst)
# Combine all necessary flags and optional flags.
# Add target processor to flags.
#ALL_CFLAGS = -mmcu=$(MCU) -DF_CPU=$(F_CPU) -I. $(CFLAGS)
ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS)
ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)
# Default target.
all: begin gccversion sizebefore $(TARGET).elf $(TARGET).hex $(TARGET).eep \
$(TARGET).lss $(TARGET).sym sizeafter finished end
# Eye candy.
# AVR Studio 3.x does not check make's exit code but relies on
# the following magic strings to be generated by the compile job.
begin:
@echo
@echo $(MSG_BEGIN)
finished:
@echo $(MSG_ERRORS_NONE)
end:
@echo $(MSG_END)
@echo
# Display size of file.
sizebefore:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); echo; fi
sizeafter:
@if [ -f $(TARGET).elf ]; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); echo; fi
# Display compiler version information.
gccversion :
@$(CC) --version
# Convert ELF to COFF for use in debugging / simulating in
# AVR Studio or VMLAB.
COFFCONVERT=$(OBJCOPY) --debugging \
--change-section-address .data-0x800000 \
--change-section-address .bss-0x800000 \
--change-section-address .noinit-0x800000 \
--change-section-address .eeprom-0x810000
coff: $(TARGET).elf
@echo
@echo $(MSG_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-avr $< $(TARGET).cof
extcoff: $(TARGET).elf
@echo
@echo $(MSG_EXTENDED_COFF) $(TARGET).cof
$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof
# Program the device.
program: $(TARGET).hex $(TARGET).eep
$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
# Create final output files (.hex, .eep) from ELF output file.
%.hex: %.elf
@echo
@echo $(MSG_FLASH) $@
$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@
%.eep: %.elf
@echo
@echo $(MSG_EEPROM) $@
-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \
--change-section-lma .eeprom=0 -O $(FORMAT) $< $@
# Create extended listing file from ELF output file.
%.lss: %.elf
@echo
@echo $(MSG_EXTENDED_LISTING) $@
$(OBJDUMP) -h -S $< > $@
# Create a symbol table from ELF output file.
%.sym: %.elf
@echo
@echo $(MSG_SYMBOL_TABLE) $@
avr-nm -n $< > $@
# Link: create ELF output file from object files.
.SECONDARY : $(TARGET).elf
.PRECIOUS : $(OBJ)
%.elf: $(OBJ)
@echo
@echo $(MSG_LINKING) $@
$(CC) $(ALL_CFLAGS) $(OBJ) --output $@ $(LDFLAGS)
# Compile: create object files from C source files.
%.o : %.c
@echo
@echo $(MSG_COMPILING) $<
$(CC) -c $(ALL_CFLAGS) $< -o $@
# Compile: create assembler files from C source files.
%.s : %.c
$(CC) -S $(ALL_CFLAGS) $< -o $@
# Assemble: create object files from assembler source files.
%.o : %.S
@echo
@echo $(MSG_ASSEMBLING) $<
$(CC) -c $(ALL_ASFLAGS) $< -o $@
# Target: clean project.
clean: begin clean_list finished end
clean_list :
@echo
@echo $(MSG_CLEANING)
# $(REMOVE) $(TARGET).hex
$(REMOVE) $(TARGET).eep
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).cof
$(REMOVE) $(TARGET).elf
$(REMOVE) $(TARGET).map
$(REMOVE) $(TARGET).obj
$(REMOVE) $(TARGET).a90
$(REMOVE) $(TARGET).sym
$(REMOVE) $(TARGET).lnk
$(REMOVE) $(TARGET).lss
$(REMOVE) $(OBJ)
$(REMOVE) $(LST)
$(REMOVE) $(SRC:.c=.s)
$(REMOVE) $(SRC:.c=.d)
# Automatically generate C source code dependencies.
# (Code originally taken from the GNU make user manual and modified
# (See README.txt Credits).)
#
# Note that this will work with sh (bash) and sed that is shipped with WinAVR
# (see the SHELL variable defined above).
# This may not work with other shells or other seds.
#
%.d: %.c
set -e; $(CC) -MM $(ALL_CFLAGS) $< \
| sed 's,\(.*\)\.o[ :]*,\1.o \1.d : ,g' > $@; \
[ -s $@ ] || rm -f $@
# Remove the '-' if you want to see the dependency files generated.
-include $(SRC:.c=.d)
# Listing of phony targets.
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \
clean clean_list program

/branches/dongfang_FC_fixedwing/old_macros.h
0,0 → 1,47
/*
For backwards compatibility only.
Ingo Busker ingo@mikrocontroller.com
*/
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
#ifndef inb
#define inb(sfr) _SFR_BYTE(sfr)
#endif
#ifndef outb
#define outb(sfr, val) (_SFR_BYTE(sfr) = (val))
#endif
#ifndef inw
#define inw(sfr) _SFR_WORD(sfr)
#endif
#ifndef outw
#define outw(sfr, val) (_SFR_WORD(sfr) = (val))
#endif
#ifndef outp
#define outp(val, sfr) outb(sfr, val)
#endif
#ifndef inp
#define inp(sfr) inb(sfr)
#endif
#ifndef BV
#define BV(bit) _BV(bit)
#endif
#ifndef PRG_RDB
#define PRG_RDB pgm_read_byte
#endif

/branches/dongfang_FC_fixedwing/output.c
0,0 → 1,130
#include <inttypes.h>
#include "output.h"
#include "eeprom.h"
#include "timer0.h"
// To access the DebugOut struct.
#include "uart0.h"
uint8_t flashCnt[2], flashMask[2];
// initializes the LED control outputs J16, J17
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);
flashCnt[0] = flashCnt[1] = 0;
flashMask[0] = flashMask[1] = 128;
}
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);
} 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));
} else if (!flashCnt[port]--) {
// rotating mask over bitmask...
flashCnt[port] = timing - 1;
if (flashMask[port] == 1)
flashMask[port] = 128;
else
flashMask[port] >>= 1;
OUTPUT_SET(port, flashMask[port] & bitmask);
}
}
void flashingLights(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);
} else {
OUTPUT_SET(0, DebugOut.Digital[0] & DIGITAL_DEBUG_MASK);
OUTPUT_SET(1, DebugOut.Digital[1] & DIGITAL_DEBUG_MASK);
}
}
void beep(uint16_t millis) {
beepTime = millis;
}
/*
* Make [numbeeps] beeps.
*/
void beepNumber(uint8_t numbeeps) {
while(numbeeps--) {
if(MKFlags & MKFLAG_MOTOR_RUN) return; //auf keinen Fall bei laufenden Motoren!
beep(100); // 0.1 second
delay_ms(250); // blocks 250 ms as pause to next beep,
// this will block the flight control loop,
// therefore do not use this function if motors are running
}
}
/*
* Beep the R/C alarm signal
*/
void beepRCAlarm(void) {
if(beepModulation == 0xFFFF) { // If not already beeping an alarm signal (?)
beepTime = 15000; // 1.5 seconds
beepModulation = 0x0C00;
}
}
/*
* 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;
if(!beepTime) {
beepTime = 6000; // 0.6 seconds
}
}
/*
* Beep the EEPROM checksum alarm
*/
void beepEEPROMAlarm(void) {
beepModulation = 0x0007;
if(!beepTime) {
beepTime = 6000; // 0.6 seconds
}
}

/branches/dongfang_FC_fixedwing/output.h
0,0 → 1,75
#ifndef _OUTPUT_H
#define _OUTPUT_H
#include <avr/io.h>
#define J3HIGH PORTD |= (1<<PORTD5)
#define J3LOW PORTD &= ~(1<<PORTD5)
#define J3TOGGLE PORTD ^= (1<<PORTD5)
#define J4HIGH PORTD |= (1<<PORTD4)
#define J4LOW PORTD &= ~(1<<PORTD4)
#define J4TOGGLE PORTD ^= (1<<PORTD4)
#define J5HIGH PORTD |= (1<<PORTD3)
#define J5LOW PORTD &= ~(1<<PORTD3)
#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 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));}
/*
* Some digital debugs. A digital debug is 2 signals on the 2 LED outputs,
* turned on and off depending on some condtions given in the code.
* Only one can be selected, by defining DIGITAL_DEBUG_MASK to the value
* of the debug.
* In the code one can do like:
* if (whatever_condition) {
* DebugOut.Digital[0] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[0] &= ~DEBUG_MYOWNDEBUGGER;
* }
* ...
* if (whatever_other_condition) {
* DebugOut.Digital[1] |= DEBUG_MYOWNDEBUGGER;
* } else {
* DebugOut.Digital[1] &= ~DEBUG_MYOWNDEBUGGER;
* }
*
* 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_PRESSURERANGE 32
#define DEBUG_CLIP 64
#define DEBUG_SENSORLIMIT 128
/*
* 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
void output_init(void);
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
0,0 → 1,258
#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "rc.h"
#include "uart0.h"
#include "controlMixer.h"
#include "configuration.h"
#include "commands.h"
// The channel array is 1-based. The 0th entry is not used.
volatile int16_t PPM_in[MAX_CHANNELS];
volatile uint8_t NewPpmData = 1;
volatile int16_t RC_Quality = 0;
int16_t RC_PRTY[4];
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
***************************************************************/
void RC_Init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
DDRD &= ~(1 << DDD6);
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);
// low level
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);
}
// Timer/Counter1 Control Register A, B, C
// Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0)
// Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0)
// 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
// The longest period is 0xFFFF / 312.5 kHz = 0.209712 s.
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);
RC_Quality = 0;
SREG = sreg;
}
/********************************************************************/
/* Every time a positive edge is detected at PD6 */
/********************************************************************/
/* t-Frame
<----------------------------------------------------------------------->
____ ______ _____ ________ ______ 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 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;
static int16_t index;
static uint16_t oldICR1 = 0;
// 16bit Input Capture Register ICR1 contains the timer value TCNT1
// at the time the edge was detected
// calculate the time delay to the previous event time which is stored in oldICR1
// calculatiing the difference of the two uint16_t and converting the result to an int16_t
// implicit handles a timer overflow 65535 -> 0 the right way.
signal = (uint16_t) ICR1 - oldICR1;
oldICR1 = ICR1;
//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;
} else { // within the PPM frame
if (index < MAX_CHANNELS - 1) { // 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
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).
if (abs(signal - PPM_in[index]) < 6) {
if (RC_Quality < 200)
RC_Quality += 10;
else
RC_Quality = 200;
}
PPM_in[index] = signal; // update channel value
}
index++; // next channel
}
}
}
#define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]]
/*
* This must be called (as the only thing) for each control loop cycle (488 Hz).
*/
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;
}
} else { // Bad signal
RC_PRTY[CONTROL_ELEVATOR] = RC_PRTY[CONTROL_AILERONS] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_RUDDER] = 0;
}
}
/*
* 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;
/*
* 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...
*/
return PPM_in[varNum + 1] + POT_OFFSET;
}
uint8_t RC_getSignalQuality(void) {
if (RC_Quality >= 160)
return SIGNAL_GOOD;
if (RC_Quality >= 140)
return SIGNAL_OK;
if (RC_Quality >= 120)
return SIGNAL_BAD;
return SIGNAL_LOST;
}
/*
* To should fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void RC_calibrate(void) {
// 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.
return COMMAND_NONE;
}
/*
* Command arguments on R/C:
* 2--3--4
* | | +
* 1 0 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
uint8_t RC_getArgument(void) {
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
0,0 → 1,42
#ifndef _RC_H
#define _RC_H
#include <inttypes.h>
#define MAX_CHANNELS 10
// Number of cycles a command must be repeated before commit.
#define COMMAND_TIMER 200
extern void RC_Init(void);
// the RC-Signal. todo: Not export any more.
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)
// defines for lookup staticParams.ChannelAssignment
#define CH_ELEVATOR 0
#define CH_AILERONS 1
#define CH_THROTTLE 2
#define CH_RUDDER 3
#define CH_POTS 4
#define POT_OFFSET 115
/*
int16_t RC_getPitch (void);
int16_t RC_getYaw (void);
int16_t RC_getRoll (void);
uint16_t RC_getThrottle (void);
uint8_t RC_hasNewRCData (void);
*/
void RC_update(void);
int16_t* RC_getEATR(void);
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_testCompassCalState(void);
#endif //_RC_H

/branches/dongfang_FC_fixedwing/sensors.h
0,0 → 1,38
#ifndef _SENSORS_H
#define _SENSORS_H
#include <inttypes.h>
/*
* Whether (pitch, roll, yaw) gyros are reversed (see analog.h).
*/
extern const uint8_t GYRO_QUADRANT;
extern const uint8_t YAW_REVERSED;
/*
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h).
*/
//extern const uint8_t ACC_QUADRANT;
/*
* Whether the FC is installed 45 degs turned. If<>0, then pitch'<-pitch+roll and roll'<-roll-pitch. If the signs are not right for you,
* change signs for the individual pitch and roll gyros in GYRO_REVERSED. All 4 combinations on FC installation with a side facing forward
* are possible.
*/
extern const uint8_t GYROS_REVERSED;
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.
* InvenSense hardware: Output a pulse on the AUTO_ZERO line.
*/
void gyro_calibrate(void);
/*
* Set some default FC parameters, depending on gyro type: Drift correction etc.
*/
void gyro_setDefaults(void);
#endif

/branches/dongfang_FC_fixedwing/timer0.c
0,0 → 1,172
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "eeprom.h"
#include "analog.h"
// for debugging!
#include "uart0.h"
#include "output.h"
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
volatile uint16_t millisecondsCount = 0;
volatile uint8_t runFlightControl = 0;
volatile uint16_t cntKompass = 0;
volatile uint16_t beepTime = 0;
volatile uint16_t beepModulation = 0xFFFF;
#ifdef USE_NAVICTRL
volatile uint8_t SendSPI = 0;
#endif
/*****************************************************
* Initialize Timer 0
*****************************************************/
// timer 0 is used for the PWM generation to control the offset voltage at the air pressure sensor
// Its overflow interrupt routine is used to generate the beep signal and the flight control motor update rate
void timer0_init(void) {
uint8_t sreg = SREG;
// disable all interrupts before reconfiguration
cli();
// Configure speaker port as output.
if (BoardRelease == 10) { // Speaker at PD2
DDRD |= (1 << DDD2);
PORTD &= ~(1 << PORTD2);
} else { // Speaker at PC7
DDRC |= (1 << DDC7);
PORTC &= ~(1 << PORTC7);
}
// set PB3 and PB4 as output for the PWM used as offset for the pressure sensor
DDRB |= (1 << DDB4) | (1 << DDB3);
PORTB &= ~((1 << PORTB4) | (1 << PORTB3));
// Timer/Counter 0 Control Register A
// Waveform Generation Mode is Fast PWM (Bits WGM02 = 0, WGM01 = 1, WGM00 = 1)
// Clear OC0A on Compare Match, set OC0A at BOTTOM, noninverting PWM (Bits COM0A1 = 1, COM0A0 = 0)
// Clear OC0B on Compare Match, set OC0B at BOTTOM, (Bits COM0B1 = 1, COM0B0 = 0)
TCCR0A &= ~((1 << COM0A0) | (1 << COM0B0));
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
// 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
// divider 8 (Bits CS02 = 0, CS01 = 1, CS00 = 0)
TCCR0B &= ~((1 << FOC0A) | (1 << FOC0B) | (1 << WGM02));
TCCR0B = (TCCR0B & 0xF8) | (0 << CS02) | (1 << CS01) | (0 << CS00);
// initialize the Output Compare Register A & B used for PWM generation on port PB3 & PB4
OCR0A = 0; // for PB3
OCR0B = 120; // for PB4
// init Timer/Counter 0 Register
TCNT0 = 0;
// Timer/Counter 0 Interrupt Mask Register
// enable timer overflow interrupt only
TIMSK0 &= ~((1 << OCIE0B) | (1 << OCIE0A));
TIMSK0 |= (1 << TOIE0);
SREG = sreg;
}
/*****************************************************/
/* Interrupt Routine of Timer 0 */
/*****************************************************/
ISR(TIMER0_OVF_vect)
{ // 9765.625 Hz
static uint8_t cnt_1ms = 1, cnt = 0;
uint8_t beeper_On = 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
#endif
if (!cnt--) { // every 10th run (9.765625kHz/10 = 976.5625Hz)
cnt = 9;
cnt_1ms ^= 1;
if (!cnt_1ms) {
if (runFlightControl == 1)
DebugOut.Digital[1] |= DEBUG_MAINLOOP_TIMER;
else
DebugOut.Digital[1] &= ~DEBUG_MAINLOOP_TIMER;
runFlightControl = 1; // every 2nd run (976.5625 Hz/2 = 488.28125 Hz)
}
millisecondsCount++; // increment millisecond counter
}
// beeper on if duration is not over
if (beepTime) {
beepTime--; // decrement BeepTime
if (beepTime & beepModulation)
beeper_On = 1;
else
beeper_On = 0;
} else { // beeper off if duration is over
beeper_On = 0;
beepModulation = 0xFFFF;
}
// if beeper is on
if (beeper_On) {
// set speaker port to high.
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)
PORTD &= ~(1 << PORTD2);// Speaker at PD2
else
PORTC &= ~(1 << PORTC7);// Speaker at PC7
}
#ifndef USE_NAVICTRL
// 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
}
#endif
}
// -----------------------------------------------------------------------
uint16_t setDelay(uint16_t t) {
return (millisecondsCount + t - 1);
}
// -----------------------------------------------------------------------
int8_t checkDelay(uint16_t t) {
return (((t - millisecondsCount) & 0x8000) >> 8); // check sign bit
}
// -----------------------------------------------------------------------
void delay_ms(uint16_t w) {
uint16_t t_stop = setDelay(w);
while (!checkDelay(t_stop))
;
}
// -----------------------------------------------------------------------
void delay_ms_Mess(uint16_t w) {
uint16_t t_stop;
t_stop = setDelay(w);
while (!checkDelay(t_stop)) {
if (analogDataReady) {
analogDataReady = 0;
analog_start();
}
}
}

/branches/dongfang_FC_fixedwing/timer0.h
0,0 → 1,21
#ifndef _TIMER0_H
#define _TIMER0_H
#include <inttypes.h>
extern volatile uint16_t millisecondsCount;
extern volatile uint8_t runFlightControl;
extern volatile uint16_t cntKompass;
extern volatile uint16_t beepModulation;
extern volatile uint16_t beepTime;
#ifdef USE_NAVICTRL
extern volatile uint8_t SendSPI;
#endif
extern void timer0_init(void);
extern void delay_ms(uint16_t w);
extern void delay_ms_Mess(uint16_t w);
extern uint16_t setDelay(uint16_t t);
extern int8_t checkDelay(uint16_t t);
#endif //_TIMER0_H

/branches/dongfang_FC_fixedwing/timer2.c
0,0 → 1,119
#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 HEF4017R_ON PORTC |= (1<<PORTC6)
#define HEF4017R_OFF PORTC &= ~(1<<PORTC6)
OutputData_t outputs[MAX_OUTPUTS];
/*****************************************************
* 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);
SREG = sreg;
}
/*****************************************************
* 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)
ISR(TIMER2_COMPA_vect) {
static uint16_t remainingPulseTime = 0;
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) {
// 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;
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;
}
}
// 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,
// as long as it happens once per pulse. This will, because all pulses are > 255+128 long.
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
// condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK.
delta = 128;
} 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 = delta;
remainingPulseTime -= delta;
}

/branches/dongfang_FC_fixedwing/timer2.h
0,0 → 1,16
#ifndef _TIMER2_H
#define _TIMER2_H
#include <inttypes.h>
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/twimaster.c
0,0 → 1,164
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/twi.h>
#include <util/delay.h>
//#include "eeprom.h"
#include "twimaster.h"
//#include "analog.h"
#include "configuration.h"
volatile uint8_t twi_state = TWI_STATE_IDLE;
volatile uint8_t dac_channel = 0;
volatile uint16_t I2CTimeout = 100;
volatile uint8_t DACValues[4];
uint8_t DACChannel = 0;
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
/**************************************************
* Initialize I2C (TWI)
**************************************************/
void I2C_init(void) {
uint8_t i;
uint8_t sreg = SREG;
cli();
// SDA is INPUT
DDRC &= ~(1 << DDC1);
// SCL is output
DDRC |= (1 << DDC0);
// pull up SDA
PORTC |= (1 << PORTC0) | (1 << PORTC1);
// TWI Status Register
// prescaler 1 (TWPS1 = 0, TWPS0 = 0)
TWSR &= ~((1 << TWPS1) | (1 << TWPS0));
// set TWI Bit Rate Register
TWBR = ((SYSCLK / SCL_CLOCK) - 16) / 2;
twi_state = TWI_STATE_IDLE;
SREG = sreg;
}
/****************************************
* Start I2C
****************************************/
void I2C_Start(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// enable TWI START Condition Bit (TWSTA = 1), MASTER
// disable TWI STOP Condition Bit (TWSTO = 0)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWEN = 1)
// enable TWI Interrupt (TWIE = 1)
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWIE);
}
/****************************************
* Stop I2C
****************************************/
void I2C_Stop(uint8_t start_state) {
twi_state = start_state;
// TWI Control Register
// clear TWI interrupt flag (TWINT=1)
// disable TWI Acknowledge Bit (TWEA = 0)
// diable TWI START Condition Bit (TWSTA = 1), no MASTER
// enable TWI STOP Condition Bit (TWSTO = 1)
// disable TWI Write Collision Flag (TWWC = 0)
// enable i2c (TWEN = 1)
// disable TWI Interrupt (TWIE = 0)
TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
}
/****************************************
* Write to I2C
****************************************/
void I2C_WriteByte(int8_t byte) {
// move byte to send into TWI Data Register
TWDR = byte;
// clear interrupt flag (TWINT = 1)
// enable i2c bus (TWEN = 1)
// enable interrupt (TWIE = 1)
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);
}
/****************************************
* Receive byte and send ACK
****************************************/
void I2C_ReceiveByte(void) {
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE) | (1 << TWEA);
}
/****************************************
* I2C receive last byte and send no ACK
****************************************/
void I2C_ReceiveLastByte(void) {
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWIE);
}
/****************************************
* Reset I2C
****************************************/
void I2C_Reset(void) {
// stop i2c bus
I2C_Stop(TWI_STATE_IDLE);
TWCR = (1 << TWINT); // reset to original state incl. interrupt flag reset
TWAMR = 0;
TWAR = 0;
TWDR = 0;
TWSR = 0;
TWBR = 0;
I2C_init();
}
/****************************************
* I2C ISR
****************************************/
ISR (TWI_vect)
{
switch (twi_state) { // First i2c_start from SendMotorData()
case 0: break;
// Writing ADC values.
case 7:
twi_state++;
I2C_WriteByte(0x98); // Address the DAC
break;
case 8:
twi_state++;
I2C_WriteByte(0x10 + (DACChannel << 1)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
break;
case 9:
twi_state++;
I2C_WriteByte(DACValues[DACChannel]);
break;
case 10:
twi_state++;
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80
break;
case 11:
twi_state++;
I2C_Stop(TWI_STATE_IDLE);
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if (DACChannel < 2) {
DACChannel++; // jump to next channel
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
} else {
DACChannel = 0; // reset dac channel counter
}
break;
default:
I2C_Stop(TWI_STATE_IDLE);
I2CTimeout = 10;
}
}

/branches/dongfang_FC_fixedwing/twimaster.h
0,0 → 1,20
#ifndef _I2C_MASTER_H
#define _I2C_MASTER_H
#include <inttypes.h>
#define TWI_STATE_IDLE 0
#define TWI_STATE_GYRO_OFFSET_TX 7
extern volatile uint8_t twi_state;
volatile extern uint8_t DACValues[4];
extern volatile uint16_t I2CTimeout;
extern void I2C_init(void); // Initialize I2C
extern void I2C_Start(uint8_t start_state); // Start I2C
extern void I2C_Stop(uint8_t start_state); // Stop I2C
extern void I2C_Reset(void); // Reset I2C
#endif

/branches/dongfang_FC_fixedwing/uart0.c
0,0 → 1,699
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <stdarg.h>
#include <string.h>
#include "eeprom.h"
#include "timer0.h"
#include "uart0.h"
#include "rc.h"
#include "externalControl.h"
#include "output.h"
#include "attitude.h"
#ifdef USE_MK3MAG
#include "mk3mag.h"
#endif
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3
#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;
uint8_t request_PPMChannels = FALSE;
uint8_t request_OutputTest = FALSE;
uint8_t request_variables = FALSE;
uint8_t DisplayLine = 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 *pRxData = 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;
typedef struct {
int16_t Heading;
}__attribute__((packed)) Heading_t;
DebugOut_t DebugOut;
Data3D_t Data3D;
UART_VersionInfo_t UART_VersionInfo;
uint16_t DebugData_Timer;
uint16_t Data3D_Timer;
uint16_t DebugData_Interval = 500; // in 1ms
uint16_t Data3D_Interval = 0; // in 1ms
#ifdef USE_MK3MAG
int16_t Compass_Timer;
#endif
// keep lables in flash to save 512 bytes of sram space
const prog_uint8_t ANALOG_LABEL[32][16] = {
//1234567890123456
"AnglePitch ", //0
"AngleRoll ",
"AngleYaw ",
"GyroPitch(PID) ",
"GyroRoll(PID) ",
"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 " };
/****************************************************************/
/* Initialization of the USART0 */
/****************************************************************/
void usart0_Init(void) {
uint8_t sreg = SREG;
uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK / (8 * USART0_BAUD) - 1);
// disable all interrupts before configuration
cli();
// disable RX-Interrupt
UCSR0B &= ~(1 << RXCIE0);
// disable TX-Interrupt
UCSR0B &= ~(1 << TXCIE0);
// set direction of RXD0 and TXD0 pins
// set RXD0 (PD0) as an input pin
PORTD |= (1 << PORTD0);
DDRD &= ~(1 << DDD0);
// set TXD0 (PD1) as an output pin
PORTD |= (1 << PORTD1);
DDRD |= (1 << DDD1);
// USART0 Baud Rate Register
// set clock divider
UBRR0H = (uint8_t) (ubrr >> 8);
UBRR0L = (uint8_t) ubrr;
// USART0 Control and Status Register A, B, C
// enable double speed operation in
UCSR0A |= (1 << U2X0);
// enable receiver and transmitter in
UCSR0B = (1 << TXEN0) | (1 << RXEN0);
// set asynchronous mode
UCSR0C &= ~(1 << UMSEL01);
UCSR0C &= ~(1 << UMSEL00);
// no parity
UCSR0C &= ~(1 << UPM01);
UCSR0C &= ~(1 << UPM00);
// 1 stop bit
UCSR0C &= ~(1 << USBS0);
// 8-bit
UCSR0B &= ~(1 << UCSZ02);
UCSR0C |= (1 << UCSZ01);
UCSR0C |= (1 << UCSZ00);
// flush receive buffer
while (UCSR0A & (1 << RXC0))
UDR0;
// enable interrupts at the end
// enable RX-Interrupt
UCSR0B |= (1 << RXCIE0);
// enable TX-Interrupt
UCSR0B |= (1 << TXCIE0);
// initialize the debug timer
DebugData_Timer = setDelay(DebugData_Interval);
// unlock rxd_buffer
rxd_buffer_locked = FALSE;
pRxData = 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;
// restore global interrupt flags
SREG = sreg;
}
/****************************************************************/
/* USART0 transmitter ISR */
/****************************************************************/
ISR(USART0_TX_vect)
{
static uint16_t ptr_txd_buffer = 0;
uint8_t tmp_tx;
if (!txd_complete) { // transmission not completed
ptr_txd_buffer++; // die [0] wurde schon gesendet
tmp_tx = txd_buffer[ptr_txd_buffer];
// if terminating character or end of txd buffer was reached
if ((tmp_tx == '\r') || (ptr_txd_buffer == TXD_BUFFER_LEN)) {
ptr_txd_buffer = 0; // reset txd pointer
txd_complete = 1; // stop transmission
}
UDR0 = tmp_tx; // send current byte will trigger this ISR again
}
// transmission completed
else
ptr_txd_buffer = 0;
}
/****************************************************************/
/* USART0 receiver ISR */
/****************************************************************/
ISR(USART0_RX_vect)
{
static uint16_t crc;
static uint8_t ptr_rxd_buffer = 0;
uint8_t crc1, crc2;
uint8_t c;
c = UDR0; // catch the received byte
if (rxd_buffer_locked)
return; // if rxd buffer is locked immediately return
// 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
}
#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
} 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];
// calculate checksum from transmitted data
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
// compare checksum to transmitted checksum bytes
if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2
== 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
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') {
wdt_enable(WDTO_250MS);
} // Reset-Commando
} else { // checksum invalid
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
ptr_rxd_buffer = 0; // reset rxd buffer pointer
}
} else { // rxd buffer overrun
ptr_rxd_buffer = 0; // reset rxd buffer
rxd_buffer_locked = FALSE; // unlock rxd buffer
}
}
// --------------------------------------------------------------------------
void AddCRC(uint16_t datalen) {
uint16_t tmpCRC = 0, i;
for (i = 0; i < datalen; i++) {
tmpCRC += txd_buffer[i];
}
tmpCRC %= 4096;
txd_buffer[i++] = '=' + tmpCRC / 64;
txd_buffer[i++] = '=' + tmpCRC % 64;
txd_buffer[i++] = '\r';
txd_complete = FALSE;
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR)
}
// --------------------------------------------------------------------------
// application example:
// 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, ...
va_list ap;
uint16_t txd_bufferIndex = 0;
uint8_t *currentBuffer;
uint8_t currentBufferIndex;
uint16_t lengthOfCurrentBuffer;
uint8_t shift = 0;
txd_buffer[txd_bufferIndex++] = '#'; // Start character
txd_buffer[txd_bufferIndex++] = 'a' + addr; // Address (a=0; b=1,...)
txd_buffer[txd_bufferIndex++] = cmd; // Command
va_start(ap, numofbuffers);
while(numofbuffers) {
currentBuffer = va_arg(ap, uint8_t*);
lengthOfCurrentBuffer = va_arg(ap, int);
currentBufferIndex = 0;
// Encode data: 3 bytes of data are encoded into 4 bytes,
// where the 2 most significant bits are both 0.
while(currentBufferIndex != lengthOfCurrentBuffer) {
if (!shift) txd_buffer[txd_bufferIndex] = 0;
txd_buffer[txd_bufferIndex] |= currentBuffer[currentBufferIndex] >> (shift + 2);
txd_buffer[++txd_bufferIndex] = (currentBuffer[currentBufferIndex] << (4 - shift)) & 0b00111111;
shift += 2;
if (shift == 6) { shift=0; txd_bufferIndex++; }
currentBufferIndex++;
}
}
// If the number of data bytes was not divisible by 3, stuff
// with 0 pseudodata until length is again divisible by 3.
if (shift == 2) {
// We need to stuff with zero bytes at the end.
txd_buffer[txd_bufferIndex] &= 0b00110000;
txd_buffer[++txd_bufferIndex] = 0;
shift = 4;
}
if (shift == 4) {
// We need to stuff with zero bytes at the end.
txd_buffer[txd_bufferIndex++] &= 0b00111100;
txd_buffer[txd_bufferIndex] = 0;
}
va_end(ap);
AddCRC(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, ...
va_list ap;
uint16_t pt = 0;
uint8_t a, b, c;
uint8_t ptr = 0;
uint8_t *pdata = 0;
int len = 0;
txd_buffer[pt++] = '#'; // Start character
txd_buffer[pt++] = 'a' + addr; // Address (a=0; b=1,...)
txd_buffer[pt++] = cmd; // Command
va_start(ap, numofbuffers);
if (numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
while (len) {
if (len) {
a = pdata[ptr++];
len--;
if ((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
} else
a = 0;
if (len) {
b = pdata[ptr++];
len--;
if ((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
} else
b = 0;
if (len) {
c = pdata[ptr++];
len--;
if ((!len) && numofbuffers) {
pdata = va_arg(ap, uint8_t*);
len = va_arg(ap, int);
ptr = 0;
numofbuffers--;
}
} else
c = 0;
txd_buffer[pt++] = '=' + (a >> 2);
txd_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
txd_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
txd_buffer[pt++] = '=' + (c & 0x3f);
}
va_end(ap);
AddCRC(pt); // add checksum after data block and initates the transmission
}
// --------------------------------------------------------------------------
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;
while (len) {
a = rxd_buffer[ptrIn++] - '=';
b = rxd_buffer[ptrIn++] - '=';
c = rxd_buffer[ptrIn++] - '=';
d = rxd_buffer[ptrIn++] - '=';
//if(ptrIn > ReceivedBytes - 3) break;
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
if (len--)
rxd_buffer[ptrOut++] = x;
else
break;
if (len--)
rxd_buffer[ptrOut++] = y;
else
break;
if (len--)
rxd_buffer[ptrOut++] = z;
else
break;
}
pRxData = &rxd_buffer[3];
RxDataLen = ptrOut - 3;
}
// --------------------------------------------------------------------------
void usart0_ProcessRxData(void) {
// We control the outputTestActive var from here: Count it down.
if (outputTestActive)
outputTestActive--;
// 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
switch (rxd_buffer[1] - 'a') {
case FC_ADDRESS:
switch (rxd_buffer[2]) {
#ifdef USE_MK3MAG
case 'K':// compass value
compassHeading = ((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));
} else {
memcpy(&outputTest[0], (uint8_t*) pRxData, 4);
}
outputTestActive = 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 'q':// request settings
if (pRxData[0] == 0xFF) {
pRxData[0] = GetParamByte(PID_ACTIVE_SET);
}
// limit settings range
if (pRxData[0] < 1)
pRxData[0] = 1; // limit to 1
else if (pRxData[0] > 5)
pRxData[0] = 5; // limit to 5
// load requested parameter set
ParamSet_ReadFromEEProm(pRxData[0]);
tempchar1 = pRxData[0];
tempchar2 = EEPARAM_REVISION;
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));
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
{
memcpy(&staticParams, (uint8_t*) &pRxData[2], sizeof(staticParams));
ParamSet_WriteToEEProm(pRxData[0]);
tempchar1 = getActiveParamSet();
beepNumber(tempchar1);
} else {
tempchar1 = 0; //indicate bad data
}
while (!txd_complete)
; // wait for previous frame to be sent
SendOutData('S', FC_ADDRESS, 1, &tempchar1, sizeof(tempchar1));
}
break;
default:
//unsupported command received
break;
} // case FC_ADDRESS:
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;
break;
case 'b': // submit extern control
memcpy(&externalControl, (uint8_t*) pRxData, sizeof(externalControl));
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 'v': // request for version and board release
request_VerInfo = TRUE;
break;
case 'x':
request_variables = TRUE;
break;
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;
break;
case 'c': // request for the 3D data
Data3D_Interval = (uint16_t) pRxData[0] * 10;
if (Data3D_Interval > 0)
request_Data3D = TRUE;
break;
default:
//unsupported command received
break;
}
break; // default:
}
// unlock the rxd buffer after processing
pRxData = 0;
RxDataLen = 0;
rxd_buffer_locked = FALSE;
}
/************************************************************************/
/* Routine für die Serielle Ausgabe */
/************************************************************************/
int16_t uart_putchar(int8_t c) {
if (c == '\n')
uart_putchar('\r');
// wait until previous character was send
loop_until_bit_is_set(UCSR0A, UDRE0);
// send character
UDR0 = c;
return (0);
}
//---------------------------------------------------------------------------------------------
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_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
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;
}
if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC bestätigen
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame,
sizeof(ConfirmFrame));
ConfirmFrame = 0;
}
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;
}
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 * yawGyroHeading) / GYRO_DEG_FACTOR_YAW); // convert to multiple of 0.1°
Data3D_Timer = setDelay(Data3D_Interval);
request_Data3D = FALSE;
}
if (request_ExternalControl && txd_complete) {
SendOutData('G', FC_ADDRESS, 1, (uint8_t *) &externalControl,
sizeof(externalControl));
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);
}
#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));
request_PPMChannels = FALSE;
}
if (request_variables && txd_complete) {
SendOutData('X', FC_ADDRESS, 1, (uint8_t *) &variables, sizeof(variables));
request_variables = FALSE;
}
}

/branches/dongfang_FC_fixedwing/uart0.h
0,0 → 1,46
#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
#include <inttypes.h>
//Baud rate of the USART
#define USART0_BAUD 57600
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 outputTestActive;
extern uint8_t outputTest[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];
}__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;
#endif //_UART0_H