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

Conflicts:
	src/examples/fixedwing_control/main.c

.gitignore

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -177,20 +177,14 @@ GLOBAL_DEPS		+= $(MAKEFILE_LIST)
 EXTRA_CLEANS		 = 
 
 ################################################################################
-# Modules
+# NuttX libraries and paths
 ################################################################################
 
-#
+include $(PX4_MK_DIR)/nuttx.mk
-# 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.
+################################################################################
-#
+# Modules
-# 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).
 
 # 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)
+$(PRODUCT_ELF):		$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS) $(GLOBAL_DEPS) $(LINK_DEPS) $(MODULE_MKFILES)
-	$(call LINK,$@,$(OBJS) $(MODULE_OBJS))
+	$(call LINK,$@,$(OBJS) $(MODULE_OBJS) $(LIBRARY_LIBS))
 
 #
 # Utility rules

makefiles/library.mk

@@ -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

@@ -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)))
+# Object files we will generate from sources
-#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
 #
-##
+OBJS			 = $(addsuffix .o,$(SRCS))
-## Object files we will generate from sources
-##
-#OBJS			:= $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
 
-OBJS		 = $(addsuffix .o,$(SRCS))
+#
-$(info SRCS $(SRCS))
+# Dependency files that will be auto-generated
-$(info OBJS $(OBJS))
+#
+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

@@ -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

@@ -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 \
+INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
-			   -ffixed-r10
+
 # 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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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 const char daemon_name[] = "hott_telemetry";
-static char *commandline_usage = "usage: hott_telemetry start|status|stop [-d <device>]";
+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 recv_req_id(int uart, uint8_t *id);
-static int send_data(int uart, uint8_t *buffer, int size);
+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;
+	static const speed_t speed = B19200;
-	int uart;
 
 	/* open uart */
-	uart = open(device, O_RDWR | O_NOCTTY);
+	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		char msg[80];
+		err(1, "Error opening port: %s", device);
-		sprintf(msg, "Error opening port: %s\n", device);
-		FATAL_MSG(msg);
 	}
-
+	
-	/* Try to set baud rate */
+	/* Back up the original uart configuration to restore it after exit */	
-	struct termios uart_config;
 	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(uart, &mode, sizeof(mode));
-		/* Read the device ID being polled */
-		read(uart, id, 1);
 
 		/* 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");
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
-				ERROR_MSG(commandline_usage);
-				exit(1);
 			}
 		}
 	}
 
 	/* 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;
+	size_t size = 0;
-	int id = 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");
+		errx(1, "missing command\n%s", commandline_usage);
-		ERROR_MSG(commandline_usage);
-		exit(1);
 	}
 
 	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");
+	errx(1, "unrecognized command\n%s", commandline_usage);
-	ERROR_MSG(commandline_usage);
-	exit(1);
 }
-
-
-

src/drivers/hott_telemetry/messages.c

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Simon Wilks <sjwilks@gmail.com>
+ *   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.eam_sensor_id = EAM_SENSOR_ID;
-	msg.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 */
+	msg.stop = STOP_BYTE;
-	struct airspeed_s airspeed;
+	memcpy(buffer, &msg, *size);
-	memset(&airspeed, 0, sizeof(airspeed));
+}
-	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
 
-	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
+void 
-	msg.speed_L = (uint8_t)speed & 0xff;
+build_gps_response(uint8_t *buffer, size_t *size)
-	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+{
+	/* 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

@@ -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			0x8e
-#define EAM_SENSOR_ID		0xe0
+#define EAM_SENSOR_TEXT_ID		0xe0
 
 /* The Electric Air Module message. */
 struct eam_module_msg {
-	uint8_t start;			/**< Start byte   				*/
+	uint8_t start;				/**< Start byte   				*/
-	uint8_t eam_sensor_id;		/**< EAM sensor ID 				*/
+	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 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_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_min;		/**< Flight time in minutes. */
-	uint8_t electric_sec;		/**< Flight time in seconds. 				*/
+	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_L;			/**< Airspeed in km/h in steps of 1 km/h */
 	uint8_t speed_H;
-	uint8_t stop;			/**< Stop byte 						*/
+	uint8_t stop;				/**< Stop byte */
-	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. 			*/
+	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

@@ -76,7 +76,6 @@
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
-#include <systemlib/ppm_decode.h>
 
 #define I2C_BUS_SPEED					400000
 #define UPDATE_RATE						400
@@ -114,6 +113,7 @@ public:
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 	virtual int	init(unsigned motors);
+	virtual ssize_t	write(file *filp, const char *buffer, size_t len);
 
 	int		set_mode(Mode mode);
 	int		set_pwm_rate(unsigned rate);
@@ -177,9 +177,10 @@ private:
 	int		gpio_ioctl(file *filp, int cmd, unsigned long arg);
 	int			mk_servo_arm(bool status);
 
-	int 		mk_servo_set(unsigned int chan, float val);
+	int 		mk_servo_set(unsigned int chan, short val);
-	int 		mk_servo_set_test(unsigned int chan, float val);
+	int 		mk_servo_set_value(unsigned int chan, short val);
 	int 		mk_servo_test(unsigned int chan);
+	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
 
 
 };
@@ -207,20 +208,20 @@ const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };	// Addresstranslat
 
 const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
 
-int addrTranslator[] = {0,0,0,0,0,0,0,0};
+int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
 
-struct MotorData_t
+struct MotorData_t {
-{
 	unsigned int Version;                        // the version of the BL (0 = old)
-  unsigned int SetPoint;                       // written by attitude controller
+	unsigned int SetPoint;                       // written by attitude controller
-  unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
-  unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
-  unsigned int ReadMode;                       // select data to read
+	unsigned int ReadMode;                       // select data to read
-  // the following bytes must be exactly in that order!
+	unsigned short RawPwmValue;							// length of PWM pulse
-  unsigned int Current;                        // in 0.1 A steps, read back from BL
+	// the following bytes must be exactly in that order!
-  unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+	unsigned int Current;                        // in 0.1 A steps, read back from BL
-  unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
-  unsigned int RoundCount;
+	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int RoundCount;
 };
 
 MotorData_t Motor[MAX_MOTORS];
@@ -314,7 +315,7 @@ MK::init(unsigned motors)
 	/* start the IO interface task */
 	_task = task_spawn("mkblctrl",
 			   SCHED_DEFAULT,
-			   SCHED_PRIORITY_MAX -20,
+			   SCHED_PRIORITY_MAX - 20,
 			   2048,
 			   (main_t)&MK::task_main_trampoline,
 			   nullptr);
@@ -346,27 +347,11 @@ MK::set_mode(Mode mode)
 	 */
 	switch (mode) {
 	case MODE_2PWM:
-		if(_num_outputs == 4) {
-			//debug("MODE_QUAD");
-		} else if(_num_outputs == 6) {
-			//debug("MODE_HEXA");
-		} else if(_num_outputs == 8) {
-			//debug("MODE_OCTO");
-		}
-		//up_pwm_servo_init(0x3);
 		up_pwm_servo_deinit();
 		_update_rate = UPDATE_RATE;	/* default output rate */
 		break;
 
 	case MODE_4PWM:
-		if(_num_outputs == 4) {
-			//debug("MODE_QUADRO");
-		} else if(_num_outputs == 6) {
-			//debug("MODE_HEXA");
-		} else if(_num_outputs == 8) {
-			//debug("MODE_OCTO");
-		}
-		//up_pwm_servo_init(0xf);
 		up_pwm_servo_deinit();
 		_update_rate = UPDATE_RATE;	/* default output rate */
 		break;
@@ -412,45 +397,55 @@ MK::set_frametype(int frametype)
 int
 MK::set_motor_count(unsigned count)
 {
-	if(count > 0) {
+	if (count > 0) {
 
 		_num_outputs = count;
 
-		if(_px4mode == MAPPING_MK) {
+		if (_px4mode == MAPPING_MK) {
-			if(_frametype == FRAME_PLUS) {
+			if (_frametype == FRAME_PLUS) {
 				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
-			} else if(_frametype == FRAME_X) {
+
+			} else if (_frametype == FRAME_X) {
 				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
 			}
-			if(_num_outputs == 4) {
+
-				if(_frametype == FRAME_PLUS) {
+			if (_num_outputs == 4) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x));
 				}
-			} else if(_num_outputs == 6) {
+
-				if(_frametype == FRAME_PLUS) {
+			} else if (_num_outputs == 6) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x));
 				}
-			} else if(_num_outputs == 8) {
+
-				if(_frametype == FRAME_PLUS) {
+			} else if (_num_outputs == 8) {
+				if (_frametype == FRAME_PLUS) {
 					memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus));
-				} else if(_frametype == FRAME_X) {
+
+				} else if (_frametype == FRAME_X) {
 					memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x));
 				}
 			}
+
 		} else {
 			fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
 			memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
 		}
 
-		if(_num_outputs == 4) {
+		if (_num_outputs == 4) {
 			fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
-		} else 	if(_num_outputs == 6) {
+
+		} else 	if (_num_outputs == 6) {
 			fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
-		} else 	if(_num_outputs == 8) {
+
+		} else 	if (_num_outputs == 8) {
 			fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
 		}
 
@@ -469,16 +464,35 @@ MK::set_motor_test(bool motortest)
 	return OK;
 }
 
+short
+MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
+{
+	short retVal = 0;
+
+	retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin;
+
+	if (retVal < outMin) {
+		retVal = outMin;
+
+	} else if (retVal > outMax) {
+		retVal = outMax;
+	}
+
+	return retVal;
+}
 
 void
 MK::task_main()
 {
+	long update_rate_in_us = 0;
+	float tmpVal = 0;
+
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
 	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-				     ORB_ID(actuator_controls_1));
+
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
@@ -492,6 +506,8 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
+
+
 	/* advertise the effective control inputs */
 	actuator_controls_effective_s controls_effective;
 	memset(&controls_effective, 0, sizeof(controls_effective));
@@ -499,21 +515,14 @@ MK::task_main()
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
 				   &controls_effective);
 
+
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_armed;
 	fds[1].events = POLLIN;
 
-	// rc input, published to ORB
-	struct rc_input_values rc_in;
-	orb_advert_t to_input_rc = 0;
-
-	memset(&rc_in, 0, sizeof(rc_in));
-	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
-	
 	log("starting");
-	long update_rate_in_us = 0;
 
 	/* loop until killed */
 	while (!_task_should_exit) {
@@ -528,6 +537,7 @@ MK::task_main()
 				update_rate_in_ms = 2;
 				_update_rate = 500;
 			}
+
 			/* reject slower than 50 Hz updates */
 			if (update_rate_in_ms > 20) {
 				update_rate_in_ms = 20;
@@ -539,8 +549,8 @@ MK::task_main()
 			_current_update_rate = _update_rate;
 		}
 
-		/* sleep waiting for data, but no more than a second */
+		/* sleep waiting for data max 100ms */
-		int ret = ::poll(&fds[0], 2, 1000);
+		int ret = ::poll(&fds[0], 2, 100);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -553,7 +563,7 @@ MK::task_main()
 		if (fds[0].revents & POLLIN) {
 
 			/* get controls - must always do this to avoid spinning */
-			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
 
 			/* can we mix? */
 			if (_mixers != nullptr) {
@@ -565,53 +575,52 @@ MK::task_main()
 				// XXX output actual limited values
 				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
 
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
-
 				/* iterate actuators */
 				for (unsigned int i = 0; i < _num_outputs; i++) {
 
 					/* last resort: catch NaN, INF and out-of-band errors */
 					if (i < outputs.noutputs &&
-						isfinite(outputs.output[i]) &&
+					    isfinite(outputs.output[i]) &&
-						outputs.output[i] >= -1.0f &&
+					    outputs.output[i] >= -1.0f &&
-						outputs.output[i] <= 1.0f) {
+					    outputs.output[i] <= 1.0f) {
 						/* scale for PWM output 900 - 2100us */
-						//outputs.output[i] = 1500 + (600 * outputs.output[i]);
+						/* nothing to do here */
-						//outputs.output[i] = 127 + (127 * outputs.output[i]);
 					} else {
 						/*
 						 * Value is NaN, INF or out of band - set to the minimum value.
 						 * This will be clearly visible on the servo status and will limit the risk of accidentally
 						 * spinning motors. It would be deadly in flight.
 						 */
-						if(outputs.output[i] < -1.0f) {
+						if (outputs.output[i] < -1.0f) {
 							outputs.output[i] = -1.0f;
-						} else if(outputs.output[i] > 1.0f) {
+
+						} else if (outputs.output[i] > 1.0f) {
 							outputs.output[i] = 1.0f;
+
 						} else {
 							outputs.output[i] = -1.0f;
 						}
 					}
 
 					/* don't go under BLCTRL_MIN_VALUE */
-					if(outputs.output[i] < BLCTRL_MIN_VALUE) {
+					if (outputs.output[i] < BLCTRL_MIN_VALUE) {
 						outputs.output[i] = BLCTRL_MIN_VALUE;
 					}
-					//_motortest = true;
-					/* output to BLCtrl's */
-					if(_motortest == true) {
-						mk_servo_test(i);
-					} else {
-						//mk_servo_set(i, outputs.output[i]);
-						mk_servo_set_test(i, outputs.output[i]);	// 8Bit
-					}
 
+					/* output to BLCtrl's */
+					if (_motortest == true) {
+						mk_servo_test(i);
+
+					} else {
+						mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+					}
 
 				}
 
-				/* and publish for anyone that cares to see */
-				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
 			}
+
+
+
 		}
 
 		/* how about an arming update? */
@@ -622,29 +631,9 @@ MK::task_main()
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
 			/* update PWM servo armed status if armed and not locked down */
-			////up_pwm_servo_arm(aa.armed && !aa.lockdown);
 			mk_servo_arm(aa.armed && !aa.lockdown);
 		}
 
-		// see if we have new PPM input data
-		if (ppm_last_valid_decode != rc_in.timestamp) {
-			// we have a new PPM frame. Publish it.
-			rc_in.channel_count = ppm_decoded_channels;
-			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
-				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
-			}
-			for (uint8_t i=0; i<rc_in.channel_count; i++) {
-				rc_in.values[i] = ppm_buffer[i];
-			}
-			rc_in.timestamp = ppm_last_valid_decode;
-
-			/* lazily advertise on first publication */
-			if (to_input_rc == 0) {
-				to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
-			} else {
-				orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
-			}
-		}
 
 	}
 
@@ -666,7 +655,7 @@ MK::task_main()
 }
 
 
