Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

imported ArduPlane from ArduPilotMega svn

This commit is contained in:
Andrew Tridgell 2011-09-09 11:29:39 +10:00
parent 1456a2a912
commit e0dc1271d6
33 changed files with 11125 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

468
ArduPlane/.cproject Normal file
View File

@ -0,0 +1,468 @@
<?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.1246468818">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1246468818" 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.1246468818" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1246468818." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.1286905935" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.1286905935.1477421615" name=""/>
<builder id="org.eclipse.cdt.build.core.settings.default.builder.2031915136" 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.150062293" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.990947382" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1924975572" 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.299915223" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.814698218" 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.2121091413" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1614998879" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="avr-gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="avr-g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<scannerConfigBuildInfo instanceId="0.1246468818">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="avr-gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="avr-g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.877812590">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="avr-gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="avr-g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.1935535476">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="makefileGenerator">
<runAction arguments="-E -P -v -dD" command="" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/${specs_file}" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfile">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/${specs_file}&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'g++ -E -P -v -dD &quot;${plugin_state_location}/specs.cpp&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="org.eclipse.cdt.managedbuilder.core.GCCWinManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-c 'gcc -E -P -v -dD &quot;${plugin_state_location}/specs.c&quot;'" command="sh" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileC">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.c" command="avr-gcc" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
<profile id="de.innot.avreclipse.core.AVRGCCManagedMakePerProjectProfileCPP">
<buildOutputProvider>
<openAction enabled="true" filePath=""/>
<parser enabled="true"/>
</buildOutputProvider>
<scannerInfoProvider id="specsFile">
<runAction arguments="-E -P -v -dD ${plugin_state_location}/specs.cpp" command="avr-g++" useDefault="true"/>
<parser enabled="true"/>
</scannerInfoProvider>
</profile>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="ArduPilotMega" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget/>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="ArduPilotMega.null.28020601" name="ArduPilotMega"/>
</storageModule>
</cproject>

80
ArduPlane/.project Normal file
View File

@ -0,0 +1,80 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>ArduPilotMega</name>
<comment></comment>
<projects>
<project>libraries</project>
</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>all</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>

25
ArduPlane/APM_Config.h Normal file
View File

@ -0,0 +1,25 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
// their default values, place the appropriate #define statements here.
// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented)
//#define SERIAL3_BAUD 38400
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
// You may also put an include statement here to point at another configuration file. This is convenient if you maintain
// different configuration files for different aircraft or HIL simulation. See the examples below
//#include "APM_Config_mavlink_hil.h"
//#include "Skywalker.h"
// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable:
/*
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#define GCS_PORT 3
*/

957
ArduPlane/APM_Config.h.reference Normal file
View File

@ -0,0 +1,957 @@
//
// Example and reference ArduPilot Mega configuration file
// =======================================================
//
// This file contains documentation and examples for configuration options
// supported by the ArduPilot Mega software.
//
// Most of these options are just that - optional. You should create
// the APM_Config.h file and use this file as a reference for options
// that you want to change. Don't copy this file directly; the options
// described here and their default values may change over time.
//
// Each item is marked with a keyword describing when you should set it:
//
// REQUIRED
// You must configure this in your APM_Config.h file. The
// software will not compile if the option is not set.
//
// OPTIONAL
// The option has a sensible default (which will be described
// here), but you may wish to override it.
//
// EXPERIMENTAL
// You should configure this option unless you are prepared
// to deal with potential problems. It may relate to a feature
// still in development, or which is not yet adequately tested.
//
// DEBUG
// The option should only be set if you are debugging the
// software, or if you are gathering information for a bug
// report.
//
// NOTE:
// Many of these settings are considered 'factory defaults', and the
// live value is stored and managed in the ArduPilot Mega EEPROM.
// Use the setup 'factoryreset' command after changing options in
// your APM_Config.h file.
//
// Units
// -----
//
// Unless indicated otherwise, numeric quantities use the following units:
//
// Measurement | Unit
// ------------+-------------------------------------
// angle | degrees
// distance | metres
// speed | metres per second
// servo angle | microseconds
// voltage | volts
// times | seconds
// throttle | percent
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL REQUIRED
//
// GPS configuration, must be one of:
//
// GPS_PROTOCOL_AUTO Auto detect GPS type (must be a supported GPS)
// GPS_PROTOCOL_NONE No GPS attached
// GPS_PROTOCOL_IMU X-Plane interface or ArduPilot IMU.
// GPS_PROTOCOL_MTK MediaTek-based GPS running the DIYDrones firmware 1.4
// GPS_PROTOCOL_MTK16 MediaTek-based GPS running the DIYDrones firmware 1.6
// GPS_PROTOCOL_UBLOX UBLOX GPS
// GPS_PROTOCOL_SIRF SiRF-based GPS in Binary mode. NOT TESTED
// GPS_PROTOCOL_NMEA Standard NMEA GPS. NOT SUPPORTED (yet?)
//
//#define GPS_PROTOCOL GPS_PROTOCOL_AUTO
//
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR OPTIONAL
// AIRSPEED_RATIO OPTIONAL
//
// Set AIRSPEED_SENSOR to ENABLED if you have an airspeed sensor attached.
// Adjust AIRSPEED_RATIO in small increments to calibrate the airspeed
// sensor relative to your GPS. The calculation and default value are optimized for speeds around 12 m/s
//
// The default assumes that an airspeed sensor is connected.
//
//#define AIRSPEED_SENSOR ENABLED
//#define AIRSPEED_RATIO 1.9936
//
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER OPTIONAL
// MAG_ORIENTATION OPTIONAL
//
// Set MAGNETOMETER to ENABLED if you have a magnetometer attached.
// Set MAG_ORIENTATION to reflect the orientation you have the magnetometer mounted with respect to ArduPilotMega
//
// The default assumes that a magnetometer is not connected.
//
//#define MAGNETOMETER DISABLED
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
//
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
//
// Configuration for Hardware-in-the-loop simulation. In these modes,
// APM is connected via one or more interfaces to simulation software
// running on another system.
//
// HIL_PROTOCOL_XPLANE Configure for the X-plane HIL interface.
// HIL_PROTOCOL_MAVLINK Configure for HIL over MAVLink.
//
// HIL_MODE_DISABLED Configure for standard flight.
// HIL_MODE_ATTITUDE Simulator provides attitude and position information.
// HIL_MODE_SENSORS Simulator provides raw sensor values.
//
// Note that currently HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE.
// Note that currently HIL_PROTOCOL_MAVLINK requires HIL_MODE_SENSORS.
//
//#define HIL_MODE HIL_MODE_DISABLED
//#define HIL_PORT 0
//#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
//
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL OPTIONAL
// GCS_PORT OPTIONAL
// MAV_SYSTEM_ID OPTIONAL
//
// The GCS_PROTOCOL option determines which (if any) ground control station
// protocol will be used. Must be one of:
//
// GCS_PROTOCOL_NONE No GCS output
// GCS_PROTOCOL_MAVLINK QGroundControl protocol
//
// The GCS_PORT option determines which serial port will be used by the
// GCS protocol. The usual values are 0 for the console/USB port,
// or 3 for the telemetry port on the oilpan. Note that some protocols
// will ignore this value and always use the console port.
//
// The MAV_SYSTEM_ID is a unique identifier for this UAV. The default value is 1.
// If you will be flying multiple UAV's each should be assigned a different ID so
// that ground stations can tell them apart.
//
//#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
//#define GCS_PORT 3
//#define MAV_SYSTEM_ID 1
//
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
// SERIAL0_BAUD OPTIONAL
//
// Baudrate for the console port. Default is 115200bps.
//
// SERIAL3_BAUD OPTIONAL
//
// Baudrate for the telemetry port. Default is 57600bps.
//
//#define SERIAL0_BAUD 115200
//#define SERIAL3_BAUD 57600
//
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring OPTIONAL
//
// See the manual for details on selecting divider resistors for battery
// monitoring via the oilpan.
//
// BATTERY_EVENT OPTIONAL
//
// Set BATTERY_EVENT to ENABLED to enable low voltage or high discharge warnings.
// The default is DISABLED.
//
// LOW_VOLTAGE OPTIONAL if BATTERY_EVENT is set.
//
// Value in volts at which ArduPilot Mega should consider the
// battery to be "low".
//
// VOLT_DIV_RATIO OPTIONAL
//
// See the manual for details. The default value corresponds to the resistors
// recommended by the manual.
//
// CURR_AMPS_PER_VOLT OPTIONAL
// CURR_AMPS_OFFSET OPTIONAL
//
// The sensitivity of the current sensor. This must be scaled if a resistor is installed on APM
// for a voltage divider on input 2 (not recommended). The offset is used for current sensors with an offset
//
//
// HIGH_DISCHARGE OPTIONAL if BATTERY_EVENT is set.
//
// Value in milliamp-hours at which a warning should be triggered. Recommended value = 80% of
// battery capacity.
//
//#define BATTERY_EVENT DISABLED
//#define LOW_VOLTAGE 9.6
//#define VOLT_DIV_RATIO 3.56
//#define CURR_AMPS_PER_VOLT 27.32
//#define CURR_AMPS_OFFSET 0.0
//#define HIGH_DISCHARGE 1760
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE OPTIONAL
//
// In order to have accurate pressure and battery voltage readings, this
// value should be set to the voltage measured at the processor.
//
// See the manual for more details. The default value should be close if you are applying 5 volts to the servo rail.
//
//#define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE OPTIONAL
// FLIGHT_MODE_CHANNEL OPTIONAL
//
// Flight modes assigned to the control channel, and the input channel that
// is read for the control mode.
//
// Use a servo tester, or the ArduPilotMega_demo test program to check your
// switch settings.
//
// ATTENTION: Some ArduPilot Mega boards have radio channels marked 0-7, and
// others have them marked the standard 1-8. The FLIGHT_MODE_CHANNEL option
// uses channel numbers 1-8 (and defaults to 8).
//
// If you only have a three-position switch or just want three modes, set your
// switch to produce 1165, 1425, and 1815 microseconds and configure
// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default.
//
// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control
// channel connected to input channel 8, the hardware failsafe mode will
// activate for any control input over 1750ms.
//
// For more modes (up to six), set your switch(es) to produce any of 1165,
// 1295, 1425, 1555, 1685, and 1815 microseconds.
//
// Flight mode | Switch Setting (ms)
// ------------+---------------------
// 1 | 1165
// 2 | 1295
// 3 | 1425
// 4 | 1555
// 5 | 1685
// 6 | 1815 (FAILSAFE if using channel 8)
//
// The following standard flight modes are available:
//
// Name | Description
// -----------------+--------------------------------------------
// |
// MANUAL | Full manual control via the hardware multiplexer.
// |
// STABILIZE | Tries to maintain level flight, but can be overridden with radio control inputs.
// |
// FLY_BY_WIRE_A | Autopilot style control via user input, with manual throttle.
// |
// FLY_BY_WIRE_B | Autopilot style control via user input, aispeed controlled with throttle.
// |
// RTL | Returns to the Home location and then LOITERs at a safe altitude.
// |
// AUTO | Autonomous flight based on programmed waypoints. Use the WaypointWriter
// | application or your Ground Control System to edit and upload
// | waypoints and other commands.
// |
//air
//
// The following non-standard modes are EXPERIMENTAL:
//
// Name | Description
// -----------------+--------------------------------------------
// |
// LOITER | Flies in a circle around the current location.
// |
// CIRCLE | Flies in a stabilized 'dumb' circle.
// |
//
//
// If you are using channel 8 for mode switching then FLIGHT_MODE_5 and
// FLIGHT_MODE_6 should be MANUAL.
//
//
//#define FLIGHT_MODE_CHANNEL 8
//
//#define FLIGHT_MODE_1 RTL
//#define FLIGHT_MODE_2 RTL
//#define FLIGHT_MODE_3 STABILIZE
//#define FLIGHT_MODE_4 STABILIZE
//#define FLIGHT_MODE_5 MANUAL
//#define FLIGHT_MODE_6 MANUAL
//
//////////////////////////////////////////////////////////////////////////////
// RC_5_FUNCT OPTIONAL
// RC_6_FUNCT OPTIONAL
// RC_7_FUNCT OPTIONAL
// RC_8_FUNCT OPTIONAL
//
// The channel 5 through 8 function assignments allow configuration of those
// channels for use with differential ailerons, flaps, flaperons, or camera
// or intrument mounts
//
//#define RC_5_FUNCT RC_5_FUNCT_NONE
//etc.
//////////////////////////////////////////////////////////////////////////////
// For automatic flap operation based on speed setpoint. If the speed setpoint is above flap_1_speed
// then the flap position shall be 0%. If the speed setpoint is between flap_1_speed and flap_2_speed
// then the flap position shall be flap_1_percent. If the speed setpoint is below flap_2_speed
// then the flap position shall be flap_2_percent. Speed setpoint is the current value of
// airspeed_cruise (if airspeed sensor enabled) or throttle_cruise (if no airspeed sensor)
// FLAP_1_PERCENT OPTIONAL
// FLAP_1_SPEED OPTIONAL
// FLAP_2_PERCENT OPTIONAL
// FLAP_2_SPEED OPTIONAL
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE OPTIONAL
// THROTTLE_FS_VALUE OPTIONAL
//
// The throttle failsafe allows you to configure a software failsafe activated
// by a setting on the throttle input channel (channel 3). Enabling this failsafe
// also enables "short failsafe" conditions (see below) based on loss of
// rc override control from the GCS
//
// This can be used to achieve a failsafe override on loss of radio control
// without having to sacrifice one of your FLIGHT_MODE settings, as the
// throttle failsafe overrides the switch-selected mode.
//
// Throttle failsafe is enabled by setting THROTTLE_FAILSAFE to 1. The default
// is for it to be enabled.
//
// If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value
// below which the failsafe engages. The default is 975ms, which is a very low
// throttle setting. Most transmitters will let you trim the manual throttle
// position up so that you cannot engage the failsafe with a regular stick movement.
//
// Configure your receiver's failsafe setting for the throttle channel to the
// absolute minimum, and use the ArduPilotMega_demo program to check that
// you cannot reach that value with the throttle control. Leave a margin of
// at least 50 microseconds between the lowest throttle setting and
// THROTTLE_FS_VALUE.
//
//#define THROTTLE_FAILSAFE ENABLED
//#define THROTTLE_FS_VALUE 950
//
//////////////////////////////////////////////////////////////////////////////
// GCS_HEARTBEAT_FAILSAFE OPTIONAL
// SHORT_FAILSAFE_ACTION OPTIONAL
// LONG_FAILSAFE_ACTION OPTIONAL
// There are two basic conditions which can trigger a failsafe. One is a loss of control signal.
// Normally this means a loss of the radio control RC signal. However if rc override from the
// GCS is in use then this can mean a loss of communication with the GCS. Such a failsafe will be
// classified as either short (greater than 1.5 seconds but less than 20) or long (greater than 20).
// Also, if GCS_HEARTBEAT_FAILSAFE is enabled and a heartbeat signal from the GCS has not been received
// in the preceeding 20 seconds then this will also trigger a "long" failsafe.
//
// The SHORT_FAILSAFE_ACTION and LONG_FAILSAFE_ACTION settings determines what APM will do when
// a failsafe mode is entered while flying in AUTO or LOITER mode. This is important in order to avoid
// accidental failsafe behaviour when flying waypoints that take the aircraft
// out of radio range.
//
// If SHORT_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes,
// the aircraft will head for home in RTL mode. If the failsafe condition is
// resolved, it will return to AUTO or LOITER mode.
// If LONG_FAILSAFE_ACTION is 1, when failsafe is entered in AUTO or LOITER modes,
// the aircraft will head for home in RTL mode. If the failsafe condition is
// resolved the aircraft will not be returned to AUTO or LOITER mode, but will continue home
// If XX_FAILSAFE_ACTION is 0 and the applicable failsafe occurs while in AUTO or LOITER
// mode the aircraft will continue in that mode ignoring the failsafe condition.
// Note that for Manual, Stabilize, and Fly-By-Wire (A and B) modes the aircraft will always
// enter a circling mode for short failsafe conditions and will be switched to RTL for long
// failsafe conditions. RTL mode is unaffected by failsafe conditions.
//
// The default is to have GCS Heartbeat failsafes DISABLED
// The default behaviour is to ignore failsafes in AUTO and LOITER modes.
//
//#define GCS_HEARTBEAT_FAILSAFE DISABLED
//#define SHORT_FAILSAFE_ACTION 0
//#define LONG_FAILSAFE_ACTION 0
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM OPTIONAL
//
// ArduPilot Mega can update its trim settings by looking at the
// radio inputs when switching out of MANUAL mode. This allows you to
// manually trim your aircraft before switching to an assisted mode, but it
// also means that you should avoid switching out of MANUAL while you have
// any control stick deflection.
//
// The default is to enable AUTO_TRIM.
//
//#define AUTO_TRIM ENABLED
//
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_REVERSE OPTIONAL
//
// A few speed controls require the throttle servo signal be reversed. Setting
// this to ENABLED will reverse the throttle output signal. Ensure that your
// throttle needs to be reversed by using the hardware failsafe and the
// ArduPilotMega_demo program before setting this option.
//
// The default is to not reverse the signal.
//
//#define THROTTLE_REVERSE DISABLED
//
//////////////////////////////////////////////////////////////////////////////
// ENABLE_STICK_MIXING OPTIONAL
//
// If this option is set to ENABLED, manual control inputs are are respected
// while in the autopilot modes (AUTO, RTL, LOITER, CIRCLE etc.)
//
// The default is to enable stick mixing, allowing the pilot to take
// emergency action without switching modes.
//
//#define ENABLE_STICK_MIXING ENABLED
//
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_OUT DEBUG
//
// When debugging, it can be useful to disable the throttle output. Set
// this option to DISABLED to disable throttle output signals.
//
// The default is to not disable throttle output.
//
//#define THROTTLE_OUT ENABLED
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY OPTIONAL
//
// If configured, inserts a delay between power-up and the beginning of IMU
// calibration during a ground start.
//
// Use this setting to give you time to position the aircraft horizontally
// for the IMU calibration.
//
// The default is to begin IMU calibration immediately at startup.
//
//#define GROUND_START_DELAY 0
//
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START OPTIONAL
//
// If air start is disabled then you will get a ground start (including IMU
// calibration) every time the AP is powered up. This means that if you get
// a power glitch or reboot for some reason in the air, you will probably
// crash, but it prevents a lot of problems on the ground like unintentional
// motor start-ups, etc.
//
// If air start is enabled then you will get an air start at power up and a
// ground start will be performed if the speed is near zero when we get gps
// lock.
//
// The default is to disable air start.
//
//#define ENABLE_AIR_START 0
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
// ALT_EST_GAIN OPTIONAL
//
// The gain of the altitude estimation function; a lower number results
// in slower error correction and smoother output. The default is a
// reasonable starting point.
//
//#define ALT_EST_GAIN 0.01
//
// ALTITUDE_MIX OPTIONAL
//
// Configures the blend between GPS and pressure altitude.
// 0 = GPS altitude, 1 = Press alt, 0.5 = half and half, etc.
//
// The default is to use only pressure altitude.
//
//#define ALTITUDE_MIX 1
//
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE OPTIONAL
//
// The speed in metres per second to maintain during cruise. The default
// is 10m/s, which is a conservative value suitable for relatively small,
// light aircraft.
//
//#define AIRSPEED_CRUISE 12
//
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO)
//
// AIRSPEED_FBW_MIN OPTIONAL
// AIRSPEED_FBW_MAX OPTIONAL
//
// Airspeed corresponding to minimum and maximum throttle in Fly By Wire B mode.
// The defaults are 6 and 30 metres per second.
//
// AIRSPEED_FBW_MAX also sets the maximum airspeed that the cruise airspeed can be "nudged" to in AUTO mode when ENABLE_STICK_MIXING is set.
// In AUTO the cruise airspeed can be increased between AIRSPEED_CRUISE and AIRSPEED_FBW_MAX by positioning the throttle
// stick in the top 1/2 of its range. Throttle stick in the bottom 1/2 provide regular AUTO control.
//
//#define AIRSPEED_FBW_MIN 6
//#define AIRSPEED_FBW_MAX 22
//
//////////////////////////////////////////////////////////////////////////////
// Servo mapping
//
// THROTTLE_MIN OPTIONAL
//
// The minimum throttle setting to which the autopilot will reduce the
// throttle while descending. The default is zero, which is
// suitable for aircraft with a steady power-off glide. Increase this
// value if your aircraft needs throttle to maintain a stable descent in
// level flight.
//
// THROTTLE_CRUISE OPTIONAL
//
// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight.
// The default is 45%, which is reasonable for a modestly powered aircraft.
//
// THROTTLE_MAX OPTIONAL
//
// The maximum throttle setting the autopilot will apply. The default is 75%.
// Reduce this value if your aicraft is overpowered, or has complex flight
// characteristics at high throttle settings.
//
//#define THROTTLE_MIN 0 // percent
//#define THROTTLE_CRUISE 45
//#define THROTTLE_MAX 75
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
// HEAD_MAX OPTIONAL
//
// The maximum commanded bank angle in either direction.
// The default is 45 degrees. Decrease this value if your aircraft is not
// stable or has difficulty maintaining altitude in a steep bank.
//
// PITCH_MAX OPTIONAL
//
// The maximum commanded pitch up angle.
// The default is 15 degrees. Care should be taken not to set this value too
// large, as the aircraft may stall.
//
// PITCH_MIN
//
// The maximum commanded pitch down angle. Note that this value must be
// negative. The default is -25 degrees. Care should be taken not to set
// this value too large as it may result in overspeeding the aircraft.
//
// PITCH_TARGET
//
// The target pitch for cruise flight. When the APM measures this pitch
// value, the pitch error will be calculated to be 0 for the pitch PID
// control loop.
//
//#define HEAD_MAX 45
//#define PITCH_MAX 15
//#define PITCH_MIN -25
//#define PITCH_TARGET 0
//////////////////////////////////////////////////////////////////////////////
// Attitude control gains
//
// Tuning values for the attitude control PID loops.
//
// The P term is the primary tuning value. This determines how the control
// deflection varies in proportion to the required correction.
//
// The I term is used to help control surfaces settle. This value should
// normally be kept low.
//
// The D term is used to control overshoot. Avoid using or adjusting this
// term if you are not familiar with tuning PID loops. It should normally
// be zero for most aircraft.
//
// Note: When tuning these values, start with changes of no more than 25% at
// a time.
//
// SERVO_ROLL_P OPTIONAL
// SERVO_ROLL_I OPTIONAL
// SERVO_ROLL_D OPTIONAL
//
// P, I and D terms for roll control. Defaults are 0.4, 0, 0.
//
// SERVO_ROLL_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 5 degrees.
//
// ROLL_SLEW_LIMIT EXPERIMENTAL
//
// Limits the slew rate of the roll control in degrees per second. If zero,
// slew rate is not limited. Default is to not limit the roll control slew rate.
// (This feature is currently not implemented.)
//
// SERVO_PITCH_P OPTIONAL
// SERVO_PITCH_I OPTIONAL
// SERVO_PITCH_D OPTIONAL
//
// P, I and D terms for the pitch control. Defaults are 0.6, 0, 0.
//
// SERVO_PITCH_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. native flight
// AoA).
// Default is 5 degrees.
//
// PITCH_COMP OPTIONAL
//
// Adds pitch input to compensate for the loss of lift due to roll control.
// Default is 0.20 (20% of roll control also applied to pitch control).
//
// SERVO_YAW_P OPTIONAL
// SERVO_YAW_I OPTIONAL
// SERVO_YAW_D OPTIONAL
//
// P, I and D terms for the YAW control. Defaults are 0., 0., 0.
// Note units of this control loop are unusual. PID input is in m/s**2.
//
// SERVO_YAW_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 0.
//
// RUDDER_MIX OPTIONAL
//
// Roll to yaw mixing. This allows for co-ordinated turns.
// Default is 0.50 (50% of roll control also applied to yaw control.)
//
//#define SERVO_ROLL_P 0.4
//#define SERVO_ROLL_I 0.0
//#define SERVO_ROLL_D 0.0
//#define SERVO_ROLL_INT_MAX 5
//#define ROLL_SLEW_LIMIT 0
//#define SERVO_PITCH_P 0.6
//#define SERVO_PITCH_I 0.0
//#define SERVO_PITCH_D 0.0
//#define SERVO_PITCH_INT_MAX 5
//#define PITCH_COMP 0.2
//#define SERVO_YAW_P 0.0 // Default is zero. A suggested value if you want to use this parameter is 0.5
//#define SERVO_YAW_I 0.0
//#define SERVO_YAW_D 0.0
//#define SERVO_YAW_INT_MAX 5
//#define RUDDER_MIX 0.5
//
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
// Tuning values for the navigation control PID loops.
//
// The P term is the primary tuning value. This determines how the control
// deflection varies in proportion to the required correction.
//
// The I term is used to control drift.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// you are not familiar with tuning PID loops.
//
// Note: When tuning these values, start with changes of no more than 25% at
// a time.
//
// NAV_ROLL_P OPTIONAL
// NAV_ROLL_I OPTIONAL
// NAV_ROLL_D OPTIONAL
//
// P, I and D terms for navigation control over roll, normally used for
// controlling the aircraft's course. The P term controls how aggressively
// the aircraft will bank to change or hold course.
// Defaults are 0.7, 0.0, 0.02.
//
// NAV_ROLL_INT_MAX OPTIONAL
//
// Maximum control offset due to the integral. This prevents the control
// output from being overdriven due to a persistent offset (e.g. crosstracking).
// Default is 5 degrees.
//
// NAV_PITCH_ASP_P OPTIONAL
// NAV_PITCH_ASP_I OPTIONAL
// NAV_PITCH_ASP_D OPTIONAL
//
// P, I and D terms for pitch adjustments made to maintain airspeed.
// Defaults are 0.65, 0, 0.
//
// NAV_PITCH_ASP_INT_MAX OPTIONAL
//
// Maximum pitch offset due to the integral. This limits the control
// output from being overdriven due to a persistent offset (eg. inability
// to maintain the programmed airspeed).
// Default is 5 degrees.
//
// NAV_PITCH_ALT_P OPTIONAL
// NAV_PITCH_ALT_I OPTIONAL
// NAV_PITCH_ALT_D OPTIONAL
//
// P, I and D terms for pitch adjustments made to maintain altitude.
// Defaults are 0.65, 0, 0.
//
// NAV_PITCH_ALT_INT_MAX OPTIONAL
//
// Maximum pitch offset due to the integral. This limits the control
// output from being overdriven due to a persistent offset (eg. inability
// to maintain the programmed altitude).
// Default is 5 meters.
//
//#define NAV_ROLL_P 0.7
//#define NAV_ROLL_I 0.01
//#define NAV_ROLL_D 0.02
//#define NAV_ROLL_INT_MAX 5
//#define NAV_PITCH_ASP_P 0.65
//#define NAV_PITCH_ASP_I 0.0
//#define NAV_PITCH_ASP_D 0.0
//#define NAV_PITCH_ASP_INT_MAX 5
//#define NAV_PITCH_ALT_P 0.65
//#define NAV_PITCH_ALT_I 0.0
//#define NAV_PITCH_ALT_D 0.0
//#define NAV_PITCH_ALT_INT_MAX 5
//
//////////////////////////////////////////////////////////////////////////////
// Energy/Altitude control gains
//
// The Energy/altitude control system uses throttle input to control aircraft
// altitude.
//
// The P term is the primary tuning value. This determines how the throttle
// setting varies in proportion to the required correction.
//
// The I term is used to compensate for small offsets.
//
// The D term is used to control overshoot. Avoid adjusting this term if
// you are not familiar with tuning PID loops.
//
// Note units of this control loop are unusual. PID input is in m**2/s**2.
//
// THROTTLE_TE_P OPTIONAL
// THROTTLE_TE_I OPTIONAL
// THROTTLE_TE_D OPTIONAL
//
// P, I and D terms for throttle adjustments made to control altitude.
// Defaults are 0.5, 0, 0.
//
// THROTTLE_TE_INT_MAX OPTIONAL
//
// Maximum throttle input due to the integral term. This limits the
// throttle from being overdriven due to a persistent offset (e.g.
// inability to maintain the programmed altitude).
// Default is 20%.
//
// THROTTLE_SLEW_LIMIT OPTIONAL
//
// Limits the slew rate of the throttle, in percent per second. Helps
// avoid sudden throttle changes, which can destabilise the aircraft.
// A setting of zero disables the feature. Range 1 to 100.
// Default is zero (disabled).
//
// P_TO_T OPTIONAL
//
// Pitch to throttle feed-forward gain. Default is 0.
//
// T_TO_P OPTIONAL
//
// Throttle to pitch feed-forward gain. Default is 0.
//
//#define THROTTLE_TE_P 0.50
//#define THROTTLE_TE_I 0.0
//#define THROTTLE_TE_D 0.0
//#define THROTTLE_TE_INT_MAX 20
//#define THROTTLE_SLEW_LIMIT 0
//#define P_TO_T 0
//#define T_TO_P 0
//
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
// XTRACK_GAIN OPTIONAL
//
// Crosstrack compensation in degrees per metre off track.
// Default value is 1.0 degrees per metre. Values lower than 0.001 will
// disable crosstrack compensation.
//
// XTRACK_ENTRY_ANGLE OPTIONAL
//
// Maximum angle used to correct for track following.
// Default value is 30 degrees.
//
//#define XTRACK_GAIN 1 // deg/m
//#define XTRACK_ENTRY_ANGLE 30 // deg
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
// Each of these logging options may be set to ENABLED to enable, or DISABLED
// to disable the logging of the respective data.
//
// LOG_ATTITUDE_FAST DEBUG
//
// Logs basic attitude info to the dataflash at 50Hz (uses more space).
// Defaults to DISABLED.
//
// LOG_ATTITUDE_MED OPTIONAL
//
// Logs basic attitude info to the dataflash at 10Hz (uses less space than
// LOG_ATTITUDE_FAST). Defaults to ENABLED.
//
// LOG_GPS OPTIONAL
//
// Logs GPS info to the dataflash at 10Hz. Defaults to ENABLED.
//
// LOG_PM OPTIONAL
//
// Logs IMU performance monitoring info every 20 seconds.
// Defaults to DISABLED.
//
// LOG_CTUN OPTIONAL
//
// Logs control loop tuning info at 10 Hz. This information is useful for tuning
// servo control loop gain values. Defaults to DISABLED.
//
// LOG_NTUN OPTIONAL
//
// Logs navigation tuning info at 10 Hz. This information is useful for tuning
// navigation control loop gain values. Defaults to DISABLED.
//
// LOG_ MODE OPTIONAL
//
// Logs changes to the flight mode upon occurrence. Defaults to ENABLED.
//
// LOG_RAW DEBUG
//
// Logs raw accelerometer and gyro data at 50 Hz (uses more space).
// Defaults to DISABLED.
//
// LOG_CMD OPTIONAL
//
// Logs new commands when they process.
// Defaults to ENABLED.
//
//#define LOG_ATTITUDE_FAST DISABLED
//#define LOG_ATTITUDE_MED ENABLED
//#define LOG_GPS ENABLED
//#define LOG_PM ENABLED
//#define LOG_CTUN DISABLED
//#define LOG_NTUN DISABLED
//#define LOG_MODE ENABLED
//#define LOG_RAW DISABLED
//#define LOG_CMD ENABLED
//#define LOG_CUR DISABLED
//
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
// WP_RADIUS_DEFAULT OPTIONAL
//
// When the user performs a factory reset on the APM, set the waypoint radius
// (the radius from a target waypoint within which the APM will consider
// itself to have arrived at the waypoint) to this value in meters. This is
// mainly intended to allow users to start using the APM without running the
// WaypointWriter first.
//
// LOITER_RADIUS_DEFAULT OPTIONAL
//
// When the user performs a factory reset on the APM, set the loiter radius
// (the distance the APM will attempt to maintain from a waypoint while
// loitering) to this value in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
// USE_CURRENT_ALT OPTIONAL
// ALT_HOLD_HOME OPTIONAL
//
// When the user performs a factory reset on the APM, set the flag for weather
// the current altitude or ALT_HOLD_HOME altitude should be used for Return To Launch.
// Also, set the value of USE_CURRENT_ALT in meters. This is mainly intended to allow
// users to start using the APM without running the WaypointWriter first.
//
//#define WP_RADIUS_DEFAULT 30
//#define LOITER_RADIUS_DEFAULT 60
//#define USE_CURRENT_ALT FALSE
//#define ALT_HOLD_HOME 100
//
//////////////////////////////////////////////////////////////////////////////
// Debugging interface
//
// DEBUG_PORT OPTIONAL
//
// The APM will periodically send messages reporting what it is doing; this
// variable determines to which serial port they will be sent. Port 0 is the
// USB serial port on the shield, port 3 is the telemetry port.
//
//#define DEBUG_PORT 0
//
//
// Do not remove - this is to discourage users from copying this file
// and using it as-is rather than editing their own.
//
#error You should not copy APM_Config.h.reference - make your own APM_Config.h file with just the options you need.

