fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 23:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware

Browse Source
This commit is contained in:
Thomas Gubler
2012-09-24 20:08:38 +02:00
parent cd886ccff1 eaa6d0d4f2
commit c8ea0560e5
1119 changed files with 38043 additions and 64640 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>
Debug/NuttX
+26 -2
View File
@@ -73,6 +73,30 @@ document showheap
. Prints the contents of the malloc heap(s).
end
################################################################################
# Task file listing
################################################################################
define showfiles
set $task = (struct _TCB *)$arg0
set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file)
printf "%d files\n", $nfiles
set $index = 0
while $index < $nfiles
set $file = &($task->filelist->fl_files[$index])
printf "%d: inode %p f_priv %p\n", $index, $file->f_inode, $file->f_priv
if $file->f_inode != 0
printf " i_name %s i_private %p\n", &$file->f_inode->i_name[0], $file->f_inode->i_private
end
set $index = $index + 1
end
end
document showfiles
. showfiles <TCB pointer>
. Prints the files opened by a task.
end
################################################################################
# Task display
################################################################################
@@ -137,8 +161,8 @@ end
#
define _showsemaphore
printf "count %d ", $arg0->semcount
if $arg0->hlist.holder != 0
set $_task = (struct _TCB *)$arg0->hlist.holder
if $arg0->holder.htcb != 0
set $_task = (struct _TCB *)$arg0->holder.htcb
printf "held by %s", $_task->name
end
printf "\n"
Firmware.sublime-project
+7 -2
View File
@@ -10,16 +10,21 @@
".built",
".context",
".depend",
".config",
".version",
"Make.dep",
".configured",
"*.sublime-project",
"*.sublime-workspace",
".project",
".cproject"
".cproject",
"cscope.out"
],
"folder_exclude_patterns":
[
".settings"
".settings",
"nuttx/arch/arm/src/board",
"nuttx/arch/arm/src/chip"
]
}
],
ROMFS/Makefile
+5 -1
View File
@@ -21,11 +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_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)/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/mixers/FMU_quad_+.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a quadrotor in the + configuration. All controls
are mixed 100%.
R: 4+ 10000 10000 10000 0
ROMFS/mixers/FMU_multirotor.mix → ROMFS/mixers/FMU_quad_x.mix
+3 -3
View File
@@ -1,7 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a quadrotor in the X configuration, with 10% contribution from
roll and pitch and 20% contribution from yaw and no deadband.
This file defines a single mixer for a quadrotor in the X configuration. All controls
are mixed 100%.
R: 4x 1000 1000 2000 0
R: 4x 10000 10000 10000 0
ROMFS/scripts/rc.FMU_quad_x
+40
View File
@@ -0,0 +1,40 @@
#
# Startup for X-quad on FMU1.5/1.6
#
echo "[init] uORB"
uorb start
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
fi
echo "[init] sensors"
#bma180 start
#l3gd20 start
mpu6000 start
hmc5883 start
ms5611 start
sensors start
echo "[init] mavlink"
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
echo "[init] commander"
commander start
echo "[init] attitude control"
attitude_estimator_bm &
multirotor_att_control start
echo "[init] starting PWM output"
fmu mode_pwm
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
echo "[init] startup done, exiting"
exit
ROMFS/scripts/rc.PX4IO
+1 -1
View File
@@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors
#
# Start MAVLink
#
mavlink -d /dev/ttyS0 -b 57600 &
mavlink start -d /dev/ttyS0 -b 57600
#
# Start the commander.
ROMFS/scripts/rc.PX4IOAR
+1 -1
View File
@@ -18,7 +18,7 @@ sh /etc/init.d/rc.sensors
#
# Start MAVLink
#
mavlink -d /dev/ttyS0 -b 57600 &
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
ROMFS/scripts/rc.sensors
+4 -4
View File
@@ -7,14 +7,14 @@
# Start sensor drivers here.
#
#ms5611 start
ms5611 start
mpu6000 start
hmc5883 start
#
# Start the sensor collection task.
#
# XXX should be 'sensors start'
#
sensors &
sensors start
#
# Test sensor functionality
ROMFS/scripts/rcS
+13 -4
View File
@@ -18,7 +18,7 @@
# can change this to prevent automatic startup of the flight script.
#
set MODE autostart
set USB no
set USB autoconnect
#
# Try to mount the microSD card.
@@ -31,6 +31,8 @@ else
echo "[init] no microSD card found"
fi
usleep 500
#
# Look for an init script on the microSD card.
#
@@ -46,11 +48,16 @@ fi
#
# Check for USB host
#
if sercon
if [ $USB != autoconnect ]
then
echo "[init] USB interface connected"
echo "[init] not connecting USB"
else
echo "[init] No USB connected"
if sercon
then
echo "[init] USB interface connected"
else
echo "[init] No USB connected"
fi
fi
#
@@ -77,6 +84,7 @@ else
if [ -f /etc/init.d/rc.PX4IOAR ]
then
echo "[init] reading /etc/init.d/rc.PX4IOAR"
usleep 500
sh /etc/init.d/rc.PX4IOAR
fi
else
@@ -92,6 +100,7 @@ else
if [ -f /etc/init.d/rc.PX4IO ]
then
echo "[init] reading /etc/init.d/rc.PX4IO"
usleep 500
sh /etc/init.d/rc.PX4IO
fi
else
Tools/px_uploader.py
+29 -15
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
@@ -111,8 +113,8 @@ class uploader(object):
READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64
def __init__(self, portname, baudrate):
# open the port
self.port = serial.Serial(portname, baudrate, timeout=10)
# open the port, keep the default timeout short so we can poll quickly
self.port = serial.Serial(portname, baudrate, timeout=0.25)
def close(self):
if self.port is not None:
@@ -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):
@@ -171,7 +173,17 @@ class uploader(object):
def __erase(self):
self.__send(uploader.CHIP_ERASE
+ uploader.EOC)
self.__getSync()
# erase is very slow, give it 10s
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):
@@ -186,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))
@@ -228,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
+62 -4
View File
@@ -248,7 +248,7 @@
being set to the priority of the parent thread; losing its configured
priority. Reported by Mike Smith.
6.21 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
6.21 2012-08-25 Gregory Nutt <gnutt@nuttx.org>
* apps/include/: Stylistic clean-up of all header files.
* apps/modbus and apps/include/modbus: A port of freemodbus-v1.5.0
@@ -259,9 +259,9 @@
* apps/modbus: Add CONFIG_MB_TERMIOS. If the driver doesn't support
termios ioctls, then don't bother trying to configure the baud, parity
etc.
* apps/nslib: If waitpid() is supported, then NSH not catches the
* apps/nshlib: If waitpid() is supported, then NSH now catches the
return value from spawned applications (provided by Mike Smith)
* apps/nslib: Lock the schedule while starting built-in applications
* apps/nshlib: Lock the scheduler while starting built-in applications
in order to eliminate race conditions (also from Mike Smith).
* apps/examples/adc, pwm, and qencoder: Add support for testing
devices with multiple ADC, PWM, and QE devices.
@@ -272,9 +272,67 @@
properties of mounted file systems.
* apps/nshlib/nsh_parse.c: Extend help command options. 'help' with
no arguments outputs a short list of commands. With -v lists all
command line details. And command name can be added to just get
command line details. A command name can be added to just get
help on one command.
* system/readline.c: If character input/output is interrupted by a
signal, then readline() will try the read/write again.
* apps/*/Make.defs: Numerous fixes needed to use the automated
configuration (from Richard Cochran).
6.22 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
* apps/netutils/thttpd/thttpd_cgi.c: Missing NULL in argv[]
list (contributed by Kate).
* apps/nshlib/nsh_parse.c: CONFIG_NSH_DISABLE_WGET not CONFIG_NSH_DISABLE_GET
in one location (found by Kate).
* apps/examples/ostest/prioinherit.c: Limit the number of test
threds to no more than 3 of each priority. Bad things happen
when the existing logic tried to created several hundred test
treads!
* apps/nshlib/nsh.h: Both CONFIG_LIBC_STRERROR and CONFIG_NSH_STRERROR
must be defined to use strerror() with NSH.
* apps/examples/*/*_main.c, system/i2c/i2c_main.c, and others: Added
configuration variable CONFIG_USER_ENTRYPOINT that may be used to change
the default entry from user_start to some other symbol. Contributed by
Kate.
* apps/netutils/webserver/httpd/c: Fix a typo that as introduced in
version r4402: 'lese' instead of 'else' (Noted by Max Holtzberg).
* tools/mkfsdata.pl: The uIP web server CGI image making perl script was
moved from apps/netutils/webserver/makefsdata to nuttx/tools/mkfsdata.pl
(Part of a larger change submitted by Max Holtzberg).
* apps/netutils/webserver, apps/examples/uip, and apps/include/netutils/httpd.h:
The "canned" version of the uIP web servers content that was at
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/README.txt
+1 -1
View File
@@ -118,7 +118,7 @@ the NuttX configuration file:
CONFIG_BUILTIN_APP_START=<application name>
that application shall be invoked immediately after system starts
*instead* of the normal, default "user_start" entry point.
*instead* of the default "user_start" entry point.
Note that <application name> must be provided as: "hello",
will call:
apps/ardrone_control/.context
View File
apps/ardrone_control/ardrone_control.c
-319
View File
@@ -1,319 +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>
__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));
/* 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, motor_test_mode,
&roll_control, &pitch_control, &yaw_control, &thrust_control);
ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, 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_control_helper.c
-60
View File
@@ -1,60 +0,0 @@
#include "ardrone_control_helper.h"
#include <unistd.h>
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
// int read_sensors_raw(sensors_raw_t *sensors_raw)
// {
// static int ret;
// ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full);
// if (ret == 0) {
// memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw));
// // printf("Timestamp %d\n", &global_data_sensors_raw->timestamp);
// } else {
// printf("Controller timeout, no new sensor values available\n");
// }
// global_data_unlock(&global_data_sensors_raw->access_conf_rate_full);
// return ret;
// }
// int read_attitude(global_data_attitude_t *attitude)
// {
// static int ret;
// ret = global_data_wait(&global_data_attitude->access_conf);
// if (ret == 0) {
// memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll));
// memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch));
// memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw));
// memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed));
// memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed));
// memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed));
// } else {
// printf("Controller timeout, no new attitude values available\n");
// }
// global_data_unlock(&global_data_attitude->access_conf);
// return ret;
// }
// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint)
// {
// if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock?
// rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw;
// rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne;
// rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se;
// rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw;
// global_data_unlock(&global_data_quad_motors_setpoint->access_conf);
// }
// }
apps/ardrone_control/ardrone_control_helper.h
-21
View File
@@ -1,21 +0,0 @@
/*
* ardrone_control_helper.h
*
* Created on: May 15, 2012
* Author: thomasgubler
*/
#ifndef ARDRONE_CONTROL_HELPER_H_
#define ARDRONE_CONTROL_HELPER_H_
#include <stdint.h>
// typedef struct {
// int16_t gyro_raw[3]; // l3gd20
// } sensors_raw_t;
// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available
// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint);
#endif /* ARDRONE_CONTROL_HELPER_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/attitude_control.c
-349
View File
@@ -1,349 +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,
bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output)
{
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;
}
*roll_output = roll_control;
*pitch_output = pitch_control;
*yaw_output = yaw_rate_control;
*thrust_output = motor_thrust;
}
void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) {
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.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
-321
View File
@@ -1,321 +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_control_helper.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/multirotor_position_control/Makefile → apps/ardrone_interface/Makefile
+2 -5
View File
@@ -32,14 +32,11 @@
############################################################################
#
# Makefile to build uORB
# Makefile to build ardrone interface
#
APPNAME = ardrone_control
APPNAME = ardrone_interface
PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 2048
# explicit list of sources - not everything is built currently
CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
include $(APPDIR)/mk/app.mk
apps/ardrone_interface/ardrone_interface.c
+379
View File
@@ -0,0 +1,379 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @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_interface.c
* Implementation of AR.Drone 1.0 / 2.0 motor 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 <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include "ardrone_motor_control.h"
__EXPORT int ardrone_interface_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int ardrone_interface_task; /**< Handle of deamon task / thread */
static int ardrone_write; /**< UART to write AR.Drone commands to */
/**
* Mainloop of ardrone_interface.
*/
int ardrone_interface_thread_main(int argc, char *argv[]);
/**
* Open the UART connected to the motor controllers
*/
static int ardrone_open_uart(struct termios *uart_config_original);
/**
* 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: ardrone_interface {start|stop|status} [-d <UART>]\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 ardrone_interface_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ardrone_interface already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
ardrone_interface_task = task_create("ardrone_interface", SCHED_PRIORITY_MAX - 15, 4096, ardrone_interface_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tardrone_interface is running\n");
} else {
printf("\tardrone_interface not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
static int ardrone_open_uart(struct termios *uart_config_original)
{
/* baud rate */
int speed = B115200;
int uart;
const char* uart_name = "/dev/ttyS1";
/* open uart */
printf("[ardrone_interface] UART is /dev/ttyS1, baud rate is 115200\n");
uart = open(uart_name, O_RDWR | O_NOCTTY);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
/* Fill the struct for the new configuration */
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
return uart;
}
int ardrone_interface_thread_main(int argc, char *argv[])
{
thread_running = true;
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
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;
if (motor_test_mode) {
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
}
/* 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 actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
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);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// XXX Re-done initialization to make sure it is accepted by the motors
// XXX should be removed after more testing, but no harm
/* close uarts */
close(ardrone_write);
//ar_multiplexing_deinit(gpios);
/* 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);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
while (!thread_should_exit) {
if (motor_test_mode) {
/* set motors to idle speed */
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 */
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
}
if (counter % 16 == 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);
counter++;
}
/* restore old UART config */
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
fflush(stdout);
thread_running = false;
return OK;
}
apps/ardrone_interface/ardrone_motor_control.c
+470
View File
@@ -0,0 +1,470 @@
/****************************************************************************
*
* Copyright (C) 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 <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#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"
static unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
static 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;
#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
/**
* @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) {
warn("GPIO: open fail");
return fd;
}
/* deactivate all outputs */
if (ioctl(fd, GPIO_SET, motor_gpios)) {
warn("GPIO: clearing pins fail");
close(fd);
return -1;
}
/* configure all motor select GPIOs as outputs */
if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
warn("GPIO: output set fail");
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;
/*
* 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 {
/* select reqested motor */
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
}
return ret;
}
int ar_deselect_motor(int fd, uint8_t motor)
{
int ret = 0;
/*
* Four GPIOS:
* GPIO_EXT1
* GPIO_EXT2
* GPIO_UART2_CTS
* GPIO_UART2_RTS
*/
if (motor == 0) {
/* deselect motor 1-4 */
ret += ioctl(fd, GPIO_SET, motor_gpios);
} else {
/* deselect reqested motor */
ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]);
}
return ret;
}
int ar_init_motors(int ardrone_uart, int gpios)
{
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
/* deselect all motors */
ar_deselect_motor(gpios, 0);
/* initialize all motors
* - select one motor at a time
* - configure motor
*/
int i;
int errcounter = 0;
/* initial setup run */
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
errcounter += ar_select_motor(gpios, i);
usleep(200);
/*
* write 0xE0 - request status
* receive one status byte
*/
write(ardrone_uart, &(initbuf[0]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* write 0x91 - request checksum
* receive 120 status bytes
*/
write(ardrone_uart, &(initbuf[1]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*120);
/*
* write 0xA1 - set status OK
* receive one status byte - should be A0
* to confirm status is OK
*/
write(ardrone_uart, &(initbuf[2]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* set as motor i, where i = 1..4
* receive nothing
*/
initbuf[3] = (uint8_t)i;
write(ardrone_uart, &(initbuf[3]), 1);
fsync(ardrone_uart);
/*
* write 0x40 - check version
* receive 11 bytes encoding the version
*/
write(ardrone_uart, &(initbuf[4]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* 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
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/*
* 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 (fsync is part of the write command */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] 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 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 {
return ret;
}
} else {
return -ERROR;
}
}
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
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[4] = {0};
float motor_calc[4] = {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);
}
//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);
}
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;
}
}
/* 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);
/* 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 */
ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
}
apps/ardrone_control/ardrone_motor_control.h → apps/ardrone_interface/ardrone_motor_control.h
+25 -11
View File
@@ -1,7 +1,6 @@
/****************************************************************************
* px4/ardrone_offboard_control.h
*
* Copyright (C) 2012 PX4 Autopilot Project. All rights reserved.
* Copyright (C) 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
@@ -14,7 +13,7 @@
* 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
* 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.
*
@@ -32,14 +31,14 @@
* 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>
/**
* @file ardrone_motor_control.h
* Definition of AR.Drone 1.0 / 2.0 motor control interface
*/
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
/**
* Generate the 5-byte motor set packet.
@@ -50,9 +49,20 @@ void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, u
/**
* Select a motor in the multiplexing.
*
* @param fd GPIO file descriptor
* @param motor Motor number, from 1 to 4, 0 selects all
*/
int ar_select_motor(int fd, uint8_t motor);
/**
* Deselect a motor in the multiplexing.
*
* @param fd GPIO file descriptor
* @param motor Motor number, from 1 to 4, 0 deselects all
*/
int ar_deselect_motor(int fd, uint8_t motor);
void ar_enable_broadcast(int fd);
int ar_multiplexing_init(void);
@@ -70,7 +80,11 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
/**
* Initialize the motors.
*/
int ar_init_motors(int ardrone_uart, int *gpios_pin);
int ar_init_motors(int ardrone_uart, int gpio);
/**
* Set LED pattern.
*/
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);
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+3 -8
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>
@@ -138,7 +133,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
/* subscribe to raw data */
int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
/* advertise attitude */
int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
apps/commander/Makefile
+1 -1
View File
@@ -37,7 +37,7 @@
APPNAME = commander
PRIORITY = SCHED_PRIORITY_MAX - 30
STACKSIZE = 4096
STACKSIZE = 2048
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
apps/commander/commander.c
+513 -247
View File
File diff suppressed because it is too large Load Diff
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
+32 -12
View File
@@ -39,14 +39,17 @@
*/
#include <stdio.h>
#include "state_machine_helper.h"
#include <unistd.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <systemlib/systemlib.h>
#include <arch/board/up_hrt.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
static const char* system_state_txt[] = {
"SYSTEM_STATE_PREFLIGHT",
"SYSTEM_STATE_STANDBY",
@@ -75,14 +78,12 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
switch (new_state) {
case SYSTEM_STATE_MISSION_ABORT: {
/* Indoor or outdoor */
uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
} else {
ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
}
// } else {
// ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
// }
}
break;
@@ -121,7 +122,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (current_status->state_machine == SYSTEM_STATE_STANDBY
|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
/* set system flags according to state */
current_status->flag_system_armed = true;
current_status->flag_system_armed = false;
mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
} else {
invalid_state = true;
@@ -198,10 +199,14 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
if (invalid_state == false || old_state != new_state) {
current_status->state_machine = new_state;
state_machine_publish(status_pub, current_status, mavlink_fd);
publish_armed_status(current_status);
ret = OK;
}
if (invalid_state) {
mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
ret = ERROR;
}
return ret;
}
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
@@ -212,6 +217,17 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
}
void publish_armed_status(const struct vehicle_status_s *current_status) {
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* 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);
}
/*
* Private functions, update the state machine
@@ -485,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) {
@@ -522,7 +540,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
{
printf("in update state request\n");
printf("[commander] Requested new mode: %d\n", (int)mode);
uint8_t ret = 1;
/* vehicle is disarmed, mode requests arming */
@@ -546,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/commander/state_machine_helper.h
+6
View File
@@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
*/
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Publish the armed state depending on the current system state
*
* @param current_status the current system status
*/
void publish_armed_status(const struct vehicle_status_s *current_status);
apps/drivers/bma180/bma180.cpp
+382 -250
View File
@@ -32,17 +32,17 @@
****************************************************************************/
/**
* @file Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
* @file bma180.cpp
* Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
*/
#include <nuttx/config.h>
#include <device/spi.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
@@ -52,14 +52,74 @@
#include <math.h>
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <arch/board/up_hrt.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
#define ADDR_ACC_X_LSB 0x02
#define ADDR_ACC_Y_LSB 0x04
#define ADDR_ACC_Z_LSB 0x06
#define ADDR_TEMPERATURE 0x08
#define ADDR_CTRL_REG0 0x0D
#define REG0_WRITE_ENABLE 0x10
#define ADDR_RESET 0x10
#define SOFT_RESET 0xB6
#define ADDR_BW_TCS 0x20
#define BW_TCS_BW_MASK (0xf<<4)
#define BW_TCS_BW_10HZ (0<<4)
#define BW_TCS_BW_20HZ (1<<4)
#define BW_TCS_BW_40HZ (2<<4)
#define BW_TCS_BW_75HZ (3<<4)
#define BW_TCS_BW_150HZ (4<<4)
#define BW_TCS_BW_300HZ (5<<4)
#define BW_TCS_BW_600HZ (6<<4)
#define BW_TCS_BW_1200HZ (7<<4)
#define ADDR_HIGH_DUR 0x27
#define HIGH_DUR_DIS_I2C (1<<0)
#define ADDR_TCO_Z 0x30
#define TCO_Z_MODE_MASK 0x3
#define ADDR_GAIN_Y 0x33
#define GAIN_Y_SHADOW_DIS (1<<0)
#define ADDR_OFFSET_LSB1 0x35
#define OFFSET_LSB1_RANGE_MASK (7<<1)
#define OFFSET_LSB1_RANGE_1G (0<<1)
#define OFFSET_LSB1_RANGE_2G (2<<1)
#define OFFSET_LSB1_RANGE_3G (3<<1)
#define OFFSET_LSB1_RANGE_4G (4<<1)
#define OFFSET_LSB1_RANGE_8G (5<<1)
#define OFFSET_LSB1_RANGE_16G (6<<1)
#define ADDR_OFFSET_T 0x37
#define OFFSET_T_READOUT_12BIT (1<<0)
extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
class BMA180 : public device::SPI
@@ -73,9 +133,6 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/**
* Diagnostics - print some basic information about the driver.
*/
@@ -94,10 +151,15 @@ private:
volatile unsigned _oldest_report;
struct accel_report *_reports;
struct accel_scale _scale;
float _range_scale;
struct accel_scale _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
unsigned _reads;
unsigned _current_lowpass;
unsigned _current_range;
perf_counter_t _sample_perf;
/**
* Start automatic measurement.
@@ -161,88 +223,44 @@ private:
int set_range(unsigned max_g);
/**
* Set the BMA180 lowpass filter.
* Set the BMA180 internal lowpass filter frequency.
*
* @param frequency Set the lowpass filter cutoff frequency to no less than
* this frequency.
* @param frequency The internal lowpass filter frequency is set to a value
* equal or greater to this.
* Zero selects the highest frequency supported.
* @return OK if the value can be supported.
*/
int set_bandwidth(unsigned frequency);
int set_lowpass(unsigned frequency);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
#define ADDR_ACC_X_LSB 0x02
#define ADDR_ACC_Y_LSB 0x04
#define ADDR_ACC_Z_LSB 0x06
#define ADDR_TEMPERATURE 0x08
#define ADDR_RESET 0x10
#define SOFT_RESET 0xB6
#define ADDR_BW_TCS 0x20
#define BW_TCS_BW_MASK (0xf<<4)
#define BW_TCS_BW_10HZ (0<<4)
#define BW_TCS_BW_20HZ (1<<4)
#define BW_TCS_BW_40HZ (2<<4)
#define BW_TCS_BW_75HZ (3<<4)
#define BW_TCS_BW_150HZ (4<<4)
#define BW_TCS_BW_300HZ (5<<4)
#define BW_TCS_BW_600HZ (6<<4)
#define BW_TCS_BW_1200HZ (7<<4)
#define ADDR_HIGH_DUR 0x27
#define HIGH_DUR_DIS_I2C (1<<0)
#define ADDR_TCO_Z 0x30
#define TCO_Z_MODE_MASK 0x3
#define ADDR_GAIN_Y 0x33
#define GAIN_Y_SHADOW_DIS (1<<0)
#define ADDR_OFFSET_LSB1 0x35
#define OFFSET_LSB1_RANGE_MASK (7<<1)
#define OFFSET_LSB1_RANGE_1G (0<<1)
#define OFFSET_LSB1_RANGE_2G (2<<1)
#define OFFSET_LSB1_RANGE_3G (3<<1)
#define OFFSET_LSB1_RANGE_4G (4<<1)
#define OFFSET_LSB1_RANGE_8G (5<<1)
#define OFFSET_LSB1_RANGE_16G (6<<1)
#define ADDR_OFFSET_T 0x37
#define OFFSET_T_READOUT_12BIT (1<<0)
/*
* Driver 'main' command.
*/
extern "C" { int bma180_main(int argc, char *argv[]); }
BMA180::BMA180(int bus, spi_dev_e device) :
SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_reads(0)
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_current_lowpass(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
{
// enable debug() calls
_debug_enabled = true;
// default scale factors
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
}
BMA180::~BMA180()
@@ -253,77 +271,73 @@ BMA180::~BMA180()
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
/* delete the perf counter */
perf_free(_sample_perf);
}
int
BMA180::init()
{
int ret;
int ret = ERROR;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup successful, finish chip init */
if (ret == OK) {
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
/* wait 10us (p49) */
usleep(10);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
}
return ret;
}
int
BMA180::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_call_interval = 0;
if (SPI::init() != OK)
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct accel_report[_num_reports];
_oldest_report = _next_report = 0;
_reports = new struct accel_report[_num_reports];
if (_reports == nullptr)
goto out;
/* set default range and lowpass */
set_range(4); /* 4G */
set_bandwidth(600); /* 600Hz */
/* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]);
return OK;
}
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
int
BMA180::close_last(struct file *filp)
{
/* stop measurement */
stop();
/* wait 10 ms (datasheet incorrectly lists 10 us on page 49) */
usleep(10000);
/* free report buffers */
if (_reports != nullptr) {
delete[] _reports;
_num_reports = 0;
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* disable I2C interface */
modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
/* switch to low-noise mode */
modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
/* disable 12-bit mode */
modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
/* disable shadow-disable mode */
modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) warnx("Failed setting range");
if (set_lowpass(75)) warnx("Failed setting lowpass");
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
} else {
ret = ERROR;
}
return OK;
out:
return ret;
}
int
BMA180::probe()
{
/* dummy read to ensure SPI state machine is sane */
read_reg(ADDR_CHIP_ID);
if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
return OK;
@@ -356,8 +370,6 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
}
}
_reads++;
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
@@ -378,22 +390,29 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ACCELIOCSPOLLRATE: {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case ACC_POLLRATE_MANUAL:
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case ACC_POLLRATE_EXTERNAL:
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
@@ -408,7 +427,7 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval;
_call.period = _call_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
@@ -419,7 +438,15 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
case ACCELIOCSQUEUEDEPTH: {
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
@@ -440,16 +467,41 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
return OK;
}
case ACCELIOCSLOWPASS:
return set_bandwidth(arg);
case SENSORIOCGQUEUEDEPTH:
return _num_reports -1;
case ACCELIORANGE:
return set_range(arg);
case SENSORIOCRESET:
/* XXX implement */
return -EINVAL;
case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
case ACCELIOCSREPORTFORMAT: /* no alternate report formats */
return -EINVAL;
case ACCELIOCGSAMPLERATE:
return 1200; /* always operating in low-noise mode */
case ACCELIOCSLOWPASS:
return set_lowpass(arg);
case ACCELIOCGLOWPASS:
return _current_lowpass;
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
return set_range(arg);
case ACCELIOCGRANGE:
return _current_range;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
@@ -494,45 +546,51 @@ int
BMA180::set_range(unsigned max_g)
{
uint8_t rangebits;
float rangescale;
if (max_g > 16) {
if (max_g == 0)
max_g = 16;
if (max_g > 16)
return -ERANGE;
} else if (max_g > 8) { /* 16G */
rangebits = OFFSET_LSB1_RANGE_16G;
rangescale = 1.98;
} else if (max_g > 4) { /* 8G */
rangebits = OFFSET_LSB1_RANGE_8G;
rangescale = 0.99;
} else if (max_g > 3) { /* 4G */
rangebits = OFFSET_LSB1_RANGE_4G;
rangescale = 0.5;
} else if (max_g > 2) { /* 3G */
rangebits = OFFSET_LSB1_RANGE_3G;
rangescale = 0.38;
} else if (max_g > 1) { /* 2G */
if (max_g <= 2) {
_current_range = 2;
rangebits = OFFSET_LSB1_RANGE_2G;
rangescale = 0.25;
} else { /* 1G */
rangebits = OFFSET_LSB1_RANGE_1G;
rangescale = 0.13;
} else if (max_g <= 3) {
_current_range = 3;
rangebits = OFFSET_LSB1_RANGE_3G;
} else if (max_g <= 4) {
_current_range = 4;
rangebits = OFFSET_LSB1_RANGE_4G;
} else if (max_g <= 8) {
_current_range = 8;
rangebits = OFFSET_LSB1_RANGE_8G;
} else if (max_g <= 16) {
_current_range = 16;
rangebits = OFFSET_LSB1_RANGE_16G;
} else {
return -EINVAL;
}
/* set new range scaling factor */
_accel_range_m_s2 = _current_range * 9.80665f;
_accel_range_scale = _accel_range_m_s2 / 8192.0f;
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
_range_scale = rangescale;
return OK;
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_OFFSET_LSB1) & OFFSET_LSB1_RANGE_MASK) ==
(OFFSET_LSB1_RANGE_MASK & rangebits));
}
int
BMA180::set_bandwidth(unsigned frequency)
BMA180::set_lowpass(unsigned frequency)
{
uint8_t bwbits;
@@ -564,10 +622,18 @@ BMA180::set_bandwidth(unsigned frequency)
bwbits = BW_TCS_BW_10HZ;
}
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
/* adjust sensor configuration */
modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
return OK;
/* block writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
(BW_TCS_BW_MASK & bwbits));
}
void
@@ -601,24 +667,30 @@ BMA180::measure_trampoline(void *arg)
void
BMA180::measure()
{
/*
* This evil is to deal with the stupid layout of the BMA180
* measurement registers vs. the SPI transaction model.
*/
union {
uint8_t bytes[10];
uint16_t words[5];
} buf;
/* BMA180 measurement registers */
// #pragma pack(push, 1)
// struct {
// uint8_t cmd;
// int16_t x;
// int16_t y;
// int16_t z;
// } raw_report;
// #pragma pack(pop)
accel_report *report = &_reports[_next_report];
/* start the performance counter */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMA180 in one pass;
* 7 bytes starting from the X LSB.
* starting from the X LSB.
*/
buf.bytes[1] = ADDR_ACC_X_LSB;
transfer(&buf.bytes[1], &buf.bytes[1], 8);
//raw_report.cmd = ADDR_ACC_X_LSB;
// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
/*
* Adjust and scale results to mg.
* Adjust and scale results to SI units.
*
* Note that we ignore the "new data" bits. At any time we read, each
* of the axis measurements are the "most recent", even if we've seen
@@ -626,13 +698,28 @@ BMA180::measure()
* measurement flow without using the external interrupt.
*/
_reports[_next_report].timestamp = hrt_absolute_time();
_reports[_next_report].x = (buf.words[1] >> 2) * _range_scale;
_reports[_next_report].y = (buf.words[2] >> 2) * _range_scale;
_reports[_next_report].z = (buf.words[3] >> 2) * _range_scale;
/*
* @todo Apply additional scaling / calibration factors here.
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report->y_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+1)) << 8) | (read_reg(ADDR_ACC_X_LSB));// XXX PX4DEV raw_report.x;
report->x_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+3)) << 8) | (read_reg(ADDR_ACC_X_LSB+2));// XXX PX4DEV raw_report.y;
report->z_raw = (((int16_t)read_reg(ADDR_ACC_X_LSB+5)) << 8) | (read_reg(ADDR_ACC_X_LSB+4));// XXX PX4DEV raw_report.z;
/* discard two non-value bits in the 16 bit measurement */
report->x_raw = (report->x_raw >> 2);
report->y_raw = (report->y_raw >> 2);
report->z_raw = (report->z_raw >> 2);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
report->y_raw = -report->y_raw;
report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report->scaling = _accel_range_scale;
report->range_m_s2 = _accel_range_m_s2;
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
@@ -643,12 +730,18 @@ BMA180::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, report);
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMA180::print_info()
{
printf("reads: %u\n", _reads);
perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
}
@@ -656,67 +749,126 @@ BMA180::print_info()
/**
* Local functions in support of the shell command.
*/
namespace
namespace bma180
{
BMA180 *g_dev;
/*
* XXX this should just be part of the generic sensors test...
*/
void start();
void test();
void reset();
void info();
int
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new BMA180(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_ACCEL);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd = -1;
struct accel_report report;
struct accel_report a_report;
ssize_t sz;
const char *reason = "test OK";
do {
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'bma180 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling");
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report))
err(1, "immediate acc read failed");
if (fd < 0) {
reason = "can't open driver";
break;
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / 9.81f));
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
/* XXX add poll-rate tests here too */
if (sz != sizeof(report)) {
reason = "immediate read failed";
break;
}
printf("single read\n");
fflush(stdout);
printf("time: %lld\n", report.timestamp);
printf("x: %f\n", report.x);
printf("y: %f\n", report.y);
printf("z: %f\n", report.z);
} while (0);
printf("BMA180: %s\n", reason);
return OK;
reset();
errx(0, "PASS");
}
int
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
fprintf(stderr, "BMA180: driver not running\n");
return -ENOENT;
}
if (g_dev == nullptr)
errx(1, "BMA180: driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
return OK;
exit(0);
}
@@ -727,48 +879,28 @@ bma180_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "BMA180: already loaded\n");
return -EBUSY;
}
/* create the driver */
g_dev = new BMA180(CONFIG_BMA180_SPI_BUS, (spi_dev_e)CONFIG_BMA180_SPI_DEVICE);
if (g_dev == nullptr) {
fprintf(stderr, "BMA180: driver alloc failed\n");
return -ENOMEM;
}
if (OK != g_dev->init()) {
fprintf(stderr, "BMA180: driver init failed\n");
usleep(100000);
delete g_dev;
g_dev = nullptr;
return -EIO;
}
printf("BMA180: driver started\n");
return OK;
}
if (!strcmp(argv[1], "start"))
bma180::start();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
return test();
bma180::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
bma180::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
return info();
bma180::info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
return -EINVAL;
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}
apps/drivers/device/i2c.cpp
+39 -21
View File
@@ -53,6 +53,7 @@ I2C::I2C(const char *name,
CDev(name, devname, irq),
// public
// protected
_retries(0),
// private
_bus(bus),
_address(address),
@@ -72,7 +73,7 @@ I2C::init()
{
int ret = OK;
// attach to the i2c bus
/* attach to the i2c bus */
_dev = up_i2cinitialize(_bus);
if (_dev == nullptr) {
@@ -98,7 +99,7 @@ I2C::init()
}
// tell the world where we are
log("on bus %d at 0x%02x", _bus, _address);
log("on I2C bus %d at 0x%02x", _bus, _address);
out:
return ret;
@@ -117,33 +118,50 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len
struct i2c_msg_s msgv[2];
unsigned msgs;
int ret;
unsigned tries = 0;
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
do {
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
msgs = 0;
msgs = 0;
if (send_len > 0) {
msgv[msgs].addr = _address;
msgv[msgs].flags = 0;
msgv[msgs].buffer = send;
msgv[msgs].length = send_len;
msgs++;
}
if (send_len > 0) {
msgv[msgs].addr = _address;
msgv[msgs].flags = 0;
msgv[msgs].buffer = send;
msgv[msgs].length = send_len;
msgs++;
}
if (recv_len > 0) {
msgv[msgs].addr = _address;
msgv[msgs].flags = I2C_M_READ;
msgv[msgs].buffer = recv;
msgv[msgs].length = recv_len;
msgs++;
}
if (recv_len > 0) {
msgv[msgs].addr = _address;
msgv[msgs].flags = I2C_M_READ;
msgv[msgs].buffer = recv;
msgv[msgs].length = recv_len;
msgs++;
}
if (msgs == 0)
return -EINVAL;
if (msgs == 0)
return -EINVAL;
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
/*
* I2C architecture means there is an unavoidable race here
* if there are any devices on the bus with a different frequency
* preference. Really, this is pointless.
*/
I2C_SETFREQUENCY(_dev, _frequency);
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
if (ret == OK)
break;
// reset the I2C bus to unwedge on error
up_i2creset(_dev);
} while (tries++ < _retries);
return ret;
}
} // namespace device
apps/drivers/device/i2c.h
+6
View File
@@ -52,6 +52,12 @@ class __EXPORT I2C : public CDev
{
protected:
/**
* The number of times a read or write operation will be retried on
* error.
*/
unsigned _retries;
/**
* @ Constructor
*
apps/drivers/device/spi.cpp
+13 -3
View File
@@ -35,8 +35,16 @@
* @file Base class for devices connected via SPI.
*
* @todo Work out if caching the mode/frequency would save any time.
*
* @todo A separate bus/device abstraction would allow for mixed interrupt-mode
* and non-interrupt-mode clients to arbitrate for the bus. As things stand,
* a bus shared between clients of both kinds is vulnerable to races between
* the two, where an interrupt-mode client will ignore the lock held by the
* non-interrupt-mode client.
*/
#include <nuttx/arch.h>
#include "spi.h"
#ifndef CONFIG_SPI_EXCHANGE
@@ -103,7 +111,7 @@ SPI::init()
}
// tell the workd where we are
log("on bus %d at %d", _bus, _device);
log("on SPI bus %d at %d", _bus, _device);
out:
return ret;
@@ -124,7 +132,8 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
return -EINVAL;
/* do common setup */
SPI_LOCK(_dev, true);
if (!up_interrupt_context())
SPI_LOCK(_dev, true);
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
@@ -135,7 +144,8 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
/* and clean up */
SPI_SELECT(_dev, _device, false);
SPI_LOCK(_dev, false);
if (!up_interrupt_context())
SPI_LOCK(_dev, false);
return OK;
}
apps/drivers/device/spi.h
+6
View File
@@ -81,6 +81,12 @@ protected:
/**
* Perform a SPI transfer.
*
* If called from interrupt context, this interface does not lock
* the bus and may interfere with non-interrupt-context callers.
*
* Clients in a mixed interrupt/non-interrupt configuration must
* ensure appropriate interlocking.
*
* At least one of send or recv must be non-null.
*
* @param send Bytes to send to the device, or nullptr if
apps/drivers/drv_accel.h
+30 -17
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define ACCEL_DEVICE_PATH "/dev/accel"
@@ -50,10 +51,18 @@
* structure.
*/
struct accel_report {
float x;
float y;
float z;
uint64_t timestamp;
float x; /**< acceleration in the NED X board axis in m/s^2 */
float y; /**< acceleration in the NED Y board axis in m/s^2 */
float z; /**< acceleration in the NED Z board axis in m/s^2 */
float temperature; /**< temperature in degrees celsius */
float range_m_s2; /**< range in m/s^2 (+- this value) */
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
@@ -73,33 +82,37 @@ ORB_DECLARE(sensor_accel);
/*
* ioctl() definitions
*
* Accelerometer drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _ACCELIOCBASE (0x2000)
#define _ACCELIOCBASE (0x2100)
#define _ACCELIOC(_n) (_IOC(_ACCELIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the ACC_POLLRATE constants */
#define ACCELIOCSPOLLRATE _ACCELIOC(0)
#define ACC_POLLRATE_MANUAL 1000000 /**< poll when read */
#define ACC_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */
/** set the internal queue depth to (arg) entries, must be at least 1 */
#define ACCELIOCSQUEUEDEPTH _ACCELIOC(1)
/** set the accel internal sample rate to at least (arg) Hz */
#define ACCELIOCSSAMPLERATE _ACCELIOC(2)
#define ACCELIOCSSAMPLERATE _ACCELIOC(0)
/** return the accel internal sample rate in Hz */
#define ACCELIOCGSAMPLERATE _ACCELIOC(1)
/** set the accel internal lowpass filter to no lower than (arg) Hz */
#define ACCELIOCSLOWPASS _ACCELIOC(3)
#define ACCELIOCSLOWPASS _ACCELIOC(2)
/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */
#define ACCELIOCSREPORTFORMAT _ACCELIOC(4)
/** return the accel internal lowpass filter in Hz */
#define ACCELIOCGLOWPASS _ACCELIOC(3)
/** set the accel scaling constants to the structure pointed to by (arg) */
#define ACCELIOCSSCALE _ACCELIOC(5)
/** get the accel scaling constants into the structure pointed to by (arg) */
#define ACCELIOCGSCALE _ACCELIOC(6)
/** set the accel measurement range to handle at least (arg) g */
#define ACCELIORANGE _ACCELIOC(6)
#define ACCELIOCSRANGE _ACCELIOC(7)
/** get the current accel measurement range in g */
#define ACCELIOCGRANGE _ACCELIOC(8)
#endif /* _DRV_ACCEL_H */
apps/drivers/drv_baro.h
+6 -11
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define BARO_DEVICE_PATH "/dev/baro"
@@ -65,19 +66,13 @@ ORB_DECLARE(sensor_baro);
* ioctl() definitions
*/
#define _BAROIOCBASE (0x2100)
#define _BAROIOCBASE (0x2200)
#define _BAROIOC(_n) (_IOC(_BAROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the BARO_POLLRATE constants */
#define BAROIOCSPOLLRATE _BAROIOC(0)
/** set corrected MSL pressure in pascals */
#define BAROIOCSMSLPRESSURE _BAROIOC(0)
#define BARO_POLLRATE_MANUAL 1000000 /**< poll when read */
#define BARO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */
/** set the internal queue depth to (arg) entries, must be at least 1 */
#define BAROIOCSQUEUEDEPTH _BAROIOC(1)
/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */
#define BAROIOCSREPORTFORMAT _BAROIOC(2)
/** get current MSL pressure in pascals */
#define BAROIOCGMSLPRESSURE _BAROIOC(1)
#endif /* _DRV_BARO_H */
apps/drivers/drv_gyro.h
+30 -18
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define GYRO_DEVICE_PATH "/dev/gyro"
@@ -50,10 +51,18 @@
* structure.
*/
struct gyro_report {
float x;
float y;
float z;
uint64_t timestamp;
float x; /**< angular velocity in the NED X board axis in rad/s */
float y; /**< angular velocity in the NED Y board axis in rad/s */
float z; /**< angular velocity in the NED Z board axis in rad/s */
float temperature; /**< temperature in degrees celcius */
float range_rad_s;
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
int16_t temperature_raw;
};
/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
@@ -75,28 +84,31 @@ ORB_DECLARE(sensor_gyro);
* ioctl() definitions
*/
#define _GYROIOCBASE (0x2200)
#define _GYROIOCBASE (0x2300)
#define _GYROIOC(_n) (_IOC(_GYROIOCBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the GYRO_POLLRATE constants */
#define GYROIOCSPOLLRATE _GYROIOC(0)
#define GYRO_POLLRATE_MANUAL 1000000 /**< poll when read */
#define GYRO_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */
/** set the internal queue depth to (arg) entries, must be at least 1 */
#define GYROIOCSQUEUEDEPTH _GYROIOC(1)
/** set the gyro internal sample rate to at least (arg) Hz */
#define GYROIOCSSAMPLERATE _GYROIOC(2)
#define GYROIOCSSAMPLERATE _GYROIOC(0)
/** return the gyro internal sample rate in Hz */
#define GYROIOCGSAMPLERATE _GYROIOC(1)
/** set the gyro internal lowpass filter to no lower than (arg) Hz */
#define GYROIOCSLOWPASS _GYROIOC(3)
#define GYROIOCSLOWPASS _GYROIOC(2)
/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */
#define GYROIOCSREPORTFORMAT _GYROIOC(4)
/** set the gyro internal lowpass filter to no lower than (arg) Hz */
#define GYROIOCGLOWPASS _GYROIOC(3)
/** set the gyro scaling constants to (arg) */
#define GYROIOCSSCALE _GYROIOC(5)
#define GYROIOCSSCALE _GYROIOC(4)
/** get the gyro scaling constants into (arg) */
#define GYROIOCGSCALE _GYROIOC(5)
/** set the gyro measurement range to handle at least (arg) degrees per second */
#define GYROIOCSRANGE _GYROIOC(6)
/** get the current gyro measurement range in degrees per second */
#define GYROIOCGRANGE _GYROIOC(7)
#endif /* _DRV_GYRO_H */
apps/drivers/drv_mag.h
+21 -18
View File
@@ -41,6 +41,7 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define MAG_DEVICE_PATH "/dev/mag"
@@ -48,12 +49,20 @@
/**
* mag report structure. Reads from the device must be in multiples of this
* structure.
*
* Output values are in gauss.
*/
struct mag_report {
uint64_t timestamp;
float x;
float y;
float z;
uint64_t timestamp;
float range_ga;
float scaling;
int16_t x_raw;
int16_t y_raw;
int16_t z_raw;
};
/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
@@ -75,28 +84,22 @@ ORB_DECLARE(sensor_mag);
* ioctl() definitions
*/
#define _MAGIOCBASE (0x2300)
#define _MAGIOC(_n) (_IOC(_MAGIOBASE, _n))
/** set the driver polling rate to (arg) Hz, or one of the MAG_POLLRATE constants */
#define MAGIOCSPOLLRATE _MAGIOC(0)
#define MAG_POLLRATE_MANUAL 1000000 /**< poll when read */
#define MAG_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */
/** set the internal queue depth to (arg) entries, must be at least 1 */
#define MAGIOCSQUEUEDEPTH _MAGIOC(1)
#define _MAGIOCBASE (0x2400)
#define _MAGIOC(_n) (_IOC(_MAGIOCBASE, _n))
/** set the mag internal sample rate to at least (arg) Hz */
#define MAGIOCSSAMPLERATE _MAGIOC(2)
#define MAGIOCSSAMPLERATE _MAGIOC(0)
/** set the mag internal lowpass filter to no lower than (arg) Hz */
#define MAGIOCSLOWPASS _MAGIOC(3)
/** set the report format to (arg); zero is the standard, 1-10 are reserved, all others are driver-specific. */
#define MAGIOCSREPORTFORMAT _MAGIOC(4)
#define MAGIOCSLOWPASS _MAGIOC(1)
/** set the mag scaling constants to the structure pointed to by (arg) */
#define MAGIOCSSCALE _MAGIOC(5)
#define MAGIOCSSCALE _MAGIOC(2)
/** copy the mag scaling constants to the structure pointed to by (arg) */
#define MAGIOCGSCALE _MAGIOC(3)
/** perform self-calibration, update scale factors to canonical units */
#define MAGIOCCALIBRATE _MAGIOC(4)
#endif /* _DRV_MAG_H */
apps/drivers/drv_mixer.h
+1 -1
View File
@@ -61,7 +61,7 @@
/*
* ioctl() definitions
*/
#define _MIXERIOCBASE (0x2400)
#define _MIXERIOCBASE (0x2500)
#define _MIXERIOC(_n) (_IOC(_MIXERIOCBASE, _n))
/** get the number of mixable outputs */
apps/drivers/drv_orb_dev.h
+4 -1
View File
@@ -58,7 +58,7 @@
/** maximum ogbject name length */
#define ORB_MAXNAME 32
#define _ORBIOCBASE (0x2500)
#define _ORBIOCBASE (0x2600)
#define _ORBIOC(_n) (_IOC(_ORBIOCBASE, _n))
/*
@@ -81,4 +81,7 @@
/** Set the minimum interval at which the topic can be seen to be updated for this subscription */
#define ORBIOCSETINTERVAL _ORBIOC(12)
/** Get the global advertiser handle for the topic */
#define ORBIOCGADVERTISER _ORBIOC(13)
#endif /* _DRV_UORB_H */
apps/drivers/drv_pwm_output.h
+1 -1
View File
@@ -94,7 +94,7 @@ ORB_DECLARE(output_pwm);
* Note that ioctls and ObjDev updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x2600
#define _PWM_SERVO_BASE 0x2700
/** arm all servo outputs handle by this driver */
#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0)
apps/drivers/drv_sensor.h
+87
View File
@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* 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 Common sensor API and ioctl definitions.
*/
#ifndef _DRV_SENSOR_H
#define _DRV_SENSOR_H
#include <stdint.h>
#include <sys/ioctl.h>
/*
* ioctl() definitions
*
* Note that a driver may not implement all of these operations, but
* if the operation is implemented it should conform to this API.
*/
#define _SENSORIOCBASE (0x2000)
#define _SENSORIOC(_n) (_IOC(_SENSORIOCBASE, _n))
/**
* Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE
* constants
*/
#define SENSORIOCSPOLLRATE _SENSORIOC(0)
/**
* Return the driver's approximate polling rate in Hz, or one of the
* SENSOR_POLLRATE values.
*/
#define SENSORIOCGPOLLRATE _SENSORIOC(1)
#define SENSOR_POLLRATE_MANUAL 1000000 /**< poll when read */
#define SENSOR_POLLRATE_EXTERNAL 1000001 /**< poll when device signals ready */
#define SENSOR_POLLRATE_MAX 1000002 /**< poll at device maximum rate */
#define SENSOR_POLLRATE_DEFAULT 1000003 /**< poll at driver normal rate */
/**
* Set the internal queue depth to (arg) entries, must be at least 1
*
* This sets the upper bound on the number of readings that can be
* read from the driver.
*/
#define SENSORIOCSQUEUEDEPTH _SENSORIOC(2)
/** return the internal queue depth */
#define SENSORIOCGQUEUEDEPTH _SENSORIOC(3)
/**
* Reset the sensor to its default configuration.
*/
#define SENSORIOCRESET _SENSORIOC(4)
#endif /* _DRV_SENSOR_H */
apps/mix_and_link/Makefile → apps/drivers/hmc5883/Makefile
+5 -1
View File
@@ -32,7 +32,11 @@
############################################################################
#
# Makefile to build the mix-and-link functions
# HMC5883 driver
#
APPNAME = hmc5883
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/hmc5883/hmc5883.cpp
+937
View File
@@ -0,0 +1,937 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* 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 hmc5883.cpp
*
* Driver for the HMC5883 magnetometer connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <arch/board/up_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_mag.h>
/*
* HMC5883 internal constants and data structures.
*/
/* Max measurement rate is 160Hz */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 160) /* microseconds */
#define ADDR_CONF_A 0x00
#define ADDR_CONF_B 0x01
#define ADDR_MODE 0x02
#define ADDR_DATA_OUT_X_MSB 0x03
#define ADDR_DATA_OUT_X_LSB 0x04
#define ADDR_DATA_OUT_Z_MSB 0x05
#define ADDR_DATA_OUT_Z_LSB 0x06
#define ADDR_DATA_OUT_Y_MSB 0x07
#define ADDR_DATA_OUT_Y_LSB 0x08
#define ADDR_STATUS 0x09
#define ADDR_ID_A 0x0a
#define ADDR_ID_B 0x0b
#define ADDR_ID_C 0x0c
#define HMC5883L_ADDRESS 0x1E
/* modes not changeable outside of driver */
#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */
#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */
#define HMC5883L_AVERAGING_2 (1 << 5)
#define HMC5883L_AVERAGING_4 (2 << 5)
#define HMC5883L_AVERAGING_8 (3 << 5)
#define MODE_REG_CONTINOUS_MODE (0 << 0)
#define MODE_REG_SINGLE_MODE (1 << 0) /* default */
#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */
#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */
#define ID_A_WHO_AM_I 'H'
#define ID_B_WHO_AM_I '4'
#define ID_C_WHO_AM_I '3'
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
class HMC5883 : public device::I2C
{
public:
HMC5883(int bus);
~HMC5883();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
work_s _work;
unsigned _measure_ticks;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
mag_report *_reports;
mag_scale _scale;
float _range_scale;
float _range_ga;
bool _collect_phase;
orb_advert_t _mag_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*
* This is the heart of the measurement state machine. This function
* alternately starts a measurement, or collects the data from the
* previous measurement.
*
* When the interval between measurements is greater than the minimum
* measurement interval, a gap is inserted between collection
* and measurement to provide the most recent measurement possible
* at the next interval.
*/
void cycle();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
/**
* Write a register.
*
* @param reg The register to write.
* @param val The value to write.
* @return OK on write success.
*/
int write_reg(uint8_t reg, uint8_t val);
/**
* Read a register.
*
* @param reg The register to read.
* @param val The value read.
* @return OK on read success.
*/
int read_reg(uint8_t reg, uint8_t &val);
/**
* Issue a measurement command.
*
* @return OK if the measurement command was successful.
*/
int measure();
/**
* Collect the result of the most recent measurement.
*/
int collect();
/**
* Convert a big-endian signed 16-bit value to a float.
*
* @param in A signed 16-bit big-endian value.
* @return The floating-point representation of the value.
*/
float meas_to_float(uint8_t in[2]);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_mag_topic(-1),
_range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */
_range_ga(0.88f),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
// default scaling
_scale.x_offset = 0;
_scale.x_scale = 1.0f;
_scale.y_offset = 0;
_scale.y_scale = 1.0f;
_scale.z_offset = 0;
_scale.z_scale = 1.0f;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
HMC5883::~HMC5883()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
}
int
HMC5883::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_reports = new struct mag_report[_num_reports];
if (_reports == nullptr)
goto out;
_oldest_report = _next_report = 0;
/* get a publish handle on the mag topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]);
if (_mag_topic < 0)
debug("failed to create sensor_mag object");
ret = OK;
out:
return ret;
}
int
HMC5883::probe()
{
uint8_t data[3] = {0, 0, 0};
_retries = 10;
if (read_reg(ADDR_ID_A, data[0]) ||
read_reg(ADDR_ID_B, data[1]) ||
read_reg(ADDR_ID_C, data[2]))
debug("read_reg fail");
_retries = 1;
if ((data[0] != ID_A_WHO_AM_I) ||
(data[1] != ID_B_WHO_AM_I) ||
(data[2] != ID_C_WHO_AM_I)) {
debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]);
return -EIO;
}
return OK;
}
ssize_t
HMC5883::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct mag_report);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
return -ENOSPC;
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
do {
_oldest_report = _next_report = 0;
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(HMC5883_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
} while (0);
return ret;
}
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct mag_report *buf = new struct mag_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports - 1;
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case MAGIOCSSAMPLERATE:
/* not supported, always 1 sample per poll */
return -EINVAL;
case MAGIOCSLOWPASS:
/* not supported, no internal filtering */
return -EINVAL;
case MAGIOCSSCALE:
/* set new scale factors */
memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
return 0;
case MAGIOCGSCALE:
/* copy out scale factors */
memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
return 0;
case MAGIOCCALIBRATE:
/* XXX perform auto-calibration */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
void
HMC5883::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_oldest_report = _next_report = 0;
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1);
}
void
HMC5883::stop()
{
work_cancel(HPWORK, &_work);
}
void
HMC5883::cycle_trampoline(void *arg)
{
HMC5883 *dev = (HMC5883 *)arg;
dev->cycle();
}
void
HMC5883::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&HMC5883::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(HMC5883_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure())
log("measure error");
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&HMC5883::cycle_trampoline,
this,
USEC2TICK(HMC5883_CONVERSION_INTERVAL));
}
int
HMC5883::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
if (OK != ret)
perf_count(_comms_errors);
return ret;
}
int
HMC5883::collect()
{
#pragma pack(push, 1)
struct { /* status register and data as read back from the device */
uint8_t x[2];
uint8_t z[2];
uint8_t y[2];
} hmc_report;
#pragma pack(pop)
struct {
int16_t x, y, z;
} report;
int ret = -EIO;
uint8_t cmd;
perf_begin(_sample_perf);
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
/*
* @note We could read the status register here, which could tell us that
* we were too early and that the output registers are still being
* written. In the common case that would just slow us down, and
* we're better off just never being early.
*/
/* get measurements from the device */
cmd = ADDR_DATA_OUT_X_MSB;
ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));
if (ret != OK) {
perf_count(_comms_errors);
debug("data/status read error");
goto out;
}
/* swap the data we just received */
report.x = (((int16_t)hmc_report.x[0]) << 8) + hmc_report.x[1];
report.y = (((int16_t)hmc_report.y[0]) << 8) + hmc_report.y[1];
report.z = (((int16_t)hmc_report.z[0]) << 8) + hmc_report.z[1];
/*
* If any of the values are -4096, there was an internal math error in the sensor.
* Generalise this to a simple range check that will also catch some bit errors.
*/
if ((abs(report.x) > 2048) ||
(abs(report.y) > 2048) ||
(abs(report.z) > 2048))
goto out;
/*
* RAW outputs
*
* to align the sensor axes with the board, x and y need to be flipped
* and y needs to be negated
*/
_reports[_next_report].x_raw = report.y;
_reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x);
/* z remains z */
_reports[_next_report].z_raw = report.z;
/* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* to align the sensor axes with the board, x and y need to be flipped */
_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, toss it */
if (_next_report == _oldest_report) {
perf_count(_buffer_overflows);
INCREMENT(_oldest_report, _num_reports);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
out:
perf_end(_sample_perf);
return ret;
}
int
HMC5883::write_reg(uint8_t reg, uint8_t val)
{
uint8_t cmd[] = { reg, val };
return transfer(&cmd[0], 2, nullptr, 0);
}
int
HMC5883::read_reg(uint8_t reg, uint8_t &val)
{
return transfer(&reg, 1, &val, 1);
}
float
HMC5883::meas_to_float(uint8_t in[2])
{
union {
uint8_t b[2];
int16_t w;
} u;
u.b[0] = in[1];
u.b[1] = in[0];
return (float) u.w;
}
void
HMC5883::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
}
/**
* Local functions in support of the shell command.
*/
namespace hmc5883
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
HMC5883 *g_dev;
void start();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
/* XXX HORRIBLE hack - the bus number should not come from here */
g_dev = new HMC5883(2);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct mag_report report;
ssize_t sz;
int ret;
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "immediate read failed");
warnx("single read");
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 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
errx(1, "failed to set queue depth");
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
errx(1, "failed to set 2Hz poll rate");
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
errx(1, "timed out waiting for sensor data");
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
hmc5883_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
hmc5883::start();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
hmc5883::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
hmc5883::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
hmc5883::info();
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}
apps/drivers/l3gd20/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# 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.
#
############################################################################
#
# Makefile to build the L3GD20 driver.
#
APPNAME = l3gd20
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/l3gd20/l3gd20.cpp
+868
View File
@@ -0,0 +1,868 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* 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 Driver for the ST L3GD20 MEMS gyro connected via SPI.
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <arch/board/up_hrt.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_gyro.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
/* register addresses */
#define ADDR_WHO_AM_I 0x0F
#define WHO_I_AM 0xD4
#define ADDR_CTRL_REG1 0x20
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
/* keep lowpass low to avoid noise issues */
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
#define RATE_380HZ_LP_30HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
#define ADDR_CTRL_REG2 0x21
#define ADDR_CTRL_REG3 0x22
#define ADDR_CTRL_REG4 0x23
#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */
#define RANGE_250DPS (0<<4)
#define RANGE_500DPS (1<<4)
#define RANGE_2000DPS (3<<4)
#define ADDR_CTRL_REG5 0x24
#define ADDR_REFERENCE 0x25
#define ADDR_OUT_TEMP 0x26
#define ADDR_STATUS_REG 0x27
#define ADDR_OUT_X_L 0x28
#define ADDR_OUT_X_H 0x29
#define ADDR_OUT_Y_L 0x2A
#define ADDR_OUT_Y_H 0x2B
#define ADDR_OUT_Z_L 0x2C
#define ADDR_OUT_Z_H 0x2D
#define ADDR_FIFO_CTRL_REG 0x2E
#define ADDR_FIFO_SRC_REG 0x2F
#define ADDR_INT1_CFG 0x30
#define ADDR_INT1_SRC 0x31
#define ADDR_INT1_TSH_XH 0x32
#define ADDR_INT1_TSH_XL 0x33
#define ADDR_INT1_TSH_YH 0x34
#define ADDR_INT1_TSH_YL 0x35
#define ADDR_INT1_TSH_ZH 0x36
#define ADDR_INT1_TSH_ZL 0x37
#define ADDR_INT1_DURATION 0x38
/* Internal configuration values */
#define REG1_POWER_NORMAL (1<<3)
#define REG1_Z_ENABLE (1<<2)
#define REG1_Y_ENABLE (1<<1)
#define REG1_X_ENABLE (1<<0)
#define REG4_BDU (1<<7)
#define REG4_BLE (1<<6)
//#define REG4_SPI_3WIRE (1<<0)
#define REG5_FIFO_ENABLE (1<<6)
#define REG5_REBOOT_MEMORY (1<<7)
#define STATUS_ZYXOR (1<<7)
#define STATUS_ZOR (1<<6)
#define STATUS_YOR (1<<5)
#define STATUS_XOR (1<<4)
#define STATUS_ZYXDA (1<<3)
#define STATUS_ZDA (1<<2)
#define STATUS_YDA (1<<1)
#define STATUS_XDA (1<<0)
#define FIFO_CTRL_BYPASS_MODE (0<<5)
#define FIFO_CTRL_FIFO_MODE (1<<5)
#define FIFO_CTRL_STREAM_MODE (1<<6)
#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
{
public:
L3GD20(int bus, spi_dev_e device);
~L3GD20();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
struct hrt_call _call;
unsigned _call_interval;
unsigned _num_reports;
volatile unsigned _next_report;
volatile unsigned _oldest_report;
struct gyro_report *_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _current_rate;
unsigned _current_range;
perf_counter_t _sample_perf;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report ring.
*/
void measure();
/**
* Read a register from the L3GD20
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
/**
* Write a register in the L3GD20
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the L3GD20
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the L3GD20 measurement range.
*
* @param max_dps The measurement range is set to permit reading at least
* this rate in degrees per second.
* Zero selects the maximum supported range.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_range(unsigned max_dps);
/**
* Set the L3GD20 internal sampling frequency.
*
* @param frequency The internal sampling frequency is set to not less than
* this value.
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
int set_samplerate(unsigned frequency);
};
/* helper macro for handling report buffer indices */
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
L3GD20::L3GD20(int bus, spi_dev_e device) :
SPI("L3GD20", GYRO_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
_call_interval(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_current_rate(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read"))
{
// enable debug() calls
_debug_enabled = true;
// default scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
}
L3GD20::~L3GD20()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr)
delete[] _reports;
/* delete the perf counter */
perf_free(_sample_perf);
}
int
L3GD20::init()
{
int ret = ERROR;
/* do SPI init (and probe) first */
if (SPI::init() != OK)
goto out;
/* allocate basic report buffers */
_num_reports = 2;
_oldest_report = _next_report = 0;
_reports = new struct gyro_report[_num_reports];
if (_reports == nullptr)
goto out;
/* advertise sensor topic */
memset(&_reports[0], 0, sizeof(_reports[0]));
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]);
/* set default configuration */
write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */
write_reg(ADDR_CTRL_REG4, REG4_BDU);
write_reg(ADDR_CTRL_REG5, 0);
write_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_STREAM_MODE); /* Enable FIFO, old data is overwritten */
set_range(500); /* default to 500dps */
set_samplerate(0); /* max sample rate */
ret = OK;
out:
return ret;
}
int
L3GD20::probe()
{
/* read dummy value to void to clear SPI statemachine on sensor */
(void)read_reg(ADDR_WHO_AM_I);
/* verify that the device is attached and functioning */
if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM)
return OK;
return -EIO;
}
ssize_t
L3GD20::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct gyro_report);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
return -ENOSPC;
/* if automatic measurement is enabled */
if (_call_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_oldest_report != _next_report) {
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
ret += sizeof(_reports[0]);
INCREMENT(_oldest_report, _num_reports);
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_oldest_report = _next_report = 0;
measure();
/* measurement will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
return ret;
}
int
L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: {
/* account for sentinel in the ring */
arg++;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 2) || (arg > 100))
return -EINVAL;
/* allocate new buffer */
struct gyro_report *buf = new struct gyro_report[arg];
if (nullptr == buf)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _num_reports -1;
case SENSORIOCRESET:
/* XXX implement */
return -EINVAL;
case GYROIOCSSAMPLERATE:
return set_samplerate(arg);
case GYROIOCGSAMPLERATE:
return _current_rate;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS:
/* XXX not implemented due to wacky interaction with samplerate */
return -EINVAL;
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
return set_range(arg);
case GYROIOCGRANGE:
return _current_range;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
uint8_t
L3GD20::read_reg(unsigned reg)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void
L3GD20::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
transfer(cmd, nullptr, sizeof(cmd));
}
void
L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
int
L3GD20::set_range(unsigned max_dps)
{
uint8_t bits = REG4_BDU;
if (max_dps == 0)
max_dps = 2000;
if (max_dps <= 250) {
_current_range = 250;
bits |= RANGE_250DPS;
} else if (max_dps <= 500) {
_current_range = 500;
bits |= RANGE_500DPS;
} else if (max_dps <= 2000) {
_current_range = 2000;
bits |= RANGE_2000DPS;
} else {
return -EINVAL;
}
_gyro_range_rad_s = _current_range / 180.0f * M_PI_F;
_gyro_range_scale = _gyro_range_rad_s / 32768.0f;
write_reg(ADDR_CTRL_REG4, bits);
return OK;
}
int
L3GD20::set_samplerate(unsigned frequency)
{
uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
if (frequency == 0)
frequency = 760;
if (frequency <= 95) {
_current_rate = 95;
bits |= RATE_95HZ_LP_25HZ;
} else if (frequency <= 190) {
_current_rate = 190;
bits |= RATE_190HZ_LP_25HZ;
} else if (frequency <= 380) {
_current_rate = 380;
bits |= RATE_380HZ_LP_30HZ;
} else if (frequency <= 760) {
_current_rate = 760;
bits |= RATE_760HZ_LP_30HZ;
} else {
return -EINVAL;
}
write_reg(ADDR_CTRL_REG1, bits);
return OK;
}
void
L3GD20::start()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_oldest_report = _next_report = 0;
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this);
}
void
L3GD20::stop()
{
hrt_cancel(&_call);
}
void
L3GD20::measure_trampoline(void *arg)
{
L3GD20 *dev = (L3GD20 *)arg;
/* make another measurement */
dev->measure();
}
void
L3GD20::measure()
{
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
uint8_t status;
int16_t x;
int16_t y;
int16_t z;
} raw_report;
#pragma pack(pop)
gyro_report *report = &_reports[_next_report];
/* start the performance counter */
perf_begin(_sample_perf);
/* fetch data from the sensor */
raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
report->timestamp = hrt_absolute_time();
/* XXX adjust for sensor alignment to board here */
report->x_raw = raw_report.x;
report->y_raw = raw_report.y;
report->z_raw = raw_report.z;
report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report->scaling = _gyro_range_scale;
report->range_rad_s = _gyro_range_rad_s;
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
/* if we are running up against the oldest report, fix it */
if (_next_report == _oldest_report)
INCREMENT(_oldest_report, _num_reports);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report);
/* stop the perf counter */
perf_end(_sample_perf);
}
void
L3GD20::print_info()
{
perf_print_counter(_sample_perf);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
}
/**
* Local functions in support of the shell command.
*/
namespace l3gd20
{
L3GD20 *g_dev;
void start();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new L3GD20(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_GYRO);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd_gyro = -1;
struct gyro_report g_report;
ssize_t sz;
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
/* reset to manual polling */
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling");
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report))
err(1, "immediate gyro read failed");
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
/* XXX add poll-rate tests here too */
reset();
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr)
errx(1, "driver not running\n");
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
l3gd20_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
l3gd20::start();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
l3gd20::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
l3gd20::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
l3gd20::info();
errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
}
apps/drivers/mpu6000/Makefile
+1 -1
View File
@@ -37,6 +37,6 @@
APPNAME = mpu6000
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/drivers/mpu6000/mpu6000.cpp
+342 -136
View File
@@ -43,6 +43,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
@@ -53,6 +54,8 @@
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
@@ -155,9 +158,6 @@ public:
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/**
* Diagnostics - print some basic information about the driver.
*/
@@ -181,12 +181,14 @@ private:
struct accel_report _accel_report;
struct accel_scale _accel_scale;
float _accel_range_scale;
int _accel_topic;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
struct gyro_report _gyro_report;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
int _gyro_topic;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
unsigned _reads;
perf_counter_t _sample_perf;
@@ -288,15 +290,17 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
_accel_range_scale(1.0f),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_gyro_range_scale(1.0f),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_reads(0),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
{
// enable debug() calls
_debug_enabled = true;
// disable debug() calls
_debug_enabled = false;
// default accel scale factors
_accel_scale.x_offset = 0;
@@ -316,6 +320,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
memset(&_accel_report, 0, sizeof(_accel_report));
memset(&_gyro_report, 0, sizeof(_gyro_report));
memset(&_call, 0, sizeof(_call));
}
MPU6000::~MPU6000()
@@ -364,12 +369,29 @@ MPU6000::init()
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
usleep(1000);
// FS & DLPF FS=2000¼/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);
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000¼/s
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
usleep(1000);
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
// product-specific scaling
switch (_product) {
case MPU6000ES_REV_C4:
@@ -394,6 +416,17 @@ MPU6000::init()
break;
}
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_accel_range_scale = (9.81f / 4096.0f);
_accel_range_m_s2 = 8.0f * 9.81f;
usleep(1000);
// INT CFG => Interrupt on Data Ready
@@ -406,30 +439,12 @@ MPU6000::init()
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
/* do CDev init for the gyro device node */
ret = _gyro->init();
return ret;
}
int
MPU6000::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_call_interval = 0;
/* XXX set default sampling/acquisition parameters */
return OK;
}
int
MPU6000::close_last(struct file *filp)
{
/* stop measurement */
stop();
return OK;
}
int
MPU6000::probe()
{
@@ -451,7 +466,7 @@ MPU6000::probe()
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
log("ID 0x%02x", _product);
debug("ID 0x%02x", _product);
return OK;
}
@@ -472,7 +487,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
if (_call_interval == 0)
measure();
/* copy out the latest report */
/* copy out the latest reports */
memcpy(buffer, &_accel_report, sizeof(_accel_report));
ret = sizeof(_accel_report);
@@ -504,22 +519,28 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ACCELIOCSPOLLRATE: {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case ACC_POLLRATE_MANUAL:
case SENSOR_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case ACC_POLLRATE_EXTERNAL:
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
@@ -534,7 +555,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval;
_call.period = _call_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
@@ -545,20 +566,46 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
case ACCELIOCSQUEUEDEPTH:
case SENSORIOCGPOLLRATE:
if (_call_interval == 0)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
case SENSORIOCGQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
case ACCELIOCSSAMPLERATE:
case ACCELIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
case ACCELIOCSLOWPASS:
case ACCELIOCGLOWPASS:
/* XXX not implemented */
return -EINVAL;
case ACCELIORANGE:
/* XXX not implemented really */
return set_range(arg);
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_scale*) arg, sizeof(_accel_scale));
return OK;
case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
case ACCELIOCSREPORTFORMAT: /* no alternate report formats */
case ACCELIOCGSCALE:
/* copy scale out */
memcpy((struct accel_scale*) arg, &_accel_scale, sizeof(_accel_scale));
return OK;
case ACCELIOCSRANGE:
case ACCELIOCGRANGE:
/* XXX not implemented */
// XXX change these two values on set:
// _accel_range_scale = (9.81f / 4096.0f);
// _accel_range_rad_s = 8.0f * 9.81f;
return -EINVAL;
default:
@@ -572,24 +619,40 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case GYROIOCSPOLLRATE:
/* gyro and accel poll rates are shared */
return ioctl(filp, ACCELIOCSPOLLRATE, arg);
/* these are shared with the accel side */
case SENSORIOCSPOLLRATE:
case SENSORIOCGPOLLRATE:
case SENSORIOCSQUEUEDEPTH:
case SENSORIOCGQUEUEDEPTH:
case SENSORIOCRESET:
return ioctl(filp, cmd, arg);
case GYROIOCSQUEUEDEPTH:
case GYROIOCSSAMPLERATE:
case GYROIOCGSAMPLERATE:
/* XXX not implemented */
return -EINVAL;
case GYROIOCSLOWPASS:
case GYROIOCGLOWPASS:
/* XXX not implemented */
return -EINVAL;
case GYROIOCSSCALE:
/* XXX not implemented really */
return set_range(arg);
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_scale*) arg, sizeof(_gyro_scale));
return OK;
case GYROIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
case GYROIOCSREPORTFORMAT: /* no alternate report formats */
case GYROIOCGSCALE:
/* copy scale out */
memcpy((struct gyro_scale*) arg, &_gyro_scale, sizeof(_gyro_scale));
return OK;
case GYROIOCSRANGE:
case GYROIOCGRANGE:
/* XXX not implemented */
// XXX change these two values on set:
// _gyro_range_scale = xx
// _gyro_range_m_s2 = xx
return -EINVAL;
default:
@@ -719,40 +782,118 @@ MPU6000::measure()
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct Report {
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint16_t accel_x;
uint16_t accel_y;
uint16_t accel_z;
uint16_t temp;
uint16_t gyro_x;
uint16_t gyro_y;
uint16_t gyro_z;
} report;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
} mpu_report;
#pragma pack(pop)
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&report, (uint8_t *)&report, sizeof(report));
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report));
/*
* Adjust and scale results to mg.
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
/*
* Adjust and scale results to m/s^2.
*/
_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
_accel_report.x = report.accel_x * _accel_range_scale;
_accel_report.y = report.accel_y * _accel_range_scale;
_accel_report.z = report.accel_z * _accel_range_scale;
_gyro_report.x = report.gyro_x * _gyro_range_scale;
_gyro_report.y = report.gyro_y * _gyro_range_scale;
_gyro_report.z = report.gyro_z * _gyro_range_scale;
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
_accel_report.x_raw = report.accel_x;
_accel_report.y_raw = report.accel_y;
_accel_report.z_raw = report.accel_z;
_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
_accel_report.scaling = _accel_range_scale;
_accel_report.range_m_s2 = _accel_range_m_s2;
_accel_report.temperature_raw = report.temp;
_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
_gyro_report.x_raw = report.gyro_x;
_gyro_report.y_raw = report.gyro_y;
_gyro_report.z_raw = report.gyro_z;
_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
_gyro_report.scaling = _gyro_range_scale;
_gyro_report.range_rad_s = _gyro_range_rad_s;
_gyro_report.temperature_raw = report.temp;
_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
/* notify anyone waiting for data */
poll_notify(POLLIN);
@@ -803,67 +944,151 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
/**
* Local functions in support of the shell command.
*/
namespace
namespace mpu6000
{
MPU6000 *g_dev;
/*
* XXX this should just be part of the generic sensors test...
*/
void start();
void test();
void reset();
void info();
int
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
if (g_dev == nullptr)
goto fail;
if (OK != g_dev->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int fd = -1;
struct accel_report report;
int fd_gyro = -1;
struct accel_report a_report;
struct gyro_report g_report;
ssize_t sz;
const char *reason = "test OK";
do {
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
ACCEL_DEVICE_PATH);
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
/* get the driver */
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", GYRO_DEVICE_PATH);
if (fd < 0) {
reason = "can't open driver";
break;
}
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling");
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report))
err(1, "immediate acc read failed");
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / 9.81f));
if (sz != sizeof(report)) {
reason = "immediate read failed";
break;
}
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report))
err(1, "immediate gyro read failed");
printf("single read\n");
fflush(stdout);
printf("time: %lld\n", report.timestamp);
printf("x: %f\n", report.x);
printf("y: %f\n", report.y);
printf("z: %f\n", report.z);
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f+0.5f));
} while (0);
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
printf("MPU6000: %s\n", reason);
return OK;
/* XXX add poll-rate tests here too */
reset();
errx(0, "PASS");
}
int
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
fprintf(stderr, "MPU6000: driver not running\n");
return -ENOENT;
}
if (g_dev == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
return OK;
exit(0);
}
@@ -874,47 +1099,28 @@ mpu6000_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "MPU6000: already loaded\n");
return -EBUSY;
}
/* create the driver */
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
if (g_dev == nullptr) {
fprintf(stderr, "MPU6000: driver alloc failed\n");
return -ENOMEM;
}
if (OK != g_dev->init()) {
fprintf(stderr, "MPU6000: driver init failed\n");
usleep(100000);
delete g_dev;
g_dev = nullptr;
return -EIO;
}
return OK;
}
if (!strcmp(argv[1], "start"))
mpu6000::start();
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
return test();
mpu6000::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
mpu6000::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
return info();
mpu6000::info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
return -EINVAL;
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
apps/drivers/ms5611/ms5611.cpp
+440 -239
View File
File diff suppressed because it is too large Load Diff
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
+53 -8
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
^^^^^^^^^^^^^^
@@ -1345,10 +1367,14 @@ examples/uip
file in the configuration driver with instruction to build applications
like:
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += dhcpc
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webserver
CONFIGURED_APPS += uiplib
CONFIGURED_APPS += dhcpc
CONFIGURED_APPS += resolv
CONFIGURED_APPS += webserver
NOTE: This example does depend on the perl script at
nuttx/tools/mkfsdata.pl. You must have perl installed on your
development system at /usr/bin/perl.
examples/usbserial
^^^^^^^^^^^^^^^^^^
@@ -1642,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/adc/adc_main.c
+11 -19
View File
@@ -57,14 +57,6 @@
* Pre-processor Definitions
****************************************************************************/
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME adc_main
# define MAIN_STRING "adc_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -223,10 +215,10 @@ static void parse_args(FAR struct adc_state_s *adc, int argc, FAR char **argv)
****************************************************************************/
/****************************************************************************
* Name: user_start/adc_main
* Name: adc_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int adc_main(int argc, char *argv[])
{
struct adc_msg_s sample[CONFIG_EXAMPLES_ADC_GROUPSIZE];
size_t readsize;
@@ -244,11 +236,11 @@ int MAIN_NAME(int argc, char *argv[])
* this test.
*/
message(MAIN_STRING "Initializing external ADC device\n");
message("adc_main: Initializing external ADC device\n");
ret = adc_devinit();
if (ret != OK)
{
message(MAIN_STRING "adc_devinit failed: %d\n", ret);
message("adc_main: adc_devinit failed: %d\n", ret);
errval = 1;
goto errout;
}
@@ -276,18 +268,18 @@ int MAIN_NAME(int argc, char *argv[])
*/
#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
message(MAIN_STRING "g_adcstate.count: %d\n", g_adcstate.count);
message("adc_main: g_adcstate.count: %d\n", g_adcstate.count);
#endif
/* Open the ADC device for reading */
message(MAIN_STRING "Hardware initialized. Opening the ADC device: %s\n",
message("adc_main: Hardware initialized. Opening the ADC device: %s\n",
g_adcstate.devpath);
fd = open(g_adcstate.devpath, O_RDONLY);
if (fd < 0)
{
message(MAIN_STRING "open %s failed: %d\n", g_adcstate.devpath, errno);
message("adc_main: open %s failed: %d\n", g_adcstate.devpath, errno);
errval = 2;
goto errout_with_dev;
}
@@ -322,17 +314,17 @@ int MAIN_NAME(int argc, char *argv[])
errval = errno;
if (errval != EINTR)
{
message(MAIN_STRING "read %s failed: %d\n",
message("adc_main: read %s failed: %d\n",
g_adcstate.devpath, errval);
errval = 3;
goto errout_with_dev;
}
message(MAIN_STRING "Interrupted read...\n");
message("adc_main: Interrupted read...\n");
}
else if (nbytes == 0)
{
message(MAIN_STRING "No data read, Ignoring\n");
message("adc_main: No data read, Ignoring\n");
}
/* Print the sample data on successful return */
@@ -342,7 +334,7 @@ int MAIN_NAME(int argc, char *argv[])
int nsamples = nbytes / sizeof(struct adc_msg_s);
if (nsamples * sizeof(struct adc_msg_s) != nbytes)
{
message(MAIN_STRING "read size=%d is not a multiple of sample size=%d, Ignoring\n",
message("adc_main: read size=%d is not a multiple of sample size=%d, Ignoring\n",
nbytes, sizeof(struct adc_msg_s));
}
else
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
+3 -13
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>
@@ -130,16 +130,6 @@
#define NUM_BUTTONS (MAX_BUTTON - MIN_BUTTON + 1)
#define BUTTON_INDEX(b) ((b)-MIN_BUTTON)
/* Is this being built as an NSH built-in application? */
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME buttons_main
# define MAIN_STRING "buttons_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -399,10 +389,10 @@ static int button7_handler(int irq, FAR void *context)
****************************************************************************/
/****************************************************************************
* user_start/buttons_main
* buttons_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int buttons_main(int argc, char *argv[])
{
uint8_t newset;
irqstate_t flags;
apps/examples/can/can_main.c
+9 -17
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
@@ -76,14 +76,6 @@
# define MAX_ID (1 << 11)
#endif
#ifdef CONFIG_NSH_BUILTIN_APPS
# define MAIN_NAME can_main
# define MAIN_STRING "can_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
/****************************************************************************
* Private Types
****************************************************************************/
@@ -109,10 +101,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start/can_main
* Name: can_main
****************************************************************************/
int MAIN_NAME(int argc, char *argv[])
int can_main(int argc, char *argv[])
{
#ifndef CONFIG_EXAMPLES_CAN_READONLY
struct can_msg_s txmsg;
@@ -150,31 +142,31 @@ int MAIN_NAME(int argc, char *argv[])
{
nmsgs = strtol(argv[1], NULL, 10);
}
message(MAIN_STRING "nmsgs: %d\n", nmsgs);
message("can_main: nmsgs: %d\n", nmsgs);
#elif defined(CONFIG_EXAMPLES_CAN_NMSGS)
message(MAIN_STRING "nmsgs: %d\n", CONFIG_EXAMPLES_CAN_NMSGS);
message("can_main: nmsgs: %d\n", CONFIG_EXAMPLES_CAN_NMSGS);
#endif
/* Initialization of the CAN hardware is performed by logic external to
* this test.
*/
message(MAIN_STRING "Initializing external CAN device\n");
message("can_main: Initializing external CAN device\n");
ret = can_devinit();
if (ret != OK)
{
message(MAIN_STRING "can_devinit failed: %d\n", ret);
message("can_main: can_devinit failed: %d\n", ret);
errval = 1;
goto errout;
}
/* Open the CAN device for reading */
message(MAIN_STRING "Hardware initialized. Opening the CAN device\n");
message("can_main: Hardware initialized. Opening the CAN device\n");
fd = open(CONFIG_EXAMPLES_CAN_DEVPATH, CAN_OFLAGS);
if (fd < 0)
{
message(MAIN_STRING "open %s failed: %d\n",
message("can_main: open %s failed: %d\n",
CONFIG_EXAMPLES_CAN_DEVPATH, errno);
errval = 2;
goto errout_with_dev;
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
+3 -9
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>
@@ -53,16 +53,10 @@
****************************************************************************/
/****************************************************************************
* user_start/hello_main
* hello_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_HELLO_BUILTIN
# define MAIN_NAME hello_main
#else
# define MAIN_NAME user_start
#endif
int MAIN_NAME(int argc, char *argv[])
int hello_main(int argc, char *argv[])
{
printf("Hello, World!!\n");
return 0;
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
+6 -19
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>
@@ -124,24 +124,11 @@ static CHelloWorld g_HelloWorld;
// Public Functions
//***************************************************************************
//***************************************************************************
// user_start
//***************************************************************************
/****************************************************************************
* Name: user_start/nxhello_main
* Name: helloxx_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_HELLOXX_BUILTIN
extern "C" int helloxx_main(int argc, char *argv[]);
# define MAIN_NAME helloxx_main
# define MAIN_STRING "helloxx_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
int MAIN_NAME(int argc, char *argv[])
int helloxx_main(int argc, char *argv[])
{
// If C++ initialization for static constructors is supported, then do
// that first
@@ -153,7 +140,7 @@ int MAIN_NAME(int argc, char *argv[])
// Exercise an explictly instantiated C++ object
CHelloWorld *pHelloWorld = new CHelloWorld;
printf(MAIN_STRING "Saying hello from the dynamically constructed instance\n");
printf("helloxx_main: Saying hello from the dynamically constructed instance\n");
pHelloWorld->HelloWorld();
// Exercise an C++ object instantiated on the stack
@@ -161,14 +148,14 @@ int MAIN_NAME(int argc, char *argv[])
#ifndef CONFIG_EXAMPLES_HELLOXX_NOSTACKCONST
CHelloWorld HelloWorld;
printf(MAIN_STRING "Saying hello from the instance constructed on the stack\n");
printf("helloxx_main: Saying hello from the instance constructed on the stack\n");
HelloWorld.HelloWorld();
#endif
// Exercise an statically constructed C++ object
#ifdef CONFIG_HAVE_CXXINITIALIZE
printf(MAIN_STRING "Saying hello from the statically constructed instance\n");
printf("helloxx_main: Saying hello from the statically constructed instance\n");
g_HelloWorld.HelloWorld();
#endif
apps/examples/lcdrw/Makefile
-105
View File
@@ -1,105 +0,0 @@
############################################################################
# apps/examples/lcdrw/Makefile
#
# 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.
#
############################################################################
-include $(TOPDIR)/.config
-include $(TOPDIR)/Make.defs
include $(APPDIR)/Make.defs
# LCD Read/Write Test
ASRCS =
CSRCS = lcdrw_main.c
AOBJS = $(ASRCS:.S=$(OBJEXT))
COBJS = $(CSRCS:.c=$(OBJEXT))
SRCS = $(ASRCS) $(CSRCS)
OBJS = $(AOBJS) $(COBJS)
ifeq ($(WINTOOL),y)
BIN = "${shell cygpath -w $(APPDIR)/libapps$(LIBEXT)}"
else
BIN = "$(APPDIR)/libapps$(LIBEXT)"
endif
ROOTDEPPATH = --dep-path .
# LCD R/W built-in application info
APPNAME = lcdrw
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
# Common build
VPATH =
all: .built
.PHONY: clean depend distclean
$(AOBJS): %$(OBJEXT): %.S
$(call ASSEMBLE, $<, $@)
$(COBJS): %$(OBJEXT): %.c
$(call COMPILE, $<, $@)
.built: $(OBJS)
@( for obj in $(OBJS) ; do \
$(call ARCHIVE, $(BIN), $${obj}); \
done ; )
@touch .built
.context:
ifeq ($(CONFIG_EXAMPLES_LCDRW_BUILTIN),y)
$(call REGISTER,$(APPNAME),$(PRIORITY),$(STACKSIZE),$(APPNAME)_main)
@touch $@
endif
context: .context
.depend: Makefile $(SRCS)
@$(MKDEP) $(ROOTDEPPATH) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
@touch $@
depend: .depend
clean:
@rm -f *.o *~ .*.swp .built
$(call CLEAN)
distclean: clean
@rm -f Make.dep .depend
-include Make.dep
apps/examples/lcdrw/lcdrw_main.c
-259
View File
@@ -1,259 +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/user_start
****************************************************************************/
#ifdef CONFIG_EXAMPLES_LCDRW_BUILTIN
# define MAIN_NAME lcdrw_main
#else
# define MAIN_NAME user_start
#endif
int MAIN_NAME(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
+3 -3
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
@@ -267,10 +267,10 @@ static void do_frees(void **mem, const int *size, const int *seq, int n)
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: mm_main
****************************************************************************/
int user_start(int argc, char *argv[])
int mm_main(int argc, char *argv[])
{
mm_showmallinfo();
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
+9 -9
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
@@ -567,10 +567,10 @@ static void succeed_stat(const char *path)
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: mount_main
****************************************************************************/
int user_start(int argc, char *argv[])
int mount_main(int argc, char *argv[])
{
int ret;
@@ -580,18 +580,18 @@ int user_start(int argc, char *argv[])
ret = create_ramdisk();
if (ret < 0)
{
printf("user_start: ERROR failed to create RAM disk\n");
printf("mount_main: ERROR failed to create RAM disk\n");
return 1;
}
#endif
/* Mount the test file system (see arch/sim/src/up_deviceimage.c */
printf("user_start: mounting %s filesystem at target=%s with source=%s\n",
printf("mount_main: mounting %s filesystem at target=%s with source=%s\n",
g_filesystemtype, g_target, g_source);
ret = mount(g_source, g_target, g_filesystemtype, 0, NULL);
printf("user_start: mount() returned %d\n", ret);
printf("mount_main: mount() returned %d\n", ret);
if (ret == 0)
{
@@ -737,16 +737,16 @@ int user_start(int argc, char *argv[])
/* Unmount the file system */
printf("user_start: Try unmount(%s)\n", g_target);
printf("mount_main: Try unmount(%s)\n", g_target);
ret = umount(g_target);
if (ret != 0)
{
printf("user_start: ERROR umount() failed, errno %d\n", errno);
printf("mount_main: ERROR umount() failed, errno %d\n", errno);
g_nerrors++;
}
printf("user_start: %d errors reported\n", g_nerrors);
printf("mount_main: %d errors reported\n", g_nerrors);
}
fflush(stdout);
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/nsh/nsh_main.c
+2 -2
View File
@@ -84,10 +84,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: nsh_main
****************************************************************************/
int user_start(int argc, char *argv[])
int nsh_main(int argc, char *argv[])
{
int exitval = 0;
int ret;
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
+3 -3
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
@@ -58,10 +58,10 @@
****************************************************************************/
/****************************************************************************
* Name: user_start
* Name: null_main
****************************************************************************/
int user_start(int argc, char *argv[])
int null_main(int argc, char *argv[])
{
return 0;
}
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
+13 -21
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>
@@ -409,7 +409,7 @@ static int user_main(int argc, char *argv[])
check_test_memory_usage();
#endif /* CONFIG_PRIORITY_INHERITANCE && !CONFIG_DISABLE_SIGNALS && !CONFIG_DISABLE_PTHREAD */
/* Compare memory usage at time user_start started until
/* Compare memory usage at time ostest_main started until
* user_main exits. These should not be identical, but should
* be similar enough that we can detect any serious OS memory
* leaks.
@@ -458,18 +458,10 @@ static void stdio_test(void)
****************************************************************************/
/****************************************************************************
* user_start/ostest_main
* ostest_main
****************************************************************************/
#ifdef CONFIG_EXAMPLES_OSTEST_BUILTIN
# define MAIN_NAME ostest_main
# define MAIN_STRING "ostest_main: "
#else
# define MAIN_NAME user_start
# define MAIN_STRING "user_start: "
#endif
int MAIN_NAME(int argc, char *argv[])
int ostest_main(int argc, char *argv[])
{
int result;
@@ -492,19 +484,19 @@ int MAIN_NAME(int argc, char *argv[])
/* Set up some environment variables */
#ifndef CONFIG_DISABLE_ENVIRON
printf(MAIN_STRING "putenv(%s)\n", g_putenv_value);
printf("ostest_main: putenv(%s)\n", g_putenv_value);
putenv(g_putenv_value); /* Varaible1=BadValue3 */
printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var1_name, g_var1_value);
printf("ostest_main: setenv(%s, %s, TRUE)\n", g_var1_name, g_var1_value);
setenv(g_var1_name, g_var1_value, TRUE); /* Variable1=GoodValue1 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var2_name, g_bad_value1);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var2_name, g_bad_value1);
setenv(g_var2_name, g_bad_value1, FALSE); /* Variable2=BadValue1 */
printf(MAIN_STRING "setenv(%s, %s, TRUE)\n", g_var2_name, g_var2_value);
printf("ostest_main: setenv(%s, %s, TRUE)\n", g_var2_name, g_var2_value);
setenv(g_var2_name, g_var2_value, TRUE); /* Variable2=GoodValue2 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
setenv(g_var3_name, g_var3_value, FALSE); /* Variable3=GoodValue3 */
printf(MAIN_STRING "setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
printf("ostest_main: setenv(%s, %s, FALSE)\n", g_var3_name, g_var3_name);
setenv(g_var3_name, g_bad_value2, FALSE); /* Variable3=GoodValue3 */
show_environment(true, true, true);
#endif
@@ -518,13 +510,13 @@ int MAIN_NAME(int argc, char *argv[])
#endif
if (result == ERROR)
{
printf(MAIN_STRING "ERROR Failed to start user_main\n");
printf("ostest_main: ERROR Failed to start user_main\n");
}
else
{
printf(MAIN_STRING "Started user_main at PID=%d\n", result);
printf("ostest_main: Started user_main at PID=%d\n", result);
}
printf(MAIN_STRING "Exitting\n");
printf("ostest_main: Exitting\n");
return 0;
}
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
+21 -3
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
@@ -58,12 +58,30 @@
#ifndef CONFIG_SEM_PREALLOCHOLDERS
# define CONFIG_SEM_PREALLOCHOLDERS 0
#endif
#define NLOWPRI_THREADS (CONFIG_SEM_PREALLOCHOLDERS+1)
/* If resources were configured for lots of holders, then run 3 low priority
* threads. Otherwise, just one.
*/
#if CONFIG_SEM_PREALLOCHOLDERS > 3
# define NLOWPRI_THREADS 3
#else
# define NLOWPRI_THREADS 1
#endif
#ifndef CONFIG_SEM_NNESTPRIO
# define CONFIG_SEM_NNESTPRIO 0
#endif
#define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1)
/* Where resources configured for lots of waiters? If so then run 3 high
* priority threads. Otherwise, just one.
*/
#if CONFIG_SEM_NNESTPRIO > 3
# define NHIGHPRI_THREADS 3
#else
# define NHIGHPRI_THREADS 1
#endif
/****************************************************************************
* Private Data
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

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