-int 
+int
 MK::mk_servo_arm(bool status)
 {
 	_armed = status;
@@ -680,12 +669,13 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 	_retries = 50;
 	uint8_t foundMotorCount = 0;
 
-	for(unsigned i=0; i<MAX_MOTORS; i++) {
+	for (unsigned i = 0; i < MAX_MOTORS; i++) {
 		Motor[i].Version = 0;
 		Motor[i].SetPoint = 0;
 		Motor[i].SetPointLowerBits = 0;
 		Motor[i].State = 0;
 		Motor[i].ReadMode = 0;
+		Motor[i].RawPwmValue = 0;
 		Motor[i].Current = 0;
 		Motor[i].MaxPWM = 0;
 		Motor[i].Temperature = 0;
@@ -695,34 +685,37 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 	uint8_t msg = 0;
 	uint8_t result[3];
 
-	for(unsigned i=0; i< count; i++) {
+	for (unsigned i = 0; i < count; i++) {
 		result[0] = 0;
 		result[1] = 0;
 		result[2] = 0;
-			
+
-		set_address( BLCTRL_BASE_ADDR + i );
+		set_address(BLCTRL_BASE_ADDR + i);
-		
+
 		if (OK == transfer(&msg, 1, &result[0], 3)) {
 			Motor[i].Current = result[0];
 			Motor[i].MaxPWM = result[1];
 			Motor[i].Temperature = result[2];
 			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
 			foundMotorCount++;
-			if(Motor[i].MaxPWM == 250) {
+
+			if (Motor[i].MaxPWM == 250) {
 				Motor[i].Version = BLCTRL_NEW;
+
 			} else {
 				Motor[i].Version = BLCTRL_OLD;
 			}
 		}
 	}
 
-	if(showOutput) {
+	if (showOutput) {
-		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n",foundMotorCount);
+		fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount);
-		for(unsigned i=0; i< foundMotorCount; i++) {
+
-			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i,Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
+		for (unsigned i = 0; i < foundMotorCount; i++) {
+			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		if(foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+		if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
 			_task_should_exit = true;
 		}
 	}
@@ -734,122 +727,136 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 
 
 int
-MK::mk_servo_set(unsigned int chan, float val)
+MK::mk_servo_set(unsigned int chan, short val)
 {
-	float tmpVal = 0;
+	short tmpVal = 0;
 	_retries = 0;
-	uint8_t result[3] = { 0,0,0 };
+	uint8_t result[3] = { 0, 0, 0 };
-	uint8_t msg[2] = { 0,0 };
+	uint8_t msg[2] = { 0, 0 };
-	uint8_t rod=0;
+	uint8_t rod = 0;
 	uint8_t bytesToSendBL2 = 2;
 
+	tmpVal = val;
 
-	tmpVal = (1023 + (1023 * val));
+	if (tmpVal > 1024) {
-	if(tmpVal > 2047) {
+		tmpVal = 1024;
-		tmpVal = 2047;
+
+	} else if (tmpVal < 0) {
+		tmpVal = 0;
 	}
 
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
+	//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
 
-	Motor[chan].SetPoint = (uint8_t) tmpVal / 3;	// divide 8
-	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 8;	// rest of divide 8
-	//rod = (uint8_t) tmpVal % 8;
-	//Motor[chan].SetPointLowerBits = rod<<1;	// rest of divide 8
 	Motor[chan].SetPointLowerBits = 0;
 
-	if(_armed == false) {
+	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
 
 	//if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) {
-		set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
+
+	if (Motor[chan].Version == BLCTRL_OLD) {
+		/*
+		*	Old BL-Ctrl 8Bit served. Version < 2.0
+		*/
+		msg[0] = Motor[chan].SetPoint;
+
+		if (Motor[chan].RoundCount >= 16) {
+			// on each 16th cyle we read out the status messages from the blctrl
+			if (OK == transfer(&msg[0], 1, &result[0], 2)) {
+				Motor[chan].Current = result[0];
+				Motor[chan].MaxPWM = result[1];
+				Motor[chan].Temperature = 255;;
 
-		if(Motor[chan].Version == BLCTRL_OLD) {
-			/*
-			*	Old BL-Ctrl 8Bit served. Version < 2.0
-			*/
-			msg[0] = Motor[chan].SetPoint;
-			if(Motor[chan].RoundCount >= 16) {
-				// on each 16th cyle we read out the status messages from the blctrl
-				if (OK == transfer(&msg[0], 1, &result[0], 2)) {
-					Motor[chan].Current = result[0];
-					Motor[chan].MaxPWM = result[1];
-					Motor[chan].Temperature = 255;;
-				} else {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
-				Motor[chan].RoundCount = 0;
 			} else {
-				if (OK != transfer(&msg[0], 1, nullptr, 0)) {
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
 			}
 
+			Motor[chan].RoundCount = 0;
+
 		} else {
-			/*
+			if (OK != transfer(&msg[0], 1, nullptr, 0)) {
-			*	New BL-Ctrl 11Bit served. Version >= 2.0
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-			*/
-			msg[0] = Motor[chan].SetPoint;
-			msg[1] = Motor[chan].SetPointLowerBits;
-
-			if(Motor[chan].SetPointLowerBits == 0) {
-				bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
 			}
-
-			if(Motor[chan].RoundCount >= 16) {
-				// on each 16th cyle we read out the status messages from the blctrl
-				if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
-					Motor[chan].Current = result[0];
-					Motor[chan].MaxPWM = result[1];
-					Motor[chan].Temperature = result[2];
-				} else {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
-				Motor[chan].RoundCount = 0;
-			} else {
-				if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
-					if((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
-				}
-			}
-
 		}
 
-		Motor[chan].RoundCount++;
+	} else {
+		/*
+		*	New BL-Ctrl 11Bit served. Version >= 2.0
+		*/
+		msg[0] = Motor[chan].SetPoint;
+		msg[1] = Motor[chan].SetPointLowerBits;
+
+		if (Motor[chan].SetPointLowerBits == 0) {
+			bytesToSendBL2 = 1;	// if setpoint lower bits are zero, we send only the higher bits - this saves time
+		}
+
+		if (Motor[chan].RoundCount >= 16) {
+			// on each 16th cyle we read out the status messages from the blctrl
+			if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) {
+				Motor[chan].Current = result[0];
+				Motor[chan].MaxPWM = result[1];
+				Motor[chan].Temperature = result[2];
+
+			} else {
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+			}
+
+			Motor[chan].RoundCount = 0;
+
+		} else {
+			if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) {
+				if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++;	// error
+			}
+		}
+
+	}
+
+	Motor[chan].RoundCount++;
 	//}
 
-	if(showDebug == true) {
+	if (showDebug == true) {
 		debugCounter++;
-		if(debugCounter == 2000) {
+
+		if (debugCounter == 2000) {
 			debugCounter = 0;
-			for(int i=0; i<_num_outputs; i++){
+
-				if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
+			for (int i = 0; i < _num_outputs; i++) {
+				if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
 					fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
 				}
 			}
+
 			fprintf(stderr, "\n");
 		}
 	}
+
 	return 0;
 }
 
 int
-MK::mk_servo_set_test(unsigned int chan, float val)
+MK::mk_servo_set_value(unsigned int chan, short val)
 {
 	_retries = 0;
 	int ret;
+	short tmpVal = 0;
+	uint8_t msg[2] = { 0, 0 };
 
-	float tmpVal = 0;
+	tmpVal = val;
 
-	uint8_t msg[2] = { 0,0 };
+	if (tmpVal > 1024) {
+		tmpVal = 1024;
 
-	tmpVal = (1023 + (1023 * val));
+	} else if (tmpVal < 0) {
-	if(tmpVal > 2048) {
+		tmpVal = 0;
-		tmpVal = 2048;
 	}
 
-	Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
 
-	if(_armed == false) {
+	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
@@ -860,7 +867,6 @@ MK::mk_servo_set_test(unsigned int chan, float val)
 	ret = transfer(&msg[0], 1, nullptr, 0);
 
 	ret = OK;
-
 	return ret;
 }
 
@@ -868,59 +874,61 @@ MK::mk_servo_set_test(unsigned int chan, float val)
 int
 MK::mk_servo_test(unsigned int chan)
 {
-	int ret=0;
+	int ret = 0;
 	float tmpVal = 0;
 	float val = -1;
 	_retries = 0;
-	uint8_t msg[2] = { 0,0 };
+	uint8_t msg[2] = { 0, 0 };
 
-	if(debugCounter >= MOTOR_SPINUP_COUNTER) {
+	if (debugCounter >= MOTOR_SPINUP_COUNTER) {
 		debugCounter = 0;
 		_motor++;
 
-		if(_motor < _num_outputs) {
+		if (_motor < _num_outputs) {
 			fprintf(stderr, "[mkblctrl] Motortest - #%i:\tspinup\n", _motor);
 		}
 
-		if(_motor >= _num_outputs) {
+		if (_motor >= _num_outputs) {
 			_motor = -1;
 			_motortest = false;
 		}
-
 	}
+
 	debugCounter++;
 
-	if(_motor == chan) {
+	if (_motor == chan) {
 		val = BLCTRL_MIN_VALUE;
+
 	} else {
 		val = -1;
 	}
 
-	tmpVal = (1023 + (1023 * val));
+	tmpVal = (511 + (511 * val));
-	if(tmpVal > 2048) {
+
-		tmpVal = 2048;
+	if (tmpVal > 1024) {
+		tmpVal = 1024;
 	}
 
-	//Motor[chan].SetPoint = (uint8_t) (tmpVal / 8);
+	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
-	//Motor[chan].SetPointLowerBits = (uint8_t) (tmpVal % 8) & 0x07;
-	Motor[chan].SetPoint = (uint8_t) tmpVal>>3;
-	Motor[chan].SetPointLowerBits = (uint8_t) tmpVal & 0x07;
 
-	if(_motor != chan) {
+	if (_motor != chan) {
 		Motor[chan].SetPoint = 0;
 		Motor[chan].SetPointLowerBits = 0;
 	}
 
-	if(Motor[chan].Version == BLCTRL_OLD) {
+	if (Motor[chan].Version == BLCTRL_OLD) {
 		msg[0] = Motor[chan].SetPoint;
+
 	} else {
 		msg[0] = Motor[chan].SetPoint;
 		msg[1] = Motor[chan].SetPointLowerBits;
 	}
 
 	set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan]));
-	if(Motor[chan].Version == BLCTRL_OLD) {
+
+	if (Motor[chan].Version == BLCTRL_OLD) {
 		ret = transfer(&msg[0], 1, nullptr, 0);
+
 	} else {
 		ret = transfer(&msg[0], 2, nullptr, 0);
 	}
@@ -931,9 +939,9 @@ MK::mk_servo_test(unsigned int chan)
 
 int
 MK::control_callback(uintptr_t handle,
-			 uint8_t control_group,
+		     uint8_t control_group,
-			 uint8_t control_index,
+		     uint8_t control_index,
-			 float &input)
+		     float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -947,7 +955,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
 	int ret;
 
 	// XXX disabled, confusing users
-	//debug("ioctl 0x%04x 0x%08x", cmd, arg);
 
 	/* try it as a GPIO ioctl first */
 	ret = gpio_ioctl(filp, cmd, arg);
@@ -978,32 +985,37 @@ int
 MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
 
 	lock();
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
-		////up_pwm_servo_arm(true);
 		mk_servo_arm(true);
 		break;
 
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+		// these are no-ops, as no safety switch
+		break;
+
 	case PWM_SERVO_DISARM:
-		////up_pwm_servo_arm(false);
 		mk_servo_arm(false);
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:
-		set_pwm_rate(arg);
+		ret = OK;
+		break;
+
+	case PWM_SERVO_SELECT_UPDATE_RATE:
+		ret = OK;
 		break;
 
 
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+		if (arg < 2150) {
+			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
+			mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
 
-		/* fake an update to the selected 'servo' channel */
-		if ((arg >= 0) && (arg <= 255)) {
-			channel = cmd - PWM_SERVO_SET(0);
-			//mk_servo_set(channel, arg);
 		} else {
 			ret = -EINVAL;
 		}
@@ -1012,20 +1024,20 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
 		/* copy the current output value from the channel */
-		*(servo_position_t *)arg = cmd - PWM_SERVO_GET(0);
+		*(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue;
+
 		break;
 
+	case PWM_SERVO_GET_RATEGROUP(0):
+	case PWM_SERVO_GET_RATEGROUP(1):
+	case PWM_SERVO_GET_RATEGROUP(2):
+	case PWM_SERVO_GET_RATEGROUP(3):
+		//*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
+		break;
+
+	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
-		/*
-		if (_mode == MODE_4PWM) {
-			*(unsigned *)arg = 4;
-		} else {
-			*(unsigned *)arg = 2;
-		}
-		 */
-
 		*(unsigned *)arg = _num_outputs;
-
 		break;
 
 	case MIXERIOCRESET:
@@ -1078,6 +1090,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
+
 			break;
 		}
 
@@ -1091,6 +1104,38 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	return ret;
 }
 
+/*
+  this implements PWM output via a write() method, for compatibility
+  with px4io
+ */
+ssize_t
+MK::write(file *filp, const char *buffer, size_t len)
+{
+	unsigned count = len / 2;
+	// loeschen uint16_t values[4];
+	uint16_t values[8];
+
+	// loeschen if (count > 4) {
+	// loeschen 	// we only have 4 PWM outputs on the FMU
+	// loeschen	count = 4;
+	// loeschen }
+	if (count > _num_outputs) {
+		// we only have 8 I2C outputs in the driver
+		count = _num_outputs;
+	}
+
+
+	// allow for misaligned values
+	memcpy(values, buffer, count * 2);
+
+	for (uint8_t i = 0; i < count; i++) {
+		Motor[i].RawPwmValue = (unsigned short)values[i];
+		mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
+	}
+
+	return count * 2;
+}
+
 void
 MK::gpio_reset(void)
 {
@@ -1229,10 +1274,10 @@ enum MappingMode {
 	MAPPING_PX4,
 };
 
-	enum FrameType {
+enum FrameType {
-		FRAME_PLUS = 0,
+	FRAME_PLUS = 0,
-		FRAME_X,
+	FRAME_X,
-	};
+};
 
 PortMode g_port_mode;
 
@@ -1297,18 +1342,17 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	g_mk->set_motor_test(motortest);
 
 
-	/* (re)set count of used motors */
-	////g_mk->set_motor_count(motorcount);
 	/* count used motors */
-
 	do {
-		if(g_mk->mk_check_for_blctrl(8, false) != 0) {
+		if (g_mk->mk_check_for_blctrl(8, false) != 0) {
 			shouldStop = 4;
+
 		} else {
 			shouldStop++;
 		}
+
 		sleep(1);
-	} while ( shouldStop < 3);
+	} while (shouldStop < 3);
 
 	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
 
@@ -1375,7 +1419,8 @@ mkblctrl_main(int argc, char *argv[])
 			if (argc > i + 1) {
 				bus = atoi(argv[i + 1]);
 				newMode = true;
-				} else {
+
+			} else {
 				errx(1, "missing argument for i2c bus (-b)");
 				return 1;
 			}
@@ -1384,17 +1429,21 @@ mkblctrl_main(int argc, char *argv[])
 		/* look for the optional frame parameter */
 		if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
 			if (argc > i + 1) {
-				if(strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
+				if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
 					px4mode = MAPPING_MK;
 					newMode = true;
-					if(strcmp(argv[i + 1], "+") == 0) {
+
+					if (strcmp(argv[i + 1], "+") == 0) {
 						frametype = FRAME_PLUS;
+
 					} else {
 						frametype = FRAME_X;
 					}
+
 				} else {
 					errx(1, "only + or x for frametype supported !");
 				}
+
 			} else {
 				errx(1, "missing argument for mkmode (-mkmode)");
 				return 1;
@@ -1409,12 +1458,12 @@ mkblctrl_main(int argc, char *argv[])
 
 		/* look for the optional -h --help parameter */
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
-			showHelp == true;
+			showHelp = true;
 		}
 
 	}
 
-	if(showHelp) {
+	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
 		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
 		exit(1);
@@ -1424,6 +1473,7 @@ mkblctrl_main(int argc, char *argv[])
 	if (g_mk == nullptr) {
 		if (mk_start(bus, motorcount) != OK) {
 			errx(1, "failed to start the MK-BLCtrl driver");
+
 		} else {
 			newMode = true;
 		}

src/drivers/px4io/px4io.cpp

@@ -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),
+	_battery_last_timestamp(0)
-	_primary_pwm_device(false)
 {
 	/* 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

@@ -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

@@ -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);
+				if (manual_sp_updated)
-				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+					/* get the RC (or otherwise user based) input */ 
+					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
 
-				gyro[0] = att.rollspeed;
+				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
-				gyro[1] = att.pitchspeed;
+				if (isfinite(manual_sp.throttle) &&
-				gyro[2] = att.yawspeed;
+							    (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

@@ -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

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
  *
  * 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
+ * @file px4_daemon_app.c
- * Deamon application example for PX4 autopilot
+ * 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 */
+#include <systemlib/systemlib.h>
-static bool thread_running = false;		/**< Deamon status flag */
+#include <systemlib/err.h>
-static int deamon_task;				/**< Handle of deamon task / thread */
+
+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);
+		warnx("%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
 }
 
 /**
- * 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

@@ -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, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__);
-#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
 
 /**
  * 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, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__);
-#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
 
 /**
  * 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, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
-#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
 
 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

@@ -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

@@ -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

@@ -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

@@ -0,0 +1,833 @@
+/*
+ * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
+ *
+ * @file attitude_estimator_so3_comp_main.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 <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "attitude_estimator_so3_comp_params.h"
+#ifdef __cplusplus
+}
+#endif
+
+extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
+static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
+static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
+static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
+static bool bFilterInit = false;
+
+//! Auxiliary variables to reduce number of repeated operations
+static float q0q0, q0q1, q0q2, q0q3;
+static float q1q1, q1q2, q1q3;
+static float q2q2, q2q3;
+static float q3q3;
+
+//! Serial packet related
+static int uart;
+static int baudrate;
+
+/**
+ * Mainloop of attitude_estimator_so3_comp.
+ */
+int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
+		"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
+		"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
+	exit(1);
+}
+
+/**
+ * The attitude_estimator_so3_comp 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 attitude_estimator_so3_comp_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("attitude_estimator_so3_comp already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp",
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_MAX - 5,
+					      12400,
+					      attitude_estimator_so3_comp_thread_main,
+					      (const char **)argv);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+
+		while(thread_running){
+			usleep(200000);
+			printf(".");
+		}
+		printf("terminated.");
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tattitude_estimator_so3_comp app is running\n");
+
+		} else {
+			printf("\tattitude_estimator_so3_comp app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+float invSqrt(float number) {
+    volatile long i;
+    volatile float x, y;
+    volatile const float f = 1.5F;
+
+    x = number * 0.5F;
+    y = number;
+    i = * (( long * ) &y);
+    i = 0x5f375a86 - ( i >> 1 );
+    y = * (( float * ) &i);
+    y = y * ( f - ( x * y * y ) );
+    return y;
+}
+
+//! Using accelerometer, sense the gravity vector.
+//! Using magnetometer, sense yaw.
+void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    // auxillary variables to reduce number of repeated operations, for 1st pass
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+}
+
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
+	float recipNorm;
+	float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
+
+	//! Make filter converge to initial solution faster
+	//! This function assumes you are in static position.
+	//! WARNING : in case air reboot, this can cause problem. But this is very
+	//!	      unlikely happen.
+	if(bFilterInit == false)
+	{
+		NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
+		bFilterInit = true;
+	}
+        	
+	//! If magnetometer measurement is available, use it.
+	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+		float hx, hy, hz, bx, bz;
+		float halfwx, halfwy, halfwz;
+	
+		// Normalise magnetometer measurement
+		// Will sqrt work better? PX4 system is powerful enough?
+    		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    		mx *= recipNorm;
+    		my *= recipNorm;
+    		mz *= recipNorm;
+    
+    		// Reference direction of Earth's magnetic field
+    		hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    		hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+		hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
+    		bx = sqrt(hx * hx + hy * hy);
+    		bz = hz;
+    
+    		// Estimated direction of magnetic field
+    		halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    		halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    
+    		// Error is sum of cross product between estimated direction and measured direction of field vectors
+    		halfex += (my * halfwz - mz * halfwy);
+    		halfey += (mz * halfwx - mx * halfwz);
+    		halfez += (mx * halfwy - my * halfwx);
+	}
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+		float halfvx, halfvy, halfvz;
+	
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		// Estimated direction of gravity and magnetic field
+		halfvx = q1q3 - q0q2;
+		halfvy = q0q1 + q2q3;
+		halfvz = q0q0 - 0.5f + q3q3;
+	
+		// Error is sum of cross product between estimated direction and measured direction of field vectors
+		halfex += ay * halfvz - az * halfvy;
+		halfey += az * halfvx - ax * halfvz;
+		halfez += ax * halfvy - ay * halfvx;
+	}
+
+	// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+	if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+		// Compute and apply integral feedback if enabled
+		if(twoKi > 0.0f) {
+			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki
+			gyro_bias[1] += twoKi * halfey * dt;
+			gyro_bias[2] += twoKi * halfez * dt;
+			gx += gyro_bias[0];	// apply integral feedback
+			gy += gyro_bias[1];
+			gz += gyro_bias[2];
+		}
+		else {
+			gyro_bias[0] = 0.0f;	// prevent integral windup
+			gyro_bias[1] = 0.0f;
+			gyro_bias[2] = 0.0f;
+		}
+
+		// Apply proportional feedback
+		gx += twoKp * halfex;
+		gy += twoKp * halfey;
+		gz += twoKp * halfez;
+	}
+	
+	//! Integrate rate of change of quaternion
+#if 0
+	gx *= (0.5f * dt);		// pre-multiply common factors
+	gy *= (0.5f * dt);
+	gz *= (0.5f * dt);
+#endif 
+
+	// Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
+	//! q_k = q_{k-1} + dt*\dot{q}
+	//! \dot{q} = 0.5*q \otimes P(\omega)
+	dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
+	dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
+	dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
+	dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); 
+
+	q0 += dt*dq0;
+	q1 += dt*dq1;
+	q2 += dt*dq2;
+	q3 += dt*dq3;
+	
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+
+	// Auxiliary variables to avoid repeated arithmetic
+        q0q0 = q0 * q0;
+       	q0q1 = q0 * q1;
+       	q0q2 = q0 * q2;
+       	q0q3 = q0 * q3;
+       	q1q1 = q1 * q1;
+       	q1q2 = q1 * q2;
+   	q1q3 = q1 * q3;
+       	q2q2 = q2 * q2;
+       	q2q3 = q2 * q3;
+       	q3q3 = q3 * q3;   
+}
+
+void send_uart_byte(char c)
+{
+	write(uart,&c,1);
+}
+
+void send_uart_bytes(uint8_t *data, int length)
+{
+	write(uart,data,(size_t)(sizeof(uint8_t)*length));
+}
+
+void send_uart_float(float f) {
+  uint8_t * b = (uint8_t *) &f;
+
+  //! Assume float is 4-bytes
+  for(int i=0; i<4; i++) {
+    
+    uint8_t b1 = (b[i] >> 4) & 0x0f;
+    uint8_t b2 = (b[i] & 0x0f);
+    
+    uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
+    uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
+    
+    send_uart_bytes(&c1,1);
+    send_uart_bytes(&c2,1);
+  }
+}
+
+void send_uart_float_arr(float *arr, int length)
+{
+	for(int i=0;i<length;++i)
+	{
+		send_uart_float(arr[i]);
+		send_uart_byte(',');
+	}
+}
+
+int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
+{
+	int speed;
+	
+	switch (baud) {
+        case 0:      speed = B0;      break;
+        case 50:     speed = B50;     break;
+        case 75:     speed = B75;     break;
+        case 110:    speed = B110;    break;
+        case 134:    speed = B134;    break;
+        case 150:    speed = B150;    break;
+        case 200:    speed = B200;    break;
+        case 300:    speed = B300;    break;
+        case 600:    speed = B600;    break;
+        case 1200:   speed = B1200;   break;
+        case 1800:   speed = B1800;   break;
+        case 2400:   speed = B2400;   break;
+        case 4800:   speed = B4800;   break;
+        case 9600:   speed = B9600;   break;
+        case 19200:  speed = B19200;  break;
+        case 38400:  speed = B38400;  break;
+        case 57600:  speed = B57600;  break;
+        case 115200: speed = B115200; break;
+        case 230400: speed = B230400; break;
+        case 460800: speed = B460800; break;
+        case 921600: speed = B921600; break;
+        default:
+                printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
+                return -EINVAL;
+        }
+
+	printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
+        uart = open(uart_name, O_RDWR | O_NOCTTY);
+
+	/* Try to set baud rate */
+        struct termios uart_config;
+        int termios_state;
+        *is_usb = false;
+
+	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
+        if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
+                /* Back up the original uart configuration to restore it after exit */
+                if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+                        printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+                        close(uart);
+                        return -1;
+                }
+
+                /* Fill the struct for the new configuration */
+                tcgetattr(uart, &uart_config);
+
+                /* Clear ONLCR flag (which appends a CR for every LF) */
+                uart_config.c_oflag &= ~ONLCR;
+
+                /* Set baud rate */
+                if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+                        printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+                        close(uart);
+                        return -1;
+                }
+
+
+                if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+                        printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+                        close(uart);
+                        return -1;
+                }
+
+        } else {
+                *is_usb = true;
+        }
+
+        return uart;
+}
+
+/*
+ * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+
+/*
+ * EKF Attitude Estimator main function.
+ *
+ * Estimates the attitude recursively once started.
+ *
+ * @param argc number of commandline arguments (plus command name)
+ * @param argv strings containing the arguments
+ */
+int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
+{
+
+const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
+
+	//! Serial debug related
+	int ch;
+	struct termios uart_config_original;
+	bool usb_uart;
+	bool debug_mode = false;
+	char *device_name = "/dev/ttyS2";
+	baudrate = 115200;
+
+	//! Time constant
+	float dt = 0.005f;
+	
+	/* output euler angles */
+	float euler[3] = {0.0f, 0.0f, 0.0f};
+
+	float Rot_matrix[9] = {1.f,  0,  0,
+			      0,  1.f,  0,
+			      0,  0,  1.f
+			     };		/**< init: identity matrix */
+
+	float acc[3] = {0.0f, 0.0f, 0.0f};
+	float gyro[3] = {0.0f, 0.0f, 0.0f};
+	float mag[3] = {0.0f, 0.0f, 0.0f};
+
+	/* work around some stupidity in task_create's argv handling */
+	argc -= 2;
+	argv += 2;
+
+	//! -d <device_name>, default : /dev/ttyS2
+	//! -b <baud_rate>,   default : 115200
+	while ((ch = getopt(argc,argv,"d:b:")) != EOF){
+		switch(ch){
+			case 'b':
+				baudrate = strtoul(optarg, NULL, 10);
+				if(baudrate == 0)
+					printf("invalid baud rate '%s'",optarg);
+				break;
+			case 'd':
+				device_name = optarg;
+				debug_mode = true;
+				break;
+			default:
+				usage("invalid argument");
+		}
+	}
+
+	if(debug_mode){
+		printf("Opening debugging port for 3D visualization\n");
+		uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
+		if (uart < 0)
+                	printf("could not open %s", device_name);
+		else
+			printf("Open port success\n");
+	}
+
+	// print text
+	printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
+	fflush(stdout);
+
+	int overloadcounter = 19;
+
+	/* store start time to guard against too slow update rates */
+	uint64_t last_run = hrt_absolute_time();
+
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+
+	//! Initialize attitude vehicle uORB message.
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+
+	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
+
+	uint64_t last_data = 0;
+	uint64_t last_measurement = 0;
+
+	/* subscribe to raw data */
+	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
+	/* rate-limit raw data updates to 200Hz */
+	orb_set_interval(sub_raw, 4);
+
+	/* subscribe to param changes */
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
+
+	/* subscribe to system state*/
+	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* advertise attitude */
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+	int loopcounter = 0;
+	int printcounter = 0;
+
+	thread_running = true;
+
+	/* advertise debug value */
+	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
+	// orb_advert_t pub_dbg = -1;
+
+	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
+	// XXX write this out to perf regs
+
+	/* keep track of sensor updates */
+	uint32_t sensor_last_count[3] = {0, 0, 0};
+	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
+
+	struct attitude_estimator_so3_comp_params so3_comp_params;
+	struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
+
+	/* initialize parameter handles */
+	parameters_init(&so3_comp_param_handles);
+
+	uint64_t start_time = hrt_absolute_time();
+	bool initialized = false;
+
+	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
+	unsigned offset_count = 0;
+
+	/* register the perf counter */
+	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
+
+	/* Main loop*/
+	while (!thread_should_exit) {
+
+		struct pollfd fds[2];
+		fds[0].fd = sub_raw;
+		fds[0].events = POLLIN;
+		fds[1].fd = sub_params;
+		fds[1].events = POLLIN;
+		int ret = poll(fds, 2, 1000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* check if we're in HIL - not getting sensor data is fine then */
+			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+
+			if (!state.flag_hil_enabled) {
+				fprintf(stderr,
+					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
+			}
+
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), sub_params, &update);
+
+				/* update parameters */
+				parameters_update(&so3_comp_param_handles, &so3_comp_params);
+			}
+
+			/* only run filter if sensor values changed */
+			if (fds[0].revents & POLLIN) {
+
+				/* get latest measurements */
+				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
+
+				if (!initialized) {
+
+					gyro_offsets[0] += raw.gyro_rad_s[0];
+					gyro_offsets[1] += raw.gyro_rad_s[1];
+					gyro_offsets[2] += raw.gyro_rad_s[2];
+					offset_count++;
+
+					if (hrt_absolute_time() - start_time > 3000000LL) {
+						initialized = true;
+						gyro_offsets[0] /= offset_count;
+						gyro_offsets[1] /= offset_count;
+						gyro_offsets[2] /= offset_count;
+					}
+
+				} else {
+
+					perf_begin(so3_comp_loop_perf);
+
+					/* Calculate data time difference in seconds */
+					dt = (raw.timestamp - last_measurement) / 1000000.0f;
+					last_measurement = raw.timestamp;
+					uint8_t update_vect[3] = {0, 0, 0};
+
+					/* Fill in gyro measurements */
+					if (sensor_last_count[0] != raw.gyro_counter) {
+						update_vect[0] = 1;
+						sensor_last_count[0] = raw.gyro_counter;
+						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						sensor_last_timestamp[0] = raw.timestamp;
+					}
+
+					gyro[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
+					gyro[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
+					gyro[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
+
+					/* update accelerometer measurements */
+					if (sensor_last_count[1] != raw.accelerometer_counter) {
+						update_vect[1] = 1;
+						sensor_last_count[1] = raw.accelerometer_counter;
+						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						sensor_last_timestamp[1] = raw.timestamp;
+					}
+
+					acc[0] = raw.accelerometer_m_s2[0];
+					acc[1] = raw.accelerometer_m_s2[1];
+					acc[2] = raw.accelerometer_m_s2[2];
+
+					/* update magnetometer measurements */
+					if (sensor_last_count[2] != raw.magnetometer_counter) {
+						update_vect[2] = 1;
+						sensor_last_count[2] = raw.magnetometer_counter;
+						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						sensor_last_timestamp[2] = raw.timestamp;
+					}
+
+					mag[0] = raw.magnetometer_ga[0];
+					mag[1] = raw.magnetometer_ga[1];
+					mag[2] = raw.magnetometer_ga[2];
+
+					uint64_t now = hrt_absolute_time();
+					unsigned int time_elapsed = now - last_run;
+					last_run = now;
+
+					if (time_elapsed > loop_interval_alarm) {
+						//TODO: add warning, cpu overload here
+						// if (overloadcounter == 20) {
+						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+						// 	overloadcounter = 0;
+						// }
+
+						overloadcounter++;
+					}
+
+					static bool const_initialized = false;
+
+					/* initialize with good values once we have a reasonable dt estimate */
+					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
+						dt = 0.005f;
+						parameters_update(&so3_comp_param_handles, &so3_comp_params);
+						const_initialized = true;
+					}
+
+					/* do not execute the filter if not initialized */
+					if (!const_initialized) {
+						continue;
+					}
+
+					uint64_t timing_start = hrt_absolute_time();
+
+					// NOTE : Accelerometer is reversed.
+					// Because proper mount of PX4 will give you a reversed accelerometer readings.
+					NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
+
+					// Convert q->R.
+					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
+        				Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3);	// 12
+        				Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2);	// 13
+        				Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3);	// 21
+        				Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
+        				Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1);	// 23
+        				Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2);	// 31
+        				Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1);	// 32
+        				Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
+
+					//1-2-3 Representation.
+					//Equation (290) 
+					//Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
+					// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
+					euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
+					euler[1] = -asinf(Rot_matrix[2]);	//! Pitch
+					euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]);		//! Yaw
+					
+					/* swap values for next iteration, check for fatal inputs */
+					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
+						/* Do something */
+					} else {
+						/* due to inputs or numerical failure the output is invalid, skip it */
+						continue;
+					}
+
+					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+
+					last_data = raw.timestamp;
+
+					/* send out */
+					att.timestamp = raw.timestamp;
+
+					// XXX Apply the same transformation to the rotation matrix
+					att.roll = euler[0] - so3_comp_params.roll_off;
+					att.pitch = euler[1] - so3_comp_params.pitch_off;
+					att.yaw = euler[2] - so3_comp_params.yaw_off;
+
+					//! Euler angle rate. But it needs to be investigated again.
+					/*
+					att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
+					att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
+					att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3);
+					*/
+					att.rollspeed = gyro[0];
+					att.pitchspeed = gyro[1];
+					att.yawspeed = gyro[2];
+
+					att.rollacc = 0;
+					att.pitchacc = 0;
+					att.yawacc = 0;
+
+					//! Quaternion
+					att.q[0] = q0;
+					att.q[1] = q1;
+					att.q[2] = q2;
+					att.q[3] = q3;
+					att.q_valid = true;
+
+					/* TODO: Bias estimation required */
+					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
+
+					/* copy rotation matrix */
+					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					att.R_valid = true;
+
+					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
+						// Broadcast
+						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+
+					} else {
+						warnx("NaN in roll/pitch/yaw estimate!");
+					}
+
+					perf_end(so3_comp_loop_perf);
+
+					//! This will print out debug packet to visualization software
+					if(debug_mode)
+					{
+						float quat[4];
+						quat[0] = q0;
+						quat[1] = q1;
+						quat[2] = q2;
+						quat[3] = q3;
+						send_uart_float_arr(quat,4);
+						send_uart_byte('\n');
+					}
+				}
+			}
+		}
+
+		loopcounter++;
+	}// while
+
+	thread_running = false;
+
+	/* Reset the UART flags to original state */
+        if (!usb_uart)
+                tcsetattr(uart, TCSANOW, &uart_config_original);
+
+	return 0;
+}

src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q15.c

@@ -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_dot_prod_q15.c    
-*    
-* Description:	Q15 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    
- */
-
-/**    
- * @addtogroup dot_prod    
- * @{    
- */
-
-/**    
- * @brief Dot product of Q15 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.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The intermediate multiplications are in 1.15 x 1.15 = 2.30 format and these    
- * results are added to a 64-bit accumulator in 34.30 format.    
- * Nonsaturating additions are used and given that there are 33 guard bits in the accumulator    
- * there is no risk of overflow.    
- * The return result is in 34.30 format.    
- */
-
-void arm_dot_prod_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  uint32_t blockSize,
-  q63_t * result)
-{
-  q63_t sum = 0;                                 /* 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 = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
-    sum = __SMLALD(*__SIMD32(pSrcA)++, *__SIMD32(pSrcB)++, sum);
-
-    /* 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[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
-    /* Calculate dot product and then store the results in a temporary buffer. */
-    sum = __SMLALD(*pSrcA++, *pSrcB++, sum);
-
-    /* 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[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
-    /* Calculate dot product and then store the results in a temporary buffer. */
-    sum += (q63_t) ((q31_t) * pSrcA++ * *pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  /* Store the result in the destination buffer in 34.30 format */
-  *result = sum;
-
-}
-
-/**    
- * @} end of dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q31.c

@@ -1,138 +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_q31.c    
-*    
-* Description:	Q31 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    
- */
-
-/**    
- * @addtogroup dot_prod    
- * @{    
- */
-
-/**    
- * @brief Dot product of Q31 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.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The intermediate multiplications are in 1.31 x 1.31 = 2.62 format and these    
- * are truncated to 2.48 format by discarding the lower 14 bits.    
- * The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format.    
- * There are 15 guard bits in the accumulator and there is no risk of overflow as long as    
- * the length of the vectors is less than 2^16 elements.    
- * The return result is in 16.48 format.    
- */
-
-void arm_dot_prod_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  uint32_t blockSize,
-  q63_t * result)
-{
-  q63_t sum = 0;                                 /* Temporary result storage */
-  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[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. */
-    inA1 = *pSrcA++;
-    inA2 = *pSrcA++;
-    inA3 = *pSrcA++;
-    inA4 = *pSrcA++;
-    inB1 = *pSrcB++;
-    inB2 = *pSrcB++;
-    inB3 = *pSrcB++;
-    inB4 = *pSrcB++;
-
-    sum += ((q63_t) inA1 * inB1) >> 14u;
-    sum += ((q63_t) inA2 * inB2) >> 14u;
-    sum += ((q63_t) inA3 * inB3) >> 14u;
-    sum += ((q63_t) inA4 * inB4) >> 14u;
-
-    /* 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 += ((q63_t) * pSrcA++ * *pSrcB++) >> 14u;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* Store the result in the destination buffer in 16.48 format */
-  *result = sum;
-}
-
-/**    
- * @} end of dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_q7.c

@@ -1,154 +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_q7.c    
-*    
-* Description:	Q7 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    
- */
-
-/**    
- * @addtogroup dot_prod    
- * @{    
- */
-
-/**    
- * @brief Dot product of Q7 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.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The intermediate multiplications are in 1.7 x 1.7 = 2.14 format and these    
- * results are added to an accumulator in 18.14 format.    
- * Nonsaturating additions are used and there is no danger of wrap around as long as    
- * the vectors are less than 2^18 elements long.    
- * The return result is in 18.14 format.    
- */
-
-void arm_dot_prod_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  uint32_t blockSize,
-  q31_t * result)
-{
-  uint32_t blkCnt;                               /* loop counter */
-
-  q31_t sum = 0;                                 /* Temporary variables to store output */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  q31_t input1, input2;                          /* Temporary variables to store input */
-  q31_t inA1, inA2, inB1, inB2;                  /* Temporary variables to store input */
-
-
-
-  /*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)
-  {
-    /* read 4 samples at a time from sourceA */
-    input1 = *__SIMD32(pSrcA)++;
-    /* read 4 samples at a time from sourceB */
-    input2 = *__SIMD32(pSrcB)++;
-
-    /* extract two q7_t samples to q15_t samples */
-    inA1 = __SXTB16(__ROR(input1, 8));
-    /* extract reminaing two samples */
-    inA2 = __SXTB16(input1);
-    /* extract two q7_t samples to q15_t samples */
-    inB1 = __SXTB16(__ROR(input2, 8));
-    /* extract reminaing two samples */
-    inB2 = __SXTB16(input2);
-
-    /* multiply and accumulate two samples at a time */
-    sum = __SMLAD(inA1, inB1, sum);
-    sum = __SMLAD(inA2, inB2, sum);
-
-    /* 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[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
-    /* Dot product and then store the results in a temporary buffer. */
-    sum = __SMLAD(*pSrcA++, *pSrcB++, sum);
-
-    /* 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[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */
-    /* Dot product and then store the results in a temporary buffer. */
-    sum += (q31_t) ((q15_t) * pSrcA++ * *pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-
-  /* Store the result in the destination buffer in 18.14 format */
-  *result = sum;
-}
-
-/**    
- * @} end of dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_f32.c

@@ -1,172 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_mult_f32.c    
-*    
-* Description:	Floating-point vector multiplication.    
-*    
-* 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.5  2010/04/26     
-*    incorporated review comments and updated with latest CMSIS layer    
-*    
-* Version 0.0.3  2010/03/10     
-*    Initial version    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**        
- * @ingroup groupMath        
- */
-
-/**        
- * @defgroup BasicMult Vector Multiplication        
- *        
- * Element-by-element multiplication 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 BasicMult        
- * @{        
- */
-
-/**        
- * @brief Floating-point vector multiplication.        
- * @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_mult_f32(
-  float32_t * pSrcA,
-  float32_t * pSrcB,
-  float32_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counters */
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  float32_t inA1, inA2, inA3, inA4;              /* temporary input variables */
-  float32_t inB1, inB2, inB3, inB4;              /* temporary input variables */
-  float32_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 * B */
-    /* Multiply the inputs and store the results in output buffer */
-    /* read sample from sourceA */
-    inA1 = *pSrcA;
-    /* read sample from sourceB */
-    inB1 = *pSrcB;
-    /* read sample from sourceA */
-    inA2 = *(pSrcA + 1);
-    /* read sample from sourceB */
-    inB2 = *(pSrcB + 1);
-
-    /* out = sourceA * sourceB */
-    out1 = inA1 * inB1;
-
-    /* read sample from sourceA */
-    inA3 = *(pSrcA + 2);
-    /* read sample from sourceB */
-    inB3 = *(pSrcB + 2);
-
-    /* out = sourceA * sourceB */
-    out2 = inA2 * inB2;
-
-    /* read sample from sourceA */
-    inA4 = *(pSrcA + 3);
-
-    /* store result to destination buffer */
-    *pDst = out1;
-
-    /* read sample from sourceB */
-    inB4 = *(pSrcB + 3);
-
-    /* out = sourceA * sourceB */
-    out3 = inA3 * inB3;
-
-    /* store result to destination buffer */
-    *(pDst + 1) = out2;
-
-    /* out = sourceA * sourceB */
-    out4 = inA4 * inB4;
-    /* store result to destination buffer */
-    *(pDst + 2) = out3;
-    /* store result to destination buffer */
-    *(pDst + 3) = out4;
-
-
-    /* update pointers to process next samples */
-    pSrcA += 4u;
-    pSrcB += 4u;
-    pDst += 4u;
-
-    /* Decrement the blockSize 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 */
-    /* Multiply the inputs and store the results in output buffer */
-    *pDst++ = (*pSrcA++) * (*pSrcB++);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of BasicMult group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q15.c

@@ -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_mult_q15.c    
-*    
-* Description:	Q15 vector multiplication.    
-*    
-* 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.5  2010/04/26     
-*    incorporated review comments and updated with latest CMSIS layer    
-*    
-* Version 0.0.3  2010/03/10     
-*    Initial version    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupMath    
- */
-
-/**    
- * @addtogroup BasicMult    
- * @{    
- */
-
-
-/**    
- * @brief           Q15 vector multiplication    
- * @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_mult_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counters */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q31_t inA1, inA2, inB1, inB2;                  /* temporary input variables */
-  q15_t out1, out2, out3, out4;                  /* temporary output variables */
-  q31_t mul1, mul2, mul3, mul4;                  /* 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)
-  {
-    /* read two samples at a time from sourceA */
-    inA1 = *__SIMD32(pSrcA)++;
-    /* read two samples at a time from sourceB */
-    inB1 = *__SIMD32(pSrcB)++;
-    /* read two samples at a time from sourceA */
-    inA2 = *__SIMD32(pSrcA)++;
-    /* read two samples at a time from sourceB */
-    inB2 = *__SIMD32(pSrcB)++;
-
-    /* multiply mul = sourceA * sourceB */
-    mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
-    mul2 = (q31_t) ((q15_t) inA1 * (q15_t) inB1);
-    mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16));
-    mul4 = (q31_t) ((q15_t) inA2 * (q15_t) inB2);
-
-    /* saturate result to 16 bit */
-    out1 = (q15_t) __SSAT(mul1 >> 15, 16);
-    out2 = (q15_t) __SSAT(mul2 >> 15, 16);
-    out3 = (q15_t) __SSAT(mul3 >> 15, 16);
-    out4 = (q15_t) __SSAT(mul4 >> 15, 16);
-
-    /* store the result */
-#ifndef ARM_MATH_BIG_ENDIAN
-
-    *__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
-    *__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
-
-#else
-
-    *__SIMD32(pDst)++ = __PKHBT(out2, out1, 16);
-    *__SIMD32(pDst)++ = __PKHBT(out4, out3, 16);
-
-#endif //      #ifndef ARM_MATH_BIG_ENDIAN
-
-    /* Decrement the blockSize 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 */
-    /* Multiply the inputs and store the result in the destination buffer */
-    *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-}
-
-/**    
- * @} end of BasicMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q31.c

@@ -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_mult_q31.c    
-*    
-* Description:	Q31 vector multiplication.    
-*    
-* 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.5  2010/04/26     
-*    incorporated review comments and updated with latest CMSIS layer    
-*    
-* Version 0.0.3  2010/03/10     
-*    Initial version    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupMath    
- */
-
-/**    
- * @addtogroup BasicMult    
- * @{    
- */
-
-/**    
- * @brief Q31 vector multiplication.    
- * @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_mult_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counters */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q31_t inA1, inA2, inA3, inA4;                  /* temporary input variables */
-  q31_t inB1, inB2, inB3, inB4;                  /* 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 * B */
-    /* Multiply the inputs and then store the results in the destination buffer. */
-    inA1 = *pSrcA++;
-    inA2 = *pSrcA++;
-    inA3 = *pSrcA++;
-    inA4 = *pSrcA++;
-    inB1 = *pSrcB++;
-    inB2 = *pSrcB++;
-    inB3 = *pSrcB++;
-    inB4 = *pSrcB++;
-
-    out1 = ((q63_t) inA1 * inB1) >> 32;
-    out2 = ((q63_t) inA2 * inB2) >> 32;
-    out3 = ((q63_t) inA3 * inB3) >> 32;
-    out4 = ((q63_t) inA4 * inB4) >> 32;
-
-    out1 = __SSAT(out1, 31);
-    out2 = __SSAT(out2, 31);
-    out3 = __SSAT(out3, 31);
-    out4 = __SSAT(out4, 31);
-
-    *pDst++ = out1 << 1u;
-    *pDst++ = out2 << 1u;
-    *pDst++ = out3 << 1u;
-    *pDst++ = out4 << 1u;
-
-    /* Decrement the blockSize 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 */
-    /* Multiply the inputs and then store the results in the destination buffer. */
-    *pDst++ =
-      (q31_t) clip_q63_to_q31(((q63_t) (*pSrcA++) * (*pSrcB++)) >> 31);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-}
-
-/**    
- * @} end of BasicMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_mult_q7.c

@@ -1,128 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_mult_q7.c    
-*    
-* Description:	Q7 vector multiplication.    
-*    
-* 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    
-*    
-* Version 0.0.5  2010/04/26     
-*    incorporated review comments and updated with latest CMSIS layer    
-*    
-* Version 0.0.3  2010/03/10 DP    
-*    Initial version    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupMath    
- */
-
-/**    
- * @addtogroup BasicMult    
- * @{    
- */
-
-/**    
- * @brief           Q7 vector multiplication    
- * @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_mult_q7(
-  q7_t * pSrcA,
-  q7_t * pSrcB,
-  q7_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counters */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q7_t out1, out2, out3, out4;                   /* Temporary variables to store the product */
-
-  /* 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 */
-    /* Multiply the inputs and store the results in temporary variables */
-    out1 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
-    out2 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
-    out3 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
-    out4 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
-
-    /* Store the results of 4 inputs in the destination buffer in single cycle by packing */
-    *__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
-
-    /* Decrement the blockSize 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 */
-    /* Multiply the inputs and store the result in the destination buffer */
-    *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-}
-
-/**    
- * @} end of BasicMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_f32.c

@@ -1,137 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_negate_f32.c    
-*    
-* Description:	Negates floating-point vectors.    
-*    
-* 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 negate Vector Negate        
- *        
- * Negates the elements of a vector.        
- *        
- * <pre>        
- *     pDst[n] = -pSrc[n],   0 <= n < blockSize.        
- * </pre>        
- */
-
-/**        
- * @addtogroup negate        
- * @{        
- */
-
-/**        
- * @brief  Negates the elements of a floating-point vector.        
- * @param[in]  *pSrc points to the input vector        
- * @param[out]  *pDst points to the output vector        
- * @param[in]  blockSize number of samples in the vector        
- * @return none.        
- */
-
-void arm_negate_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)
-  {
-    /* read inputs from source */
-    in1 = *pSrc;
-    in2 = *(pSrc + 1);
-    in3 = *(pSrc + 2);
-    in4 = *(pSrc + 3);
-
-    /* negate the input */
-    in1 = -in1;
-    in2 = -in2;
-    in3 = -in3;
-    in4 = -in4;
-
-    /* store the result to destination */
-    *pDst = in1;
-    *(pDst + 1) = in2;
-    *(pDst + 2) = in3;
-    *(pDst + 3) = in4;
-
-    /* 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 */
-
-  /* Initialize blkCnt with number of samples */
-  blkCnt = blockSize;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C = -A */
-    /* Negate and then store the results in the destination buffer. */
-    *pDst++ = -*pSrc++;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of negate group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q15.c

@@ -1,137 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_negate_q15.c    
-*    
-* Description:	Negates Q15 vectors.    
-*    
-* 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 negate        
- * @{        
- */
-
-/**        
- * @brief  Negates the elements of a Q15 vector.        
- * @param[in]  *pSrc points to the input vector        
- * @param[out]  *pDst points to the output vector        
- * @param[in]  blockSize number of samples in the 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 Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF.        
- */
-
-void arm_negate_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counter */
-  q15_t in;
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  q31_t in1, in2;                                /* 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 */
-    /* Read two inputs at a time */
-    in1 = _SIMD32_OFFSET(pSrc);
-    in2 = _SIMD32_OFFSET(pSrc + 2);
-
-    /* negate two samples at a time */
-    in1 = __QSUB16(0, in1);
-
-    /* negate two samples at a time */
-    in2 = __QSUB16(0, in2);
-
-    /* store the result to destination 2 samples at a time */
-    _SIMD32_OFFSET(pDst) = in1;
-    /* store the result to destination 2 samples at a time */
-    _SIMD32_OFFSET(pDst + 2) = in2;
-
-
-    /* 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 */
-
-  /* Initialize blkCnt with number of samples */
-  blkCnt = blockSize;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C = -A */
-    /* Negate and then store the result in the destination buffer. */
-    in = *pSrc++;
-    *pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of negate group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q31.c

@@ -1,124 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_negate_q31.c    
-*    
-* Description:	Negates Q31 vectors.    
-*    
-* 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 negate    
- * @{    
- */
-
-/**    
- * @brief  Negates the elements of a Q31 vector.    
- * @param[in]  *pSrc points to the input vector    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the 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_negate_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t blockSize)
-{
-  q31_t in;                                      /* Temporary variable */
-  uint32_t blkCnt;                               /* loop counter */
-
-#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 */
-    /* Negate and then store the results in the destination buffer. */
-    in1 = *pSrc++;
-    in2 = *pSrc++;
-    in3 = *pSrc++;
-    in4 = *pSrc++;
-
-    *pDst++ = __QSUB(0, in1);
-    *pDst++ = __QSUB(0, in2);
-    *pDst++ = __QSUB(0, in3);
-    *pDst++ = __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 */
-    /* Negate and then store the result in the destination buffer. */
-    in = *pSrc++;
-    *pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**    
- * @} end of negate group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_negate_q7.c

@@ -1,120 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_negate_q7.c    
-*    
-* Description:	Negates Q7 vectors.    
-*    
-* 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 negate    
- * @{    
- */
-
-/**    
- * @brief  Negates the elements of a Q7 vector.    
- * @param[in]  *pSrc points to the input vector    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the vector    
- * @return none.    
- *    
- * <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_negate_q7(
-  q7_t * pSrc,
-  q7_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counter */
-  q7_t in;
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q31_t input;                                   /* Input values1-4 */
-  q31_t zero = 0x00000000;
-
-
-  /*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 four inputs */
-    input = *__SIMD32(pSrc)++;
-
-    /* Store the Negated results in the destination buffer in a single cycle by packing the results */
-    *__SIMD32(pDst)++ = __QSUB8(zero, input);
-
-    /* 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 */
-    /* Negate and then store the results in the destination buffer. */ \
-      in = *pSrc++;
-    *pDst++ = (in == (q7_t) 0x80) ? 0x7f : -in;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**    
- * @} end of negate group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_f32.c

@@ -1,158 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_offset_f32.c    
-*    
-* Description:	Floating-point vector offset.    
-*    
-* 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 offset Vector Offset        
- *        
- * Adds a constant offset to each element of a vector.        
- *        
- * <pre>        
- *     pDst[n] = pSrc[n] + offset,   0 <= n < blockSize.        
- * </pre>        
- *        
- * There are separate functions for floating-point, Q7, Q15, and Q31 data types.        
- */
-
-/**        
- * @addtogroup offset        
- * @{        
- */
-
-/**        
- * @brief  Adds a constant offset to a floating-point vector.        
- * @param[in]  *pSrc points to the input vector        
- * @param[in]  offset is the offset to be added        
- * @param[out]  *pDst points to the output vector        
- * @param[in]  blockSize number of samples in the vector        
- * @return none.        
- */
-
-
-void arm_offset_f32(
-  float32_t * pSrc,
-  float32_t offset,
-  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;
-
-  /*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 + offset */
-    /* Add offset and then store the results in the destination buffer. */
-    /* read samples from source */
-    in1 = *pSrc;
-    in2 = *(pSrc + 1);
-
-    /* add offset to input */
-    in1 = in1 + offset;
-
-    /* read samples from source */
-    in3 = *(pSrc + 2);
-
-    /* add offset to input */
-    in2 = in2 + offset;
-
-    /* read samples from source */
-    in4 = *(pSrc + 3);
-
-    /* add offset to input */
-    in3 = in3 + offset;
-
-    /* store result to destination */
-    *pDst = in1;
-
-    /* add offset to input */
-    in4 = in4 + offset;
-
-    /* store result to destination */
-    *(pDst + 1) = in2;
-
-    /* store result to destination */
-    *(pDst + 2) = in3;
-
-    /* store result to destination */
-    *(pDst + 3) = in4;
-
-    /* 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 */
-
-  /* Initialize blkCnt with number of samples */
-  blkCnt = blockSize;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C = A + offset */
-    /* Add offset and then store the result in the destination buffer. */
-    *pDst++ = (*pSrc++) + offset;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of offset group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q15.c

@@ -1,131 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_offset_q15.c    
-*    
-* Description:	Q15 vector offset.    
-*    
-* 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 offset    
- * @{    
- */
-
-/**    
- * @brief  Adds a constant offset to a Q15 vector.    
- * @param[in]  *pSrc points to the input vector    
- * @param[in]  offset is the offset to be added    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function uses saturating arithmetic.    
- * Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated.    
- */
-
-void arm_offset_q15(
-  q15_t * pSrc,
-  q15_t offset,
-  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 offset_packed;                           /* Offset packed to 32 bit */
-
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-  /* Offset is packed to 32 bit in order to use SIMD32 for addition */
-  offset_packed = __PKHBT(offset, offset, 16);
-
-  /* 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 + offset */
-    /* Add offset and then store the results in the destination buffer, 2 samples at a time. */
-    *__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
-    *__SIMD32(pDst)++ = __QADD16(*__SIMD32(pSrc)++, offset_packed);
-
-    /* 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 + offset */
-    /* Add offset and then store the results in the destination buffer. */
-    *pDst++ = (q15_t) __QADD16(*pSrc++, offset);
-
-    /* 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 + offset */
-    /* Add offset and then store the results in the destination buffer. */
-    *pDst++ = (q15_t) __SSAT(((q31_t) * pSrc++ + offset), 16);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of offset group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q31.c

@@ -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_offset_q31.c    
-*    
-* Description:	Q31 vector offset.    
-*    
-* 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 offset    
- * @{    
- */
-
-/**    
- * @brief  Adds a constant offset to a Q31 vector.    
- * @param[in]  *pSrc points to the input vector    
- * @param[in]  offset is the offset to be added    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function uses saturating arithmetic.    
- * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated.    
- */
-
-void arm_offset_q31(
-  q31_t * pSrc,
-  q31_t offset,
-  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 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 + offset */
-    /* Add offset and then store the results in the destination buffer. */
-    in1 = *pSrc++;
-    in2 = *pSrc++;
-    in3 = *pSrc++;
-    in4 = *pSrc++;
-
-    *pDst++ = __QADD(in1, offset);
-    *pDst++ = __QADD(in2, offset);
-    *pDst++ = __QADD(in3, offset);
-    *pDst++ = __QADD(in4, offset);
-
-    /* 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 + offset */
-    /* Add offset and then store the result in the destination buffer. */
-    *pDst++ = __QADD(*pSrc++, offset);
-
-    /* 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 + offset */
-    /* Add offset and then store the result in the destination buffer. */
-    *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of offset group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_offset_q7.c

@@ -1,130 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_offset_q7.c    
-*    
-* Description:	Q7 vector offset.    
-*    
-* 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 offset    
- * @{    
- */
-
-/**    
- * @brief  Adds a constant offset to a Q7 vector.    
- * @param[in]  *pSrc points to the input vector    
- * @param[in]  offset is the offset to be added    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function uses saturating arithmetic.    
- * Results outside of the allowable Q7 range [0x80 0x7F] are saturated.    
- */
-
-void arm_offset_q7(
-  q7_t * pSrc,
-  q7_t offset,
-  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 */
-  q31_t offset_packed;                           /* Offset packed to 32 bit */
-
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-  /* Offset is packed to 32 bit in order to use SIMD32 for addition */
-  offset_packed = __PACKq7(offset, offset, offset, offset);
-
-  /* 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 + offset */
-    /* Add offset and then store the results in the destination bufferfor 4 samples at a time. */
-    *__SIMD32(pDst)++ = __QADD8(*__SIMD32(pSrc)++, offset_packed);
-
-    /* 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 + offset */
-    /* Add offset and then store the result in the destination buffer. */
-    *pDst++ = (q7_t) __SSAT(*pSrc++ + offset, 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 + offset */
-    /* Add offset and then store the result in the destination buffer. */
-    *pDst++ = (q7_t) __SSAT((q15_t) * pSrc++ + offset, 8);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of offset group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c

@@ -1,161 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_scale_f32.c    
-*    
-* Description:	Multiplies a floating-point vector by a scalar.    
-*    
-* 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 scale Vector Scale        
- *        
- * Multiply a vector by a scalar value.  For floating-point data, the algorithm used is:        
- *        
- * <pre>        
- *     pDst[n] = pSrc[n] * scale,   0 <= n < blockSize.        
- * </pre>        
- *        
- * In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by        
- * a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>.        
- * The shift allows the gain of the scaling operation to exceed 1.0.        
- * The algorithm used with fixed-point data is:        
- *        
- * <pre>        
- *     pDst[n] = (pSrc[n] * scaleFract) << shift,   0 <= n < blockSize.        
- * </pre>        
- *        
- * The overall scale factor applied to the fixed-point data is        
- * <pre>        
- *     scale = scaleFract * 2^shift.        
- * </pre>        
- */
-
-/**        
- * @addtogroup scale        
- * @{        
- */
-
-/**        
- * @brief Multiplies a floating-point vector by a scalar.        
- * @param[in]       *pSrc points to the input vector        
- * @param[in]       scale scale factor to be applied        
- * @param[out]      *pDst points to the output vector        
- * @param[in]       blockSize number of samples in the vector        
- * @return none.        
- */
-
-
-void arm_scale_f32(
-  float32_t * pSrc,
-  float32_t scale,
-  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 variabels */
-
-  /*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 * scale */
-    /* Scale the input and then store the results in the destination buffer. */
-    /* read input samples from source */
-    in1 = *pSrc;
-    in2 = *(pSrc + 1);
-
-    /* multiply with scaling factor */
-    in1 = in1 * scale;
-
-    /* read input sample from source */
-    in3 = *(pSrc + 2);
-
-    /* multiply with scaling factor */
-    in2 = in2 * scale;
-
-    /* read input sample from source */
-    in4 = *(pSrc + 3);
-
-    /* multiply with scaling factor */
-    in3 = in3 * scale;
-    in4 = in4 * scale;
-    /* store the result to destination */
-    *pDst = in1;
-    *(pDst + 1) = in2;
-    *(pDst + 2) = in3;
-    *(pDst + 3) = in4;
-
-    /* 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 */
-
-  /* Initialize blkCnt with number of samples */
-  blkCnt = blockSize;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C = A * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    *pDst++ = (*pSrc++) * scale;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of scale group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q15.c

@@ -1,157 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_scale_q15.c    
-*    
-* Description:	Multiplies a Q15 vector by a scalar.    
-*    
-* 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 scale    
- * @{    
- */
-
-/**    
- * @brief Multiplies a Q15 vector by a scalar.    
- * @param[in]       *pSrc points to the input vector    
- * @param[in]       scaleFract fractional portion of the scale value    
- * @param[in]       shift number of bits to shift the result by    
- * @param[out]      *pDst points to the output vector    
- * @param[in]       blockSize number of samples in the vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format.    
- * These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format.    
- */
-
-
-void arm_scale_q15(
-  q15_t * pSrc,
-  q15_t scaleFract,
-  int8_t shift,
-  q15_t * pDst,
-  uint32_t blockSize)
-{
-  int8_t kShift = 15 - shift;                    /* shift to apply after scaling */
-  uint32_t blkCnt;                               /* loop counter */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q15_t in1, in2, in3, in4;
-  q31_t inA1, inA2;                              /* Temporary variables */
-  q31_t out1, out2, out3, out4;
-
-
-  /*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)
-  {
-    /* Reading 2 inputs from memory */
-    inA1 = *__SIMD32(pSrc)++;
-    inA2 = *__SIMD32(pSrc)++;
-
-    /* C = A * scale */
-    /* Scale the inputs and then store the 2 results in the destination buffer        
-     * in single cycle by packing the outputs */
-    out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract);
-    out2 = (q31_t) ((q15_t) inA1 * scaleFract);
-    out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract);
-    out4 = (q31_t) ((q15_t) inA2 * scaleFract);
-
-    /* apply shifting */
-    out1 = out1 >> kShift;
-    out2 = out2 >> kShift;
-    out3 = out3 >> kShift;
-    out4 = out4 >> kShift;
-
-    /* saturate the output */
-    in1 = (q15_t) (__SSAT(out1, 16));
-    in2 = (q15_t) (__SSAT(out2, 16));
-    in3 = (q15_t) (__SSAT(out3, 16));
-    in4 = (q15_t) (__SSAT(out4, 16));
-
-    /* store the result to destination */
-    *__SIMD32(pDst)++ = __PKHBT(in2, in1, 16);
-    *__SIMD32(pDst)++ = __PKHBT(in4, in3, 16);
-
-    /* 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 * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    *pDst++ = (q15_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 16));
-
-    /* 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 * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    *pDst++ = (q15_t) (__SSAT(((q31_t) * pSrc++ * scaleFract) >> kShift, 16));
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of scale group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q31.c

@@ -1,221 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_scale_q31.c    
-*    
-* Description:	Multiplies a Q31 vector by a scalar.    
-*    
-* 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 scale       
- * @{       
- */
-
-/**       
- * @brief Multiplies a Q31 vector by a scalar.       
- * @param[in]       *pSrc points to the input vector       
- * @param[in]       scaleFract fractional portion of the scale value       
- * @param[in]       shift number of bits to shift the result by       
- * @param[out]      *pDst points to the output vector       
- * @param[in]       blockSize number of samples in the vector       
- * @return none.       
- *       
- * <b>Scaling and Overflow Behavior:</b>       
- * \par       
- * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format.       
- * These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format.       
- */
-
-void arm_scale_q31(
-  q31_t * pSrc,
-  q31_t scaleFract,
-  int8_t shift,
-  q31_t * pDst,
-  uint32_t blockSize)
-{
-  int8_t kShift = shift + 1;                     /* Shift to apply after scaling */
-  int8_t sign = (kShift & 0x80);
-  uint32_t blkCnt;                               /* loop counter */
-  q31_t in, out;
-
-#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 variabels */
-
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-  if(sign == 0u)
-  {
-    /* 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)
-    {
-      /* read four inputs from source */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      in3 = *(pSrc + 2);
-      in4 = *(pSrc + 3);
-
-      /* multiply input with scaler value */
-      in1 = ((q63_t) in1 * scaleFract) >> 32;
-      in2 = ((q63_t) in2 * scaleFract) >> 32;
-      in3 = ((q63_t) in3 * scaleFract) >> 32;
-      in4 = ((q63_t) in4 * scaleFract) >> 32;
-
-      /* apply shifting */
-      out1 = in1 << kShift;
-      out2 = in2 << kShift;
-
-      /* saturate the results. */
-      if(in1 != (out1 >> kShift))
-        out1 = 0x7FFFFFFF ^ (in1 >> 31);
-
-      if(in2 != (out2 >> kShift))
-        out2 = 0x7FFFFFFF ^ (in2 >> 31);
-
-      out3 = in3 << kShift;
-      out4 = in4 << kShift;
-
-      *pDst = out1;
-      *(pDst + 1) = out2;
-
-      if(in3 != (out3 >> kShift))
-        out3 = 0x7FFFFFFF ^ (in3 >> 31);
-
-      if(in4 != (out4 >> kShift))
-        out4 = 0x7FFFFFFF ^ (in4 >> 31);
-
-      /* Store result destination */
-      *(pDst + 2) = out3;
-      *(pDst + 3) = out4;
-
-      /* Update pointers to process next sampels */
-      pSrc += 4u;
-      pDst += 4u;
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-
-  }
-  else
-  {
-    kShift = -kShift;
-
-    /* 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)
-    {
-      /* read four inputs from source */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      in3 = *(pSrc + 2);
-      in4 = *(pSrc + 3);
-
-      /* multiply input with scaler value */
-      in1 = ((q63_t) in1 * scaleFract) >> 32;
-      in2 = ((q63_t) in2 * scaleFract) >> 32;
-      in3 = ((q63_t) in3 * scaleFract) >> 32;
-      in4 = ((q63_t) in4 * scaleFract) >> 32;
-
-      /* apply shifting */
-      out1 = in1 >> kShift;
-      out2 = in2 >> kShift;
-
-      out3 = in3 >> kShift;
-      out4 = in4 >> kShift;
-
-      /* Store result destination */
-      *pDst = out1;
-      *(pDst + 1) = out2;
-
-      *(pDst + 2) = out3;
-      *(pDst + 3) = out4;
-
-      /* Update pointers to process next sampels */
-      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 */
-
-  /* Initialize blkCnt with number of samples */
-  blkCnt = blockSize;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C = A * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    in = *pSrc++;
-    in = ((q63_t) in * scaleFract) >> 32;
-
-    if(sign == 0)
-    {
-      out = in << kShift;
-      if(in != (out >> kShift))
-        out = 0x7FFFFFFF ^ (in >> 31);
-    }
-    else
-    {
-      out = in >> kShift;
-    }
-
-    *pDst++ = out;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**       
- * @} end of scale group       
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_q7.c

@@ -1,144 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_scale_q7.c    
-*    
-* Description:	Multiplies a Q7 vector by a scalar.    
-*    
-* 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 scale    
- * @{    
- */
-
-/**    
- * @brief Multiplies a Q7 vector by a scalar.    
- * @param[in]       *pSrc points to the input vector    
- * @param[in]       scaleFract fractional portion of the scale value    
- * @param[in]       shift number of bits to shift the result by    
- * @param[out]      *pDst points to the output vector    
- * @param[in]       blockSize number of samples in the vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.7 format.    
- * These are multiplied to yield a 2.14 intermediate result and this is shifted with saturation to 1.7 format.    
- */
-
-void arm_scale_q7(
-  q7_t * pSrc,
-  q7_t scaleFract,
-  int8_t shift,
-  q7_t * pDst,
-  uint32_t blockSize)
-{
-  int8_t kShift = 7 - shift;                     /* shift to apply after scaling */
-  uint32_t blkCnt;                               /* loop counter */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q7_t in1, in2, in3, in4, out1, out2, out3, out4;      /* Temporary variables to store input & output */
-
-
-  /*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)
-  {
-    /* Reading 4 inputs from memory */
-    in1 = *pSrc++;
-    in2 = *pSrc++;
-    in3 = *pSrc++;
-    in4 = *pSrc++;
-
-    /* C = A * scale */
-    /* Scale the inputs and then store the results in the temporary variables. */
-    out1 = (q7_t) (__SSAT(((in1) * scaleFract) >> kShift, 8));
-    out2 = (q7_t) (__SSAT(((in2) * scaleFract) >> kShift, 8));
-    out3 = (q7_t) (__SSAT(((in3) * scaleFract) >> kShift, 8));
-    out4 = (q7_t) (__SSAT(((in4) * scaleFract) >> kShift, 8));
-
-    /* Packing the individual outputs into 32bit and storing in    
-     * destination buffer in single write */
-    *__SIMD32(pDst)++ = __PACKq7(out1, out2, out3, out4);
-
-    /* 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 * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    *pDst++ = (q7_t) (__SSAT(((*pSrc++) * scaleFract) >> kShift, 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 * scale */
-    /* Scale the input and then store the result in the destination buffer. */
-    *pDst++ = (q7_t) (__SSAT((((q15_t) * pSrc++ * scaleFract) >> kShift), 8));
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of scale group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q15.c

@@ -1,243 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_shift_q15.c    
-*    
-* Description:	Shifts the elements of a Q15 vector by a specified number of bits.    
-*    
-* 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 shift    
- * @{    
- */
-
-/**    
- * @brief  Shifts the elements of a Q15 vector a specified number of bits.    
- * @param[in]  *pSrc points to the input vector    
- * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.    
- * @param[out]  *pDst points to the output vector    
- * @param[in]  blockSize number of samples in the 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_shift_q15(
-  q15_t * pSrc,
-  int8_t shiftBits,
-  q15_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counter */
-  uint8_t sign;                                  /* Sign of shiftBits */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  q15_t in1, in2;                                /* Temporary variables */
-
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-  /* Getting the sign of shiftBits */
-  sign = (shiftBits & 0x80);
-
-  /* If the shift value is positive then do right shift else left shift */
-  if(sign == 0u)
-  {
-    /* 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)
-    {
-      /* Read 2 inputs */
-      in1 = *pSrc++;
-      in2 = *pSrc++;
-      /* C = A << shiftBits */
-      /* Shift the inputs and then store the results in the destination buffer. */
-#ifndef  ARM_MATH_BIG_ENDIAN
-
-      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
-                                  __SSAT((in2 << shiftBits), 16), 16);
-
-#else
-
-      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
-                                  __SSAT((in1 << shiftBits), 16), 16);
-
-#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
-
-      in1 = *pSrc++;
-      in2 = *pSrc++;
-
-#ifndef  ARM_MATH_BIG_ENDIAN
-
-      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
-                                  __SSAT((in2 << shiftBits), 16), 16);
-
-#else
-
-      *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
-                                  __SSAT((in1 << shiftBits), 16), 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 << shiftBits */
-      /* Shift and then store the results in the destination buffer. */
-      *pDst++ = __SSAT((*pSrc++ << shiftBits), 16);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-  else
-  {
-    /* 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)
-    {
-      /* Read 2 inputs */
-      in1 = *pSrc++;
-      in2 = *pSrc++;
-
-      /* C = A >> shiftBits */
-      /* Shift the inputs and then store the results in the destination buffer. */
-#ifndef  ARM_MATH_BIG_ENDIAN
-
-      *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
-                                  (in2 >> -shiftBits), 16);
-
-#else
-
-      *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
-                                  (in1 >> -shiftBits), 16);
-
-#endif /* #ifndef  ARM_MATH_BIG_ENDIAN    */
-
-      in1 = *pSrc++;
-      in2 = *pSrc++;
-
-#ifndef  ARM_MATH_BIG_ENDIAN
-
-      *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
-                                  (in2 >> -shiftBits), 16);
-
-#else
-
-      *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
-                                  (in1 >> -shiftBits), 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 >> shiftBits */
-      /* Shift the inputs and then store the results in the destination buffer. */
-      *pDst++ = (*pSrc++ >> -shiftBits);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  /* Getting the sign of shiftBits */
-  sign = (shiftBits & 0x80);
-
-  /* If the shift value is positive then do right shift else left shift */
-  if(sign == 0u)
-  {
-    /* Initialize blkCnt with number of samples */
-    blkCnt = blockSize;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A << shiftBits */
-      /* Shift and then store the results in the destination buffer. */
-      *pDst++ = __SSAT(((q31_t) * pSrc++ << shiftBits), 16);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-  else
-  {
-    /* Initialize blkCnt with number of samples */
-    blkCnt = blockSize;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A >> shiftBits */
-      /* Shift the inputs and then store the results in the destination buffer. */
-      *pDst++ = (*pSrc++ >> -shiftBits);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of shift group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q31.c

@@ -1,195 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_shift_q31.c    
-*    
-* Description:	Shifts the elements of a Q31 vector by a specified number of bits.    
-*    
-* 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 shift Vector Shift        
- *        
- * Shifts the elements of a fixed-point vector by a specified number of bits.        
- * There are separate functions for Q7, Q15, and Q31 data types.        
- * The underlying algorithm used is:        
- *        
- * <pre>        
- *     pDst[n] = pSrc[n] << shift,   0 <= n < blockSize.        
- * </pre>        
- *        
- * If <code>shift</code> is positive then the elements of the vector are shifted to the left.        
- * If <code>shift</code> is negative then the elements of the vector are shifted to the right.        
- */
-
-/**        
- * @addtogroup shift        
- * @{        
- */
-
-/**        
- * @brief  Shifts the elements of a Q31 vector a specified number of bits.        
- * @param[in]  *pSrc points to the input vector        
- * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.        
- * @param[out]  *pDst points to the output vector        
- * @param[in]  blockSize number of samples in the 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_shift_q31(
-  q31_t * pSrc,
-  int8_t shiftBits,
-  q31_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counter */
-  uint8_t sign = (shiftBits & 0x80);             /* Sign of shiftBits */
-
-#ifndef ARM_MATH_CM0
-
-  q31_t in1, in2, in3, in4;                      /* Temporary input variables */
-  q31_t out1, out2, out3, out4;                  /* Temporary output variables */
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-
-  if(sign == 0u)
-  {
-    /* 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  << shiftBits */
-      /* Shift the input and then store the results in the destination buffer. */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      out1 = in1 << shiftBits;
-      in3 = *(pSrc + 2);
-      out2 = in2 << shiftBits;
-      in4 = *(pSrc + 3);
-      if(in1 != (out1 >> shiftBits))
-        out1 = 0x7FFFFFFF ^ (in1 >> 31);
-
-      if(in2 != (out2 >> shiftBits))
-        out2 = 0x7FFFFFFF ^ (in2 >> 31);
-
-      *pDst = out1;
-      out3 = in3 << shiftBits;
-      *(pDst + 1) = out2;
-      out4 = in4 << shiftBits;
-
-      if(in3 != (out3 >> shiftBits))
-        out3 = 0x7FFFFFFF ^ (in3 >> 31);
-
-      if(in4 != (out4 >> shiftBits))
-        out4 = 0x7FFFFFFF ^ (in4 >> 31);
-
-      *(pDst + 2) = out3;
-      *(pDst + 3) = out4;
-
-      /* Update destination pointer to process next sampels */
-      pSrc += 4u;
-      pDst += 4u;
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-  else
-  {
-
-    /* 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 >>  shiftBits */
-      /* Shift the input and then store the results in the destination buffer. */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      in3 = *(pSrc + 2);
-      in4 = *(pSrc + 3);
-
-      *pDst = (in1 >> -shiftBits);
-      *(pDst + 1) = (in2 >> -shiftBits);
-      *(pDst + 2) = (in3 >> -shiftBits);
-      *(pDst + 3) = (in4 >> -shiftBits);
-
-
-      pSrc += 4u;
-      pDst += 4u;
-
-      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 (>> or <<) shiftBits */
-    /* Shift the input and then store the result in the destination buffer. */
-    *pDst++ = (sign == 0u) ? clip_q63_to_q31((q63_t) * pSrc++ << shiftBits) :
-      (*pSrc++ >> -shiftBits);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-
-}
-
-/**        
- * @} end of shift group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_shift_q7.c

@@ -1,215 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_shift_q7.c    
-*    
-* Description:	Processing function for the Q7 Shifting    
-*    
-* 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 shift        
- * @{        
- */
-
-
-/**        
- * @brief  Shifts the elements of a Q7 vector a specified number of bits.        
- * @param[in]  *pSrc points to the input vector        
- * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.        
- * @param[out]  *pDst points to the output vector        
- * @param[in]  blockSize number of samples in the 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.        
- * Results outside of the allowable Q7 range [0x8 0x7F] will be saturated.        
- */
-
-void arm_shift_q7(
-  q7_t * pSrc,
-  int8_t shiftBits,
-  q7_t * pDst,
-  uint32_t blockSize)
-{
-  uint32_t blkCnt;                               /* loop counter */
-  uint8_t sign;                                  /* Sign of shiftBits */
-
-#ifndef ARM_MATH_CM0
-
-/* Run the below code for Cortex-M4 and Cortex-M3 */
-  q7_t in1;                                      /* Input value1 */
-  q7_t in2;                                      /* Input value2 */
-  q7_t in3;                                      /* Input value3 */
-  q7_t in4;                                      /* Input value4 */
-
-
-  /*loop Unrolling */
-  blkCnt = blockSize >> 2u;
-
-  /* Getting the sign of shiftBits */
-  sign = (shiftBits & 0x80);
-
-  /* If the shift value is positive then do right shift else left shift */
-  if(sign == 0u)
-  {
-    /* 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 << shiftBits */
-      /* Read 4 inputs */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      in3 = *(pSrc + 2);
-      in4 = *(pSrc + 3);
-
-      /* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
-      *__SIMD32(pDst)++ = __PACKq7(__SSAT((in1 << shiftBits), 8),
-                                   __SSAT((in2 << shiftBits), 8),
-                                   __SSAT((in3 << shiftBits), 8),
-                                   __SSAT((in4 << shiftBits), 8));
-      /* Update source pointer to process next sampels */
-      pSrc += 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;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A << shiftBits */
-      /* Shift the input and then store the result in the destination buffer. */
-      *pDst++ = (q7_t) __SSAT((*pSrc++ << shiftBits), 8);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-  else
-  {
-    shiftBits = -shiftBits;
-    /* 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 >> shiftBits */
-      /* Read 4 inputs */
-      in1 = *pSrc;
-      in2 = *(pSrc + 1);
-      in3 = *(pSrc + 2);
-      in4 = *(pSrc + 3);
-
-      /* Store the Shifted result in the destination buffer in single cycle by packing the outputs */
-      *__SIMD32(pDst)++ = __PACKq7((in1 >> shiftBits), (in2 >> shiftBits),
-                                   (in3 >> shiftBits), (in4 >> shiftBits));
-
-
-      pSrc += 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;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A >> shiftBits */
-      /* Shift the input and then store the result in the destination buffer. */
-      in1 = *pSrc++;
-      *pDst++ = (in1 >> shiftBits);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  /* Getting the sign of shiftBits */
-  sign = (shiftBits & 0x80);
-
-  /* If the shift value is positive then do right shift else left shift */
-  if(sign == 0u)
-  {
-    /* Initialize blkCnt with number of samples */
-    blkCnt = blockSize;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A << shiftBits */
-      /* Shift the input and then store the result in the destination buffer. */
-      *pDst++ = (q7_t) __SSAT(((q15_t) * pSrc++ << shiftBits), 8);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-  else
-  {
-    /* Initialize blkCnt with number of samples */
-    blkCnt = blockSize;
-
-    while(blkCnt > 0u)
-    {
-      /* C = A >> shiftBits */
-      /* Shift the input and then store the result in the destination buffer. */
-      *pDst++ = (*pSrc++ >> -shiftBits);
-
-      /* Decrement the loop counter */
-      blkCnt--;
-    }
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-}
-
-/**        
- * @} end of shift group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c

@@ -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_sub_f32.c    
-*    
-* Description:	Floating-point vector subtraction.    
-*    
-* 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 BasicSub Vector Subtraction        
- *        
- * Element-by-element subtraction 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 BasicSub        
- * @{        
- */
-
-
-/**        
- * @brief Floating-point vector subtraction.        
- * @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_sub_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 variables */
-  float32_t inB1, inB2, inB3, inB4;              /* 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 - B */
-    /* Subtract and then store the results in the destination buffer. */
-    /* Read 4 input samples from sourceA and sourceB */
-    inA1 = *pSrcA;
-    inB1 = *pSrcB;
-    inA2 = *(pSrcA + 1);
-    inB2 = *(pSrcB + 1);
-    inA3 = *(pSrcA + 2);
-    inB3 = *(pSrcB + 2);
-    inA4 = *(pSrcA + 3);
-    inB4 = *(pSrcB + 3);
-
-    /* dst = srcA - srcB */
-    /* subtract and store the result */
-    *pDst = inA1 - inB1;
-    *(pDst + 1) = inA2 - inB2;
-    *(pDst + 2) = inA3 - inB3;
-    *(pDst + 3) = inA4 - inB4;
-
-
-    /* Update pointers to process next sampels */
-    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 */
-    /* Subtract and then store the results in the destination buffer. */
-    *pDst++ = (*pSrcA++) - (*pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of BasicSub group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q15.c

@@ -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_sub_q15.c    
-*    
-* Description:	Q15 vector subtraction.    
-*    
-* 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 BasicSub    
- * @{    
- */
-
-/**    
- * @brief Q15 vector subtraction.    
- * @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_sub_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;
-  q31_t 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 */
-    /* Subtract and then store the results in the destination buffer two samples at a time. */
-    inA1 = *__SIMD32(pSrcA)++;
-    inA2 = *__SIMD32(pSrcA)++;
-    inB1 = *__SIMD32(pSrcB)++;
-    inB2 = *__SIMD32(pSrcB)++;
-
-    *__SIMD32(pDst)++ = __QSUB16(inA1, inB1);
-    *__SIMD32(pDst)++ = __QSUB16(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 */
-    /* Subtract and then store the result in the destination buffer. */
-    *pDst++ = (q15_t) __QSUB16(*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 */
-    /* Subtract and then store the result 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 BasicSub group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q31.c

@@ -1,141 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_sub_q31.c    
-*    
-* Description:	Q31 vector subtraction.    
-*    
-* 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 BasicSub    
- * @{    
- */
-
-/**    
- * @brief Q31 vector subtraction.    
- * @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_sub_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 */
-    /* Subtract 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++ = __QSUB(inA1, inB1);
-    *pDst++ = __QSUB(inA2, inB2);
-    *pDst++ = __QSUB(inA3, inB3);
-    *pDst++ = __QSUB(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 */
-    /* Subtract and then store the result in the destination buffer. */
-    *pDst++ = __QSUB(*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 */
-    /* Subtract and then store the result 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 BasicSub group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_q7.c

@@ -1,126 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_sub_q7.c    
-*    
-* Description:	Q7 vector subtraction.    
-*    
-* 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 BasicSub    
- * @{    
- */
-
-/**    
- * @brief Q7 vector subtraction.    
- * @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_sub_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 */
-    /* Subtract and then store the results in the destination buffer 4 samples at a time. */
-    *__SIMD32(pDst)++ = __QSUB8(*__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 */
-    /* Subtract and then store the result in the destination buffer. */
-    *pDst++ = __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 */
-    /* Subtract and then store the result 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 BasicSub group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c

@@ -1,174 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_conj_f32.c    
-*    
-* Description:	Floating-point complex conjugate.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @defgroup cmplx_conj Complex Conjugate        
- *        
- * Conjugates the elements of a complex data vector.        
- *       
- * The <code>pSrc</code> points to the source data and        
- * <code>pDst</code> points to the where the result should be written.        
- * <code>numSamples</code> specifies the number of complex samples        
- * and the data in each array is stored in an interleaved fashion        
- * (real, imag, real, imag, ...).        
- * Each array has a total of <code>2*numSamples</code> values.        
- * The underlying algorithm is used:        
- *        
- * <pre>        
- * for(n=0; n<numSamples; n++) {        
- *     pDst[(2*n)+0)] = pSrc[(2*n)+0];     // real part        
- *     pDst[(2*n)+1)] = -pSrc[(2*n)+1];    // imag part        
- * }        
- * </pre>        
- *        
- * There are separate functions for floating-point, Q15, and Q31 data types.        
- */
-
-/**        
- * @addtogroup cmplx_conj        
- * @{        
- */
-
-/**        
- * @brief  Floating-point complex conjugate.        
- * @param  *pSrc points to the input vector        
- * @param  *pDst points to the output vector        
- * @param  numSamples number of complex samples in each vector        
- * @return none.        
- */
-
-void arm_cmplx_conj_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t numSamples)
-{
-  uint32_t blkCnt;                               /* loop counter */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  float32_t inR1, inR2, inR3, inR4;
-  float32_t inI1, inI2, inI3, inI4;
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0]+jC[1] = A[0]+ j (-1) A[1] */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    /* read real input samples */
-    inR1 = pSrc[0];
-    /* store real samples to destination */
-    pDst[0] = inR1;
-    inR2 = pSrc[2];
-    pDst[2] = inR2;
-    inR3 = pSrc[4];
-    pDst[4] = inR3;
-    inR4 = pSrc[6];
-    pDst[6] = inR4;
-
-    /* read imaginary input samples */
-    inI1 = pSrc[1];
-    inI2 = pSrc[3];
-
-    /* conjugate input */
-    inI1 = -inI1;
-
-    /* read imaginary input samples */
-    inI3 = pSrc[5];
-
-    /* conjugate input */
-    inI2 = -inI2;
-
-    /* read imaginary input samples */
-    inI4 = pSrc[7];
-
-    /* conjugate input */
-    inI3 = -inI3;
-
-    /* store imaginary samples to destination */
-    pDst[1] = inI1;
-    pDst[3] = inI2;
-
-    /* conjugate input */
-    inI4 = -inI4;
-
-    /* store imaginary samples to destination */
-    pDst[5] = inI3;
-
-    /* increment source pointer by 8 to process next sampels */
-    pSrc += 8u;
-
-    /* store imaginary sample to destination */
-    pDst[7] = inI4;
-
-    /* increment destination pointer by 8 to store next samples */
-    pDst += 8u;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  blkCnt = numSamples;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* realOut + j (imagOut) = realIn + j (-1) imagIn */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    *pDst++ = *pSrc++;
-    *pDst++ = -*pSrc++;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of cmplx_conj group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c

@@ -1,153 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_conj_q15.c    
-*    
-* Description:	Q15 complex conjugate.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_conj    
- * @{    
- */
-
-/**    
- * @brief  Q15 complex conjugate.    
- * @param  *pSrc points to the input vector    
- * @param  *pDst points to the output vector    
- * @param  numSamples number of complex 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_cmplx_conj_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples)
-{
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-  q31_t in1, in2, in3, in4;
-  q31_t zero = 0;
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0]+jC[1] = A[0]+ j (-1) A[1] */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    in1 = *__SIMD32(pSrc)++;
-    in2 = *__SIMD32(pSrc)++;
-    in3 = *__SIMD32(pSrc)++;
-    in4 = *__SIMD32(pSrc)++;
-
-#ifndef ARM_MATH_BIG_ENDIAN
-
-    in1 = __QASX(zero, in1);
-    in2 = __QASX(zero, in2);
-    in3 = __QASX(zero, in3);
-    in4 = __QASX(zero, in4);
-
-#else
-
-    in1 = __QSAX(zero, in1);
-    in2 = __QSAX(zero, in2);
-    in3 = __QSAX(zero, in3);
-    in4 = __QSAX(zero, in4);
-
-#endif //       #ifndef ARM_MATH_BIG_ENDIAN
-
-    in1 = ((uint32_t) in1 >> 16) | ((uint32_t) in1 << 16);
-    in2 = ((uint32_t) in2 >> 16) | ((uint32_t) in2 << 16);
-    in3 = ((uint32_t) in3 >> 16) | ((uint32_t) in3 << 16);
-    in4 = ((uint32_t) in4 >> 16) | ((uint32_t) in4 << 16);
-
-    *__SIMD32(pDst)++ = in1;
-    *__SIMD32(pDst)++ = in2;
-    *__SIMD32(pDst)++ = in3;
-    *__SIMD32(pDst)++ = in4;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    *pDst++ = *pSrc++;
-    *pDst++ = __SSAT(-*pSrc++, 16);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  q15_t in;
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* realOut + j (imagOut) = realIn+ j (-1) imagIn */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    *pDst++ = *pSrc++;
-    in = *pSrc++;
-    *pDst++ = (in == (q15_t) 0x8000) ? 0x7fff : -in;
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of cmplx_conj group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c

@@ -1,172 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_conj_q31.c    
-*    
-* Description:	Q31 complex conjugate.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @addtogroup cmplx_conj        
- * @{        
- */
-
-/**        
- * @brief  Q31 complex conjugate.        
- * @param  *pSrc points to the input vector        
- * @param  *pDst points to the output vector        
- * @param  numSamples number of complex 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_cmplx_conj_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples)
-{
-  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 inR1, inR2, inR3, inR4;                  /* Temporary real variables */
-  q31_t inI1, inI2, inI3, inI4;                  /* Temporary imaginary variables */
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0]+jC[1] = A[0]+ j (-1) A[1] */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    /* Saturated to 0x7fffffff if the input is -1(0x80000000) */
-    /* read real input sample */
-    inR1 = pSrc[0];
-    /* store real input sample */
-    pDst[0] = inR1;
-
-    /* read imaginary input sample */
-    inI1 = pSrc[1];
-
-    /* read real input sample */
-    inR2 = pSrc[2];
-    /* store real input sample */
-    pDst[2] = inR2;
-
-    /* read imaginary input sample */
-    inI2 = pSrc[3];
-
-    /* negate imaginary input sample */
-    inI1 = __QSUB(0, inI1);
-
-    /* read real input sample */
-    inR3 = pSrc[4];
-    /* store real input sample */
-    pDst[4] = inR3;
-
-    /* read imaginary input sample */
-    inI3 = pSrc[5];
-
-    /* negate imaginary input sample */
-    inI2 = __QSUB(0, inI2);
-
-    /* read real input sample */
-    inR4 = pSrc[6];
-    /* store real input sample */
-    pDst[6] = inR4;
-
-    /* negate imaginary input sample */
-    inI3 = __QSUB(0, inI3);
-
-    /* store imaginary input sample */
-    inI4 = pSrc[7];
-
-    /* store imaginary input samples */
-    pDst[1] = inI1;
-
-    /* negate imaginary input sample */
-    inI4 = __QSUB(0, inI4);
-
-    /* store imaginary input samples */
-    pDst[3] = inI2;
-
-    /* increment source pointer by 8 to proecess next samples */
-    pSrc += 8u;
-
-    /* store imaginary input samples */
-    pDst[5] = inI3;
-    pDst[7] = inI4;
-
-    /* increment destination pointer by 8 to process next samples */
-    pDst += 8u;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  blkCnt = numSamples;
-
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C[0]+jC[1] = A[0]+ j (-1) A[1] */
-    /* Calculate Complex Conjugate and then store the results in the destination buffer. */
-    /* Saturated to 0x7fffffff if the input is -1(0x80000000) */
-    *pDst++ = *pSrc++;
-    in = *pSrc++;
-    *pDst++ = (in == 0x80000000) ? 0x7fffffff : -in;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of cmplx_conj group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c

@@ -1,160 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_dot_prod_f32.c    
-*    
-* Description:	Floating-point complex 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @defgroup cmplx_dot_prod Complex Dot Product    
- *    
- * Computes the dot product of two complex vectors.    
- * The vectors are multiplied element-by-element and then summed.    
- *   
- * The <code>pSrcA</code> points to the first complex input vector and    
- * <code>pSrcB</code> points to the second complex input vector.    
- * <code>numSamples</code> specifies the number of complex samples    
- * and the data in each array is stored in an interleaved fashion    
- * (real, imag, real, imag, ...).    
- * Each array has a total of <code>2*numSamples</code> values.    
- *    
- * The underlying algorithm is used:    
- * <pre>    
- * realResult=0;    
- * imagResult=0;    
- * for(n=0; n<numSamples; n++) {    
- *     realResult += pSrcA[(2*n)+0]*pSrcB[(2*n)+0] - pSrcA[(2*n)+1]*pSrcB[(2*n)+1];    
- *     imagResult += pSrcA[(2*n)+0]*pSrcB[(2*n)+1] + pSrcA[(2*n)+1]*pSrcB[(2*n)+0];    
- * }    
- * </pre>    
- *    
- * There are separate functions for floating-point, Q15, and Q31 data types.    
- */
-
-/**    
- * @addtogroup cmplx_dot_prod    
- * @{    
- */
-
-/**    
- * @brief  Floating-point complex dot product    
- * @param  *pSrcA points to the first input vector    
- * @param  *pSrcB points to the second input vector    
- * @param  numSamples number of complex samples in each vector    
- * @param  *realResult real part of the result returned here    
- * @param  *imagResult imaginary part of the result returned here    
- * @return none.    
- */
-
-void arm_cmplx_dot_prod_f32(
-  float32_t * pSrcA,
-  float32_t * pSrcB,
-  uint32_t numSamples,
-  float32_t * realResult,
-  float32_t * imagResult)
-{
-  float32_t real_sum = 0.0f, imag_sum = 0.0f;    /* Temporary result storage */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += (*pSrcA++) * (*pSrcB++);
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += (*pSrcA++) * (*pSrcB++);
-
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  /* Store the real and imaginary results in the destination buffers */
-  *realResult = real_sum;
-  *imagResult = imag_sum;
-}
-
-/**    
- * @} end of cmplx_dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c

@@ -1,144 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_dot_prod_q15.c    
-*    
-* Description:	Processing function for the Q15 Complex 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_dot_prod    
- * @{    
- */
-
-/**    
- * @brief  Q15 complex dot product    
- * @param  *pSrcA points to the first input vector    
- * @param  *pSrcB points to the second input vector    
- * @param  numSamples number of complex samples in each vector    
- * @param  *realResult real part of the result returned here    
- * @param  *imagResult imaginary part of the result returned here    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function is implemented using an internal 64-bit accumulator.    
- * The intermediate 1.15 by 1.15 multiplications are performed with full precision and yield a 2.30 result.    
- * These are accumulated in a 64-bit accumulator with 34.30 precision.    
- * As a final step, the accumulators are converted to 8.24 format.    
- * The return results <code>realResult</code> and <code>imagResult</code> are in 8.24 format.    
- */
-
-void arm_cmplx_dot_prod_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  uint32_t numSamples,
-  q31_t * realResult,
-  q31_t * imagResult)
-{
-  q63_t real_sum = 0, imag_sum = 0;              /* Temporary result storage */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += ((q31_t) * pSrcA++ * *pSrcB++);
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  /* Store the real and imaginary results in 8.24 format  */
-  /* Convert real data in 34.30 to 8.24 by 6 right shifts */
-  *realResult = (q31_t) (real_sum) >> 6;
-  /* Convert imaginary data in 34.30 to 8.24 by 6 right shifts */
-  *imagResult = (q31_t) (imag_sum) >> 6;
-}
-
-/**    
- * @} end of cmplx_dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c

@@ -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_cmplx_dot_prod_q31.c    
-*    
-* Description:	Q31 complex 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_dot_prod    
- * @{    
- */
-
-/**    
- * @brief  Q31 complex dot product    
- * @param  *pSrcA points to the first input vector    
- * @param  *pSrcB points to the second input vector    
- * @param  numSamples number of complex samples in each vector    
- * @param  *realResult real part of the result returned here    
- * @param  *imagResult imaginary part of the result returned here    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function is implemented using an internal 64-bit accumulator.    
- * The intermediate 1.31 by 1.31 multiplications are performed with 64-bit precision and then shifted to 16.48 format.    
- * The internal real and imaginary accumulators are in 16.48 format and provide 15 guard bits.    
- * Additions are nonsaturating and no overflow will occur as long as <code>numSamples</code> is less than 32768.    
- * The return results <code>realResult</code> and <code>imagResult</code> are in 16.48 format.    
- * Input down scaling is not required.    
- */
-
-void arm_cmplx_dot_prod_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  uint32_t numSamples,
-  q63_t * realResult,
-  q63_t * imagResult)
-{
-  q63_t real_sum = 0, imag_sum = 0;              /* Temporary result storage */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    /* Convert real data in 2.62 to 16.48 by 14 right shifts */
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    /* Convert imag data in 2.62 to 16.48 by 14 right shifts */
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples  is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* CReal = A[0]* B[0] + A[2]* B[2] + A[4]* B[4] + .....+ A[numSamples-2]* B[numSamples-2] */
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    /* CImag = A[1]* B[1] + A[3]* B[3] + A[5]* B[5] + .....+ A[numSamples-1]* B[numSamples-1] */
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* outReal = realA[0]* realB[0] + realA[2]* realB[2] + realA[4]* realB[4] + .....+ realA[numSamples-2]* realB[numSamples-2] */
-    real_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-    /* outImag = imagA[1]* imagB[1] + imagA[3]* imagB[3] + imagA[5]* imagB[5] + .....+ imagA[numSamples-1]* imagB[numSamples-1] */
-    imag_sum += (q63_t) * pSrcA++ * (*pSrcB++) >> 14;
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  /* Store the real and imaginary results in 16.48 format  */
-  *realResult = real_sum;
-  *imagResult = imag_sum;
-}
-
-/**    
- * @} end of cmplx_dot_prod group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c

@@ -1,157 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_mag_f32.c    
-*    
-* Description:	Floating-point complex magnitude.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @defgroup cmplx_mag Complex Magnitude    
- *    
- * Computes the magnitude of the elements of a complex data vector.    
- *   
- * The <code>pSrc</code> points to the source data and    
- * <code>pDst</code> points to the where the result should be written.    
- * <code>numSamples</code> specifies the number of complex samples    
- * in the input array and the data is stored in an interleaved fashion    
- * (real, imag, real, imag, ...).    
- * The input array has a total of <code>2*numSamples</code> values;    
- * the output array has a total of <code>numSamples</code> values.    
- * The underlying algorithm is used:    
- *    
- * <pre>    
- * for(n=0; n<numSamples; n++) {    
- *     pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2);    
- * }    
- * </pre>    
- *    
- * There are separate functions for floating-point, Q15, and Q31 data types.    
- */
-
-/**    
- * @addtogroup cmplx_mag    
- * @{    
- */
-/**    
- * @brief Floating-point complex magnitude.    
- * @param[in]       *pSrc points to complex input buffer    
- * @param[out]      *pDst points to real output buffer    
- * @param[in]       numSamples number of complex samples in the input vector    
- * @return none.    
- *    
- */
-
-
-void arm_cmplx_mag_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t numSamples)
-{
-  float32_t realIn, imagIn;                      /* Temporary variables to hold input values */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    /* store the result in the destination buffer. */
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    /* store the result in the destination buffer. */
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* out = sqrt((real * real) + (imag * imag)) */
-    realIn = *pSrc++;
-    imagIn = *pSrc++;
-    /* store the result in the destination buffer. */
-    arm_sqrt_f32((realIn * realIn) + (imagIn * imagIn), pDst++);
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of cmplx_mag group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c

@@ -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_cmplx_mag_q15.c    
-*    
-* Description:	Q15 complex magnitude.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_mag    
- * @{    
- */
-
-
-/**    
- * @brief  Q15 complex magnitude    
- * @param  *pSrc points to the complex input vector    
- * @param  *pDst points to the real output vector    
- * @param  numSamples number of complex samples in the input vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function implements 1.15 by 1.15 multiplications and finally output is converted into 2.14 format.    
- */
-
-void arm_cmplx_mag_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples)
-{
-  q31_t acc0, acc1;                              /* Accumulators */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-  q31_t in1, in2, in3, in4;
-  q31_t acc2, acc3;
-
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
-    in1 = *__SIMD32(pSrc)++;
-    in2 = *__SIMD32(pSrc)++;
-    in3 = *__SIMD32(pSrc)++;
-    in4 = *__SIMD32(pSrc)++;
-
-    acc0 = __SMUAD(in1, in1);
-    acc1 = __SMUAD(in2, in2);
-    acc2 = __SMUAD(in3, in3);
-    acc3 = __SMUAD(in4, in4);
-
-    /* store the result in 2.14 format in the destination buffer. */
-    arm_sqrt_q15((q15_t) ((acc0) >> 17), pDst++);
-    arm_sqrt_q15((q15_t) ((acc1) >> 17), pDst++);
-    arm_sqrt_q15((q15_t) ((acc2) >> 17), pDst++);
-    arm_sqrt_q15((q15_t) ((acc3) >> 17), pDst++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
-    in1 = *__SIMD32(pSrc)++;
-    acc0 = __SMUAD(in1, in1);
-
-    /* store the result in 2.14 format in the destination buffer. */
-    arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  q15_t real, imag;                              /* Temporary variables to hold input values */
-
-  while(numSamples > 0u)
-  {
-    /* out = sqrt(real * real + imag * imag) */
-    real = *pSrc++;
-    imag = *pSrc++;
-
-    acc0 = (real * real);
-    acc1 = (imag * imag);
-
-    /* store the result in 2.14 format in the destination buffer. */
-    arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++);
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of cmplx_mag group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c

@@ -1,177 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_mag_q31.c    
-*    
-* Description:	Q31 complex magnitude    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @addtogroup cmplx_mag        
- * @{        
- */
-
-/**        
- * @brief  Q31 complex magnitude        
- * @param  *pSrc points to the complex input vector        
- * @param  *pDst points to the real output vector        
- * @param  numSamples number of complex samples in the input vector        
- * @return none.        
- *        
- * <b>Scaling and Overflow Behavior:</b>        
- * \par        
- * The function implements 1.31 by 1.31 multiplications and finally output is converted into 2.30 format.        
- * Input down scaling is not required.        
- */
-
-void arm_cmplx_mag_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples)
-{
-  q31_t real, imag;                              /* Temporary variables to hold input values */
-  q31_t acc0, acc1;                              /* Accumulators */
-  uint32_t blkCnt;                               /* loop counter */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  q31_t real1, real2, imag1, imag2;              /* Temporary variables to hold input values */
-  q31_t out1, out2, out3, out4;                  /* Accumulators */
-  q63_t mul1, mul2, mul3, mul4;                  /* Temporary variables */
-
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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)
-  {
-    /* read complex input from source buffer */
-    real1 = pSrc[0];
-    imag1 = pSrc[1];
-    real2 = pSrc[2];
-    imag2 = pSrc[3];
-
-    /* calculate power of input values */
-    mul1 = (q63_t) real1 *real1;
-    mul2 = (q63_t) imag1 *imag1;
-    mul3 = (q63_t) real2 *real2;
-    mul4 = (q63_t) imag2 *imag2;
-
-    /* get the result to 3.29 format */
-    out1 = (q31_t) (mul1 >> 33);
-    out2 = (q31_t) (mul2 >> 33);
-    out3 = (q31_t) (mul3 >> 33);
-    out4 = (q31_t) (mul4 >> 33);
-
-    /* add real and imaginary accumulators */
-    out1 = out1 + out2;
-    out3 = out3 + out4;
-
-    /* read complex input from source buffer */
-    real1 = pSrc[4];
-    imag1 = pSrc[5];
-    real2 = pSrc[6];
-    imag2 = pSrc[7];
-
-    /* calculate square root */
-    arm_sqrt_q31(out1, &pDst[0]);
-
-    /* calculate power of input values */
-    mul1 = (q63_t) real1 *real1;
-
-    /* calculate square root */
-    arm_sqrt_q31(out3, &pDst[1]);
-
-    /* calculate power of input values */
-    mul2 = (q63_t) imag1 *imag1;
-    mul3 = (q63_t) real2 *real2;
-    mul4 = (q63_t) imag2 *imag2;
-
-    /* get the result to 3.29 format */
-    out1 = (q31_t) (mul1 >> 33);
-    out2 = (q31_t) (mul2 >> 33);
-    out3 = (q31_t) (mul3 >> 33);
-    out4 = (q31_t) (mul4 >> 33);
-
-    /* add real and imaginary accumulators */
-    out1 = out1 + out2;
-    out3 = out3 + out4;
-
-    /* calculate square root */
-    arm_sqrt_q31(out1, &pDst[2]);
-
-    /* increment destination by 8 to process next samples */
-    pSrc += 8u;
-
-    /* calculate square root */
-    arm_sqrt_q31(out3, &pDst[3]);
-
-    /* increment destination by 4 to process next samples */
-    pDst += 4u;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  blkCnt = numSamples;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 2.30 format in the destination buffer. */
-    arm_sqrt_q31(acc0 + acc1, pDst++);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of cmplx_mag group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c

@@ -1,207 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_mag_squared_f32.c    
-*    
-* Description:	Floating-point complex magnitude squared.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @defgroup cmplx_mag_squared Complex Magnitude Squared        
- *        
- * Computes the magnitude squared of the elements of a complex data vector.        
- *       
- * The <code>pSrc</code> points to the source data and        
- * <code>pDst</code> points to the where the result should be written.        
- * <code>numSamples</code> specifies the number of complex samples        
- * in the input array and the data is stored in an interleaved fashion        
- * (real, imag, real, imag, ...).        
- * The input array has a total of <code>2*numSamples</code> values;        
- * the output array has a total of <code>numSamples</code> values.        
- *        
- * The underlying algorithm is used:        
- *        
- * <pre>        
- * for(n=0; n<numSamples; n++) {        
- *     pDst[n] = pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2;        
- * }        
- * </pre>        
- *        
- * There are separate functions for floating-point, Q15, and Q31 data types.        
- */
-
-/**        
- * @addtogroup cmplx_mag_squared        
- * @{        
- */
-
-
-/**        
- * @brief  Floating-point complex magnitude squared        
- * @param[in]  *pSrc points to the complex input vector        
- * @param[out]  *pDst points to the real output vector        
- * @param[in]  numSamples number of complex samples in the input vector        
- * @return none.        
- */
-
-void arm_cmplx_mag_squared_f32(
-  float32_t * pSrc,
-  float32_t * pDst,
-  uint32_t numSamples)
-{
-  float32_t real, imag;                          /* Temporary variables to store real and imaginary values */
-  uint32_t blkCnt;                               /* loop counter */
-
-#ifndef ARM_MATH_CM0
-  float32_t real1, real2, real3, real4;          /* Temporary variables to hold real values */
-  float32_t imag1, imag2, imag3, imag4;          /* Temporary variables to hold imaginary values */
-  float32_t mul1, mul2, mul3, mul4;              /* Temporary variables */
-  float32_t mul5, mul6, mul7, mul8;              /* Temporary variables */
-  float32_t out1, out2, out3, out4;              /* Temporary variables to hold output values */
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0] = (A[0] * A[0] + A[1] * A[1]) */
-    /* read real input sample from source buffer */
-    real1 = pSrc[0];
-    /* read imaginary input sample from source buffer */
-    imag1 = pSrc[1];
-
-    /* calculate power of real value */
-    mul1 = real1 * real1;
-
-    /* read real input sample from source buffer */
-    real2 = pSrc[2];
-
-    /* calculate power of imaginary value */
-    mul2 = imag1 * imag1;
-
-    /* read imaginary input sample from source buffer */
-    imag2 = pSrc[3];
-
-    /* calculate power of real value */
-    mul3 = real2 * real2;
-
-    /* read real input sample from source buffer */
-    real3 = pSrc[4];
-
-    /* calculate power of imaginary value */
-    mul4 = imag2 * imag2;
-
-    /* read imaginary input sample from source buffer */
-    imag3 = pSrc[5];
-
-    /* calculate power of real value */
-    mul5 = real3 * real3;
-    /* calculate power of imaginary value */
-    mul6 = imag3 * imag3;
-
-    /* read real input sample from source buffer */
-    real4 = pSrc[6];
-
-    /* accumulate real and imaginary powers */
-    out1 = mul1 + mul2;
-
-    /* read imaginary input sample from source buffer */
-    imag4 = pSrc[7];
-
-    /* accumulate real and imaginary powers */
-    out2 = mul3 + mul4;
-
-    /* calculate power of real value */
-    mul7 = real4 * real4;
-    /* calculate power of imaginary value */
-    mul8 = imag4 * imag4;
-
-    /* store output to destination */
-    pDst[0] = out1;
-
-    /* accumulate real and imaginary powers */
-    out3 = mul5 + mul6;
-
-    /* store output to destination */
-    pDst[1] = out2;
-
-    /* accumulate real and imaginary powers */
-    out4 = mul7 + mul8;
-
-    /* store output to destination */
-    pDst[2] = out3;
-
-    /* increment destination pointer by 8 to process next samples */
-    pSrc += 8u;
-
-    /* store output to destination */
-    pDst[3] = out4;
-
-    /* increment destination pointer by 4 to process next samples */
-    pDst += 4u;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  blkCnt = numSamples;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
-    real = *pSrc++;
-    imag = *pSrc++;
-
-    /* out = (real * real) + (imag * imag) */
-    /* store the result in the destination buffer. */
-    *pDst++ = (real * real) + (imag * imag);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of cmplx_mag_squared group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c

@@ -1,140 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_mag_squared_q15.c    
-*    
-* Description:	Q15 complex magnitude squared.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_mag_squared    
- * @{    
- */
-
-/**    
- * @brief  Q15 complex magnitude squared    
- * @param  *pSrc points to the complex input vector    
- * @param  *pDst points to the real output vector    
- * @param  numSamples number of complex samples in the input vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function implements 1.15 by 1.15 multiplications and finally output is converted into 3.13 format.    
- */
-
-void arm_cmplx_mag_squared_q15(
-  q15_t * pSrc,
-  q15_t * pDst,
-  uint32_t numSamples)
-{
-  q31_t acc0, acc1;                              /* Accumulators */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-  q31_t in1, in2, in3, in4;
-  q31_t acc2, acc3;
-
-  /*loop Unrolling */
-  blkCnt = numSamples >> 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[0] = (A[0] * A[0] + A[1] * A[1]) */
-    in1 = *__SIMD32(pSrc)++;
-    in2 = *__SIMD32(pSrc)++;
-    in3 = *__SIMD32(pSrc)++;
-    in4 = *__SIMD32(pSrc)++;
-
-    acc0 = __SMUAD(in1, in1);
-    acc1 = __SMUAD(in2, in2);
-    acc2 = __SMUAD(in3, in3);
-    acc3 = __SMUAD(in4, in4);
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ = (q15_t) (acc0 >> 17);
-    *pDst++ = (q15_t) (acc1 >> 17);
-    *pDst++ = (q15_t) (acc2 >> 17);
-    *pDst++ = (q15_t) (acc3 >> 17);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
-    in1 = *__SIMD32(pSrc)++;
-    acc0 = __SMUAD(in1, in1);
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ = (q15_t) (acc0 >> 17);
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  q15_t real, imag;                              /* Temporary variables to store real and imaginary values */
-
-  while(numSamples > 0u)
-  {
-    /* out = ((real * real) + (imag * imag)) */
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (real * real);
-    acc1 = (imag * imag);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17);
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of cmplx_mag_squared group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c

@@ -1,153 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:		arm_cmplx_mag_squared_q31.c    
-*    
-* Description:	Q31 complex magnitude squared.    
-*    
-* 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.    
-* ---------------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup cmplx_mag_squared    
- * @{    
- */
-
-
-/**    
- * @brief  Q31 complex magnitude squared    
- * @param  *pSrc points to the complex input vector    
- * @param  *pDst points to the real output vector    
- * @param  numSamples number of complex samples in the input vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function implements 1.31 by 1.31 multiplications and finally output is converted into 3.29 format.    
- * Input down scaling is not required.    
- */
-
-void arm_cmplx_mag_squared_q31(
-  q31_t * pSrc,
-  q31_t * pDst,
-  uint32_t numSamples)
-{
-  q31_t real, imag;                              /* Temporary variables to store real and imaginary values */
-  q31_t acc0, acc1;                              /* Accumulators */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counter */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[0] = (A[0] * A[0] + A[1] * A[1]) */
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[0] = (A[0] * A[0] + A[1] * A[1]) */
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    /* Decrement the loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* out = ((real * real) + (imag * imag)) */
-    real = *pSrc++;
-    imag = *pSrc++;
-    acc0 = (q31_t) (((q63_t) real * real) >> 33);
-    acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
-    /* store the result in 3.29 format in the destination buffer. */
-    *pDst++ = acc0 + acc1;
-
-    /* Decrement the loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of cmplx_mag_squared group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c

@@ -1,199 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_cmplx_f32.c    
-*    
-* Description:	Floating-point complex-by-complex multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @defgroup CmplxByCmplxMult Complex-by-Complex Multiplication        
- *        
- * Multiplies a complex vector by another complex vector and generates a complex result.        
- * The data in the complex arrays is stored in an interleaved fashion        
- * (real, imag, real, imag, ...).        
- * The parameter <code>numSamples</code> represents the number of complex        
- * samples processed.  The complex arrays have a total of <code>2*numSamples</code>        
- * real values.        
- *        
- * The underlying algorithm is used:        
- *        
- * <pre>        
- * for(n=0; n<numSamples; n++) {        
- *     pDst[(2*n)+0] = pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1];        
- *     pDst[(2*n)+1] = pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0];        
- * }        
- * </pre>        
- *        
- * There are separate functions for floating-point, Q15, and Q31 data types.        
- */
-
-/**        
- * @addtogroup CmplxByCmplxMult        
- * @{        
- */
-
-
-/**        
- * @brief  Floating-point complex-by-complex multiplication        
- * @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]  numSamples number of complex samples in each vector        
- * @return none.        
- */
-
-void arm_cmplx_mult_cmplx_f32(
-  float32_t * pSrcA,
-  float32_t * pSrcB,
-  float32_t * pDst,
-  uint32_t numSamples)
-{
-  float32_t a1, b1, c1, d1;                      /* Temporary variables to store real and imaginary values */
-  uint32_t blkCnt;                               /* loop counters */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  float32_t a2, b2, c2, d2;                      /* Temporary variables to store real and imaginary values */
-  float32_t acc1, acc2, acc3, acc4;
-
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a1 = *pSrcA;                /* A[2 * i] */
-    c1 = *pSrcB;                /* B[2 * i] */
-
-    b1 = *(pSrcA + 1);          /* A[2 * i + 1] */
-    acc1 = a1 * c1;             /* acc1 = A[2 * i] * B[2 * i] */
-
-    a2 = *(pSrcA + 2);          /* A[2 * i + 2] */
-    acc2 = (b1 * c1);           /* acc2 = A[2 * i + 1] * B[2 * i] */
-
-    d1 = *(pSrcB + 1);          /* B[2 * i + 1] */
-    c2 = *(pSrcB + 2);          /* B[2 * i + 2] */
-    acc1 -= b1 * d1;            /* acc1 =      A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1] */
-
-    d2 = *(pSrcB + 3);          /* B[2 * i + 3] */
-    acc3 = a2 * c2;             /* acc3 =       A[2 * i + 2] * B[2 * i + 2] */
-
-    b2 = *(pSrcA + 3);          /* A[2 * i + 3] */
-    acc2 += (a1 * d1);          /* acc2 =      A[2 * i + 1] * B[2 * i] + A[2 * i] * B[2 * i + 1] */
-
-    a1 = *(pSrcA + 4);          /* A[2 * i + 4] */
-    acc4 = (a2 * d2);           /* acc4 =   A[2 * i + 2] * B[2 * i + 3] */
-
-    c1 = *(pSrcB + 4);          /* B[2 * i + 4] */
-    acc3 -= (b2 * d2);          /* acc3 =       A[2 * i + 2] * B[2 * i + 2] - A[2 * i + 3] * B[2 * i + 3] */
-    *pDst = acc1;               /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1] */
-
-    b1 = *(pSrcA + 5);          /* A[2 * i + 5] */
-    acc4 += b2 * c2;            /* acc4 =   A[2 * i + 2] * B[2 * i + 3] + A[2 * i + 3] * B[2 * i + 2] */
-
-    *(pDst + 1) = acc2;         /* C[2 * i + 1] = A[2 * i + 1] * B[2 * i] + A[2 * i] * B[2 * i + 1]  */
-    acc1 = (a1 * c1);
-
-    d1 = *(pSrcB + 5);
-    acc2 = (b1 * c1);
-
-    *(pDst + 2) = acc3;
-    *(pDst + 3) = acc4;
-
-    a2 = *(pSrcA + 6);
-    acc1 -= (b1 * d1);
-
-    c2 = *(pSrcB + 6);
-    acc2 += (a1 * d1);
-
-    b2 = *(pSrcA + 7);
-    acc3 = (a2 * c2);
-
-    d2 = *(pSrcB + 7);
-    acc4 = (b2 * c2);
-
-    *(pDst + 4) = acc1;
-    pSrcA += 8u;
-
-    acc3 -= (b2 * d2);
-    acc4 += (a2 * d2);
-
-    *(pDst + 5) = acc2;
-    pSrcB += 8u;
-
-    *(pDst + 6) = acc3;
-    *(pDst + 7) = acc4;
-
-    pDst += 8u;
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  blkCnt = numSamples;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a1 = *pSrcA++;
-    b1 = *pSrcA++;
-    c1 = *pSrcB++;
-    d1 = *pSrcB++;
-
-    /* store the result in the destination buffer. */
-    *pDst++ = (a1 * c1) - (b1 * d1);
-    *pDst++ = (a1 * d1) + (b1 * c1);
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of CmplxByCmplxMult group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c

@@ -1,185 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_cmplx_q15.c    
-*    
-* Description:	Q15 complex-by-complex multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup CmplxByCmplxMult    
- * @{    
- */
-
-/**    
- * @brief  Q15 complex-by-complex multiplication    
- * @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]  numSamples number of complex samples in each vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function implements 1.15 by 1.15 multiplications and finally output is converted into 3.13 format.    
- */
-
-void arm_cmplx_mult_cmplx_q15(
-  q15_t * pSrcA,
-  q15_t * pSrcB,
-  q15_t * pDst,
-  uint32_t numSamples)
-{
-  q15_t a, b, c, d;                              /* Temporary variables to store real and imaginary values */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counters */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17);
-    /* store the result in 3.13 format in the destination buffer. */
-    *pDst++ =
-      (q15_t) (q31_t) (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17);
-
-    /* Decrement the blockSize loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of CmplxByCmplxMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c

@@ -1,318 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_cmplx_q31.c    
-*    
-* Description:	Q31 complex-by-complex multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup CmplxByCmplxMult    
- * @{    
- */
-
-
-/**    
- * @brief  Q31 complex-by-complex multiplication    
- * @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]  numSamples number of complex samples in each vector    
- * @return none.    
- *    
- * <b>Scaling and Overflow Behavior:</b>    
- * \par    
- * The function implements 1.31 by 1.31 multiplications and finally output is converted into 3.29 format.    
- * Input down scaling is not required.    
- */
-
-void arm_cmplx_mult_cmplx_q31(
-  q31_t * pSrcA,
-  q31_t * pSrcB,
-  q31_t * pDst,
-  uint32_t numSamples)
-{
-  q31_t a, b, c, d;                              /* Temporary variables to store real and imaginary values */
-  uint32_t blkCnt;                               /* loop counters */
-  q31_t mul1, mul2, mul3, mul4;
-  q31_t out1, out2;
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-  /* If the blockSize is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 1u;
-
-  /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.     
-   ** a second loop below computes the remaining 1 sample. */
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-  /* If the blockSize is not a multiple of 2, compute any remaining output samples here.     
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x2u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1].  */
-    /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i].  */
-    a = *pSrcA++;
-    b = *pSrcA++;
-    c = *pSrcB++;
-    d = *pSrcB++;
-
-    mul1 = (q31_t) (((q63_t) a * c) >> 32);
-    mul2 = (q31_t) (((q63_t) b * d) >> 32);
-    mul3 = (q31_t) (((q63_t) a * d) >> 32);
-    mul4 = (q31_t) (((q63_t) b * c) >> 32);
-
-    mul1 = (mul1 >> 1);
-    mul2 = (mul2 >> 1);
-    mul3 = (mul3 >> 1);
-    mul4 = (mul4 >> 1);
-
-    out1 = mul1 - mul2;
-    out2 = mul3 + mul4;
-
-    /* store the real result in 3.29 format in the destination buffer. */
-    *pDst++ = out1;
-    /* store the imag result in 3.29 format in the destination buffer. */
-    *pDst++ = out2;
-
-    /* Decrement the blockSize loop counter */
-    blkCnt--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of CmplxByCmplxMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c

@@ -1,217 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_real_f32.c    
-*    
-* Description:	Floating-point complex by real multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**        
- * @ingroup groupCmplxMath        
- */
-
-/**        
- * @defgroup CmplxByRealMult Complex-by-Real Multiplication        
- *        
- * Multiplies a complex vector by a real vector and generates a complex result.        
- * The data in the complex arrays is stored in an interleaved fashion        
- * (real, imag, real, imag, ...).        
- * The parameter <code>numSamples</code> represents the number of complex        
- * samples processed.  The complex arrays have a total of <code>2*numSamples</code>        
- * real values while the real array has a total of <code>numSamples</code>        
- * real values.        
- *        
- * The underlying algorithm is used:        
- *        
- * <pre>        
- * for(n=0; n<numSamples; n++) {        
- *     pCmplxDst[(2*n)+0] = pSrcCmplx[(2*n)+0] * pSrcReal[n];        
- *     pCmplxDst[(2*n)+1] = pSrcCmplx[(2*n)+1] * pSrcReal[n];        
- * }        
- * </pre>        
- *        
- * There are separate functions for floating-point, Q15, and Q31 data types.        
- */
-
-/**        
- * @addtogroup CmplxByRealMult        
- * @{        
- */
-
-
-/**        
- * @brief  Floating-point complex-by-real multiplication        
- * @param[in]  *pSrcCmplx points to the complex input vector        
- * @param[in]  *pSrcReal points to the real input vector        
- * @param[out]  *pCmplxDst points to the complex output vector        
- * @param[in]  numSamples number of samples in each vector        
- * @return none.        
- */
-
-void arm_cmplx_mult_real_f32(
-  float32_t * pSrcCmplx,
-  float32_t * pSrcReal,
-  float32_t * pCmplxDst,
-  uint32_t numSamples)
-{
-  float32_t in;                                  /* Temporary variable to store input value */
-  uint32_t blkCnt;                               /* loop counters */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  float32_t inA1, inA2, inA3, inA4;              /* Temporary variables to hold input data */
-  float32_t inA5, inA6, inA7, inA8;              /* Temporary variables to hold input data */
-  float32_t inB1, inB2, inB3, inB4;              /* Temporary variables to hold input data */
-  float32_t out1, out2, out3, out4;              /* Temporary variables to hold output data */
-  float32_t out5, out6, out7, out8;              /* Temporary variables to hold output data */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    /* read input from complex input buffer */
-    inA1 = pSrcCmplx[0];
-    inA2 = pSrcCmplx[1];
-    /* read input from real input buffer */
-    inB1 = pSrcReal[0];
-
-    /* read input from complex input buffer */
-    inA3 = pSrcCmplx[2];
-
-    /* multiply complex buffer real input with real buffer input */
-    out1 = inA1 * inB1;
-
-    /* read input from complex input buffer */
-    inA4 = pSrcCmplx[3];
-
-    /* multiply complex buffer imaginary input with real buffer input */
-    out2 = inA2 * inB1;
-
-    /* read input from real input buffer */
-    inB2 = pSrcReal[1];
-    /* read input from complex input buffer */
-    inA5 = pSrcCmplx[4];
-
-    /* multiply complex buffer real input with real buffer input */
-    out3 = inA3 * inB2;
-
-    /* read input from complex input buffer */
-    inA6 = pSrcCmplx[5];
-    /* read input from real input buffer */
-    inB3 = pSrcReal[2];
-
-    /* multiply complex buffer imaginary input with real buffer input */
-    out4 = inA4 * inB2;
-
-    /* read input from complex input buffer */
-    inA7 = pSrcCmplx[6];
-
-    /* multiply complex buffer real input with real buffer input */
-    out5 = inA5 * inB3;
-
-    /* read input from complex input buffer */
-    inA8 = pSrcCmplx[7];
-
-    /* multiply complex buffer imaginary input with real buffer input */
-    out6 = inA6 * inB3;
-
-    /* read input from real input buffer */
-    inB4 = pSrcReal[3];
-
-    /* store result to destination bufer */
-    pCmplxDst[0] = out1;
-
-    /* multiply complex buffer real input with real buffer input */
-    out7 = inA7 * inB4;
-
-    /* store result to destination bufer */
-    pCmplxDst[1] = out2;
-
-    /* multiply complex buffer imaginary input with real buffer input */
-    out8 = inA8 * inB4;
-
-    /* store result to destination bufer */
-    pCmplxDst[2] = out3;
-    pCmplxDst[3] = out4;
-    pCmplxDst[4] = out5;
-
-    /* incremnet complex input buffer by 8 to process next samples */
-    pSrcCmplx += 8u;
-
-    /* store result to destination bufer */
-    pCmplxDst[5] = out6;
-
-    /* increment real input buffer by 4 to process next samples */
-    pSrcReal += 4u;
-
-    /* store result to destination bufer */
-    pCmplxDst[6] = out7;
-    pCmplxDst[7] = out8;
-
-    /* increment destination buffer by 8 to process next sampels */
-    pCmplxDst += 8u;
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.        
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-  blkCnt = numSamples;
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    in = *pSrcReal++;
-    /* store the result in the destination buffer. */
-    *pCmplxDst++ = (*pSrcCmplx++) * (in);
-    *pCmplxDst++ = (*pSrcCmplx++) * (in);
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-}
-
-/**        
- * @} end of CmplxByRealMult group        
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c

@@ -1,195 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_real_q15.c    
-*    
-* Description:	Q15 complex by real multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup CmplxByRealMult    
- * @{    
- */
-
-
-/**    
- * @brief  Q15 complex-by-real multiplication    
- * @param[in]  *pSrcCmplx points to the complex input vector    
- * @param[in]  *pSrcReal points to the real input vector    
- * @param[out]  *pCmplxDst points to the complex output vector    
- * @param[in]  numSamples 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_cmplx_mult_real_q15(
-  q15_t * pSrcCmplx,
-  q15_t * pSrcReal,
-  q15_t * pCmplxDst,
-  uint32_t numSamples)
-{
-  q15_t in;                                      /* Temporary variable to store input value */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counters */
-  q31_t inA1, inA2;                              /* Temporary variables to hold input data */
-  q31_t inB1;                                    /* Temporary variables to hold input data */
-  q15_t out1, out2, out3, out4;                  /* Temporary variables to hold output data */
-  q31_t mul1, mul2, mul3, mul4;                  /* Temporary variables to hold intermediate data */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    /* read complex number both real and imaginary from complex input buffer */
-    inA1 = *__SIMD32(pSrcCmplx)++;
-    /* read two real values at a time from real input buffer */
-    inB1 = *__SIMD32(pSrcReal)++;
-    /* read complex number both real and imaginary from complex input buffer */
-    inA2 = *__SIMD32(pSrcCmplx)++;
-
-    /* multiply complex number with real numbers */
-#ifndef ARM_MATH_BIG_ENDIAN
-
-    mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1));
-    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1));
-    mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16));
-    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16));
-
-#else
-
-    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
-    mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16));
-    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1);
-    mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1);
-
-#endif //      #ifndef ARM_MATH_BIG_ENDIAN
-
-    /* saturate the result */
-    out1 = (q15_t) __SSAT(mul1 >> 15u, 16);
-    out2 = (q15_t) __SSAT(mul2 >> 15u, 16);
-    out3 = (q15_t) __SSAT(mul3 >> 15u, 16);
-    out4 = (q15_t) __SSAT(mul4 >> 15u, 16);
-
-    /* pack real and imaginary outputs and store them to destination */
-    *__SIMD32(pCmplxDst)++ = __PKHBT(out1, out2, 16);
-    *__SIMD32(pCmplxDst)++ = __PKHBT(out3, out4, 16);
-
-    inA1 = *__SIMD32(pSrcCmplx)++;
-    inB1 = *__SIMD32(pSrcReal)++;
-    inA2 = *__SIMD32(pSrcCmplx)++;
-
-#ifndef ARM_MATH_BIG_ENDIAN
-
-    mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1));
-    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1));
-    mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16));
-    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16));
-
-#else
-
-    mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
-    mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16));
-    mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1);
-    mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1);
-
-#endif //      #ifndef ARM_MATH_BIG_ENDIAN
-
-    out1 = (q15_t) __SSAT(mul1 >> 15u, 16);
-    out2 = (q15_t) __SSAT(mul2 >> 15u, 16);
-    out3 = (q15_t) __SSAT(mul3 >> 15u, 16);
-    out4 = (q15_t) __SSAT(mul4 >> 15u, 16);
-
-    *__SIMD32(pCmplxDst)++ = __PKHBT(out1, out2, 16);
-    *__SIMD32(pCmplxDst)++ = __PKHBT(out3, out4, 16);
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    in = *pSrcReal++;
-    /* store the result in the destination buffer. */
-    *pCmplxDst++ =
-      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
-    *pCmplxDst++ =
-      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* realOut = realA * realB.            */
-    /* imagOut = imagA * realB.                */
-    in = *pSrcReal++;
-    /* store the result in the destination buffer. */
-    *pCmplxDst++ =
-      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
-    *pCmplxDst++ =
-      (q15_t) __SSAT((((q31_t) (*pSrcCmplx++) * (in)) >> 15), 16);
-
-    /* Decrement the numSamples loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of CmplxByRealMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c

@@ -1,215 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_cmplx_mult_real_q31.c    
-*    
-* Description:	Q31 complex by real multiplication    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
-/**    
- * @ingroup groupCmplxMath    
- */
-
-/**    
- * @addtogroup CmplxByRealMult    
- * @{    
- */
-
-
-/**    
- * @brief  Q31 complex-by-real multiplication    
- * @param[in]  *pSrcCmplx points to the complex input vector    
- * @param[in]  *pSrcReal points to the real input vector    
- * @param[out]  *pCmplxDst points to the complex output vector    
- * @param[in]  numSamples 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_cmplx_mult_real_q31(
-  q31_t * pSrcCmplx,
-  q31_t * pSrcReal,
-  q31_t * pCmplxDst,
-  uint32_t numSamples)
-{
-  q31_t inA1;                                    /* Temporary variable to store input value */
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-  uint32_t blkCnt;                               /* loop counters */
-  q31_t inA2, inA3, inA4;                        /* Temporary variables to hold input data */
-  q31_t inB1, inB2;                              /* Temporary variabels to hold input data */
-  q31_t out1, out2, out3, out4;                  /* Temporary variables to hold output data */
-
-  /* loop Unrolling */
-  blkCnt = numSamples >> 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[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    /* read real input from complex input buffer */
-    inA1 = *pSrcCmplx++;
-    inA2 = *pSrcCmplx++;
-    /* read input from real input bufer */
-    inB1 = *pSrcReal++;
-    inB2 = *pSrcReal++;
-    /* read imaginary input from complex input buffer */
-    inA3 = *pSrcCmplx++;
-    inA4 = *pSrcCmplx++;
-
-    /* multiply complex input with real input */
-    out1 = ((q63_t) inA1 * inB1) >> 32;
-    out2 = ((q63_t) inA2 * inB1) >> 32;
-    out3 = ((q63_t) inA3 * inB2) >> 32;
-    out4 = ((q63_t) inA4 * inB2) >> 32;
-
-    /* sature the result */
-    out1 = __SSAT(out1, 31);
-    out2 = __SSAT(out2, 31);
-    out3 = __SSAT(out3, 31);
-    out4 = __SSAT(out4, 31);
-
-    /* get result in 1.31 format */
-    out1 = out1 << 1;
-    out2 = out2 << 1;
-    out3 = out3 << 1;
-    out4 = out4 << 1;
-
-    /* store the result to destination buffer */
-    *pCmplxDst++ = out1;
-    *pCmplxDst++ = out2;
-    *pCmplxDst++ = out3;
-    *pCmplxDst++ = out4;
-
-    /* read real input from complex input buffer */
-    inA1 = *pSrcCmplx++;
-    inA2 = *pSrcCmplx++;
-    /* read input from real input bufer */
-    inB1 = *pSrcReal++;
-    inB2 = *pSrcReal++;
-    /* read imaginary input from complex input buffer */
-    inA3 = *pSrcCmplx++;
-    inA4 = *pSrcCmplx++;
-
-    /* multiply complex input with real input */
-    out1 = ((q63_t) inA1 * inB1) >> 32;
-    out2 = ((q63_t) inA2 * inB1) >> 32;
-    out3 = ((q63_t) inA3 * inB2) >> 32;
-    out4 = ((q63_t) inA4 * inB2) >> 32;
-
-    /* sature the result */
-    out1 = __SSAT(out1, 31);
-    out2 = __SSAT(out2, 31);
-    out3 = __SSAT(out3, 31);
-    out4 = __SSAT(out4, 31);
-
-    /* get result in 1.31 format */
-    out1 = out1 << 1;
-    out2 = out2 << 1;
-    out3 = out3 << 1;
-    out4 = out4 << 1;
-
-    /* store the result to destination buffer */
-    *pCmplxDst++ = out1;
-    *pCmplxDst++ = out2;
-    *pCmplxDst++ = out3;
-    *pCmplxDst++ = out4;
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-  /* If the numSamples is not a multiple of 4, compute any remaining output samples here.    
-   ** No loop unrolling is used. */
-  blkCnt = numSamples % 0x4u;
-
-  while(blkCnt > 0u)
-  {
-    /* C[2 * i] = A[2 * i] * B[i].            */
-    /* C[2 * i + 1] = A[2 * i + 1] * B[i].        */
-    /* read real input from complex input buffer */
-    inA1 = *pSrcCmplx++;
-    inA2 = *pSrcCmplx++;
-    /* read input from real input bufer */
-    inB1 = *pSrcReal++;
-
-    /* multiply complex input with real input */
-    out1 = ((q63_t) inA1 * inB1) >> 32;
-    out2 = ((q63_t) inA2 * inB1) >> 32;
-
-    /* sature the result */
-    out1 = __SSAT(out1, 31);
-    out2 = __SSAT(out2, 31);
-
-    /* get result in 1.31 format */
-    out1 = out1 << 1;
-    out2 = out2 << 1;
-
-    /* store the result to destination buffer */
-    *pCmplxDst++ = out1;
-    *pCmplxDst++ = out2;
-
-    /* Decrement the numSamples loop counter */
-    blkCnt--;
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  while(numSamples > 0u)
-  {
-    /* realOut = realA * realB.            */
-    /* imagReal = imagA * realB.               */
-    inA1 = *pSrcReal++;
-    /* store the result in the destination buffer. */
-    *pCmplxDst++ =
-      (q31_t) clip_q63_to_q31(((q63_t) * pSrcCmplx++ * inA1) >> 31);
-    *pCmplxDst++ =
-      (q31_t) clip_q63_to_q31(((q63_t) * pSrcCmplx++ * inA1) >> 31);
-
-    /* Decrement the numSamples loop counter */
-    numSamples--;
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of CmplxByRealMult group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_f32.c

@@ -1,79 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_pid_init_f32.c    
-*    
-* Description:	Floating-point PID Control initialization function    
-*				   
-*    
-* 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.    
-* ------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
- /**    
- * @addtogroup PID    
- * @{    
- */
-
-/**    
- * @brief  Initialization function for the floating-point PID Control.   
- * @param[in,out] *S points to an instance of the PID structure.   
- * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state & 1 = reset the state.   
- * @return none.   
- * \par Description:   
- * \par    
- * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
- * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
- * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
- * also sets the state variables to all zeros.    
- */
-
-void arm_pid_init_f32(
-  arm_pid_instance_f32 * S,
-  int32_t resetStateFlag)
-{
-
-  /* Derived coefficient A0 */
-  S->A0 = S->Kp + S->Ki + S->Kd;
-
-  /* Derived coefficient A1 */
-  S->A1 = (-S->Kp) - ((float32_t) 2.0 * S->Kd);
-
-  /* Derived coefficient A2 */
-  S->A2 = S->Kd;
-
-  /* Check whether state needs reset or not */
-  if(resetStateFlag)
-  {
-    /* Clear the state buffer.  The size will be always 3 samples */
-    memset(S->state, 0, 3u * sizeof(float32_t));
-  }
-
-}
-
-/**    
- * @} end of PID group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q15.c

@@ -1,114 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_pid_init_q15.c    
-*    
-* Description:	Q15 PID Control initialization function    
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
- /**    
- * @addtogroup PID    
- * @{    
- */
-
-/**    
- * @details    
- * @param[in,out] *S points to an instance of the Q15 PID structure.    
- * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.    
- * @return none.    
- * \par Description:   
- * \par    
- * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
- * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
- * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
- * also sets the state variables to all zeros.    
- */
-
-void arm_pid_init_q15(
-  arm_pid_instance_q15 * S,
-  int32_t resetStateFlag)
-{
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  /* Derived coefficient A0 */
-  S->A0 = __QADD16(__QADD16(S->Kp, S->Ki), S->Kd);
-
-  /* Derived coefficients and pack into A1 */
-
-#ifndef  ARM_MATH_BIG_ENDIAN
-
-  S->A1 = __PKHBT(-__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), S->Kd, 16);
-
-#else
-
-  S->A1 = __PKHBT(S->Kd, -__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), 16);
-
-#endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
-
-  /* Check whether state needs reset or not */
-  if(resetStateFlag)
-  {
-    /* Clear the state buffer.  The size will be always 3 samples */
-    memset(S->state, 0, 3u * sizeof(q15_t));
-  }
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  q31_t temp;                                    /*to store the sum */
-
-  /* Derived coefficient A0 */
-  temp = S->Kp + S->Ki + S->Kd;
-  S->A0 = (q15_t) __SSAT(temp, 16);
-
-  /* Derived coefficients and pack into A1 */
-  temp = -(S->Kd + S->Kd + S->Kp);
-  S->A1 = (q15_t) __SSAT(temp, 16);
-  S->A2 = S->Kd;
-
-
-
-  /* Check whether state needs reset or not */
-  if(resetStateFlag)
-  {
-    /* Clear the state buffer.  The size will be always 3 samples */
-    memset(S->state, 0, 3u * sizeof(q15_t));
-  }
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-}
-
-/**    
- * @} end of PID group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_init_q31.c

@@ -1,99 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_pid_init_q31.c    
-*    
-* Description:	Q31 PID Control initialization function     
-*    
-* 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.    
-* ------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
- /**    
- * @addtogroup PID    
- * @{    
- */
-
-/**    
- * @brief  Initialization function for the Q31 PID Control.   
- * @param[in,out] *S points to an instance of the Q31 PID structure.   
- * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.   
- * @return none.    
- * \par Description:   
- * \par    
- * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n   
- * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code>    
- * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd)    
- * also sets the state variables to all zeros.    
- */
-
-void arm_pid_init_q31(
-  arm_pid_instance_q31 * S,
-  int32_t resetStateFlag)
-{
-
-#ifndef ARM_MATH_CM0
-
-  /* Run the below code for Cortex-M4 and Cortex-M3 */
-
-  /* Derived coefficient A0 */
-  S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd);
-
-  /* Derived coefficient A1 */
-  S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp);
-
-
-#else
-
-  /* Run the below code for Cortex-M0 */
-
-  q31_t temp;
-
-  /* Derived coefficient A0 */
-  temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki);
-  S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd);
-
-  /* Derived coefficient A1 */
-  temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd);
-  S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp);
-
-#endif /* #ifndef ARM_MATH_CM0 */
-
-  /* Derived coefficient A2 */
-  S->A2 = S->Kd;
-
-  /* Check whether state needs reset or not */
-  if(resetStateFlag)
-  {
-    /* Clear the state buffer.  The size will be always 3 samples */
-    memset(S->state, 0, 3u * sizeof(q31_t));
-  }
-
-}
-
-/**    
- * @} end of PID group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_f32.c

@@ -1,57 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_pid_reset_f32.c    
-*    
-* Description:	Floating-point PID Control reset function   
-*    
-* 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.    
-* ------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
- /**    
- * @addtogroup PID    
- * @{    
- */
-
-/**    
-* @brief  Reset function for the floating-point PID Control.   
-* @param[in] *S	Instance pointer of PID control data structure.   
-* @return none.    
-* \par Description:   
-* The function resets the state buffer to zeros.    
-*/
-void arm_pid_reset_f32(
-  arm_pid_instance_f32 * S)
-{
-
-  /* Clear the state buffer.  The size will be always 3 samples */
-  memset(S->state, 0, 3u * sizeof(float32_t));
-}
-
-/**    
- * @} end of PID group    
- */

src/modules/mathlib/CMSIS/DSP_Lib/Source/ControllerFunctions/arm_pid_reset_q15.c

@@ -1,56 +0,0 @@
-/* ----------------------------------------------------------------------    
-* Copyright (C) 2010 ARM Limited. All rights reserved.    
-*    
-* $Date:        15. February 2012  
-* $Revision: 	V1.1.0  
-*    
-* Project: 	    CMSIS DSP Library    
-* Title:	    arm_pid_reset_q15.c    
-*    
-* Description:	Q15 PID Control reset function   
-*    
-* 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.    
-* -------------------------------------------------------------------- */
-
-#include "arm_math.h"
-
- /**    
- * @addtogroup PID    
- * @{    
- */
-
-/**    
-* @brief  Reset function for the Q15 PID Control.   
-* @param[in] *S		Instance pointer of PID control data structure.   
-* @return none.    
-* \par Description:   
-* The function resets the state buffer to zeros.    
-*/
-void arm_pid_reset_q15(
-  arm_pid_instance_q15 * S)
-{
-  /* Reset state to zero, The size will be always 3 samples */
-  memset(S->state, 0, 3u * sizeof(q15_t));
-}
-
-/**    
- * @} end of PID group    
- */