69
ArduPlane/APM_Config_mavlink_hil.h Normal file
View File

@ -0,0 +1,69 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE MAVLINK HIL INTERFACE
// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
// Enable Autopilot Flight Mode
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
#define FLIGHT_MODE_2 RTL
#define FLIGHT_MODE_3 FLY_BY_WIRE_A
#define FLIGHT_MODE_4 FLY_BY_WIRE_B
#define FLIGHT_MODE_5 STABILIZE
#define FLIGHT_MODE_6 MANUAL
// Hardware in the loop protocol
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
// HIL_MODE SELECTION
//
// Mavlink supports
// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude
// 2. HIL_MODE_SENSORS: full sensor simulation
#define HIL_MODE HIL_MODE_ATTITUDE
// HIL_PORT SELCTION
//
// PORT 0
// Port 0 is the port usually used for HIL. You attatch your HIL computer
// to APM using a USB cable. You can run telemetry on Port 3
//
// PORT 1
// Available for special situations. This uses the port that would have
// been used for the gps
// as the hardware in the loop port. You will have to solder
// headers onto the gps port connection on the apm
// and connect via an ftdi cable.
// The buad rate is controlled by SERIAL1_BAUD in this mode.
//
// PORT 3
// If you don't require telemetry communication with a gcs while running
// hardware in the loop you may use the telemetry port as the hardware in
// the loop port. Alternatively, use a telemetry/HIL shim like FGShim
// https://ardupilot-mega.googlecode.com/svn/Tools/trunk/FlightGear
//
// The buad rate is controlled by SERIAL3_BAUD in this mode.
#define HIL_PORT 0
// You can set your gps protocol here for your actual
// hardware and leave it without affecting the hardware
// in the loop simulation
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
// Ground control station comms
#if HIL_PORT != 3
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
# define GCS_PORT 3
#endif
// Sensors
// All sensors are supported in all modes.
// The magnetometer is not used in
// HIL_MODE_ATTITUDE but you may leave it
// enabled if you wish.
#define AIRSPEED_SENSOR ENABLED
#define MAGNETOMETER ENABLED
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED

22
ArduPlane/APM_Config_xplane.h Normal file
View File

@ -0,0 +1,22 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE ORIGINAL X-PLANE INTERFACE
// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE!
#define FLIGHT_MODE_CHANNEL 8
#define FLIGHT_MODE_1 AUTO
#define FLIGHT_MODE_2 RTL
#define FLIGHT_MODE_3 FLY_BY_WIRE_A
#define FLIGHT_MODE_4 FLY_BY_WIRE_A
#define FLIGHT_MODE_5 MANUAL
#define FLIGHT_MODE_6 MANUAL
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE
#define HIL_MODE HIL_MODE_ATTITUDE
#define HIL_PORT 0
#define AIRSPEED_CRUISE 25
#define THROTTLE_FAILSAFE ENABLED
#define AIRSPEED_SENSOR ENABLED

976
ArduPlane/ArduPilotMega.pde Normal file
View File

@ -0,0 +1,976 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduPilotMega V2.23"
/*
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short
Thanks to: Chris Anderson, HappyKillMore, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi
Please contribute your ideas!
This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
*/
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <ModeFilter.h>
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <memcheck.h>
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include "HIL.h"
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g;
////////////////////////////////////////////////////////////////////////////////
// prototypes
static void update_events(void);
////////////////////////////////////////////////////////////////////////////////
// Sensors
////////////////////////////////////////////////////////////////////////////////
//
// There are three basic options related to flight sensor selection.
//
// - Normal flight mode. Real sensors are used.
// - HIL Attitude mode. Most sensors are disabled, as the HIL
// protocol supplies attitude information directly.
// - HIL Sensors mode. Synthetic sensors are configured that
// supply data from the simulation.
//
// All GPS access should be through this pointer.
static GPS *g_gps;
// flight modes convenience array
static AP_Int8 *flight_modes = &g.flight_mode1;
#if HIL_MODE == HIL_MODE_DISABLED
// real sensors
static AP_ADC_ADS7844 adc;
static APM_BMP085_Class barometer;
static AP_Compass_HMC5843 compass(Parameters::k_param_compass);
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL);
#else
#error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL
#elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators
AP_ADC_HIL adc;
APM_BMP085_HIL_Class barometer;
AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL);
#elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc;
AP_DCM_HIL dcm;
AP_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used
AP_IMU_Shim imu; // never used
#else
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
#elif HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
HIL_XPLANE hil;
#endif // HIL PROTOCOL
#endif // HIL_MODE
// We may have a hil object instantiated just for mission planning
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
GCS_MAVLINK hil(Parameters::k_param_streamrates_port0);
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS
// Normal
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration);
#else
// hil imu
AP_IMU_Shim imu;
#endif
// normal dcm
AP_DCM dcm(&imu, g_gps);
#endif
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
#else
// If we are not using a GCS, we need a stub that does nothing.
GCS_Class gcs;
#endif
////////////////////////////////////////////////////////////////////////////////
// SONAR selection
////////////////////////////////////////////////////////////////////////////////
//
ModeFilter sonar_mode_filter;
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#elif SONAR_TYPE == MAX_SONAR_LV
// XXX honestly I think these output the same values
// If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar(&adc, &sonar_mode_filter);//(SONAR_PORT, &adc);
#endif
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
byte control_mode = INITIALISING;
byte oldSwitchPosition; // for remembering the control mode switch
bool inverted_flight = false;
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"Manual",
"Circle",
"Stabilize",
"",
"",
"FBW_A",
"FBW_B",
"",
"",
"",
"Auto",
"RTL",
"Loiter",
"Takeoff",
"Land"};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
3 Throttle
4 Rudder (if we have ailerons)
5 Mode
6 TBD
7 TBD
8 TBD
*/
// Failsafe
// --------
static int failsafe; // track which type of failsafe is being processed
static bool ch3_failsafe;
static byte crash_timer;
// Radio
// -----
static uint16_t elevon1_trim = 1500; // TODO: handle in EEProm
static uint16_t elevon2_trim = 1500;
static uint16_t ch1_temp = 1500; // Used for elevon mixing
static uint16_t ch2_temp = 1500;
static int16_t rc_override[8] = {0,0,0,0,0,0,0,0};
static bool rc_override_active = false;
static uint32_t rc_override_fs_timer = 0;
static uint32_t ch3_failsafe_timer = 0;
// for elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are equivalent aileron and elevator, not left and right elevon
// LED output
// ----------
static bool GPS_light; // status of the GPS light
// GPS variables
// -------------
static const float t7 = 10000000.0; // used to scale GPS values for EEPROM storage
static float scaleLongUp = 1; // used to reverse longitude scaling
static float scaleLongDown = 1; // used to reverse longitude scaling
static byte ground_start_count = 5; // have we achieved first lock and set Home?
static int ground_start_avg; // 5 samples to avg speed for ground start
static bool GPS_enabled = false; // used to quit "looking" for gps with auto-detect if none present
// Location & Navigation
// ---------------------
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
static long hold_course = -1; // deg * 100 dir of plane
static byte command_must_index; // current command memory location
static byte command_may_index; // current command memory location
static byte command_must_ID; // current command ID
static byte command_may_ID; // current command ID
// Airspeed
// --------
static int airspeed; // m/s * 100
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
static float airspeed_error; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
// Location Errors
// ---------------
static long bearing_error; // deg * 100 : 0 to 36000
static long altitude_error; // meters * 100 we are off in altitude
static float crosstrack_error; // meters we are off trackline
// Battery Sensors
// ---------------
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float current_amps;
static float current_total;
// Airspeed Sensors
// ----------------
static float airspeed_raw; // Airspeed Sensor - is a float to better handle filtering
static int airspeed_pressure; // airspeed as a pressure value
// Barometer Sensor variables
// --------------------------
static unsigned long abs_pressure;
// Altitude Sensor variables
// ----------------------
static int sonar_alt;
// flight mode specific
// --------------------
static bool takeoff_complete = true; // Flag for using gps ground course instead of IMU yaw. Set false when takeoff command processes.
static bool land_complete;
static long takeoff_altitude;
// static int landing_distance; // meters;
static int landing_pitch; // pitch for landing set by commands
static int takeoff_pitch;
// Loiter management
// -----------------
static long old_target_bearing; // deg * 100
static int loiter_total; // deg : how many times to loiter * 360
static int loiter_delta; // deg : how far we just turned
static int loiter_sum; // deg : how far we have turned around a waypoint
static long loiter_time; // millis : when we started LOITER mode
static int loiter_time_max; // millis : how long to stay in LOITER mode
// these are the values for navigation control functions
// ----------------------------------------------------
static long nav_roll; // deg * 100 : target roll angle
static long nav_pitch; // deg * 100 : target pitch angle
static int throttle_nudge = 0; // 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
// Waypoints
// ---------
static long wp_distance; // meters - distance between plane and next waypoint
static long wp_totalDistance; // meters - distance between old and next waypoint
// repeating event control
// -----------------------
static byte event_id; // what to do - see defines
static long event_timer; // when the event was asked for in ms
static uint16_t event_delay; // how long to delay the next firing of event in millis
static int event_repeat = 0; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
static int event_value; // per command value, such as PWM for servos
static int event_undo_value; // the value used to cycle events (alternate value to event_value)
// delay command
// --------------
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
static long condition_start;
static int condition_rate;
// 3D Location vectors
// -------------------
static struct Location home; // home location
static struct Location prev_WP; // last waypoint
static struct Location current_loc; // current location
static struct Location next_WP; // next waypoint
static struct Location next_command; // command preloaded
static struct Location guided_WP; // guided mode waypoint
static long target_altitude; // used for altitude management between waypoints
static long offset_altitude; // used for altitude management between waypoints
static bool home_is_set; // Flag for if we have g_gps lock and have set the home location
// IMU variables
// -------------
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
// Performance monitoring
// ----------------------
static long perf_mon_timer; // Metric based on accel gain deweighting
static int G_Dt_max = 0; // Max main loop cycle time in milliseconds
static int gps_fix_count = 0;
static int pmTest1 = 0;
// System Timers
// --------------
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
static int mainLoop_count;
static unsigned long medium_loopTimer; // Time in miliseconds of medium loop
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
static uint8_t delta_ms_medium_loop;
static byte slow_loopCounter;
static byte superslow_loopCounter;
static byte counter_one_herz;
static unsigned long nav_loopTimer; // used to track the elapsed time for GPS nav
static unsigned long dTnav; // Delta Time in milliseconds for navigation computations
static float load; // % MCU cycles used
////////////////////////////////////////////////////////////////////////////////
// Top-level logic
////////////////////////////////////////////////////////////////////////////////
void setup() {
memcheck_init();
init_ardupilot();
}
void loop()
{
// We want this to execute at 50Hz if possible
// -------------------------------------------
if (millis()-fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
G_Dt = (float)delta_ms_fast_loop / 1000.f;
fast_loopTimer = millis();
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// Execute the medium loop
// -----------------------
medium_loop();
counter_one_herz++;
if(counter_one_herz == 50){
one_second_loop();
counter_one_herz = 0;
}
if (millis() - perf_mon_timer > 20000) {
if (mainLoop_count != 0) {
gcs.send_message(MSG_PERF_REPORT);
if (g.log_bitmask & MASK_LOG_PM)
Log_Write_Performance();
resetPerfData();
}
}
fast_loopTimeStamp = millis();
}
}
// Main loop 50Hz
static void fast_loop()
{
// This is the fast loop - we want it to execute at 50Hz if possible
// -----------------------------------------------------------------
if (delta_ms_fast_loop > G_Dt_max)
G_Dt_max = delta_ms_fast_loop;
// Read radio
// ----------
read_radio();
// try to send any deferred messages if the serial port now has
// some space available
gcs.send_message(MSG_RETRY_DEFERRED);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_RETRY_DEFERRED);
#endif
// check for loss of control signal failsafe condition
// ------------------------------------
check_short_failsafe();
// Read Airspeed
// -------------
if (g.airspeed_enabled == true && HIL_MODE != HIL_MODE_ATTITUDE) {
read_airspeed();
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
calc_airspeed_errors();
}
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
hil.update();
#endif
dcm.update_DCM(G_Dt);
// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();
# if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw();
#endif
// inertial navigation
// ------------------
#if INERTIAL_NAVIGATION == ENABLED
// TODO: implement inertial nav function
inertialNavigation();
#endif
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();
// apply desired roll, pitch and yaw to the plane
// ----------------------------------------------
if (control_mode > MANUAL)
stabilize();
// write out the servo PWM values
// ------------------------------
set_servos();
// XXX is it appropriate to be doing the comms below on the fast loop?
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT
// kick the HIL to process incoming sensor packets
hil.update();
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
hil.data_stream_send(45,1000);
#else
hil.send_message(MSG_SERVO_OUT);
#endif
#elif HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE == HIL_MODE_DISABLED && HIL_PORT == 0
// Case for hil object on port 0 just for mission planning
hil.update();
hil.data_stream_send(45,1000);
#endif
// kick the GCS to process uplink data
gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000);
#endif
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
}
static void medium_loop()
{
// This is the start of the medium (10 Hz) loop pieces
// -----------------------------------------
switch(medium_loopCounter) {
// This case deals with the GPS
//-------------------------------
case 0:
medium_loopCounter++;
if(GPS_enabled) update_GPS();
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
#endif
/*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Vector3f tempaccel = imu.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t"));
Serial.println(tempaccel.z, DEC);
}*/
break;
// This case performs some navigation computations
//------------------------------------------------
case 1:
medium_loopCounter++;
if(g_gps->new_data){
g_gps->new_data = false;
dTnav = millis() - nav_loopTimer;
nav_loopTimer = millis();
// calculate the plane's desired bearing
// -------------------------------------
navigate();
}
break;
// command processing
//------------------------------
case 2:
medium_loopCounter++;
// Read altitude from sensors
// ------------------
update_alt();
if(g.sonar_enabled) sonar_alt = sonar.read();
// altitude smoothing
// ------------------
if (control_mode != FLY_BY_WIRE_B)
calc_altitude_error();
// perform next command
// --------------------
update_commands();
break;
// This case deals with sending high rate telemetry
//-------------------------------------------------
case 3:
medium_loopCounter++;
#if HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor);
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
#endif
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
// XXX this should be a "GCS medium loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(5,45);
// send all requested output streams with rates requested
// between 5 and 45 Hz
#else
gcs.send_message(MSG_ATTITUDE); // Sends attitude data
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(5,45);
#endif
break;
// This case controls the slow loop
//---------------------------------
case 4:
medium_loopCounter = 0;
delta_ms_medium_loop = millis() - medium_loopTimer;
medium_loopTimer = millis();
if (g.battery_monitoring != 0){
read_battery();
}
slow_loop();
break;
}
}
static void slow_loop()
{
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter){
case 0:
slow_loopCounter++;
check_long_failsafe();
superslow_loopCounter++;
if(superslow_loopCounter >=200) { // 200 = Execute every minute
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled) {
compass.save_offsets();
}
#endif
superslow_loopCounter = 0;
}
break;
case 1:
slow_loopCounter++;
// Read 3-position switch on radio
// -------------------------------
read_control_switch();
// Read Control Surfaces/Mix switches
// ----------------------------------
update_servo_switches();
break;
case 2:
slow_loopCounter = 0;
update_events();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
gcs.data_stream_send(3,5);
// send all requested output streams with rates requested
// between 3 and 5 Hz
#else
gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(3,5);
#endif
break;
}
}
static void one_second_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
// send a heartbeat
gcs.send_message(MSG_HEARTBEAT);
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,3);
// send all requested output streams with rates requested
// between 1 and 3 Hz
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
hil.data_stream_send(1,3);
#endif
}
static void update_GPS(void)
{
g_gps->update();
update_GPS_light();
if (g_gps->new_data && g_gps->fix) {
// XXX We should be sending GPS data off one of the regular loops so that we send
// no-GPS-fix data too
#if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK
gcs.send_message(MSG_LOCATION);
#endif
// for performance
// ---------------
gps_fix_count++;
if(ground_start_count > 1){
ground_start_count--;
ground_start_avg += g_gps->ground_speed;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0) {
SendDebugln("!! bad loc");
ground_start_count = 5;
} else {
if(ENABLE_AIR_START == 1 && (ground_start_avg / 5) < SPEEDFILT){
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
init_home();
} else if (ENABLE_AIR_START == 0) {
init_home();
}
ground_start_count = 0;
}
}
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
}
}
static void update_current_flight_mode(void)
{
if(control_mode == AUTO){
crash_checker();
switch(command_must_ID){
case MAV_CMD_NAV_TAKEOFF:
if (hold_course > -1) {
calc_nav_roll();
} else {
nav_roll = 0;
}
if (g.airspeed_enabled == true)
{
calc_nav_pitch();
if (nav_pitch < (long)takeoff_pitch) nav_pitch = (long)takeoff_pitch;
} else {
nav_pitch = (long)((float)g_gps->ground_speed / (float)g.airspeed_cruise * (float)takeoff_pitch * 0.5);
nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch);
}
g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle
// What is the case for doing something else? Why wouldn't you want max throttle for TO?
// ******************************
break;
case MAV_CMD_NAV_LAND:
calc_nav_roll();
if (g.airspeed_enabled == true){
calc_nav_pitch();
calc_throttle();
}else{
calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle
calc_throttle(); // throttle based on altitude error
nav_pitch = landing_pitch; // pitch held constant
}
if (land_complete){
g.channel_throttle.servo_out = 0;
}
break;
default:
hold_course = -1;
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
}
}else{
switch(control_mode){
case RTL:
case LOITER:
case GUIDED:
hold_course = -1;
crash_checker();
calc_nav_roll();
calc_nav_pitch();
calc_throttle();
break;
case FLY_BY_WIRE_A:
// fake Navigation output using sticks
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min;
// We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign.
nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority
if (inverted_flight) nav_pitch = -nav_pitch;
break;
case FLY_BY_WIRE_B:
// fake Navigation output using sticks
// We use g.pitch_limit_min because its magnitude is
// normally greater than g.pitch_limit_max
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if (g.airspeed_enabled == true)
{
airspeed_error = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) +
((int)g.flybywire_airspeed_min * 100);
// Intermediate calculation - airspeed_error is just desired airspeed at this point
airspeed_energy_error = (long)(((long)airspeed_error *
(long)airspeed_error) -
((long)airspeed * (long)airspeed))/20000;
//Changed 0.00005f * to / 20000 to avoid floating point calculation
airspeed_error = (airspeed_error - airspeed);
}
calc_throttle();
calc_nav_pitch();
break;
case STABILIZE:
nav_roll = 0;
nav_pitch = 0;
// throttle is passthrough
break;
case CIRCLE:
// we have no GPS installed and have lost radio contact
// or we just want to fly around in a gentle circle w/o GPS
// ----------------------------------------------------
nav_roll = g.roll_limit / 3;
nav_pitch = 0;
if (failsafe != FAILSAFE_NONE){
g.channel_throttle.servo_out = g.throttle_cruise;
}
break;
case MANUAL:
// servo_out is for Sim control only
// ---------------------------------
g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
}
}
}
static void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO){
verify_commands();
}else{
switch(control_mode){
case LOITER:
case RTL:
case GUIDED:
update_loiter();
calc_bearing_error();
break;
}
}
}
static void update_alt()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
// this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO;
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100)
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
#endif
// Calculate new climb rate
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
}

