/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 "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<scannerConfigBuildInfo instanceId="0.1994915586"> |
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="makefileGenerator"> |
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/${specs_file}"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'g++ -E -P -v -dD "${plugin_state_location}/specs.cpp"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC"> |
<buildOutputProvider> |
<openAction enabled="true" filePath=""/> |
<parser enabled="true"/> |
</buildOutputProvider> |
<scannerInfoProvider id="specsFile"> |
<runAction arguments="-c 'gcc -E -P -v -dD "${plugin_state_location}/specs.c"'" command="sh" useDefault="true"/> |
<parser enabled="true"/> |
</scannerInfoProvider> |
</profile> |
</scannerConfigBuildInfo> |
</storageModule> |
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> |
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> |
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> |
</cconfiguration> |
</storageModule> |
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> |
<project id="dongfang_FC_fixedwing.null.1262997060" name="dongfang_FC_fixedwing"/> |
</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 |