Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into new_state_machine

Browse Source 
Conflicts:
	src/examples/fixedwing_control/main.c
This commit is contained in:
Julian Oes
2013-06-12 12:24:52 +02:00
parent f5c157e74d 42ce3112ad
commit 7f90ebf537
329 changed files with 5893 additions and 121390 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+22 -58
View File
@@ -1,62 +1,26 @@
.built
.context
*.context
*.bdat
*.pdat
.depend
.updated
.config
.config-e
.version
.project
.cproject
apps/builtin/builtin_list.h
apps/builtin/builtin_proto.h
Make.dep
*.pyc
*.o
*.a
*.d
*~
*.dSYM
Images/*.bin
Images/*.px4
nuttx/Make.defs
nuttx/setenv.sh
nuttx/arch/arm/include/board
nuttx/arch/arm/include/chip
nuttx/arch/arm/src/board
nuttx/arch/arm/src/chip
nuttx/include/apps
nuttx/include/arch
nuttx/include/math.h
nuttx/include/nuttx/config.h
nuttx/include/nuttx/version.h
nuttx/tools/mkconfig
nuttx/tools/mkconfig.exe
nuttx/tools/mkversion
nuttx/tools/mkversion.exe
nuttx/nuttx
nuttx/System.map
nuttx/nuttx.bin
nuttx/nuttx.hex
.configured
.settings
Firmware.sublime-workspace
.DS_Store
cscope.out
.configX-e
nuttx-export.zip
.~lock.*
dot.gdbinit
mavlink/include/mavlink/v0.9/
.*.swp
.swp
core
.gdbinit
mkdeps
Archives
Build
!ROMFS/*/*.d
!ROMFS/*/*/*.d
!ROMFS/*/*/*/*.d
*.dSYM
*.o
*.pyc
*~
.*.swp
.context
.cproject
.DS_Store
.gdbinit
.project
.settings
.swp
.~lock.*
Archives/*
Build/*
core
cscope.out
dot.gdbinit
Firmware.sublime-workspace
Images/*.bin
Images/*.px4
mavlink/include/mavlink/v0.9/
Makefile
+5
View File
@@ -95,9 +95,14 @@ all: $(STAGED_FIRMWARES)
#
# Copy FIRMWARES into the image directory.
#
# XXX copying the .bin files is a hack to work around the PX4IO uploader
# not supporting .px4 files, and it should be deprecated onced that
# is taken care of.
#
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
@echo %% Copying $@
$(Q) $(COPY) $< $@
$(Q) $(COPY) $(patsubst %.px4,%.bin,$<) $(patsubst %.px4,%.bin,$@)
#
# Generate FIRMWARES.
ROMFS/px4fmu_common/mixers/FMU_CCPM.mix
+53
View File
@@ -0,0 +1,53 @@
Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
==================================================
Output 0 - Rear Servo Mixer
----------------
Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
M: 2
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
Output 1 - Left Servo Mixer
-----------------
Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 8660 8660 0 -10000 10000
Output 2 - Right Servo Mixer
----------------
Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - 0.866 * Aileron (Roll - 0)
M: 3
O: 10000 10000 0 -10000 10000
S: 0 3 -10000 -10000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 -8660 -8660 0 -10000 10000
Output 3 - Tail Servo Mixer
----------------
Tail Servo = Yaw (control index = 2)
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
Output 4 - Motor speed mixer
-----------------
This would be the motor speed control output from governor power demand- not sure what index to use here?
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 0 20000 -10000 -10000 10000
ROMFS/px4fmu_common/mixers/IO_pass.mix
+38
View File
@@ -0,0 +1,38 @@
Passthrough mixer for PX4IO
============================
This file defines passthrough mixers suitable for testing.
Channel group 0, channels 0-7 are passed directly through to the outputs.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 0 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 1 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 2 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
Tools/logconv.py
+59
View File
@@ -0,0 +1,59 @@
#!/usr/bin/env python
"""Convert binary log generated by sdlog to CSV format
Usage: python logconv.py <log.bin>"""
__author__ = "Anton Babushkin"
__version__ = "0.1"
import struct, sys
def _unpack_packet(data):
s = ""
s += "Q" #.timestamp = buf.raw.timestamp,
s += "fff" #.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
s += "fff" #.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
s += "fff" #.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
s += "f" #.baro = buf.raw.baro_pres_mbar,
s += "f" #.baro_alt = buf.raw.baro_alt_meter,
s += "f" #.baro_temp = buf.raw.baro_temp_celcius,
s += "ffff" #.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
s += "ffffffff" #.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
s += "f" #.vbat = buf.batt.voltage_v,
s += "f" #.bat_current = buf.batt.current_a,
s += "f" #.bat_discharged = buf.batt.discharged_mah,
s += "ffff" #.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
s += "fff" #.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
s += "iii" #.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
s += "fff" #.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
s += "fffffffff" #.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
s += "fff" #.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
s += "ffff" #.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
s += "ffffff" #.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
s += "f" #.diff_pressure = buf.diff_pres.differential_pressure_pa,
s += "f" #.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
s += "f" #.true_airspeed = buf.airspeed.true_airspeed_m_s
s += "iii" # to align to 280
d = struct.unpack(s, data)
return d
def _main():
if len(sys.argv) < 2:
print "Usage:\npython logconv.py <log.bin>"
return
fn = sys.argv[1]
sysvector_size = 280
f = open(fn, "r")
while True:
data = f.read(sysvector_size)
if len(data) < sysvector_size:
break
a = []
for i in _unpack_packet(data):
a.append(str(i))
print ";".join(a)
f.close()
if __name__ == "__main__":
_main()
Tools/sdlog2_dump.py
+293
View File
@@ -0,0 +1,293 @@
#!/usr/bin/env python
"""Dump binary log generated by sdlog2 or APM as CSV
Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]]
-v Use plain debug output instead of CSV.
-e Recover from errors.
-d Use "delimiter" in CSV. Default is ",".
-n Use "null" as placeholder for empty values in CSV. Default is empty.
-m MSG[.field1,field2,...]
Dump only messages of specified type, and only specified fields.
Multiple -m options allowed."""
__author__ = "Anton Babushkin"
__version__ = "1.2"
import struct, sys
class SDLog2Parser:
BLOCK_SIZE = 8192
MSG_HEADER_LEN = 3
MSG_HEAD1 = 0xA3
MSG_HEAD2 = 0x95
MSG_FORMAT_PACKET_LEN = 89
MSG_FORMAT_STRUCT = "BB4s16s64s"
MSG_TYPE_FORMAT = 0x80
FORMAT_TO_STRUCT = {
"b": ("b", None),
"B": ("B", None),
"h": ("h", None),
"H": ("H", None),
"i": ("i", None),
"I": ("I", None),
"f": ("f", None),
"n": ("4s", None),
"N": ("16s", None),
"Z": ("64s", None),
"c": ("h", 0.01),
"C": ("H", 0.01),
"e": ("i", 0.01),
"E": ("I", 0.01),
"L": ("i", 0.0000001),
"M": ("b", None),
"q": ("q", None),
"Q": ("Q", None),
}
__csv_delim = ","
__csv_null = ""
__msg_filter = []
__time_msg = None
__debug_out = False
__correct_errors = False
def __init__(self):
return
def reset(self):
self.__msg_descrs = {} # message descriptions by message type map
self.__msg_labels = {} # message labels by message name map
self.__msg_names = [] # message names in the same order as FORMAT messages
self.__buffer = "" # buffer for input binary data
self.__ptr = 0 # read pointer in buffer
self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label"
self.__csv_data = {} # current values for all columns
self.__csv_updated = False
self.__msg_filter_map = {} # filter in form of map, with '*" expanded to full list of fields
def setCSVDelimiter(self, csv_delim):
self.__csv_delim = csv_delim
def setCSVNull(self, csv_null):
self.__csv_null = csv_null
def setMsgFilter(self, msg_filter):
self.__msg_filter = msg_filter
def setTimeMsg(self, time_msg):
self.__time_msg = time_msg
def setDebugOut(self, debug_out):
self.__debug_out = debug_out
def setCorrectErrors(self, correct_errors):
self.__correct_errors = correct_errors
def process(self, fn):
self.reset()
if self.__debug_out:
# init __msg_filter_map
for msg_name, show_fields in self.__msg_filter:
self.__msg_filter_map[msg_name] = show_fields
first_data_msg = True
f = open(fn, "r")
bytes_read = 0
while True:
chunk = f.read(self.BLOCK_SIZE)
if len(chunk) == 0:
break
self.__buffer = self.__buffer[self.__ptr:] + chunk
self.__ptr = 0
while self.__bytesLeft() >= self.MSG_HEADER_LEN:
head1 = ord(self.__buffer[self.__ptr])
head2 = ord(self.__buffer[self.__ptr+1])
if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2):
if self.__correct_errors:
self.__ptr += 1
continue
else:
raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2))
msg_type = ord(self.__buffer[self.__ptr+2])
if msg_type == self.MSG_TYPE_FORMAT:
# parse FORMAT message
if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN:
break
self.__parseMsgDescr()
else:
# parse data message
msg_descr = self.__msg_descrs[msg_type]
if msg_descr == None:
raise Exception("Unknown msg type: %i" % msg_type)
msg_length = msg_descr[0]
if self.__bytesLeft() < msg_length:
break
if first_data_msg:
# build CSV columns and init data map
self.__initCSV()
first_data_msg = False
self.__parseMsg(msg_descr)
bytes_read += self.__ptr
if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
self.__printCSVRow()
f.close()
def __bytesLeft(self):
return len(self.__buffer) - self.__ptr
def __filterMsg(self, msg_name):
show_fields = "*"
if len(self.__msg_filter_map) > 0:
show_fields = self.__msg_filter_map.get(msg_name)
return show_fields
def __initCSV(self):
if len(self.__msg_filter) == 0:
for msg_name in self.__msg_names:
self.__msg_filter.append((msg_name, "*"))
for msg_name, show_fields in self.__msg_filter:
if show_fields == "*":
show_fields = self.__msg_labels.get(msg_name, [])
self.__msg_filter_map[msg_name] = show_fields
for field in show_fields:
full_label = msg_name + "." + field
self.__csv_columns.append(full_label)
self.__csv_data[full_label] = None
print self.__csv_delim.join(self.__csv_columns)
def __printCSVRow(self):
s = []
for full_label in self.__csv_columns:
v = self.__csv_data[full_label]
if v == None:
v = self.__csv_null
else:
v = str(v)
s.append(v)
print self.__csv_delim.join(s)
def __parseMsgDescr(self):
data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])
msg_type = data[0]
if msg_type != self.MSG_TYPE_FORMAT:
msg_length = data[1]
msg_name = data[2].strip("\0")
msg_format = data[3].strip("\0")
msg_labels = data[4].strip("\0").split(",")
# Convert msg_format to struct.unpack format string
msg_struct = ""
msg_mults = []
for c in msg_format:
try:
f = self.FORMAT_TO_STRUCT[c]
msg_struct += f[0]
msg_mults.append(f[1])
except KeyError as e:
raise Exception("Unsupported format char: %s in message %s (%i)" % (c, msg_name, msg_type))
msg_struct = "<" + msg_struct # force little-endian
self.__msg_descrs[msg_type] = (msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults)
self.__msg_labels[msg_name] = msg_labels
self.__msg_names.append(msg_name)
if self.__debug_out:
if self.__filterMsg(msg_name) != None:
print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % (
msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)
self.__ptr += self.MSG_FORMAT_PACKET_LEN
def __parseMsg(self, msg_descr):
msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults = msg_descr
if not self.__debug_out and self.__time_msg != None and msg_name == self.__time_msg and self.__csv_updated:
self.__printCSVRow()
self.__csv_updated = False
show_fields = self.__filterMsg(msg_name)
if (show_fields != None):
data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))
for i in xrange(len(data)):
if type(data[i]) is str:
data[i] = data[i].strip("\0")
m = msg_mults[i]
if m != None:
data[i] = data[i] * m
if self.__debug_out:
s = []
for i in xrange(len(data)):
label = msg_labels[i]
if show_fields == "*" or label in show_fields:
s.append(label + "=" + str(data[i]))
print "MSG %s: %s" % (msg_name, ", ".join(s))
else:
# update CSV data buffer
for i in xrange(len(data)):
label = msg_labels[i]
if label in show_fields:
self.__csv_data[msg_name + "." + label] = data[i]
if self.__time_msg != None and msg_name != self.__time_msg:
self.__csv_updated = True
if self.__time_msg == None:
self.__printCSVRow()
self.__ptr += msg_length
def _main():
if len(sys.argv) < 2:
print "Usage: python sdlog2_dump.py <log.bin> [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n"
print "\t-v\tUse plain debug output instead of CSV.\n"
print "\t-e\tRecover from errors.\n"
print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n"
print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n"
print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed."
print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n"
return
fn = sys.argv[1]
debug_out = False
correct_errors = False
msg_filter = []
csv_null = ""
csv_delim = ","
time_msg = None
opt = None
for arg in sys.argv[2:]:
if opt != None:
if opt == "d":
csv_delim = arg
elif opt == "n":
csv_null = arg
elif opt == "t":
time_msg = arg
elif opt == "m":
show_fields = "*"
a = arg.split(".")
if len(a) > 1:
show_fields = a[1].split(",")
msg_filter.append((a[0], show_fields))
opt = None
else:
if arg == "-v":
debug_out = True
elif arg == "-e":
correct_errors = True
elif arg == "-d":
opt = "d"
elif arg == "-n":
opt = "n"
elif arg == "-m":
opt = "m"
elif arg == "-t":
opt = "t"
if csv_delim == "\\t":
csv_delim = "\t"
parser = SDLog2Parser()
parser.setCSVDelimiter(csv_delim)
parser.setCSVNull(csv_null)
parser.setMsgFilter(msg_filter)
parser.setTimeMsg(time_msg)
parser.setDebugOut(debug_out)
parser.setCorrectErrors(correct_errors)
parser.process(fn)
if __name__ == "__main__":
_main()
apps/.gitignore
+10
View File
@@ -0,0 +1,10 @@
*.a
*.bdat
*.pdat
.built
.config
.depend
.updated
builtin/builtin_list.h
builtin/builtin_proto.h
Make.dep
makefiles/config_px4fmu_default.mk
+10 -3
View File
@@ -56,12 +56,14 @@ MODULES += systemcmds/tests
MODULES += modules/commander
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
#
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/position_estimator_mc
MODULES += modules/attitude_estimator_so3_comp
#MODULES += modules/position_estimator_mc
MODULES += modules/position_estimator
MODULES += modules/att_pos_estimator_ekf
@@ -78,17 +80,22 @@ MODULES += modules/multirotor_pos_control
# Logging
#
MODULES += modules/sdlog
MODULES += modules/sdlog2
#
# Libraries
# Library modules
#
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/mathlib
MODULES += modules/mathlib/CMSIS
MODULES += modules/controllib
MODULES += modules/uORB
#
# Libraries
#
LIBRARIES += modules/mathlib/CMSIS
#
# Demo apps
#
makefiles/firmware.mk
+64 -16
View File
@@ -177,20 +177,14 @@ GLOBAL_DEPS += $(MAKEFILE_LIST)
EXTRA_CLEANS =
################################################################################
# Modules
# NuttX libraries and paths
################################################################################
#
# We don't actually know what a module is called; all we have is a path fragment
# that we can search for, and where we expect to find a module.mk file.
#
# As such, we replicate the successfully-found path inside WORK_DIR for the
# module's build products in order to keep modules separated from each other.
#
# XXX If this becomes unwieldy or breaks for other reasons, we will need to
# move to allocating directory names and keeping tabs on makefiles via
# the directory name. That will involve arithmetic (it'd probably be time
# for GMSL).
include $(PX4_MK_DIR)/nuttx.mk
################################################################################
# Modules
################################################################################
# where to look for modules
MODULE_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
@@ -249,10 +243,64 @@ $(MODULE_CLEANS):
clean
################################################################################
# NuttX libraries and paths
# Libraries
################################################################################
include $(PX4_MK_DIR)/nuttx.mk
# where to look for libraries
LIBRARY_SEARCH_DIRS += $(WORK_DIR) $(MODULE_SRC) $(PX4_MODULE_SRC)
# sort and unique the library list
LIBRARIES := $(sort $(LIBRARIES))
# locate the first instance of a library by full path or by looking on the
# library search path
define LIBRARY_SEARCH
$(firstword $(abspath $(wildcard $(1)/library.mk)) \
$(abspath $(foreach search_dir,$(LIBRARY_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/library.mk))) \
MISSING_$1)
endef
# make a list of library makefiles and check that we found them all
LIBRARY_MKFILES := $(foreach library,$(LIBRARIES),$(call LIBRARY_SEARCH,$(library)))
MISSING_LIBRARIES := $(subst MISSING_,,$(filter MISSING_%,$(LIBRARY_MKFILES)))
ifneq ($(MISSING_LIBRARIES),)
$(error Can't find library(s): $(MISSING_LIBRARIES))
endif
# Make a list of the archive files we expect to build from libraries
# Note that this path will typically contain a double-slash at the WORK_DIR boundary; this must be
# preserved as it is used below to get the absolute path for the library.mk file correct.
#
LIBRARY_LIBS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)library.a)
# rules to build module objects
.PHONY: $(LIBRARY_LIBS)
$(LIBRARY_LIBS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_LIBS): mkfile = $(patsubst %library.a,%library.mk,$(relpath))
$(LIBRARY_LIBS): workdir = $(@D)
$(LIBRARY_LIBS): $(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
$(Q) $(MKDIR) -p $(workdir)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
-C $(workdir) \
LIBRARY_WORK_DIR=$(workdir) \
LIBRARY_LIB=$@ \
LIBRARY_MK=$(mkfile) \
LIBRARY_NAME=$(lastword $(subst /, ,$(workdir))) \
library
# make a list of phony clean targets for modules
LIBRARY_CLEANS := $(foreach path,$(dir $(LIBRARY_MKFILES)),$(WORK_DIR)$(path)/clean)
# rules to clean modules
.PHONY: $(LIBRARY_CLEANS)
$(LIBRARY_CLEANS): relpath = $(patsubst $(WORK_DIR)%,%,$@)
$(LIBRARY_CLEANS): mkfile = $(patsubst %clean,%library.mk,$(relpath))
$(LIBRARY_CLEANS):
@$(ECHO) %% cleaning using $(mkfile)
$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
LIBRARY_WORK_DIR=$(dir $@) \
LIBRARY_MK=$(mkfile) \
clean
################################################################################
# ROMFS generation
@@ -420,8 +468,8 @@ $(PRODUCT_BUNDLE): $(PRODUCT_BIN)
$(PRODUCT_BIN): $(PRODUCT_ELF)
$(call SYM_TO_BIN,$<,$@)
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS))
$(PRODUCT_ELF): $(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
$(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS))
#
# Utility rules
makefiles/library.mk
+169
View File
@@ -0,0 +1,169 @@
#
# Copyright (c) 2013 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.
#
#
# Framework makefile for PX4 libraries
#
# This makefile is invoked by firmware.mk to build each of the linraries
# that will subsequently be linked into the firmware image.
#
# Applications are built as standard ar archives. Unlike modules,
# all public symbols in library objects are visible across the entire
# firmware stack.
#
# In general, modules should be preferred to libraries when possible.
# Libraries may also be pre-built.
#
# IMPORTANT NOTE:
#
# This makefile assumes it is being invoked in the library's output directory.
#
#
# Variables that can be set by the library's library.mk:
#
#
# SRCS (optional)
#
# Lists the .c, cpp and .S files that should be compiled/assembled to
# produce the library.
#
# PREBUILT_LIB (optional)
#
# Names the prebuilt library in the source directory that should be
# linked into the firmware.
#
# INCLUDE_DIRS (optional, must be appended, ignored if SRCS not set)
#
# The list of directories searched for include files. If non-standard
# includes (e.g. those from another module) are required, paths to search
# can be added here.
#
#
#
# Variables visible to the library's library.mk:
#
# CONFIG
# BOARD
# LIBRARY_WORK_DIR
# LIBRARY_LIB
# LIBRARY_MK
# Anything set in setup.mk, board_$(BOARD).mk and the toolchain file.
# Anything exported from config_$(CONFIG).mk
#
################################################################################
# No user-serviceable parts below.
################################################################################
ifeq ($(LIBRARY_MK),)
$(error No library makefile specified)
endif
$(info %% LIBRARY_MK = $(LIBRARY_MK))
#
# Get the board/toolchain config
#
include $(PX4_MK_DIR)/board_$(BOARD).mk
#
# Get the library's config
#
include $(LIBRARY_MK)
LIBRARY_SRC := $(dir $(LIBRARY_MK))
$(info % LIBRARY_NAME = $(LIBRARY_NAME))
$(info % LIBRARY_SRC = $(LIBRARY_SRC))
$(info % LIBRARY_LIB = $(LIBRARY_LIB))
$(info % LIBRARY_WORK_DIR = $(LIBRARY_WORK_DIR))
#
# Things that, if they change, might affect everything
#
GLOBAL_DEPS += $(MAKEFILE_LIST)
################################################################################
# Build rules
################################################################################
#
# What we're going to build
#
library: $(LIBRARY_LIB)
ifneq ($(PREBUILT_LIB),)
VPATH = $(LIBRARY_SRC)
$(LIBRARY_LIB): $(PREBUILT_LIB) $(GLOBAL_DEPS)
@$(ECHO) "PREBUILT: $(PREBUILT_LIB)"
$(Q) $(COPY) $< $@
else
##
## Object files we will generate from sources
##
OBJS = $(addsuffix .o,$(SRCS))
#
# SRCS -> OBJS rules
#
$(OBJS): $(GLOBAL_DEPS)
vpath %.c $(LIBRARY_SRC)
$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
$(call COMPILE,$<,$@)
vpath %.cpp $(LIBRARY_SRC)
$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
$(call COMPILEXX,$<,$@)
vpath %.S $(LIBRARY_SRC)
$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
$(call ASSEMBLE,$<,$@)
#
# Built product rules
#
$(LIBRARY_LIB): $(OBJS) $(GLOBAL_DEPS)
$(call ARCHIVE,$@,$(OBJS))
endif
#
# Utility rules
#
clean:
$(Q) $(REMOVE) $(LIBRARY_LIB) $(OBJS)
makefiles/module.mk
+9 -22
View File
@@ -35,7 +35,7 @@
# This makefile is invoked by firmware.mk to build each of the modules
# that will subsequently be linked into the firmware image.
#
# Applications are built as prelinked objects with a limited set of exported
# Modules are built as prelinked objects with a limited set of exported
# symbols, as the global namespace is shared between all modules. Normally an
# module will just export one or more <command>_main functions.
#
@@ -183,30 +183,15 @@ CXXFLAGS += -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
#
module: $(MODULE_OBJ) $(MODULE_COMMAND_FILES)
##
## Locate sources (allows relative source paths in module.mk)
##
#define SRC_SEARCH
# $(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
#endef
#
#ABS_SRCS ?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
#MISSING_SRCS := $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
#ifneq ($(MISSING_SRCS),)
#$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
#endif
#ifeq ($(ABS_SRCS),)
#$(error $(MODULE_MK): nothing to compile in SRCS)
#endif
# Object files we will generate from sources
#
##
## Object files we will generate from sources
##
#OBJS := $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
OBJS = $(addsuffix .o,$(SRCS))
OBJS = $(addsuffix .o,$(SRCS))
$(info SRCS $(SRCS))
$(info OBJS $(OBJS))
#
# Dependency files that will be auto-generated
#
DEPS = $(addsuffix .d,$(SRCS))
#
# SRCS -> OBJS rules
@@ -239,3 +224,5 @@ $(MODULE_OBJ): $(OBJS) $(GLOBAL_DEPS)
clean:
$(Q) $(REMOVE) $(MODULE_PRELINK) $(OBJS)
-include $(DEPS)
makefiles/nuttx.mk
+5 -1
View File
@@ -69,10 +69,14 @@ INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
LIB_DIRS += $(NUTTX_EXPORT_DIR)libs
LIBS += -lapps -lnuttx
LINK_DEPS += $(NUTTX_EXPORT_DIR)libs/libapps.a \
NUTTX_LIBS = $(NUTTX_EXPORT_DIR)libs/libapps.a \
$(NUTTX_EXPORT_DIR)libs/libnuttx.a
LINK_DEPS += $(NUTTX_LIBS)
$(NUTTX_CONFIG_HEADER): $(NUTTX_ARCHIVE)
@$(ECHO) %% Unpacking $(NUTTX_ARCHIVE)
$(Q) $(UNZIP_CMD) -q -o -d $(WORK_DIR) $(NUTTX_ARCHIVE)
$(Q) $(TOUCH) $@
$(LDSCRIPT): $(NUTTX_CONFIG_HEADER)
$(NUTTX_LIBS): $(NUTTX_CONFIG_HEADER)
makefiles/toolchain_gnu-arm-eabi.mk
+15 -6
View File
@@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump
# XXX this is pulled pretty directly from the fmu Make.defs - needs cleanup
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
# Base CPU flags for each of the supported architectures.
#
@@ -70,6 +70,14 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \
-march=armv7-m \
-mfloat-abi=soft
ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \
-ffixed-r10
ARCHINSTRUMENTATIONDEFINES_CORTEXM3 =
# Pick the right set of flags for the architecture.
#
ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH))
@@ -91,8 +99,8 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \
# enable precise stack overflow tracking
# note - requires corresponding support in NuttX
INSTRUMENTATIONDEFINES = -finstrument-functions \
-ffixed-r10
INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
@@ -219,7 +227,7 @@ endef
define PRELINK
@$(ECHO) "PRELINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
$(Q) $(LD) -Ur -Map $1.map -o $1 $2 && $(OBJCOPY) --localize-hidden $1
endef
# Update the archive $1 with the files in $2
@@ -235,7 +243,7 @@ endef
define LINK
@$(ECHO) "LINK: $1"
@$(MKDIR) -p $(dir $1)
$(Q) $(LD) $(LDFLAGS) -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
$(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group
endef
# Convert $1 from a linked object to a raw binary in $2
@@ -280,6 +288,7 @@ define BIN_TO_OBJ
$(Q) $(OBJCOPY) $2 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
--strip-symbol $(call BIN_SYM_PREFIX,$1)_end \
--rename-section .data=.rodata
$(Q) $(REMOVE) $2.c $2.c.o
endef
nuttx/.gitignore
+28
View File
@@ -0,0 +1,28 @@
*.a
.config
.config-e
.configX-e
.depend
.version
arch/arm/include/board
arch/arm/include/chip
arch/arm/src/board
arch/arm/src/chip
include/apps
include/arch
include/math.h
include/nuttx/config.h
include/nuttx/version.h
Make.defs
Make.dep
mkdeps
nuttx
nuttx-export.zip
nuttx.bin
nuttx.hex
setenv.sh
System.map
tools/mkconfig
tools/mkconfig.exe
tools/mkversion
tools/mkversion.exe
nuttx/arch/arm/src/armv7-m/Toolchain.defs
+1 -1
View File
@@ -243,7 +243,7 @@ endif
ifeq ($(CONFIG_ARMV7M_TOOLCHAIN),GNU_EABI)
CROSSDEV ?= arm-none-eabi-
ARCROSSDEV ?= arm-none-eabi-
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
ifeq ($(CONFIG_ARCH_CORTEXM4),y)
ifeq ($(CONFIG_ARCH_FPU),y)
ARCHCPUFLAGS = -mcpu=cortex-m4 -mthumb -march=armv7e-m -mfpu=fpv4-sp-d16 -mfloat-abi=hard
nuttx/configs/px4fmu/common/Make.defs
+1 -1
View File
@@ -58,7 +58,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
ARCHCPUFLAGS = -mcpu=cortex-m4 \
-mthumb \
-march=armv7e-m \
nuttx/configs/px4fmu/nsh/defconfig
Executable → Regular
+6 -6
View File
@@ -248,7 +248,7 @@ CONFIG_SERIAL_TERMIOS=y
CONFIG_SERIAL_CONSOLE_REINIT=y
CONFIG_STANDARD_SERIAL=y
CONFIG_USART1_SERIAL_CONSOLE=y
CONFIG_USART1_SERIAL_CONSOLE=n
CONFIG_USART2_SERIAL_CONSOLE=n
CONFIG_USART3_SERIAL_CONSOLE=n
CONFIG_UART4_SERIAL_CONSOLE=n
@@ -561,7 +561,7 @@ CONFIG_START_MONTH=1
CONFIG_START_DAY=1
CONFIG_GREGORIAN_TIME=n
CONFIG_JULIAN_TIME=n
CONFIG_DEV_CONSOLE=y
CONFIG_DEV_CONSOLE=n
CONFIG_DEV_LOWCONSOLE=n
CONFIG_MUTEX_TYPES=n
CONFIG_PRIORITY_INHERITANCE=y
@@ -717,7 +717,7 @@ CONFIG_ARCH_BZERO=n
# zero for all dynamic allocations.
#
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=8
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_NFILE_STREAMS=25
@@ -925,7 +925,7 @@ CONFIG_USBDEV_TRACE_NRECORDS=512
# Size of the serial receive/transmit buffers. Default 256.
#
CONFIG_CDCACM=y
CONFIG_CDCACM_CONSOLE=n
CONFIG_CDCACM_CONSOLE=y
#CONFIG_CDCACM_EP0MAXPACKET
CONFIG_CDCACM_EPINTIN=1
#CONFIG_CDCACM_EPINTIN_FSSIZE
@@ -955,7 +955,7 @@ CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v1.6"
# CONFIG_NSH_FILEIOSIZE - Size of a static I/O buffer
# CONFIG_NSH_STRERROR - Use strerror(errno)
# CONFIG_NSH_LINELEN - Maximum length of one command line
# CONFIG_NSH_MAX_ARGUMENTS - Maximum number of arguments for command line
# CONFIG_NSH_MAXARGUMENTS - Maximum number of arguments for command line
# CONFIG_NSH_NESTDEPTH - Max number of nested if-then[-else]-fi
# CONFIG_NSH_DISABLESCRIPT - Disable scripting support
# CONFIG_NSH_DISABLEBG - Disable background commands
@@ -988,7 +988,7 @@ CONFIG_NSH_BUILTIN_APPS=y
CONFIG_NSH_FILEIOSIZE=512
CONFIG_NSH_STRERROR=y
CONFIG_NSH_LINELEN=128
CONFIG_NSH_MAX_ARGUMENTS=12
CONFIG_NSH_MAXARGUMENTS=12
CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_DISABLESCRIPT=n
CONFIG_NSH_DISABLEBG=n
nuttx/configs/px4io/common/Make.defs
+1 -1
View File
@@ -58,7 +58,7 @@ NM = $(CROSSDEV)nm
OBJCOPY = $(CROSSDEV)objcopy
OBJDUMP = $(CROSSDEV)objdump
MAXOPTIMIZATION = -O3
MAXOPTIMIZATION ?= -O3
ARCHCPUFLAGS = -mcpu=cortex-m3 \
-mthumb \
-march=armv7-m
src/drivers/gps/ubx.cpp
+1 -1
View File
@@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
msg.msg_class = msg_class;
msg.msg_id = msg_id;
msg.rate = rate;
send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
}
void
src/drivers/hmc5883/hmc5883.cpp
+1 -1
View File
@@ -329,7 +329,7 @@ HMC5883::HMC5883(int bus) :
_calibrated(false)
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// default scaling
_scale.x_offset = 0;
src/drivers/hott_telemetry/hott_telemetry_main.c
+64 -70
View File
@@ -53,6 +53,7 @@
#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include "messages.h"
@@ -60,56 +61,44 @@
static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static char *daemon_name = "hott_telemetry";
static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
static const char daemon_name[] = "hott_telemetry";
static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d <device>]";
/* A little console messaging experiment - console helper macro */
#define FATAL_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg); exit(1);
#define ERROR_MSG(_msg) fprintf(stderr, "[%s] %s\n", daemon_name, _msg);
#define INFO_MSG(_msg) printf("[%s] %s\n", daemon_name, _msg);
/**
* Deamon management function.
*/
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
* Mainloop of daemon.
*/
int hott_telemetry_thread_main(int argc, char *argv[]);
static int read_data(int uart, int *id);
static int send_data(int uart, uint8_t *buffer, int size);
static int recv_req_id(int uart, uint8_t *id);
static int send_data(int uart, uint8_t *buffer, size_t size);
static int open_uart(const char *device, struct termios *uart_config_original)
static int
open_uart(const char *device, struct termios *uart_config_original)
{
/* baud rate */
int speed = B19200;
int uart;
static const speed_t speed = B19200;
/* open uart */
uart = open(device, O_RDWR | O_NOCTTY);
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
char msg[80];
sprintf(msg, "Error opening port: %s\n", device);
FATAL_MSG(msg);
err(1, "Error opening port: %s", device);
}
/* Try to set baud rate */
struct termios uart_config;
/* Back up the original uart configuration to restore it after exit */
int termios_state;
/* Back up the original uart configuration to restore it after exit */
char msg[80];
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
sprintf(msg, "Error getting baudrate / termios config for %s: %d\n", device, termios_state);
close(uart);
FATAL_MSG(msg);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
struct termios uart_config;
tcgetattr(uart, &uart_config);
/* Clear ONLCR flag (which appends a CR for every LF) */
@@ -117,16 +106,14 @@ static int open_uart(const char *device, struct termios *uart_config_original)
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n",
device, termios_state);
close(uart);
FATAL_MSG(msg);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
sprintf(msg, "Error setting baudrate / termios config for %s (tcsetattr)\n", device);
close(uart);
FATAL_MSG(msg);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
}
/* Activate single wire mode */
@@ -135,39 +122,41 @@ static int open_uart(const char *device, struct termios *uart_config_original)
return uart;
}
int read_data(int uart, int *id)
int
recv_req_id(int uart, uint8_t *id)
{
const int timeout = 1000;
static const int timeout_ms = 1000; // TODO make it a define
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
char mode;
uint8_t mode;
if (poll(fds, 1, timeout) > 0) {
if (poll(fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */
read(uart, &mode, 1);
/* Read the device ID being polled */
read(uart, id, 1);
read(uart, &mode, sizeof(mode));
/* if we have a binary mode request */
if (mode != BINARY_MODE_REQUEST_ID) {
return ERROR;
}
/* Read the device ID being polled */
read(uart, id, sizeof(*id));
} else {
ERROR_MSG("UART timeout on TX/RX port");
warnx("UART timeout on TX/RX port");
return ERROR;
}
return OK;
}
int send_data(int uart, uint8_t *buffer, int size)
int
send_data(int uart, uint8_t *buffer, size_t size)
{
usleep(POST_READ_DELAY_IN_USECS);
uint16_t checksum = 0;
for (int i = 0; i < size; i++) {
for (size_t i = 0; i < size; i++) {
if (i == size - 1) {
/* Set the checksum: the first uint8_t is taken as the checksum. */
buffer[i] = checksum & 0xff;
@@ -176,7 +165,7 @@ int send_data(int uart, uint8_t *buffer, int size)
checksum += buffer[i];
}
write(uart, &buffer[i], 1);
write(uart, &buffer[i], sizeof(buffer[i]));
/* Sleep before sending the next byte. */
usleep(POST_WRITE_DELAY_IN_USECS);
@@ -190,13 +179,14 @@ int send_data(int uart, uint8_t *buffer, int size)
return OK;
}
int hott_telemetry_thread_main(int argc, char *argv[])
int
hott_telemetry_thread_main(int argc, char *argv[])
{
INFO_MSG("starting");
warnx("starting");
thread_running = true;
char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
@@ -206,45 +196,55 @@ int hott_telemetry_thread_main(int argc, char *argv[])
} else {
thread_running = false;
ERROR_MSG("missing parameter to -d");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "missing parameter to -d\n%s", commandline_usage);
}
}
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
struct termios uart_config_original;
int uart = open_uart(device, &uart_config_original);
const int uart = open_uart(device, &uart_config_original);
if (uart < 0) {
ERROR_MSG("Failed opening HoTT UART, exiting.");
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
exit(ERROR);
}
messages_init();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
int size = 0;
int id = 0;
size_t size = 0;
uint8_t id = 0;
bool connected = true;
while (!thread_should_exit) {
if (read_data(uart, &id) == OK) {
if (recv_req_id(uart, &id) == OK) {
if (!connected) {
connected = true;
warnx("OK");
}
switch (id) {
case ELECTRIC_AIR_MODULE:
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;
default:
continue; // Not a module we support.
}
send_data(uart, buffer, size);
} else {
connected = false;
warnx("syncing");
}
}
INFO_MSG("exiting");
warnx("exiting");
close(uart);
@@ -256,23 +256,22 @@ int hott_telemetry_thread_main(int argc, char *argv[])
/**
* Process command line arguments and tart the daemon.
*/
int hott_telemetry_main(int argc, char *argv[])
int
hott_telemetry_main(int argc, char *argv[])
{
if (argc < 1) {
ERROR_MSG("missing command");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "missing command\n%s", commandline_usage);
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
INFO_MSG("deamon already running");
warnx("deamon already running");
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("hott_telemetry",
deamon_task = task_spawn(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2048,
@@ -288,19 +287,14 @@ int hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
INFO_MSG("daemon is running");
warnx("daemon is running");
} else {
INFO_MSG("daemon not started");
warnx("daemon not started");
}
exit(0);
}
ERROR_MSG("unrecognized command");
ERROR_MSG(commandline_usage);
exit(1);
errx(1, "unrecognized command\n%s", commandline_usage);
}
src/drivers/hott_telemetry/messages.c
+153 -21
View File
@@ -1,7 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Simon Wilks <sjwilks@gmail.com>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Simon Wilks <sjwilks@gmail.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -34,30 +34,44 @@
/**
* @file messages.c
* @author Simon Wilks <sjwilks@gmail.com>
*
*/
#include "messages.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/geo/geo.h>
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
static int airspeed_sub = -1;
static int battery_sub = -1;
static int gps_sub = -1;
static int home_sub = -1;
static int sensor_sub = -1;
void messages_init(void)
static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
void
messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
home_sub = orb_subscribe(ORB_ID(home_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
}
void build_eam_response(uint8_t *buffer, int *size)
void
build_eam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
@@ -74,26 +88,144 @@ void build_eam_response(uint8_t *buffer, int *size)
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.eam_sensor_id = ELECTRIC_AIR_MODULE;
msg.sensor_id = EAM_SENSOR_ID;
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = TEMP_ZERO_CELSIUS;
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* get a local copy of the current sensor values */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
msg.speed_L = (uint8_t)speed & 0xff;
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
void
build_gps_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
struct gps_module_msg msg = { 0 };
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_visible;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
msg.gps_fix = (uint8_t)(gps.fix_type + 48);
/* No point collecting more data if we don't have a 3D fix yet */
if (gps.fix_type > 2) {
/* Current flight direction */
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
/* GPS speed */
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
/* Get latitude in degrees, minutes and seconds */
double lat = ((double)(gps.lat))*1e-7d;
/* Set the N or S specifier */
msg.latitude_ns = 0;
if (lat < 0) {
msg.latitude_ns = 1;
lat = abs(lat);
}
int deg;
int min;
int sec;
convert_to_degrees_minutes_seconds(lat, &deg, &min, &sec);
uint16_t lat_min = (uint16_t)(deg * 100 + min);
msg.latitude_min_L = (uint8_t)lat_min & 0xff;
msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff;
uint16_t lat_sec = (uint16_t)(sec);
msg.latitude_sec_L = (uint8_t)lat_sec & 0xff;
msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff;
/* Get longitude in degrees, minutes and seconds */
double lon = ((double)(gps.lon))*1e-7d;
/* Set the E or W specifier */
msg.longitude_ew = 0;
if (lon < 0) {
msg.longitude_ew = 1;
lon = abs(lon);
}
convert_to_degrees_minutes_seconds(lon, &deg, &min, &sec);
uint16_t lon_min = (uint16_t)(deg * 100 + min);
msg.longitude_min_L = (uint8_t)lon_min & 0xff;
msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff;
uint16_t lon_sec = (uint16_t)(sec);
msg.longitude_sec_L = (uint8_t)lon_sec & 0xff;
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) home_sub postion report */
bool updated;
orb_check(home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), home_sub, &home);
home_lat = ((double)(home.lat))*1e-7d;
home_lon = ((double)(home.lon))*1e-7d;
home_position_set = true;
}
/* Distance from home */
if (home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
msg.distance_L = (uint8_t)dist & 0xff;
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
/* Direction back to home */
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
msg.home_direction = (uint8_t)bearing >> 1;
}
}
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
}
void
convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec)
{
*deg = (int)val;
double delta = val - *deg;
const double min_d = delta * 60.0d;
*min = (int)min_d;
delta = min_d - *min;
*sec = (int)(delta * 10000.0d);
}
src/drivers/hott_telemetry/messages.h
+97 -25
View File
@@ -44,11 +44,6 @@
#include <stdlib.h>
/* The buffer size used to store messages. This must be at least as big as the number of
* fields in the largest message struct.
*/
#define MESSAGE_BUFFER_SIZE 50
/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request.
* Note that the value specified here is lower than 5000 (5ms) as time is lost constucting
* the message after the read which takes some milliseconds.
@@ -66,18 +61,18 @@
#define TEMP_ZERO_CELSIUS 0x14
/* Electric Air Module (EAM) constants. */
#define ELECTRIC_AIR_MODULE 0x8e
#define EAM_SENSOR_ID 0xe0
#define EAM_SENSOR_ID 0x8e
#define EAM_SENSOR_TEXT_ID 0xe0
/* The Electric Air Module message. */
struct eam_module_msg {
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor ID */
uint8_t start; /**< Start byte */
uint8_t eam_sensor_id; /**< EAM sensor */
uint8_t warning;
uint8_t sensor_id; /**< Sensor ID, why different? */
uint8_t sensor_id; /**< Sensor ID, why different? */
uint8_t alarm_inverse1;
uint8_t alarm_inverse2;
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */
uint8_t cell2_L;
uint8_t cell3_L;
uint8_t cell4_L;
@@ -95,30 +90,107 @@ struct eam_module_msg {
uint8_t batt1_voltage_H;
uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */
uint8_t batt2_voltage_H;
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */
uint8_t temperature2;
uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */
uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */
uint8_t altitude_H;
uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */
uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */
uint8_t current_H;
uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */
uint8_t main_voltage_H;
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */
uint8_t battery_capacity_H;
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */
uint8_t climbrate_H;
uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */
uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */
uint8_t rpm_H;
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t electric_min; /**< Flight time in minutes. */
uint8_t electric_sec; /**< Flight time in seconds. */
uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */
uint8_t speed_H;
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
uint8_t stop; /**< Stop byte */
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define MESSAGE_BUFFER_SIZE sizeof(union { \
struct eam_module_msg eam; \
})
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8A
#define GPS_SENSOR_TEXT_ID 0xA0
/**
* The GPS sensor message
* Struct based on: https://code.google.com/p/diy-hott-gps/downloads
*/
struct gps_module_msg {
uint8_t start; /**< Start byte */
uint8_t sensor_id; /**< GPS sensor ID*/
uint8_t warning; /**< Byte 3: 0…= warning beeps */
uint8_t sensor_text_id; /**< GPS Sensor text mode ID */
uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */
uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */
uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */
uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */
uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */
uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */
uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */
uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */
uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */
uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */
uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */
uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */
uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */
uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/
uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */
uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */
uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */
uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */
uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */
uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */
uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */
uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */
uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */
uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */
uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */
uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */
uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */
uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */
uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */
uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */
uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */
uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */
uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */
uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */
uint8_t vibration; /**< Byte 39: vibration (1 bytes) */
uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */
uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */
uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */
uint8_t version; /**< Byte 43: 00 version number */
uint8_t stop; /**< Byte 44: 0x7D Ende byte */
uint8_t checksum; /**< Byte 45: Parity Byte */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gps_module_msg gps; \
})
void messages_init(void);
void build_eam_response(uint8_t *buffer, int *size);
void build_eam_response(uint8_t *buffer, size_t *size);
void build_gps_response(uint8_t *buffer, size_t *size);
float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
#endif /* MESSAGES_H_ */
src/drivers/mkblctrl/mkblctrl.cpp
+287 -237
View File
File diff suppressed because it is too large Load Diff
src/drivers/px4io/px4io.cpp
+86 -9
View File
@@ -106,7 +106,7 @@ public:
* @param rate The rate in Hz actuator outpus are sent to IO.
* Min 10 Hz, max 400 Hz
*/
int set_update_rate(int rate);
int set_update_rate(int rate);
/**
* Set the battery current scaling and bias
@@ -114,7 +114,15 @@ public:
* @param amp_per_volt
* @param amp_bias
*/
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
/**
* Push failsafe values to IO.
*
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
* @param len Number of channels, could up to 8
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Print the current status of IO
@@ -326,11 +334,11 @@ PX4IO::PX4IO() :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0),
_primary_pwm_device(false)
_battery_last_timestamp(0)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -689,6 +697,19 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
}
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
uint16_t regs[_max_actuators];
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int
PX4IO::io_set_arming_state()
{
@@ -1250,7 +1271,7 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
@@ -1262,7 +1283,8 @@ PX4IO::print_status()
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : ""));
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
printf("alarms 0x%04x%s%s%s%s%s%s%s\n",
alarms,
@@ -1273,11 +1295,14 @@ PX4IO::print_status()
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""));
/* now clear alarms */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF);
printf("vbatt %u ibatt %u vbatt scale %u\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
printf("amp_per_volt %.3f amp_offset %.3f mAh discharged %.3f\n",
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
@@ -1471,7 +1496,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
ret = mixer_send(buf, strnlen(buf, 1024));
ret = mixer_send(buf, strnlen(buf, 2048));
break;
}
@@ -1612,6 +1637,13 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
for (;;) {
/* sweep all servos between 1000..2000 */
@@ -1646,6 +1678,16 @@ test(void)
if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
}
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
warnx("User abort\n");
close(console);
exit(0);
}
}
}
}
@@ -1715,6 +1757,41 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
{
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i+2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
if (g_dev != nullptr) {
@@ -1842,5 +1919,5 @@ px4io_main(int argc, char *argv[])
monitor();
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
}
src/drivers/stm32/drv_hrt.c
+1 -1
View File
@@ -330,7 +330,7 @@ static void hrt_call_invoke(void);
/*
* PPM decoder tuning parameters
*/
# define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */
# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */
# define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */
# define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */
# define PPM_MIN_START 2500 /* shortest valid start gap */
src/examples/fixedwing_control/main.c
+109 -21
View File
@@ -33,10 +33,13 @@
****************************************************************************/
/**
* @file main.c
* Implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller flying manual attitude control or auto waypoint control.
*
* Example implementation of a fixed wing attitude controller. This file is a complete
* fixed wing controller for manual attitude control or auto waypoint control.
* There is no need to touch any other system components to extend / modify the
* complete control architecture.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
@@ -60,7 +63,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h>
#include <systemlib/pid/pid.h>
@@ -73,8 +75,15 @@
#include "params.h"
/* Prototypes */
/**
* Daemon management function.
*
* This function allows to start / stop the background task (daemon).
* The purpose of it is to be able to start the controller on the
* command line, query its status and stop it, without giving up
* the command line to one particular process or the need for bg/fg
* ^Z support by the shell.
*/
__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
@@ -88,10 +97,34 @@ int fixedwing_control_thread_main(int argc, char *argv[]);
*/
static void usage(const char *reason);
/**
* Control roll and pitch angle.
*
* This very simple roll and pitch controller takes the current roll angle
* of the system and compares it to a reference. Pitch is controlled to zero and yaw remains
* uncontrolled (tutorial code, not intended for flight).
*
* @param att_sp The current attitude setpoint - the values the system would like to reach.
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param speed_body The velocity of the system. Currently unused.
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
* Control heading.
*
* This very simple heading to roll angle controller outputs the desired roll angle based on
* the current position of the system, the desired position (the setpoint) and the current
* heading.
*
* @param pos The current position of the system
* @param sp The current position setpoint
* @param att The current attitude
* @param att_sp The attitude setpoint. This is the output of the controller
*/
void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
@@ -103,7 +136,7 @@ static struct params p;
static struct param_handles ph;
void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp,
float speed_body[], struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
@@ -148,13 +181,23 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
* Calculate heading error of current position to desired position
*/
/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
/*
* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution,
* so they need to be scaled by 1e7 and converted to IEEE double precision floating point.
*/
float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
/* calculate heading error */
float yaw_err = att->yaw - bearing;
/* apply control gain */
att_sp->roll_body = yaw_err * p.hdng_p;
float roll_command = yaw_err * p.hdng_p;
/* limit output, this commonly is a tuning parameter, too */
if (att_sp->roll_body < -0.6f) {
att_sp->roll_body = -0.6f;
} else if (att_sp->roll_body > 0.6f) {
att_sp->roll_body = 0.6f;
}
}
/* Main Thread */
@@ -176,7 +219,32 @@ int fixedwing_control_thread_main(int argc, char *argv[])
parameters_init(&ph);
parameters_update(&ph, &p);
/* declare and safely initialize all structs to zero */
/*
* PX4 uses a publish/subscribe design pattern to enable
* multi-threaded communication.
*
* The most elegant aspect of this is that controllers and
* other processes can either 'react' to new data, or run
* at their own pace.
*
* PX4 developer guide:
* https://pixhawk.ethz.ch/px4/dev/shared_object_communication
*
* Wikipedia description:
* http://en.wikipedia.org/wiki/Publish–subscribe_pattern
*
*/
/*
* Declare and safely initialize all structs to zero.
*
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -192,20 +260,24 @@ int fixedwing_control_thread_main(int argc, char *argv[])
struct vehicle_global_position_setpoint_s global_sp;
memset(&global_sp, 0, sizeof(global_sp));
/* output structs */
/* output structs - this is what is sent to the mixer */
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
/* publish actuator controls */
/* publish actuator controls with zero values */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
/*
* Advertise that this controller will publish actuator
* control values and the rate setpoint
*/
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
/* subscribe */
/* subscribe to topics. */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -215,8 +287,9 @@ int fixedwing_control_thread_main(int argc, char *argv[])
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */
float gyro[3] = {0.0f, 0.0f, 0.0f};
float speed_body[3] = {0.0f, 0.0f, 0.0f};
/* RC failsafe check */
bool throttle_half_once = false;
struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }};
@@ -235,7 +308,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
int ret = poll(fds, 2, 500);
if (ret < 0) {
/* poll error, this will not really happen in practice */
/*
* Poll error, this will not really happen in practice,
* but its good design practice to make output an error message.
*/
warnx("poll error");
} else if (ret == 0) {
@@ -261,6 +337,8 @@ int fixedwing_control_thread_main(int argc, char *argv[])
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_sp_sub, &global_sp_updated);
bool manual_sp_updated;
orb_check(manual_sp_sub, &manual_sp_updated);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
@@ -268,6 +346,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
/* currently speed in body frame is not used, but here for reference */
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
@@ -285,12 +364,19 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
}
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
if (manual_sp_updated)
/* get the RC (or otherwise user based) input */
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
gyro[0] = att.rollspeed;
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.6f) &&
(manual_sp.throttle <= 1.0f)) {
throttle_half_once = true;
}
/* get the system status and the flight mode we're in */
orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
/* control */
@@ -307,7 +393,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
actuators.control[2] = 0.0f;
/* simple attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
@@ -316,10 +402,12 @@ int fixedwing_control_thread_main(int argc, char *argv[])
actuators.control[4] = 0.0f;
} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
if (vstatus.rc_signal_lost && throttle_half_once) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
@@ -350,7 +438,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
src/examples/fixedwing_control/params.c
+1 -1
View File
@@ -45,7 +45,7 @@
/**
*
*/
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.1f);
/**
*
src/examples/px4_daemon_app/px4_daemon_app.c
+29 -25
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
* Copyright (c) 2012, 2013 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
@@ -33,27 +32,33 @@
****************************************************************************/
/**
* @file px4_deamon_app.c
* Deamon application example for PX4 autopilot
* @file px4_daemon_app.c
* daemon application example for PX4 autopilot
*
* @author Example User <mail@example.com>
*/
#include <nuttx/config.h>
#include <nuttx/sched.h>
#include <unistd.h>
#include <stdio.h>
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
static bool thread_should_exit = false; /**< daemon exit flag */
static bool thread_running = false; /**< daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
/**
* Deamon management function.
* daemon management function.
*/
__EXPORT int px4_deamon_app_main(int argc, char *argv[]);
__EXPORT int px4_daemon_app_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
* Mainloop of daemon.
*/
int px4_deamon_thread_main(int argc, char *argv[]);
int px4_daemon_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
@@ -64,20 +69,19 @@ static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
warnx("%s\n", reason);
errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
}
/**
* The deamon app only briefly exists to start
* The daemon 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 px4_deamon_app_main(int argc, char *argv[])
int px4_daemon_app_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
@@ -85,17 +89,17 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("deamon already running\n");
warnx("daemon already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("deamon",
daemon_task = task_spawn("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
px4_deamon_thread_main,
px4_daemon_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
@@ -107,9 +111,9 @@ int px4_deamon_app_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tdeamon app is running\n");
warnx("\trunning\n");
} else {
printf("\tdeamon app not started\n");
warnx("\tnot started\n");
}
exit(0);
}
@@ -118,18 +122,18 @@ int px4_deamon_app_main(int argc, char *argv[])
exit(1);
}
int px4_deamon_thread_main(int argc, char *argv[]) {
int px4_daemon_thread_main(int argc, char *argv[]) {
printf("[deamon] starting\n");
warnx("[daemon] starting\n");
thread_running = true;
while (!thread_should_exit) {
printf("Hello Deamon!\n");
warnx("Hello daemon!\n");
sleep(10);
}
printf("[deamon] exiting.\n");
warnx("[daemon] exiting.\n");
thread_running = false;
src/include/mavlink/mavlink_log.h
+27 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -47,27 +47,42 @@
*/
#include <sys/ioctl.h>
/*
/**
* The mavlink log device node; must be opened before messages
* can be logged.
*/
#define MAVLINK_LOG_DEVICE "/dev/mavlink"
/**
* The maximum string length supported.
*/
#define MAVLINK_LOG_MAXLEN 50
#define MAVLINK_IOC_SEND_TEXT_INFO _IOC(0x1100, 1)
#define MAVLINK_IOC_SEND_TEXT_CRITICAL _IOC(0x1100, 2)
#define MAVLINK_IOC_SEND_TEXT_EMERGENCY _IOC(0x1100, 3)
#ifdef __cplusplus
extern "C" {
#endif
__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
#ifdef __cplusplus
}
#endif
/*
* The va_args implementation here is not beautiful, but obviously we run into the same issues
* the GCC devs saw, and are using their solution:
*
* http://gcc.gnu.org/onlinedocs/cpp/Variadic-Macros.html
*/
/**
* Send a mavlink emergency message.
*
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#else
#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#endif
#define mavlink_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
/**
* Send a mavlink critical message.
@@ -75,11 +90,7 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#else
#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#endif
#define mavlink_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
/**
* Send a mavlink info message.
@@ -87,14 +98,10 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#else
#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#endif
#define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
struct mavlink_logmessage {
char text[51];
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
};
@@ -116,5 +123,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
#endif
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+3 -1
View File
@@ -683,7 +683,8 @@ int KalmanNav::correctPos()
// fault detetcion
float beta = y.dot(S.inverse() * y);
if (beta > _faultPos.get()) {
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / sqrtf(RPos(0, 0))),
@@ -693,6 +694,7 @@ int KalmanNav::correctPos()
double(y(4) / sqrtf(RPos(4, 4))),
double(y(5) / sqrtf(RPos(5, 5))));
}
counter++;
return ret_ok;
}
src/modules/att_pos_estimator_ekf/kalman_main.cpp
+8 -7
View File
@@ -44,6 +44,7 @@
#include <string.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include "KalmanNav.hpp"
@@ -73,7 +74,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
exit(1);
}
@@ -94,13 +95,13 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("kalman_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("kalman_demo",
deamon_task = task_spawn("att_pos_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4096,
@@ -116,10 +117,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tkalman_demo app is running\n");
warnx("is running\n");
} else {
printf("\tkalman_demo app not started\n");
warnx("not started\n");
}
exit(0);
@@ -132,7 +133,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
int kalman_demo_thread_main(int argc, char *argv[])
{
printf("[kalman_demo] starting\n");
warnx("starting\n");
using namespace math;
@@ -144,7 +145,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
nav.update();
}
printf("[kalman_demo] exiting.\n");
printf("exiting.\n");
thread_running = false;
src/modules/attitude_estimator_so3_comp/README
+5
View File
@@ -0,0 +1,5 @@
Synopsis
nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
Option -d is for debugging packet. See code for detailed packet structure.
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
Executable
+833
View File
File diff suppressed because it is too large Load Diff
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
Executable
+63
View File
@@ -0,0 +1,63 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.c
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include "attitude_estimator_so3_comp_params.h"
/* This is filter gain for nonlinear SO3 complementary filter */
/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
will compensate gyro bias which depends on temperature and vibration of your vehicle */
PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
//! You can set this gain higher if you want more fast response.
//! But note that higher gain will give you also higher overshoot.
PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
//! This gain is depend on your vehicle status.
/* offsets in roll, pitch and yaw of sensor plane and body */
PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
{
/* Filter gain parameters */
h->Kp = param_find("SO3_COMP_KP");
h->Ki = param_find("SO3_COMP_KI");
/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
h->roll_off = param_find("ATT_ROLL_OFFS");
h->pitch_off = param_find("ATT_PITCH_OFFS");
h->yaw_off = param_find("ATT_YAW_OFFS");
return OK;
}
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
{
/* Update filter gain */
param_get(h->Kp, &(p->Kp));
param_get(h->Ki, &(p->Ki));
/* Update attitude offset */
param_get(h->roll_off, &(p->roll_off));
param_get(h->pitch_off, &(p->pitch_off));
param_get(h->yaw_off, &(p->yaw_off));
return OK;
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
Executable
+44
View File
@@ -0,0 +1,44 @@
/*
* Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
*
* @file attitude_estimator_so3_comp_params.h
*
* Implementation of nonlinear complementary filters on the SO(3).
* This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
* Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
*
* Theory of nonlinear complementary filters on the SO(3) is based on [1].
* Quaternion realization of [1] is based on [2].
* Optmized quaternion update code is based on Sebastian Madgwick's implementation.
*
* References
* [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
* [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
*/
#include <systemlib/param/param.h>
struct attitude_estimator_so3_comp_params {
float Kp;
float Ki;
float roll_off;
float pitch_off;
float yaw_off;
};
struct attitude_estimator_so3_comp_param_handles {
param_t Kp, Ki;
param_t roll_off, pitch_off, yaw_off;
};
/**
* Initialize all parameter handles and values
*
*/
int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
/**
* Update all parameters
*
*/
int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
src/modules/attitude_estimator_so3_comp/module.mk
+8
View File
@@ -0,0 +1,8 @@
#
# Attitude estimator (Nonlinear SO3 complementary Filter)
#
MODULE_COMMAND = attitude_estimator_so3_comp
SRCS = attitude_estimator_so3_comp_main.cpp \
attitude_estimator_so3_comp_params.c
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+9 -8
View File
@@ -47,6 +47,7 @@
#include <systemlib/systemlib.h>
#include <controllib/fixedwing.hpp>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <math.h>
@@ -80,7 +81,7 @@ usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n");
fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
@@ -101,13 +102,13 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("control_demo already running\n");
warnx("already running");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn("control_demo",
deamon_task = task_spawn("fixedwing_backside",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
5120,
@@ -128,10 +129,10 @@ int fixedwing_backside_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tcontrol_demo app is running\n");
warnx("is running");
} else {
printf("\tcontrol_demo app not started\n");
warnx("not started");
}
exit(0);
@@ -144,7 +145,7 @@ int fixedwing_backside_main(int argc, char *argv[])
int control_demo_thread_main(int argc, char *argv[])
{
printf("[control_Demo] starting\n");
warnx("starting");
using namespace control;
@@ -156,7 +157,7 @@ int control_demo_thread_main(int argc, char *argv[])
autopilot.update();
}
printf("[control_demo] exiting.\n");
warnx("exiting.");
thread_running = false;
@@ -165,6 +166,6 @@ int control_demo_thread_main(int argc, char *argv[])
void test()
{
printf("beginning control lib test\n");
warnx("beginning control lib test");
control::basicBlocksTest();
}
src/modules/fixedwing_backside/module.mk
+2 -1
View File
@@ -37,4 +37,5 @@
MODULE_COMMAND = fixedwing_backside
SRCS = fixedwing_backside_main.cpp
SRCS = fixedwing_backside_main.cpp \
params.c
src/modules/gpio_led/gpio_led.c
+217
View File
@@ -0,0 +1,217 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* 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 gpio_led.c
*
* Status LED via GPIO driver.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
struct gpio_led_s {
struct work_s work;
int gpio_fd;
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
bool led_state;
int counter;
};
static struct gpio_led_s gpio_led_data;
static bool gpio_led_started = false;
__EXPORT int gpio_led_main(int argc, char *argv[]);
void gpio_led_start(FAR void *arg);
void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_1;
if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (argc > 2) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
pin = GPIO_EXT_2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
} else {
gpio_led_started = true;
}
exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
/* INVALID COMMAND */
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
}
}
}
void gpio_led_start(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* open GPIO device */
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
if (priv->gpio_fd < 0) {
warnx("[gpio_led] GPIO: open fail\n");
return;
}
/* configure GPIO pin */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */
memset(&priv->status, 0, sizeof(priv->status));
priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* add worker to queue */
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
return;
}
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
/* check for status updates*/
bool status_updated;
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
/* select pattern for current status */
int pattern = 0;
if (priv->status.flag_system_armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
} else {
pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning)
}
} else {
if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm)
}
}
/* blink pattern */
bool led_state_new = (pattern & (1 << priv->counter)) != 0;
if (led_state_new != priv->led_state) {
priv->led_state = led_state_new;
if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
}
priv->counter++;
if (priv->counter > 5)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
if (gpio_led_started)
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
}
src/modules/gpio_led/module.mk
+39
View File
@@ -0,0 +1,39 @@
############################################################################
#
# Copyright (C) 2013 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.
#
############################################################################
#
# Status LED via GPIO driver
#
MODULE_COMMAND = gpio_led
SRCS = gpio_led.c
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_f32.c
-159
View File
@@ -1,159 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_f32.c
*
* Description: Vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
#include <math.h>
/**
* @ingroup groupMath
*/
/**
* @defgroup BasicAbs Vector Absolute Value
*
* Computes the absolute value of a vector on an element-by-element basis.
*
* <pre>
* pDst[n] = abs(pSrcA[n]), 0 <= n < blockSize.
* </pre>
*
* The operation can be done in-place by setting the input and output pointers to the same buffer.
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Floating-point vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*/
void arm_abs_f32(
float32_t * pSrc,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t in1, in2, in3, in4; /* temporary variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute and then store the results in the destination buffer. */
/* read sample from source */
in1 = *pSrc;
in2 = *(pSrc + 1);
in3 = *(pSrc + 2);
/* find absolute value */
in1 = fabsf(in1);
/* read sample from source */
in4 = *(pSrc + 3);
/* find absolute value */
in2 = fabsf(in2);
/* read sample from source */
*pDst = in1;
/* find absolute value */
in3 = fabsf(in3);
/* find absolute value */
in4 = fabsf(in4);
/* store result to destination */
*(pDst + 1) = in2;
/* store result to destination */
*(pDst + 2) = in3;
/* store result to destination */
*(pDst + 3) = in4;
/* Update source pointer to process next sampels */
pSrc += 4u;
/* Update destination pointer to process next sampels */
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute and then store the results in the destination buffer. */
*pDst++ = fabsf(*pSrc++);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q15.c
-173
View File
@@ -1,173 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q15.c
*
* Description: Q15 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q15 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.
*/
void arm_abs_q15(
q15_t * pSrc,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q15_t in1; /* Input value1 */
q15_t in2; /* Input value2 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Read two inputs */
in1 = *pSrc++;
in2 = *pSrc++;
/* Store the Absolute result in the destination buffer by packing the two values, in a single cycle */
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ =
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
#else
*__SIMD32(pDst)++ =
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
in1 = *pSrc++;
in2 = *pSrc++;
#ifndef ARM_MATH_BIG_ENDIAN
*__SIMD32(pDst)++ =
__PKHBT(((in1 > 0) ? in1 : __QSUB16(0, in1)),
((in2 > 0) ? in2 : __QSUB16(0, in2)), 16);
#else
*__SIMD32(pDst)++ =
__PKHBT(((in2 > 0) ? in2 : __QSUB16(0, in2)),
((in1 > 0) ? in1 : __QSUB16(0, in1)), 16);
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in1 = *pSrc++;
/* Calculate absolute value of input and then store the result in the destination buffer. */
*pDst++ = (in1 > 0) ? in1 : __QSUB16(0, in1);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
q15_t in; /* Temporary input variable */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in = *pSrc++;
/* Calculate absolute value of input and then store the result in the destination buffer. */
*pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAbs group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q31.c
-125
View File
@@ -1,125 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q31.c
*
* Description: Q31 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q31 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF.
*/
void arm_abs_q31(
q31_t * pSrc,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q31_t in; /* Input value */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
in1 = *pSrc++;
in2 = *pSrc++;
in3 = *pSrc++;
in4 = *pSrc++;
*pDst++ = (in1 > 0) ? in1 : __QSUB(0, in1);
*pDst++ = (in2 > 0) ? in2 : __QSUB(0, in2);
*pDst++ = (in3 > 0) ? in3 : __QSUB(0, in3);
*pDst++ = (in4 > 0) ? in4 : __QSUB(0, in4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = |A| */
/* Calculate absolute value of the input (if -1 then saturated to 0x7fffffff) and then store the results in the destination buffer. */
in = *pSrc++;
*pDst++ = (in > 0) ? in : ((in == 0x80000000) ? 0x7fffffff : -in);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_abs_q7.c
-152
View File
@@ -1,152 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_abs_q7.c
*
* Description: Q7 vector absolute value.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAbs
* @{
*/
/**
* @brief Q7 vector absolute value.
* @param[in] *pSrc points to the input buffer
* @param[out] *pDst points to the output buffer
* @param[in] blockSize number of samples in each vector
* @return none.
*
* \par Conditions for optimum performance
* Input and output buffers should be aligned by 32-bit
*
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F.
*/
void arm_abs_q7(
q7_t * pSrc,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
q7_t in; /* Input value1 */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t in1, in2, in3, in4; /* temporary input variables */
q31_t out1, out2, out3, out4; /* temporary output variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = |A| */
/* Read inputs */
in1 = (q31_t) * pSrc;
in2 = (q31_t) * (pSrc + 1);
in3 = (q31_t) * (pSrc + 2);
/* find absolute value */
out1 = (in1 > 0) ? in1 : __QSUB8(0, in1);
/* read input */
in4 = (q31_t) * (pSrc + 3);
/* find absolute value */
out2 = (in2 > 0) ? in2 : __QSUB8(0, in2);
/* store result to destination */
*pDst = (q7_t) out1;
/* find absolute value */
out3 = (in3 > 0) ? in3 : __QSUB8(0, in3);
/* find absolute value */
out4 = (in4 > 0) ? in4 : __QSUB8(0, in4);
/* store result to destination */
*(pDst + 1) = (q7_t) out2;
/* store result to destination */
*(pDst + 2) = (q7_t) out3;
/* store result to destination */
*(pDst + 3) = (q7_t) out4;
/* update pointers to process next samples */
pSrc += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
blkCnt = blockSize;
#endif // #define ARM_MATH_CM0
while(blkCnt > 0u)
{
/* C = |A| */
/* Read the input */
in = *pSrc++;
/* Store the Absolute result in the destination buffer */
*pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? 0x7f : -in);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAbs group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c
-145
View File
@@ -1,145 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_f32.c
*
* Description: Floating-point vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup BasicAdd Vector Addition
*
* Element-by-element addition of two vectors.
*
* <pre>
* pDst[n] = pSrcA[n] + pSrcB[n], 0 <= n < blockSize.
* </pre>
*
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Floating-point vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*/
void arm_add_f32(
float32_t * pSrcA,
float32_t * pSrcB,
float32_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
float32_t inA1, inA2, inA3, inA4; /* temporary input variabels */
float32_t inB1, inB2, inB3, inB4; /* temporary input variables */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
/* read four inputs from sourceA and four inputs from sourceB */
inA1 = *pSrcA;
inB1 = *pSrcB;
inA2 = *(pSrcA + 1);
inB2 = *(pSrcB + 1);
inA3 = *(pSrcA + 2);
inB3 = *(pSrcB + 2);
inA4 = *(pSrcA + 3);
inB4 = *(pSrcB + 3);
/* C = A + B */
/* add and store result to destination */
*pDst = inA1 + inB1;
*(pDst + 1) = inA2 + inB2;
*(pDst + 2) = inA3 + inB3;
*(pDst + 3) = inA4 + inB4;
/* update pointers to process next samples */
pSrcA += 4u;
pSrcB += 4u;
pDst += 4u;
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (*pSrcA++) + (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
}
/**
* @} end of BasicAdd group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q15.c
-135
View File
@@ -1,135 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q15.c
*
* Description: Q15 vector addition
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q15 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
*/
void arm_add_q15(
q15_t * pSrcA,
q15_t * pSrcB,
q15_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inB1, inB2;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
inA1 = *__SIMD32(pSrcA)++;
inA2 = *__SIMD32(pSrcA)++;
inB1 = *__SIMD32(pSrcB)++;
inB2 = *__SIMD32(pSrcB)++;
*__SIMD32(pDst)++ = __QADD16(inA1, inB1);
*__SIMD32(pDst)++ = __QADD16(inA2, inB2);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q15_t) __QADD16(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q15_t) __SSAT(((q31_t) * pSrcA++ + *pSrcB++), 16);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q31.c
-143
View File
@@ -1,143 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q31.c
*
* Description: Q31 vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q31 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] will be saturated.
*/
void arm_add_q31(
q31_t * pSrcA,
q31_t * pSrcB,
q31_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
q31_t inA1, inA2, inA3, inA4;
q31_t inB1, inB2, inB3, inB4;
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
inA1 = *pSrcA++;
inA2 = *pSrcA++;
inB1 = *pSrcB++;
inB2 = *pSrcB++;
inA3 = *pSrcA++;
inA4 = *pSrcA++;
inB3 = *pSrcB++;
inB4 = *pSrcB++;
*pDst++ = __QADD(inA1, inB1);
*pDst++ = __QADD(inA2, inB2);
*pDst++ = __QADD(inA3, inB3);
*pDst++ = __QADD(inA4, inB4);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = __QADD(*pSrcA++, *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrcA++ + *pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_q7.c
-129
View File
@@ -1,129 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_add_q7.c
*
* Description: Q7 vector addition.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* -------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @addtogroup BasicAdd
* @{
*/
/**
* @brief Q7 vector addition.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[out] *pDst points to the output vector
* @param[in] blockSize number of samples in each vector
* @return none.
*
* <b>Scaling and Overflow Behavior:</b>
* \par
* The function uses saturating arithmetic.
* Results outside of the allowable Q7 range [0x80 0x7F] will be saturated.
*/
void arm_add_q7(
q7_t * pSrcA,
q7_t * pSrcB,
q7_t * pDst,
uint32_t blockSize)
{
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q7_t) __SSAT(*pSrcA++ + *pSrcB++, 8);
/* Decrement the loop counter */
blkCnt--;
}
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
while(blkCnt > 0u)
{
/* C = A + B */
/* Add and then store the results in the destination buffer. */
*pDst++ = (q7_t) __SSAT((q15_t) * pSrcA++ + *pSrcB++, 8);
/* Decrement the loop counter */
blkCnt--;
}
#endif /* #ifndef ARM_MATH_CM0 */
}
/**
* @} end of BasicAdd group
*/
src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c
-125
View File
@@ -1,125 +0,0 @@
/* ----------------------------------------------------------------------
* Copyright (C) 2010 ARM Limited. All rights reserved.
*
* $Date: 15. February 2012
* $Revision: V1.1.0
*
* Project: CMSIS DSP Library
* Title: arm_dot_prod_f32.c
*
* Description: Floating-point dot product.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* Version 1.1.0 2012/02/15
* Updated with more optimizations, bug fixes and minor API changes.
*
* Version 1.0.10 2011/7/15
* Big Endian support added and Merged M0 and M3/M4 Source code.
*
* Version 1.0.3 2010/11/29
* Re-organized the CMSIS folders and updated documentation.
*
* Version 1.0.2 2010/11/11
* Documentation updated.
*
* Version 1.0.1 2010/10/05
* Production release and review comments incorporated.
*
* Version 1.0.0 2010/09/20
* Production release and review comments incorporated.
*
* Version 0.0.7 2010/06/10
* Misra-C changes done
* ---------------------------------------------------------------------------- */
#include "arm_math.h"
/**
* @ingroup groupMath
*/
/**
* @defgroup dot_prod Vector Dot Product
*
* Computes the dot product of two vectors.
* The vectors are multiplied element-by-element and then summed.
* There are separate functions for floating-point, Q7, Q15, and Q31 data types.
*/
/**
* @addtogroup dot_prod
* @{
*/
/**
* @brief Dot product of floating-point vectors.
* @param[in] *pSrcA points to the first input vector
* @param[in] *pSrcB points to the second input vector
* @param[in] blockSize number of samples in each vector
* @param[out] *result output result returned here
* @return none.
*/
void arm_dot_prod_f32(
float32_t * pSrcA,
float32_t * pSrcB,
uint32_t blockSize,
float32_t * result)
{
float32_t sum = 0.0f; /* Temporary result storage */
uint32_t blkCnt; /* loop counter */
#ifndef ARM_MATH_CM0
/* Run the below code for Cortex-M4 and Cortex-M3 */
/*loop Unrolling */
blkCnt = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer */
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
sum += (*pSrcA++) * (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
** No loop unrolling is used. */
blkCnt = blockSize % 0x4u;
#else
/* Run the below code for Cortex-M0 */
/* Initialize blkCnt with number of samples */
blkCnt = blockSize;
#endif /* #ifndef ARM_MATH_CM0 */
while(blkCnt > 0u)
{
/* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
/* Calculate dot product and then store the result in a temporary buffer. */
sum += (*pSrcA++) * (*pSrcB++);
/* Decrement the loop counter */
blkCnt--;
}
/* Store the result back in the destination buffer */
*result = sum;
}
/**
* @} end of dot_prod group
*/

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