370
ArduPlane/Attitude.pde Normal file
View File

@ -0,0 +1,370 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
static void stabilize()
{
float ch1_inf = 1.0;
float ch2_inf = 1.0;
float ch4_inf = 1.0;
float speed_scaler;
if (g.airspeed_enabled == true){
if(airspeed > 0)
speed_scaler = (STANDARD_SPEED * 100) / airspeed;
else
speed_scaler = 2.0;
speed_scaler = constrain(speed_scaler, 0.5, 2.0);
} else {
if (g.channel_throttle.servo_out > 0){
speed_scaler = 0.5 + ((float)THROTTLE_CRUISE / g.channel_throttle.servo_out / 2.0); // First order taylor expansion of square root
// Should maybe be to the 2/7 power, but we aren't goint to implement that...
}else{
speed_scaler = 1.67;
}
speed_scaler = constrain(speed_scaler, 0.6, 1.67); // This case is constrained tighter as we don't have real speed info
}
if(crash_timer > 0){
nav_roll = 0;
}
if (inverted_flight) {
// we want to fly upside down. We need to cope with wrap of
// the roll_sensor interfering with wrap of nav_roll, which
// would really confuse the PID code. The easiest way to
// handle this is to ensure both go in the same direction from
// zero
nav_roll += 18000;
if (dcm.roll_sensor < 0) nav_roll -= 36000;
}
// For Testing Only
// roll_sensor = (radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 10;
// Serial.printf_P(PSTR(" roll_sensor "));
// Serial.print(roll_sensor,DEC);
// Calculate dersired servo output for the roll
// ---------------------------------------------
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler);
long tempcalc = nav_pitch +
fabs(dcm.roll_sensor * g.kff_pitch_compensation) +
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
(dcm.pitch_sensor - g.pitch_trim);
if (inverted_flight) {
// when flying upside down the elevator control is inverted
tempcalc = -tempcalc;
}
g.channel_pitch.servo_out = g.pidServoPitch.get_pid(tempcalc, delta_ms_fast_loop, speed_scaler);
// Mix Stick input to allow users to override control surfaces
// -----------------------------------------------------------
if ((control_mode < FLY_BY_WIRE_A) || (ENABLE_STICK_MIXING == 1 && control_mode > FLY_BY_WIRE_B)) {
// TODO: use RC_Channel control_mix function?
ch1_inf = (float)g.channel_roll.radio_in - (float)g.channel_roll.radio_trim;
ch1_inf = fabs(ch1_inf);
ch1_inf = min(ch1_inf, 400.0);
ch1_inf = ((400.0 - ch1_inf) /400.0);
ch2_inf = (float)g.channel_pitch.radio_in - g.channel_pitch.radio_trim;
ch2_inf = fabs(ch2_inf);
ch2_inf = min(ch2_inf, 400.0);
ch2_inf = ((400.0 - ch2_inf) /400.0);
// scale the sensor input based on the stick input
// -----------------------------------------------
g.channel_roll.servo_out *= ch1_inf;
g.channel_pitch.servo_out *= ch2_inf;
// Mix in stick inputs
// -------------------
g.channel_roll.servo_out += g.channel_roll.pwm_to_angle();
g.channel_pitch.servo_out += g.channel_pitch.pwm_to_angle();
//Serial.printf_P(PSTR(" servo_out[CH_ROLL] "));
//Serial.println(servo_out[CH_ROLL],DEC);
}
// stick mixing performed for rudder for all cases including FBW unless disabled for higher modes
// important for steering on the ground during landing
// -----------------------------------------------
if (control_mode <= FLY_BY_WIRE_B || ENABLE_STICK_MIXING == 1) {
ch4_inf = (float)g.channel_rudder.radio_in - (float)g.channel_rudder.radio_trim;
ch4_inf = fabs(ch4_inf);
ch4_inf = min(ch4_inf, 400.0);
ch4_inf = ((400.0 - ch4_inf) /400.0);
}
// Apply output to Rudder
// ----------------------
calc_nav_yaw(speed_scaler);
g.channel_rudder.servo_out *= ch4_inf;
g.channel_rudder.servo_out += g.channel_rudder.pwm_to_angle();
// Call slew rate limiter if used
// ------------------------------
//#if(ROLL_SLEW_LIMIT != 0)
// g.channel_roll.servo_out = roll_slew_limit(g.channel_roll.servo_out);
//#endif
}
static void crash_checker()
{
if(dcm.pitch_sensor < -4500){
crash_timer = 255;
}
if(crash_timer > 0)
crash_timer--;
}
static void calc_throttle()
{
if (g.airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge;
// no airspeed sensor, we use nav pitch to determine the proper throttle output
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
if (nav_pitch >= 0) {
g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max;
} else {
g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min;
}
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
} else {
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
// positive energy errors make the throttle go higher
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error, dTnav);
g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current"
}
}
/*****************************************
* Calculate desired roll/pitch/yaw angles (in medium freq loop)
*****************************************/
// Yaw is separated into a function for future implementation of heading hold on rolling take-off
// ----------------------------------------------------------------------------------------
static void calc_nav_yaw(float speed_scaler)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
Vector3f temp = imu.get_accel();
long error = -temp.y;
// Control is a feedforward from the aileron control + a PID to coordinate the turn (drive y axis accel to zero)
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out + g.pidServoRudder.get_pid(error, delta_ms_fast_loop, speed_scaler);
#else
g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
// XXX probably need something here based on heading
#endif
}
static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (g.airspeed_enabled == true) {
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav);
} else {
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error, dTnav);
}
nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get());
}
#define YAW_DAMPENER 0
static void calc_nav_roll()
{
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc.
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega;
omega = dcm.get_gyro();
// rate limiter
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -6000, 6000); // limit input
int dampener = rate * YAW_DAMPENER; // 34377 * .175 = 6000
// add in yaw dampener
nav_roll -= dampener;
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
}
/*****************************************
* Roll servo slew limit
*****************************************/
/*
float roll_slew_limit(float servo)
{
static float last;
float temp = constrain(servo, last-ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f, last + ROLL_SLEW_LIMIT * delta_ms_fast_loop/1000.f);
last = servo;
return temp;
}*/
/*****************************************
* Throttle slew limit
*****************************************/
static void throttle_slew_limit()
{
static int last = 1000;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last);
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
last = g.channel_throttle.radio_out;
}
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_I(void)
{
g.pidNavRoll.reset_I();
g.pidNavPitchAirspeed.reset_I();
g.pidNavPitchAltitude.reset_I();
g.pidTeThrottle.reset_I();
// g.pidAltitudeThrottle.reset_I();
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
static void set_servos(void)
{
int flapSpeedSource = 0;
if(control_mode == MANUAL){
// do a direct pass through of radio values
if (g.mix_mode == 0){
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL);
g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH);
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_out = g.rc_6.radio_in;
} else {
if (g.mix_mode == 0) {
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_rudder.calc_pwm();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.servo_out = g.channel_roll.servo_out;
g.rc_5.calc_pwm();
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.servo_out = g.channel_roll.servo_out;
g.rc_6.calc_pwm();
}
}else{
/*Elevon mode*/
float ch1;
float ch2;
ch1 = BOOL_TO_SIGN(g.reverse_elevons) * (g.channel_pitch.servo_out - g.channel_roll.servo_out);
ch2 = g.channel_pitch.servo_out + g.channel_roll.servo_out;
g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX));
g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
}
#if THROTTLE_OUT == 0
g.channel_throttle.servo_out = 0;
#else
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
#endif
g.channel_throttle.calc_pwm();
/* TO DO - fix this for RC_Channel library
#if THROTTLE_REVERSE == 1
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
#endif
*/
throttle_slew_limit();
}
if(control_mode <= FLY_BY_WIRE_B) {
if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.radio_out = g.rc_5.radio_in;
if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.radio_out = g.rc_6.radio_in;
} else if (control_mode >= FLY_BY_WIRE_C) {
if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise;
} else {
flapSpeedSource = g.throttle_cruise;
}
if ( flapSpeedSource > g.flap_1_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = 0;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = 0;
} else if (flapSpeedSource > g.flap_2_speed) {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_1_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_1_percent;
} else {
if(g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO) g.rc_5.servo_out = g.flap_2_percent;
if(g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO) g.rc_6.servo_out = g.flap_2_percent;
}
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
APM_RC.OutputCh(CH_5, g.rc_5.radio_out); // send to Servos
APM_RC.OutputCh(CH_6, g.rc_6.radio_out); // send to Servos
#endif
}
static void demo_servos(byte i) {
while(i > 0){
gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
mavlink_delay(400);
APM_RC.OutputCh(1, 1600);
mavlink_delay(200);
APM_RC.OutputCh(1, 1500);
#endif
mavlink_delay(400);
i--;
}
}

206
ArduPlane/GCS.h Normal file
View File

@ -0,0 +1,206 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file GCS.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __GCS_H
#define __GCS_H
#include <FastSerial.h>
#include <AP_Common.h>
#include <GCS_MAVLink.h>
#include <GPS.h>
#include <Stream.h>
#include <stdint.h>
///
/// @class GCS
/// @brief Class describing the interface between the APM code
/// proper and the GCS implementation.
///
/// GCS' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call GCS
/// internal functions - all calls to the GCS should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class GCS_Class
{
public:
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required before
/// GCS messages are exchanged.
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @note The stream is currently BetterStream so that we can use the _P
/// methods; this may change if Arduino adds them to Print.
///
/// @param port The stream over which messages are exchanged.
///
void init(FastSerial *port) { _port = port; }
/// Update GCS state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {}
/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the GCS driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(uint8_t id, int32_t param = 0) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const char *str) {}
#define send_text_P(severity, msg) send_text(severity, msg)
/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const prog_char_t *str) {}
/// Send acknowledgement for a message.
///
/// @param id The message ID being acknowledged.
/// @param sum1 Checksum byte 1 from the message being acked.
/// @param sum2 Checksum byte 2 from the message being acked.
///
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
/// Emit an update of the "current" waypoints, often previous, current and
/// next.
///
void print_current_waypoints() {}
//
// The following interfaces are not currently implemented as their counterparts
// are not called in the mainline code. XXX ripe for re-specification.
//
/// Send a text message with printf-style formatting.
///
/// @param severity A value describing the importance of the message.
/// @param fmt The format string to send.
/// @param ... Additional arguments to the format string.
///
// void send_message(uint8_t severity, const char *fmt, ...) {}
/// Log a waypoint
///
/// @param wp The waypoint to log.
/// @param index The index of the waypoint.
// void print_waypoint(struct Location *wp, uint8_t index) {}
// test if frequency within range requested for loop
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
}
// send streams which match frequency range
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
protected:
/// The stream we are communicating over
FastSerial *_port;
};
//
// GCS class definitions.
//
// These are here so that we can declare the GCS object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class GCS_MAVLINK
/// @brief The mavlink protocol for qgroundcontrol
///
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
class GCS_MAVLINK : public GCS_Class
{
public:
GCS_MAVLINK(AP_Var::Key key);
void update(void);
void init(FastSerial *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
void data_stream_send(uint16_t freqMin, uint16_t freqMax);
private:
void handleMessage(mavlink_message_t * msg);
/// Perform queued sending operations
///
void _queued_send();
AP_Var *_queued_parameter; ///< next parameter to be sent in queue
uint16_t _queued_parameter_index; ///< next queued parameter's index
uint16_t _queued_parameter_count; ///< saved count of parameters for queued send
/// Count the number of reportable parameters.
///
/// Not all parameters can be reported via MAVlink. We count the number that are
/// so that we can report to a GCS the number of parameters it should expect when it
/// requests the full set.
///
/// @return The number of reportable parameters.
///
uint16_t _count_parameters(); ///< count reportable parameters
uint16_t _parameter_count; ///< cache of reportable parameters
AP_Var *_find_parameter(uint16_t index); ///< find a reportable parameter by index
mavlink_channel_t chan;
uint16_t packet_drops;
// waypoints
uint16_t requested_interface; // request port to use
uint16_t waypoint_request_i; // request index
uint16_t waypoint_dest_sysid; // where to send requests
uint16_t waypoint_dest_compid; // "
bool waypoint_sending; // currently in send process
bool waypoint_receiving; // currently receiving
uint16_t waypoint_count;
uint32_t waypoint_timelast_send; // milliseconds
uint32_t waypoint_timelast_receive; // milliseconds
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
float junk; //used to return a junk value for interface
// data stream rates
AP_Var_group _group;
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
AP_Int16 streamRateRawController;
AP_Int16 streamRatePosition;
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
};
#endif // GCS_PROTOCOL_MAVLINK
#endif // __GCS_H

1147
ArduPlane/GCS_Mavlink.pde Normal file
View File

File diff suppressed because it is too large Load Diff

135
ArduPlane/HIL.h Normal file
View File

@ -0,0 +1,135 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file HIL.h
/// @brief Interface definition for the various Ground Control System protocols.
#ifndef __HIL_H
#define __HIL_H
# if HIL_MODE != HIL_MODE_DISABLED
#include <FastSerial.h>
#include <AP_Common.h>
#include <GPS.h>
#include <Stream.h>
#include <stdint.h>
///
/// @class HIL
/// @brief Class describing the interface between the APM code
/// proper and the HIL implementation.
///
/// HIL' are currently implemented inside the sketch and as such have
/// access to all global state. The sketch should not, however, call HIL
/// internal functions - all calls to the HIL should be routed through
/// this interface (or functions explicitly exposed by a subclass).
///
class HIL_Class
{
public:
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required before
/// HIL messages are exchanged.
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @note The stream is currently BetterStream so that we can use the _P
/// methods; this may change if Arduino adds them to Print.
///
/// @param port The stream over which messages are exchanged.
///
void init(FastSerial *port) { _port = port; }
/// Update HIL state.
///
/// This may involve checking for received bytes on the stream,
/// or sending additional periodic messages.
void update(void) {}
/// Send a message with a single numeric parameter.
///
/// This may be a standalone message, or the HIL driver may
/// have its own way of locating additional parameters to send.
///
/// @param id ID of the message to send.
/// @param param Explicit message parameter.
///
void send_message(uint8_t id, int32_t param = 0) {}
/// Send a text message.
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const char *str) {}
/// Send a text message with a PSTR()
///
/// @param severity A value describing the importance of the message.
/// @param str The text to be sent.
///
void send_text(uint8_t severity, const prog_char_t *str) {}
/// Send acknowledgement for a message.
///
/// @param id The message ID being acknowledged.
/// @param sum1 Checksum byte 1 from the message being acked.
/// @param sum2 Checksum byte 2 from the message being acked.
///
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {}
protected:
/// The stream we are communicating over
FastSerial *_port;
};
//
// HIL class definitions.
//
// These are here so that we can declare the HIL object early in the sketch
// and then reference it statically rather than via a pointer.
//
///
/// @class HIL_MAVLINK
/// @brief The mavlink protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
// uses gcs
#endif // HIL_PROTOCOL_MAVLINK
///
/// @class HIL_XPLANE
/// @brief The xplane protocol for hil
///
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
class HIL_XPLANE : public HIL_Class
{
public:
HIL_XPLANE();
void update(void);
void init(FastSerial *port);
void send_message(uint8_t id, uint32_t param = 0);
void send_text(uint8_t severity, const char *str);
void send_text(uint8_t severity, const prog_char_t *str);
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2);
private:
void output_HIL();
void output_HIL_();
void output_int(int value);
void output_byte(byte value);
void print_buffer();
AP_GPS_IMU * hilPacketDecoder;
byte buf_len;
byte out_buffer[32];
};
#endif // HIL_PROTOCOL_XPLANE
#endif // HIL not disabled
#endif // __HIL_H

131
ArduPlane/HIL_Xplane.pde Normal file
View File

@ -0,0 +1,131 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
void HIL_XPLANE::output_HIL(void)
{
// output real-time sensor data
Serial.printf_P(PSTR("AAA")); // Message preamble
output_int((int)(g.channel_roll.servo_out)); // 0 bytes 0, 1
output_int((int)(g.channel_pitch.servo_out)); // 1 bytes 2, 3
output_int((int)(g.channel_throttle.servo_out)); // 2 bytes 4, 5
output_int((int)(g.channel_rudder.servo_out)); // 3 bytes 6, 7
output_int((int)wp_distance); // 4 bytes 8,9
output_int((int)bearing_error); // 5 bytes 10,11
output_int((int)altitude_error); // 6 bytes 12, 13
output_int((int)energy_error); // 7 bytes 14,15
output_byte((int)g.waypoint_index); // 8 bytes 16
output_byte(control_mode); // 9 bytes 17
// print out the buffer and checksum
// ---------------------------------
print_buffer();
}
void HIL_XPLANE::output_int(int value)
{
//buf_len += 2;
out_buffer[buf_len++] = value & 0xff;
out_buffer[buf_len++] = (value >> 8) & 0xff;
}
void HIL_XPLANE::output_byte(byte value)
{
out_buffer[buf_len++] = value;
}
void HIL_XPLANE::print_buffer()
{
byte ck_a = 0;
byte ck_b = 0;
for (byte i = 0;i < buf_len; i++){
Serial.print (out_buffer[i]);
}
Serial.print('\r');
Serial.print('\n');
buf_len = 0;
}
HIL_XPLANE::HIL_XPLANE() :
buf_len(0)
{
}
void
HIL_XPLANE::init(FastSerial * port)
{
HIL_Class::init(port);
hilPacketDecoder = new AP_GPS_IMU(port);
hilPacketDecoder->init();
}
void
HIL_XPLANE::update(void)
{
hilPacketDecoder->update();
airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy
calc_airspeed_errors();
dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000,
hilPacketDecoder->pitch_sensor*M_PI/18000,
hilPacketDecoder->ground_course*M_PI/18000,
0,0,0);
// set gps hil sensor
g_gps->setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2,
(float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0);
}
void
HIL_XPLANE::send_message(uint8_t id, uint32_t param)
{
// TODO: split output by actual request
uint64_t timeStamp = micros();
switch(id) {
case MSG_HEARTBEAT:
break;
case MSG_EXTENDED_STATUS:
break;
case MSG_ATTITUDE:
break;
case MSG_LOCATION:
break;
case MSG_LOCAL_LOCATION:
break;
case MSG_GPS_RAW:
break;
case MSG_SERVO_OUT:
output_HIL();
break;
case MSG_RADIO_OUT:
break;
case MSG_RAW_IMU:
break;
case MSG_GPS_STATUS:
break;
case MSG_CURRENT_WAYPOINT:
break;
defualt:
break;
}
}
void
HIL_XPLANE::send_text(uint8_t severity, const char *str)
{
}
void
HIL_XPLANE::send_text(uint8_t severity, const prog_char_t *str)
{
}
void
HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif

