fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Simon Wilks
2012-09-24 23:13:11 +02:00
parent ccafa0e982 eaa6d0d4f2
commit 4adca98991
658 changed files with 13438 additions and 7985 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.cproject
-116
View File
@@ -1,116 +0,0 @@
<?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="cdt.managedbuild.toolchain.gnu.cross.base.1480167869">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.1480167869" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<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 artifactName="PX4 Firmware" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.cross.base.1480167869" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.1480167869.1872839368" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.cross.base.1856969168" name="cdt.managedbuild.toolchain.gnu.cross.base" superClass="cdt.managedbuild.toolchain.gnu.cross.base">
<option id="cdt.managedbuild.option.gnu.cross.prefix.991032273" name="Prefix" superClass="cdt.managedbuild.option.gnu.cross.prefix"/>
<option id="cdt.managedbuild.option.gnu.cross.path.272414815" name="Path" superClass="cdt.managedbuild.option.gnu.cross.path"/>
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.targetPlatform.gnu.cross.28710194" isAbstract="false" osList="all" superClass="cdt.managedbuild.targetPlatform.gnu.cross"/>
<builder id="cdt.managedbuild.builder.gnu.cross.65631855" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.builder.gnu.cross"/>
<tool id="cdt.managedbuild.tool.gnu.cross.c.compiler.1999202405" name="Cross GCC Compiler" superClass="cdt.managedbuild.tool.gnu.cross.c.compiler">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1330238701" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.cpp.compiler.917097290" name="Cross G++ Compiler" superClass="cdt.managedbuild.tool.gnu.cross.cpp.compiler">
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1246192262" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.c.linker.901081720" name="Cross GCC Linker" superClass="cdt.managedbuild.tool.gnu.cross.c.linker"/>
<tool id="cdt.managedbuild.tool.gnu.cross.cpp.linker.2000759535" name="Cross G++ Linker" superClass="cdt.managedbuild.tool.gnu.cross.cpp.linker">
<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.476227307" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.cross.archiver.747956554" name="Cross GCC Archiver" superClass="cdt.managedbuild.tool.gnu.cross.archiver"/>
<tool id="cdt.managedbuild.tool.gnu.cross.assembler.1942271155" name="Cross GCC Assembler" superClass="cdt.managedbuild.tool.gnu.cross.assembler">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.30395998" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="PX4 Firmware.null.1571077338" name="PX4 Firmware"/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Default">
<resource resourceType="PROJECT" workspacePath="/PX4 Firmware"/>
</configuration>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1480167869;cdt.managedbuild.toolchain.gnu.cross.base.1480167869.1872839368;cdt.managedbuild.tool.gnu.cross.c.compiler.1999202405;cdt.managedbuild.tool.gnu.c.compiler.input.1330238701">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.1480167869;cdt.managedbuild.toolchain.gnu.cross.base.1480167869.1872839368;cdt.managedbuild.tool.gnu.cross.cpp.compiler.917097290;cdt.managedbuild.tool.gnu.cpp.compiler.input.1246192262">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.managedbuilder.core.GCCManagedMakePerProjectProfileCPP"/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings">
<doc-comment-owner id="org.eclipse.cdt.ui.doxygen">
<path value=""/>
</doc-comment-owner>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="firmware" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>all</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>distclean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="upload" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>upload</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="ROMFS" path="ROMFS" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cproject>
.gitignore
+3
View File
@@ -3,6 +3,8 @@
.depend
.config
.version
.project
.cproject
apps/namedapp/namedapp_list.h
apps/namedapp/namedapp_proto.h
Make.dep
@@ -35,3 +37,4 @@ Firmware.sublime-workspace
.DS_Store
nsh_romfsimg.h
cscope.out
.configX-e
.project
-135
View File
@@ -1,135 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>PX4 Firmware</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
<dictionary>
<key>?name?</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.append_environment</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
<value>make</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
<value>clean</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.contents</key>
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
<value>false</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
<value>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>
<filteredResources>
<filter>
<id>1344101890673</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-*.o</arguments>
</matcher>
</filter>
<filter>
<id>1344101890683</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-.dep</arguments>
</matcher>
</filter>
<filter>
<id>1344101890687</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-.context</arguments>
</matcher>
</filter>
<filter>
<id>1344101890691</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-.depend</arguments>
</matcher>
</filter>
<filter>
<id>1344101890695</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-.built</arguments>
</matcher>
</filter>
<filter>
<id>1344101890698</id>
<name></name>
<type>6</type>
<matcher>
<id>org.eclipse.ui.ide.multiFilter</id>
<arguments>1.0-name-matches-false-false-*.a</arguments>
</matcher>
</filter>
</filteredResources>
</projectDescription>
ROMFS/Makefile
+3 -1
View File
@@ -21,13 +21,15 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
$(SRCROOT)/scripts/rc.standalone~init.d/rc.standalone \
$(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \
$(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \
$(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \
$(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \
$(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \
$(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
$(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
$(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
$(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix
$(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
$(SRCROOT)/logging/logconv.m~logging/logconv.m
#
# Add the PX4IO firmware to the spec if someone has dropped it into the
ROMFS/logging/logconv.m
+98
View File
@@ -0,0 +1,98 @@
clear all
clc
if exist('actuator_outputs0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_outputs0.bin')
fileSize = length(myFile)
fid = fopen('actuator_outputs0.bin', 'r');
elements = int64(fileSize./(16*4+8))
for i=1:(elements-2)
% timestamp
actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
end
end
if exist('actuator_controls0.bin', 'file')
% Read actuators file
myFile = java.io.File('actuator_controls0.bin')
fileSize = length(myFile)
fid = fopen('actuator_controls0.bin', 'r');
elements = int64(fileSize./(8*4+8))
for i=1:elements
% timestamp
actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
% actuators 1-16
% quadrotor: motor 1-4 on the first four positions
actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
end
end
if exist('sensor_combined.bin', 'file')
% Read sensor combined file
% Type definition: Firmware/apps/uORB/topics/sensor_combined.h
% Struct: sensor_combined_s
fileInfo = dir('sensor_combined.bin');
fileSize = fileInfo.bytes;
fid = fopen('sensor_combined.bin', 'r');
for i=1:elements
% timestamp
sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
% gyro raw
sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
% gyro counter
sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
% gyro in rad/s
sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
% accelerometer raw
sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% accelerometer counter
sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
% accel in m/s2
sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
% accel mode
sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
% accel range
sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag raw
sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
% padding bytes
fread(fid, 1, 'int16', 0, 'ieee-le');
% mag in Gauss
sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
% mag mode
sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
% mag range
sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag cuttoff freq
sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
% mag counter
sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
% baro pressure millibar
% baro alt meter
% baro temp celcius
% battery voltage
% adc voltage (3 channels)
sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
% baro counter and battery counter
sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
% battery voltage valid flag
sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
end
end
ROMFS/scripts/rc.PX4FMU_quad_x → ROMFS/scripts/rc.FMU_quad_x
+1 -1
View File
@@ -7,7 +7,7 @@ uorb start
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters]
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
fi
Tools/px_uploader.py
+26 -16
View File
@@ -1,3 +1,4 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
@@ -102,7 +103,8 @@ class uploader(object):
REBOOT = chr(0x30)
INFO_BL_REV = chr(1) # bootloader protocol revision
BL_REV = 2 # supported bootloader protocol
BL_REV_MIN = 2 # minimum supported bootloader protocol
BL_REV_MAX = 3 # maximum supported bootloader protocol
INFO_BOARD_ID = chr(2) # board type
INFO_BOARD_REV = chr(3) # board revision
INFO_FLASH_SIZE = chr(4) # max firmware size in bytes
@@ -148,16 +150,16 @@ class uploader(object):
+ uploader.EOC)
self.__getSync()
def __trySync(self):
c = self.__recv()
if (c != self.INSYNC):
#print("unexpected 0x%x instead of INSYNC" % ord(c))
return False;
c = self.__recv()
if (c != self.OK):
#print("unexpected 0x%x instead of OK" % ord(c))
return False
return True
# def __trySync(self):
# c = self.__recv()
# if (c != self.INSYNC):
# #print("unexpected 0x%x instead of INSYNC" % ord(c))
# return False;
# c = self.__recv()
# if (c != self.OK):
# #print("unexpected 0x%x instead of OK" % ord(c))
# return False
# return True
# send the GET_DEVICE command and wait for an info parameter
def __getInfo(self, param):
@@ -172,10 +174,16 @@ class uploader(object):
self.__send(uploader.CHIP_ERASE
+ uploader.EOC)
# erase is very slow, give it 10s
old_timeout = self.port.timeout
self.port.timeout = 10
self.__getSync()
self.port.timeout = old_timeout
deadline = time.time() + 10
while time.time() < deadline:
try:
self.__getSync()
return
except RuntimeError as ex:
# we timed out, that's OK
continue
raise RuntimeError("timed out waiting for erase")
# send a PROG_MULTI command to write a collection of bytes
def __program_multi(self, data):
@@ -190,6 +198,7 @@ class uploader(object):
self.__send(uploader.READ_MULTI
+ chr(len(data))
+ uploader.EOC)
self.port.flush()
programmed = self.__recv(len(data))
if (programmed != data):
print("got " + binascii.hexlify(programmed))
@@ -232,7 +241,8 @@ class uploader(object):
# get the bootloader protocol ID first
bl_rev = self.__getInfo(uploader.INFO_BL_REV)
if bl_rev != uploader.BL_REV:
if (bl_rev < uploader.BL_REV_MIN) or (bl_rev > uploader.BL_REV_MAX):
print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
raise RuntimeError("Bootloader protocol mismatch")
self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
apps/ChangeLog.txt
Executable → Regular
+31
View File
@@ -305,3 +305,34 @@
netutils/webserver/httpd_fsdata.c has been replaced with a dynamically
built configuration located at apps/examples/uip (Contributed by
Max Holtzberg).
* apps/netutils/webserver: Several inenhancements from Kate including the
ability to elide scripting and SERVER headers and the ability to map
files into memory before transferring them.
* apps/netutils/webserver: Add ability to map a URL to CGI function.
Contributed by Kate.
* apps/nshlib/nsh_mntcmds.c: The changes of 6.21 introduced holes in the
error handling: Now the number of arguments to mount can be 0 or 4.
Additional parameter checking is required to prevent mysterious errors
(submiteed by Kate).
* apps/netutils/webserver/httpd_mmap.c: Fix errors when the mmap()
length is zero (submitted by Kate).
* apps/netutils/webserver/httpd_sendfile.c: Add and option,
CONFIG_NETUTILS_HTTPD_SENDFILE to transfer files using the NuttX
sendfile() interface.
* apps/netutils/discover: A UDP network discovery utility contributed
by Max Holtzberg.
* apps/examples/discover: A test example for the UDP network discovery
utility (also contribed by Max Holtzberg).
* apps/examples/*/main.c: Too many files called main.c. Each renamed
to something unique so that they will not collide in the archive.
* apps/netutils/xmlrpc: The Embeddable Lightweight XML-RPC Server
discussed at http://www.drdobbs.com/web-development/\
an-embeddable-lightweight-xml-rpc-server/184405364. Contributed by
Max Holtzberg.
* apps/netutils/uip_listenon.c: Logic in uip_server.c that creates
the listening socket was moved to this new file to support re-use.
Contributed by Kate.
* apps/netutils/webserver/httpd.c: The option CONFIG_NETUTILS_HTTPD_SINGLECONNECT
can now be used to limit the server to a single thread. Option
CONFIG_NETUTILS_HTTPD_TIMEOUT can be used to generate HTTP 408 errors.
Both from Kate.
apps/Make.defs
+1 -1
View File
@@ -3,7 +3,7 @@
# Common make definitions provided to all applications
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/ardrone_control/.context
View File
apps/ardrone_control/ardrone_control.c
-321
View File
@@ -1,321 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file Implementation of AR.Drone 1.0 / 2.0 control interface
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <errno.h>
#include <debug.h>
#include <termios.h>
#include <time.h>
#include <sys/prctl.h>
#include <arch/board/up_hrt.h>
#include "ardrone_control.h"
#include "attitude_control.h"
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int ardrone_control_main(int argc, char *argv[]);
// static void turn_xy_plane(const float_vect3 *vector, float yaw,
// float_vect3 *result);
// static void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
// float_vect3 *result);
// static void turn_xy_plane(const float_vect3 *vector, float yaw,
// float_vect3 *result)
// {
// //turn clockwise
// static uint16_t counter;
// result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
// result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
// result->z = vector->z; //leave direction normal to xy-plane untouched
// counter++;
// }
// static void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
// float_vect3 *result)
// {
// turn_xy_plane(vector, yaw, result);
// // result->x = vector->x;
// // result->y = vector->y;
// // result->z = vector->z;
// // result->x = cos(yaw) * vector->x + sin(yaw) * vector->y;
// // result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y;
// // result->z = vector->z; //leave direction normal to xy-plane untouched
// }
int ardrone_control_main(int argc, char *argv[])
{
/* welcome user */
printf("[ardrone_control] Control started, taking over motors\n");
/* default values for arguments */
char *ardrone_uart_name = "/dev/ttyS1";
/* File descriptors */
int ardrone_write;
int gpios;
enum {
CONTROL_MODE_RATES = 0,
CONTROL_MODE_ATTITUDE = 1,
} control_mode = CONTROL_MODE_ATTITUDE;
char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
bool motor_test_mode = false;
/* read commandline arguments */
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
if (argc > i + 1) {
ardrone_uart_name = argv[i + 1];
} else {
printf(commandline_usage);
return ERROR;
}
} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
if (argc > i + 1) {
if (strcmp(argv[i + 1], "rates") == 0) {
control_mode = CONTROL_MODE_RATES;
} else if (strcmp(argv[i + 1], "attitude") == 0) {
control_mode = CONTROL_MODE_ATTITUDE;
} else {
printf(commandline_usage);
return ERROR;
}
} else {
printf(commandline_usage);
return ERROR;
}
} else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
}
/* open uarts */
printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name);
ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_control] Failed opening AR.Drone UART, exiting.\n");
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, &gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_control] Failed initializing AR.Drone motors, exiting.\n");
exit(ERROR);
}
/* Led animation */
int counter = 0;
int led_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
if (state.state_machine == SYSTEM_STATE_MANUAL ||
state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED ||
state.state_machine == SYSTEM_STATE_AUTO ||
state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
state.state_machine == SYSTEM_STATE_EMCY_LANDING ||
motor_test_mode) {
if (control_mode == CONTROL_MODE_RATES) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
control_rates(ardrone_write, &raw, &setpoint);
} else if (control_mode == CONTROL_MODE_ATTITUDE) {
// XXX Add failsafe logic for RC loss situations
/* hardcore, last-resort safety checking */
//if (status->rc_signal_lost) {
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
/* get a local copy of attitude setpoint */
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
att_sp.yaw_body = -manual.yaw * M_PI_F;
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.3f;
} else {
if (state.state_machine == SYSTEM_STATE_MANUAL ||
state.state_machine == SYSTEM_STATE_GROUND_READY ||
state.state_machine == SYSTEM_STATE_STABILIZED ||
state.state_machine == SYSTEM_STATE_AUTO ||
state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
att_sp.thrust = manual.throttle;
} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
/* immediately cut off motors */
att_sp.thrust = 0.0f;
} else {
/* limit motor throttle to zero for an unknown mode */
att_sp.thrust = 0.0f;
}
}
float roll_control, pitch_control, yaw_control, thrust_control;
multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
} else {
/* invalid mode, complain */
if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n");
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
if (counter % 30 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
led_counter++;
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 200 Hz */
usleep(5000);
// This is a hardcore debug code piece to validate
// the motor interface
// uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
// ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20);
// write(ardrone_write, motorSpeedBuf, 5);
// usleep(15000);
counter++;
}
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
printf("[ardrone_control] ending now...\n");
fflush(stdout);
return OK;
}
apps/ardrone_control/ardrone_control.h
-12
View File
@@ -1,12 +0,0 @@
/*
* ardrone_control.h
*
* Created on: Mar 23, 2012
* Author: thomasgubler
*/
#ifndef ARDRONE_CONTROL_H_
#define ARDRONE_CONTROL_H_
#endif /* ARDRONE_CONTROL_H_ */
apps/ardrone_control/ardrone_motor_control.c
-299
View File
@@ -1,299 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file ardrone_motor_control.c
* Implementation of AR.Drone 1.0 / 2.0 motor control interface
*/
#include "ardrone_motor_control.h"
static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
typedef union {
uint16_t motor_value;
uint8_t bytes[2];
} motor_union_t;
/**
* @brief Generate the 8-byte motor set packet
*
* @return the number of bytes (8)
*/
void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
{
motor_buf[0] = 0x20;
motor_buf[1] = 0x00;
motor_buf[2] = 0x00;
motor_buf[3] = 0x00;
motor_buf[4] = 0x00;
/*
* {0x20, 0x00, 0x00, 0x00, 0x00};
* 0x20 is start sign / motor command
*/
motor_union_t curr_motor;
uint16_t nineBitMask = 0x1FF;
/* Set motor 1 */
curr_motor.motor_value = (motor1 & nineBitMask) << 4;
motor_buf[0] |= curr_motor.bytes[1];
motor_buf[1] |= curr_motor.bytes[0];
/* Set motor 2 */
curr_motor.motor_value = (motor2 & nineBitMask) << 3;
motor_buf[1] |= curr_motor.bytes[1];
motor_buf[2] |= curr_motor.bytes[0];
/* Set motor 3 */
curr_motor.motor_value = (motor3 & nineBitMask) << 2;
motor_buf[2] |= curr_motor.bytes[1];
motor_buf[3] |= curr_motor.bytes[0];
/* Set motor 4 */
curr_motor.motor_value = (motor4 & nineBitMask) << 1;
motor_buf[3] |= curr_motor.bytes[1];
motor_buf[4] |= curr_motor.bytes[0];
}
void ar_enable_broadcast(int fd)
{
ar_select_motor(fd, 0);
}
int ar_multiplexing_init()
{
int fd;
fd = open(GPIO_DEVICE_PATH, 0);
if (fd < 0) {
printf("GPIO: open fail\n");
return fd;
}
/* deactivate all outputs */
int ret = 0;
ret += ioctl(fd, GPIO_SET, motor_gpios);
if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
printf("GPIO: output set fail\n");
close(fd);
return -1;
}
if (ret < 0) {
printf("GPIO: clearing pins fail\n");
close(fd);
return -1;
}
return fd;
}
int ar_multiplexing_deinit(int fd)
{
if (fd < 0) {
printf("GPIO: no valid descriptor\n");
return fd;
}
int ret = 0;
/* deselect motor 1-4 */
ret += ioctl(fd, GPIO_SET, motor_gpios);
if (ret != 0) {
printf("GPIO: clear failed %d times\n", ret);
}
if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
printf("GPIO: input set fail\n");
return -1;
}
close(fd);
return ret;
}
int ar_select_motor(int fd, uint8_t motor)
{
int ret = 0;
unsigned long gpioset;
/*
* Four GPIOS:
* GPIO_EXT1
* GPIO_EXT2
* GPIO_UART2_CTS
* GPIO_UART2_RTS
*/
/* select motor 0 to enable broadcast */
if (motor == 0) {
/* select motor 1-4 */
ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
} else {
/* deselect all */
ret += ioctl(fd, GPIO_SET, motor_gpios);
/* select reqested motor */
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
/* deselect all others */
// gpioset = motor_gpios ^ motor_gpio[motor - 1];
// ret += ioctl(fd, GPIO_SET, gpioset);
}
return ret;
}
int ar_init_motors(int ardrone_uart, int *gpios_pin)
{
/* Initialize multiplexing */
*gpios_pin = ar_multiplexing_init();
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
/* initialize all motors
* - select one motor at a time
* - configure motor
*/
int i;
int errcounter = 0;
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
initbuf[3] = i;
errcounter += ar_select_motor(*gpios_pin, i);
write(ardrone_uart, initbuf + 0, 1);
/* sleep 400 ms */
usleep(200000);
usleep(200000);
write(ardrone_uart, initbuf + 1, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 2, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 3, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 4, 1);
/* wait 50 ms */
usleep(50000);
/* enable multicast */
write(ardrone_uart, multicastbuf + 0, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 1, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 2, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 3, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 4, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 5, 1);
/* wait 5 ms */
usleep(50000);
}
/* start the multicast part */
errcounter += ar_select_motor(*gpios_pin, 0);
if (errcounter != 0) {
fprintf(stderr, "[ar motors] init sequence incomplete, failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
}
/*
* Sets the leds on the motor controllers, 1 turns led on, 0 off.
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
{
/*
* 2 bytes are sent. The first 3 bits describe the command: 011 means led control
* the following 4 bits are the red leds for motor 4, 3, 2, 1
* then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
* the last bit is unknown.
*
* The packet is therefore:
* 011 rrrr 0000 gggg 0
*/
uint8_t leds[2];
leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
write(ardrone_uart, leds, 2);
}
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
const int min_motor_interval = 20000;
static uint64_t last_motor_time = 0;
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
uint8_t buf[5] = {0};
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret;
if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) {
return OK;
} else {
return ret;
}
} else {
return -ERROR;
}
}
apps/ardrone_control/ardrone_motor_control.h
-76
View File
@@ -1,76 +0,0 @@
/****************************************************************************
* px4/ardrone_offboard_control.h
*
* Copyright (C) 2012 PX4 Autopilot Project. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <nuttx/config.h>
#include <pthread.h>
#include <poll.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_gpio.h>
/**
* Generate the 5-byte motor set packet.
*
* @return the number of bytes (5)
*/
void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
/**
* Select a motor in the multiplexing.
*/
int ar_select_motor(int fd, uint8_t motor);
void ar_enable_broadcast(int fd);
int ar_multiplexing_init(void);
int ar_multiplexing_deinit(int fd);
/**
* Write four motor commands to an already initialized port.
*
* Writing 0 stops a motor, values from 1-512 encode the full thrust range.
* on some motor controller firmware revisions a minimum value of 10 is
* required to spin the motors.
*/
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
/**
* Initialize the motors.
*/
int ar_init_motors(int ardrone_uart, int *gpios_pin);
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
apps/ardrone_control/attitude_control.c
-355
View File
@@ -1,355 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file attitude_control.c
* Implementation of attitude controller
*/
#include "attitude_control.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include "ardrone_motor_control.h"
#include <float.h>
#include <math.h>
#include "pid.h"
#include <arch/board/up_hrt.h>
#define MAX_MOTOR_COUNT 16
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
struct actuator_controls_s *actuators, bool verbose)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
static PID_t yaw_pos_controller;
static PID_t yaw_speed_controller;
static PID_t pitch_controller;
static PID_t roll_controller;
static float pid_yawpos_lim;
static float pid_yawspeed_lim;
static float pid_att_lim;
static bool initialized = false;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
pid_init(&yaw_pos_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU],
PID_MODE_DERIVATIV_CALC, 154);
pid_init(&yaw_speed_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
PID_MODE_DERIVATIV_CALC, 155);
pid_init(&pitch_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
PID_MODE_DERIVATIV_SET, 156);
pid_init(&roll_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
PID_MODE_DERIVATIV_SET, 157);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
initialized = true;
}
/* load new parameters with lower rate */
if (motor_skip_counter % 50 == 0) {
pid_set_parameters(&yaw_pos_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
pid_set_parameters(&yaw_speed_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
pid_set_parameters(&pitch_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_set_parameters(&roll_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
pid_yawspeed_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
}
/*Calculate Controllers*/
//control Nick
float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET],
att->pitch, att->pitchspeed, deltaT);
//control Roll
float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET],
att->roll, att->rollspeed, deltaT);
//control Yaw Speed
float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); //attitude_setpoint_bodyframe.z is yaw speed!
/*
* compensate the vertical loss of thrust
* when thrust plane has an angle.
* start with a factor of 1.0 (no change)
*/
float zcompensation = 1.0f;
if (fabsf(att->roll) > 1.0f) {
zcompensation *= 1.85081571768f;
} else {
zcompensation *= 1.0f / cosf(att->roll);
}
if (fabsf(att->pitch) > 1.0f) {
zcompensation *= 1.85081571768f;
} else {
zcompensation *= 1.0f / cosf(att->pitch);
}
float motor_thrust = 0.0f;
// FLYING MODES
motor_thrust = att_sp->thrust;
//printf("mot0: %3.1f\n", motor_thrust);
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
/* limit yaw rate output */
if (yaw_rate_control > pid_yawspeed_lim) {
yaw_rate_control = pid_yawspeed_lim;
yaw_speed_controller.saturated = 1;
}
if (yaw_rate_control < -pid_yawspeed_lim) {
yaw_rate_control = -pid_yawspeed_lim;
yaw_speed_controller.saturated = 1;
}
if (pitch_control > pid_att_lim) {
pitch_control = pid_att_lim;
pitch_controller.saturated = 1;
}
if (pitch_control < -pid_att_lim) {
pitch_control = -pid_att_lim;
pitch_controller.saturated = 1;
}
if (roll_control > pid_att_lim) {
roll_control = pid_att_lim;
roll_controller.saturated = 1;
}
if (roll_control < -pid_att_lim) {
roll_control = -pid_att_lim;
roll_controller.saturated = 1;
}
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
}
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) {
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
unsigned int motor_skip_counter = 0;
const float min_thrust = 0.02f; /**< 2% minimum thrust */
const float max_thrust = 1.0f; /**< 100% max thrust */
const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */
const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */
const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */
/* initialize all fields to zero */
uint16_t motor_pwm[MAX_MOTOR_COUNT] = {0};
float motor_calc[MAX_MOTOR_COUNT] = {0};
float output_band = 0.0f;
float band_factor = 0.75f;
const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */
float yaw_factor = 1.0f;
if (motor_thrust <= min_thrust) {
motor_thrust = min_thrust;
output_band = 0.0f;
} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
output_band = band_factor * (motor_thrust - min_thrust);
} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * startpoint_full_control;
} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
output_band = band_factor * (max_thrust - motor_thrust);
}
if (verbose && motor_skip_counter % 100 == 0) {
printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control);
}
//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
// if we are not in the output band
if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
&& motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
&& motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
&& motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
yaw_factor = 0.5f;
// FRONT (MOTOR 1)
motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
// RIGHT (MOTOR 2)
motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
// BACK (MOTOR 3)
motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
// LEFT (MOTOR 4)
motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
}
if (verbose && motor_skip_counter % 100 == 0) {
printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]);
}
for (int i = 0; i < 4; i++) {
//check for limits
if (motor_calc[i] < motor_thrust - output_band) {
motor_calc[i] = motor_thrust - output_band;
}
if (motor_calc[i] > motor_thrust + output_band) {
motor_calc[i] = motor_thrust + output_band;
}
}
if (verbose && motor_skip_counter % 100 == 0) {
printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]);
}
/* set the motor values */
/* scale up from 0..1 to 10..512) */
motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
if (verbose && motor_skip_counter % 100 == 0) {
printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
}
/* Keep motors spinning while armed and prevent overflows */
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
/* send motors via UART */
if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
motor_skip_counter++;
}
apps/ardrone_control/pid.c
-109
View File
@@ -1,109 +0,0 @@
#include "pid.h"
#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
uint8_t mode, uint8_t plot_i)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->mode = mode;
pid->plot_i = plot_i;
pid->count = 0;
pid->saturated = 0;
pid->sp = 0;
pid->error_previous = 0;
pid->integral = 0;
}
void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
// pid->mode = mode;
// pid->sp = 0;
// pid->error_previous = 0;
// pid->integral = 0;
}
//void pid_set(PID_t *pid, float sp)
//{
// pid->sp = sp;
// pid->error_previous = 0;
// pid->integral = 0;
//}
/**
*
* @param pid
* @param val
* @param dt
* @return
*/
float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
wait(dt)
goto start
*/
float i, d;
pid->sp = sp;
float error = pid->sp - val;
if (pid->saturated && (pid->integral * error > 0)) {
//Output is saturated and the integral would get bigger (positive or negative)
i = pid->integral;
//Reset saturation. If we are still saturated this will be set again at output limit check.
pid->saturated = 0;
} else {
i = pid->integral + (error * dt);
}
// Anti-Windup. Needed if we don't use the saturation above.
if (pid->intmax != 0.0) {
if (i > pid->intmax) {
pid->integral = pid->intmax;
} else if (i < -pid->intmax) {
pid->integral = -pid->intmax;
} else {
pid->integral = i;
}
//Send Controller integrals
// Disabled because of new possibilities with debug_vect.
// Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
// if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
// {
// mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
// }
}
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / dt;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
} else {
d = 0;
}
pid->error_previous = error;
return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
}
apps/ardrone_control/pid.h
-40
View File
@@ -1,40 +0,0 @@
/*
* pid.h
*
* Created on: May 29, 2012
* Author: thomasgubler
*/
#ifndef PID_H_
#define PID_H_
#include <stdint.h>
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1
typedef struct {
float kp;
float ki;
float kd;
float intmax;
float sp;
float integral;
float error_previous;
uint8_t mode;
uint8_t plot_i;
uint8_t count;
uint8_t saturated;
} PID_t;
void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
//void pid_set(PID_t *pid, float sp);
float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
#endif /* PID_H_ */
apps/ardrone_control/position_control.c
-308
View File
@@ -1,308 +0,0 @@
/****************************************************************************
* ardrone_control/position_control.c
*
* Copyright (C) 2008, 2012 Thomas Gubler, Julian Oes, Lorenz Meier. All rights reserved.
* Author: Based on the pixhawk quadrotor controller
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "position_control.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <stdbool.h>
#include <float.h>
#include "pid.h"
#ifndef FM_PI
#define FM_PI 3.1415926535897932384626433832795f
#endif
#define CONTROL_PID_POSITION_INTERVAL 0.020
int read_lock_position(global_data_position_t *position)
{
static int ret;
ret = global_data_wait(&global_data_position->access_conf);
if (ret == 0) {
memcpy(&position->lat, &global_data_position->lat, sizeof(global_data_position->lat));
memcpy(&position->lon, &global_data_position->lon, sizeof(global_data_position->lon));
memcpy(&position->alt, &global_data_position->alt, sizeof(global_data_position->alt));
memcpy(&position->relative_alt, &global_data_position->relative_alt, sizeof(global_data_position->relative_alt));
memcpy(&position->vx, &global_data_position->vx, sizeof(global_data_position->vx));
memcpy(&position->vy, &global_data_position->vy, sizeof(global_data_position->vy));
memcpy(&position->vz, &global_data_position->vz, sizeof(global_data_position->vz));
memcpy(&position->hdg, &global_data_position->hdg, sizeof(global_data_position->hdg));
} else {
printf("Controller timeout, no new position values available\n");
}
global_data_unlock(&global_data_position->access_conf);
return ret;
}
float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
const double radius_earth = 6371000.0;
return radius_earth * c;
}
float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
{
double lat_now_rad = lat_now / 180.0 * M_PI;
double lon_now_rad = lon_now / 180.0 * M_PI;
double lat_next_rad = lat_next / 180.0 * M_PI;
double lon_next_rad = lon_next / 180.0 * M_PI;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
if (theta < 0) {
theta = theta + 2 * FM_PI;
}
return theta;
}
void control_position(void)
{
static PID_t distance_controller;
static int read_ret;
static global_data_position_t position_estimated;
static uint16_t counter;
static bool initialized;
static uint16_t pm_counter;
static float lat_next;
static float lon_next;
static float pitch_current;
static float thrust_total;
if (initialized == false) {
global_data_lock(&global_data_parameter_storage->access_conf);
pid_init(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
PID_MODE_DERIVATIV_CALC, 150);//150
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
global_data_unlock(&global_data_parameter_storage->access_conf);
thrust_total = 0.0f;
/* Position initialization */
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
lat_next = position_estimated.lat;
lon_next = position_estimated.lon;
/* attitude initialization */
global_data_lock(&global_data_attitude->access_conf);
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
initialized = true;
}
/* load new parameters with 10Hz */
if (counter % 50 == 0) {
if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
/* check whether new parameters are available */
if (global_data_parameter_storage->counter > pm_counter) {
pid_set_parameters(&distance_controller,
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
//
// pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
// pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
pm_counter = global_data_parameter_storage->counter;
printf("Position controller changed pid parameters\n");
}
}
global_data_unlock(&global_data_parameter_storage->access_conf);
}
/* Wait for new position estimate */
do {
read_ret = read_lock_position(&position_estimated);
} while (read_ret != 0);
/* Get next waypoint */ //TODO: add local copy
if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
lat_next = global_data_position_setpoint->x;
lon_next = global_data_position_setpoint->y;
global_data_unlock(&global_data_position_setpoint->access_conf);
}
/* Get distance to waypoint */
float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
// if(counter % 5 == 0)
// printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
/* Get bearing to waypoint (direction on earth surface to next waypoint) */
float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
if (counter % 5 == 0)
printf("bearing: %.4f\n", bearing);
/* Calculate speed in direction of bearing (needed for controller) */
float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy);
// if(counter % 5 == 0)
// printf("speed_norm: %.4f\n", speed_norm);
float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
/* Control Thrust in bearing direction */
float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
// if(counter % 5 == 0)
// printf("horizontal thrust: %.4f\n", horizontal_thrust);
/* Get total thrust (from remote for now) */
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum?
global_data_unlock(&global_data_rc_channels->access_conf);
}
const float max_gas = 500.0f;
thrust_total *= max_gas / 20000.0f; //TODO: check this
thrust_total += max_gas / 2.0f;
if (horizontal_thrust > thrust_total) {
horizontal_thrust = thrust_total;
} else if (horizontal_thrust < -thrust_total) {
horizontal_thrust = -thrust_total;
}
//TODO: maybe we want to add a speed controller later...
/* Calclulate thrust in east and north direction */
float thrust_north = cosf(bearing) * horizontal_thrust;
float thrust_east = sinf(bearing) * horizontal_thrust;
if (counter % 10 == 0) {
printf("thrust north: %.4f\n", thrust_north);
printf("thrust east: %.4f\n", thrust_east);
fflush(stdout);
}
/* Get current attitude */
if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
pitch_current = global_data_attitude->pitch;
global_data_unlock(&global_data_attitude->access_conf);
}
/* Get desired pitch & roll */
float pitch_desired = 0.0f;
float roll_desired = 0.0f;
if (thrust_total != 0) {
float pitch_fraction = -thrust_north / thrust_total;
float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
if (roll_fraction < -1) {
roll_fraction = -1;
} else if (roll_fraction > 1) {
roll_fraction = 1;
}
// if(counter % 5 == 0)
// {
// printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
// fflush(stdout);
// }
pitch_desired = asinf(pitch_fraction);
roll_desired = asinf(roll_fraction);
}
/*Broadcast desired angles */
global_data_lock(&global_data_ardrone_control->access_conf);
global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[0] = roll_desired;
global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[1] = pitch_desired;
global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[2] = bearing; //TODO: add yaw setpoint
global_data_unlock(&global_data_ardrone_control->access_conf);
global_data_broadcast(&global_data_ardrone_control->access_conf);
counter++;
}
apps/ardrone_control/position_control.h
-13
View File
@@ -1,13 +0,0 @@
/*
* position_control.h
*
* Created on: May 29, 2012
* Author: thomasgubler
*/
#ifndef POSITION_CONTROL_H_
#define POSITION_CONTROL_H_
void control_position(void);
#endif /* POSITION_CONTROL_H_ */
apps/ardrone_control/rate_control.c
-320
View File
@@ -1,320 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Tobias Naegeli <nagelit@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file rate_control.c
* Implementation of attitude rate control
*/
#include "rate_control.h"
#include "ardrone_motor_control.h"
#include <arch/board/up_hrt.h>
extern int ardrone_write;
extern int gpios;
typedef struct {
uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration
uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration
uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration
uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration
uint8_t target_system; ///< System ID of the system that should set these motor commands
} quad_motors_setpoint_t;
void control_rates(int ardrone_write, struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints)
{
static quad_motors_setpoint_t actuators_desired;
//static quad_motors_setpoint_t quad_motors_setpoint_desired;
static int16_t outputBand = 0;
// static uint16_t control_counter;
static hrt_abstime now_time;
static hrt_abstime last_time;
static float setpointXrate;
static float setpointYrate;
static float setpointZrate;
static float setpointRateCast[3];
static float Kp;
// static float Ki;
static float setpointThrustCast;
static float startpointFullControll;
static float maxThrustSetpoints;
static float gyro_filtered[3];
static float gyro_filtered_offset[3];
static float gyro_alpha;
static float gyro_alpha_offset;
// static float errXrate;
static float attRatesScaled[3];
static uint16_t offsetCnt;
// static float antiwindup;
static int motor_skip_counter;
static int read_ret;
static bool initialized;
if (initialized == false) {
initialized = true;
/* Read sensors for initial values */
gyro_filtered_offset[0] = 0.00026631611f * (float)raw->gyro_raw[0];
gyro_filtered_offset[1] = 0.00026631611f * (float)raw->gyro_raw[1];
gyro_filtered_offset[2] = 0.00026631611f * (float)raw->gyro_raw[2];
gyro_filtered[0] = 0.00026631611f * (float)raw->gyro_raw[0];
gyro_filtered[1] = 0.00026631611f * (float)raw->gyro_raw[1];
gyro_filtered[2] = 0.00026631611f * (float)raw->gyro_raw[2];
outputBand = 0;
startpointFullControll = 150.0f;
maxThrustSetpoints = 511.0f;
//Kp=60;
//Kp=40.0f;
//Kp=45;
Kp = 30.0f;
// Ki=0.0f;
// antiwindup=50.0f;
}
/* Get setpoint */
//Rate Controller
setpointRateCast[0] = -((float)setpoints->motor_right_ne - 9999.0f) * 0.01f / 180.0f * 3.141f;
setpointRateCast[1] = -((float)setpoints->motor_front_nw - 9999.0f) * 0.01f / 180.0f * 3.141f;
setpointRateCast[2] = 0; //-((float)setpoints->motor_back_se-9999.0f)*0.01f;
//Ki=actuatorDesired.motorRight_NE*0.001f;
setpointThrustCast = setpoints->motor_left_sw;
attRatesScaled[0] = 0.000317603994f * (float)raw->gyro_raw[0];
attRatesScaled[1] = 0.000317603994f * (float)raw->gyro_raw[1];
attRatesScaled[2] = 0.000317603994f * (float)raw->gyro_raw[2];
//filtering of the gyroscope values
//compute filter coefficient alpha
//gyro_alpha=0.005/(2.0f*3.1415f*200.0f+0.005f);
//gyro_alpha=0.009;
gyro_alpha = 0.09f;
gyro_alpha_offset = 0.001f;
//gyro_alpha=0.001;
//offset estimation and filtering
offsetCnt++;
uint8_t i;
for (i = 0; i < 3; i++) {
if (offsetCnt < 5000) {
gyro_filtered_offset[i] = attRatesScaled[i] * gyro_alpha_offset + gyro_filtered_offset[i] * (1 - gyro_alpha_offset);
}
gyro_filtered[i] = 1.0f * ((attRatesScaled[i] - gyro_filtered_offset[i]) * gyro_alpha + gyro_filtered[i] * (1 - gyro_alpha)) - 0 * setpointRateCast[i];
}
// //START DEBUG
// /* write filtered values to global_data_attitude */
// global_data_attitude->rollspeed = gyro_filtered[0];
// global_data_attitude->pitchspeed = gyro_filtered[1];
// global_data_attitude->yawspeed = gyro_filtered[2];
// //END DEBUG
//rate controller
//X-axis
setpointXrate = -Kp * (setpointRateCast[0] - gyro_filtered[0]);
//Y-axis
setpointYrate = -Kp * (setpointRateCast[1] - gyro_filtered[1]);
//Z-axis
setpointZrate = -Kp * (setpointRateCast[2] - gyro_filtered[2]);
//Mixing
if (setpointThrustCast <= 0) {
setpointThrustCast = 0;
outputBand = 0;
}
if ((setpointThrustCast < startpointFullControll) && (setpointThrustCast > 0)) {
outputBand = 0.75f * setpointThrustCast;
}
if ((setpointThrustCast >= startpointFullControll) && (setpointThrustCast < maxThrustSetpoints - 0.75f * startpointFullControll)) {
outputBand = 0.75f * startpointFullControll;
}
if (setpointThrustCast >= maxThrustSetpoints - 0.75f * startpointFullControll) {
setpointThrustCast = 0.75f * startpointFullControll;
outputBand = 0.75f * startpointFullControll;
}
actuators_desired.motor_front_nw = setpointThrustCast + (setpointXrate + setpointYrate + setpointZrate);
actuators_desired.motor_right_ne = setpointThrustCast + (-setpointXrate + setpointYrate - setpointZrate);
actuators_desired.motor_back_se = setpointThrustCast + (-setpointXrate - setpointYrate + setpointZrate);
actuators_desired.motor_left_sw = setpointThrustCast + (setpointXrate - setpointYrate - setpointZrate);
if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) > (setpointThrustCast + outputBand)) {
actuators_desired.motor_front_nw = setpointThrustCast + outputBand;
}
if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) < (setpointThrustCast - outputBand)) {
actuators_desired.motor_front_nw = setpointThrustCast - outputBand;
}
if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) > (setpointThrustCast + outputBand)) {
actuators_desired.motor_right_ne = setpointThrustCast + outputBand;
}
if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) < (setpointThrustCast - outputBand)) {
actuators_desired.motor_right_ne = setpointThrustCast - outputBand;
}
if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) > (setpointThrustCast + outputBand)) {
actuators_desired.motor_back_se = setpointThrustCast + outputBand;
}
if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) < (setpointThrustCast - outputBand)) {
actuators_desired.motor_back_se = setpointThrustCast - outputBand;
}
if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) > (setpointThrustCast + outputBand)) {
actuators_desired.motor_left_sw = setpointThrustCast + outputBand;
}
if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) < (setpointThrustCast - outputBand)) {
actuators_desired.motor_left_sw = setpointThrustCast - outputBand;
}
//printf("%lu,%lu,%lu,%lu\n",actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
if (motor_skip_counter % 5 == 0) {
uint8_t motorSpeedBuf[5];
ar_get_motor_packet(motorSpeedBuf, actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
// uint8_t* motorSpeedBuf = ar_get_motor_packet(1, 1, 1, 1);
// if(motor_skip_counter %50 == 0)
// {
// if(0==actuators_desired.motor_front_nw || 0 == actuators_desired.motor_right_ne || 0 == actuators_desired.motor_back_se || 0 == actuators_desired.motor_left_sw)
// printf("Motors set: %u, %u, %u, %u\n", actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
// printf("input: %u\n", setpoints->motor_front_nw);
// printf("Roll casted desired: %f, Pitch casted desired: %f, Yaw casted desired: %f\n", setpointRateCast[0], setpointRateCast[1], setpointRateCast[2]);
// }
write(ardrone_write, motorSpeedBuf, 5);
// motor_skip_counter = 0;
}
motor_skip_counter++;
//START DEBUG
// global_data_lock(&global_data_ardrone_control->access_conf);
// global_data_ardrone_control->timestamp = hrt_absolute_time();
// global_data_ardrone_control->gyro_scaled[0] = attRatesScaled[0];
// global_data_ardrone_control->gyro_scaled[1] = attRatesScaled[1];
// global_data_ardrone_control->gyro_scaled[2] = attRatesScaled[2];
// global_data_ardrone_control->gyro_filtered[0] = gyro_filtered[0];
// global_data_ardrone_control->gyro_filtered[1] = gyro_filtered[1];
// global_data_ardrone_control->gyro_filtered[2] = gyro_filtered[2];
// global_data_ardrone_control->gyro_filtered_offset[0] = gyro_filtered_offset[0];
// global_data_ardrone_control->gyro_filtered_offset[1] = gyro_filtered_offset[1];
// global_data_ardrone_control->gyro_filtered_offset[2] = gyro_filtered_offset[2];
// global_data_ardrone_control->setpoint_rate_cast[0] = setpointRateCast[0];
// global_data_ardrone_control->setpoint_rate_cast[1] = setpointRateCast[1];
// global_data_ardrone_control->setpoint_rate_cast[2] = setpointRateCast[2];
// global_data_ardrone_control->setpoint_thrust_cast = setpointThrustCast;
// global_data_ardrone_control->setpoint_rate[0] = setpointXrate;
// global_data_ardrone_control->setpoint_rate[1] = setpointYrate;
// global_data_ardrone_control->setpoint_rate[2] = setpointZrate;
// global_data_ardrone_control->motor_front_nw = actuators_desired.motor_front_nw;
// global_data_ardrone_control->motor_right_ne = actuators_desired.motor_right_ne;
// global_data_ardrone_control->motor_back_se = actuators_desired.motor_back_se;
// global_data_ardrone_control->motor_left_sw = actuators_desired.motor_left_sw;
// global_data_unlock(&global_data_ardrone_control->access_conf);
// global_data_broadcast(&global_data_ardrone_control->access_conf);
//END DEBUG
// gettimeofday(&tv, NULL);
// now = ((uint32_t)tv.tv_sec) * 1000 + tv.tv_usec/1000;
// time_elapsed = now - last_run;
// if (time_elapsed*1000 > CONTROL_LOOP_USLEEP)
// {
// sleep_time = (int32_t)CONTROL_LOOP_USLEEP - ((int32_t)time_elapsed*1000 - (int32_t)CONTROL_LOOP_USLEEP);
//
// if(motor_skip_counter %500 == 0)
// {
// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
// }
// }
//
// if (sleep_time <= 0)
// {
// printf("WARNING: CPU Overload!\n");
// printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
// usleep(CONTROL_LOOP_USLEEP);
// }
// else
// {
// usleep(sleep_time);
// }
// last_run = now;
//
// now_time = hrt_absolute_time();
// if(control_counter % 500 == 0)
// {
// printf("Now: %lu\n",(unsigned long)now_time);
// printf("Last: %lu\n",(unsigned long)last_time);
// printf("Difference: %lu\n", (unsigned long)(now_time - last_time));
// printf("now seconds: %lu\n", (unsigned long)(now_time / 1000000));
// }
// last_time = now_time;
//
// now_time = hrt_absolute_time() / 1000000;
// if(now_time - last_time > 0)
// {
// printf("Counter: %ld\n",control_counter);
// last_time = now_time;
// control_counter = 0;
// }
// control_counter++;
}
apps/ardrone_interface/ardrone_interface.c
+36 -15
View File
@@ -191,12 +191,26 @@ int ardrone_interface_thread_main(int argc, char *argv[])
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
bool motor_test_mode = false;
int test_motor = -1;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
if (i+1 < argc) {
int motor = atoi(argv[i+1]);
if (motor > 0 && motor < 5) {
test_motor = motor;
} else {
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
}
} else {
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
}
struct termios uart_config_original;
@@ -205,18 +219,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(&uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* Led animation */
int counter = 0;
int led_counter = 0;
@@ -237,6 +239,18 @@ int ardrone_interface_thread_main(int argc, char *argv[])
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(&uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
@@ -253,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
//ar_multiplexing_deinit(gpios);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(&uart_config_original);
@@ -279,7 +293,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
if (motor_test_mode) {
/* set motors to idle speed */
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
if (test_motor > 0 && test_motor < 5) {
int motors[4] = {0, 0, 0, 0};
motors[test_motor - 1] = 10;
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
} else {
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
}
} else {
/* MAIN OPERATION MODE */
@@ -292,7 +313,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.failsafe) {
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
apps/ardrone_interface/ardrone_motor_control.c
+33 -12
View File
@@ -43,6 +43,8 @@
#include <unistd.h>
#include <drivers/drv_gpio.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@@ -224,9 +226,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
errcounter += ar_select_motor(gpios, i);
write(ardrone_uart, &(initbuf[3]), 1);
fsync(ardrone_uart);
usleep(200);
/*
* write 0xE0 - request status
@@ -270,29 +270,34 @@ int ar_init_motors(int ardrone_uart, int gpios)
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* sleep 400 ms */
usleep(400000);
/* sleep 200 ms */
usleep(200000);
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
usleep(200);
/*
* first round
* write six times A0 - enable broadcast
* receive nothing
*/
// for (int i = 0; i < sizeof(multicastbuf); i++) {
write(ardrone_uart, multicastbuf, 6);
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
usleep(200000);
write(ardrone_uart, multicastbuf, 6);
/*
* second round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/* set motors to zero speed */
/* set motors to zero speed (fsync is part of the write command */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
fsync(ardrone_uart);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@@ -324,12 +329,28 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
const unsigned int min_motor_interval = 4900;
static uint64_t last_motor_time = 0;
static struct actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
outputs.output[0] = motor1;
outputs.output[1] = motor2;
outputs.output[2] = motor3;
outputs.output[3] = motor4;
static orb_advert_t pub = 0;
if (pub == 0) {
pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
}
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
uint8_t buf[5] = {0};
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret;
ret = write(ardrone_fd, buf, sizeof(buf));
fsync(ardrone_fd);
/* publish just written values */
orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
if (ret == sizeof(buf)) {
return OK;
} else {
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+2 -7
View File
@@ -2,7 +2,6 @@
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
* Laurens Mackay <mackayl@student.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -35,14 +34,10 @@
****************************************************************************/
/*
* @file Extended Kalman Filter for Attitude Estimation
* @file attitude_estimator_ekf_main.c
* Extended Kalman Filter for Attitude Estimation.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
apps/commander/commander.c
+169 -54
View File
@@ -74,6 +74,16 @@
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <arch/board/up_cpuload.h>
extern struct system_load_s system_load;
@@ -294,7 +304,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
mag_minima[2][i] = FLT_MAX;
}
mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions.");
int fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
uint64_t calibration_start = hrt_absolute_time();
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
@@ -426,23 +449,36 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
} else {
/* announce and set new offset */
char offset_output[50];
sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
mavlink_log_info(mavlink_fd, offset_output);
// char offset_output[50];
// sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
if (param_set(param_find("SENSOR_MAG_XOFF"), &(mag_offset[0]))) {
if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) {
fprintf(stderr, "[commander] Setting X mag offset failed!\n");
}
if (param_set(param_find("SENSOR_MAG_YOFF"), &(mag_offset[1]))) {
if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) {
fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENSOR_MAG_ZOFF"), &(mag_offset[2]))) {
if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) {
fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
}
}
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale = {
mag_offset[0],
1.0f,
mag_offset[1],
1.0f,
mag_offset[2],
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
free(mag_maxima[0]);
free(mag_maxima[1]);
free(mag_maxima[2]);
@@ -468,6 +504,20 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
int calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -490,25 +540,39 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (param_set(param_find("SENSOR_GYRO_XOFF"), &(gyro_offset[0]))) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X gyro offset failed!");
}
if (param_set(param_find("SENSOR_GYRO_YOFF"), &(gyro_offset[1]))) {
if (param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Y gyro offset failed!");
}
if (param_set(param_find("SENSOR_GYRO_ZOFF"), &(gyro_offset[2]))) {
if (param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* exit to gyro calibration mode */
status->flag_preflight_gyro_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
char offset_output[50];
sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
mavlink_log_info(mavlink_fd, offset_output);
// char offset_output[50];
// sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined);
}
@@ -531,6 +595,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
int calibration_counter = 0;
float accel_offset[3] = {0.0f, 0.0f, 0.0f};
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -556,28 +633,56 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
/* add the removed length from x / y to z, since we induce a scaling issue else */
float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
/* if length is correct, zero results here */
accel_offset[2] = accel_offset[2] + total_len;
if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) {
float scale = 9.80665f / total_len;
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
}
if (param_set(param_find("SENSOR_ACC_YOFF"), &(accel_offset[1]))) {
if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
}
if (param_set(param_find("SENSOR_ACC_ZOFF"), &(accel_offset[2]))) {
if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
}
if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
}
if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
}
if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
}
fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offset[0],
scale,
accel_offset[1],
scale,
accel_offset[2],
scale,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* exit to gyro calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
char offset_output[50];
sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
(double)accel_offset[1], (double)accel_offset[2]);
mavlink_log_info(mavlink_fd, offset_output);
// char offset_output[50];
// sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0],
// (double)accel_offset[1], (double)accel_offset[2]);
// mavlink_log_info(mavlink_fd, offset_output);
close(sub_sensor_combined);
}
@@ -960,7 +1065,7 @@ int commander_thread_main(int argc, char *argv[])
/* create pthreads */
pthread_attr_t command_handling_attr;
pthread_attr_init(&command_handling_attr);
pthread_attr_setstacksize(&command_handling_attr, 4096);
pthread_attr_setstacksize(&command_handling_attr, 6000);
pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
pthread_attr_t subsystem_info_attr;
@@ -973,7 +1078,7 @@ int commander_thread_main(int argc, char *argv[])
uint8_t flight_env;
/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
float battery_voltage = 12.0f;
bool battery_voltage_valid = true;
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
@@ -1010,6 +1115,8 @@ int commander_thread_main(int argc, char *argv[])
/* now initialized */
commander_initialized = true;
uint64_t start_time = hrt_absolute_time();
while (!thread_should_exit) {
/* Get current values */
@@ -1019,7 +1126,14 @@ int commander_thread_main(int argc, char *argv[])
battery_voltage = sensors.battery_voltage_v;
battery_voltage_valid = sensors.battery_voltage_valid;
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
/*
* Only update battery voltage estimate if voltage is
* valid and system has been running for two and a half seconds
*/
if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
}
/* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
@@ -1084,6 +1198,38 @@ int commander_thread_main(int argc, char *argv[])
ioctl(buzzer, TONE_SET_ALARM, 0);
}
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
}
low_voltage_counter++;
}
/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
}
/* End battery voltage check */
/* Check if last transition deserved an audio event */
#warning This code depends on state that is no longer? maintained
#if 0
@@ -1142,37 +1288,6 @@ int commander_thread_main(int argc, char *argv[])
//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
}
low_voltage_counter++;
}
/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
}
/* End battery voltage check */
/* Start RC state check */
bool prev_lost = current_status.rc_signal_lost;
apps/commander/commander.h
-2
View File
@@ -39,8 +39,6 @@
#ifndef COMMANDER_H_
#define COMMANDER_H_
#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f
#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f
#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
apps/commander/state_machine_helper.c
+11 -4
View File
@@ -220,7 +220,10 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
void publish_armed_status(const struct vehicle_status_s *current_status) {
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
armed.failsafe = current_status->rc_signal_lost;
/* lock down actuators if required */
// XXX FIXME Currently any loss of RC will completely disable all actuators
// needs proper failsafe
armed.lockdown = (current_status->rc_signal_lost || current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -498,6 +501,8 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -559,13 +564,15 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
}
}
/* Switch on HIL if in standby */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
/* Switch on HIL if in standby and not already in HIL mode */
if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
&& !current_status->flag_hil_enabled) {
/* Enable HIL on request */
current_status->flag_hil_enabled = true;
ret = OK;
state_machine_publish(status_pub, current_status, mavlink_fd);
printf("[commander] Enabling HIL\n");
publish_armed_status(current_status);
printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
}
/* NEVER actually switch off HIL without reboot */
apps/drivers/hmc5883/hmc5883.cpp
+9 -7
View File
@@ -527,13 +527,13 @@ HMC5883::start()
_oldest_report = _next_report = 0;
/* schedule a cycle to start things */
work_queue(&_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
}
void
HMC5883::stop()
{
work_cancel(&_work);
work_cancel(HPWORK, &_work);
}
void
@@ -567,7 +567,8 @@ HMC5883::cycle()
if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(&_work,
work_queue(HPWORK,
&_work,
(worker_t)&HMC5883::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL));
@@ -584,7 +585,8 @@ HMC5883::cycle()
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(&_work,
work_queue(HPWORK,
&_work,
(worker_t)&HMC5883::cycle_trampoline,
this,
USEC2TICK(HMC5883_CONVERSION_INTERVAL));
@@ -612,8 +614,8 @@ HMC5883::collect()
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t y[2];
uint8_t z[2];
uint8_t y[2];
} hmc_report;
#pragma pack(pop)
struct {
@@ -833,7 +835,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z);
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
/* set the queue depth to 10 */
@@ -863,7 +865,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z);
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
}
apps/drivers/mpu6000/mpu6000.cpp
+4 -2
View File
@@ -369,8 +369,10 @@ MPU6000::init()
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 98Hz (low pass filter)
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
apps/drivers/ms5611/ms5611.cpp
+28 -26
View File
@@ -541,13 +541,13 @@ MS5611::start()
_oldest_report = _next_report = 0;
/* schedule a cycle to start things */
work_queue(&_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
}
void
MS5611::stop()
{
work_cancel(&_work);
work_cancel(HPWORK, &_work);
}
void
@@ -585,7 +585,8 @@ MS5611::cycle()
(_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(&_work,
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
@@ -602,7 +603,8 @@ MS5611::cycle()
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(&_work,
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
USEC2TICK(MS5611_CONVERSION_INTERVAL));
@@ -713,30 +715,30 @@ MS5611::collect()
* double precision: ms5611_read: 992 events, 258641us elapsed, min 202us max 305us
* single precision: ms5611_read: 963 events, 208066us elapsed, min 202us max 241us
*/
#if 0/* USE_FLOAT */
/* tropospheric properties (0-11km) for standard atmosphere */
const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */
const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */
const float g = 9.80665f; /* gravity constant in m/s/s */
const float R = 287.05f; /* ideal gas constant in J/kg/K */
// /* tropospheric properties (0-11km) for standard atmosphere */
// const float T1 = 15.0f + 273.15f; /* temperature at base height in Kelvin */
// const float a = -6.5f / 1000f; /* temperature gradient in degrees per metre */
// const float g = 9.80665f; /* gravity constant in m/s/s */
// const float R = 287.05f; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
float p1 = _msl_pressure / 1000.0f;
// /* current pressure at MSL in kPa */
// float p1 = _msl_pressure / 1000.0f;
// /* measured pressure in kPa */
// float p = P / 1000.0f;
// /*
// * Solve:
// *
// * / -(aR / g) \
// * | (p / p1) . T1 | - T1
// * \ /
// * h = ------------------------------- + h1
// * a
// */
// _reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* measured pressure in kPa */
float p = P / 1000.0f;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
_reports[_next_report].altitude = (((powf((p / p1), (-(a * R) / g))) * T1) - T1) / a;
#else
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
@@ -759,7 +761,7 @@ MS5611::collect()
* a
*/
_reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
#endif
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
apps/examples/Kconfig
+56 -48
View File
@@ -3,115 +3,115 @@
# see misc/tools/kconfig-language.txt.
#
menu "ADC example"
menu "ADC Example"
source "$APPSDIR/examples/adc/Kconfig"
endmenu
menu "Buttons example"
menu "Buttons Example"
source "$APPSDIR/examples/buttons/Kconfig"
endmenu
menu "CAN example"
menu "CAN Example"
source "$APPSDIR/examples/can/Kconfig"
endmenu
menu "USB CDC/ACM class driver example"
menu "USB CDC/ACM Class Driver Example"
source "$APPSDIR/examples/cdcacm/Kconfig"
endmenu
menu "USB composite class driver example"
menu "USB composite Class Driver Example"
source "$APPSDIR/examples/composite/Kconfig"
endmenu
menu "DHCP server example"
menu "DHCP Server Example"
source "$APPSDIR/examples/dhcpd/Kconfig"
endmenu
menu "FTP client example"
menu "FTP Client Example"
source "$APPSDIR/examples/ftpc/Kconfig"
endmenu
menu "FTP server example"
menu "FTP Server Example"
source "$APPSDIR/examples/ftpd/Kconfig"
endmenu
menu "\"Hello, World!\" example"
menu "\"Hello, World!\" Example"
source "$APPSDIR/examples/hello/Kconfig"
endmenu
menu "\"Hello, World!\" C++ example"
menu "\"Hello, World!\" C++ Example"
source "$APPSDIR/examples/helloxx/Kconfig"
endmenu
menu "USB HID keyboard example"
menu "USB HID Keyboard Example"
source "$APPSDIR/examples/hidkbd/Kconfig"
endmenu
menu "IGMP example"
menu "IGMP Example"
source "$APPSDIR/examples/igmp/Kconfig"
endmenu
menu "LCD read/write example"
menu "LCD Read/Write Example"
source "$APPSDIR/examples/lcdrw/Kconfig"
endmenu
menu "Memory management example"
menu "Memory Management Example"
source "$APPSDIR/examples/mm/Kconfig"
endmenu
menu "File system mount example"
menu "File System Mount Example"
source "$APPSDIR/examples/mount/Kconfig"
endmenu
menu "FreeModBus example"
menu "FreeModBus Example"
source "$APPSDIR/examples/modbus/Kconfig"
endmenu
menu "Network test example"
menu "Network Test Example"
source "$APPSDIR/examples/nettest/Kconfig"
endmenu
menu "NuttShell (NSH) example"
menu "NuttShell (NSH) Example"
source "$APPSDIR/examples/nsh/Kconfig"
endmenu
menu "NULL example"
menu "NULL Example"
source "$APPSDIR/examples/null/Kconfig"
endmenu
menu "NX graphics example"
menu "NX Graphics Example"
source "$APPSDIR/examples/nx/Kconfig"
endmenu
menu "NxConsole example"
menu "NxConsole Example"
source "$APPSDIR/examples/nxconsole/Kconfig"
endmenu
menu "NXFFS file system example"
menu "NXFFS File System Example"
source "$APPSDIR/examples/nxffs/Kconfig"
endmenu
menu "NXFLAT example"
menu "NXFLAT Example"
source "$APPSDIR/examples/nxflat/Kconfig"
endmenu
menu "NX graphics \"Hello, World!\" example"
menu "NX Graphics \"Hello, World!\" Example"
source "$APPSDIR/examples/nxhello/Kconfig"
endmenu
menu "NX graphics image example"
menu "NX Graphics image Example"
source "$APPSDIR/examples/nximage/Kconfig"
endmenu
menu "NX graphics lines example"
menu "NX Graphics lines Example"
source "$APPSDIR/examples/nxlines/Kconfig"
endmenu
menu "NX graphics text example"
menu "NX Graphics Text Example"
source "$APPSDIR/examples/nxtext/Kconfig"
endmenu
menu "OS test example"
menu "OS Test Example"
source "$APPSDIR/examples/ostest/Kconfig"
endmenu
@@ -119,82 +119,90 @@ menu "Pascal \"Hello, World!\"example"
source "$APPSDIR/examples/pashello/Kconfig"
endmenu
menu "Pipe example"
menu "Pipe Example"
source "$APPSDIR/examples/pipe/Kconfig"
endmenu
menu "Poll example"
menu "Poll Example"
source "$APPSDIR/examples/poll/Kconfig"
endmenu
menu "Pulse width modulation (PWM) example"
menu "Pulse Width Modulation (PWM) Example"
source "$APPSDIR/examples/pwm/Kconfig"
endmenu
menu "Quadrature encoder example"
menu "Quadrature Encoder Example"
source "$APPSDIR/examples/qencoder/Kconfig"
endmenu
menu "RGMP example"
menu "RGMP Example"
source "$APPSDIR/examples/rgmp/Kconfig"
endmenu
menu "ROMFS example"
menu "ROMFS Example"
source "$APPSDIR/examples/romfs/Kconfig"
endmenu
menu "sendmail example"
menu "sendmail Example"
source "$APPSDIR/examples/sendmail/Kconfig"
endmenu
menu "Serial loopback example"
menu "Serial Loopback Example"
source "$APPSDIR/examples/serloop/Kconfig"
endmenu
menu "Telnet daemon example"
menu "Telnet Daemon Example"
source "$APPSDIR/examples/telnetd/Kconfig"
endmenu
menu "THTTPD web server example"
menu "THTTPD Web Server Example"
source "$APPSDIR/examples/thttpd/Kconfig"
endmenu
menu "TIFF generation example"
menu "TIFF Generation Example"
source "$APPSDIR/examples/tiff/Kconfig"
endmenu
menu "Touchscreen example"
menu "Touchscreen Example"
source "$APPSDIR/examples/touchscreen/Kconfig"
endmenu
menu "UDP example"
menu "UDP Example"
source "$APPSDIR/examples/udp/Kconfig"
endmenu
menu "uIP web server example"
menu "UDP Discovery Daemon Example"
source "$APPSDIR/examples/discover/Kconfig"
endmenu
menu "uIP Web Server Example"
source "$APPSDIR/examples/uip/Kconfig"
endmenu
menu "USB serial test example"
menu "USB Serial Test Example"
source "$APPSDIR/examples/usbserial/Kconfig"
endmenu
menu "USB mass storage class example"
menu "USB Mass Storage Class Example"
source "$APPSDIR/examples/usbstorage/Kconfig"
endmenu
menu "USB serial terminal example"
menu "USB Serial Terminal Example"
source "$APPSDIR/examples/usbterm/Kconfig"
endmenu
menu "Watchdog timer example"
menu "Watchdog timer Example"
source "$APPSDIR/examples/watchdog/Kconfig"
endmenu
menu "wget example"
menu "wget Example"
source "$APPSDIR/examples/wget/Kconfig"
endmenu
menu "WLAN example"
menu "WLAN Example"
source "$APPSDIR/examples/wlan/Kconfig"
endmenu
menu "XML RPC Example"
source "$APPSDIR/examples/xmlrpc/Kconfig"
endmenu
apps/examples/Make.defs
+8
View File
@@ -58,6 +58,10 @@ ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
CONFIGURED_APPS += examples/dhcpd
endif
ifeq ($(CONFIG_EXAMPLE_DISCOVER),y)
CONFIGURED_APPS += examples/discover
endif
ifeq ($(CONFIG_EXAMPLES_FTPC),y)
CONFIGURED_APPS += examples/ftpc
endif
@@ -221,3 +225,7 @@ endif
ifeq ($(CONFIG_EXAMPLES_WLAN),y)
CONFIGURED_APPS += examples/wlan
endif
ifeq ($(CONFIG_EXAMPLES_XMLRPC),y)
CONFIGURED_APPS += examples/xmlrpc
endif
apps/examples/Makefile
+8 -6
View File
@@ -37,11 +37,12 @@
# Sub-directories
SUBDIRS = adc buttons can cdcacm composite dhcpd ftpc ftpd hello helloxx \
hidkbd igmp lcdrw mm modbus mount nettest nsh null nx nxconsole nxffs \
nxflat nxhello nximage nxlines nxtext ostest pashello pipe poll pwm \
qencoder rgmp romfs serloop telnetd thttpd tiff touchscreen udp uip \
usbserial sendmail usbstorage usbterm watchdog wget wlan
SUBDIRS = adc buttons can cdcacm composite dhcpd discover ftpc ftpd hello
SUBDIRS += helloxx hidkbd igmp lcdrw mm modbus mount nettest nsh null nx
SUBDIRS += nxconsole nxffs nxflat nxhello nximage nxlines nxtext ostest
SUBDIRS += pashello pipe poll pwm qencoder rgmp romfs serloop telnetd
SUBDIRS += thttpd tiff touchscreen udp uip usbserial sendmail usbstorage
SUBDIRS += usbterm watchdog wget wlan
# Sub-directories that might need context setup. Directories may need
# context setup for a variety of reasons, but the most common is because
@@ -56,7 +57,8 @@ SUBDIRS = adc buttons can cdcacm composite dhcpd ftpc ftpd hello helloxx \
CNTXTDIRS = pwm
ifeq ($(CONFIG_NSH_BUILTIN_APPS),y)
CNTXTDIRS += adc can cdcacm composite ftpd dhcpd modbus nettest qencoder telnetd watchdog
CNTXTDIRS += adc can cdcacm composite discover ftpd dhcpd modbus nettest
CNTXTDIRS += qencoder telnetd watchdog
endif
ifeq ($(CONFIG_EXAMPLES_HELLO_BUILTIN),y)
apps/examples/README.txt
+45 -4
View File
@@ -275,6 +275,28 @@ examples/dhcpd
CONFIGURED_APPS += uiplib
examples/discover
^^^^^^^^^^^^^^^^^
This example exercises netutils/discover utility. This example initializes
and starts the UDP discover daemon. This daemon is useful for discovering
devices in local networks, especially with DHCP configured devices. It
listens for UDP broadcasts which also can include a device class so that
groups of devices can be discovered. It is also possible to address all
classes with a kind of broadcast discover.
This example will automatically be built as an NSH built-in if
CONFIG_NSH_BUILTIN_APPS is selected. Otherwise, it will be a standalone
program with entry point "discover_main".
NuttX configuration settings:
CONFIG_EXAMPLE_DISCOVER_DHCPC - DHCP Client
CONFIG_EXAMPLE_DISCOVER_NOMAC - Use canned MAC address
CONFIG_EXAMPLE_DISCOVER_IPADDR - Target IP address
CONFIG_EXAMPLE_DISCOVER_DRIPADDR - Router IP address
CONFIG_EXAMPLE_DISCOVER_NETMASK - Network Mask
examples/ftpc
^^^^^^^^^^^^^
@@ -1111,7 +1133,7 @@ examples/rgmp
See http://rgmp.sourceforge.net/wiki/index.php/Main_Page for further
At present, the RGMP example folder contains only an empty main.c file.
At present, the RGMP example folder contains only an empty rgmp_main.c file.
examples/romfs
^^^^^^^^^^^^^^
@@ -1646,8 +1668,27 @@ examples/wget
file in the configuration driver with instruction to build applications
like:
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webclient
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webclient
examples/xmlrpc
This example exercises the "Embeddable Lightweight XML-RPC Server" which
is discussed at:
http://www.drdobbs.com/web-development/an-embeddable-lightweight-xml-rpc-server/184405364
Configuration options:
CONFIG_EXAMPLES_XMLRPC_BUFFERSIZE - HTTP buffer size. Default 1024
CONFIG_EXAMPLES_XMLRPC_DHCPC - Use DHCP Client. Default n. Ignored
if CONFIG_NSH_BUILTIN_APPS is selected.
CONFIG_EXAMPLES_XMLRPC_NOMAC - Use Canned MAC Address. Defaul n. Ignored
if CONFIG_NSH_BUILTIN_APPS is selected.
CONFIG_EXAMPLES_XMLRPC_IPADDR - Target IP address. Default 0x0a000002.
Ignored if CONFIG_NSH_BUILTIN_APPS is selected.
CONFIG_EXAMPLES_XMLRPC_DRIPADDR - Default Router IP address (Gateway).
Default 0x0a000001. Ignored if CONFIG_NSH_BUILTIN_APPS is selected.
CONFIG_EXAMPLES_XMLRPC_NETMASK - Network Mask. Default 0xffffff00
Ignored if CONFIG_NSH_BUILTIN_APPS is selected.
apps/examples/buttons/Makefile
+2 -2
View File
@@ -2,7 +2,7 @@
# apps/examples/buttons/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -40,7 +40,7 @@ include $(APPDIR)/Make.defs
# Hello, World! Example
ASRCS =
CSRCS = main.c
CSRCS = buttons_main.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
apps/examples/buttons/main.c → apps/examples/buttons/buttons_main.c
+1 -1
View File
@@ -1,5 +1,5 @@
/****************************************************************************
* examples/buttons/main.c
* examples/buttons/buttons_main.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
apps/examples/can/can_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/can/can_main.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/hello/Makefile
+1 -1
View File
@@ -46,7 +46,7 @@ STACKSIZE = 2048
# Hello, World! Example
ASRCS =
CSRCS = main.c
CSRCS = hello_main.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
apps/examples/hello/main.c → apps/examples/hello/hello_main.c
+1 -1
View File
@@ -1,5 +1,5 @@
/****************************************************************************
* examples/hello/main.c
* examples/hello/hello_main.c
*
* Copyright (C) 2008, 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
apps/examples/helloxx/Makefile
+2 -2
View File
@@ -2,7 +2,7 @@
# apps/examples/helloxx/Makefile
#
# Copyright (C) 2009-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -41,7 +41,7 @@ include $(APPDIR)/Make.defs
ASRCS =
CSRCS =
CXXSRCS = main.cxx
CXXSRCS = helloxx_main.cxx
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
apps/examples/helloxx/main.cxx → apps/examples/helloxx/helloxx_main.cxx
+1 -1
View File
@@ -1,5 +1,5 @@
//***************************************************************************
// examples/helloxx/main.cxx
// examples/helloxx/helloxx_main.cxx
//
// Copyright (C) 2009, 2011-2012 Gregory Nutt. All rights reserved.
// Author: Gregory Nutt <gnutt@nuttx.org>
apps/examples/lcdrw/lcdrw_main.c
-253
View File
@@ -1,253 +0,0 @@
/****************************************************************************
* examples/lcdrw/lcdrw_main.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <nuttx/lcd/lcd.h>
#include <nuttx/nx/nxglib.h>
/****************************************************************************
* Definitions
****************************************************************************/
/* Configuration ************************************************************/
/* Most of the NX configuration settings are probbably *not* needed by this
* example. But, presumeably you are using NX too and so the checks might
* be good for you.
*/
#ifndef CONFIG_NX
# error "CONFIG_NX must be defined to use this test"
#endif
#ifndef CONFIG_NX_LCDDRIVER
# error "CONFIG_NX_LCDDRIVER must be defined to use this test"
#endif
#ifndef CONFIG_EXAMPLES_LCDRW_BPP
# define CONFIG_EXAMPLES_LCDRW_BPP 16
#endif
#if CONFIG_EXAMPLES_LCDRW_BPP != 16
# error "Currently only RGB565 is supported -- feel free to extend"
#endif
#ifdef CONFIG_NX_DISABLE_16BPP
# error "CONFIG_NX_DISABLE_16BPP disables 16-bit support"
#endif
#ifndef CONFIG_EXAMPLES_LDCRW_DEVNO
# define CONFIG_EXAMPLES_LDCRW_DEVNO 0
#endif
#ifndef CONFIG_EXAMPLES_LDCRW_XRES
# define CONFIG_EXAMPLES_LDCRW_XRES 240
#endif
#ifndef CONFIG_EXAMPLES_LDCRW_YRES
# define CONFIG_EXAMPLES_LDCRW_YRES 320
#endif
/****************************************************************************
* Private Types
****************************************************************************/
struct lcdrw_instance_s
{
/* LCD device handle and planeinfo */
FAR struct lcd_dev_s *dev;
struct lcd_planeinfo_s pinfo;
};
/****************************************************************************
* Private Data
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: lcdrw_initialize
****************************************************************************/
static inline int lcdrw_initialize(FAR struct lcdrw_instance_s *inst)
{
int ret;
/* Initialize the LCD device */
printf("screens_initialize: Initializing LCD\n");
ret = up_lcdinitialize();
if (ret < 0)
{
fprintf(stderr, "screens_initialize: up_lcdinitialize failed: %d\n", -ret);
return ret;
}
/* Get the device instance. */
printf("Get LCD instance\n");
inst->dev = up_lcdgetdev(CONFIG_EXAMPLES_LDCRW_DEVNO);
if (!inst->dev)
{
fprintf(stderr, "up_lcdgetdev failed, devno=%d\n", CONFIG_EXAMPLES_LDCRW_DEVNO);
return ret;
}
/* Turn the LCD on at 75% power. This should not be necessary. */
(void)inst->dev->setpower(inst->dev, ((3*CONFIG_LCD_MAXPOWER + 3)/4));
/* Get the planeinfo structure */
ret = inst->dev->getplaneinfo(inst->dev, 0, &inst->pinfo);
if (ret < 0)
{
fprintf(stderr, "getplaneinfo failed: %d\n", ret);
}
return ret;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: lcdrw_main
****************************************************************************/
int lcdrw_main(int argc, char *argv[])
{
struct lcdrw_instance_s inst;
nxgl_coord_t row;
nxgl_coord_t col;
uint16_t value;
uint32_t offset;
FAR uint16_t *ptr;
int ret;
/* Initialize the LCD driver */
ret = lcdrw_initialize(&inst);
if (ret < 0)
{
exit(1);
}
/* Loop, writing all possible values to the LCD */
value = 0;
for (row = 0; row < CONFIG_EXAMPLES_LDCRW_YRES; row++)
{
/* Create a dummy row. The important thing is to try all
* bit combinations in a predictable way.
*/
ptr = (FAR uint16_t*)inst.pinfo.buffer;
for (col = 0; col < CONFIG_EXAMPLES_LDCRW_XRES; col++)
{
*ptr++ = value++;
}
/* Write the row to the LCD */
ret = inst.pinfo.putrun(row, 0, inst.pinfo.buffer,
CONFIG_EXAMPLES_LDCRW_XRES);
if (ret < 0)
{
fprintf(stderr, "putrun failed: %d\n", ret);
exit(1);
}
}
/* Print a header */
printf(" ");
for (col = 0; col < 15; col++)
{
printf("---%x ", col);
}
printf("---f\n");
/* Then read each line back from the LCD. */
offset = 0;
for (row = 0; row < CONFIG_EXAMPLES_LDCRW_YRES; row++)
{
/* Read the row */
ret = inst.pinfo.getrun(row, 0, inst.pinfo.buffer,
CONFIG_EXAMPLES_LDCRW_XRES);
if (ret < 0)
{
fprintf(stderr, "getrun failed: %d\n", ret);
exit(1);
}
/* Then dump the row to the display */
ptr = (FAR uint16_t*)inst.pinfo.buffer;
for (col = 0; col < CONFIG_EXAMPLES_LDCRW_XRES; col++)
{
if ((offset & 15) == 0)
{
printf("%06x ", offset);
}
value = *ptr++;
offset++;
if ((offset & 15) == 0)
{
printf("%04x\n", value);
}
else
{
printf("%04x ", value);
}
}
}
fflush(stdout);
return 0;
}
apps/examples/mm/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/mm/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/mm/mm_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/mm/mm_main.c
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/mount/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/Makefile
#
# Copyright (C) 2007-2008, 2010-2010 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/mount/mount.h
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/mount/mount.h
*
* Copyright (C) 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/mount/mount_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/mount/mount_main.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/mount/ramdisk.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/mount/ramdisk.c
*
* Copyright (C) 2008-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/nsh/Kconfig
+2
View File
@@ -6,6 +6,8 @@
config EXAMPLES_NSH
bool "NuttShell (NSH) example"
default n
select NSH_LIBRARY
select SYSTEM_READLINE
---help---
Enable the NuttShell (NSH) example
apps/examples/nsh/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/nsh/Makefile
#
# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/null/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# examples/null/Makefile
#
# Copyright (C) 2007-2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/null/null_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/null/null_main.c
*
* Copyright (C) 2007, 2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/Kconfig
+29
View File
@@ -10,4 +10,33 @@ config EXAMPLES_OSTEST
Enable the OS test example
if EXAMPLES_OSTEST
config EXAMPLES_OSTEST_BUILTIN
bool "NSH built-in application"
default y if NSH_LIBRARY
default n if !NSH_LIBRARY
---help---
Build the OS test example as an NSH built-in application.
config EXAMPLES_OSTEST_LOOPS
int "OS test loop"
default 1
---help---
Used to control the number of executions of the test. If undefined, the test
executes one time. If defined to be zero, the test runs forever.
config EXAMPLES_OSTEST_STACKSIZE
int "OS test stack size"
default 8192
---help---
Size of the stack used to create the ostest task. Default is 8192.
config EXAMPLES_OSTEST_NBARRIER_THREADS
int "Number of barrier threads"
default 8
---help---
Specifies the number of threads to create in the barrier test. The default
is 8 but a smaller number may be needed on systems without sufficient memory
to start so many threads.
endif
apps/examples/ostest/Makefile
+1 -1
View File
@@ -46,7 +46,7 @@ STACKSIZE = 2048
# NuttX OS Test
ASRCS =
CSRCS = main.c dev_null.c
CSRCS = ostest_main.c dev_null.c
ifeq ($(CONFIG_ARCH_FPU),y)
CSRCS += fpu.c
apps/examples/ostest/barrier.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/barrier.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/cancel.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/cancel.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/cond.c
+1 -1
View File
@@ -2,7 +2,7 @@
* cond.c
*
* Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/dev_null.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/dev_null.c
*
* Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/mutex.c
+1 -1
View File
@@ -2,7 +2,7 @@
* mutex.c
*
* Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/main.c → apps/examples/ostest/ostest_main.c
+1 -1
View File
@@ -1,5 +1,5 @@
/****************************************************************************
* apps/examples/ostest/main.c
* apps/examples/ostest/ostest_main.c
*
* Copyright (C) 2007-2009, 2011-2012 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
apps/examples/ostest/posixtimer.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/posixtimer.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/prioinherit.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/prioinherit.c
*
* Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/rmutex.c
+1 -1
View File
@@ -2,7 +2,7 @@
* rmutex.c
*
* Copyright (C) 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/roundrobin.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/roundrobin.c
*
* Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/sem.c
+1 -1
View File
@@ -2,7 +2,7 @@
* sem.c
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/sighand.c
+1 -1
View File
@@ -2,7 +2,7 @@
* apps/examples/ostest/sighand.c
*
* Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/timedmqueue.c
+1 -1
View File
@@ -2,7 +2,7 @@
* apps/examples/ostest/mqueue.c
*
* Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/ostest/timedwait.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/ostest/timedwait.c
*
* Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/pipe/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/pipe/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/pipe/interlock_test.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/pipe/interlock_test.c
*
* Copyright (C) 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/pipe/pipe.h
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/pipe/pipe.h
*
* Copyright (C) 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/pipe/pipe_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/pipe/pipe_main.c
*
* Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/pipe/redirect_test.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/pipe/redirect_test.c
*
* Copyright (C) 2008-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/pipe/transfer_test.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/pipe/transfer_test.c
*
* Copyright (C) 2008-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/poll/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/poll/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/poll/Makefile.host
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/poll/Makefile.host
#
# Copyright (C) 2008, 2009, 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/poll/poll_internal.h
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/poll/poll_internal.h
*
* Copyright (C) 2008, 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/poll/poll_listener.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/poll/poll_listener.c
*
* Copyright (C) 2008 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/poll/poll_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/poll/poll_main.c
*
* Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/poll/select_listener.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/poll/select_listener.c
*
* Copyright (C) 2008, 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/px4_deamon_app/px4_deamon_app.c
+14 -15
View File
@@ -60,21 +60,6 @@ int px4_deamon_thread_main(int argc, char *argv[]);
*/
static void usage(const char *reason);
int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
while (true) {
printf("Hello Deamon!\n");
sleep(10);
if (thread_should_exit) break;
}
printf("[deamon] exiting.\n");
return 0;
}
static void
usage(const char *reason)
{
@@ -128,3 +113,17 @@ int px4_deamon_app_main(int argc, char *argv[])
usage("unrecognized command");
exit(1);
}
int px4_deamon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
while (!thread_should_exit) {
printf("Hello Deamon!\n");
sleep(10);
}
printf("[deamon] exiting.\n");
return 0;
}
apps/examples/romfs/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/examples/romfs/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/examples/romfs/romfs_main.c
+1 -1
View File
@@ -2,7 +2,7 @@
* examples/romfs/romfs_main.c
*
* Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/examples/serloop/Makefile
+2 -2
View File
@@ -2,7 +2,7 @@
# apps/examples/serloop/Makefile
#
# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -40,7 +40,7 @@ include $(APPDIR)/Make.defs
# Mindlessly simple console loopack test
ASRCS =
CSRCS = main.c
CSRCS = serloop_main.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
apps/examples/serloop/main.c → apps/examples/serloop/serloop_main.c
+2 -2
View File
@@ -1,8 +1,8 @@
/****************************************************************************
* examples/serloop/main.c
* examples/serloop/serloop_main.c
*
* Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
apps/fixedwing_control/fixedwing_control.c
+79 -18
View File
@@ -63,8 +63,79 @@
#include <uORB/topics/actuator_controls.h>
#include <systemlib/param/param.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int fixedwing_control_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: fixedwing_control {start|stop|status}\n\n");
exit(1);
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int fixedwing_control_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("fixedwing_control already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_create("fixedwing_control", SCHED_PRIORITY_MAX - 20, 4096, fixedwing_control_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tfixedwing_control is running\n");
} else {
printf("\tfixedwing_control not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
#define PID_DT 0.01f
#define PID_SCALER 0.1f
#define PID_DERIVMODE_CALC 0
@@ -133,8 +204,8 @@ typedef struct {
/* Next waypoint*/
float wp_x;
float wp_y;
double wp_x;
double wp_y;
float wp_z;
/* Setpoints */
@@ -349,9 +420,6 @@ static void get_parameters(plane_data_t * pdata)
param_get(att_awu, &(pdata->intmax_att));
param_get(pos_awu, &(pdata->intmax_pos));
pdata->airspeed = 10;
pdata->wp_x = 48;
pdata->wp_y = 8;
pdata->wp_z = 100;
pdata->mode = 1;
}
@@ -617,14 +685,12 @@ static float calc_pitch_setpoint()
}
/**
* calc_throttle_setpoint
*
* Calculates the throttle setpoint for different flight modes
*
* @return throttle output setpoint
*
*/
static float calc_throttle_setpoint()
{
float setpoint = 0;
@@ -647,8 +713,7 @@ static float calc_throttle_setpoint()
return setpoint;
}
/*
* fixedwing_control_main
/**
*
* @param argc number of arguments
* @param argv argument array
@@ -656,8 +721,7 @@ static float calc_throttle_setpoint()
* @return 0
*
*/
int fixedwing_control_main(int argc, char *argv[])
int fixedwing_control_thread_main(int argc, char *argv[])
{
/* default values for arguments */
char *fixedwing_uart_name = "/dev/ttyS1";
@@ -686,7 +750,6 @@ int fixedwing_control_main(int argc, char *argv[])
/* output structs */
struct actuator_controls_s actuators;
struct actuator_armed_s armed;
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
@@ -694,8 +757,6 @@ int fixedwing_control_main(int argc, char *argv[])
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
actuators.control[i] = 0.0f;
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
armed.armed = false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
/* Subscribe to global position, attitude and rc */
@@ -771,6 +832,10 @@ int fixedwing_control_main(int argc, char *argv[])
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
plane_data.wp_x = global_setpoint.lat / (double)1e7;
plane_data.wp_y = global_setpoint.lon / (double)1e7;
plane_data.wp_z = global_setpoint.altitude;
/* parameter values */
if (loopcounter % 20 == 0) {
get_parameters(&plane_data);
@@ -841,10 +906,6 @@ int fixedwing_control_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* publish actuator setpoints (for mixer) */
/* arming state depends on commander arming state */
armed.armed = state.flag_system_armed;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
loopcounter++;
apps/fixedwing_control/fixedwing_control.zip
BIN
View File
Binary file not shown.
apps/fixedwing_control/pid.c
-118
View File
@@ -1,118 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Ivan Ovinnikov <oivan@ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file pid.c
* Implementation of a fixed wing attitude and position controller.
*/
#include "pid.h"
#include "fixedwing_control.h"
/*******************************************************************************
* pid()
*
* Calculates the PID control output given an error
*
* Input: float error, uint16_t dt, float scaler, float K_p, float K_i, float K_d
*
* Output: PID control value
*
******************************************************************************/
static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax)
{
// PID parameters
float Kp = K_p;
float Ki = K_i;
float Kd = K_d;
float delta_time = dt; // delta time
float lerror; // last error value
float imax = intmax; // max integral value
float integrator;
float derivative;
float lderiv;
int fCut = 20; // anything above 20 Hz is considered noise - low pass filter for the derivative
float output = 0; // the output of the PID controller
output += error * Kp;
if ((fabs(Kd) > 0) && (dt > 0)) {
if (PID_DERIVMODE_CALC) {
derivative = (error - lerror) / delta_time;
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
float RC = 1 / (2 * M_PI * fCut);
derivative = lderiv +
(delta_time / (RC + delta_time)) * (derivative - lderiv);
// update state
lerror = error;
lderiv = derivative;
} else {
derivative = error_deriv;
}
// add in derivative component
output += Kd * derivative;
}
printf("PID derivative %i\n", (int)(1000 * derivative));
// scale the P and D components
output *= scaler;
// Compute integral component if time has elapsed
if ((fabs(Ki) > 0) && (dt > 0)) {
integrator += (error * Ki) * scaler * delta_time;
if (integrator < -imax) {
integrator = -imax;
} else if (integrator > imax) {
integrator = imax;
}
output += integrator;
}
printf("PID Integrator %i\n", (int)(1000 * integrator));
return output;
}
apps/fixedwing_control/pid.h
-46
View File
@@ -1,46 +0,0 @@
/****************************************************************************
* pid.h
*
* Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
* Authors: Ivan Ovinnikov <oivan@ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#ifndef PID_H_
#define PID_H_
static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
#endif /* PID_H_ */
apps/gps/gps.c
+158 -80
View File
@@ -38,7 +38,6 @@
*/
#include "gps.h"
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
@@ -58,6 +57,10 @@
#include <v1.0/common/mavlink.h>
#include <mavlink/mavlink_log.h>
static bool thread_should_exit; /**< Deamon status flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* GPS module readout and publishing.
*
@@ -68,6 +71,16 @@
*/
__EXPORT int gps_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int gps_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
/****************************************************************************
* Definitions
****************************************************************************/
@@ -91,7 +104,6 @@ enum GPS_MODES {
};
#define AUTO_DETECTION_COUNT 8
const int autodetection_baudrates[] = {B9600, B38400, B38400, B9600, B9600, B38400, B9600, B38400};
const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_UBX, GPS_MODE_MTK, GPS_MODE_NMEA, GPS_MODE_NMEA}; //nmea is the fall-back if nothing else works, therefore we try the standard modes again before finally trying nmea
@@ -101,52 +113,66 @@ const enum GPS_MODES autodetection_gpsmodes[] = {GPS_MODE_UBX, GPS_MODE_MTK, GPS
****************************************************************************/
int open_port(char *port);
void close_port(int fd);
void close_port(int *fd);
void setup_port(char *device, int speed, int *fd)
void setup_port(char *device, int speed, int *fd);
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int gps_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
/* open port (baud rate is set in defconfig file) */
*fd = open_port(device);
if (!strcmp(argv[1], "start")) {
if (*fd != -1) {
if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
if (thread_running) {
printf("gps already running\n");
/* this is not an error */
exit(0);
}
} else {
fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
fflush(stdout);
thread_should_exit = false;
deamon_task = task_create("gps", SCHED_PRIORITY_DEFAULT, 4096, gps_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
close(*fd);
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
/* Set baud rate */
cfsetispeed(&uart_config, speed);
cfsetospeed(&uart_config, speed);
if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
close(*fd);
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tgps is running\n");
} else {
printf("\tgps not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
/*
* Main function of gps app.
*/
int gps_main(int argc, char *argv[])
{
int gps_thread_main(int argc, char *argv[]) {
/* welcome message */
printf("[gps] Initialized. Searching for GPS receiver..\n");
/* default values */
const char *commandline_usage = "\tusage: %s -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
const char *commandline_usage = "\tusage: %s {start|stop|status} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n";
char *device = "/dev/ttyS3";
char mode[10];
strcpy(mode, "all");
@@ -163,9 +189,10 @@ int gps_main(int argc, char *argv[])
/* read arguments */
int i;
for (i = 1; i < argc; i++) {
for (i = 0; i < argc; i++) {
if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { //device set
printf(commandline_usage, argv[0]);
thread_running = false;
return 0;
}
@@ -175,6 +202,7 @@ int gps_main(int argc, char *argv[])
} else {
printf(commandline_usage, argv[0]);
thread_running = false;
return 0;
}
}
@@ -185,6 +213,7 @@ int gps_main(int argc, char *argv[])
} else {
printf(commandline_usage, argv[0]);
thread_running = false;
return 0;
}
}
@@ -195,6 +224,7 @@ int gps_main(int argc, char *argv[])
} else {
printf(commandline_usage, argv[0]);
thread_running = false;
return 0;
}
}
@@ -205,6 +235,7 @@ int gps_main(int argc, char *argv[])
} else {
printf(commandline_usage, argv[0]);
thread_running = false;
return 0;
}
}
@@ -225,45 +256,25 @@ int gps_main(int argc, char *argv[])
case -1: gps_baud_try_all = true; break;
case 0: current_gps_speed = B0; break;
case 50: current_gps_speed = B50; break;
case 75: current_gps_speed = B75; break;
case 110: current_gps_speed = B110; break;
case 134: current_gps_speed = B134; break;
case 150: current_gps_speed = B150; break;
case 200: current_gps_speed = B200; break;
case 300: current_gps_speed = B300; break;
case 600: current_gps_speed = B600; break;
case 1200: current_gps_speed = B1200; break;
case 1800: current_gps_speed = B1800; break;
case 2400: current_gps_speed = B2400; break;
case 4800: current_gps_speed = B4800; break;
case 9600: current_gps_speed = B9600; break;
case 19200: current_gps_speed = B19200; break;
case 38400: current_gps_speed = B38400; break;
case 57600: current_gps_speed = B57600; break;
case 115200: current_gps_speed = B115200; break;
case 230400: current_gps_speed = B230400; break;
case 460800: current_gps_speed = B460800; break;
case 921600: current_gps_speed = B921600; break;
default:
@@ -290,11 +301,14 @@ int gps_main(int argc, char *argv[])
} else {
fprintf(stderr, "\t[gps] Invalid mode argument\n");
printf(commandline_usage);
thread_running = false;
return ERROR;
}
/* Declare file descriptor for gps here, open port later in setup_port */
int fd;
while (true) {
while (!thread_should_exit) {
/* Infinite retries or break if retry == false */
/* Loop over all configurations of baud rate and protocol */
@@ -319,15 +333,14 @@ int gps_main(int argc, char *argv[])
* if the gps was once running the wtachdog thread will not return but instead try to reconfigure the gps (depending on the mode/protocol)
*/
if (current_gps_mode == GPS_MODE_UBX) { //TODO: make a small enum with all modes to avoid all the strcpy
if (current_gps_mode == GPS_MODE_UBX) {
if (gps_verbose) printf("[gps] Trying UBX mode at %d baud\n", current_gps_speed);
mavlink_log_info(mavlink_fd, "GPS: trying to connect to a ubx module");
int fd;
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a ubx module");
setup_port(device, current_gps_speed, &fd);
/* start ubx thread and watchdog */
pthread_t ubx_thread;
pthread_t ubx_watchdog_thread;
@@ -342,13 +355,18 @@ int gps_main(int argc, char *argv[])
pthread_attr_t ubx_loop_attr;
pthread_attr_init(&ubx_loop_attr);
pthread_attr_setstacksize(&ubx_loop_attr, 3000);
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&fd);
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
struct arg_struct args;
args.fd_ptr = &fd;
args.thread_should_exit_ptr = &thread_should_exit;
pthread_create(&ubx_thread, &ubx_loop_attr, ubx_loop, (void *)&args);
sleep(2); // XXX TODO Check if this is too short, try to lower sleeps in UBX driver
pthread_attr_t ubx_wd_attr;
pthread_attr_init(&ubx_wd_attr);
pthread_attr_setstacksize(&ubx_wd_attr, 1400);
int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&fd);
int pthread_create_res = pthread_create(&ubx_watchdog_thread, &ubx_wd_attr, ubx_watchdog_loop, (void *)&args);
if (pthread_create_res != 0) fprintf(stderr, "[gps] ERROR: could not create ubx watchdog thread, pthread_create =%d\n", pthread_create_res);
@@ -363,18 +381,23 @@ int gps_main(int argc, char *argv[])
gps_mode_success = true;
terminate_gps_thread = false;
/* maybe the watchdog was stopped through the thread_should_exit flag */
} else if (thread_should_exit) {
pthread_join(ubx_thread, NULL);
if (gps_verbose) printf("[gps] ubx watchdog and ubx loop threads have been terminated, exiting.");
close(mavlink_fd);
close_port(&fd);
thread_running = false;
return 0;
}
close_port(fd);
}
if (current_gps_mode == GPS_MODE_MTK) {
close_port(&fd);
} else if (current_gps_mode == GPS_MODE_MTK) {
if (gps_verbose) printf("[gps] trying MTK binary mode at %d baud\n", current_gps_speed);
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a MTK module");
int fd;
setup_port(device, current_gps_speed, &fd);
/* start mtk thread and watchdog */
@@ -394,9 +417,14 @@ int gps_main(int argc, char *argv[])
pthread_attr_t mtk_loop_attr;
pthread_attr_init(&mtk_loop_attr);
pthread_attr_setstacksize(&mtk_loop_attr, 2048);
pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&fd);
struct arg_struct args;
args.fd_ptr = &fd;
args.thread_should_exit_ptr = &thread_should_exit;
pthread_create(&mtk_thread, &mtk_loop_attr, mtk_loop, (void *)&args);
sleep(2);
pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&fd);
pthread_create(&mtk_watchdog_thread, NULL, mtk_watchdog_loop, (void *)&args);
/* wait for threads to complete */
pthread_join(mtk_watchdog_thread, (void *)&fd);
@@ -407,23 +435,21 @@ int gps_main(int argc, char *argv[])
terminate_gps_thread = true;
pthread_join(mtk_thread, NULL);
// if(true == gps_mode_try_all)
// strcpy(mode, "nmea");
//if(true == gps_mode_try_all)
//strcpy(mode, "nmea");
gps_mode_success = true;
terminate_gps_thread = false;
}
close_port(fd);
close_port(&fd);
}
if (current_gps_mode == GPS_MODE_NMEA) {
} else if (current_gps_mode == GPS_MODE_NMEA) {
if (gps_verbose) printf("[gps] Trying NMEA mode at %d baud\n", current_gps_speed);
mavlink_log_info(mavlink_fd, "[gps] trying to connect to a NMEA module");
int fd;
setup_port(device, current_gps_speed, &fd);
/* start nmea thread and watchdog */
@@ -440,9 +466,14 @@ int gps_main(int argc, char *argv[])
pthread_attr_t nmea_loop_attr;
pthread_attr_init(&nmea_loop_attr);
pthread_attr_setstacksize(&nmea_loop_attr, 4096);
pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&fd);
struct arg_struct args;
args.fd_ptr = &fd;
args.thread_should_exit_ptr = &thread_should_exit;
pthread_create(&nmea_thread, &nmea_loop_attr, nmea_loop, (void *)&args);
sleep(2);
pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&fd);
pthread_create(&nmea_watchdog_thread, NULL, nmea_watchdog_loop, (void *)&args);
/* wait for threads to complete */
pthread_join(nmea_watchdog_thread, (void *)&fd);
@@ -457,14 +488,23 @@ int gps_main(int argc, char *argv[])
terminate_gps_thread = false;
}
close_port(fd);
close_port(&fd);
}
/* exit quickly if stop command has been received */
if (thread_should_exit) {
printf("[gps] stopped, exiting.\n");
close(mavlink_fd);
thread_running = false;
return 0;
}
/* if both, mode and baud is set by argument, we only need one loop*/
if (gps_mode_try_all == false && gps_baud_try_all == false)
break;
}
if (retry) {
printf("[gps] No configuration was successful, retrying in %d seconds \n", RETRY_INTERVAL_SECONDS);
mavlink_log_info(mavlink_fd, "[gps] No configuration was successful, retrying...");
@@ -480,9 +520,19 @@ int gps_main(int argc, char *argv[])
sleep(RETRY_INTERVAL_SECONDS);
}
printf("[gps] exiting.\n");
close(mavlink_fd);
thread_running = false;
return 0;
}
return ERROR;
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "\tusage: gps {start|status|stop} -d devicename -b baudrate -m mode\n\tmodes are:\n\t\tubx\n\t\tmtkcustom\n\t\tnmea\n\t\tall\n");
exit(1);
}
int open_port(char *port)
@@ -495,11 +545,39 @@ int open_port(char *port)
}
void close_port(int fd)
void close_port(int *fd)
{
/* Close serial port */
close(fd);
close(*fd);
}
void setup_port(char *device, int speed, int *fd)
{
/* open port (baud rate is set in defconfig file) */
*fd = open_port(device);
if (*fd != -1) {
if (gps_verbose) printf("[gps] Port opened: %s at %d speed\r\n", device, speed);
} else {
fprintf(stderr, "[gps] Could not open port, exiting gps app!\r\n");
fflush(stdout);
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
if ((termios_state = tcgetattr(*fd, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR getting baudrate / termios config for %s: %d\r\n", device, termios_state);
close(*fd);
}
if (gps_verbose) printf("[gps] Try to set baud rate %d now\n",speed);
/* Set baud rate */
cfsetispeed(&uart_config, speed);
cfsetospeed(&uart_config, speed);
if ((termios_state = tcsetattr(*fd, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[gps] ERROR setting baudrate / termios config for %s (tcsetattr)\r\n", device);
close(*fd);
}
}
apps/gps/gps.h
+6 -8
View File
@@ -6,15 +6,13 @@
*/
#ifndef GPS_H_
#define GPS_H_
/****************************************************************************
* Included Files
****************************************************************************/
int gps_fd;
//extern gps_bin_ubx_state_t * ubx_state;
#define GPS_H
#include <stdbool.h>
struct arg_struct {
int *fd_ptr;
bool *thread_should_exit_ptr;
};
#endif /* GPS_H_ */
apps/gps/mtk.c
+28 -19
View File
@@ -35,12 +35,14 @@
/* @file MTK custom binary (3DR) protocol implementation */
#include "gps.h"
#include "mtk.h"
#include <nuttx/config.h>
#include <unistd.h>
#include <sys/prctl.h>
#include <pthread.h>
#include <poll.h>
#include <fcntl.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
@@ -121,7 +123,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer)
if (mtk_state->ck_a == packet->ck_a && mtk_state->ck_b == packet->ck_b) {
mtk_gps->lat = packet->latitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->lon = packet->longitude * 10; // mtk: degrees*1e6, mavlink/ubx: degrees*1e7
mtk_gps->alt = (int32_t)packet->msl_altitude * 10; // conversion from centimeters to millimeters, and from uint32_t to int16_t
mtk_gps->alt = (int32_t)(packet->msl_altitude * 10); // conversion from centimeters to millimeters, and from uint32_t to int16_t
mtk_gps->fix_type = packet->fix_type;
mtk_gps->eph = packet->hdop;
mtk_gps->epv = 65535; //unknown in mtk custom mode
@@ -180,7 +182,7 @@ int mtk_parse(uint8_t b, char *gps_rx_buffer)
}
int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate
int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size) // returns 1 if the thread should terminate
{
// printf("in read_gps_mtk\n");
uint8_t ret = 0;
@@ -191,7 +193,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = fd;
fds.fd = *fd;
fds.events = POLLIN;
// This blocks the task until there is something on the buffer
@@ -206,7 +208,7 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
}
if (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) {
if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c);
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid NMEA sentence.
@@ -243,11 +245,11 @@ int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size) // returns 1 if
return ret;
}
int configure_gps_mtk(int fd)
int configure_gps_mtk(int *fd)
{
int success = 0;
size_t result_write;
result_write = write(fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ));
result_write = write(*fd, MEDIATEK_REFRESH_RATE_10HZ, strlen(MEDIATEK_REFRESH_RATE_10HZ));
if (result_write != strlen(MEDIATEK_REFRESH_RATE_10HZ)) {
printf("[gps] Set update speed to 10 Hz failed\r\n");
@@ -258,7 +260,7 @@ int configure_gps_mtk(int fd)
}
//set custom mode
result_write = write(fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE));
result_write = write(*fd, MEDIATEK_CUSTOM_BINARY_MODE, strlen(MEDIATEK_CUSTOM_BINARY_MODE));
if (result_write != strlen(MEDIATEK_CUSTOM_BINARY_MODE)) {
//global_data_send_subsystem_info(&mtk_present);
@@ -273,7 +275,7 @@ int configure_gps_mtk(int fd)
return success;
}
void *mtk_loop(void *arg)
void *mtk_loop(void *args)
{
// int oldstate;
// pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, oldstate);
@@ -282,8 +284,10 @@ void *mtk_loop(void *arg)
/* Set thread name */
prctl(PR_SET_NAME, "gps mtk read", getpid());
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
// int buffer_size = 1000;
@@ -311,25 +315,29 @@ void *mtk_loop(void *arg)
/* advertise GPS topic */
struct vehicle_gps_position_s mtk_gps_d;
mtk_gps = &mtk_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps);
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), mtk_gps);
while (1) {
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
if (OK == read_gps_mtk(fd, gps_rx_buffer, MTK_BUFFER_SIZE)) {
/* publish new GPS position */
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, &mtk_gps);
orb_publish(ORB_ID(vehicle_gps_position), gps_handle, mtk_gps);
} else {
/* de-advertise */
close(gps_handle);
break;
}
}
close(gps_handle);
if(gps_verbose) printf("[gps] mtk loop is going to terminate\n");
return NULL;
}
void *mtk_watchdog_loop(void *arg)
void *mtk_watchdog_loop(void *args)
{
// printf("in mtk watchdog loop\n");
fflush(stdout);
@@ -337,8 +345,10 @@ void *mtk_watchdog_loop(void *arg)
/* Set thread name */
prctl(PR_SET_NAME, "gps mtk watchdog", getpid());
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
bool mtk_healthy = false;
@@ -349,7 +359,7 @@ void *mtk_watchdog_loop(void *arg)
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (1) {
while (!(*thread_should_exit)) {
fflush(stdout);
/* if we have no update for a long time reconfigure gps */
@@ -416,8 +426,7 @@ void *mtk_watchdog_loop(void *arg)
usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] mtk watchdog is going to terminate\n");
close(mavlink_fd);
return NULL;
}
apps/gps/mtk.h
+4 -4
View File
@@ -87,12 +87,12 @@ void mtk_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
int mtk_parse(uint8_t b, char *gps_rx_buffer);
int read_gps_mtk(int fd, char *gps_rx_buffer, int buffer_size);
int read_gps_mtk(int *fd, char *gps_rx_buffer, int buffer_size);
int configure_gps_mtk(int fd);
int configure_gps_mtk(int *fd);
void *mtk_loop(void *arg);
void *mtk_loop(void *args);
void *mtk_watchdog_loop(void *arg);
void *mtk_watchdog_loop(void *args);
#endif /* MTK_H_ */
apps/gps/nmea_helper.c
+28 -18
View File
@@ -34,13 +34,17 @@
****************************************************************************/
/* @file NMEA protocol implementation */
#include "gps.h"
#include "nmea_helper.h"
#include <sys/prctl.h>
#include <unistd.h>
#include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include <arch/board/up_hrt.h>
#define NMEA_HEALTH_SUCCESS_COUNTER_LIMIT 2
#define NMEA_HEALTH_FAIL_COUNTER_LIMIT 2
@@ -59,7 +63,7 @@ extern bool gps_verbose;
extern int current_gps_speed;
int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
{
int ret = 1;
char c;
@@ -69,7 +73,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = fd;
fds.fd = *fd;
fds.events = POLLIN;
// NMEA or SINGLE-SENTENCE GPS mode
@@ -86,7 +90,7 @@ int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info,
}
if (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) {
if (read(*fd, &c, 1) > 0) {
// detect start while acquiring stream
// printf("Char = %c\n", c);
if (!start_flag && (c == '$')) {
@@ -155,13 +159,15 @@ float ndeg2degree(float val)
return val;
}
void *nmea_loop(void *arg)
void *nmea_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps nmea read", getpid());
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
nmeaINFO info_d;
@@ -174,11 +180,11 @@ void *nmea_loop(void *arg)
nmea_zero_INFO(info);
/* advertise GPS topic */
struct vehicle_gps_position_s nmea_gps_d = {0};
struct vehicle_gps_position_s nmea_gps_d = {.counter=0};
nmea_gps = &nmea_gps_d;
orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps);
while (1) {
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
uint8_t read_res = read_gps_nmea(fd, gps_rx_buffer, NMEA_BUFFER_SIZE, info, &parser);
@@ -200,11 +206,11 @@ void *nmea_loop(void *arg)
// printf("%d.%d.%d %d:%d:%d:%d\n", timeinfo.tm_year, timeinfo.tm_mon, timeinfo.tm_mday, timeinfo.tm_hour, timeinfo.tm_min, timeinfo.tm_sec, info->utc.hsec);
nmea_gps->timestamp = hrt_absolute_time();
nmea_gps->time_gps_usec = epoch * 1e6 + info->utc.hsec * 1e4;
nmea_gps->time_gps_usec = (uint64_t)((epoch)*1000000 + (info->utc.hsec)*10000);
nmea_gps->fix_type = (uint8_t)info->fix;
nmea_gps->lat = (int32_t)ndeg2degree(info->lat) * 1e7;
nmea_gps->lon = (int32_t)ndeg2degree(info->lon) * 1e7;
nmea_gps->alt = (int32_t)(info->elv * 1e3);
nmea_gps->lat = (int32_t)(ndeg2degree(info->lat) * 1e7f);
nmea_gps->lon = (int32_t)(ndeg2degree(info->lon) * 1e7f);
nmea_gps->alt = (int32_t)(info->elv * 1000.0f);
nmea_gps->eph = (uint16_t)(info->HDOP * 100); //TODO:test scaling
nmea_gps->epv = (uint16_t)(info->VDOP * 100); //TODO:test scaling
nmea_gps->vel = (uint16_t)(info->speed * 1000 / 36); //*1000/3600*100
@@ -251,12 +257,12 @@ void *nmea_loop(void *arg)
//destroy gps parser
nmea_parser_destroy(&parser);
if(gps_verbose) printf("[gps] nmea loop is going to terminate\n");
return NULL;
}
void *nmea_watchdog_loop(void *arg)
void *nmea_watchdog_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps nmea watchdog", getpid());
@@ -267,9 +273,14 @@ void *nmea_watchdog_loop(void *arg)
uint8_t nmea_success_count = 0;
bool once_ok = false;
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
//int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (1) {
while (!(*thread_should_exit)) {
// printf("nmea_watchdog_loop : while ");
/* if we have no update for a long time print warning (in nmea mode there is no reconfigure) */
pthread_mutex_lock(nmea_mutex);
@@ -328,8 +339,7 @@ void *nmea_watchdog_loop(void *arg)
usleep(NMEA_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] nmea watchdog loop is going to terminate\n");
close(mavlink_fd);
return NULL;
}
apps/gps/nmea_helper.h
+1 -1
View File
@@ -30,7 +30,7 @@ extern pthread_mutex_t *nmea_mutex;
int read_gps_nmea(int fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser);
void *nmea_loop(void *arg);
apps/gps/ubx.c
+51 -43
View File
@@ -35,12 +35,16 @@
/* @file U-Blox protocol implementation */
#include "ubx.h"
#include "gps.h"
#include <sys/prctl.h>
#include <poll.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
@@ -340,8 +344,8 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
ubx_gps->time_gps_usec = (uint64_t)epoch * 1e6; //TODO: test this
ubx_gps->time_gps_usec += packet->time_nanoseconds * 1e-3;
ubx_gps->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
ubx_gps->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
@@ -474,7 +478,7 @@ int ubx_parse(uint8_t b, char *gps_rx_buffer)
if (ubx_state->ck_a == packet->ck_a && ubx_state->ck_b == packet->ck_b) {
ubx_gps->vel = (uint16_t)packet->speed;
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3);
ubx_gps->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
ubx_gps->timestamp = hrt_absolute_time();
ubx_gps->counter++;
@@ -570,44 +574,44 @@ void calculate_ubx_checksum(uint8_t *message, uint8_t length)
// printf("[%x,%x]\n", message[length-2], message[length-1]);
}
int configure_gps_ubx(int fd)
int configure_gps_ubx(int *fd)
{
fflush((FILE *)fd);
//fflush(((FILE *)fd));
//TODO: write this in a loop once it is tested
//UBX_CFG_PRT_PART:
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_PRT, sizeof(UBX_CONFIG_MESSAGE_PRT) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_POSLLH:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_POSLLH) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_TIMEUTC:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_TIMEUTC) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_DOP:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_DOP, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_DOP) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_SOL:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SOL, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SOL) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_SVINFO:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_SVINFO) / sizeof(uint8_t) , *fd);
usleep(100000);
//NAV_VELNED:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED, sizeof(UBX_CONFIG_MESSAGE_MSG_NAV_VELNED) / sizeof(uint8_t) , *fd);
usleep(100000);
//RXM_SVSI:
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , fd);
write_config_message_ubx(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI, sizeof(UBX_CONFIG_MESSAGE_MSG_RXM_SVSI) / sizeof(uint8_t) , *fd);
usleep(100000);
return 0;
@@ -615,16 +619,15 @@ int configure_gps_ubx(int fd)
int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size)
{
uint8_t ret = 0;
uint8_t c;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = fd;
fds.fd = *fd;
fds.events = POLLIN;
// UBX GPS mode
@@ -641,7 +644,7 @@ int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size)
}
if (poll(&fds, 1, 1000) > 0) {
if (read(fd, &c, 1) > 0) {
if (read(*fd, &c, 1) > 0) {
// printf("Read %x\n",c);
if (rx_count >= buffer_size) {
@@ -685,7 +688,7 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
uint8_t ck_a = 0;
uint8_t ck_b = 0;
int i;
unsigned int i;
for (i = 2; i < length; i++) {
ck_a = ck_a + message[i];
@@ -702,14 +705,16 @@ int write_config_message_ubx(uint8_t *message, size_t length, int fd)
}
void *ubx_watchdog_loop(void *arg)
void *ubx_watchdog_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps ubx watchdog", getpid());
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* GPS watchdog error message skip counter */
@@ -721,7 +726,9 @@ void *ubx_watchdog_loop(void *arg)
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
while (1) {
//int err_skip_counter = 0;
while (!(*thread_should_exit)) {
/* if some values are to old reconfigure gps */
int i;
pthread_mutex_lock(ubx_mutex);
@@ -732,7 +739,7 @@ void *ubx_watchdog_loop(void *arg)
// printf("timestamp_now=%llu\n", timestamp_now);
// printf("last_message_timestamps=%llu\n", ubx_state->last_message_timestamps[i]);
if (timestamp_now - ubx_state->last_message_timestamps[i] > UBX_WATCHDOG_CRITICAL_TIME_MICROSECONDS) {
// printf("Warning: GPS ubx message %d not received for a long time\n", i);
//printf("Warning: GPS ubx message %d not received for a long time\n", i);
all_okay = false;
}
}
@@ -748,7 +755,7 @@ void *ubx_watchdog_loop(void *arg)
// err_skip_counter = 20;
// }
// err_skip_counter--;
// printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
//printf("gps_mode_try_all =%u, ubx_fail_count=%u, ubx_healthy=%u, once_ok=%u\n", gps_mode_try_all, ubx_fail_count, ubx_healthy, once_ok);
/* If we have too many failures and another mode or baud should be tried, exit... */
@@ -766,7 +773,6 @@ void *ubx_watchdog_loop(void *arg)
ubx_healthy = false;
ubx_success_count = 0;
}
/* trying to reconfigure the gps configuration */
configure_gps_ubx(fd);
fflush(stdout);
@@ -777,7 +783,7 @@ void *ubx_watchdog_loop(void *arg)
ubx_success_count++;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
// global_data_send_subsystem_info(&ubx_present_enabled_healthy);
mavlink_log_info(mavlink_fd, "[gps] UBX module found, status ok\n");
ubx_healthy = true;
@@ -790,18 +796,20 @@ void *ubx_watchdog_loop(void *arg)
usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
}
if(gps_verbose) printf("[gps] ubx loop is going to terminate\n");
close(mavlink_fd);
return NULL;
}
void *ubx_loop(void *arg)
void *ubx_loop(void *args)
{
/* Set thread name */
prctl(PR_SET_NAME, "gps ubx read", getpid());
/* Retrieve file descriptor */
int fd = *((int *)arg);
/* Retrieve file descriptor and thread flag */
struct arg_struct *arguments = (struct arg_struct *)args;
int *fd = arguments->fd_ptr;
bool *thread_should_exit = arguments->thread_should_exit_ptr;
/* Initialize gps stuff */
char gps_rx_buffer[UBX_BUFFER_SIZE];
@@ -809,14 +817,13 @@ void *ubx_loop(void *arg)
if (gps_verbose) printf("[gps] UBX protocol driver starting..\r\n");
//set parameters for ubx
//set parameters for ubx_state
// //ubx state
// gps_bin_ubx_state_t * ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
// printf("gps: ubx_state created\n");
// ubx_decode_init();
// ubx_state->print_errors = false;
//ubx state
ubx_state = malloc(sizeof(gps_bin_ubx_state_t));
//printf("gps: ubx_state created\n");
ubx_decode_init();
ubx_state->print_errors = false;
/* set parameters for ubx */
@@ -825,7 +832,7 @@ void *ubx_loop(void *arg)
printf("[gps] Configuration of gps module to ubx failed\r\n");
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present);
} else {
@@ -834,17 +841,17 @@ void *ubx_loop(void *arg)
// XXX Shouldn't the system status only change if the module is known to work ok?
/* Write shared variable sys_status */
// TODO enable this again
//global_data_send_subsystem_info(&ubx_present_enabled);
}
struct vehicle_gps_position_s ubx_gps_d = {0};
struct vehicle_gps_position_s ubx_gps_d = {.counter = 0};
ubx_gps = &ubx_gps_d;
orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps);
while (1) {
while (!(*thread_should_exit)) {
/* Parse a message from the gps receiver */
if (0 == read_gps_ubx(fd, gps_rx_buffer, UBX_BUFFER_SIZE)) {
/* publish new GPS position */
@@ -857,7 +864,8 @@ void *ubx_loop(void *arg)
}
}
if(gps_verbose) printf("[gps] ubx read is going to terminate\n");
close(gps_pub);
return NULL;
}
apps/gps/ubx.h
+4 -4
View File
@@ -298,17 +298,17 @@ void ubx_checksum(uint8_t b, uint8_t *ck_a, uint8_t *ck_b);
int ubx_parse(uint8_t b, char *gps_rx_buffer);
int configure_gps_ubx(int fd);
int configure_gps_ubx(int *fd);
int read_gps_ubx(int fd, char *gps_rx_buffer, int buffer_size);
int read_gps_ubx(int *fd, char *gps_rx_buffer, int buffer_size);
int write_config_message_ubx(uint8_t *message, size_t length, int fd);
void calculate_ubx_checksum(uint8_t *message, uint8_t length);
void *ubx_watchdog_loop(void *arg);
void *ubx_watchdog_loop(void *args);
void *ubx_loop(void *arg);
void *ubx_loop(void *args);
#endif /* UBX_H_ */
apps/interpreters/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/interpreters/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/interpreters/ficl/Makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# apps/interpreters/ficl/Makefile
#
# Copyright (C) 2011 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
apps/interpreters/ficl/README.txt
Executable → Regular
View File
apps/mavlink/mavlink.c
+188 -128
View File
@@ -61,7 +61,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -84,7 +83,7 @@
/* define MAVLink specific parameters */
PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_GENERIC);
PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR);
__EXPORT int mavlink_main(int argc, char *argv[]);
int mavlink_thread_main(int argc, char *argv[]);
@@ -96,7 +95,7 @@ static int mavlink_task;
/* terminate MAVLink on user request - disabled by default */
static bool mavlink_link_termination_allowed = false;
mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_FIXED_WING, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_QUADROTOR, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255
static uint8_t chan = MAVLINK_COMM_0;
static mavlink_status_t status;
@@ -409,6 +408,10 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const s
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
if (c_status->flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* set arming state */
if (actuator->armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
@@ -542,6 +545,7 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
/* manual_control_setpoint triggers this message */
@@ -588,7 +592,6 @@ static void *uorb_receiveloop(void *arg)
struct sensor_combined_s raw;
struct vehicle_attitude_s att;
struct vehicle_gps_position_s gps;
struct ardrone_control_s ar_control;
struct vehicle_local_position_setpoint_s local_sp;
struct vehicle_global_position_setpoint_s global_sp;
struct vehicle_attitude_setpoint_s att_sp;
@@ -712,7 +715,6 @@ static void *uorb_receiveloop(void *arg)
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(subs->actuators_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->actuators_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -746,15 +748,22 @@ static void *uorb_receiveloop(void *arg)
*/
const int timeout = 1000;
/*
* Last sensor loop time
* some outputs are better timestamped
* with this "global" reference.
*/
uint64_t last_sensor_timestamp = 0;
while (!thread_should_exit) {
int poll_ret = poll(fds, fdsc_count, timeout);
/* handle the poll result */
if (poll_ret == 0) {
/* XXX this means none of our providers is giving us data - might be an error? */
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
} else if (poll_ret < 0) {
/* XXX this is seriously bad - should be an emergency */
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
} else {
int ifds = 0;
@@ -765,19 +774,54 @@ static void *uorb_receiveloop(void *arg)
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
last_sensor_timestamp = buf.raw.timestamp;
/* send raw imu data */
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
/* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */
//mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000);
/* send scaled imu data */
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp,
// mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0],
// buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0],
// buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0],
// buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
/* mark individual fields as changed */
uint16_t fields_updated = 0;
static unsigned accel_counter = 0;
static unsigned gyro_counter = 0;
static unsigned mag_counter = 0;
static unsigned baro_counter = 0;
if (accel_counter != buf.raw.accelerometer_counter) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
accel_counter = buf.raw.accelerometer_counter;
}
if (gyro_counter != buf.raw.gyro_counter) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
gyro_counter = buf.raw.gyro_counter;
}
if (mag_counter != buf.raw.magnetometer_counter) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
mag_counter = buf.raw.magnetometer_counter;
}
if (baro_counter != buf.raw.baro_counter) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_counter = buf.raw.baro_counter;
}
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1],
buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0],
buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2],
buf.raw.magnetometer_ga[0],
buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2],
buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */,
buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius);
buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius,
fields_updated);
/* send pressure */
//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100);
@@ -791,7 +835,7 @@ static void *uorb_receiveloop(void *arg)
orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
attitude_counter++;
}
@@ -810,26 +854,6 @@ static void *uorb_receiveloop(void *arg)
gps_counter++;
}
// /* --- ARDRONE CONTROL OUTPUTS --- */
// if (fds[ifds++].revents & POLLIN) {
// /* copy ardrone control data into local buffer */
// orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
// uint64_t timestamp = buf.ar_control.timestamp;
// float setpoint_roll = buf.ar_control.setpoint_attitude[0];
// float setpoint_pitch = buf.ar_control.setpoint_attitude[1];
// float setpoint_yaw = buf.ar_control.setpoint_attitude[2];
// float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
// float control_roll = buf.ar_control.attitude_control_output[0];
// float control_pitch = buf.ar_control.attitude_control_output[1];
// float control_yaw = buf.ar_control.attitude_control_output[2];
// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
// mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
// }
/* --- SYSTEM STATUS --- */
if (fds[ifds++].revents & POLLIN) {
/* immediately communicate state changes back to user */
@@ -852,7 +876,8 @@ static void *uorb_receiveloop(void *arg)
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */
// TODO decide where to send channels
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
}
/* --- VEHICLE GLOBAL POSITION --- */
@@ -910,41 +935,24 @@ static void *uorb_receiveloop(void *arg)
/* Only send in HIL mode */
if (mavlink_hil_enabled) {
/* hacked HIL implementation in order for the APM Planner to work
* (correct cmd: mavlink_msg_hil_controls_send())
*/
mavlink_msg_rc_channels_scaled_send(chan,
hrt_absolute_time(),
0, // port 0
buf.att_sp.roll_body,
buf.att_sp.pitch_body,
buf.att_sp.thrust,
buf.att_sp.yaw_body,
0,
0,
0,
0,
1 /*rssi=1*/);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* correct HIL message as per MAVLink spec */
/* HIL message as per MAVLink spec */
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
buf.att_sp.roll_body,
buf.att_sp.pitch_body,
buf.att_sp.yaw_body,
buf.att_sp.thrust,
0,
0,
0,
0,
mavlink_mode,
0);
hrt_absolute_time(),
buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */
buf.att_sp.pitch_body,
buf.att_sp.yaw_body,
buf.att_sp.thrust,
0,
0,
0,
0,
mavlink_mode,
0);
}
}
@@ -952,7 +960,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs);
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
0 /* port 0 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -980,7 +988,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -991,7 +999,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
3 /* port 3 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1008,7 +1016,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -1019,7 +1027,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
5 /* port 5 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1036,7 +1044,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
buf.act_outputs.output[0],
buf.act_outputs.output[1],
@@ -1047,7 +1055,7 @@ static void *uorb_receiveloop(void *arg)
buf.act_outputs.output[6],
buf.act_outputs.output[7]);
if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000,
7 /* port 7 */,
buf.act_outputs.output[ 8],
buf.act_outputs.output[ 9],
@@ -1065,7 +1073,7 @@ static void *uorb_receiveloop(void *arg)
/* copy local position data into local buffer */
orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0);
}
/* --- ACTUATOR ARMED --- */
@@ -1076,16 +1084,18 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators);
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", buf.actuators.control[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]);
}
/* --- DEBUG KEY/VALUE --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value);
/* Enforce null termination */
buf.debug.key[sizeof(buf.debug.key) - 1] = '\0';
mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value);
}
}
}
@@ -1226,26 +1236,36 @@ void handleMessage(mavlink_message_t *msg)
/* Handle quadrotor motor setpoints */
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT) {
mavlink_set_quad_motors_setpoint_t quad_motors_setpoint;
mavlink_msg_set_quad_motors_setpoint_decode(msg, &quad_motors_setpoint);
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
// printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
if (quad_motors_setpoint.target_system == mavlink_system.sysid) {
ardrone_motors.motor_front_nw = quad_motors_setpoint.motor_front_nw;
ardrone_motors.motor_right_ne = quad_motors_setpoint.motor_right_ne;
ardrone_motors.motor_back_se = quad_motors_setpoint.motor_back_se;
ardrone_motors.motor_left_sw = quad_motors_setpoint.motor_left_sw;
if (mavlink_system.sysid < 4) {
ardrone_motors.p1 = quad_motors_setpoint.roll[mavlink_system.sysid];
ardrone_motors.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid];
ardrone_motors.p3 = quad_motors_setpoint.yaw[mavlink_system.sysid];
ardrone_motors.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid];
ardrone_motors.counter++;
ardrone_motors.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (ardrone_motors_pub <= 0) {
ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
/* only send if RC is off */
if (v_status.rc_signal_lost) {
/* check if input has to be enabled */
if (!v_status.flag_control_offboard_enabled) {
/* XXX Enable offboard control */
}
/* XXX decode mode and set flags */
// if (mode == abc) xxx flag_control_rates_enabled;
/* check if topic has to be advertised */
if (ardrone_motors_pub <= 0) {
ardrone_motors_pub = orb_advertise(ORB_ID(ardrone_motors_setpoint), &ardrone_motors);
}
/* Publish */
orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
}
/* Publish */
orb_publish(ORB_ID(ardrone_motors_setpoint), ardrone_motors_pub, &ardrone_motors);
}
}
@@ -1282,10 +1302,10 @@ void handleMessage(mavlink_message_t *msg)
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt/1000;
hil_global_pos.vx = hil_state.vx;
hil_global_pos.vy = hil_state.vy;
hil_global_pos.vz = hil_state.vz;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
/* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time();
@@ -1303,6 +1323,48 @@ void handleMessage(mavlink_message_t *msg)
hil_attitude.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
struct rc_channels_s rc_hil;
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.chan[0].raw = 1510 + man.x * 500;
rc_hil.chan[1].raw = 1520 + man.y * 500;
rc_hil.chan[2].raw = 1590 + man.r * 500;
rc_hil.chan[3].raw = 1420 + man.z * 500;
rc_hil.chan[0].scaled = man.x;
rc_hil.chan[1].scaled = man.y;
rc_hil.chan[2].scaled = man.r;
rc_hil.chan[3].scaled = man.z;
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
mc.roll = man.x;
mc.pitch = man.y;
mc.yaw = man.r;
mc.roll = man.z;
/* fake RC channels with manual control input from simulator */
if (rc_pub == 0) {
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
} else {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
}
if (mc_pub == 0) {
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
} else {
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
}
}
}
}
@@ -1507,8 +1569,6 @@ int mavlink_thread_main(int argc, char *argv[])
mavlink_wpm_init(wpm);
uint16_t counter = 0;
/* arm counter to go off immediately */
int lowspeed_counter = 10;
/* make sure all threads have registered their subscriptions */
while (!mavlink_subs.initialized) {
@@ -1516,33 +1576,33 @@ int mavlink_thread_main(int argc, char *argv[])
}
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 921600) {
/* 500 Hz / 2 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
if (baudrate >= 460800) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 460800) {
/* 250 Hz / 4 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
/* 100 Hz / 10 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
} else if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
@@ -1550,18 +1610,19 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 0.2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
/* 1 Hz / 1000 ms */
@@ -1574,6 +1635,9 @@ int mavlink_thread_main(int argc, char *argv[])
thread_running = true;
/* arm counter to go off immediately */
int lowspeed_counter = 10;
while (!thread_should_exit) {
/* get local and global position */
@@ -1598,7 +1662,7 @@ int mavlink_thread_main(int argc, char *argv[])
int32_t system_type;
param_get(param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_AUTOPILOT_ENUM_END) {
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type;
}
@@ -1608,20 +1672,16 @@ int mavlink_thread_main(int argc, char *argv[])
get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, system_type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f,
v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm,
v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4);
/* send over MAVLink */
if ((hrt_absolute_time() - rc.timestamp) < 200000) {
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
}
lowspeed_counter = 0;
}
lowspeed_counter++;
@@ -1708,7 +1768,7 @@ int mavlink_main(int argc, char *argv[])
}
thread_should_exit = false;
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 6000, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}

Some files were not shown because too many files have changed in this diff Show More