750
ArduPlane/Log.pde Normal file
View File

@ -0,0 +1,750 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
#define HEAD_BYTE1 0xA3 // Decimal 163
#define HEAD_BYTE2 0x95 // Decimal 149
#define END_BYTE 0xBA // Decimal 186
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t help_log(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n"
"Commands:\n"
" dump <n> dump log <n>\n"
" erase erase all logs\n"
" enable <name>|all enable logging <name> or everything\n"
" disable <name>|all disable logging <name> or everything\n"
"\n"));
return 0;
}
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs},
{"help", help_log}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
static void get_log_boundaries(byte log_num, int & start_page, int & end_page);
static bool
print_log_menu(void)
{
int log_start;
int log_end;
byte last_log_num = get_num_logs();
Serial.printf_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none"));
}else{
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(MODE);
PLOG(RAW);
PLOG(CMD);
PLOG(CUR);
#undef PLOG
}
Serial.println();
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs available for download\n"));
}else{
Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) {
get_log_boundaries(i, log_start, log_end);
Serial.printf_P(PSTR("Log number %d, start page %d, end page %d\n"),
i, log_start, log_end);
}
Serial.println();
}
return(true);
}
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
{
byte dump_log;
int dump_log_start;
int dump_log_end;
byte last_log_num;
// check that the requested log number can be read
dump_log = argv[1].i;
last_log_num = get_num_logs();
if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
return(-1);
}
get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"),
dump_log,
dump_log_start,
dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Log read complete\n"));
return 0;
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
for(int i = 10 ; i > 0; i--) {
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i);
delay(1000);
}
Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j);
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INDEX_MSG);
DataFlash.WriteByte(0);
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
Serial.printf_P(PSTR("\nLog erased.\n"));
return 0;
}
static int8_t
select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
if (argc != 2) {
Serial.printf_P(PSTR("missing log type\n"));
return(-1);
}
bits = 0;
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// that name as the argument to the command, and set the bit in
// bits accordingly.
//
if (!strcasecmp_P(argv[1].str, PSTR("all"))) {
bits = ~0;
} else {
#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s
TARG(ATTITUDE_FAST);
TARG(ATTITUDE_MED);
TARG(GPS);
TARG(PM);
TARG(CTUN);
TARG(NTUN);
TARG(MODE);
TARG(RAW);
TARG(CMD);
TARG(CUR);
#undef TARG
}
if (!strcasecmp_P(argv[0].str, PSTR("enable"))) {
g.log_bitmask.set_and_save(g.log_bitmask | bits);
}else{
g.log_bitmask.set_and_save(g.log_bitmask & ~bits);
}
return(0);
}
static int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
}
static byte get_num_logs(void)
{
int page = 1;
byte data;
byte log_step = 0;
DataFlash.StartRead(1);
while (page == 1) {
data = DataFlash.ReadByte();
switch(log_step){ //This is a state machine to read the packets
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
return num_logs;
}else{
log_step=0; // Restart, we have a problem...
}
break;
}
page = DataFlash.GetPage();
}
return 0;
}
// send the number of the last log?
static void start_new_log(byte num_existing_logs)
{
int start_pages[50] = {0,0,0};
int end_pages[50] = {0,0,0};
if(num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(i+1,start_pages[i],end_pages[i]);
}
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
if(num_existing_logs > 0)
start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
else
start_pages[0] = 2;
num_existing_logs++;
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INDEX_MSG);
DataFlash.WriteByte(num_existing_logs);
for(int i=0;i<MAX_NUM_LOGS;i++) {
DataFlash.WriteInt(start_pages[i]);
DataFlash.WriteInt(end_pages[i]);
}
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
}else{
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full - logging discontinued"));
}
}
static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
{
int page = 1;
byte data;
byte log_step = 0;
DataFlash.StartRead(1);
while (page == 1) {
data = DataFlash.ReadByte();
switch(log_step) //This is a state machine to read the packets
{
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
for(int i=0;i<log_num;i++) {
start_page = DataFlash.ReadInt();
end_page = DataFlash.ReadInt();
}
if(log_num==num_logs)
end_page = find_last_log_page(start_page);
return; // This is the normal exit point
}else{
log_step=0; // Restart, we have a problem...
}
break;
}
page = DataFlash.GetPage();
}
// Error condition if we reach here with page = 2 TO DO - report condition
}
static int find_last_log_page(int bottom_page)
{
int top_page = 4096;
int look_page;
long check;
while((top_page - bottom_page) > 1) {
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
check = DataFlash.ReadLong();
if(check == (long)0xFFFFFFFF)
top_page = look_page;
else
bottom_page = look_page;
}
return top_page;
}
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(log_roll);
DataFlash.WriteInt(log_pitch);
DataFlash.WriteInt(log_yaw);
DataFlash.WriteByte(END_BYTE);
}
// Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt(mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(dcm.gyro_sat_count);
DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(dcm.renorm_sqrt_count);
DataFlash.WriteByte(dcm.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_health() * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE);
}
// Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
static void Log_Write_Cmd(byte num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
static void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_wp_with_index(0);
Log_Write_Cmd(0, &cmd);
for (int i = 1; i <= g.waypoint_total; i++){
cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd);
}
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning()
{
Vector3f accel = imu.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(g.channel_roll.servo_out));
DataFlash.WriteInt((int)nav_roll);
DataFlash.WriteInt((int)dcm.roll_sensor);
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch);
DataFlash.WriteInt((int)dcm.pitch_sensor);
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
}
#endif
// Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)dcm.yaw_sensor);
DataFlash.WriteInt((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing);
DataFlash.WriteInt(altitude_error);
DataFlash.WriteInt((int)airspeed);
DataFlash.WriteInt((int)(nav_gain_scaler*1000));
DataFlash.WriteByte(END_BYTE);
}
// Write a mode packet. Total length : 5 bytes
static void Log_Write_Mode(byte mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteByte(END_BYTE);
}
// Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(log_Time);
DataFlash.WriteByte(log_Fix);
DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude);
DataFlash.WriteLong(log_Longitude);
DataFlash.WriteInt(sonar_alt); // This one is just temporary for testing out sonar in fixed wing
DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteByte(END_BYTE);
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Raw()
{
Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z);
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
DataFlash.WriteByte(END_BYTE);
}
#endif
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.channel_throttle.control_in);
DataFlash.WriteInt((int)(battery_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total);
DataFlash.WriteByte(END_BYTE);
}
// Read a Current packet
static void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt());
}
// Read an control tuning packet
static void Log_Read_Control_Tuning()
{
float logvar;
Serial.printf_P(PSTR("CTUN:"));
for (int y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a nav tuning packet
static void Log_Read_Nav_Tuning()
{
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
(float)((uint16_t)DataFlash.ReadInt())/100.0,
DataFlash.ReadInt(),
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/1000.0);
}
// Read a performance packet
static void Log_Read_Performance()
{
long pm_time;
int logvar;
Serial.printf_P(PSTR("PM:"));
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y = 1; y <= 9; y++) {
if(y < 3 || y > 7){
logvar = DataFlash.ReadInt();
}else{
logvar = DataFlash.ReadByte();
}
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read a command processing packet
static void Log_Read_Cmd()
{
byte logvarb;
long logvarl;
Serial.printf_P(PSTR("CMD:"));
for(int i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte();
Serial.print(logvarb, DEC);
Serial.print(comma);
}
for(int i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong();
Serial.print(logvarl, DEC);
Serial.print(comma);
}
Serial.println(" ");
}
static void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG)
Serial.printf_P(PSTR("AIR START - "));
else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.printf_P(PSTR("GROUND START - "));
else
Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
}
// Read an attitude packet
static void Log_Read_Attitude()
{
Serial.printf_P(PSTR("ATT: %d, %d, %u\n"),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(uint16_t)DataFlash.ReadInt());
}
// Read a mode packet
static void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
}
// Read a GPS packet
static void Log_Read_GPS()
{
Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f\n"),
DataFlash.ReadLong(),
(int)DataFlash.ReadByte(),
(int)DataFlash.ReadByte(),
(float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadLong() / t7,
(float)DataFlash.ReadInt(), // This one is just temporary for testing out sonar in fixed wing
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0,
(float)DataFlash.ReadLong() / 100.0);
}
// Read a raw accel/gyro packet
static void Log_Read_Raw()
{
float logvar;
Serial.printf_P(PSTR("RAW:"));
for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar);
Serial.print(comma);
}
Serial.println(" ");
}
// Read the DataFlash log memory : Packet Parser
static void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step = 0;
int packet_count = 0;
int page = start_page;
#ifdef AIRFRAME_NAME
Serial.printf_P(PSTR((AIRFRAME_NAME)
#endif
Serial.printf_P(PSTR("\n" THISFIRMWARE
"\nFree RAM: %lu\n"),
freeRAM());
DataFlash.StartRead(start_page);
while (page < end_page && page != -1){
data = DataFlash.ReadByte();
switch(log_step) // This is a state machine to read the packets
{
case 0:
if(data == HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data == HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data == LOG_ATTITUDE_MSG){
Log_Read_Attitude();
log_step++;
}else if(data == LOG_MODE_MSG){
Log_Read_Mode();
log_step++;
}else if(data == LOG_CONTROL_TUNING_MSG){
Log_Read_Control_Tuning();
log_step++;
}else if(data == LOG_NAV_TUNING_MSG){
Log_Read_Nav_Tuning();
log_step++;
}else if(data == LOG_PERFORMANCE_MSG){
Log_Read_Performance();
log_step++;
}else if(data == LOG_RAW_MSG){
Log_Read_Raw();
log_step++;
}else if(data == LOG_CMD_MSG){
Log_Read_Cmd();
log_step++;
}else if(data == LOG_CURRENT_MSG){
Log_Read_Current();
log_step++;
}else if(data == LOG_STARTUP_MSG){
Log_Read_Startup();
log_step++;
}else {
if(data == LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
}else{
Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem...
}
}
break;
case 3:
if(data == END_BYTE){
packet_count++;
}else{
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
}
log_step = 0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
}
Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count);
}
#else // LOGGING_ENABLED
// dummy functions
static void Log_Write_Mode(byte mode) {}
static void Log_Write_Startup(byte type) {}
static void Log_Write_Cmd(byte num, struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long log_gps_alt, long log_mix_alt,
long log_Ground_Speed, long log_Ground_Course, byte log_Fix, byte log_NumSats) {}
static void Log_Write_Performance() {}
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
static byte get_num_logs(void) { return 0; }
static void start_new_log(byte num_existing_logs) {}
static void Log_Write_Attitude(int log_roll, int log_pitch, uint16_t log_yaw) {}
static void Log_Write_Control_Tuning() {}
static void Log_Write_Raw() {}
#endif // LOGGING_ENABLED

10
ArduPlane/Makefile Normal file
View File

@ -0,0 +1,10 @@
#
# Trivial makefile for building APM
#
#
# Select 'mega' for the original APM, or 'mega2560' for the V2 APM.
#
BOARD = mega
include ../libraries/AP_Common/Arduino.mk

433
ArduPlane/Mavlink_Common.h Normal file
View File

@ -0,0 +1,433 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef Mavlink_Common_H
#define Mavlink_Common_H
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
byte mavdelay = 0;
static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
{
//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC); //This line for debug only
if (sysid != mavlink_system.sysid){
return 1;
// Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem
// If it is addressed to our system ID we assume it is for us
//}else if(compid != mavlink_system.compid){
// gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch"));
// return 1; // XXX currently not receiving correct compid from gcs
}else{
return 0; // no error
}
}
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MSG_EXTENDED_STATUS1:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST2;
nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc.
break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LOITER;
break;
case INITIALISING:
mode = MAV_MODE_UNINIT;
nav_mode = MAV_NAV_GROUNDED;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage * 1000,
battery_remaining,
packet_drops);
break;
}
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
break;
}
case MSG_ATTITUDE:
{
CHECK_PAYLOAD_SIZE(ATTITUDE);
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(
chan,
timeStamp,
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
break;
}
case MSG_LOCATION:
{
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed * rot.a.x,
g_gps->ground_speed * rot.b.x,
g_gps->ground_speed * rot.c.x);
break;
}
case MSG_NAV_CONTROLLER_OUTPUT:
{
if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
altitude_error / 1.0e2,
airspeed_error,
crosstrack_error);
}
break;
}
case MSG_LOCAL_LOCATION:
{
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(
chan,
timeStamp,
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
(current_loc.alt - home.alt) / 1.0e2,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
}
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
break;
}
case MSG_SERVO_OUT:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
break;
}
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
break;
}
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
break;
}
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
(dcm.yaw_sensor / 100) % 360,
(int)g.channel_throttle.servo_out,
current_loc.alt / 100.0,
climb_rate);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU1:
{
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(
chan,
timeStamp,
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send(
chan,
timeStamp,
(float)barometer.Press/100.0,
(float)(barometer.Press-g.ground_pressure)/100.0,
(int)(barometer.Temp*10));
break;
}
case MSG_RAW_IMU3:
{
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
break;
}
default:
break;
}
return true;
}
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
// switch types in mavlink_try_send_message()
static struct mavlink_queue {
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) {
// don't send status MAVLink messages for 1 second after
// bootup, to try to prevent Xbee bricking
return;
}
mavlink_msg_statustext_send(
chan,
severity,
(const int8_t*) str);
}
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)
{
}
#endif // mavlink in use
#endif // inclusion guard

465
ArduPlane/Parameters.h Normal file
View File

@ -0,0 +1,465 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef PARAMETERS_H
#define PARAMETERS_H
#include <AP_Common.h>
// Global parameter class.
//
class Parameters {
public:
// The version of the layout as described by the parameter enum.
//
// When changing the parameter enum in an incompatible fashion, this
// value should be incremented by one.
//
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 11;
// The parameter software_type is set up solely for ground station use
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
// values within that range to identify different branches.
//
static const uint16_t k_software_type = 0; // 0 for APM trunk
//
// Parameter identities.
//
// The enumeration defined here is used to ensure that every parameter
// or parameter group has a unique ID number. This number is used by
// AP_Var to store and locate parameters in EEPROM.
//
// Note that entries without a number are assigned the next number after
// the entry preceding them. When adding new entries, ensure that they
// don't overlap.
//
// Try to group related variables together, and assign them a set
// range in the enumeration. Place these groups in numerical order
// at the end of the enumeration.
//
// WARNING: Care should be taken when editing this enumeration as the
// AP_Var load/save code depends on the values here to identify
// variables saved in EEPROM.
//
//
enum {
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type,
// Misc
//
k_param_auto_trim,
k_param_switch_enable,
k_param_log_bitmask,
k_param_pitch_trim,
k_param_mix_mode,
k_param_reverse_elevons,
k_param_reverse_ch1_elevon,
k_param_reverse_ch2_elevon,
k_param_flap_1_percent,
k_param_flap_1_speed,
k_param_flap_2_percent,
k_param_flap_2_speed,
k_param_num_resets,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
k_param_streamrates_port3,
k_param_sysid_this_mav,
k_param_sysid_my_gcs,
k_param_serial3_baud,
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
//
// 130: Sensor parameters
//
k_param_IMU_calibration = 130,
k_param_altitude_mix,
k_param_airspeed_ratio,
k_param_ground_temperature,
k_param_ground_pressure,
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_pack_capacity,
k_param_airspeed_offset,
k_param_sonar_enabled,
k_param_airspeed_enabled,
//
// 150: Navigation parameters
//
k_param_crosstrack_gain = 150,
k_param_crosstrack_entry_angle,
k_param_roll_limit,
k_param_pitch_limit_max,
k_param_pitch_limit_min,
k_param_airspeed_cruise,
k_param_RTL_altitude,
k_param_inverted_flight_ch,
//
// 170: Radio settings
//
k_param_channel_roll = 170,
k_param_channel_pitch,
k_param_channel_throttle,
k_param_channel_yaw,
k_param_rc_5,
k_param_rc_6,
k_param_rc_7,
k_param_rc_8,
k_param_throttle_min,
k_param_throttle_max,
k_param_throttle_fs_enabled,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_short_fs_action,
k_param_long_fs_action,
k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate,
k_param_rc_5_funct,
k_param_rc_6_funct,
k_param_rc_7_funct,
k_param_rc_8_funct,
//
// 200: Feed-forward gains
//
k_param_kff_pitch_compensation = 200,
k_param_kff_rudder_mix,
k_param_kff_pitch_to_throttle,
k_param_kff_throttle_to_pitch,
//
// 210: flight modes
//
k_param_flight_mode_channel = 210,
k_param_flight_mode1,
k_param_flight_mode2,
k_param_flight_mode3,
k_param_flight_mode4,
k_param_flight_mode5,
k_param_flight_mode6,
//
// 220: Waypoint data
//
k_param_waypoint_mode = 220,
k_param_waypoint_total,
k_param_waypoint_index,
k_param_waypoint_radius,
k_param_loiter_radius,
//
// 240: PID Controllers
//
// Heading-to-roll PID:
// heading error from command to roll command deviation from trim
// (bank to turn strategy)
//
k_param_heading_to_roll_PID = 240,
// Roll-to-servo PID:
// roll error from command to roll servo deviation from trim
// (tracks commanded bank angle)
//
k_param_roll_to_servo_PID,
//
// Pitch control
//
// Pitch-to-servo PID:
// pitch error from command to pitch servo deviation from trim
// (front-side strategy)
//
k_param_pitch_to_servo_PID,
// Airspeed-to-pitch PID:
// airspeed error from command to pitch servo deviation from trim
// (back-side strategy)
//
k_param_airspeed_to_pitch_PID,
//
// Yaw control
//
// Yaw-to-servo PID:
// yaw rate error from command to yaw servo deviation from trim
// (stabilizes dutch roll)
//
k_param_yaw_to_servo_PID,
//
// Throttle control
//
// Energy-to-throttle PID:
// total energy error from command to throttle servo deviation from trim
// (throttle back-side strategy alternative)
//
k_param_energy_to_throttle_PID,
// Altitude-to-pitch PID:
// altitude error from command to pitch servo deviation from trim
// (throttle front-side strategy alternative)
//
k_param_altitude_to_pitch_PID,
// 254,255: reserved
};
AP_Int16 format_version;
AP_Int8 software_type;
// Telemetry control
//
AP_Int16 sysid_this_mav;
AP_Int16 sysid_my_gcs;
AP_Int8 serial3_baud;
// Feed-forward gains
//
AP_Float kff_pitch_compensation;
AP_Float kff_rudder_mix;
AP_Float kff_pitch_to_throttle;
AP_Float kff_throttle_to_pitch;
// Crosstrack navigation
//
AP_Float crosstrack_gain;
AP_Int16 crosstrack_entry_angle;
// Estimation
//
AP_Float altitude_mix;
AP_Float airspeed_ratio;
AP_Int16 airspeed_offset;
// Waypoints
//
AP_Int8 waypoint_mode;
AP_Int8 waypoint_total;
AP_Int8 waypoint_index;
AP_Int8 waypoint_radius;
AP_Int8 loiter_radius;
// Fly-by-wire
//
AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
// Failsafe
AP_Int8 short_fs_action;
AP_Int8 long_fs_action;
AP_Int8 gcs_heartbeat_fs_enabled;
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
// Navigational maneuvering limits
//
AP_Int16 roll_limit;
AP_Int16 pitch_limit_max;
AP_Int16 pitch_limit_min;
// Misc
//
AP_Int8 auto_trim;
AP_Int8 switch_enable;
AP_Int8 mix_mode;
AP_Int8 reverse_elevons;
AP_Int8 reverse_ch1_elevon;
AP_Int8 reverse_ch2_elevon;
AP_Int16 num_resets;
AP_Int16 log_bitmask;
AP_Int16 airspeed_cruise;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 sonar_enabled;
AP_Int8 airspeed_enabled;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
// RC channels
RC_Channel channel_roll;
RC_Channel channel_pitch;
RC_Channel channel_throttle;
RC_Channel channel_rudder;
RC_Channel rc_5;
RC_Channel rc_6;
RC_Channel rc_7;
RC_Channel rc_8;
AP_Int8 rc_5_funct;
AP_Int8 rc_6_funct;
AP_Int8 rc_7_funct;
AP_Int8 rc_8_funct;
// PID controllers
//
PID pidNavRoll;
PID pidServoRoll;
PID pidServoPitch;
PID pidNavPitchAirspeed;
PID pidServoRudder;
PID pidTeThrottle;
PID pidNavPitchAltitude;
uint8_t junk;
// Note: keep initializers here in the same order as they are declared above.
Parameters() :
// variable default key name
//-------------------------------------------------------------------------------------------------------
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
serial3_baud (SERIAL3_BAUD/1000, k_param_serial3_baud, PSTR("SERIAL3_BAUD")),
kff_pitch_compensation (PITCH_COMP, k_param_kff_pitch_compensation, PSTR("KFF_PTCHCOMP")),
kff_rudder_mix (RUDDER_MIX, k_param_kff_rudder_mix, PSTR("KFF_RDDRMIX")),
kff_pitch_to_throttle (P_TO_T, k_param_kff_pitch_to_throttle, PSTR("KFF_PTCH2THR")),
kff_throttle_to_pitch (T_TO_P, k_param_kff_throttle_to_pitch, PSTR("KFF_THR2PTCH")),
crosstrack_gain (XTRACK_GAIN_SCALED, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE_CENTIDEGREE,k_param_crosstrack_entry_angle, PSTR("XTRK_ANGLE_CD")),
altitude_mix (ALTITUDE_MIX, k_param_altitude_mix, PSTR("ALT_MIX")),
airspeed_ratio (AIRSPEED_RATIO, k_param_airspeed_ratio, PSTR("ARSPD_RATIO")),
airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")),
/* XXX waypoint_mode missing here */
waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")),
waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
flybywire_airspeed_min (AIRSPEED_FBW_MIN, k_param_flybywire_airspeed_min, PSTR("ARSPD_FBW_MIN")),
flybywire_airspeed_max (AIRSPEED_FBW_MAX, k_param_flybywire_airspeed_max, PSTR("ARSPD_FBW_MAX")),
throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")),
throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")),
throttle_slewrate (THROTTLE_SLEW_LIMIT, k_param_throttle_slewrate, PSTR("THR_SLEWRATE")),
throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")),
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
short_fs_action (SHORT_FAILSAFE_ACTION, k_param_short_fs_action, PSTR("FS_SHORT_ACTN")),
long_fs_action (LONG_FAILSAFE_ACTION, k_param_long_fs_action, PSTR("FS_LONG_ACTN")),
gcs_heartbeat_fs_enabled(GCS_HEARTBEAT_FAILSAFE, k_param_gcs_heartbeat_fs_enabled, PSTR("FS_GCS_ENABL")),
flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLTMODE_CH")),
flight_mode1 (FLIGHT_MODE_1, k_param_flight_mode1, PSTR("FLTMODE1")),
flight_mode2 (FLIGHT_MODE_2, k_param_flight_mode2, PSTR("FLTMODE2")),
flight_mode3 (FLIGHT_MODE_3, k_param_flight_mode3, PSTR("FLTMODE3")),
flight_mode4 (FLIGHT_MODE_4, k_param_flight_mode4, PSTR("FLTMODE4")),
flight_mode5 (FLIGHT_MODE_5, k_param_flight_mode5, PSTR("FLTMODE5")),
flight_mode6 (FLIGHT_MODE_6, k_param_flight_mode6, PSTR("FLTMODE6")),
roll_limit (HEAD_MAX_CENTIDEGREE, k_param_roll_limit, PSTR("LIM_ROLL_CD")),
pitch_limit_max (PITCH_MAX_CENTIDEGREE, k_param_pitch_limit_max, PSTR("LIM_PITCH_MAX")),
pitch_limit_min (PITCH_MIN_CENTIDEGREE, k_param_pitch_limit_min, PSTR("LIM_PITCH_MIN")),
auto_trim (AUTO_TRIM, k_param_auto_trim, PSTR("TRIM_AUTO")),
switch_enable (REVERSE_SWITCH, k_param_switch_enable, PSTR("SWITCH_ENABLE")),
mix_mode (ELEVON_MIXING, k_param_mix_mode, PSTR("ELEVON_MIXING")),
reverse_elevons (ELEVON_REVERSE, k_param_reverse_elevons, PSTR("ELEVON_REVERSE")),
reverse_ch1_elevon (ELEVON_CH1_REVERSE, k_param_reverse_ch1_elevon, PSTR("ELEVON_CH1_REV")),
reverse_ch2_elevon (ELEVON_CH2_REVERSE, k_param_reverse_ch2_elevon, PSTR("ELEVON_CH2_REV")),
num_resets (0, k_param_num_resets, PSTR("SYS_NUM_RESETS")),
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),
flap_1_percent (FLAP_1_PERCENT, k_param_flap_1_percent, PSTR("FLAP_1_PERCNT")),
flap_1_speed (FLAP_1_SPEED, k_param_flap_1_speed, PSTR("FLAP_1_SPEED")),
flap_2_percent (FLAP_2_PERCENT, k_param_flap_2_percent, PSTR("FLAP_2_PERCNT")),
flap_2_speed (FLAP_2_SPEED, k_param_flap_2_speed, PSTR("FLAP_2_SPEED")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
inverted_flight_ch (0, k_param_inverted_flight_ch, PSTR("INVERTEDFLT_CH")),
sonar_enabled (SONAR_ENABLED, k_param_sonar_enabled, PSTR("SONAR_ENABLE")),
airspeed_enabled (AIRSPEED_SENSOR, k_param_airspeed_enabled, PSTR("ARSPD_ENABLE")),
rc_5_funct (RC_5_FUNCT, k_param_rc_5_funct, PSTR("RC5_FUNCT")),
rc_6_funct (RC_6_FUNCT, k_param_rc_6_funct, PSTR("RC6_FUNCT")),
rc_7_funct (RC_7_FUNCT, k_param_rc_7_funct, PSTR("RC7_FUNCT")),
rc_8_funct (RC_8_FUNCT, k_param_rc_8_funct, PSTR("RC8_FUNCT")),
// Note - total parameter name length must be less than 14 characters for MAVLink compatibility!
// RC channel group key name
//----------------------------------------------------------------------
channel_roll (k_param_channel_roll, PSTR("RC1_")),
channel_pitch (k_param_channel_pitch, PSTR("RC2_")),
channel_throttle (k_param_channel_throttle, PSTR("RC3_")),
channel_rudder (k_param_channel_yaw, PSTR("RC4_")),
rc_5 (k_param_rc_5, PSTR("RC5_")),
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
// PID controller group key name initial P initial I initial D initial imax
//---------------------------------------------------------------------------------------------------------------------------------------
pidNavRoll (k_param_heading_to_roll_PID, PSTR("HDNG2RLL_"), NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
pidServoRoll (k_param_roll_to_servo_PID, PSTR("RLL2SRV_"), SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
pidServoPitch (k_param_pitch_to_servo_PID, PSTR("PTCH2SRV_"), SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
pidNavPitchAirspeed (k_param_airspeed_to_pitch_PID, PSTR("ARSP2PTCH_"), NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
pidServoRudder (k_param_yaw_to_servo_PID, PSTR("YW2SRV_"), SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
pidTeThrottle (k_param_energy_to_throttle_PID, PSTR("ENRGY2THR_"), THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
pidNavPitchAltitude (k_param_altitude_to_pitch_PID, PSTR("ALT2PTCH_"), NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM),
junk(0) // XXX just so that we can add things without worrying about the trailing comma
{
}
};
#endif // PARAMETERS_H

94
ArduPlane/climb_rate.pde Normal file
View File

@ -0,0 +1,94 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
struct DataPoint {
unsigned long x;
long y;
};
DataPoint history[ALTITUDE_HISTORY_LENGTH]; // Collection of (x,y) points to regress a rate of change from
unsigned char hindex; // Index in history for the current data point
unsigned long xoffset;
unsigned char n;
// Intermediate variables for regression calculation
long xi;
long yi;
long xiyi;
unsigned long xi2;
#if 0 // currently unused
static void add_altitude_data(unsigned long xl, long y)
{
//Reset the regression if our X variable overflowed
if (xl < xoffset)
n = 0;
//To allow calculation of sum(xi*yi), make sure X hasn't exceeded 2^32/2^15/length
if (xl - xoffset > 131072/ALTITUDE_HISTORY_LENGTH)
n = 0;
if (n == ALTITUDE_HISTORY_LENGTH) {
xi -= history[hindex].x;
yi -= history[hindex].y;
xiyi -= (long)history[hindex].x * history[hindex].y;
xi2 -= history[hindex].x * history[hindex].x;
} else {
if (n == 0) {
xoffset = xl;
xi = 0;
yi = 0;
xiyi = 0;
xi2 = 0;
}
n++;
}
history[hindex].x = xl - xoffset;
history[hindex].y = y;
xi += history[hindex].x;
yi += history[hindex].y;
xiyi += (long)history[hindex].x * history[hindex].y;
xi2 += history[hindex].x * history[hindex].x;
if (++hindex >= ALTITUDE_HISTORY_LENGTH)
hindex = 0;
}
#endif
#if 0 // unused
static void recalc_climb_rate()
{
float slope = ((float)xi*(float)yi - ALTITUDE_HISTORY_LENGTH*(float)xiyi) / ((float)xi*(float)xi - ALTITUDE_HISTORY_LENGTH*(float)xi2);
climb_rate = (int)(slope*100);
}
static void print_climb_debug_info()
{
unsigned char i, j;
recalc_climb_rate();
//SendDebugln_P("Climb rate:");
for (i=0; i<ALTITUDE_HISTORY_LENGTH; i++) {
j = i + hindex;
if (j >= ALTITUDE_HISTORY_LENGTH) j -= ALTITUDE_HISTORY_LENGTH;
//SendDebug_P(" ");
//SendDebug(j,DEC);
//SendDebug_P(": ");
//SendDebug(history[j].x,DEC);
//SendDebug_P(", ");
//SendDebugln(history[j].y,DEC);
}
//SendDebug_P(" sum(xi) = ");
//SendDebugln(xi,DEC);
//SendDebug_P(" sum(yi) = ");
//SendDebugln(yi,DEC);
//SendDebug_P(" sum(xi*yi) = ");
//SendDebugln(xiyi,DEC);
//SendDebug_P(" sum(xi^2) = ");
//SendDebugln(xi2,DEC);
//SendDebug_P(" Climb rate = ");
//SendDebug((float)climb_rate/100,2);
//SendDebugln_P(" m/s");
}
#endif

85
ArduPlane/command description.txt Normal file
View File

@ -0,0 +1,85 @@
ArduPilotMega 2.0 Commands
Command Structure in bytes
0 0x00 byte Command ID
1 0x01 byte Parameter 1
2 0x02 long Parameter 2
3 0x03 ..
4 0x04 ..
5 0x05 ..
6 0x06 long Parameter 3
7 0x07 ..
8 0x08 ..
9 0x09 ..
10 0x0A long Parameter 4
11 0x0B ..
11 0x0C ..
11 0x0D ..
***********************************
Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon
0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon
0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon
0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon
0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
***********************************
May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT - alt (finish) rate (cm/sec) -
Note: rate must be > 10 cm/sec due to integer math
MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
0x72 / 114 MAV_CMD_CONDITION_DISTANCE - - distance (meters) -
0x71 / 115 MAV_CMD_CONDITION_YAW angle speed direction (-1,1) rel (1), abs (0)
***********************************
Unexecuted commands > MAV_CMD_NAV_LAST are dropped when ready for the next command < MAV_CMD_NAV_LAST so plan/queue commands accordingly!
For example if you had a string of CMD_MAV_CONDITION commands following a 0x10 command that had not finished when the waypoint was
reached, the unexecuted CMD_MAV_CONDITION and CMD_MAV_DO commands would be skipped and the next command < MAV_CMD_NAV_LAST would be loaded
***********************************
Now Commands - these commands are executed once until no more new now commands are available
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0xB1 / 177 MAV_CMD_DO_JUMP index - repeat count -
Note: The repeat count must be greater than 1 for the command to execute. Use a repeat count of 1 if you intend a single use.
0XB2 / 178 MAV_CMD_DO_CHANGE_SPEED Speed type Speed (m/s) Throttle (Percent) -
(0=Airspeed, 1=Ground Speed) (-1 indicates no change)(-1 indicates no change)
0xB3 / 179 MAV_CMD_DO_SET_HOME Use current altitude lat lon
(1=use current location, 0=use specified location)
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER Param number Param value (NOT CURRENTLY IMPLEMENTED IN APM)
0xB5 / 181 MAV_CMD_DO_SET_RELAY Relay number On/off (1/0) - -
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY Relay number Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
0xB7 / 183 MAV_CMD_DO_SET_SERVO Servo number (5-8) On/off (1/0) - -
0xB6 / 184 MAV_CMD_DO_REPEAT_SERVO Servo number (5-8) Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event

258
ArduPlane/commands.pde Normal file
View File

@ -0,0 +1,258 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands()
{
//read_EEPROM_waypoint_info();
g.waypoint_index.set_and_save(0);
command_must_index = 0;
command_may_index = 0;
next_command.id = CMD_BLANK;
}
static void update_auto()
{
if (g.waypoint_index >= g.waypoint_total) {
handle_no_commands();
if(g.waypoint_total == 0) {
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
}
}
}
// this is only used by an air-start
static void reload_commands_airstart()
{
init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_WP_index();
}
// Getters
// -------
static struct Location get_wp_with_index(int i)
{
struct Location temp;
long mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.options = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem);
mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
}
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask
// XXX What is this trying to do?
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){
temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else {
temp.options = 0;
}
eeprom_write_byte((uint8_t *) mem, temp.id);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.options);
mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1);
mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat);
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng);
}
static void increment_WP_index()
{
if (g.waypoint_index <= g.waypoint_total) {
g.waypoint_index.set_and_save(g.waypoint_index + 1);
SendDebug_P("MSG - WP index is incremented to ");
}else{
//SendDebug_P("MSG <increment_WP_index> Failed to increment WP index of ");
// This message is used excessively at the end of a mission
}
//SendDebugln(g.waypoint_index,DEC);
}
static void decrement_WP_index()
{
if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1);
}
}
static long read_alt_to_hold()
{
if(g.RTL_altitude < 0)
return current_loc.alt;
else
return g.RTL_altitude + home.alt;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(struct Location *wp)
{
//gcs.send_text_P(SEVERITY_LOW,PSTR("load WP"));
SendDebug_P("MSG - wp_index: ");
SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP = next_WP;
// Load the next_WP slot
// ---------------------
next_WP = *wp;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
offset_altitude = 0;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
gcs.print_current_waypoints();
}
static void set_guided_WP(void)
{
SendDebug_P("MSG - set_guided_wp");
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = guided_WP;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - prev_WP.alt;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
reset_crosstrack();
}
// run this at setup on the ground
// -------------------------------
void init_home()
{
SendDebugln("MSG: init home");
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
}
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude, 0);
home_is_set = true;
Serial.printf_P(PSTR("gps alt: %ld"), home.alt);
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);
// Save prev loc
// -------------
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
guided_WP = home;
guided_WP.alt += g.RTL_altitude;
}

536
ArduPlane/commands_logic.pde Normal file
View File

@ -0,0 +1,536 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_must()
{
// reset navigation integrators
// -------------------------
reset_I();
switch(next_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
do_land();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
do_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
default:
break;
}
}
static void
handle_process_may()
{
switch(next_command.id){
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
/* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also)
gcs.send_text_P(SEVERITY_LOW,PSTR("Landing options set"));
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
landing_pitch = next_command.lng * 100;
g.airspeed_cruise = next_command.alt * 100;
g.throttle_cruise = next_command.lat;
landing_distance = next_command.p1;
//landing_roll = command.lng;
SendDebug_P("MSG: throttle_cruise = ");
SendDebugln(g.throttle_cruise,DEC);
break;
*/
default:
break;
}
}
static void handle_process_now()
{
switch(next_command.id){
case MAV_CMD_DO_JUMP:
do_jump();
break;
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
break;
case MAV_CMD_DO_SET_SERVO:
do_set_servo();
break;
case MAV_CMD_DO_SET_RELAY:
do_set_relay();
break;
case MAV_CMD_DO_REPEAT_SERVO:
do_repeat_servo();
break;
case MAV_CMD_DO_REPEAT_RELAY:
do_repeat_relay();
break;
}
}
static void handle_no_commands()
{
next_command = home;
next_command.alt = read_alt_to_hold();
next_command.id = MAV_CMD_NAV_LOITER_UNLIM;
}
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
static bool verify_must()
{
switch(command_must_ID) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT:
return verify_nav_wp();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
break;
case MAV_CMD_NAV_LOITER_TURNS:
return verify_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
break;
default:
gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd"));
return false;
break;
}
}
static bool verify_may()
{
switch(command_may_ID) {
case NO_COMMAND:
break;
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
break;
case MAV_CMD_CONDITION_DISTANCE:
return verify_within_distance();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
default:
gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd"));
break;
}
return false;
}
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
static void do_RTL(void)
{
control_mode = RTL;
crash_timer = 0;
next_WP = home;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
// output control mode to the ground station
gcs.send_message(MSG_HEARTBEAT);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
static void do_takeoff()
{
set_next_WP(&next_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch = (int)next_command.p1 * 100;
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
takeoff_altitude = next_command.alt;
//Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude);
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
}
static void do_nav_wp()
{
set_next_WP(&next_command);
}
static void do_land()
{
set_next_WP(&next_command);
}
static void do_loiter_unlimited()
{
set_next_WP(&next_command);
}
static void do_loiter_turns()
{
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360;
}
static void do_loiter_time()
{
set_next_WP(&next_command);
loiter_time = millis();
loiter_time_max = next_command.p1; // units are (seconds * 10)
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
static bool verify_takeoff()
{
if (g_gps->ground_speed > 300){
if(hold_course == -1){
// save our current course to take off
if(g.compass_enabled) {
hold_course = dcm.yaw_sensor;
} else {
hold_course = g_gps->ground_course;
}
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
if (current_loc.alt > takeoff_altitude) {
hold_course = -1;
takeoff_complete = true;
return true;
} else {
return false;
}
}
static bool verify_land()
{
// we don't verify landing - we never go to a new Must command after Land
if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100)))
|| (current_loc.alt <= next_WP.alt + 300)){
land_complete = 1; //Set land_complete if we are within 2 seconds distance or within 3 meters altitude
if(hold_course == -1){
// save our current course to land
//hold_course = yaw_sensor;
// save the course line of the runway to land
hold_course = crosstrack_bearing;
}
}
if(hold_course > -1){
// recalc bearing error with hold_course;
nav_bearing = hold_course;
// recalc bearing error
calc_bearing_error();
}
update_crosstrack();
return false;
}
static bool verify_nav_wp()
{
hold_course = -1;
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug_P("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[30];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
return true;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
gcs.send_text_P(SEVERITY_MEDIUM,PSTR("<verify_must: MAV_CMD_NAV_WAYPOINT> Missed WP"));
return true;
}
return false;
}
static bool verify_loiter_unlim()
{
update_loiter();
calc_bearing_error();
return false;
}
static bool verify_loiter_time()
{
update_loiter();
calc_bearing_error();
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
return true;
}
return false;
}
static bool verify_loiter_turns()
{
update_loiter();
calc_bearing_error();
if(loiter_sum > loiter_total) {
loiter_total = 0;
gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
// clear the command queue;
return true;
}
return false;
}
static bool verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home"));
return true;
}else{
return false;
}
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
{
condition_start = millis();
condition_value = next_command.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
{
condition_rate = next_command.lat;
condition_value = next_command.alt;
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
offset_altitude = 0; // For future nav calculations
}
static void do_within_distance()
{
condition_value = next_command.lat;
}
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
static bool verify_wait_delay()
{
if ((unsigned)(millis() - condition_start) > condition_value){
condition_value = 0;
return true;
}
return false;
}
static bool verify_change_alt()
{
if( (condition_rate>=0 && current_loc.alt >= condition_value) || (condition_rate<=0 && current_loc.alt <= condition_value)) {
condition_value = 0;
return true;
}
target_altitude += condition_rate / 10;
return false;
}
static bool verify_within_distance()
{
if (wp_distance < condition_value){
condition_value = 0;
return true;
}
return false;
}
/********************************************************************************/
// Do (Now) commands
/********************************************************************************/
static void do_loiter_at_location()
{
next_WP = current_loc;
}
static void do_jump()
{
struct Location temp;
if(next_command.lat > 0) {
command_must_index = 0;
command_may_index = 0;
temp = get_wp_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1);
} else if (next_command.lat == -1) {
g.waypoint_index.set_and_save(next_command.p1 - 1);
}
}
static void do_change_speed()
{
// Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_command.alt > 0)
g.airspeed_cruise.set_and_save(next_command.alt * 100);
if(next_command.lat > 0)
g.throttle_cruise.set_and_save(next_command.lat);
}
static void do_set_home()
{
if(next_command.p1 == 1 && GPS_enabled) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = next_command.lng; // Lon * 10**7
home.lat = next_command.lat; // Lat * 10**7
home.alt = max(next_command.alt, 0);
home_is_set = true;
}
}
static void do_set_servo()
{
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
}
static void do_set_relay()
{
if (next_command.p1 == 1) {
relay_on();
} else if (next_command.p1 == 0) {
relay_off();
}else{
relay_toggle();
}
}
static void do_repeat_servo()
{
event_id = next_command.p1 - 1;
if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) {
event_timer = 0;
event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.lat * 2;
event_value = next_command.alt;
switch(next_command.p1) {
case CH_5:
event_undo_value = g.rc_5.radio_trim;
break;
case CH_6:
event_undo_value = g.rc_6.radio_trim;
break;
case CH_7:
event_undo_value = g.rc_7.radio_trim;
break;
case CH_8:
event_undo_value = g.rc_8.radio_trim;
break;
}
update_events();
}
}
static void do_repeat_relay()
{
event_id = RELAY_TOGGLE;
event_timer = 0;
event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
event_repeat = next_command.alt * 2;
update_events();
}

159
ArduPlane/commands_process.pde Normal file
View File

@ -0,0 +1,159 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
static void change_command(uint8_t cmd_index)
{
struct Location temp = get_wp_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ){
gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else {
command_must_index = NO_COMMAND;
next_command.id = NO_COMMAND;
g.waypoint_index.set_and_save(cmd_index - 1);
load_next_command_from_EEPROM();
process_next_command();
}
}
// called by 10 Hz loop
// --------------------
static void update_commands(void)
{
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if(home_is_set == false){
return; // don't do commands
}
if(control_mode == AUTO){
load_next_command_from_EEPROM();
process_next_command();
} // Other (eg GCS_Auto) modes may be implemented here
}
static void verify_commands(void)
{
if(verify_must()){
command_must_index = NO_COMMAND;
}
if(verify_may()){
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
}
}
static void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
}
// If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == NO_COMMAND){
// we are out of commands!
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
}
}
static void process_next_command()
{
// these are Navigation/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
increment_WP_index();
//save_command_index(); // TO DO - fix - to Recover from in air Restart
command_must_index = g.waypoint_index;
//SendDebug_P("MSG <process_next_command> new command_must_id ");
//SendDebug(next_command.id,DEC);
//SendDebug_P(" index:");
//SendDebugln(command_must_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_must();
}
}
// these are Condition/May commands
// ----------------------
if (command_may_index == 0){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
increment_WP_index(); // this command is from the command list in EEPROM
command_may_index = g.waypoint_index;
//SendDebug_P("MSG <process_next_command> new command_may_id ");
//SendDebug(next_command.id,DEC);
//Serial.printf_P(PSTR("new command_may_index "));
//Serial.println(command_may_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
}
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM
//SendDebug_P("MSG <process_next_command> new command_now_id ");
//SendDebug(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
}
}
}
/**************************************************/
// These functions implement the commands.
/**************************************************/
static void process_must()
{
gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// clear May indexes
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
command_must_ID = next_command.id;
handle_process_must();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
}
static void process_may()
{
gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
command_may_ID = next_command.id;
handle_process_may();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
}
static void process_now()
{
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
gcs.send_text(SEVERITY_LOW, "<process_now>");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
}

740
ArduPlane/config.h Normal file
View File

@ -0,0 +1,740 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
//
// DO NOT EDIT this file to adjust your configuration. Create your own
// APM_Config.h and use APM_Config.h.example as a reference.
//
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
///
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
// Default and automatic configuration details.
//
// Notes for maintainers:
//
// - Try to keep this file organised in the same order as APM_Config.h.example
//
#include "defines.h"
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.h.
///
#include "APM_Config.h" // <== THIS INCLUDE, DO NOT EDIT IT. EVER.
///
/// DO NOT EDIT THIS INCLUDE - if you want to make a local change, make that
/// change in your local copy of APM_Config.h.
///
// Just so that it's completely clear...
#define ENABLED 1
#define DISABLED 0
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// HARDWARE CONFIGURATION AND CONNECTIONS
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_SENSOR
// AIRSPEED_RATIO
//
#ifndef AIRSPEED_SENSOR
# define AIRSPEED_SENSOR DISABLED
#endif
#ifndef AIRSPEED_RATIO
# define AIRSPEED_RATIO 1.9936 // Note - this varies from the value in ArduPilot due to the difference in ADC resolution
#endif
//////////////////////////////////////////////////////////////////////////////
// Sonar
//
#ifndef SONAR_ENABLED
# define SONAR_ENABLED DISABLED
#endif
#ifndef SONAR_PIN
# define SONAR_PIN AN4 // AN5, AP_RANGEFINDER_PITOT_TUBE
#endif
#ifndef SONAR_TYPE
# define SONAR_TYPE MAX_SONAR_LV // MAX_SONAR_XL,
#endif
//////////////////////////////////////////////////////////////////////////////
// HIL_PROTOCOL OPTIONAL
// HIL_MODE OPTIONAL
// HIL_PORT OPTIONAL
#ifndef HIL_MODE
#define HIL_MODE HIL_MODE_DISABLED
#endif
#ifndef HIL_PROTOCOL
#define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK
#endif
#ifndef HIL_PORT
#define HIL_PORT 0
#endif
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
# undef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_NONE
#endif
// If we are in XPlane, diasble the mag
#if HIL_MODE != HIL_MODE_DISABLED // we are in HIL mode
// check xplane settings
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE
// MAGNETOMETER not supported by XPLANE
# undef MAGNETOMETER
# define MAGNETOMETER DISABLED
# if HIL_MODE != HIL_MODE_ATTITUDE
# error HIL_PROTOCOL_XPLANE requires HIL_MODE_ATTITUDE
# endif
#endif
#endif
//////////////////////////////////////////////////////////////////////////////
// GPS_PROTOCOL
//
// Note that this test must follow the HIL_PROTOCOL block as the HIL
// setup may override the GPS configuration.
//
#ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif
//////////////////////////////////////////////////////////////////////////////
// GCS_PROTOCOL
// GCS_PORT
//
#ifndef GCS_PROTOCOL
# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK
#endif
#ifndef GCS_PORT
# define GCS_PORT 3
#endif
#ifndef MAV_SYSTEM_ID
# define MAV_SYSTEM_ID 1
#endif
//////////////////////////////////////////////////////////////////////////////
// Serial port speeds.
//
#ifndef SERIAL0_BAUD
# define SERIAL0_BAUD 115200
#endif
#ifndef SERIAL3_BAUD
# define SERIAL3_BAUD 57600
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56
#endif
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32
#endif
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0
#endif
#ifndef HIGH_DISCHARGE
# define HIGH_DISCHARGE 1760
#endif
//////////////////////////////////////////////////////////////////////////////
// INPUT_VOLTAGE
//
#ifndef INPUT_VOLTAGE
# define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail
#endif
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER
# define MAGNETOMETER DISABLED
#endif
#ifndef MAG_ORIENTATION
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Radio channel limits
//
// Note that these are not called out in APM_Config.h.reference.
//
#ifndef CH5_MIN
# define CH5_MIN 1000
#endif
#ifndef CH5_MAX
# define CH5_MAX 2000
#endif
#ifndef CH6_MIN
# define CH6_MIN 1000
#endif
#ifndef CH6_MAX
# define CH6_MAX 2000
#endif
#ifndef CH7_MIN
# define CH7_MIN 1000
#endif
#ifndef CH7_MAX
# define CH7_MAX 2000
#endif
#ifndef CH8_MIN
# define CH8_MIN 1000
#endif
#ifndef CH8_MAX
# define CH8_MAX 2000
#endif
//////////////////////////////////////////////////////////////////////////////
#ifndef RC_5_FUNCT
# define RC_5_FUNCT RC_5_FUNCT_NONE
#endif
#ifndef RC_6_FUNCT
# define RC_6_FUNCT RC_6_FUNCT_NONE
#endif
#ifndef RC_7_FUNCT
# define RC_7_FUNCT RC_7_FUNCT_NONE
#endif
#ifndef RC_8_FUNCT
# define RC_8_FUNCT RC_8_FUNCT_NONE
#endif
#ifndef FLAP_1_PERCENT
# define FLAP_1_PERCENT 0
#endif
#ifndef FLAP_1_SPEED
# define FLAP_1_SPEED 255
#endif
#ifndef FLAP_2_PERCENT
# define FLAP_2_PERCENT 0
#endif
#ifndef FLAP_2_SPEED
# define FLAP_2_SPEED 255
#endif
//////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
//
#ifndef FLIGHT_MODE_CHANNEL
# define FLIGHT_MODE_CHANNEL 8
#endif
#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8)
# error XXX
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
# error XXX
#endif
#if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 RTL
#endif
#if !defined(FLIGHT_MODE_2)
# define FLIGHT_MODE_2 RTL
#endif
#if !defined(FLIGHT_MODE_3)
# define FLIGHT_MODE_3 STABILIZE
#endif
#if !defined(FLIGHT_MODE_4)
# define FLIGHT_MODE_4 STABILIZE
#endif
#if !defined(FLIGHT_MODE_5)
# define FLIGHT_MODE_5 MANUAL
#endif
#if !defined(FLIGHT_MODE_6)
# define FLIGHT_MODE_6 MANUAL
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_FAILSAFE
// THROTTLE_FS_VALUE
// SHORT_FAILSAFE_ACTION
// LONG_FAILSAFE_ACTION
// GCS_HEARTBEAT_FAILSAFE
//
#ifndef THROTTLE_FAILSAFE
# define THROTTLE_FAILSAFE ENABLED
#endif
#ifndef THROTTLE_FS_VALUE
# define THROTTLE_FS_VALUE 950
#endif
#ifndef SHORT_FAILSAFE_ACTION
# define SHORT_FAILSAFE_ACTION 0
#endif
#ifndef LONG_FAILSAFE_ACTION
# define LONG_FAILSAFE_ACTION 0
#endif
#ifndef GCS_HEARTBEAT_FAILSAFE
# define GCS_HEARTBEAT_FAILSAFE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// AUTO_TRIM
//
#ifndef AUTO_TRIM
# define AUTO_TRIM ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_REVERSE
//
#ifndef THROTTLE_REVERSE
# define THROTTLE_REVERSE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_STICK_MIXING
//
#ifndef ENABLE_STICK_MIXING
# define ENABLE_STICK_MIXING ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// THROTTLE_OUT
//
#ifndef THROTTE_OUT
# define THROTTLE_OUT ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// STARTUP BEHAVIOUR
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// GROUND_START_DELAY
//
#ifndef GROUND_START_DELAY
# define GROUND_START_DELAY 0
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE_AIR_START
//
#ifndef ENABLE_AIR_START
# define ENABLE_AIR_START DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE REVERSE_SWITCH
//
#ifndef REVERSE_SWITCH
# define REVERSE_SWITCH ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// ENABLE ELEVON_MIXING
//
#ifndef ELEVON_MIXING
# define ELEVON_MIXING DISABLED
#endif
#ifndef ELEVON_REVERSE
# define ELEVON_REVERSE DISABLED
#endif
#ifndef ELEVON_CH1_REVERSE
# define ELEVON_CH1_REVERSE DISABLED
#endif
#ifndef ELEVON_CH2_REVERSE
# define ELEVON_CH2_REVERSE DISABLED
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// FLIGHT AND NAVIGATION CONTROL
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Altitude measurement and control.
//
#ifndef ALT_EST_GAIN
# define ALT_EST_GAIN 0.01
#endif
#ifndef ALTITUDE_MIX
# define ALTITUDE_MIX 1
#endif
//////////////////////////////////////////////////////////////////////////////
// AIRSPEED_CRUISE
//
#ifndef AIRSPEED_CRUISE
# define AIRSPEED_CRUISE 12 // 12 m/s
#endif
#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control
//
#ifndef AIRSPEED_FBW_MIN
# define AIRSPEED_FBW_MIN 6
#endif
#ifndef AIRSPEED_FBW_MAX
# define AIRSPEED_FBW_MAX 22
#endif
/* The following parmaeters have no corresponding control implementation
#ifndef THROTTLE_ALT_P
# define THROTTLE_ALT_P 0.32
#endif
#ifndef THROTTLE_ALT_I
# define THROTTLE_ALT_I 0.0
#endif
#ifndef THROTTLE_ALT_D
# define THROTTLE_ALT_D 0.0
#endif
#ifndef THROTTLE_ALT_INT_MAX
# define THROTTLE_ALT_INT_MAX 20
#endif
#define THROTTLE_ALT_INT_MAX_CM THROTTLE_ALT_INT_MAX*100
*/
//////////////////////////////////////////////////////////////////////////////
// Servo Mapping
//
#ifndef THROTTLE_MIN
# define THROTTLE_MIN 0 // percent
#endif
#ifndef THROTTLE_CRUISE
# define THROTTLE_CRUISE 45
#endif
#ifndef THROTTLE_MAX
# define THROTTLE_MAX 75
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//
#ifndef HEAD_MAX
# define HEAD_MAX 45
#endif
#ifndef PITCH_MAX
# define PITCH_MAX 15
#endif
#ifndef PITCH_MIN
# define PITCH_MIN -25
#endif
#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100
//////////////////////////////////////////////////////////////////////////////
// Attitude control gains
//
#ifndef SERVO_ROLL_P
# define SERVO_ROLL_P 0.4
#endif
#ifndef SERVO_ROLL_I
# define SERVO_ROLL_I 0.0
#endif
#ifndef SERVO_ROLL_D
# define SERVO_ROLL_D 0.0
#endif
#ifndef SERVO_ROLL_INT_MAX
# define SERVO_ROLL_INT_MAX 5
#endif
#define SERVO_ROLL_INT_MAX_CENTIDEGREE SERVO_ROLL_INT_MAX*100
#ifndef ROLL_SLEW_LIMIT
# define ROLL_SLEW_LIMIT 0
#endif
#ifndef SERVO_PITCH_P
# define SERVO_PITCH_P 0.6
#endif
#ifndef SERVO_PITCH_I
# define SERVO_PITCH_I 0.0
#endif
#ifndef SERVO_PITCH_D
# define SERVO_PITCH_D 0.0
#endif
#ifndef SERVO_PITCH_INT_MAX
# define SERVO_PITCH_INT_MAX 5
#endif
#define SERVO_PITCH_INT_MAX_CENTIDEGREE SERVO_PITCH_INT_MAX*100
#ifndef PITCH_COMP
# define PITCH_COMP 0.2
#endif
#ifndef SERVO_YAW_P
# define SERVO_YAW_P 0.0
#endif
#ifndef SERVO_YAW_I
# define SERVO_YAW_I 0.0
#endif
#ifndef SERVO_YAW_D
# define SERVO_YAW_D 0.0
#endif
#ifndef SERVO_YAW_INT_MAX
# define SERVO_YAW_INT_MAX 0
#endif
#ifndef RUDDER_MIX
# define RUDDER_MIX 0.5
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
#ifndef NAV_ROLL_P
# define NAV_ROLL_P 0.7
#endif
#ifndef NAV_ROLL_I
# define NAV_ROLL_I 0.0
#endif
#ifndef NAV_ROLL_D
# define NAV_ROLL_D 0.02
#endif
#ifndef NAV_ROLL_INT_MAX
# define NAV_ROLL_INT_MAX 5
#endif
#define NAV_ROLL_INT_MAX_CENTIDEGREE NAV_ROLL_INT_MAX*100
#ifndef NAV_PITCH_ASP_P
# define NAV_PITCH_ASP_P 0.65
#endif
#ifndef NAV_PITCH_ASP_I
# define NAV_PITCH_ASP_I 0.0
#endif
#ifndef NAV_PITCH_ASP_D
# define NAV_PITCH_ASP_D 0.0
#endif
#ifndef NAV_PITCH_ASP_INT_MAX
# define NAV_PITCH_ASP_INT_MAX 5
#endif
#define NAV_PITCH_ASP_INT_MAX_CMSEC NAV_PITCH_ASP_INT_MAX*100
#ifndef NAV_PITCH_ALT_P
# define NAV_PITCH_ALT_P 0.65
#endif
#ifndef NAV_PITCH_ALT_I
# define NAV_PITCH_ALT_I 0.0
#endif
#ifndef NAV_PITCH_ALT_D
# define NAV_PITCH_ALT_D 0.0
#endif
#ifndef NAV_PITCH_ALT_INT_MAX
# define NAV_PITCH_ALT_INT_MAX 5
#endif
#define NAV_PITCH_ALT_INT_MAX_CM NAV_PITCH_ALT_INT_MAX*100
//////////////////////////////////////////////////////////////////////////////
// Energy/Altitude control gains
//
#ifndef THROTTLE_TE_P
# define THROTTLE_TE_P 0.50
#endif
#ifndef THROTTLE_TE_I
# define THROTTLE_TE_I 0.0
#endif
#ifndef THROTTLE_TE_D
# define THROTTLE_TE_D 0.0
#endif
#ifndef THROTTLE_TE_INT_MAX
# define THROTTLE_TE_INT_MAX 20
#endif
#ifndef THROTTLE_SLEW_LIMIT
# define THROTTLE_SLEW_LIMIT 0
#endif
#ifndef P_TO_T
# define P_TO_T 0
#endif
#ifndef T_TO_P
# define T_TO_P 0
#endif
#ifndef PITCH_TARGET
# define PITCH_TARGET 0
#endif
//////////////////////////////////////////////////////////////////////////////
// Crosstrack compensation
//
#ifndef XTRACK_GAIN
# define XTRACK_GAIN 1 // deg/m
#endif
#ifndef XTRACK_ENTRY_ANGLE
# define XTRACK_ENTRY_ANGLE 30 // deg
#endif
# define XTRACK_GAIN_SCALED XTRACK_GAIN*100
# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE*100
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// DEBUGGING
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Dataflash logging control
//
#ifndef LOGGING_ENABLED
# define LOGGING_ENABLED ENABLED
#endif
#ifndef LOG_ATTITUDE_FAST
# define LOG_ATTITUDE_FAST DISABLED
#endif
#ifndef LOG_ATTITUDE_MED
# define LOG_ATTITUDE_MED ENABLED
#endif
#ifndef LOG_GPS
# define LOG_GPS ENABLED
#endif
#ifndef LOG_PM
# define LOG_PM ENABLED
#endif
#ifndef LOG_CTUN
# define LOG_CTUN DISABLED
#endif
#ifndef LOG_NTUN
# define LOG_NTUN DISABLED
#endif
#ifndef LOG_MODE
# define LOG_MODE ENABLED
#endif
#ifndef LOG_RAW
# define LOG_RAW DISABLED
#endif
#ifndef LOG_CMD
# define LOG_CMD ENABLED
#endif
#ifndef LOG_CUR
# define LOG_CUR DISABLED
#endif
// calculate the default log_bitmask
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
#define DEFAULT_LOG_BITMASK \
LOGBIT(ATTITUDE_FAST) | \
LOGBIT(ATTITUDE_MED) | \
LOGBIT(GPS) | \
LOGBIT(PM) | \
LOGBIT(CTUN) | \
LOGBIT(NTUN) | \
LOGBIT(MODE) | \
LOGBIT(RAW) | \
LOGBIT(CMD) | \
LOGBIT(CUR)
#ifndef DEBUG_PORT
# define DEBUG_PORT 0
#endif
#if DEBUG_PORT == 0
# define SendDebug_P(a) Serial.print_P(PSTR(a))
# define SendDebugln_P(a) Serial.println_P(PSTR(a))
# define SendDebug Serial.print
# define SendDebugln Serial.println
#elif DEBUG_PORT == 1
# define SendDebug_P(a) Serial1.print_P(PSTR(a))
# define SendDebugln_P(a) Serial1.println_P(PSTR(a))
# define SendDebug Serial1.print
# define SendDebugln Serial1.println
#elif DEBUG_PORT == 2
# define SendDebug_P(a) Serial2.print_P(PSTR(a))
# define SendDebugln_P(a) Serial2.println_P(PSTR(a))
# define SendDebug Serial2.print
# define SendDebugln Serial2.println
#elif DEBUG_PORT == 3
# define SendDebug_P(a) Serial3.print_P(PSTR(a))
# define SendDebugln_P(a) Serial3.println_P(PSTR(a))
# define SendDebug Serial3.print
# define SendDebugln Serial3.println
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation defaults
//
#ifndef WP_RADIUS_DEFAULT
# define WP_RADIUS_DEFAULT 30
#endif
#ifndef LOITER_RADIUS_DEFAULT
# define LOITER_RADIUS_DEFAULT 60
#endif
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 100
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
#ifndef USE_CURRENT_ALT
# define USE_CURRENT_ALT FALSE
#endif
#ifndef INVERTED_FLIGHT_PWM
# define INVERTED_FLIGHT_PWM 1750
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items
//
#ifndef STANDARD_SPEED
# define STANDARD_SPEED 15.0
#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED)
#endif
#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE)
// use this to enable servos in HIL mode
#ifndef HIL_SERVOS
# define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED
#endif
// delay to prevent Xbee bricking, in milliseconds
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
#endif

62
ArduPlane/control_modes.pde Normal file
View File

@ -0,0 +1,62 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
set_mode(flight_modes[switchPosition]);
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
// reset navigation integrators
// -------------------------
reset_I();
}
if (g.inverted_flight_ch != 0) {
// if the user has configured an inverted flight channel, then
// fly upside down when that channel goes above INVERTED_FLIGHT_PWM
inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM);
}
}
static byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
if (pulsewidth > 1490 && pulsewidth <= 1620) return 3;
if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual
if (pulsewidth >= 1750) return 5; // Hardware Manual
return 0;
}
static void reset_control_switch()
{
oldSwitchPosition = 0;
read_control_switch();
SendDebug_P("MSG: reset_control_switch");
SendDebugln(oldSwitchPosition , DEC);
}
static void update_servo_switches()
{
if (!g.switch_enable) {
// switches are disabled in EEPROM (see SWITCH_ENABLE option)
// this means the EEPROM control of all channel reversal is enabled
return;
}
// up is reverse
// up is elevon
g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode
if (g.mix_mode == 0) {
g.channel_roll.set_reverse((PINE & 128) ? true : false);
g.channel_pitch.set_reverse((PINE & 64) ? true : false);
g.channel_rudder.set_reverse((PINL & 64) ? true : false);
} else {
g.reverse_elevons = (PINE & 128) ? true : false;
g.reverse_ch1_elevon = (PINE & 64) ? true : false;
g.reverse_ch2_elevon = (PINL & 64) ? true : false;
}
}

68
ArduPlane/createTags Executable file
View File

@ -0,0 +1,68 @@
#!/bin/bash
#" Autocompletion enabled vim for arduino pde's
ctags -RV --language-force=C++ --c++-kinds=+p --fields=+iaS --extra=+q \
. \
../libraries/* \
/usr/lib/avr/include
# sample vimrc file
# you'll need to have omnicpp plugin for vim
#"set compatible
#" Vim5 and later versions support syntax highlighting. Uncommenting the next
#" line enables syntax highlighting by default.
#syntax on
#filetype on
#au BufNewFile,BufRead *.pde set filetype=cpp
#" If using a dark background within the editing area and syntax highlighting
#" turn on this option as well
#"set background=dark
#" Uncomment the following to have Vim jump to the last position when
#" reopening a file
#if has("autocmd")
#au BufReadPost * if line("'\"") > 1 && line("'\"") <= line("$") | exe "normal! g'\"" | endif
#endif
#" Uncomment the following to have Vim load indentation rules and plugins
#" according to the detected filetype.
#if has("autocmd")
#filetype plugin indent on
#endif
#" The following are commented out as they cause vim to behave a lot
#" differently from regular Vi. They are highly recommended though.
#set showcmd " Show (partial) command in status line.
#set showmatch " Show matching brackets.
#set ignorecase " Do case insensitive matching
#set smartcase " Do smart case matching
#set incsearch " Incremental search
#set autowrite " Automatically save before commands like :next and :make
#set hidden " Hide buffers when they are abandoned
#set mouse=a " Enable mouse usage (all modes)
#set vb
#" build tags of your own project with Ctrl-F12
#map <C-F12> :!ctags -R --sort=yes --c++-kinds=+p --fields=+iaS --extra=+q .<CR>
#" OmniCppComplete
#let OmniCpp_NamespaceSearch = 1
#let OmniCpp_GlobalScopeSearch = 1
#let OmniCpp_ShowAccess = 1
#let OmniCpp_ShowPrototypeInAbbr = 1 " show function parameters
#let OmniCpp_MayCompleteDot = 1 " autocomplete after .
#let OmniCpp_MayCompleteArrow = 1 " autocomplete after ->
#let OmniCpp_MayCompleteScope = 1 " autocomplete after ::
#let OmniCpp_DefaultNamespaces = ["std", "_GLIBCXX_STD"]
#" arduino syntax
#au BufNewFile,BufRead *.pde setf arduino
#" automatically open and close the popup menu / preview window
#"au CursorMovedI,InsertLeave * if pumvisible() == 0|silent! pclose|endif
#"set completeopt=menuone,menu,longest,preview
#set ts=4
#set sw=4

294
ArduPlane/defines.h Normal file
View File

@ -0,0 +1,294 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Internal defines, don't edit and expect things to work
// -------------------------------------------------------
#define TRUE 1
#define FALSE 0
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
#define DEBUG 0
#define LOITER_RANGE 60 // for calculating power outside of loiter radius
#define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel.
// failsafe
// ----------------------
#define FAILSAFE_NONE 0
#define FAILSAFE_SHORT 1
#define FAILSAFE_LONG 2
#define FAILSAFE_GCS 3
#define FAILSAFE_SHORT_TIME 1500 // Miliiseconds
#define FAILSAFE_LONG_TIME 20000 // Miliiseconds
// active altitude sensor
// ----------------------
#define SONAR 0
#define BARO 1
#define T6 1000000
#define T7 10000000
// GPS type codes - use the names, not the numbers
#define GPS_PROTOCOL_NONE -1
#define GPS_PROTOCOL_NMEA 0
#define GPS_PROTOCOL_SIRF 1
#define GPS_PROTOCOL_UBLOX 2
#define GPS_PROTOCOL_IMU 3
#define GPS_PROTOCOL_MTK 4
#define GPS_PROTOCOL_HIL 5
#define GPS_PROTOCOL_MTK16 6
#define GPS_PROTOCOL_AUTO 7
// Radio channels
// Note channels are from 0!
//
// XXX these should be CH_n defines from RC.h at some point.
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define CH_ROLL CH_1
#define CH_PITCH CH_2
#define CH_THROTTLE CH_3
#define CH_RUDDER CH_4
#define CH_YAW CH_4
#define RC_5_FUNCT_NONE 0
#define RC_5_FUNCT_AILERON 1
#define RC_5_FUNCT_FLAP_AUTO 2
#define RC_5_FUNCT_FLAPERON 3
#define RC_6_FUNCT_NONE 0
#define RC_6_FUNCT_AILERON 1
#define RC_6_FUNCT_FLAP_AUTO 2
#define RC_6_FUNCT_FLAPERON 3
#define RC_7_FUNCT_NONE 0
#define RC_8_FUNCT_NONE 0
// HIL enumerations
#define HIL_PROTOCOL_XPLANE 1
#define HIL_PROTOCOL_MAVLINK 2
#define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2
// GCS enumeration
#define GCS_PROTOCOL_STANDARD 0 // standard APM protocol
#define GCS_PROTOCOL_LEGACY 1 // legacy ArduPilot protocol
#define GCS_PROTOCOL_XPLANE 2 // X-Plane HIL simulation
#define GCS_PROTOCOL_DEBUGTERMINAL 3 //Human-readable debug interface for use with a dumb terminal
#define GCS_PROTOCOL_MAVLINK 4 // binary protocol for qgroundcontrol
#define GCS_PROTOCOL_NONE -1 // No GCS output
// Auto Pilot modes
// ----------------
#define MANUAL 0
#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle
#define STABILIZE 2
#define FLY_BY_WIRE_A 5 // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
#define FLY_BY_WIRE_B 6 // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
#define FLY_BY_WIRE_C 7 // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
#define AUTO 10
#define RTL 11
#define LOITER 12
//#define TAKEOFF 13 // This is not used by APM. It appears here for consistency with ACM
//#define LAND 14 // This is not used by APM. It appears here for consistency with ACM
#define GUIDED 15
#define INITIALISING 16 // in startup routines
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
// Command/Waypoint/Location Options Bitmask
//--------------------
#define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative altitude
//repeating events
#define NO_REPEAT 0
#define CH_5_TOGGLE 1
#define CH_6_TOGGLE 2
#define CH_7_TOGGLE 3
#define CH_8_TOGGLE 4
#define RELAY_TOGGLE 5
#define STOP_REPEAT 10
#define MAV_CMD_CONDITION_YAW 23
// GCS Message ID's
/// NOTE: to ensure we never block on sending MAVLink messages
/// please keep each MSG_ to a single MAVLink message. If need be
/// create new MSG_ IDs for additional messages on the same
/// stream
#define MSG_ACKNOWLEDGE 0x00
#define MSG_HEARTBEAT 0x01
#define MSG_ATTITUDE 0x02
#define MSG_LOCATION 0x03
#define MSG_PRESSURE 0x04
#define MSG_STATUS_TEXT 0x05
#define MSG_PERF_REPORT 0x06
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS1 0x0a
#define MSG_EXTENDED_STATUS2 0x0b
#define MSG_CPU_LOAD 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
#define MSG_COMMAND_LIST 0x22
#define MSG_COMMAND_MODE_CHANGE 0x23
#define MSG_CURRENT_WAYPOINT 0x24
#define MSG_VALUE_REQUEST 0x30
#define MSG_VALUE_SET 0x31
#define MSG_VALUE 0x32
#define MSG_PID_REQUEST 0x40
#define MSG_PID_SET 0x41
#define MSG_PID 0x42
#define MSG_VFR_HUD 0x4a
#define MSG_TRIM_STARTUP 0x50
#define MSG_TRIM_MIN 0x51
#define MSG_TRIM_MAX 0x52
#define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU1 0x60
#define MSG_RAW_IMU2 0x61
#define MSG_RAW_IMU3 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70
#define MSG_PIN_REQUEST 0x80
#define MSG_PIN_SET 0x81
#define MSG_DATAFLASH_REQUEST 0x90
#define MSG_DATAFLASH_SET 0x91
#define MSG_EEPROM_REQUEST 0xa0
#define MSG_EEPROM_SET 0xa1
#define MSG_POSITION_CORRECT 0xb0
#define MSG_ATTITUDE_CORRECT 0xb1
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2
#define SEVERITY_HIGH 3
#define SEVERITY_CRITICAL 4
// Logging parameters
#define LOG_INDEX_MSG 0xF0
#define LOG_ATTITUDE_MSG 0x01
#define LOG_GPS_MSG 0x02
#define LOG_MODE_MSG 0X03
#define LOG_CONTROL_TUNING_MSG 0X04
#define LOG_NAV_TUNING_MSG 0X05
#define LOG_PERFORMANCE_MSG 0X06
#define LOG_RAW_MSG 0x07
#define LOG_CMD_MSG 0x08
#define LOG_CURRENT_MSG 0x09
#define LOG_STARTUP_MSG 0x0A
#define TYPE_AIRSTART_MSG 0x00
#define TYPE_GROUNDSTART_MSG 0x01
#define MAX_NUM_LOGS 50
#define MASK_LOG_ATTITUDE_FAST (1<<0)
#define MASK_LOG_ATTITUDE_MED (1<<1)
#define MASK_LOG_GPS (1<<2)
#define MASK_LOG_PM (1<<3)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
// Waypoint Modes
// ----------------
#define ABS_WP 0
#define REL_WP 1
// Command Queues
// ---------------
#define COMMAND_MUST 0
#define COMMAND_MAY 1
#define COMMAND_NOW 2
// Events
// ------
#define EVENT_WILL_REACH_WAYPOINT 1
#define EVENT_SET_NEW_WAYPOINT_INDEX 2
#define EVENT_LOADED_WAYPOINT 3
#define EVENT_LOOP 4
// Climb rate calculations
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
#define BATTERY_PIN2 1
#define BATTERY_PIN3 2
#define BATTERY_PIN4 3
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
#define CURRENT_PIN_1 1 // and current
#define RELAY_PIN 47
// sonar
#define MAX_SONAR_XL 0
#define MAX_SONAR_LV 1
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
#define AN4 4
#define AN5 5
// Hardware Parameters
#define SLIDE_SWITCH_PIN 40
#define PUSHBUTTON_PIN 41
#define A_LED_PIN 37 //36 = B, 37 = A, 35 = C
#define B_LED_PIN 36
#define C_LED_PIN 35
#define SPEEDFILT 400 // centimeters/second; the speed below which a groundstart will be triggered
// EEPROM addresses
#define EEPROM_MAX_ADDR 4096
// parameters get the first 1KiB of EEPROM, remainder is for waypoints
#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP
#define WP_SIZE 15
#define ONBOARD_PARAM_NAME_LENGTH 15
#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe
// convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1)
#define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1)

127
ArduPlane/events.pde Normal file
View File

@ -0,0 +1,127 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void failsafe_short_on_event()
{
// This is how to handle a short loss of control signal failsafe.
failsafe = FAILSAFE_SHORT;
ch3_failsafe_timer = millis();
SendDebug_P("Failsafe - Short event on");
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
set_mode(CIRCLE);
break;
case AUTO:
case LOITER:
if(g.short_fs_action == 1) {
set_mode(RTL);
}
break;
case CIRCLE:
case RTL:
default:
break;
}
SendDebug_P("flight mode = ");
SendDebugln(control_mode, DEC);
}
static void failsafe_long_on_event()
{
// This is how to handle a long loss of control signal failsafe.
SendDebug_P("Failsafe - Long event on");
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
switch(control_mode)
{
case MANUAL:
case STABILIZE:
case FLY_BY_WIRE_A: // middle position
case FLY_BY_WIRE_B: // middle position
set_mode(RTL);
break;
case AUTO:
case LOITER:
case CIRCLE:
if(g.long_fs_action == 1) {
set_mode(RTL);
}
break;
case RTL:
default:
break;
}
}
static void failsafe_short_off_event()
{
// We're back in radio contact
SendDebug_P("Failsafe - Short event off");
failsafe = FAILSAFE_NONE;
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
// Reset control integrators
// ---------------------
reset_I();
}
#if BATTERY_EVENT == ENABLED
static void low_battery_event(void)
{
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
}
#endif
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0){
event_repeat --;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay_toggle();
}
}
}
static void relay_on()
{
PORTL |= B00000100;
}
static void relay_off()
{
PORTL &= ~B00000100;
}
static void relay_toggle()
{
PORTL ^= B00000100;
}

208
ArduPlane/navigation.pde Normal file
View File

@ -0,0 +1,208 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
g_gps->new_data = false;
return;
}
if(next_WP.lat == 0){
return;
}
// waypoint distance from plane
// ----------------------------
wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0){
gcs.send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));
//Serial.println(wp_distance,DEC);
//print_current_waypoints();
return;
}
// target_bearing is where we should be heading
// --------------------------------------------
target_bearing = get_bearing(&current_loc, &next_WP);
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
}
#if 0
// Disabled for now
void calc_distance_error()
{
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
wp_distance = max(distance_estimate,10);
}
#endif
static void calc_airspeed_errors()
{
// XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) {
airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed;
airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
} else {
airspeed_error = g.airspeed_cruise - airspeed;
airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
}
}
static void calc_bearing_error()
{
if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor;
} else {
// TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - g_gps->ground_course;
}
bearing_error = wrap_180(bearing_error);
}
static void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
// limit climb rates
target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
// stay within a certain range
if(prev_WP.alt > next_WP.alt){
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
} else if (command_may_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude = next_WP.alt;
}
/*
// Disabled for now
#if AIRSPEED_SENSOR == 1
long altitude_estimate; // for smoothing GPS output
// special thanks to Ryan Beall for this one
float pitch_angle = pitch_sensor - g.pitch_trim; // pitch_angle = pitch sensor - angle of attack of your plane at level *100 (50 = .5°)
pitch_angle = constrain(pitch_angle, -2000, 2000);
float scale = sin(radians(pitch_angle * .01));
altitude_estimate += (float)airspeed * .0002 * scale;
altitude_estimate -= ALT_EST_GAIN * (float)(altitude_estimate - current_loc.alt);
// compute altitude error for throttle control
altitude_error = target_altitude - altitude_estimate;
#else
altitude_error = target_altitude - current_loc.alt;
#endif
*/
altitude_error = target_altitude - current_loc.alt;
}
static long wrap_360(long error)
{
if (error > 36000) error -= 36000;
if (error < 0) error += 36000;
return error;
}
static long wrap_180(long error)
{
if (error > 18000) error -= 36000;
if (error < -18000) error += 36000;
return error;
}
static void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
power = constrain(power, 0.5, 1);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
/*
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
}
update_crosstrack();
*/
nav_bearing = wrap_360(nav_bearing);
}
static void update_crosstrack(void)
{
// Crosstrack Error
// ----------------
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing = wrap_360(nav_bearing);
}
}
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
}
static long get_distance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}

50
ArduPlane/planner.pde Normal file
View File

@ -0,0 +1,50 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command planner_menu_commands[] PROGMEM = {
{"gcs", planner_gcs},
};
// A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands);
static int8_t
planner_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Planner Mode\n\nThis mode is not intended for manual use\n\n"));
planner_menu.run();
return 0;
}
static int8_t
planner_gcs(uint8_t argc, const Menu::arg *argv)
{
gcs.init(&Serial);
int loopcount = 0;
while (1) {
if (millis()-fast_loopTimer > 19) {
fast_loopTimer = millis();
gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000);
if ((loopcount % 5) == 0) // 10 hz
gcs.data_stream_send(5,45);
if ((loopcount % 16) == 0) { // 3 hz
gcs.data_stream_send(1,5);
gcs.send_message(MSG_HEARTBEAT);
}
#endif
loopcount++;
}
}
return 0;
}

231
ArduPlane/radio.pde Normal file
View File

@ -0,0 +1,231 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static void init_rc_in()
{
// set rc reversing
update_servo_switches();
// set rc channel ranges
g.channel_roll.set_angle(SERVO_MAX);
g.channel_pitch.set_angle(SERVO_MAX);
g.channel_rudder.set_angle(SERVO_MAX);
g.channel_throttle.set_range(0, 100);
// set rc dead zones
g.channel_roll.dead_zone = 60;
g.channel_pitch.dead_zone = 60;
g.channel_rudder.dead_zone = 60;
g.channel_throttle.dead_zone = 6;
//set auxiliary ranges
if (g.rc_5_funct == RC_5_FUNCT_AILERON) {
g.rc_5.set_angle(SERVO_MAX);
} else if (g.rc_5_funct == RC_5_FUNCT_FLAP_AUTO || g.rc_5_funct == RC_5_FUNCT_FLAPERON) {
g.rc_5.set_range(0,100);
} else {
g.rc_5.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
if (g.rc_6_funct == RC_6_FUNCT_AILERON) {
g.rc_6.set_angle(SERVO_MAX);
} else if (g.rc_6_funct == RC_6_FUNCT_FLAP_AUTO || g.rc_6_funct == RC_6_FUNCT_FLAPERON) {
g.rc_6.set_range(0,100);
} else {
g.rc_6.set_range(0,1000); // Insert proper init for camera mount, etc., here
}
g.rc_7.set_range(0,1000); // Insert proper init for camera mount, etc., here
g.rc_8.set_range(0,1000);
}
static void init_rc_out()
{
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.Init(); // APM Radio initialization
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
}
static void read_radio()
{
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
if(g.mix_mode == 0){
g.channel_roll.set_pwm(ch1_temp);
g.channel_pitch.set_pwm(ch2_temp);
}else{
g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
}
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
// TO DO - go through and patch throttle reverse for RC_Channel library compatibility
#if THROTTLE_REVERSE == 1
radio_in[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_in[CH_THROTTLE];
#endif
control_failsafe(g.channel_throttle.radio_in);
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
if(g.airspeed_enabled == true) {
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
}
} else {
airspeed_nudge = 0;
throttle_nudge = 0;
}
/*
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in);
*/
}
static void control_failsafe(uint16_t pwm)
{
if(g.throttle_fs_enabled == 0)
return;
// Check for failsafe condition based on loss of GCS control
if (rc_override_active) {
if(millis() - rc_override_fs_timer > FAILSAFE_SHORT_TIME) {
ch3_failsafe = true;
} else {
ch3_failsafe = false;
}
//Check for failsafe and debounce funky reads
} else if (g.throttle_fs_enabled) {
if (pwm < (unsigned)g.throttle_fs_value){
// we detect a failsafe from radio
// throttle has dropped below the mark
failsafeCounter++;
if (failsafeCounter == 9){
SendDebug_P("MSG FS ON ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 10) {
ch3_failsafe = true;
}else if (failsafeCounter > 10){
failsafeCounter = 11;
}
}else if(failsafeCounter > 0){
// we are no longer in failsafe condition
// but we need to recover quickly
failsafeCounter--;
if (failsafeCounter > 3){
failsafeCounter = 3;
}
if (failsafeCounter == 1){
SendDebug_P("MSG FS OFF ");
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 0) {
ch3_failsafe = false;
}else if (failsafeCounter <0){
failsafeCounter = -1;
}
}
}
}
static void trim_control_surfaces()
{
read_radio();
// Store control surface trim values
// ---------------------------------
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
}else{
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
//Recompute values here using new values for elevon1_trim and elevon2_trim
//We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
}
static void trim_radio()
{
for (int y = 0; y < 30; y++) {
read_radio();
}
// Store the trim values
// ---------------------
if(g.mix_mode == 0){
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
//g.channel_throttle.radio_trim = g.channel_throttle.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.radio_trim = g.rc_5.radio_in; // Second aileron channel
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.radio_trim = g.rc_6.radio_in; // Second aileron channel
} else {
elevon1_trim = ch1_temp;
elevon2_trim = ch2_temp;
uint16_t center = 1500;
g.channel_roll.radio_trim = center;
g.channel_pitch.radio_trim = center;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
}
// save to eeprom
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
//g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
if (g.rc_5_funct == RC_5_FUNCT_AILERON) g.rc_5.save_eeprom();
if (g.rc_6_funct == RC_6_FUNCT_AILERON) g.rc_6.save_eeprom();
}

126
ArduPlane/sensors.pde Normal file
View File

@ -0,0 +1,126 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
void ReadSCP1000(void) {}
static void init_barometer(void)
{
int flashcount = 0;
long ground_pressure = 0;
int ground_temperature;
while(ground_pressure == 0){
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = barometer.Press;
ground_temperature = barometer.Temp;
mavlink_delay(20);
//Serial.printf("barometer.Press %ld\n", barometer.Press);
}
for(int i = 0; i < 30; i++){ // We take some readings...
#if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets
#endif
barometer.Read(); // Get initial data from absolute pressure sensor
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
mavlink_delay(20);
if(flashcount == 5) {
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(B_LED_PIN, LOW);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, HIGH);
digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, HIGH);
}
flashcount++;
}
g.ground_pressure.set_and_save(ground_pressure);
g.ground_temperature.set_and_save(ground_temperature / 10.0f);
abs_pressure = ground_pressure;
Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete."));
}
static long read_barometer(void)
{
float x, scaling, temp;
barometer.Read(); // Get new data from absolute pressure sensor
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
scaling = (float)g.ground_pressure / (float)abs_pressure;
temp = ((float)g.ground_temperature) + 273.15f;
x = log(scaling) * temp * 29271.267f;
return (x / 10);
}
// in M/S * 100
static void read_airspeed(void)
{
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU // Xplane will supply the airspeed
if (g.airspeed_offset == 0) {
// runtime enabling of airspeed, we need to do instant
// calibration before we can use it. This isn't as
// accurate as the 50 point average in zero_airspeed(),
// but it is better than using it uncalibrated
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
g.airspeed_offset.set_and_save(airspeed_raw);
}
airspeed_raw = ((float)adc.Ch(AIRSPEED_CH) * .25) + (airspeed_raw * .75);
airspeed_pressure = max(((int)airspeed_raw - g.airspeed_offset), 0);
airspeed = sqrt((float)airspeed_pressure * g.airspeed_ratio) * 100;
#endif
calc_airspeed_errors();
}
static void zero_airspeed(void)
{
airspeed_raw = (float)adc.Ch(AIRSPEED_CH);
for(int c = 0; c < 10; c++){
delay(20);
airspeed_raw = (airspeed_raw * .90) + ((float)adc.Ch(AIRSPEED_CH) * .10);
}
g.airspeed_offset.set_and_save(airspeed_raw);
}
#endif // HIL_MODE != HIL_MODE_ATTITUDE
static void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
if(g.battery_monitoring == 1)
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
if(g.battery_monitoring == 2)
battery_voltage = battery_voltage4;
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
battery_voltage = battery_voltage1;
if(g.battery_monitoring == 4) {
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
}
#if BATTERY_EVENT == ENABLED
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
#endif
}

602
ArduPlane/setup.pde Normal file
View File

@ -0,0 +1,602 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// Functions called from the setup menu
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
// Command/function table for the setup menu
static const struct Menu::command setup_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"reset", setup_factory},
{"radio", setup_radio},
{"modes", setup_flightmodes},
{"compass", setup_compass},
{"declination", setup_declination},
{"battery", setup_batt_monitor},
{"show", setup_show},
{"erase", setup_erase},
};
// Create the setup menu object.
MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu.
static int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
Serial.printf_P(PSTR("Setup Mode\n"
"\n"
"IMPORTANT: if you have not previously set this system up, use the\n"
"'reset' command to initialize the EEPROM to sensible default values\n"
"and then the 'radio' command to configure for your radio.\n"
"\n"));
// Run the setup menu. When the menu exits, we will return to the main menu.
setup_menu.run();
return 0;
}
// Print the current configuration.
// Called by the setup menu 'show' command.
static int8_t
setup_show(uint8_t argc, const Menu::arg *argv)
{
// clear the area
print_blanks(8);
report_radio();
report_batt_monitor();
report_gains();
report_xtrack();
report_throttle();
report_flight_modes();
report_imu();
report_compass();
Serial.printf_P(PSTR("Raw Values\n"));
print_divider();
AP_Var_menu_show(argc, argv);
return(0);
}
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
// Called by the setup menu 'factoryreset' command.
static int8_t
setup_factory(uint8_t argc, const Menu::arg *argv)
{
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
AP_Var::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
for (;;) {
}
// note, cannot actually return here
return(0);
}
// Perform radio setup.
// Called by the setup menu 'radio' command.
static int8_t
setup_radio(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\n\nRadio Setup:\n"));
uint8_t i;
for(i = 0; i < 100;i++){
delay(20);
read_radio();
}
if(g.channel_roll.radio_in < 500){
while(1){
Serial.printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
g.channel_roll.radio_min = g.channel_roll.radio_in;
g.channel_pitch.radio_min = g.channel_pitch.radio_in;
g.channel_throttle.radio_min = g.channel_throttle.radio_in;
g.channel_rudder.radio_min = g.channel_rudder.radio_in;
g.rc_5.radio_min = g.rc_5.radio_in;
g.rc_6.radio_min = g.rc_6.radio_in;
g.rc_7.radio_min = g.rc_7.radio_in;
g.rc_8.radio_min = g.rc_8.radio_in;
g.channel_roll.radio_max = g.channel_roll.radio_in;
g.channel_pitch.radio_max = g.channel_pitch.radio_in;
g.channel_throttle.radio_max = g.channel_throttle.radio_in;
g.channel_rudder.radio_max = g.channel_rudder.radio_in;
g.rc_5.radio_max = g.rc_5.radio_in;
g.rc_6.radio_max = g.rc_6.radio_in;
g.rc_7.radio_max = g.rc_7.radio_in;
g.rc_8.radio_max = g.rc_8.radio_in;
g.channel_roll.radio_trim = g.channel_roll.radio_in;
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
g.rc_5.radio_trim = 1500;
g.rc_6.radio_trim = 1500;
g.rc_7.radio_trim = 1500;
g.rc_8.radio_trim = 1500;
Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
g.channel_roll.update_min_max();
g.channel_pitch.update_min_max();
g.channel_throttle.update_min_max();
g.channel_rudder.update_min_max();
g.rc_5.update_min_max();
g.rc_6.update_min_max();
g.rc_7.update_min_max();
g.rc_8.update_min_max();
if(Serial.available() > 0){
Serial.flush();
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
g.channel_rudder.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
print_done();
break;
}
}
trim_radio();
report_radio();
return(0);
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, mode = 0;
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
trim_radio();
while(1){
delay(20);
read_radio();
switchPosition = readSwitch();
// look for control switch change
if (oldSwitchPosition != switchPosition){
// force position 5 to MANUAL
if (switchPosition > 4) {
flight_modes[switchPosition] = MANUAL;
}
// update our current mode
mode = flight_modes[switchPosition];
// update the user
print_switch(switchPosition, mode);
// Remember switch position
oldSwitchPosition = switchPosition;
}
// look for stick input
int radioInputSwitch = radio_input_switch();
if (radioInputSwitch != 0){
mode += radioInputSwitch;
while (
mode != MANUAL &&
mode != CIRCLE &&
mode != STABILIZE &&
mode != FLY_BY_WIRE_A &&
mode != FLY_BY_WIRE_B &&
mode != AUTO &&
mode != RTL &&
mode != LOITER)
{
if (mode < MANUAL)
mode = LOITER;
else if (mode >LOITER)
mode = MANUAL;
else
mode += radioInputSwitch;
}
// Override position 5
if(switchPosition > 4)
mode = MANUAL;
// save new mode
flight_modes[switchPosition] = mode;
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(Serial.available() > 0){
// save changes
for (mode=0; mode<6; mode++)
flight_modes[mode].save();
report_flight_modes();
print_done();
return (0);
}
}
}
static int8_t
setup_declination(uint8_t argc, const Menu::arg *argv)
{
compass.set_declination(radians(argv[1].f));
report_compass();
return 0;
}
static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
{
int c;
Serial.printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
do {
c = Serial.read();
} while (-1 == c);
if (('y' != c) && ('Y' != c))
return(-1);
zero_eeprom();
return 0;
}
static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("on"))) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
g.compass_enabled = true;
}
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
g.compass_enabled = false;
} else {
Serial.printf_P(PSTR("\nOptions:[on,off]\n"));
report_compass();
return 0;
}
g.compass_enabled.save();
report_compass();
return 0;
}
static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i >= 0 && argv[1].i <= 4){
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOptions: 0-4"));
}
report_batt_monitor();
return 0;
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
static void report_batt_monitor()
{
//print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
print_blanks(2);
}
static void report_radio()
{
//print_blanks(2);
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
static void report_gains()
{
//print_blanks(2);
Serial.printf_P(PSTR("Gains\n"));
print_divider();
Serial.printf_P(PSTR("servo roll:\n"));
print_PID(&g.pidServoRoll);
Serial.printf_P(PSTR("servo pitch:\n"));
print_PID(&g.pidServoPitch);
Serial.printf_P(PSTR("servo rudder:\n"));
print_PID(&g.pidServoRudder);
Serial.printf_P(PSTR("nav roll:\n"));
print_PID(&g.pidNavRoll);
Serial.printf_P(PSTR("nav pitch airspeed:\n"));
print_PID(&g.pidNavPitchAirspeed);
Serial.printf_P(PSTR("energry throttle:\n"));
print_PID(&g.pidTeThrottle);
Serial.printf_P(PSTR("nav pitch alt:\n"));
print_PID(&g.pidNavPitchAltitude);
print_blanks(2);
}
static void report_xtrack()
{
//print_blanks(2);
Serial.printf_P(PSTR("Crosstrack\n"));
print_divider();
// radio
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"),
(float)g.crosstrack_gain,
(int)g.crosstrack_entry_angle);
print_blanks(2);
}
static void report_throttle()
{
//print_blanks(2);
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
"failsafe_enabled: %d\n"
"failsafe_value: %d\n"),
(int)g.throttle_min,
(int)g.throttle_max,
(int)g.throttle_cruise,
(int)g.throttle_fs_enabled,
(int)g.throttle_fs_value);
print_blanks(2);
}
static void report_imu()
{
//print_blanks(2);
Serial.printf_P(PSTR("IMU\n"));
print_divider();
print_gyro_offsets();
print_accel_offsets();
print_blanks(2);
}
static void report_compass()
{
//print_blanks(2);
Serial.printf_P(PSTR("Compass: "));
switch (compass.product_id) {
case AP_COMPASS_TYPE_HMC5883L:
Serial.println_P(PSTR("HMC5883L"));
break;
case AP_COMPASS_TYPE_HMC5843:
Serial.println_P(PSTR("HMC5843"));
break;
case AP_COMPASS_TYPE_HIL:
Serial.println_P(PSTR("HIL"));
break;
default:
Serial.println_P(PSTR("??"));
break;
}
print_divider();
print_enabled(g.compass_enabled);
// mag declination
Serial.printf_P(PSTR("Mag Declination: %4.4f\n"),
degrees(compass.get_declination()));
Vector3f offsets = compass.get_offsets();
// mag offsets
Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
offsets.x,
offsets.y,
offsets.z);
print_blanks(2);
}
static void report_flight_modes()
{
//print_blanks(2);
Serial.printf_P(PSTR("Flight modes\n"));
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, flight_modes[i]);
}
print_blanks(2);
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
static void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
pid->kD(),
(long)pid->imax());
}
static void
print_radio_values()
{
Serial.printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max);
Serial.printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.radio_max);
Serial.printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
}
static void
print_switch(byte p, byte m)
{
Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]);
}
static void
print_done()
{
Serial.printf_P(PSTR("\nSaved Settings\n\n"));
}
static void
print_blanks(int num)
{
while(num > 0){
num--;
Serial.println("");
}
}
static void
print_divider(void)
{
for (int i = 0; i < 40; i++) {
Serial.printf_P(PSTR("-"));
}
Serial.println("");
}
static int8_t
radio_input_switch(void)
{
static int8_t bouncer = 0;
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
} else {
return 0;
}
}
static void zero_eeprom(void)
{
byte b = 0;
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);
}
Serial.printf_P(PSTR("done\n"));
}
static void print_enabled(bool b)
{
if(b)
Serial.printf_P(PSTR("en"));
else
Serial.printf_P(PSTR("dis"));
Serial.printf_P(PSTR("abled\n"));
}
static void
print_accel_offsets(void)
{
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.ax(),
(float)imu.ay(),
(float)imu.az());
}
static void
print_gyro_offsets(void)
{
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.gx(),
(float)imu.gy(),
(float)imu.gz());
}
#endif // CLI_ENABLED

553
ArduPlane/system.pde Normal file
View File

@ -0,0 +1,553 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*****************************************************************************
The init_ardupilot function processes everything we need for an in - air restart
We will determine later if we are actually on the ground and process a
ground start in that case.
*****************************************************************************/
#if CLI_ENABLED == ENABLED
// Functions called from the top-level menu
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\n"
" setup setup mode\n"
" test test mode\n"
"\n"
"Move the slide switch and reset to FLY.\n"
"\n"));
return(0);
}
// Command/function table for the top-level menu.
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
{"help", main_menu_help},
{"planner", planner_mode}
};
// Create the top-level menu object.
MENU(main_menu, "APM trunk", main_menu_commands);
#endif // CLI_ENABLED
static void init_ardupilot()
{
// Console serial port
//
// The console port buffers are defined to be sufficiently large to support
// the console's use as a logging device, optionally as the GPS port when
// GPS_PROTOCOL_IMU is selected, and as the telemetry port.
//
// XXX This could be optimised to reduce the buffer sizes in the cases
// where they are not otherwise required.
//
Serial.begin(SERIAL0_BAUD, 128, 128);
// GPS serial port.
//
// Not used if the IMU/X-Plane GPS is in use.
//
// XXX currently the EM406 (SiRF receiver) is nominally configured
// at 57600, however it's not been supported to date. We should
// probably standardise on 38400.
//
// XXX the 128 byte receive buffer may be too small for NMEA, depending
// on the message set configured.
//
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT == 1 // TODO: figure out a better way to do this
// Steal gps port for hardware in the loop
Serial1.begin(115200, 128, 128);
#else
// standard gps running
Serial1.begin(38400, 128, 16);
#endif
Serial.printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %lu\n"),
freeRAM());
//
// Check the EEPROM format version before loading any parameters from EEPROM.
//
if (!g.format_version.load()) {
Serial.println_P(PSTR("\nEEPROM blank - resetting all parameters to defaults...\n"));
delay(100); // wait for serial msg to flush
AP_Var::erase_all();
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
} else if (g.format_version != Parameters::k_format_version) {
Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)"
"\n\nForcing complete parameter reset..."),
g.format_version.get(), Parameters::k_format_version);
delay(100); // wait for serial msg to flush
// erase all parameters
AP_Var::erase_all();
// save the new format version
g.format_version.set_and_save(Parameters::k_format_version);
Serial.println_P(PSTR("done."));
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Var::load_all();
Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before);
Serial.printf_P(PSTR("using %u bytes of memory (%u resets)\n"),
AP_Var::get_memory_use(), (unsigned)g.num_resets);
}
// keep a record of how many resets have happened. This can be
// used to detect in-flight resets
g.num_resets.set_and_save(g.num_resets+1);
// Telemetry port.
//
// Not used if telemetry is going to the console.
//
// XXX for unidirectional protocols, we could (should) minimize
// the receive buffer, and the transmit buffer could also be
// shrunk for protocols that don't send large messages.
//
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
mavlink_system.sysid = g.sysid_this_mav;
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization
if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false;
} else {
dcm.set_compass(&compass);
compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
}
/*
Init is depricated - Jason
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
Serial.print("Sonar init: "); Serial.println(SONAR_PIN, DEC);
}
*/
#endif
DataFlash.Init(); // DataFlash log initialization
// Do GPS init
g_gps = &g_gps_driver;
g_gps->init(); // GPS Initialization
g_gps->callback = mavlink_delay;
// init the GCS
#if GCS_PORT == 3
gcs.init(&Serial3);
#else
gcs.init(&Serial);
#endif
//mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_FIXED_WING;
// init the HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PORT == 3
hil.init(&Serial3);
#elif HIL_PORT == 1
hil.init(&Serial1);
#else
hil.init(&Serial);
#endif
#endif
// We may have a hil object instantiated just for mission planning
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
hil.init(&Serial);
#endif
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
pinMode(PUSHBUTTON_PIN, INPUT); // unused
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
#if CLI_ENABLED == ENABLED
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
Serial.printf_P(PSTR("\n"
"Entering interactive setup mode...\n"
"\n"
"If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n"
"Type 'help' to list commands, 'exit' to leave a submenu.\n"
"Visit the 'setup' menu for first-time configuration.\n"));
for (;;) {
Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n"));
main_menu.run();
}
}
#endif // CLI_ENABLED
if(g.log_bitmask != 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
byte last_log_num = get_num_logs();
start_new_log(last_log_num);
}
// read in the flight switches
update_servo_switches();
if (ENABLE_AIR_START == 1) {
// Perform an air start and get back to flying
gcs.send_text_P(SEVERITY_LOW,PSTR("<init_ardupilot> AIR START"));
// Get necessary data from EEPROM
//----------------
//read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.init(IMU::WARM_START);
dcm.set_centripetal(1);
#endif
// This delay is important for the APM_RC library to work.
// We need some time for the comm between the 328 and 1280 to be established.
int old_pulse = 0;
while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 ||
APM_RC.InputCh(g.flight_mode_channel) == 1000 ||
APM_RC.InputCh(g.flight_mode_channel) == 1200)) {
old_pulse = APM_RC.InputCh(g.flight_mode_channel);
delay(25);
}
GPS_enabled = false;
g_gps->update();
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED) GPS_enabled = true;
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_AIRSTART_MSG);
reload_commands_airstart(); // Get set to resume AUTO from where we left off
}else {
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
}
set_mode(MANUAL);
// set the correct flight mode
// ---------------------------
reset_control_switch();
}
//********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//********************************************************************************
static void startup_ground(void)
{
set_mode(INITIALISING);
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> GROUND START"));
#if(GROUND_START_DELAY > 0)
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> With Delay"));
delay(GROUND_START_DELAY * 1000);
#endif
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
// Makes the servos wiggle
// step 1 = 1 wiggle
// -----------------------
demo_servos(1);
//IMU ground start
//------------------------
//
startup_IMU_ground();
// read the radio to set trims
// ---------------------------
trim_radio(); // This was commented out as a HACK. Why? I don't find a problem.
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.airspeed_enabled == true)
{
// initialize airspeed sensor
// --------------------------
zero_airspeed();
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> zero airspeed calibrated"));
}
else
{
gcs.send_text_P(SEVERITY_LOW,PSTR("<startup_ground> NO airspeed"));
}
#endif
// Save the settings for in-air restart
// ------------------------------------
//save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
// Read in the GPS - see if one is connected
GPS_enabled = false;
for (byte counter = 0; ; counter++) {
g_gps->update();
if (g_gps->status() != 0 || HIL_MODE != HIL_MODE_DISABLED){
GPS_enabled = true;
break;
}
if (counter >= 2) {
GPS_enabled = false;
break;
}
}
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
demo_servos(3);
gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
}
static void set_mode(byte mode)
{
if(control_mode == mode){
// don't switch modes if we are already in the correct mode.
return;
}
if(g.auto_trim > 0 && control_mode == MANUAL)
trim_control_surfaces();
control_mode = mode;
crash_timer = 0;
switch(control_mode)
{
case INITIALISING:
case MANUAL:
case CIRCLE:
case STABILIZE:
case FLY_BY_WIRE_A:
case FLY_BY_WIRE_B:
break;
case AUTO:
update_auto();
break;
case RTL:
do_RTL();
break;
case LOITER:
do_loiter_at_location();
break;
case GUIDED:
set_guided_WP();
break;
default:
do_RTL();
break;
}
// output control mode to the ground station
gcs.send_message(MSG_MODE_CHANGE);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
static void check_long_failsafe()
{
// only act on changes
// -------------------
if(failsafe != FAILSAFE_LONG && failsafe != FAILSAFE_GCS){
if(rc_override_active && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_LONG;
failsafe_long_on_event();
}
if(! rc_override_active && failsafe == FAILSAFE_SHORT && millis() - ch3_failsafe_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_LONG;
failsafe_long_on_event();
}
if(g.gcs_heartbeat_fs_enabled && millis() - rc_override_fs_timer > FAILSAFE_LONG_TIME) {
failsafe = FAILSAFE_GCS;
failsafe_long_on_event();
}
} else {
// We do not change state but allow for user to change mode
if(failsafe == FAILSAFE_GCS && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && rc_override_active && millis() - rc_override_fs_timer < FAILSAFE_SHORT_TIME) failsafe = FAILSAFE_NONE;
if(failsafe == FAILSAFE_LONG && !rc_override_active && !ch3_failsafe) failsafe = FAILSAFE_NONE;
}
}
static void check_short_failsafe()
{
// only act on changes
// -------------------
if(failsafe == FAILSAFE_NONE){
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
failsafe_short_on_event();
}
}
if(failsafe == FAILSAFE_SHORT){
if(!ch3_failsafe) {
failsafe_short_off_event();
}
}
}
static void startup_IMU_ground(void)
{
#if HIL_MODE != HIL_MODE_ATTITUDE
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!!
// -----------------------
demo_servos(2);
gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
imu.init(IMU::COLD_START, mavlink_delay);
dcm.set_centripetal(1);
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, HIGH); // Set LED B high to indicate IMU ready
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, LOW);
}
static void update_GPS_light(void)
{
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// ---------------------------------------------------------------------
switch (g_gps->status()) {
case(2):
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
break;
case(1):
if (g_gps->valid_read == true){
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
if (GPS_light){
digitalWrite(C_LED_PIN, LOW);
} else {
digitalWrite(C_LED_PIN, HIGH);
}
g_gps->valid_read = false;
}
break;
default:
digitalWrite(C_LED_PIN, LOW);
break;
}
}
static void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
dcm.gyro_sat_count = 0;
imu.adc_constraints = 0;
dcm.renorm_sqrt_count = 0;
dcm.renorm_blowup_count = 0;
gps_fix_count = 0;
pmTest1 = 0;
perf_mon_timer = millis();
}
/*
* This function gets the current value of the heap and stack pointers.
* The stack pointer starts at the top of RAM and grows downwards. The heap pointer
* starts just above the static variables etc. and grows upwards. SP should always
* be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6 - Feb - 2009.
*/
static unsigned long freeRAM() {
uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer
free(stackptr); // free up the memory again (sets stackptr to 0)
stackptr = (uint8_t *)(SP); // save value of stack pointer
return stackptr - heapptr;
}
/*
map from a 8 bit EEPROM baud rate to a real baud rate
*/
static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
{
switch (rate) {
case 9: return 9600;
case 19: return 19200;
case 38: return 38400;
case 57: return 57600;
case 111: return 111100;
case 115: return 115200;
}
Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
return default_baud;
}

688
ArduPlane/test.pde Normal file
View File

@ -0,0 +1,688 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if CLI_ENABLED == ENABLED
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
static int8_t test_gyro(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"failsafe", test_failsafe},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
{"dipswitches", test_dipswitches},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
#if HIL_MODE == HIL_MODE_DISABLED
{"adc", test_adc},
{"gps", test_gps},
{"rawgps", test_rawgps},
{"imu", test_imu},
{"gyro", test_gyro},
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"compass", test_mag},
{"current", test_current},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
{"imu", test_imu},
{"gyro", test_gyro},
{"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
static int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
return 0;
}
static void print_hit_enter()
{
Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
}
static int8_t
test_eedump(uint8_t argc, const Menu::arg *argv)
{
int i, j;
// hexdump the EEPROM
for (i = 0; i < EEPROM_MAX_ADDR; i += 16) {
Serial.printf_P(PSTR("%04x:"), i);
for (j = 0; j < 16; j++)
Serial.printf_P(PSTR(" %02x"), eeprom_read_byte((const uint8_t *)(i + j)));
Serial.println();
}
return(0);
}
static int8_t
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
Serial.printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in,
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
#if THROTTLE_REVERSE == 1
Serial.printf_P(PSTR("Throttle is reversed in config: \n"));
delay(1000);
#endif
// read the radio to set trims
// ---------------------------
trim_radio();
while(1){
delay(20);
read_radio();
update_servo_switches();
g.channel_roll.calc_pwm();
g.channel_pitch.calc_pwm();
g.channel_throttle.calc_pwm();
g.channel_rudder.calc_pwm();
// write out the servo PWM values
// ------------------------------
set_servos();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_roll.control_in,
g.channel_pitch.control_in,
g.channel_throttle.control_in,
g.channel_rudder.control_in,
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in,
g.rc_8.control_in);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
byte fail_test;
print_hit_enter();
for(int i = 0; i < 50; i++){
delay(20);
read_radio();
}
// read the radio to set trims
// ---------------------------
trim_radio();
oldSwitchPosition = readSwitch();
Serial.printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.channel_throttle.control_in > 0){
delay(20);
read_radio();
}
while(1){
delay(20);
read_radio();
if(g.channel_throttle.control_in > 0){
Serial.printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in);
fail_test++;
}
if(oldSwitchPosition != readSwitch()){
Serial.printf_P(PSTR("CONTROL MODE CHANGED: "));
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
Serial.printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
Serial.println(flight_mode_strings[readSwitch()]);
fail_test++;
}
if(fail_test > 0){
return (0);
}
if(Serial.available() > 0){
Serial.printf_P(PSTR("LOS caused no change in APM.\n"));
return (0);
}
}
}
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring >=1 && g.battery_monitoring < 4) {
for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize
delay(20);
read_battery();
}
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
battery_voltage1,
battery_voltage2,
battery_voltage3,
battery_voltage4);
} else {
Serial.printf_P(PSTR("Not enabled\n"));
}
return (0);
}
static int8_t
test_current(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring == 4) {
print_hit_enter();
delta_ms_medium_loop = 100;
while(1){
delay(100);
read_radio();
read_battery();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery_voltage,
current_amps,
current_total);
// write out the servo PWM values
// ------------------------------
set_servos();
if(Serial.available() > 0){
return (0);
}
}
} else {
Serial.printf_P(PSTR("Not enabled\n"));
return (0);
}
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
Serial.printf_P(PSTR("Relay on\n"));
relay_on();
delay(3000);
if(Serial.available() > 0){
return (0);
}
Serial.printf_P(PSTR("Relay off\n"));
relay_off();
delay(3000);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
// save the alitude above home option
if(g.RTL_altitude < 0){
Serial.printf_P(PSTR("Hold current altitude\n"));
}else{
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
}
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total);
Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i);
test_wp_print(&temp, i);
}
return (0);
}
static void
test_wp_print(struct Location *cmd, byte wp_index)
{
Serial.printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
cmd->alt,
cmd->lat,
cmd->lng);
}
static int8_t
test_xbee(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n"));
while(1){
if (Serial3.available())
Serial3.write(Serial3.read());
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
Serial.printf_P(PSTR("Control CH "));
Serial.println(FLIGHT_MODE_CHANNEL, DEC);
while(1){
delay(20);
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
Serial.printf_P(PSTR("Position %d\n"), switchPosition);
oldSwitchPosition = switchPosition;
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_dipswitches(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
if (!g.switch_enable) {
Serial.println_P(PSTR("dip switches disabled, using EEPROM"));
}
while(1){
delay(100);
update_servo_switches();
if (g.mix_mode == 0) {
Serial.printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"),
(int)g.channel_roll.get_reverse(),
(int)g.channel_pitch.get_reverse(),
(int)g.channel_rudder.get_reverse());
} else {
Serial.printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"),
(int)g.reverse_elevons,
(int)g.reverse_ch1_elevon,
(int)g.reverse_ch2_elevon);
}
if(Serial.available() > 0){
return (0);
}
}
}
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
#if HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("ADC\n"));
delay(1000);
while(1){
for (int i=0;i<9;i++) Serial.printf_P(PSTR("%d\t"),adc.Ch(i));
Serial.println();
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
delay(333);
// Blink GPS LED if we don't have a fix
// ------------------------------------
update_GPS_light();
g_gps->update();
if (g_gps->new_data){
Serial.printf_P(PSTR("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n"),
g_gps->latitude,
g_gps->longitude,
g_gps->altitude/100,
g_gps->num_sats);
}else{
Serial.printf_P(PSTR("."));
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_imu(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("Calibrating."));
imu.init(IMU::COLD_START);
print_hit_enter();
delay(1000);
while(1){
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
dcm.update_DCM(G_Dt);
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
medium_loopCounter = 0;
}
}
// We are using the IMU
// ---------------------
Serial.printf_P(PSTR("r: %d\tp: %d\t y: %d\n"),
(int)dcm.roll_sensor / 100,
(int)dcm.pitch_sensor / 100,
(uint16_t)dcm.yaw_sensor / 100);
}
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_gyro(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init();
delay(1000);
Serial.printf_P(PSTR("Gyro | Accel\n"));
delay(1000);
while(1){
imu.update(); // need this because we are not calling the DCM
Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel();
Serial.printf_P(PSTR("%d\t%d\t%d\t|\t%d\t%d\t%d\n"),
(int)gyros.x,
(int)gyros.y,
(int)gyros.z,
(int)accels.x,
(int)accels.y,
(int)accels.z);
delay(100);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
Serial.printf_P(PSTR("Compass: "));
print_enabled(false);
return (0);
}
compass.set_orientation(MAG_ORIENTATION);
if (!compass.init()) {
Serial.println_P(PSTR("Compass initialisation failed!"));
return 0;
}
dcm.set_compass(&compass);
report_compass();
// we need the DCM initialised for this test
imu.init(IMU::COLD_START);
int counter = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
print_hit_enter();
while(1) {
delay(20);
if (millis() - fast_loopTimer > 19) {
delta_ms_fast_loop = millis() - fast_loopTimer;
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis();
// IMU
// ---
dcm.update_DCM(G_Dt);
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
counter=0;
}
}
if (Serial.available() > 0) {
break;
}
}
// save offsets. This allows you to get sane offset values using
// the CLI before you go flying.
Serial.println_P(PSTR("saving offsets"));
compass.save_offsets();
return (0);
}
#endif // HIL_MODE == HIL_MODE_DISABLED || HIL_MODE == HIL_MODE_SENSORS
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
#if HIL_MODE == HIL_MODE_DISABLED
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
unsigned airspeed_ch = adc.Ch(AIRSPEED_CH);
// Serial.println(adc.Ch(AIRSPEED_CH));
Serial.printf_P(PSTR("airspeed_ch: %u\n"), airspeed_ch);
if (g.airspeed_enabled == false){
Serial.printf_P(PSTR("airspeed: "));
print_enabled(false);
return (0);
}else{
print_hit_enter();
zero_airspeed();
Serial.printf_P(PSTR("airspeed: "));
print_enabled(true);
while(1){
delay(20);
read_airspeed();
Serial.printf_P(PSTR("%fm/s\n"), airspeed / 100.0);
if(Serial.available() > 0){
return (0);
}
}
}
}
static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter();
home.alt = 0;
wp_distance = 0;
init_barometer();
while(1){
delay(100);
current_loc.alt = read_barometer() + home.alt;
Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld\n"),
current_loc.alt / 100.0,
abs_pressure);
if(Serial.available() > 0){
return (0);
}
}
}
static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
while(1){
if (Serial3.available()){
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read());
digitalWrite(B_LED_PIN, LOW);
}
if (Serial1.available()){
digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LOW);
}
if(Serial.available() > 0){
return (0);
}
}
}
#endif // HIL_MODE == HIL_MODE_DISABLED
#endif // CLI_ENABLED