forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into fixedwing
This commit is contained in:
commit
8cc9fb9e2c
|
@ -0,0 +1,19 @@
|
||||||
|
#!/bin/sh
|
||||||
|
astyle \
|
||||||
|
--style=linux \
|
||||||
|
--indent=force-tab=8 \
|
||||||
|
--indent-cases \
|
||||||
|
--indent-preprocessor \
|
||||||
|
--break-blocks=all \
|
||||||
|
--pad-oper \
|
||||||
|
--pad-header \
|
||||||
|
--unpad-paren \
|
||||||
|
--keep-one-line-blocks \
|
||||||
|
--keep-one-line-statements \
|
||||||
|
--align-pointer=name \
|
||||||
|
--suffix=none \
|
||||||
|
--lineend=linux \
|
||||||
|
$*
|
||||||
|
#--ignore-exclude-errors-x \
|
||||||
|
#--exclude=EASTL \
|
||||||
|
#--align-reference=name \
|
|
@ -0,0 +1,53 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Redistribution and use in source and binary forms, with or without
|
||||||
|
# modification, are permitted provided that the following conditions
|
||||||
|
# are met:
|
||||||
|
#
|
||||||
|
# 1. Redistributions of source code must retain the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer.
|
||||||
|
# 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer in
|
||||||
|
# the documentation and/or other materials provided with the
|
||||||
|
# distribution.
|
||||||
|
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
# used to endorse or promote products derived from this software
|
||||||
|
# without specific prior written permission.
|
||||||
|
#
|
||||||
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Control library
|
||||||
|
#
|
||||||
|
CSRCS = test_params.c
|
||||||
|
|
||||||
|
CXXSRCS = block/Block.cpp \
|
||||||
|
block/BlockParam.cpp \
|
||||||
|
block/UOrbPublication.cpp \
|
||||||
|
block/UOrbSubscription.cpp \
|
||||||
|
blocks.cpp \
|
||||||
|
fixedwing.cpp
|
||||||
|
|
||||||
|
CXXHDRS = block/Block.hpp \
|
||||||
|
block/BlockParam.hpp \
|
||||||
|
block/UOrbPublication.hpp \
|
||||||
|
block/UOrbSubscription.hpp \
|
||||||
|
blocks.hpp \
|
||||||
|
fixedwing.hpp
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,210 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Block.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "BlockParam.hpp"
|
||||||
|
#include "UOrbSubscription.hpp"
|
||||||
|
#include "UOrbPublication.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
Block::Block(SuperBlock *parent, const char *name) :
|
||||||
|
_name(name),
|
||||||
|
_parent(parent),
|
||||||
|
_dt(0),
|
||||||
|
_subscriptions(),
|
||||||
|
_params()
|
||||||
|
{
|
||||||
|
if (getParent() != NULL) {
|
||||||
|
getParent()->getChildren().add(this);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::getName(char *buf, size_t n)
|
||||||
|
{
|
||||||
|
if (getParent() == NULL) {
|
||||||
|
strncpy(buf, _name, n);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
char parentName[blockNameLengthMax];
|
||||||
|
getParent()->getName(parentName, n);
|
||||||
|
|
||||||
|
if (!strcmp(_name, "")) {
|
||||||
|
strncpy(buf, parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updateParams()
|
||||||
|
{
|
||||||
|
BlockParamBase *param = getParams().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (param != NULL) {
|
||||||
|
if (count++ > maxParamsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max params for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//printf("updating param: %s\n", param->getName());
|
||||||
|
param->update();
|
||||||
|
param = param->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updateSubscriptions()
|
||||||
|
{
|
||||||
|
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (sub != NULL) {
|
||||||
|
if (count++ > maxSubscriptionsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max subscriptions for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
sub->update();
|
||||||
|
sub = sub->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Block::updatePublications()
|
||||||
|
{
|
||||||
|
UOrbPublicationBase *pub = getPublications().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (pub != NULL) {
|
||||||
|
if (count++ > maxPublicationsPerBlock) {
|
||||||
|
char name[blockNameLengthMax];
|
||||||
|
getName(name, blockNameLengthMax);
|
||||||
|
printf("exceeded max publications for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub->update();
|
||||||
|
pub = pub->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::setDt(float dt)
|
||||||
|
{
|
||||||
|
Block::setDt(dt);
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->setDt(dt);
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildParams()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updateParams();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildSubscriptions()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updateSubscriptions();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SuperBlock::updateChildPublications()
|
||||||
|
{
|
||||||
|
Block *child = getChildren().getHead();
|
||||||
|
int count = 0;
|
||||||
|
|
||||||
|
while (child != NULL) {
|
||||||
|
if (count++ > maxChildrenPerBlock) {
|
||||||
|
char name[40];
|
||||||
|
getName(name, 40);
|
||||||
|
printf("exceeded max children for block: %s\n", name);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
child->updatePublications();
|
||||||
|
child = child->getSibling();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,131 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Block.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <inttypes.h>
|
||||||
|
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
static const uint16_t maxChildrenPerBlock = 100;
|
||||||
|
static const uint16_t maxParamsPerBlock = 100;
|
||||||
|
static const uint16_t maxSubscriptionsPerBlock = 100;
|
||||||
|
static const uint16_t maxPublicationsPerBlock = 100;
|
||||||
|
static const uint8_t blockNameLengthMax = 80;
|
||||||
|
|
||||||
|
// forward declaration
|
||||||
|
class BlockParamBase;
|
||||||
|
class UOrbSubscriptionBase;
|
||||||
|
class UOrbPublicationBase;
|
||||||
|
class SuperBlock;
|
||||||
|
|
||||||
|
/**
|
||||||
|
*/
|
||||||
|
class __EXPORT Block :
|
||||||
|
public ListNode<Block *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend class BlockParamBase;
|
||||||
|
// methods
|
||||||
|
Block(SuperBlock *parent, const char *name);
|
||||||
|
void getName(char *name, size_t n);
|
||||||
|
virtual ~Block() {};
|
||||||
|
virtual void updateParams();
|
||||||
|
virtual void updateSubscriptions();
|
||||||
|
virtual void updatePublications();
|
||||||
|
virtual void setDt(float dt) { _dt = dt; }
|
||||||
|
// accessors
|
||||||
|
float getDt() { return _dt; }
|
||||||
|
protected:
|
||||||
|
// accessors
|
||||||
|
SuperBlock *getParent() { return _parent; }
|
||||||
|
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||||
|
List<UOrbPublicationBase *> & getPublications() { return _publications; }
|
||||||
|
List<BlockParamBase *> & getParams() { return _params; }
|
||||||
|
// attributes
|
||||||
|
const char *_name;
|
||||||
|
SuperBlock *_parent;
|
||||||
|
float _dt;
|
||||||
|
List<UOrbSubscriptionBase *> _subscriptions;
|
||||||
|
List<UOrbPublicationBase *> _publications;
|
||||||
|
List<BlockParamBase *> _params;
|
||||||
|
};
|
||||||
|
|
||||||
|
class __EXPORT SuperBlock :
|
||||||
|
public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend class Block;
|
||||||
|
// methods
|
||||||
|
SuperBlock(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_children() {
|
||||||
|
}
|
||||||
|
virtual ~SuperBlock() {};
|
||||||
|
virtual void setDt(float dt);
|
||||||
|
virtual void updateParams() {
|
||||||
|
Block::updateParams();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildParams();
|
||||||
|
}
|
||||||
|
virtual void updateSubscriptions() {
|
||||||
|
Block::updateSubscriptions();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildSubscriptions();
|
||||||
|
}
|
||||||
|
virtual void updatePublications() {
|
||||||
|
Block::updatePublications();
|
||||||
|
|
||||||
|
if (getChildren().getHead() != NULL) updateChildPublications();
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
// methods
|
||||||
|
List<Block *> & getChildren() { return _children; }
|
||||||
|
void updateChildParams();
|
||||||
|
void updateChildSubscriptions();
|
||||||
|
void updateChildPublications();
|
||||||
|
// attributes
|
||||||
|
List<Block *> _children;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,77 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Blockparam.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "BlockParam.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
BlockParamBase::BlockParamBase(Block *parent, const char *name) :
|
||||||
|
_handle(PARAM_INVALID)
|
||||||
|
{
|
||||||
|
char fullname[blockNameLengthMax];
|
||||||
|
|
||||||
|
if (parent == NULL) {
|
||||||
|
strncpy(fullname, name, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
char parentName[blockNameLengthMax];
|
||||||
|
parent->getName(parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
if (!strcmp(name, "")) {
|
||||||
|
strncpy(fullname, parentName, blockNameLengthMax);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
|
||||||
|
}
|
||||||
|
|
||||||
|
parent->getParams().add(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
_handle = param_find(fullname);
|
||||||
|
|
||||||
|
if (_handle == PARAM_INVALID)
|
||||||
|
printf("error finding param: %s\n", fullname);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,85 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file BlockParam.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A base class for block params that enables traversing linked list.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
BlockParamBase(Block *parent, const char *name);
|
||||||
|
virtual ~BlockParamBase() {};
|
||||||
|
virtual void update() = 0;
|
||||||
|
const char *getName() { return param_name(_handle); }
|
||||||
|
protected:
|
||||||
|
param_t _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Parameters that are tied to blocks for updating and nameing.
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT BlockParam : public BlockParamBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
BlockParam(Block *block, const char *name) :
|
||||||
|
BlockParamBase(block, name),
|
||||||
|
_val() {
|
||||||
|
update();
|
||||||
|
}
|
||||||
|
T get() { return _val; }
|
||||||
|
void set(T val) { _val = val; }
|
||||||
|
void update() {
|
||||||
|
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
T _val;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,71 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Node.h
|
||||||
|
*
|
||||||
|
* A node of a linked list.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT ListNode
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ListNode() : _sibling(NULL) {
|
||||||
|
}
|
||||||
|
void setSibling(T sibling) { _sibling = sibling; }
|
||||||
|
T getSibling() { return _sibling; }
|
||||||
|
T get() {
|
||||||
|
return _sibling;
|
||||||
|
}
|
||||||
|
protected:
|
||||||
|
T _sibling;
|
||||||
|
};
|
||||||
|
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT List
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
List() : _head() {
|
||||||
|
}
|
||||||
|
void add(T newNode) {
|
||||||
|
newNode->setSibling(getHead());
|
||||||
|
setHead(newNode);
|
||||||
|
}
|
||||||
|
T getHead() { return _head; }
|
||||||
|
private:
|
||||||
|
void setHead(T &head) { _head = head; }
|
||||||
|
T _head;
|
||||||
|
};
|
|
@ -0,0 +1,39 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file UOrbPublication.cpp
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UOrbPublication.hpp"
|
|
@ -0,0 +1,118 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file UOrbPublication.h
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
class Block;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Base publication warapper class, used in list traversal
|
||||||
|
* of various publications.
|
||||||
|
*/
|
||||||
|
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
UOrbPublicationBase(
|
||||||
|
List<UOrbPublicationBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
_meta(meta),
|
||||||
|
_handle() {
|
||||||
|
if (list != NULL) list->add(this);
|
||||||
|
}
|
||||||
|
void update() {
|
||||||
|
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
|
||||||
|
}
|
||||||
|
virtual void *getDataVoidPtr() = 0;
|
||||||
|
virtual ~UOrbPublicationBase() {
|
||||||
|
orb_unsubscribe(getHandle());
|
||||||
|
}
|
||||||
|
const struct orb_metadata *getMeta() { return _meta; }
|
||||||
|
int getHandle() { return _handle; }
|
||||||
|
protected:
|
||||||
|
void setHandle(orb_advert_t handle) { _handle = handle; }
|
||||||
|
const struct orb_metadata *_meta;
|
||||||
|
orb_advert_t _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UOrb Publication wrapper class
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class UOrbPublication :
|
||||||
|
public T, // this must be first!
|
||||||
|
public UOrbPublicationBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param list A list interface for adding to list during construction
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
*/
|
||||||
|
UOrbPublication(
|
||||||
|
List<UOrbPublicationBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
T(), // initialize data structure to zero
|
||||||
|
UOrbPublicationBase(list, meta) {
|
||||||
|
// It is important that we call T()
|
||||||
|
// before we publish the data, so we
|
||||||
|
// call this here instead of the base class
|
||||||
|
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
|
||||||
|
}
|
||||||
|
virtual ~UOrbPublication() {}
|
||||||
|
/*
|
||||||
|
* XXX
|
||||||
|
* This function gets the T struct, assuming
|
||||||
|
* the struct is the first base class, this
|
||||||
|
* should use dynamic cast, but doesn't
|
||||||
|
* seem to be available
|
||||||
|
*/
|
||||||
|
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,51 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file UOrbSubscription.cpp
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "UOrbSubscription.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
bool __EXPORT UOrbSubscriptionBase::updated()
|
||||||
|
{
|
||||||
|
bool isUpdated = false;
|
||||||
|
orb_check(_handle, &isUpdated);
|
||||||
|
return isUpdated;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,137 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file UOrbSubscription.h
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include "Block.hpp"
|
||||||
|
#include "List.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
class Block;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Base subscription warapper class, used in list traversal
|
||||||
|
* of various subscriptions.
|
||||||
|
*/
|
||||||
|
class __EXPORT UOrbSubscriptionBase :
|
||||||
|
public ListNode<control::UOrbSubscriptionBase *>
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
*/
|
||||||
|
UOrbSubscriptionBase(
|
||||||
|
List<UOrbSubscriptionBase *> * list,
|
||||||
|
const struct orb_metadata *meta) :
|
||||||
|
_meta(meta),
|
||||||
|
_handle() {
|
||||||
|
if (list != NULL) list->add(this);
|
||||||
|
}
|
||||||
|
bool updated();
|
||||||
|
void update() {
|
||||||
|
if (updated()) {
|
||||||
|
orb_copy(_meta, _handle, getDataVoidPtr());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
virtual void *getDataVoidPtr() = 0;
|
||||||
|
virtual ~UOrbSubscriptionBase() {
|
||||||
|
orb_unsubscribe(_handle);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
const struct orb_metadata *getMeta() { return _meta; }
|
||||||
|
int getHandle() { return _handle; }
|
||||||
|
protected:
|
||||||
|
// accessors
|
||||||
|
void setHandle(int handle) { _handle = handle; }
|
||||||
|
// attributes
|
||||||
|
const struct orb_metadata *_meta;
|
||||||
|
int _handle;
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UOrb Subscription wrapper class
|
||||||
|
*/
|
||||||
|
template<class T>
|
||||||
|
class __EXPORT UOrbSubscription :
|
||||||
|
public T, // this must be first!
|
||||||
|
public UOrbSubscriptionBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
*
|
||||||
|
* @param list A list interface for adding to list during construction
|
||||||
|
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||||
|
* for the topic.
|
||||||
|
* @param interval The minimum interval in milliseconds between updates
|
||||||
|
*/
|
||||||
|
UOrbSubscription(
|
||||||
|
List<UOrbSubscriptionBase *> * list,
|
||||||
|
const struct orb_metadata *meta, unsigned interval) :
|
||||||
|
T(), // initialize data structure to zero
|
||||||
|
UOrbSubscriptionBase(list, meta) {
|
||||||
|
setHandle(orb_subscribe(getMeta()));
|
||||||
|
orb_set_interval(getHandle(), interval);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deconstructor
|
||||||
|
*/
|
||||||
|
virtual ~UOrbSubscription() {}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* XXX
|
||||||
|
* This function gets the T struct, assuming
|
||||||
|
* the struct is the first base class, this
|
||||||
|
* should use dynamic cast, but doesn't
|
||||||
|
* seem to be available
|
||||||
|
*/
|
||||||
|
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||||
|
T getData() { return T(*this); }
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,486 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file blocks.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
#include "blocks.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
int basicBlocksTest()
|
||||||
|
{
|
||||||
|
blockLimitTest();
|
||||||
|
blockLimitSymTest();
|
||||||
|
blockLowPassTest();
|
||||||
|
blockHighPassTest();
|
||||||
|
blockIntegralTest();
|
||||||
|
blockIntegralTrapTest();
|
||||||
|
blockDerivativeTest();
|
||||||
|
blockPTest();
|
||||||
|
blockPITest();
|
||||||
|
blockPDTest();
|
||||||
|
blockPIDTest();
|
||||||
|
blockOutputTest();
|
||||||
|
blockRandUniformTest();
|
||||||
|
blockRandGaussTest();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLimit::update(float input)
|
||||||
|
{
|
||||||
|
if (input > getMax()) {
|
||||||
|
input = _max.get();
|
||||||
|
|
||||||
|
} else if (input < getMin()) {
|
||||||
|
input = getMin();
|
||||||
|
}
|
||||||
|
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLimitTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLimit\t\t\t: ");
|
||||||
|
BlockLimit limit(NULL, "TEST");
|
||||||
|
// initial state
|
||||||
|
ASSERT(equal(1.0f, limit.getMax()));
|
||||||
|
ASSERT(equal(-1.0f, limit.getMin()));
|
||||||
|
ASSERT(equal(0.0f, limit.getDt()));
|
||||||
|
// update
|
||||||
|
ASSERT(equal(-1.0f, limit.update(-2.0f)));
|
||||||
|
ASSERT(equal(1.0f, limit.update(2.0f)));
|
||||||
|
ASSERT(equal(0.0f, limit.update(0.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLimitSym::update(float input)
|
||||||
|
{
|
||||||
|
if (input > getMax()) {
|
||||||
|
input = _max.get();
|
||||||
|
|
||||||
|
} else if (input < -getMax()) {
|
||||||
|
input = -getMax();
|
||||||
|
}
|
||||||
|
|
||||||
|
return input;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLimitSymTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLimitSym\t\t: ");
|
||||||
|
BlockLimitSym limit(NULL, "TEST");
|
||||||
|
// initial state
|
||||||
|
ASSERT(equal(1.0f, limit.getMax()));
|
||||||
|
ASSERT(equal(0.0f, limit.getDt()));
|
||||||
|
// update
|
||||||
|
ASSERT(equal(-1.0f, limit.update(-2.0f)));
|
||||||
|
ASSERT(equal(1.0f, limit.update(2.0f)));
|
||||||
|
ASSERT(equal(0.0f, limit.update(0.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockLowPass::update(float input)
|
||||||
|
{
|
||||||
|
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||||
|
float a = b / (1 + b);
|
||||||
|
setState(a * input + (1 - a)*getState());
|
||||||
|
return getState();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockLowPassTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockLowPass\t\t: ");
|
||||||
|
BlockLowPass lowPass(NULL, "TEST_LP");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(10.0f, lowPass.getFCut()));
|
||||||
|
ASSERT(equal(0.0f, lowPass.getState()));
|
||||||
|
ASSERT(equal(0.0f, lowPass.getDt()));
|
||||||
|
// set dt
|
||||||
|
lowPass.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, lowPass.getDt()));
|
||||||
|
// set state
|
||||||
|
lowPass.setState(1.0f);
|
||||||
|
ASSERT(equal(1.0f, lowPass.getState()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
|
||||||
|
|
||||||
|
// test end condition
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
lowPass.update(2.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(2.0f, lowPass.getState()));
|
||||||
|
ASSERT(equal(2.0f, lowPass.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
float BlockHighPass::update(float input)
|
||||||
|
{
|
||||||
|
float b = 2 * float(M_PI) * getFCut() * getDt();
|
||||||
|
float a = 1 / (1 + b);
|
||||||
|
setY(a * (getY() + input - getU()));
|
||||||
|
setU(input);
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockHighPassTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockHighPass\t\t: ");
|
||||||
|
BlockHighPass highPass(NULL, "TEST_HP");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(10.0f, highPass.getFCut()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getU()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getY()));
|
||||||
|
ASSERT(equal(0.0f, highPass.getDt()));
|
||||||
|
// set dt
|
||||||
|
highPass.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, highPass.getDt()));
|
||||||
|
// set state
|
||||||
|
highPass.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, highPass.getU()));
|
||||||
|
highPass.setY(1.0f);
|
||||||
|
ASSERT(equal(1.0f, highPass.getY()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(0.2746051f, highPass.update(2.0f)));
|
||||||
|
|
||||||
|
// test end condition
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
highPass.update(2.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(0.0f, highPass.getY()));
|
||||||
|
ASSERT(equal(0.0f, highPass.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockIntegral::update(float input)
|
||||||
|
{
|
||||||
|
// trapezoidal integration
|
||||||
|
setY(_limit.update(getY() + input * getDt()));
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockIntegralTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockIntegral\t\t: ");
|
||||||
|
BlockIntegral integral(NULL, "TEST_I");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(1.0f, integral.getMax()));
|
||||||
|
ASSERT(equal(0.0f, integral.getDt()));
|
||||||
|
// set dt
|
||||||
|
integral.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, integral.getDt()));
|
||||||
|
// set Y
|
||||||
|
integral.setY(0.9f);
|
||||||
|
ASSERT(equal(0.9f, integral.getY()));
|
||||||
|
|
||||||
|
// test exceed max
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(1.0f, integral.update(1.0f)));
|
||||||
|
// test exceed min
|
||||||
|
integral.setY(-0.9f);
|
||||||
|
ASSERT(equal(-0.9f, integral.getY()));
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(-1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(-1.0f, integral.update(-1.0f)));
|
||||||
|
// test update
|
||||||
|
integral.setY(0.1f);
|
||||||
|
ASSERT(equal(0.2f, integral.update(1.0)));
|
||||||
|
ASSERT(equal(0.2f, integral.getY()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockIntegralTrap::update(float input)
|
||||||
|
{
|
||||||
|
// trapezoidal integration
|
||||||
|
setY(_limit.update(getY() +
|
||||||
|
(getU() + input) / 2.0f * getDt()));
|
||||||
|
setU(input);
|
||||||
|
return getY();
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockIntegralTrapTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockIntegralTrap\t\t: ");
|
||||||
|
BlockIntegralTrap integral(NULL, "TEST_I");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(1.0f, integral.getMax()));
|
||||||
|
ASSERT(equal(0.0f, integral.getDt()));
|
||||||
|
// set dt
|
||||||
|
integral.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, integral.getDt()));
|
||||||
|
// set U
|
||||||
|
integral.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, integral.getU()));
|
||||||
|
// set Y
|
||||||
|
integral.setY(0.9f);
|
||||||
|
ASSERT(equal(0.9f, integral.getY()));
|
||||||
|
|
||||||
|
// test exceed max
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(1.0f, integral.update(1.0f)));
|
||||||
|
// test exceed min
|
||||||
|
integral.setU(-1.0f);
|
||||||
|
integral.setY(-0.9f);
|
||||||
|
ASSERT(equal(-0.9f, integral.getY()));
|
||||||
|
|
||||||
|
for (int i = 0; i < 100; i++) {
|
||||||
|
integral.update(-1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(-1.0f, integral.update(-1.0f)));
|
||||||
|
// test update
|
||||||
|
integral.setU(2.0f);
|
||||||
|
integral.setY(0.1f);
|
||||||
|
ASSERT(equal(0.25f, integral.update(1.0)));
|
||||||
|
ASSERT(equal(0.25f, integral.getY()));
|
||||||
|
ASSERT(equal(1.0f, integral.getU()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
float BlockDerivative::update(float input)
|
||||||
|
{
|
||||||
|
float output = _lowPass.update((input - getU()) / getDt());
|
||||||
|
setU(input);
|
||||||
|
return output;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockDerivativeTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockDerivative\t\t: ");
|
||||||
|
BlockDerivative derivative(NULL, "TEST_D");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, derivative.getU()));
|
||||||
|
ASSERT(equal(10.0f, derivative.getLP()));
|
||||||
|
// set dt
|
||||||
|
derivative.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, derivative.getDt()));
|
||||||
|
// set U
|
||||||
|
derivative.setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, derivative.getU()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(8.6269744f, derivative.update(2.0f)));
|
||||||
|
ASSERT(equal(2.0f, derivative.getU()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockP\t\t\t: ");
|
||||||
|
BlockP blockP(NULL, "TEST_P");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockP.getKP()));
|
||||||
|
ASSERT(equal(0.0f, blockP.getDt()));
|
||||||
|
// set dt
|
||||||
|
blockP.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockP.getDt()));
|
||||||
|
// test update
|
||||||
|
ASSERT(equal(0.4f, blockP.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPITest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPI\t\t\t: ");
|
||||||
|
BlockPI blockPI(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPI.getKP()));
|
||||||
|
ASSERT(equal(0.1f, blockPI.getKI()));
|
||||||
|
ASSERT(equal(0.0f, blockPI.getDt()));
|
||||||
|
ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
|
||||||
|
// set dt
|
||||||
|
blockPI.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPI.getDt()));
|
||||||
|
// set integral state
|
||||||
|
blockPI.getIntegral().setY(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
|
||||||
|
ASSERT(equal(0.43f, blockPI.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPDTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPD\t\t\t: ");
|
||||||
|
BlockPD blockPD(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPD.getKP()));
|
||||||
|
ASSERT(equal(0.01f, blockPD.getKD()));
|
||||||
|
ASSERT(equal(0.0f, blockPD.getDt()));
|
||||||
|
ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
|
||||||
|
// set dt
|
||||||
|
blockPD.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPD.getDt()));
|
||||||
|
// set derivative state
|
||||||
|
blockPD.getDerivative().setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
|
||||||
|
ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockPIDTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockPID\t\t\t: ");
|
||||||
|
BlockPID blockPID(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.2f, blockPID.getKP()));
|
||||||
|
ASSERT(equal(0.1f, blockPID.getKI()));
|
||||||
|
ASSERT(equal(0.01f, blockPID.getKD()));
|
||||||
|
ASSERT(equal(0.0f, blockPID.getDt()));
|
||||||
|
ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
|
||||||
|
ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
|
||||||
|
// set dt
|
||||||
|
blockPID.setDt(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPID.getDt()));
|
||||||
|
// set derivative state
|
||||||
|
blockPID.getDerivative().setU(1.0f);
|
||||||
|
ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
|
||||||
|
// set integral state
|
||||||
|
blockPID.getIntegral().setY(0.1f);
|
||||||
|
ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
|
||||||
|
// test update
|
||||||
|
// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
|
||||||
|
ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockOutputTest()
|
||||||
|
{
|
||||||
|
printf("Test BlockOutput\t\t: ");
|
||||||
|
BlockOutput blockOutput(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockOutput.getDt()));
|
||||||
|
ASSERT(equal(0.5f, blockOutput.get()));
|
||||||
|
ASSERT(equal(-1.0f, blockOutput.getMin()));
|
||||||
|
ASSERT(equal(1.0f, blockOutput.getMax()));
|
||||||
|
// test update below min
|
||||||
|
blockOutput.update(-2.0f);
|
||||||
|
ASSERT(equal(-1.0f, blockOutput.get()));
|
||||||
|
// test update above max
|
||||||
|
blockOutput.update(2.0f);
|
||||||
|
ASSERT(equal(1.0f, blockOutput.get()));
|
||||||
|
// test trim
|
||||||
|
blockOutput.update(0.0f);
|
||||||
|
ASSERT(equal(0.5f, blockOutput.get()));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockRandUniformTest()
|
||||||
|
{
|
||||||
|
srand(1234);
|
||||||
|
printf("Test BlockRandUniform\t\t: ");
|
||||||
|
BlockRandUniform blockRandUniform(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockRandUniform.getDt()));
|
||||||
|
ASSERT(equal(-1.0f, blockRandUniform.getMin()));
|
||||||
|
ASSERT(equal(1.0f, blockRandUniform.getMax()));
|
||||||
|
// test update
|
||||||
|
int n = 10000;
|
||||||
|
float mean = blockRandUniform.update();
|
||||||
|
|
||||||
|
// recursive mean algorithm from Knuth
|
||||||
|
for (int i = 2; i < n + 1; i++) {
|
||||||
|
float val = blockRandUniform.update();
|
||||||
|
mean += (val - mean) / i;
|
||||||
|
ASSERT(val <= blockRandUniform.getMax());
|
||||||
|
ASSERT(val >= blockRandUniform.getMin());
|
||||||
|
}
|
||||||
|
|
||||||
|
ASSERT(equal(mean, (blockRandUniform.getMin() +
|
||||||
|
blockRandUniform.getMax()) / 2, 1e-1));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int blockRandGaussTest()
|
||||||
|
{
|
||||||
|
srand(1234);
|
||||||
|
printf("Test BlockRandGauss\t\t: ");
|
||||||
|
BlockRandGauss blockRandGauss(NULL, "TEST");
|
||||||
|
// test initial state
|
||||||
|
ASSERT(equal(0.0f, blockRandGauss.getDt()));
|
||||||
|
ASSERT(equal(1.0f, blockRandGauss.getMean()));
|
||||||
|
ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
|
||||||
|
// test update
|
||||||
|
int n = 10000;
|
||||||
|
float mean = blockRandGauss.update();
|
||||||
|
float sum = 0;
|
||||||
|
|
||||||
|
// recursive mean, stdev algorithm from Knuth
|
||||||
|
for (int i = 2; i < n + 1; i++) {
|
||||||
|
float val = blockRandGauss.update();
|
||||||
|
float newMean = mean + (val - mean) / i;
|
||||||
|
sum += (val - mean) * (val - newMean);
|
||||||
|
mean = newMean;
|
||||||
|
}
|
||||||
|
|
||||||
|
float stdDev = sqrt(sum / (n - 1));
|
||||||
|
ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
|
||||||
|
ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
|
||||||
|
printf("PASS\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,494 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file blocks.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <assert.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <mathlib/math/test/test.hpp>
|
||||||
|
|
||||||
|
#include "block/Block.hpp"
|
||||||
|
#include "block/BlockParam.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
int __EXPORT basicBlocksTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A limiter/ saturation.
|
||||||
|
* The output of update is the input, bounded
|
||||||
|
* by min/max.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLimit : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLimit(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_min(this, "MIN"),
|
||||||
|
_max(this, "MAX")
|
||||||
|
{};
|
||||||
|
virtual ~BlockLimit() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _min.get(); }
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _min;
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLimitTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A symmetric limiter/ saturation.
|
||||||
|
* Same as limiter but with only a max, is used for
|
||||||
|
* upper limit of +max, and lower limit of -max
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLimitSym : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLimitSym(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_max(this, "MAX")
|
||||||
|
{};
|
||||||
|
virtual ~BlockLimitSym() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLimitSymTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A low pass filter as described here:
|
||||||
|
* http://en.wikipedia.org/wiki/Low-pass_filter.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockLowPass : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockLowPass(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_state(0),
|
||||||
|
_fCut(this, "") // only one parameter, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockLowPass() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getState() { return _state; }
|
||||||
|
float getFCut() { return _fCut.get(); }
|
||||||
|
void setState(float state) { _state = state; }
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _state;
|
||||||
|
BlockParam<float> _fCut;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockLowPassTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A high pass filter as described here:
|
||||||
|
* http://en.wikipedia.org/wiki/High-pass_filter.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockHighPass : public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockHighPass(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_y(0),
|
||||||
|
_fCut(this, "") // only one parameter, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockHighPass() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getFCut() {return _fCut.get();}
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockParam<float> _fCut; /**< cut-off frequency, Hz */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockHighPassTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A rectangular integrator.
|
||||||
|
* A limiter is built into the class to bound the
|
||||||
|
* integral's internal state. This is important
|
||||||
|
* for windup protection.
|
||||||
|
* @see Limit
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockIntegral: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockIntegral(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_y(0),
|
||||||
|
_limit(this, "") {};
|
||||||
|
virtual ~BlockIntegral() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getMax() {return _limit.getMax();}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockLimitSym _limit; /**< limiter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockIntegralTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A trapezoidal integrator.
|
||||||
|
* http://en.wikipedia.org/wiki/Trapezoidal_rule
|
||||||
|
* A limiter is built into the class to bound the
|
||||||
|
* integral's internal state. This is important
|
||||||
|
* for windup protection.
|
||||||
|
* @see Limit
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockIntegralTrap : public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockIntegralTrap(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_y(0),
|
||||||
|
_limit(this, "") {};
|
||||||
|
virtual ~BlockIntegralTrap() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getY() {return _y;}
|
||||||
|
float getMax() {return _limit.getMax();}
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
void setY(float y) {_y = y;}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
float _y; /**< previous output */
|
||||||
|
BlockLimitSym _limit; /**< limiter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockIntegralTrapTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A simple derivative approximation.
|
||||||
|
* This uses the previous and current input.
|
||||||
|
* This has a built in low pass filter.
|
||||||
|
* @see LowPass
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockDerivative : public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockDerivative(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_u(0),
|
||||||
|
_lowPass(this, "LP")
|
||||||
|
{};
|
||||||
|
virtual ~BlockDerivative() {};
|
||||||
|
float update(float input);
|
||||||
|
// accessors
|
||||||
|
void setU(float u) {_u = u;}
|
||||||
|
float getU() {return _u;}
|
||||||
|
float getLP() {return _lowPass.getFCut();}
|
||||||
|
protected:
|
||||||
|
// attributes
|
||||||
|
float _u; /**< previous input */
|
||||||
|
BlockLowPass _lowPass; /**< low pass filter */
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockDerivativeTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockP: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockP(SuperBlock *parent, const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_kP(this, "") // only one param, no need to name
|
||||||
|
{};
|
||||||
|
virtual ~BlockP() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input;
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
protected:
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-integral controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPI: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPI(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_integral(this, "I"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kI(this, "I")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPI() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKI() * getIntegral().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKI() { return _kI.get(); }
|
||||||
|
BlockIntegral &getIntegral() { return _integral; }
|
||||||
|
private:
|
||||||
|
BlockIntegral _integral;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kI;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPITest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-derivative controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPD: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPD(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_derivative(this, "D"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kD(this, "D")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPD() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKD() * getDerivative().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKD() { return _kD.get(); }
|
||||||
|
BlockDerivative &getDerivative() { return _derivative; }
|
||||||
|
private:
|
||||||
|
BlockDerivative _derivative;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kD;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPDTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A proportional-integral-derivative controller.
|
||||||
|
* @link http://en.wikipedia.org/wiki/PID_controller
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockPID: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockPID(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_integral(this, "I"),
|
||||||
|
_derivative(this, "D"),
|
||||||
|
_kP(this, "P"),
|
||||||
|
_kI(this, "I"),
|
||||||
|
_kD(this, "D")
|
||||||
|
{};
|
||||||
|
virtual ~BlockPID() {};
|
||||||
|
float update(float input) {
|
||||||
|
return getKP() * input +
|
||||||
|
getKI() * getIntegral().update(input) +
|
||||||
|
getKD() * getDerivative().update(input);
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getKP() { return _kP.get(); }
|
||||||
|
float getKI() { return _kI.get(); }
|
||||||
|
float getKD() { return _kD.get(); }
|
||||||
|
BlockIntegral &getIntegral() { return _integral; }
|
||||||
|
BlockDerivative &getDerivative() { return _derivative; }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockIntegral _integral;
|
||||||
|
BlockDerivative _derivative;
|
||||||
|
BlockParam<float> _kP;
|
||||||
|
BlockParam<float> _kI;
|
||||||
|
BlockParam<float> _kD;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockPIDTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* An output trim/ saturation block
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockOutput: public SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockOutput(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_trim(this, "TRIM"),
|
||||||
|
_limit(this, ""),
|
||||||
|
_val(0) {
|
||||||
|
update(0);
|
||||||
|
};
|
||||||
|
virtual ~BlockOutput() {};
|
||||||
|
void update(float input) {
|
||||||
|
_val = _limit.update(input + getTrim());
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _limit.getMin(); }
|
||||||
|
float getMax() { return _limit.getMax(); }
|
||||||
|
float getTrim() { return _trim.get(); }
|
||||||
|
float get() { return _val; }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _trim;
|
||||||
|
BlockLimit _limit;
|
||||||
|
float _val;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockOutputTest();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A uniform random number generator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockRandUniform: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockRandUniform(SuperBlock *parent,
|
||||||
|
const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_min(this, "MIN"),
|
||||||
|
_max(this, "MAX") {
|
||||||
|
// seed should be initialized somewhere
|
||||||
|
// in main program for all calls to rand
|
||||||
|
// XXX currently in nuttx if you seed to 0, rand breaks
|
||||||
|
};
|
||||||
|
virtual ~BlockRandUniform() {};
|
||||||
|
float update() {
|
||||||
|
static float rand_max = MAX_RAND;
|
||||||
|
float rand_val = rand();
|
||||||
|
float bounds = getMax() - getMin();
|
||||||
|
return getMin() + (rand_val * bounds) / rand_max;
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMin() { return _min.get(); }
|
||||||
|
float getMax() { return _max.get(); }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _min;
|
||||||
|
BlockParam<float> _max;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockRandUniformTest();
|
||||||
|
|
||||||
|
class __EXPORT BlockRandGauss: public Block
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// methods
|
||||||
|
BlockRandGauss(SuperBlock *parent,
|
||||||
|
const char *name) :
|
||||||
|
Block(parent, name),
|
||||||
|
_mean(this, "MEAN"),
|
||||||
|
_stdDev(this, "DEV") {
|
||||||
|
// seed should be initialized somewhere
|
||||||
|
// in main program for all calls to rand
|
||||||
|
// XXX currently in nuttx if you seed to 0, rand breaks
|
||||||
|
};
|
||||||
|
virtual ~BlockRandGauss() {};
|
||||||
|
float update() {
|
||||||
|
static float V1, V2, S;
|
||||||
|
static int phase = 0;
|
||||||
|
float X;
|
||||||
|
|
||||||
|
if (phase == 0) {
|
||||||
|
do {
|
||||||
|
float U1 = (float)rand() / MAX_RAND;
|
||||||
|
float U2 = (float)rand() / MAX_RAND;
|
||||||
|
V1 = 2 * U1 - 1;
|
||||||
|
V2 = 2 * U2 - 1;
|
||||||
|
S = V1 * V1 + V2 * V2;
|
||||||
|
} while (S >= 1 || fabsf(S) < 1e-8f);
|
||||||
|
|
||||||
|
X = V1 * float(sqrt(-2 * float(log(S)) / S));
|
||||||
|
|
||||||
|
} else
|
||||||
|
X = V2 * float(sqrt(-2 * float(log(S)) / S));
|
||||||
|
|
||||||
|
phase = 1 - phase;
|
||||||
|
return X * getStdDev() + getMean();
|
||||||
|
}
|
||||||
|
// accessors
|
||||||
|
float getMean() { return _mean.get(); }
|
||||||
|
float getStdDev() { return _stdDev.get(); }
|
||||||
|
private:
|
||||||
|
// attributes
|
||||||
|
BlockParam<float> _mean;
|
||||||
|
BlockParam<float> _stdDev;
|
||||||
|
};
|
||||||
|
|
||||||
|
int __EXPORT blockRandGaussTest();
|
||||||
|
|
||||||
|
} // namespace control
|
|
@ -0,0 +1,351 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file fixedwing.cpp
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "fixedwing.hpp"
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
namespace fixedwing
|
||||||
|
{
|
||||||
|
|
||||||
|
BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_rLowPass(this, "R_LP"),
|
||||||
|
_rWashout(this, "R_HP"),
|
||||||
|
_r2Rdr(this, "R2RDR"),
|
||||||
|
_rudder(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockYawDamper::~BlockYawDamper() {};
|
||||||
|
|
||||||
|
void BlockYawDamper::update(float rCmd, float r)
|
||||||
|
{
|
||||||
|
_rudder = _r2Rdr.update(rCmd -
|
||||||
|
_rWashout.update(_rLowPass.update(r)));
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_yawDamper(this, ""),
|
||||||
|
_pLowPass(this, "P_LP"),
|
||||||
|
_qLowPass(this, "Q_LP"),
|
||||||
|
_p2Ail(this, "P2AIL"),
|
||||||
|
_q2Elv(this, "Q2ELV"),
|
||||||
|
_aileron(0),
|
||||||
|
_elevator(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockStabilization::~BlockStabilization() {};
|
||||||
|
|
||||||
|
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
|
||||||
|
float p, float q, float r)
|
||||||
|
{
|
||||||
|
_aileron = _p2Ail.update(
|
||||||
|
pCmd - _pLowPass.update(p));
|
||||||
|
_elevator = _q2Elv.update(
|
||||||
|
qCmd - _qLowPass.update(q));
|
||||||
|
_yawDamper.update(rCmd, r);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_psi2Phi(this, "PSI2PHI"),
|
||||||
|
_phi2P(this, "PHI2P"),
|
||||||
|
_phiLimit(this, "PHI_LIM")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockHeadingHold::~BlockHeadingHold() {};
|
||||||
|
|
||||||
|
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
|
||||||
|
{
|
||||||
|
float psiError = _wrap_pi(psiCmd - psi);
|
||||||
|
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
|
||||||
|
return _phi2P.update(phiCmd - phi);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_v2Theta(this, "V2THE"),
|
||||||
|
_theta2Q(this, "THE2Q"),
|
||||||
|
_theLimit(this, "THE"),
|
||||||
|
_vLimit(this, "V")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
|
||||||
|
|
||||||
|
float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
|
||||||
|
{
|
||||||
|
// negative sign because nose over to increase speed
|
||||||
|
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
|
||||||
|
return _theta2Q.update(thetaCmd - theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_v2Thr(this, "V2THR")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
|
||||||
|
|
||||||
|
float BlockVelocityHoldFrontside::update(float vCmd, float v)
|
||||||
|
{
|
||||||
|
return _v2Thr.update(vCmd - v);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_h2Thr(this, "H2THR"),
|
||||||
|
_throttle(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
|
||||||
|
|
||||||
|
void BlockAltitudeHoldBackside::update(float hCmd, float h)
|
||||||
|
{
|
||||||
|
_throttle = _h2Thr.update(hCmd - h);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_h2Theta(this, "H2THE"),
|
||||||
|
_theta2Q(this, "THE2Q")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
|
||||||
|
|
||||||
|
float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
|
||||||
|
{
|
||||||
|
float thetaCmd = _h2Theta.update(hCmd - h);
|
||||||
|
return _theta2Q.update(thetaCmd - theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
|
||||||
|
const char *name,
|
||||||
|
BlockStabilization *stabilization) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_stabilization(stabilization),
|
||||||
|
_headingHold(this, ""),
|
||||||
|
_velocityHold(this, ""),
|
||||||
|
_altitudeHold(this, ""),
|
||||||
|
_trimAil(this, "TRIM_AIL"),
|
||||||
|
_trimElv(this, "TRIM_ELV"),
|
||||||
|
_trimRdr(this, "TRIM_RDR"),
|
||||||
|
_trimThr(this, "TRIM_THR")
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
|
||||||
|
|
||||||
|
void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
|
||||||
|
float h, float v,
|
||||||
|
float phi, float theta, float psi,
|
||||||
|
float p, float q, float r)
|
||||||
|
{
|
||||||
|
_altitudeHold.update(hCmd, h);
|
||||||
|
_stabilization->update(
|
||||||
|
_headingHold.update(psiCmd, phi, psi, p),
|
||||||
|
_velocityHold.update(vCmd, v, theta, q),
|
||||||
|
rCmd,
|
||||||
|
p, q, r);
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
_xtYawLimit(this, "XT2YAW"),
|
||||||
|
_xt2Yaw(this, "XT2YAW"),
|
||||||
|
_psiCmd(0)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockWaypointGuidance::~BlockWaypointGuidance() {};
|
||||||
|
|
||||||
|
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
|
||||||
|
vehicle_attitude_s &att,
|
||||||
|
vehicle_global_position_setpoint_s &posCmd,
|
||||||
|
vehicle_global_position_setpoint_s &lastPosCmd)
|
||||||
|
{
|
||||||
|
|
||||||
|
// heading to waypoint
|
||||||
|
float psiTrack = get_bearing_to_next_waypoint(
|
||||||
|
(double)pos.lat / (double)1e7d,
|
||||||
|
(double)pos.lon / (double)1e7d,
|
||||||
|
(double)posCmd.lat / (double)1e7d,
|
||||||
|
(double)posCmd.lon / (double)1e7d);
|
||||||
|
|
||||||
|
// cross track
|
||||||
|
struct crosstrack_error_s xtrackError;
|
||||||
|
get_distance_to_line(&xtrackError,
|
||||||
|
(double)pos.lat / (double)1e7d,
|
||||||
|
(double)pos.lon / (double)1e7d,
|
||||||
|
(double)lastPosCmd.lat / (double)1e7d,
|
||||||
|
(double)lastPosCmd.lon / (double)1e7d,
|
||||||
|
(double)posCmd.lat / (double)1e7d,
|
||||||
|
(double)posCmd.lon / (double)1e7d);
|
||||||
|
|
||||||
|
_psiCmd = _wrap_2pi(psiTrack -
|
||||||
|
_xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
// subscriptions
|
||||||
|
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
|
||||||
|
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
|
||||||
|
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
|
||||||
|
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
|
||||||
|
_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
|
||||||
|
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
|
||||||
|
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
|
||||||
|
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||||
|
// publications
|
||||||
|
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
|
||||||
|
|
||||||
|
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
|
||||||
|
BlockUorbEnabledAutopilot(parent, name),
|
||||||
|
_stabilization(this, ""), // no name needed, already unique
|
||||||
|
_backsideAutopilot(this, "", &_stabilization),
|
||||||
|
_guide(this, ""),
|
||||||
|
_vCmd(this, "V_CMD"),
|
||||||
|
_attPoll(),
|
||||||
|
_lastPosCmd(),
|
||||||
|
_timeStamp(0)
|
||||||
|
{
|
||||||
|
_attPoll.fd = _att.getHandle();
|
||||||
|
_attPoll.events = POLLIN;
|
||||||
|
}
|
||||||
|
|
||||||
|
void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
{
|
||||||
|
// wait for a sensor update, check for exit condition every 100 ms
|
||||||
|
if (poll(&_attPoll, 1, 100) < 0) return; // poll error
|
||||||
|
|
||||||
|
uint64_t newTimeStamp = hrt_absolute_time();
|
||||||
|
float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
|
||||||
|
_timeStamp = newTimeStamp;
|
||||||
|
|
||||||
|
// check for sane values of dt
|
||||||
|
// to prevent large control responses
|
||||||
|
if (dt > 1.0f || dt < 0) return;
|
||||||
|
|
||||||
|
// set dt for all child blocks
|
||||||
|
setDt(dt);
|
||||||
|
|
||||||
|
// store old position command before update if new command sent
|
||||||
|
if (_posCmd.updated()) {
|
||||||
|
_lastPosCmd = _posCmd.getData();
|
||||||
|
}
|
||||||
|
|
||||||
|
// check for new updates
|
||||||
|
if (_param_update.updated()) updateParams();
|
||||||
|
|
||||||
|
// get new information from subscriptions
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
// default all output to zero unless handled by mode
|
||||||
|
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||||
|
_actuators.control[i] = 0.0f;
|
||||||
|
|
||||||
|
// handle autopilot modes
|
||||||
|
if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||||
|
_stabilization.update(
|
||||||
|
_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
|
||||||
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||||
|
_actuators.control[CH_AIL] = _stabilization.getAileron();
|
||||||
|
_actuators.control[CH_ELV] = _stabilization.getElevator();
|
||||||
|
_actuators.control[CH_RDR] = _stabilization.getRudder();
|
||||||
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
|
|
||||||
|
} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
|
||||||
|
// update guidance
|
||||||
|
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
||||||
|
|
||||||
|
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||||
|
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
|
||||||
|
|
||||||
|
// commands
|
||||||
|
float rCmd = 0;
|
||||||
|
|
||||||
|
_backsideAutopilot.update(
|
||||||
|
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
|
||||||
|
_pos.alt, v,
|
||||||
|
_att.roll, _att.pitch, _att.yaw,
|
||||||
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed
|
||||||
|
);
|
||||||
|
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
|
||||||
|
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
|
||||||
|
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
|
||||||
|
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
|
||||||
|
|
||||||
|
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
|
_actuators.control[CH_AIL] = _manual.roll;
|
||||||
|
_actuators.control[CH_ELV] = _manual.pitch;
|
||||||
|
_actuators.control[CH_RDR] = _manual.yaw;
|
||||||
|
_actuators.control[CH_THR] = _manual.throttle;
|
||||||
|
}
|
||||||
|
|
||||||
|
// update all publications
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
|
||||||
|
{
|
||||||
|
// send one last publication when destroyed, setting
|
||||||
|
// all output to zero
|
||||||
|
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||||
|
_actuators.control[i] = 0.0f;
|
||||||
|
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace fixedwing
|
||||||
|
|
||||||
|
} // namespace control
|
||||||
|
|
|
@ -0,0 +1,438 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file fixedwing.h
|
||||||
|
*
|
||||||
|
* Controller library code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_rates_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||||
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/actuator_controls.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <poll.h>
|
||||||
|
|
||||||
|
#include "blocks.hpp"
|
||||||
|
#include "block/UOrbSubscription.hpp"
|
||||||
|
#include "block/UOrbPublication.hpp"
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
#include <systemlib/geo/geo.h>
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace control
|
||||||
|
{
|
||||||
|
|
||||||
|
namespace fixedwing
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* BlockYawDamper
|
||||||
|
*
|
||||||
|
* This block has more explations to help new developers
|
||||||
|
* add their own blocks. It includes a limited explanation
|
||||||
|
* of some C++ basics.
|
||||||
|
*
|
||||||
|
* Block: The generic class describing a typical block as you
|
||||||
|
* would expect in Simulink or ScicosLab. A block can have
|
||||||
|
* parameters. It cannot have other blocks.
|
||||||
|
*
|
||||||
|
* SuperBlock: A superblock is a block that can have other
|
||||||
|
* blocks. It has methods that manage the blocks beneath it.
|
||||||
|
*
|
||||||
|
* BlockYawDamper inherits from SuperBlock publically, this
|
||||||
|
* means that any public function in SuperBlock are public within
|
||||||
|
* BlockYawDamper and may be called from outside the
|
||||||
|
* class methods. Any protected function within block
|
||||||
|
* are private to the class and may not be called from
|
||||||
|
* outside this class. Protected should be preferred
|
||||||
|
* where possible to public as it is important to
|
||||||
|
* limit access to the bare minimum to prevent
|
||||||
|
* accidental errors.
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockYawDamper : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
/**
|
||||||
|
* Declaring other blocks used by this block
|
||||||
|
*
|
||||||
|
* In this section we declare all child blocks that
|
||||||
|
* this block is composed of. They are private
|
||||||
|
* members so only this block has direct access to
|
||||||
|
* them.
|
||||||
|
*/
|
||||||
|
BlockLowPass _rLowPass;
|
||||||
|
BlockHighPass _rWashout;
|
||||||
|
BlockP _r2Rdr;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Declaring output values and accessors
|
||||||
|
*
|
||||||
|
* If we have any output values for the block we
|
||||||
|
* declare them here. Output can be directly returned
|
||||||
|
* through the update function, but outputs should be
|
||||||
|
* declared here if the information will likely be requested
|
||||||
|
* again, or if there are multiple outputs.
|
||||||
|
*
|
||||||
|
* You should only be able to set outputs from this block,
|
||||||
|
* so the outputs are declared in the private section.
|
||||||
|
* A get accessor is provided
|
||||||
|
* in the public section for other blocks to get the
|
||||||
|
* value of the output.
|
||||||
|
*/
|
||||||
|
float _rudder;
|
||||||
|
public:
|
||||||
|
/**
|
||||||
|
* BlockYawDamper Constructor
|
||||||
|
*
|
||||||
|
* The job of the constructor is to initialize all
|
||||||
|
* parameter in this block and initialize all child
|
||||||
|
* blocks. Note also, that the output values must be
|
||||||
|
* initialized here. The order of the members in the
|
||||||
|
* member initialization list should follow the
|
||||||
|
* order in which they are declared within the class.
|
||||||
|
* See the private member declarations above.
|
||||||
|
*
|
||||||
|
* Block Construction
|
||||||
|
*
|
||||||
|
* All blocks are constructed with their parent block
|
||||||
|
* and their name. This allows parameters within the
|
||||||
|
* block to construct a fully qualified name from
|
||||||
|
* concatenating the two. If the name provided to the
|
||||||
|
* block is "", then the block will use the parent
|
||||||
|
* name as it's name. This is useful in cases where
|
||||||
|
* you have a block that has parameters "MIN", "MAX",
|
||||||
|
* such as BlockLimit and you do not want an extra name
|
||||||
|
* to qualify them since the parent block has no "MIN",
|
||||||
|
* "MAX" parameters.
|
||||||
|
*
|
||||||
|
* Block Parameter Construction
|
||||||
|
*
|
||||||
|
* Block parameters are named constants, they are
|
||||||
|
* constructed using:
|
||||||
|
* BlockParam::BlockParam(Block * parent, const char * name)
|
||||||
|
* This funciton takes both a parent block and a name.
|
||||||
|
* The constructore then uses the parent name and the name of
|
||||||
|
* the paramter to ask the px4 param library if it has any
|
||||||
|
* parameters with this name. If it does, a handle to the
|
||||||
|
* parameter is retrieved.
|
||||||
|
*
|
||||||
|
* Block/ BlockParam Naming
|
||||||
|
*
|
||||||
|
* When desigining new blocks, the naming of the parameters and
|
||||||
|
* blocks determines the fully qualified name of the parameters
|
||||||
|
* within the ground station, so it is important to choose
|
||||||
|
* short, easily understandable names. Again, when a name of
|
||||||
|
* "" is passed, the parent block name is used as the value to
|
||||||
|
* prepend to paramters names.
|
||||||
|
*/
|
||||||
|
BlockYawDamper(SuperBlock *parent, const char *name);
|
||||||
|
/**
|
||||||
|
* Block deconstructor
|
||||||
|
*
|
||||||
|
* It is always a good idea to declare a virtual
|
||||||
|
* deconstructor so that upon calling delete from
|
||||||
|
* a class derived from this, all of the
|
||||||
|
* deconstructors all called, the derived class first, and
|
||||||
|
* then the base class
|
||||||
|
*/
|
||||||
|
virtual ~BlockYawDamper();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Block update function
|
||||||
|
*
|
||||||
|
* The job of the update function is to compute the output
|
||||||
|
* values for the block. In a simple block with one output,
|
||||||
|
* the output may be returned directly. If the output is
|
||||||
|
* required frequenly by other processses, it might be a
|
||||||
|
* good idea to declare a member to store the temporary
|
||||||
|
* variable.
|
||||||
|
*/
|
||||||
|
void update(float rCmd, float r);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Rudder output value accessor
|
||||||
|
*
|
||||||
|
* This is a public accessor function, which means that the
|
||||||
|
* private value _rudder is returned to anyone calling
|
||||||
|
* BlockYawDamper::getRudder(). Note thate a setRudder() is
|
||||||
|
* not provided, this is because the updateParams() call
|
||||||
|
* for a block provides the mechanism for updating the
|
||||||
|
* paramter.
|
||||||
|
*/
|
||||||
|
float getRudder() { return _rudder; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Stability augmentation system.
|
||||||
|
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockStabilization : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockYawDamper _yawDamper;
|
||||||
|
BlockLowPass _pLowPass;
|
||||||
|
BlockLowPass _qLowPass;
|
||||||
|
BlockP _p2Ail;
|
||||||
|
BlockP _q2Elv;
|
||||||
|
float _aileron;
|
||||||
|
float _elevator;
|
||||||
|
public:
|
||||||
|
BlockStabilization(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockStabilization();
|
||||||
|
void update(float pCmd, float qCmd, float rCmd,
|
||||||
|
float p, float q, float r);
|
||||||
|
float getAileron() { return _aileron; }
|
||||||
|
float getElevator() { return _elevator; }
|
||||||
|
float getRudder() { return _yawDamper.getRudder(); }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Heading hold autopilot block.
|
||||||
|
* Aircraft Control and Simulation, Stevens and Lewis
|
||||||
|
* Heading hold, pg. 348
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockHeadingHold : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockP _psi2Phi;
|
||||||
|
BlockP _phi2P;
|
||||||
|
BlockLimitSym _phiLimit;
|
||||||
|
public:
|
||||||
|
BlockHeadingHold(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockHeadingHold();
|
||||||
|
/**
|
||||||
|
* returns pCmd
|
||||||
|
*/
|
||||||
|
float update(float psiCmd, float phi, float psi, float p);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside/ Backside Control Systems
|
||||||
|
*
|
||||||
|
* Frontside :
|
||||||
|
* velocity error -> throttle
|
||||||
|
* altitude error -> elevator
|
||||||
|
*
|
||||||
|
* Backside :
|
||||||
|
* velocity error -> elevator
|
||||||
|
* altitude error -> throttle
|
||||||
|
*
|
||||||
|
* Backside control systems are more resilient at
|
||||||
|
* slow speeds on the back-side of the power
|
||||||
|
* required curve/ landing etc. Less performance
|
||||||
|
* than frontside at high speeds.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside velocity hold autopilot block.
|
||||||
|
* v -> theta -> q -> elevator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockVelocityHoldBackside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _v2Theta;
|
||||||
|
BlockPID _theta2Q;
|
||||||
|
BlockLimit _theLimit;
|
||||||
|
BlockLimit _vLimit;
|
||||||
|
public:
|
||||||
|
BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockVelocityHoldBackside();
|
||||||
|
/**
|
||||||
|
* returns qCmd
|
||||||
|
*/
|
||||||
|
float update(float vCmd, float v, float theta, float q);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside velocity hold autopilot block.
|
||||||
|
* v -> throttle
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _v2Thr;
|
||||||
|
public:
|
||||||
|
BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockVelocityHoldFrontside();
|
||||||
|
/**
|
||||||
|
* returns throttle
|
||||||
|
*/
|
||||||
|
float update(float vCmd, float v);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside altitude hold autopilot block.
|
||||||
|
* h -> throttle
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _h2Thr;
|
||||||
|
float _throttle;
|
||||||
|
public:
|
||||||
|
BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockAltitudeHoldBackside();
|
||||||
|
void update(float hCmd, float h);
|
||||||
|
float getThrottle() { return _throttle; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Frontside altitude hold autopilot block.
|
||||||
|
* h -> theta > q -> elevator
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockPID _h2Theta;
|
||||||
|
BlockPID _theta2Q;
|
||||||
|
public:
|
||||||
|
BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockAltitudeHoldFrontside();
|
||||||
|
/**
|
||||||
|
* return qCmd
|
||||||
|
*/
|
||||||
|
float update(float hCmd, float h, float theta, float q);
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Backside autopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockBacksideAutopilot : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockStabilization *_stabilization;
|
||||||
|
BlockHeadingHold _headingHold;
|
||||||
|
BlockVelocityHoldBackside _velocityHold;
|
||||||
|
BlockAltitudeHoldBackside _altitudeHold;
|
||||||
|
BlockParam<float> _trimAil;
|
||||||
|
BlockParam<float> _trimElv;
|
||||||
|
BlockParam<float> _trimRdr;
|
||||||
|
BlockParam<float> _trimThr;
|
||||||
|
public:
|
||||||
|
BlockBacksideAutopilot(SuperBlock *parent,
|
||||||
|
const char *name,
|
||||||
|
BlockStabilization *stabilization);
|
||||||
|
virtual ~BlockBacksideAutopilot();
|
||||||
|
void update(float hCmd, float vCmd, float rCmd, float psiCmd,
|
||||||
|
float h, float v,
|
||||||
|
float phi, float theta, float psi,
|
||||||
|
float p, float q, float r);
|
||||||
|
float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
|
||||||
|
float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
|
||||||
|
float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
|
||||||
|
float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Waypoint Guidance block
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockWaypointGuidance : public SuperBlock
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockLimitSym _xtYawLimit;
|
||||||
|
BlockP _xt2Yaw;
|
||||||
|
float _psiCmd;
|
||||||
|
public:
|
||||||
|
BlockWaypointGuidance(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockWaypointGuidance();
|
||||||
|
void update(vehicle_global_position_s &pos,
|
||||||
|
vehicle_attitude_s &att,
|
||||||
|
vehicle_global_position_setpoint_s &posCmd,
|
||||||
|
vehicle_global_position_setpoint_s &lastPosCmd);
|
||||||
|
float getPsiCmd() { return _psiCmd; }
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* UorbEnabledAutopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
|
||||||
|
{
|
||||||
|
protected:
|
||||||
|
// subscriptions
|
||||||
|
UOrbSubscription<vehicle_attitude_s> _att;
|
||||||
|
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||||
|
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||||
|
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||||
|
UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
|
||||||
|
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||||
|
UOrbSubscription<vehicle_status_s> _status;
|
||||||
|
UOrbSubscription<parameter_update_s> _param_update;
|
||||||
|
// publications
|
||||||
|
UOrbPublication<actuator_controls_s> _actuators;
|
||||||
|
public:
|
||||||
|
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~BlockUorbEnabledAutopilot();
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Multi-mode Autopilot
|
||||||
|
*/
|
||||||
|
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
BlockStabilization _stabilization;
|
||||||
|
BlockBacksideAutopilot _backsideAutopilot;
|
||||||
|
BlockWaypointGuidance _guide;
|
||||||
|
BlockParam<float> _vCmd;
|
||||||
|
|
||||||
|
struct pollfd _attPoll;
|
||||||
|
vehicle_global_position_setpoint_s _lastPosCmd;
|
||||||
|
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
||||||
|
uint64_t _timeStamp;
|
||||||
|
public:
|
||||||
|
BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
|
||||||
|
void update();
|
||||||
|
virtual ~BlockMultiModeBacksideAutopilot();
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace fixedwing
|
||||||
|
|
||||||
|
} // namespace control
|
||||||
|
|
|
@ -0,0 +1,22 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
// WARNING:
|
||||||
|
// do not changes these unless
|
||||||
|
// you want to recompute the
|
||||||
|
// answers for all of the unit tests
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
|
||||||
|
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Redistribution and use in source and binary forms, with or without
|
||||||
|
# modification, are permitted provided that the following conditions
|
||||||
|
# are met:
|
||||||
|
#
|
||||||
|
# 1. Redistributions of source code must retain the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer.
|
||||||
|
# 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer in
|
||||||
|
# the documentation and/or other materials provided with the
|
||||||
|
# distribution.
|
||||||
|
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
# used to endorse or promote products derived from this software
|
||||||
|
# without specific prior written permission.
|
||||||
|
#
|
||||||
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Basic example application
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = control_demo
|
||||||
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
|
STACKSIZE = 2048
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,168 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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
|
||||||
|
* 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 control_demo.cpp
|
||||||
|
* Demonstration of control library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <controllib/fixedwing.hpp>
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||||
|
static bool thread_running = false; /**< Deamon status flag */
|
||||||
|
static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deamon management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int control_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Mainloop of deamon.
|
||||||
|
*/
|
||||||
|
int control_demo_thread_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Test function
|
||||||
|
*/
|
||||||
|
void test();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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: control_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The deamon app only briefly exists to start
|
||||||
|
* the background job. The stack size assigned in the
|
||||||
|
* Makefile does only apply to this management task.
|
||||||
|
*
|
||||||
|
* The actual stack size should be set in the call
|
||||||
|
* to task_create().
|
||||||
|
*/
|
||||||
|
int control_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
|
if (thread_running) {
|
||||||
|
printf("control_demo already running\n");
|
||||||
|
/* this is not an error */
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
thread_should_exit = false;
|
||||||
|
deamon_task = task_spawn("control_demo",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MAX - 10,
|
||||||
|
4096,
|
||||||
|
control_demo_thread_main,
|
||||||
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "test")) {
|
||||||
|
test();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
thread_should_exit = true;
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "status")) {
|
||||||
|
if (thread_running) {
|
||||||
|
printf("\tcontrol_demo app is running\n");
|
||||||
|
|
||||||
|
} else {
|
||||||
|
printf("\tcontrol_demo app not started\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
int control_demo_thread_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
printf("[control_Demo] starting\n");
|
||||||
|
|
||||||
|
using namespace control;
|
||||||
|
|
||||||
|
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
|
||||||
|
|
||||||
|
thread_running = true;
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
autopilot.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("[control_demo] exiting.\n");
|
||||||
|
|
||||||
|
thread_running = false;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void test()
|
||||||
|
{
|
||||||
|
printf("beginning control lib test\n");
|
||||||
|
control::basicBlocksTest();
|
||||||
|
}
|
|
@ -0,0 +1,63 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
// currently tuned for easystar from arkhangar in HIL
|
||||||
|
//https://github.com/arktools/arkhangar
|
||||||
|
|
||||||
|
// 16 is max name length
|
||||||
|
|
||||||
|
// gyro low pass filter
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
|
||||||
|
|
||||||
|
// yaw washout
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
|
||||||
|
|
||||||
|
// stabilization mode
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
|
||||||
|
|
||||||
|
// psi -> phi -> p
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f); // heading 2 roll
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f); // roll to roll rate
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f); // roll limit
|
||||||
|
|
||||||
|
// velocity -> theta
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
|
||||||
|
|
||||||
|
|
||||||
|
// theta -> q
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
|
||||||
|
|
||||||
|
// h -> thr
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
|
||||||
|
|
||||||
|
// crosstrack
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
|
||||||
|
|
||||||
|
// speed command
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
|
||||||
|
|
||||||
|
// trim
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
|
|
@ -0,0 +1,627 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file KalmanNav.cpp
|
||||||
|
*
|
||||||
|
* kalman filter navigation code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <poll.h>
|
||||||
|
|
||||||
|
#include "KalmanNav.hpp"
|
||||||
|
|
||||||
|
// constants
|
||||||
|
static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
|
||||||
|
static const float R = 6.371000e6f; // earth radius, m
|
||||||
|
static const float RSq = 4.0589641e13f; // radius squared
|
||||||
|
static const float g = 9.8f; // gravitational accel. m/s^2
|
||||||
|
|
||||||
|
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||||
|
SuperBlock(parent, name),
|
||||||
|
// ekf matrices
|
||||||
|
F(9, 9),
|
||||||
|
G(9, 6),
|
||||||
|
P(9, 9),
|
||||||
|
V(6, 6),
|
||||||
|
// attitude measurement ekf matrices
|
||||||
|
HAtt(6, 9),
|
||||||
|
RAtt(6, 6),
|
||||||
|
// gps measurement ekf matrices
|
||||||
|
HGps(6, 9),
|
||||||
|
RGps(6, 6),
|
||||||
|
// attitude representations
|
||||||
|
C_nb(),
|
||||||
|
q(),
|
||||||
|
// subscriptions
|
||||||
|
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
|
||||||
|
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
|
||||||
|
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
|
||||||
|
// publications
|
||||||
|
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
|
||||||
|
_att(&getPublications(), ORB_ID(vehicle_attitude)),
|
||||||
|
// timestamps
|
||||||
|
_pubTimeStamp(hrt_absolute_time()),
|
||||||
|
_fastTimeStamp(hrt_absolute_time()),
|
||||||
|
_slowTimeStamp(hrt_absolute_time()),
|
||||||
|
_attTimeStamp(hrt_absolute_time()),
|
||||||
|
_outTimeStamp(hrt_absolute_time()),
|
||||||
|
// frame count
|
||||||
|
_navFrames(0),
|
||||||
|
// state
|
||||||
|
fN(0), fE(0), fD(0),
|
||||||
|
phi(0), theta(0), psi(0),
|
||||||
|
vN(0), vE(0), vD(0),
|
||||||
|
lat(0), lon(0), alt(0),
|
||||||
|
// parameters for ground station
|
||||||
|
_vGyro(this, "V_GYRO"),
|
||||||
|
_vAccel(this, "V_ACCEL"),
|
||||||
|
_rMag(this, "R_MAG"),
|
||||||
|
_rGpsV(this, "R_GPS_V"),
|
||||||
|
_rGpsGeo(this, "R_GPS_GEO"),
|
||||||
|
_rGpsAlt(this, "R_GPS_ALT"),
|
||||||
|
_rAccel(this, "R_ACCEL")
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// initial state covariance matrix
|
||||||
|
P = Matrix::identity(9) * 1.0f;
|
||||||
|
|
||||||
|
// wait for gps lock
|
||||||
|
while (1) {
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
if (_gps.fix_type > 2) break;
|
||||||
|
|
||||||
|
printf("[kalman_demo] waiting for gps lock\n");
|
||||||
|
usleep(1000000);
|
||||||
|
}
|
||||||
|
|
||||||
|
// initial state
|
||||||
|
phi = 0.0f;
|
||||||
|
theta = 0.0f;
|
||||||
|
psi = 0.0f;
|
||||||
|
vN = _gps.vel_n;
|
||||||
|
vE = _gps.vel_e;
|
||||||
|
vD = _gps.vel_d;
|
||||||
|
setLatDegE7(_gps.lat);
|
||||||
|
setLonDegE7(_gps.lon);
|
||||||
|
setAltE3(_gps.alt);
|
||||||
|
|
||||||
|
// initialize quaternions
|
||||||
|
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||||
|
|
||||||
|
// initialize dcm
|
||||||
|
C_nb = Dcm(q);
|
||||||
|
|
||||||
|
// initialize F to identity
|
||||||
|
F = Matrix::identity(9);
|
||||||
|
|
||||||
|
// HGps is constant
|
||||||
|
HGps(0, 3) = 1.0f;
|
||||||
|
HGps(1, 4) = 1.0f;
|
||||||
|
HGps(2, 5) = 1.0f;
|
||||||
|
HGps(3, 6) = 1.0f;
|
||||||
|
HGps(4, 7) = 1.0f;
|
||||||
|
HGps(5, 8) = 1.0f;
|
||||||
|
|
||||||
|
// initialize all parameters
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::update()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
struct pollfd fds[2];
|
||||||
|
fds[0].fd = _sensors.getHandle();
|
||||||
|
fds[0].events = POLLIN;
|
||||||
|
fds[1].fd = _param_update.getHandle();
|
||||||
|
fds[1].events = POLLIN;
|
||||||
|
|
||||||
|
// poll 20 milliseconds for new data
|
||||||
|
int ret = poll(fds, 2, 20);
|
||||||
|
|
||||||
|
// check return value
|
||||||
|
if (ret < 0) {
|
||||||
|
// XXX this is seriously bad - should be an emergency
|
||||||
|
return;
|
||||||
|
|
||||||
|
} else if (ret == 0) { // timeout
|
||||||
|
// run anyway
|
||||||
|
} else if (ret > 0) {
|
||||||
|
// update params when requested
|
||||||
|
if (fds[1].revents & POLLIN) {
|
||||||
|
printf("updating params\n");
|
||||||
|
updateParams();
|
||||||
|
}
|
||||||
|
|
||||||
|
// if no new sensor data, return
|
||||||
|
if (!(fds[0].revents & POLLIN)) return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get new timestamp
|
||||||
|
uint64_t newTimeStamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
// check updated subscriptions
|
||||||
|
bool gpsUpdate = _gps.updated();
|
||||||
|
|
||||||
|
// get new information from subscriptions
|
||||||
|
// this clears update flag
|
||||||
|
updateSubscriptions();
|
||||||
|
|
||||||
|
// count fast frames
|
||||||
|
_navFrames += 1;
|
||||||
|
|
||||||
|
// fast prediciton step
|
||||||
|
// note, using sensors timestamp so we can account
|
||||||
|
// for packet lag
|
||||||
|
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
|
||||||
|
_fastTimeStamp = _sensors.timestamp;
|
||||||
|
predictFast(dtFast);
|
||||||
|
|
||||||
|
// slow prediction step
|
||||||
|
float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
|
||||||
|
|
||||||
|
if (dtSlow > 1.0f / 200) { // 200 Hz
|
||||||
|
_slowTimeStamp = _sensors.timestamp;
|
||||||
|
predictSlow(dtSlow);
|
||||||
|
}
|
||||||
|
|
||||||
|
// gps correction step
|
||||||
|
if (gpsUpdate) {
|
||||||
|
correctGps();
|
||||||
|
}
|
||||||
|
|
||||||
|
// attitude correction step
|
||||||
|
if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
|
||||||
|
_attTimeStamp = _sensors.timestamp;
|
||||||
|
correctAtt();
|
||||||
|
}
|
||||||
|
|
||||||
|
// publication
|
||||||
|
if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
|
||||||
|
_pubTimeStamp = newTimeStamp;
|
||||||
|
updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
// output
|
||||||
|
if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
|
||||||
|
_outTimeStamp = newTimeStamp;
|
||||||
|
printf("nav: %4d Hz\n", _navFrames);
|
||||||
|
_navFrames = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::updatePublications()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// position publication
|
||||||
|
_pos.timestamp = _pubTimeStamp;
|
||||||
|
_pos.time_gps_usec = _gps.timestamp;
|
||||||
|
_pos.valid = true;
|
||||||
|
_pos.lat = getLatDegE7();
|
||||||
|
_pos.lon = getLonDegE7();
|
||||||
|
_pos.alt = float(alt);
|
||||||
|
_pos.relative_alt = float(alt); // TODO, make relative
|
||||||
|
_pos.vx = vN;
|
||||||
|
_pos.vy = vE;
|
||||||
|
_pos.vz = vD;
|
||||||
|
_pos.hdg = psi;
|
||||||
|
|
||||||
|
// attitude publication
|
||||||
|
_att.timestamp = _pubTimeStamp;
|
||||||
|
_att.roll = phi;
|
||||||
|
_att.pitch = theta;
|
||||||
|
_att.yaw = psi;
|
||||||
|
_att.rollspeed = _sensors.gyro_rad_s[0];
|
||||||
|
_att.pitchspeed = _sensors.gyro_rad_s[1];
|
||||||
|
_att.yawspeed = _sensors.gyro_rad_s[2];
|
||||||
|
// TODO, add gyro offsets to filter
|
||||||
|
_att.rate_offsets[0] = 0.0f;
|
||||||
|
_att.rate_offsets[1] = 0.0f;
|
||||||
|
_att.rate_offsets[2] = 0.0f;
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||||
|
_att.R[i][j] = C_nb(i, j);
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) _att.q[i] = q(i);
|
||||||
|
|
||||||
|
_att.R_valid = true;
|
||||||
|
_att.q_valid = true;
|
||||||
|
_att.counter = _navFrames;
|
||||||
|
|
||||||
|
// update publications
|
||||||
|
SuperBlock::updatePublications();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::predictFast(float dt)
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
Vector3 w(_sensors.gyro_rad_s);
|
||||||
|
|
||||||
|
// attitude
|
||||||
|
q = q + q.derivative(w) * dt;
|
||||||
|
|
||||||
|
// renormalize quaternion if needed
|
||||||
|
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
|
||||||
|
q = q.unit();
|
||||||
|
}
|
||||||
|
|
||||||
|
// C_nb update
|
||||||
|
C_nb = Dcm(q);
|
||||||
|
|
||||||
|
// euler update
|
||||||
|
EulerAngles euler(C_nb);
|
||||||
|
phi = euler.getPhi();
|
||||||
|
theta = euler.getTheta();
|
||||||
|
psi = euler.getPsi();
|
||||||
|
|
||||||
|
// specific acceleration in nav frame
|
||||||
|
Vector3 accelB(_sensors.accelerometer_m_s2);
|
||||||
|
Vector3 accelN = C_nb * accelB;
|
||||||
|
fN = accelN(0);
|
||||||
|
fE = accelN(1);
|
||||||
|
fD = accelN(2);
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float sinL = sinf(lat);
|
||||||
|
float cosL = cosf(lat);
|
||||||
|
|
||||||
|
// position update
|
||||||
|
// neglects angular deflections in local gravity
|
||||||
|
// see Titerton pg. 70
|
||||||
|
float LDot = vN / (R + float(alt));
|
||||||
|
float lDot = vE / (cosL * (R + float(alt)));
|
||||||
|
float vNDot = fN - vE * (2 * omega +
|
||||||
|
lDot) * sinL +
|
||||||
|
vD * LDot;
|
||||||
|
float vDDot = fD - vE * (2 * omega + lDot) * cosL -
|
||||||
|
vN * LDot + g;
|
||||||
|
float vEDot = fE + vN * (2 * omega + lDot) * sinL +
|
||||||
|
vDDot * (2 * omega * cosL);
|
||||||
|
|
||||||
|
// rectangular integration
|
||||||
|
vN += vNDot * dt;
|
||||||
|
vE += vEDot * dt;
|
||||||
|
vD += vDDot * dt;
|
||||||
|
lat += double(LDot * dt);
|
||||||
|
lon += double(lDot * dt);
|
||||||
|
alt += double(-vD * dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::predictSlow(float dt)
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float sinL = sinf(lat);
|
||||||
|
float cosL = cosf(lat);
|
||||||
|
float cosLSq = cosL * cosL;
|
||||||
|
float tanL = tanf(lat);
|
||||||
|
|
||||||
|
// F Matrix
|
||||||
|
// Titterton pg. 291
|
||||||
|
//
|
||||||
|
// difference from Jacobian
|
||||||
|
// multiplity by dt for all elements
|
||||||
|
// add 1.0 to diagonal elements
|
||||||
|
|
||||||
|
F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
|
||||||
|
F(0, 2) = (vN / R) * dt;
|
||||||
|
F(0, 4) = (1.0f / R) * dt;
|
||||||
|
F(0, 6) = (-omega * sinL) * dt;
|
||||||
|
F(0, 8) = (-vE / RSq) * dt;
|
||||||
|
|
||||||
|
F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
|
||||||
|
F(1, 2) = (omega * cosL + vE / R) * dt;
|
||||||
|
F(1, 3) = (-1.0f / R) * dt;
|
||||||
|
F(1, 8) = (vN / RSq) * dt;
|
||||||
|
|
||||||
|
F(2, 0) = (-vN / R) * dt;
|
||||||
|
F(2, 1) = (-omega * cosL - vE / R) * dt;
|
||||||
|
F(2, 4) = (-tanL / R) * dt;
|
||||||
|
F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
|
||||||
|
F(2, 8) = (vE * tanL / RSq) * dt;
|
||||||
|
|
||||||
|
F(3, 1) = (-fD) * dt;
|
||||||
|
F(3, 2) = (fE) * dt;
|
||||||
|
F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
|
||||||
|
F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
|
||||||
|
F(3, 5) = (vN / R) * dt;
|
||||||
|
F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
|
||||||
|
F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
|
||||||
|
|
||||||
|
F(4, 0) = (fD) * dt;
|
||||||
|
F(4, 2) = (-fN) * dt;
|
||||||
|
F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
|
||||||
|
F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
|
||||||
|
F(4, 5) = (2 * omega * cosL + vE / R) * dt;
|
||||||
|
F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
|
||||||
|
vN * vE / (R * cosLSq)) * dt;
|
||||||
|
F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
|
||||||
|
|
||||||
|
F(5, 0) = (-fE) * dt;
|
||||||
|
F(5, 1) = (fN) * dt;
|
||||||
|
F(5, 3) = (-2 * vN / R) * dt;
|
||||||
|
F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
|
||||||
|
F(5, 6) = (2 * omega * vE * sinL) * dt;
|
||||||
|
F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
|
||||||
|
|
||||||
|
F(6, 3) = (1 / R) * dt;
|
||||||
|
F(6, 8) = (-vN / RSq) * dt;
|
||||||
|
|
||||||
|
F(7, 4) = (1 / (R * cosL)) * dt;
|
||||||
|
F(7, 6) = (vE * tanL / (R * cosL)) * dt;
|
||||||
|
F(7, 8) = (-vE / (cosL * RSq)) * dt;
|
||||||
|
|
||||||
|
F(8, 5) = (-1) * dt;
|
||||||
|
|
||||||
|
// G Matrix
|
||||||
|
// Titterton pg. 291
|
||||||
|
G(0, 0) = -C_nb(0, 0) * dt;
|
||||||
|
G(0, 1) = -C_nb(0, 1) * dt;
|
||||||
|
G(0, 2) = -C_nb(0, 2) * dt;
|
||||||
|
G(1, 0) = -C_nb(1, 0) * dt;
|
||||||
|
G(1, 1) = -C_nb(1, 1) * dt;
|
||||||
|
G(1, 2) = -C_nb(1, 2) * dt;
|
||||||
|
G(2, 0) = -C_nb(2, 0) * dt;
|
||||||
|
G(2, 1) = -C_nb(2, 1) * dt;
|
||||||
|
G(2, 2) = -C_nb(2, 2) * dt;
|
||||||
|
|
||||||
|
G(3, 3) = C_nb(0, 0) * dt;
|
||||||
|
G(3, 4) = C_nb(0, 1) * dt;
|
||||||
|
G(3, 5) = C_nb(0, 2) * dt;
|
||||||
|
G(4, 3) = C_nb(1, 0) * dt;
|
||||||
|
G(4, 4) = C_nb(1, 1) * dt;
|
||||||
|
G(4, 5) = C_nb(1, 2) * dt;
|
||||||
|
G(5, 3) = C_nb(2, 0) * dt;
|
||||||
|
G(5, 4) = C_nb(2, 1) * dt;
|
||||||
|
G(5, 5) = C_nb(2, 2) * dt;
|
||||||
|
|
||||||
|
// predict equations for kalman filter
|
||||||
|
P = F * P * F.transpose() + G * V * G.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::correctAtt()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
// trig
|
||||||
|
float cosPhi = cosf(phi);
|
||||||
|
float cosTheta = cosf(theta);
|
||||||
|
float cosPsi = cosf(psi);
|
||||||
|
float sinPhi = sinf(phi);
|
||||||
|
float sinTheta = sinf(theta);
|
||||||
|
float sinPsi = sinf(psi);
|
||||||
|
|
||||||
|
// mag measurement
|
||||||
|
Vector3 zMag(_sensors.magnetometer_ga);
|
||||||
|
zMag = zMag.unit();
|
||||||
|
|
||||||
|
// mag predicted measurement
|
||||||
|
// choosing some typical magnetic field properties,
|
||||||
|
// TODO dip/dec depend on lat/ lon/ time
|
||||||
|
static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
|
||||||
|
static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
|
||||||
|
float bN = cosf(dip) * cosf(dec);
|
||||||
|
float bE = cosf(dip) * sinf(dec);
|
||||||
|
float bD = sinf(dip);
|
||||||
|
Vector3 bNav(bN, bE, bD);
|
||||||
|
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
|
||||||
|
|
||||||
|
// accel measurement
|
||||||
|
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||||
|
zAccel = zAccel.unit();
|
||||||
|
|
||||||
|
// accel predicted measurement
|
||||||
|
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
|
||||||
|
|
||||||
|
// combined measurement
|
||||||
|
Vector zAtt(6);
|
||||||
|
Vector zAttHat(6);
|
||||||
|
|
||||||
|
for (int i = 0; i < 3; i++) {
|
||||||
|
zAtt(i) = zMag(i);
|
||||||
|
zAtt(i + 3) = zAccel(i);
|
||||||
|
zAttHat(i) = zMagHat(i);
|
||||||
|
zAttHat(i + 3) = zAccelHat(i);
|
||||||
|
}
|
||||||
|
|
||||||
|
// HMag , HAtt (0-2,:)
|
||||||
|
float tmp1 =
|
||||||
|
cosPsi * cosTheta * bN +
|
||||||
|
sinPsi * cosTheta * bE -
|
||||||
|
sinTheta * bD;
|
||||||
|
HAtt(0, 1) = -(
|
||||||
|
cosPsi * sinTheta * bN +
|
||||||
|
sinPsi * sinTheta * bE +
|
||||||
|
cosTheta * bD
|
||||||
|
);
|
||||||
|
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
|
||||||
|
HAtt(1, 0) =
|
||||||
|
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
|
||||||
|
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
|
||||||
|
cosPhi * cosTheta * bD;
|
||||||
|
HAtt(1, 1) = sinPhi * tmp1;
|
||||||
|
HAtt(1, 2) = -(
|
||||||
|
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
|
||||||
|
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
|
||||||
|
);
|
||||||
|
HAtt(2, 0) = -(
|
||||||
|
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
|
||||||
|
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
|
||||||
|
(sinPhi * cosTheta) * bD
|
||||||
|
);
|
||||||
|
HAtt(2, 1) = cosPhi * tmp1;
|
||||||
|
HAtt(2, 2) = -(
|
||||||
|
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
|
||||||
|
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
|
||||||
|
);
|
||||||
|
|
||||||
|
// HAccel , HAtt (3-5,:)
|
||||||
|
HAtt(3, 1) = cosTheta;
|
||||||
|
HAtt(4, 0) = -cosPhi * cosTheta;
|
||||||
|
HAtt(4, 1) = sinPhi * sinTheta;
|
||||||
|
HAtt(5, 0) = sinPhi * cosTheta;
|
||||||
|
HAtt(5, 1) = cosPhi * sinTheta;
|
||||||
|
|
||||||
|
// compute correction
|
||||||
|
Vector y = zAtt - zAttHat; // residual
|
||||||
|
Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
|
||||||
|
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||||
|
Vector xCorrect = K * y;
|
||||||
|
|
||||||
|
// check correciton is sane
|
||||||
|
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||||
|
float val = xCorrect(i);
|
||||||
|
|
||||||
|
if (isnan(val) || isinf(val)) {
|
||||||
|
// abort correction and return
|
||||||
|
printf("[kalman_demo] numerical failure in att correction\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// correct state
|
||||||
|
phi += xCorrect(PHI);
|
||||||
|
theta += xCorrect(THETA);
|
||||||
|
psi += xCorrect(PSI);
|
||||||
|
|
||||||
|
// update state covariance
|
||||||
|
P = P - K * HAtt * P;
|
||||||
|
|
||||||
|
// fault detection
|
||||||
|
float beta = y.dot(S.inverse() * y);
|
||||||
|
printf("attitude: beta = %8.4f\n", (double)beta);
|
||||||
|
|
||||||
|
if (beta > 10.0f) {
|
||||||
|
//printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||||
|
//printf("y:\n"); y.print();
|
||||||
|
}
|
||||||
|
|
||||||
|
// update quaternions from euler
|
||||||
|
// angle correction
|
||||||
|
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::correctGps()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
Vector y(6);
|
||||||
|
y(0) = _gps.vel_n - vN;
|
||||||
|
y(1) = _gps.vel_e - vE;
|
||||||
|
y(2) = _gps.vel_d - vD;
|
||||||
|
y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
|
||||||
|
y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
|
||||||
|
y(5) = double(_gps.alt) / 1.0e3 - alt;
|
||||||
|
|
||||||
|
// compute correction
|
||||||
|
Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
|
||||||
|
Matrix K = P * HGps.transpose() * S.inverse();
|
||||||
|
Vector xCorrect = K * y;
|
||||||
|
|
||||||
|
// check correction is sane
|
||||||
|
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||||
|
float val = xCorrect(i);
|
||||||
|
|
||||||
|
if (isnan(val) || isinf(val)) {
|
||||||
|
// abort correction and return
|
||||||
|
printf("[kalman_demo] numerical failure in gps correction\n");
|
||||||
|
// fallback to GPS
|
||||||
|
vN = _gps.vel_n;
|
||||||
|
vE = _gps.vel_e;
|
||||||
|
vD = _gps.vel_d;
|
||||||
|
setLatDegE7(_gps.lat);
|
||||||
|
setLonDegE7(_gps.lon);
|
||||||
|
setAltE3(_gps.alt);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// correct state
|
||||||
|
vN += xCorrect(VN);
|
||||||
|
vE += xCorrect(VE);
|
||||||
|
vD += xCorrect(VD);
|
||||||
|
lat += double(xCorrect(LAT));
|
||||||
|
lon += double(xCorrect(LON));
|
||||||
|
alt += double(xCorrect(ALT));
|
||||||
|
|
||||||
|
// update state covariance
|
||||||
|
P = P - K * HGps * P;
|
||||||
|
|
||||||
|
// fault detetcion
|
||||||
|
float beta = y.dot(S.inverse() * y);
|
||||||
|
printf("gps: beta = %8.4f\n", (double)beta);
|
||||||
|
|
||||||
|
if (beta > 100.0f) {
|
||||||
|
//printf("fault in gps: beta = %8.4f\n", (double)beta);
|
||||||
|
//printf("y:\n"); y.print();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void KalmanNav::updateParams()
|
||||||
|
{
|
||||||
|
using namespace math;
|
||||||
|
using namespace control;
|
||||||
|
SuperBlock::updateParams();
|
||||||
|
|
||||||
|
// gyro noise
|
||||||
|
V(0, 0) = _vGyro.get(); // gyro x, rad/s
|
||||||
|
V(1, 1) = _vGyro.get(); // gyro y
|
||||||
|
V(2, 2) = _vGyro.get(); // gyro z
|
||||||
|
|
||||||
|
// accel noise
|
||||||
|
V(3, 3) = _vAccel.get(); // accel x, m/s^2
|
||||||
|
V(4, 4) = _vAccel.get(); // accel y
|
||||||
|
V(5, 5) = _vAccel.get(); // accel z
|
||||||
|
|
||||||
|
// magnetometer noise
|
||||||
|
RAtt(0, 0) = _rMag.get(); // normalized direction
|
||||||
|
RAtt(1, 1) = _rMag.get();
|
||||||
|
RAtt(2, 2) = _rMag.get();
|
||||||
|
|
||||||
|
// accelerometer noise
|
||||||
|
RAtt(3, 3) = _rAccel.get(); // normalized direction
|
||||||
|
RAtt(4, 4) = _rAccel.get();
|
||||||
|
RAtt(5, 5) = _rAccel.get();
|
||||||
|
|
||||||
|
// gps noise
|
||||||
|
RGps(0, 0) = _rGpsV.get(); // vn, m/s
|
||||||
|
RGps(1, 1) = _rGpsV.get(); // ve
|
||||||
|
RGps(2, 2) = _rGpsV.get(); // vd
|
||||||
|
RGps(3, 3) = _rGpsGeo.get(); // L, rad
|
||||||
|
RGps(4, 4) = _rGpsGeo.get(); // l, rad
|
||||||
|
RGps(5, 5) = _rGpsAlt.get(); // h, m
|
||||||
|
}
|
|
@ -0,0 +1,116 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file KalmanNav.hpp
|
||||||
|
*
|
||||||
|
* kalman filter navigation code
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
//#define MATRIX_ASSERT
|
||||||
|
//#define VECTOR_ASSERT
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
#include <controllib/blocks.hpp>
|
||||||
|
#include <controllib/block/BlockParam.hpp>
|
||||||
|
#include <controllib/block/UOrbSubscription.hpp>
|
||||||
|
#include <controllib/block/UOrbPublication.hpp>
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_attitude.h>
|
||||||
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
|
#include <uORB/topics/sensor_combined.h>
|
||||||
|
#include <uORB/topics/vehicle_gps_position.h>
|
||||||
|
#include <uORB/topics/parameter_update.h>
|
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <poll.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
class KalmanNav : public control::SuperBlock
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
KalmanNav(SuperBlock *parent, const char *name);
|
||||||
|
virtual ~KalmanNav() {};
|
||||||
|
void update();
|
||||||
|
virtual void updatePublications();
|
||||||
|
void predictFast(float dt);
|
||||||
|
void predictSlow(float dt);
|
||||||
|
void correctAtt();
|
||||||
|
void correctGps();
|
||||||
|
virtual void updateParams();
|
||||||
|
protected:
|
||||||
|
math::Matrix F;
|
||||||
|
math::Matrix G;
|
||||||
|
math::Matrix P;
|
||||||
|
math::Matrix V;
|
||||||
|
math::Matrix HAtt;
|
||||||
|
math::Matrix RAtt;
|
||||||
|
math::Matrix HGps;
|
||||||
|
math::Matrix RGps;
|
||||||
|
math::Dcm C_nb;
|
||||||
|
math::Quaternion q;
|
||||||
|
control::UOrbSubscription<sensor_combined_s> _sensors;
|
||||||
|
control::UOrbSubscription<vehicle_gps_position_s> _gps;
|
||||||
|
control::UOrbSubscription<parameter_update_s> _param_update;
|
||||||
|
control::UOrbPublication<vehicle_global_position_s> _pos;
|
||||||
|
control::UOrbPublication<vehicle_attitude_s> _att;
|
||||||
|
uint64_t _pubTimeStamp;
|
||||||
|
uint64_t _fastTimeStamp;
|
||||||
|
uint64_t _slowTimeStamp;
|
||||||
|
uint64_t _attTimeStamp;
|
||||||
|
uint64_t _outTimeStamp;
|
||||||
|
uint16_t _navFrames;
|
||||||
|
float fN, fE, fD;
|
||||||
|
// states
|
||||||
|
enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
|
||||||
|
float phi, theta, psi;
|
||||||
|
float vN, vE, vD;
|
||||||
|
double lat, lon, alt;
|
||||||
|
control::BlockParam<float> _vGyro;
|
||||||
|
control::BlockParam<float> _vAccel;
|
||||||
|
control::BlockParam<float> _rMag;
|
||||||
|
control::BlockParam<float> _rGpsV;
|
||||||
|
control::BlockParam<float> _rGpsGeo;
|
||||||
|
control::BlockParam<float> _rGpsAlt;
|
||||||
|
control::BlockParam<float> _rAccel;
|
||||||
|
int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
|
||||||
|
void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||||
|
int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
|
||||||
|
void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
|
||||||
|
int32_t getAltE3() { return int32_t(alt * 1.0e3); }
|
||||||
|
void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
|
||||||
|
};
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Redistribution and use in source and binary forms, with or without
|
||||||
|
# modification, are permitted provided that the following conditions
|
||||||
|
# are met:
|
||||||
|
#
|
||||||
|
# 1. Redistributions of source code must retain the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer.
|
||||||
|
# 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer in
|
||||||
|
# the documentation and/or other materials provided with the
|
||||||
|
# distribution.
|
||||||
|
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
# used to endorse or promote products derived from this software
|
||||||
|
# without specific prior written permission.
|
||||||
|
#
|
||||||
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Basic example application
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = kalman_demo
|
||||||
|
PRIORITY = SCHED_PRIORITY_MAX - 30
|
||||||
|
STACKSIZE = 2048
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,152 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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
|
||||||
|
* 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 kalman_demo.cpp
|
||||||
|
* Demonstration of control library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
#include <drivers/drv_hrt.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include "KalmanNav.hpp"
|
||||||
|
|
||||||
|
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||||
|
static bool thread_running = false; /**< Deamon status flag */
|
||||||
|
static int deamon_task; /**< Handle of deamon task / thread */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Deamon management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int kalman_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Mainloop of deamon.
|
||||||
|
*/
|
||||||
|
int kalman_demo_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: kalman_demo {start|stop|status} [-p <additional params>]\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The deamon app only briefly exists to start
|
||||||
|
* the background job. The stack size assigned in the
|
||||||
|
* Makefile does only apply to this management task.
|
||||||
|
*
|
||||||
|
* The actual stack size should be set in the call
|
||||||
|
* to task_create().
|
||||||
|
*/
|
||||||
|
int kalman_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "start")) {
|
||||||
|
|
||||||
|
if (thread_running) {
|
||||||
|
printf("kalman_demo already running\n");
|
||||||
|
/* this is not an error */
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
thread_should_exit = false;
|
||||||
|
deamon_task = task_spawn("kalman_demo",
|
||||||
|
SCHED_DEFAULT,
|
||||||
|
SCHED_PRIORITY_MAX - 5,
|
||||||
|
4096,
|
||||||
|
kalman_demo_thread_main,
|
||||||
|
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "stop")) {
|
||||||
|
thread_should_exit = true;
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "status")) {
|
||||||
|
if (thread_running) {
|
||||||
|
printf("\tkalman_demo app is running\n");
|
||||||
|
|
||||||
|
} else {
|
||||||
|
printf("\tkalman_demo app not started\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
int kalman_demo_thread_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
printf("[kalman_demo] starting\n");
|
||||||
|
|
||||||
|
using namespace math;
|
||||||
|
|
||||||
|
thread_running = true;
|
||||||
|
|
||||||
|
KalmanNav nav(NULL, "KF");
|
||||||
|
|
||||||
|
while (!thread_should_exit) {
|
||||||
|
nav.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("[kalman_demo] exiting.\n");
|
||||||
|
|
||||||
|
thread_running = false;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
|
@ -0,0 +1,10 @@
|
||||||
|
#include <systemlib/param/param.h>
|
||||||
|
|
||||||
|
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
|
||||||
|
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
|
|
@ -0,0 +1,42 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||||
|
#
|
||||||
|
# Redistribution and use in source and binary forms, with or without
|
||||||
|
# modification, are permitted provided that the following conditions
|
||||||
|
# are met:
|
||||||
|
#
|
||||||
|
# 1. Redistributions of source code must retain the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer.
|
||||||
|
# 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer in
|
||||||
|
# the documentation and/or other materials provided with the
|
||||||
|
# distribution.
|
||||||
|
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
# used to endorse or promote products derived from this software
|
||||||
|
# without specific prior written permission.
|
||||||
|
#
|
||||||
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
#
|
||||||
|
############################################################################
|
||||||
|
|
||||||
|
#
|
||||||
|
# Basic example application
|
||||||
|
#
|
||||||
|
|
||||||
|
APPNAME = math_demo
|
||||||
|
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||||
|
STACKSIZE = 8192
|
||||||
|
|
||||||
|
include $(APPDIR)/mk/app.mk
|
|
@ -0,0 +1,105 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2012 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
|
||||||
|
* 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 math_demo.cpp
|
||||||
|
* Demonstration of math library
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <nuttx/config.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <systemlib/systemlib.h>
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Management function.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Test function
|
||||||
|
*/
|
||||||
|
void test();
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 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: math_demo {test}\n\n");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The deamon app only briefly exists to start
|
||||||
|
* the background job. The stack size assigned in the
|
||||||
|
* Makefile does only apply to this management task.
|
||||||
|
*
|
||||||
|
* The actual stack size should be set in the call
|
||||||
|
* to task_create().
|
||||||
|
*/
|
||||||
|
int math_demo_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
|
||||||
|
if (argc < 1)
|
||||||
|
usage("missing command");
|
||||||
|
|
||||||
|
if (!strcmp(argv[1], "test")) {
|
||||||
|
test();
|
||||||
|
exit(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
usage("unrecognized command");
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void test()
|
||||||
|
{
|
||||||
|
printf("beginning math lib test\n");
|
||||||
|
using namespace math;
|
||||||
|
vectorTest();
|
||||||
|
matrixTest();
|
||||||
|
vector3Test();
|
||||||
|
eulerAnglesTest();
|
||||||
|
quaternionTest();
|
||||||
|
dcmTest();
|
||||||
|
}
|
|
@ -0,0 +1,159 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,173 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,125 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,152 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,143 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,129 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,125 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,138 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,154 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,172 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,152 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,143 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,128 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,137 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,137 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,124 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,120 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,158 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,131 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,130 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,161 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,157 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,221 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,144 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,243 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,195 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,215 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,135 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,141 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,126 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,174 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,153 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,172 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,160 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,144 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,157 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,145 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,177 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,207 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,140 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,153 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,199 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,185 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,318 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,217 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,195 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,215 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,79 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,114 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,99 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,57 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,56 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* 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
|
||||||
|
*/
|
|
@ -0,0 +1,57 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_pid_reset_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 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 Q31 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_q31(
|
||||||
|
arm_pid_instance_q31 * S)
|
||||||
|
{
|
||||||
|
|
||||||
|
/* Clear the state buffer. The size will be always 3 samples */
|
||||||
|
memset(S->state, 0, 3u * sizeof(q31_t));
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of PID group
|
||||||
|
*/
|
|
@ -0,0 +1,428 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sin_cos_f32.c
|
||||||
|
*
|
||||||
|
* Description: Sine and Cosine calculation for floating-point values.
|
||||||
|
*
|
||||||
|
* 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 groupController
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup SinCos Sine Cosine
|
||||||
|
*
|
||||||
|
* Computes the trigonometric sine and cosine values using a combination of table lookup
|
||||||
|
* and linear interpolation.
|
||||||
|
* There are separate functions for Q31 and floating-point data types.
|
||||||
|
* The input to the floating-point version is in degrees while the
|
||||||
|
* fixed-point Q31 have a scaled input with the range
|
||||||
|
* [-1 0.9999] mapping to [-180 179] degrees.
|
||||||
|
*
|
||||||
|
* The implementation is based on table lookup using 360 values together with linear interpolation.
|
||||||
|
* The steps used are:
|
||||||
|
* -# Calculation of the nearest integer table index.
|
||||||
|
* -# Compute the fractional portion (fract) of the input.
|
||||||
|
* -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.
|
||||||
|
* -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.
|
||||||
|
* -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.
|
||||||
|
* -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup SinCos
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Cosine Table is generated from following loop
|
||||||
|
* <pre>for(i = 0; i < 360; i++)
|
||||||
|
* {
|
||||||
|
* cosTable[i]= cos((i-180) * PI/180.0);
|
||||||
|
* } </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const float32_t cosTable[360] = {
|
||||||
|
-0.999847695156391270f, -0.999390827019095760f, -0.998629534754573830f,
|
||||||
|
-0.997564050259824200f, -0.996194698091745550f, -0.994521895368273290f,
|
||||||
|
-0.992546151641321980f, -0.990268068741570250f,
|
||||||
|
-0.987688340595137660f, -0.984807753012208020f, -0.981627183447663980f,
|
||||||
|
-0.978147600733805690f, -0.974370064785235250f, -0.970295726275996470f,
|
||||||
|
-0.965925826289068200f, -0.961261695938318670f,
|
||||||
|
-0.956304755963035440f, -0.951056516295153530f, -0.945518575599316740f,
|
||||||
|
-0.939692620785908320f, -0.933580426497201740f, -0.927183854566787310f,
|
||||||
|
-0.920504853452440150f, -0.913545457642600760f,
|
||||||
|
-0.906307787036649940f, -0.898794046299167040f, -0.891006524188367790f,
|
||||||
|
-0.882947592858926770f, -0.874619707139395740f, -0.866025403784438710f,
|
||||||
|
-0.857167300702112220f, -0.848048096156425960f,
|
||||||
|
-0.838670567945424160f, -0.829037572555041620f, -0.819152044288991580f,
|
||||||
|
-0.809016994374947340f, -0.798635510047292940f, -0.788010753606721900f,
|
||||||
|
-0.777145961456970680f, -0.766044443118977900f,
|
||||||
|
-0.754709580222772010f, -0.743144825477394130f, -0.731353701619170460f,
|
||||||
|
-0.719339800338651300f, -0.707106781186547460f, -0.694658370458997030f,
|
||||||
|
-0.681998360062498370f, -0.669130606358858240f,
|
||||||
|
-0.656059028990507500f, -0.642787609686539360f, -0.629320391049837280f,
|
||||||
|
-0.615661475325658290f, -0.601815023152048380f, -0.587785252292473030f,
|
||||||
|
-0.573576436351045830f, -0.559192903470746680f,
|
||||||
|
-0.544639035015027080f, -0.529919264233204790f, -0.515038074910054270f,
|
||||||
|
-0.499999999999999780f, -0.484809620246337000f, -0.469471562785890530f,
|
||||||
|
-0.453990499739546750f, -0.438371146789077510f,
|
||||||
|
-0.422618261740699330f, -0.406736643075800100f, -0.390731128489273600f,
|
||||||
|
-0.374606593415912070f, -0.358367949545300270f, -0.342020143325668710f,
|
||||||
|
-0.325568154457156420f, -0.309016994374947340f,
|
||||||
|
-0.292371704722736660f, -0.275637355816999050f, -0.258819045102520850f,
|
||||||
|
-0.241921895599667790f, -0.224951054343864810f, -0.207911690817759120f,
|
||||||
|
-0.190808995376544800f, -0.173648177666930300f,
|
||||||
|
-0.156434465040231040f, -0.139173100960065350f, -0.121869343405147370f,
|
||||||
|
-0.104528463267653330f, -0.087155742747658235f, -0.069756473744125330f,
|
||||||
|
-0.052335956242943620f, -0.034899496702500733f,
|
||||||
|
-0.017452406437283477f, 0.000000000000000061f, 0.017452406437283376f,
|
||||||
|
0.034899496702501080f, 0.052335956242943966f, 0.069756473744125455f,
|
||||||
|
0.087155742747658138f, 0.104528463267653460f,
|
||||||
|
0.121869343405147490f, 0.139173100960065690f, 0.156434465040230920f,
|
||||||
|
0.173648177666930410f, 0.190808995376544920f, 0.207911690817759450f,
|
||||||
|
0.224951054343864920f, 0.241921895599667900f,
|
||||||
|
0.258819045102520740f, 0.275637355816999160f, 0.292371704722736770f,
|
||||||
|
0.309016994374947450f, 0.325568154457156760f, 0.342020143325668820f,
|
||||||
|
0.358367949545300380f, 0.374606593415911960f,
|
||||||
|
0.390731128489273940f, 0.406736643075800210f, 0.422618261740699440f,
|
||||||
|
0.438371146789077460f, 0.453990499739546860f, 0.469471562785890860f,
|
||||||
|
0.484809620246337110f, 0.500000000000000110f,
|
||||||
|
0.515038074910054380f, 0.529919264233204900f, 0.544639035015027200f,
|
||||||
|
0.559192903470746790f, 0.573576436351046050f, 0.587785252292473140f,
|
||||||
|
0.601815023152048270f, 0.615661475325658290f,
|
||||||
|
0.629320391049837500f, 0.642787609686539360f, 0.656059028990507280f,
|
||||||
|
0.669130606358858240f, 0.681998360062498480f, 0.694658370458997370f,
|
||||||
|
0.707106781186547570f, 0.719339800338651190f,
|
||||||
|
0.731353701619170570f, 0.743144825477394240f, 0.754709580222772010f,
|
||||||
|
0.766044443118978010f, 0.777145961456970900f, 0.788010753606722010f,
|
||||||
|
0.798635510047292830f, 0.809016994374947450f,
|
||||||
|
0.819152044288991800f, 0.829037572555041620f, 0.838670567945424050f,
|
||||||
|
0.848048096156425960f, 0.857167300702112330f, 0.866025403784438710f,
|
||||||
|
0.874619707139395740f, 0.882947592858926990f,
|
||||||
|
0.891006524188367900f, 0.898794046299167040f, 0.906307787036649940f,
|
||||||
|
0.913545457642600870f, 0.920504853452440370f, 0.927183854566787420f,
|
||||||
|
0.933580426497201740f, 0.939692620785908430f,
|
||||||
|
0.945518575599316850f, 0.951056516295153530f, 0.956304755963035440f,
|
||||||
|
0.961261695938318890f, 0.965925826289068310f, 0.970295726275996470f,
|
||||||
|
0.974370064785235250f, 0.978147600733805690f,
|
||||||
|
0.981627183447663980f, 0.984807753012208020f, 0.987688340595137770f,
|
||||||
|
0.990268068741570360f, 0.992546151641321980f, 0.994521895368273290f,
|
||||||
|
0.996194698091745550f, 0.997564050259824200f,
|
||||||
|
0.998629534754573830f, 0.999390827019095760f, 0.999847695156391270f,
|
||||||
|
1.000000000000000000f, 0.999847695156391270f, 0.999390827019095760f,
|
||||||
|
0.998629534754573830f, 0.997564050259824200f,
|
||||||
|
0.996194698091745550f, 0.994521895368273290f, 0.992546151641321980f,
|
||||||
|
0.990268068741570360f, 0.987688340595137770f, 0.984807753012208020f,
|
||||||
|
0.981627183447663980f, 0.978147600733805690f,
|
||||||
|
0.974370064785235250f, 0.970295726275996470f, 0.965925826289068310f,
|
||||||
|
0.961261695938318890f, 0.956304755963035440f, 0.951056516295153530f,
|
||||||
|
0.945518575599316850f, 0.939692620785908430f,
|
||||||
|
0.933580426497201740f, 0.927183854566787420f, 0.920504853452440370f,
|
||||||
|
0.913545457642600870f, 0.906307787036649940f, 0.898794046299167040f,
|
||||||
|
0.891006524188367900f, 0.882947592858926990f,
|
||||||
|
0.874619707139395740f, 0.866025403784438710f, 0.857167300702112330f,
|
||||||
|
0.848048096156425960f, 0.838670567945424050f, 0.829037572555041620f,
|
||||||
|
0.819152044288991800f, 0.809016994374947450f,
|
||||||
|
0.798635510047292830f, 0.788010753606722010f, 0.777145961456970900f,
|
||||||
|
0.766044443118978010f, 0.754709580222772010f, 0.743144825477394240f,
|
||||||
|
0.731353701619170570f, 0.719339800338651190f,
|
||||||
|
0.707106781186547570f, 0.694658370458997370f, 0.681998360062498480f,
|
||||||
|
0.669130606358858240f, 0.656059028990507280f, 0.642787609686539360f,
|
||||||
|
0.629320391049837500f, 0.615661475325658290f,
|
||||||
|
0.601815023152048270f, 0.587785252292473140f, 0.573576436351046050f,
|
||||||
|
0.559192903470746790f, 0.544639035015027200f, 0.529919264233204900f,
|
||||||
|
0.515038074910054380f, 0.500000000000000110f,
|
||||||
|
0.484809620246337110f, 0.469471562785890860f, 0.453990499739546860f,
|
||||||
|
0.438371146789077460f, 0.422618261740699440f, 0.406736643075800210f,
|
||||||
|
0.390731128489273940f, 0.374606593415911960f,
|
||||||
|
0.358367949545300380f, 0.342020143325668820f, 0.325568154457156760f,
|
||||||
|
0.309016994374947450f, 0.292371704722736770f, 0.275637355816999160f,
|
||||||
|
0.258819045102520740f, 0.241921895599667900f,
|
||||||
|
0.224951054343864920f, 0.207911690817759450f, 0.190808995376544920f,
|
||||||
|
0.173648177666930410f, 0.156434465040230920f, 0.139173100960065690f,
|
||||||
|
0.121869343405147490f, 0.104528463267653460f,
|
||||||
|
0.087155742747658138f, 0.069756473744125455f, 0.052335956242943966f,
|
||||||
|
0.034899496702501080f, 0.017452406437283376f, 0.000000000000000061f,
|
||||||
|
-0.017452406437283477f, -0.034899496702500733f,
|
||||||
|
-0.052335956242943620f, -0.069756473744125330f, -0.087155742747658235f,
|
||||||
|
-0.104528463267653330f, -0.121869343405147370f, -0.139173100960065350f,
|
||||||
|
-0.156434465040231040f, -0.173648177666930300f,
|
||||||
|
-0.190808995376544800f, -0.207911690817759120f, -0.224951054343864810f,
|
||||||
|
-0.241921895599667790f, -0.258819045102520850f, -0.275637355816999050f,
|
||||||
|
-0.292371704722736660f, -0.309016994374947340f,
|
||||||
|
-0.325568154457156420f, -0.342020143325668710f, -0.358367949545300270f,
|
||||||
|
-0.374606593415912070f, -0.390731128489273600f, -0.406736643075800100f,
|
||||||
|
-0.422618261740699330f, -0.438371146789077510f,
|
||||||
|
-0.453990499739546750f, -0.469471562785890530f, -0.484809620246337000f,
|
||||||
|
-0.499999999999999780f, -0.515038074910054270f, -0.529919264233204790f,
|
||||||
|
-0.544639035015027080f, -0.559192903470746680f,
|
||||||
|
-0.573576436351045830f, -0.587785252292473030f, -0.601815023152048380f,
|
||||||
|
-0.615661475325658290f, -0.629320391049837280f, -0.642787609686539360f,
|
||||||
|
-0.656059028990507500f, -0.669130606358858240f,
|
||||||
|
-0.681998360062498370f, -0.694658370458997030f, -0.707106781186547460f,
|
||||||
|
-0.719339800338651300f, -0.731353701619170460f, -0.743144825477394130f,
|
||||||
|
-0.754709580222772010f, -0.766044443118977900f,
|
||||||
|
-0.777145961456970680f, -0.788010753606721900f, -0.798635510047292940f,
|
||||||
|
-0.809016994374947340f, -0.819152044288991580f, -0.829037572555041620f,
|
||||||
|
-0.838670567945424160f, -0.848048096156425960f,
|
||||||
|
-0.857167300702112220f, -0.866025403784438710f, -0.874619707139395740f,
|
||||||
|
-0.882947592858926770f, -0.891006524188367790f, -0.898794046299167040f,
|
||||||
|
-0.906307787036649940f, -0.913545457642600760f,
|
||||||
|
-0.920504853452440150f, -0.927183854566787310f, -0.933580426497201740f,
|
||||||
|
-0.939692620785908320f, -0.945518575599316740f, -0.951056516295153530f,
|
||||||
|
-0.956304755963035440f, -0.961261695938318670f,
|
||||||
|
-0.965925826289068200f, -0.970295726275996470f, -0.974370064785235250f,
|
||||||
|
-0.978147600733805690f, -0.981627183447663980f, -0.984807753012208020f,
|
||||||
|
-0.987688340595137660f, -0.990268068741570250f,
|
||||||
|
-0.992546151641321980f, -0.994521895368273290f, -0.996194698091745550f,
|
||||||
|
-0.997564050259824200f, -0.998629534754573830f, -0.999390827019095760f,
|
||||||
|
-0.999847695156391270f, -1.000000000000000000f
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Sine Table is generated from following loop
|
||||||
|
* <pre>for(i = 0; i < 360; i++)
|
||||||
|
* {
|
||||||
|
* sinTable[i]= sin((i-180) * PI/180.0);
|
||||||
|
* } </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
static const float32_t sinTable[360] = {
|
||||||
|
-0.017452406437283439f, -0.034899496702500699f, -0.052335956242943807f,
|
||||||
|
-0.069756473744125524f, -0.087155742747658638f, -0.104528463267653730f,
|
||||||
|
-0.121869343405147550f, -0.139173100960065740f,
|
||||||
|
-0.156434465040230980f, -0.173648177666930280f, -0.190808995376544970f,
|
||||||
|
-0.207911690817759310f, -0.224951054343864780f, -0.241921895599667730f,
|
||||||
|
-0.258819045102521020f, -0.275637355816999660f,
|
||||||
|
-0.292371704722737050f, -0.309016994374947510f, -0.325568154457156980f,
|
||||||
|
-0.342020143325668880f, -0.358367949545300210f, -0.374606593415912240f,
|
||||||
|
-0.390731128489274160f, -0.406736643075800430f,
|
||||||
|
-0.422618261740699500f, -0.438371146789077290f, -0.453990499739546860f,
|
||||||
|
-0.469471562785891080f, -0.484809620246337170f, -0.499999999999999940f,
|
||||||
|
-0.515038074910054380f, -0.529919264233204900f,
|
||||||
|
-0.544639035015026860f, -0.559192903470746900f, -0.573576436351046380f,
|
||||||
|
-0.587785252292473250f, -0.601815023152048160f, -0.615661475325658400f,
|
||||||
|
-0.629320391049837720f, -0.642787609686539470f,
|
||||||
|
-0.656059028990507280f, -0.669130606358858350f, -0.681998360062498590f,
|
||||||
|
-0.694658370458997140f, -0.707106781186547570f, -0.719339800338651410f,
|
||||||
|
-0.731353701619170570f, -0.743144825477394240f,
|
||||||
|
-0.754709580222771790f, -0.766044443118978010f, -0.777145961456971010f,
|
||||||
|
-0.788010753606722010f, -0.798635510047292720f, -0.809016994374947450f,
|
||||||
|
-0.819152044288992020f, -0.829037572555041740f,
|
||||||
|
-0.838670567945424050f, -0.848048096156426070f, -0.857167300702112330f,
|
||||||
|
-0.866025403784438710f, -0.874619707139395850f, -0.882947592858927100f,
|
||||||
|
-0.891006524188367900f, -0.898794046299166930f,
|
||||||
|
-0.906307787036650050f, -0.913545457642600980f, -0.920504853452440370f,
|
||||||
|
-0.927183854566787420f, -0.933580426497201740f, -0.939692620785908430f,
|
||||||
|
-0.945518575599316850f, -0.951056516295153640f,
|
||||||
|
-0.956304755963035550f, -0.961261695938318890f, -0.965925826289068310f,
|
||||||
|
-0.970295726275996470f, -0.974370064785235250f, -0.978147600733805690f,
|
||||||
|
-0.981627183447663980f, -0.984807753012208020f,
|
||||||
|
-0.987688340595137660f, -0.990268068741570360f, -0.992546151641322090f,
|
||||||
|
-0.994521895368273400f, -0.996194698091745550f, -0.997564050259824200f,
|
||||||
|
-0.998629534754573830f, -0.999390827019095760f,
|
||||||
|
-0.999847695156391270f, -1.000000000000000000f, -0.999847695156391270f,
|
||||||
|
-0.999390827019095760f, -0.998629534754573830f, -0.997564050259824200f,
|
||||||
|
-0.996194698091745550f, -0.994521895368273290f,
|
||||||
|
-0.992546151641321980f, -0.990268068741570250f, -0.987688340595137770f,
|
||||||
|
-0.984807753012208020f, -0.981627183447663980f, -0.978147600733805580f,
|
||||||
|
-0.974370064785235250f, -0.970295726275996470f,
|
||||||
|
-0.965925826289068310f, -0.961261695938318890f, -0.956304755963035440f,
|
||||||
|
-0.951056516295153530f, -0.945518575599316740f, -0.939692620785908320f,
|
||||||
|
-0.933580426497201740f, -0.927183854566787420f,
|
||||||
|
-0.920504853452440260f, -0.913545457642600870f, -0.906307787036649940f,
|
||||||
|
-0.898794046299167040f, -0.891006524188367790f, -0.882947592858926880f,
|
||||||
|
-0.874619707139395740f, -0.866025403784438600f,
|
||||||
|
-0.857167300702112220f, -0.848048096156426070f, -0.838670567945423940f,
|
||||||
|
-0.829037572555041740f, -0.819152044288991800f, -0.809016994374947450f,
|
||||||
|
-0.798635510047292830f, -0.788010753606722010f,
|
||||||
|
-0.777145961456970790f, -0.766044443118978010f, -0.754709580222772010f,
|
||||||
|
-0.743144825477394240f, -0.731353701619170460f, -0.719339800338651080f,
|
||||||
|
-0.707106781186547460f, -0.694658370458997250f,
|
||||||
|
-0.681998360062498480f, -0.669130606358858240f, -0.656059028990507160f,
|
||||||
|
-0.642787609686539250f, -0.629320391049837390f, -0.615661475325658180f,
|
||||||
|
-0.601815023152048270f, -0.587785252292473140f,
|
||||||
|
-0.573576436351046050f, -0.559192903470746900f, -0.544639035015027080f,
|
||||||
|
-0.529919264233204900f, -0.515038074910054160f, -0.499999999999999940f,
|
||||||
|
-0.484809620246337060f, -0.469471562785890810f,
|
||||||
|
-0.453990499739546750f, -0.438371146789077400f, -0.422618261740699440f,
|
||||||
|
-0.406736643075800150f, -0.390731128489273720f, -0.374606593415912010f,
|
||||||
|
-0.358367949545300270f, -0.342020143325668710f,
|
||||||
|
-0.325568154457156640f, -0.309016994374947400f, -0.292371704722736770f,
|
||||||
|
-0.275637355816999160f, -0.258819045102520740f, -0.241921895599667730f,
|
||||||
|
-0.224951054343865000f, -0.207911690817759310f,
|
||||||
|
-0.190808995376544800f, -0.173648177666930330f, -0.156434465040230870f,
|
||||||
|
-0.139173100960065440f, -0.121869343405147480f, -0.104528463267653460f,
|
||||||
|
-0.087155742747658166f, -0.069756473744125302f,
|
||||||
|
-0.052335956242943828f, -0.034899496702500969f, -0.017452406437283512f,
|
||||||
|
0.000000000000000000f, 0.017452406437283512f, 0.034899496702500969f,
|
||||||
|
0.052335956242943828f, 0.069756473744125302f,
|
||||||
|
0.087155742747658166f, 0.104528463267653460f, 0.121869343405147480f,
|
||||||
|
0.139173100960065440f, 0.156434465040230870f, 0.173648177666930330f,
|
||||||
|
0.190808995376544800f, 0.207911690817759310f,
|
||||||
|
0.224951054343865000f, 0.241921895599667730f, 0.258819045102520740f,
|
||||||
|
0.275637355816999160f, 0.292371704722736770f, 0.309016994374947400f,
|
||||||
|
0.325568154457156640f, 0.342020143325668710f,
|
||||||
|
0.358367949545300270f, 0.374606593415912010f, 0.390731128489273720f,
|
||||||
|
0.406736643075800150f, 0.422618261740699440f, 0.438371146789077400f,
|
||||||
|
0.453990499739546750f, 0.469471562785890810f,
|
||||||
|
0.484809620246337060f, 0.499999999999999940f, 0.515038074910054160f,
|
||||||
|
0.529919264233204900f, 0.544639035015027080f, 0.559192903470746900f,
|
||||||
|
0.573576436351046050f, 0.587785252292473140f,
|
||||||
|
0.601815023152048270f, 0.615661475325658180f, 0.629320391049837390f,
|
||||||
|
0.642787609686539250f, 0.656059028990507160f, 0.669130606358858240f,
|
||||||
|
0.681998360062498480f, 0.694658370458997250f,
|
||||||
|
0.707106781186547460f, 0.719339800338651080f, 0.731353701619170460f,
|
||||||
|
0.743144825477394240f, 0.754709580222772010f, 0.766044443118978010f,
|
||||||
|
0.777145961456970790f, 0.788010753606722010f,
|
||||||
|
0.798635510047292830f, 0.809016994374947450f, 0.819152044288991800f,
|
||||||
|
0.829037572555041740f, 0.838670567945423940f, 0.848048096156426070f,
|
||||||
|
0.857167300702112220f, 0.866025403784438600f,
|
||||||
|
0.874619707139395740f, 0.882947592858926880f, 0.891006524188367790f,
|
||||||
|
0.898794046299167040f, 0.906307787036649940f, 0.913545457642600870f,
|
||||||
|
0.920504853452440260f, 0.927183854566787420f,
|
||||||
|
0.933580426497201740f, 0.939692620785908320f, 0.945518575599316740f,
|
||||||
|
0.951056516295153530f, 0.956304755963035440f, 0.961261695938318890f,
|
||||||
|
0.965925826289068310f, 0.970295726275996470f,
|
||||||
|
0.974370064785235250f, 0.978147600733805580f, 0.981627183447663980f,
|
||||||
|
0.984807753012208020f, 0.987688340595137770f, 0.990268068741570250f,
|
||||||
|
0.992546151641321980f, 0.994521895368273290f,
|
||||||
|
0.996194698091745550f, 0.997564050259824200f, 0.998629534754573830f,
|
||||||
|
0.999390827019095760f, 0.999847695156391270f, 1.000000000000000000f,
|
||||||
|
0.999847695156391270f, 0.999390827019095760f,
|
||||||
|
0.998629534754573830f, 0.997564050259824200f, 0.996194698091745550f,
|
||||||
|
0.994521895368273400f, 0.992546151641322090f, 0.990268068741570360f,
|
||||||
|
0.987688340595137660f, 0.984807753012208020f,
|
||||||
|
0.981627183447663980f, 0.978147600733805690f, 0.974370064785235250f,
|
||||||
|
0.970295726275996470f, 0.965925826289068310f, 0.961261695938318890f,
|
||||||
|
0.956304755963035550f, 0.951056516295153640f,
|
||||||
|
0.945518575599316850f, 0.939692620785908430f, 0.933580426497201740f,
|
||||||
|
0.927183854566787420f, 0.920504853452440370f, 0.913545457642600980f,
|
||||||
|
0.906307787036650050f, 0.898794046299166930f,
|
||||||
|
0.891006524188367900f, 0.882947592858927100f, 0.874619707139395850f,
|
||||||
|
0.866025403784438710f, 0.857167300702112330f, 0.848048096156426070f,
|
||||||
|
0.838670567945424050f, 0.829037572555041740f,
|
||||||
|
0.819152044288992020f, 0.809016994374947450f, 0.798635510047292720f,
|
||||||
|
0.788010753606722010f, 0.777145961456971010f, 0.766044443118978010f,
|
||||||
|
0.754709580222771790f, 0.743144825477394240f,
|
||||||
|
0.731353701619170570f, 0.719339800338651410f, 0.707106781186547570f,
|
||||||
|
0.694658370458997140f, 0.681998360062498590f, 0.669130606358858350f,
|
||||||
|
0.656059028990507280f, 0.642787609686539470f,
|
||||||
|
0.629320391049837720f, 0.615661475325658400f, 0.601815023152048160f,
|
||||||
|
0.587785252292473250f, 0.573576436351046380f, 0.559192903470746900f,
|
||||||
|
0.544639035015026860f, 0.529919264233204900f,
|
||||||
|
0.515038074910054380f, 0.499999999999999940f, 0.484809620246337170f,
|
||||||
|
0.469471562785891080f, 0.453990499739546860f, 0.438371146789077290f,
|
||||||
|
0.422618261740699500f, 0.406736643075800430f,
|
||||||
|
0.390731128489274160f, 0.374606593415912240f, 0.358367949545300210f,
|
||||||
|
0.342020143325668880f, 0.325568154457156980f, 0.309016994374947510f,
|
||||||
|
0.292371704722737050f, 0.275637355816999660f,
|
||||||
|
0.258819045102521020f, 0.241921895599667730f, 0.224951054343864780f,
|
||||||
|
0.207911690817759310f, 0.190808995376544970f, 0.173648177666930280f,
|
||||||
|
0.156434465040230980f, 0.139173100960065740f,
|
||||||
|
0.121869343405147550f, 0.104528463267653730f, 0.087155742747658638f,
|
||||||
|
0.069756473744125524f, 0.052335956242943807f, 0.034899496702500699f,
|
||||||
|
0.017452406437283439f, 0.000000000000000122f
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Floating-point sin_cos function.
|
||||||
|
* @param[in] theta input value in degrees
|
||||||
|
* @param[out] *pSinVal points to the processed sine output.
|
||||||
|
* @param[out] *pCosVal points to the processed cos output.
|
||||||
|
* @return none.
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_sin_cos_f32(
|
||||||
|
float32_t theta,
|
||||||
|
float32_t * pSinVal,
|
||||||
|
float32_t * pCosVal)
|
||||||
|
{
|
||||||
|
int32_t i; /* Index for reading nearwst output values */
|
||||||
|
float32_t x1 = -179.0f; /* Initial input value */
|
||||||
|
float32_t y0, y1; /* nearest output values */
|
||||||
|
float32_t y2, y3;
|
||||||
|
float32_t fract; /* fractional part of input */
|
||||||
|
|
||||||
|
/* Calculation of fractional part */
|
||||||
|
if(theta > 0.0f)
|
||||||
|
{
|
||||||
|
fract = theta - (float32_t) ((int32_t) theta);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
fract = (theta - (float32_t) ((int32_t) theta)) + 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* index calculation for reading nearest output values */
|
||||||
|
i = (uint32_t) (theta - x1);
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(i < 0)
|
||||||
|
{
|
||||||
|
i = 0;
|
||||||
|
}
|
||||||
|
else if(i >= 359)
|
||||||
|
{
|
||||||
|
i = 358;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* reading nearest sine output values */
|
||||||
|
y0 = sinTable[i];
|
||||||
|
y1 = sinTable[i + 1u];
|
||||||
|
|
||||||
|
/* reading nearest cosine output values */
|
||||||
|
y2 = cosTable[i];
|
||||||
|
y3 = cosTable[i + 1u];
|
||||||
|
|
||||||
|
y1 = y1 - y0;
|
||||||
|
y3 = y3 - y2;
|
||||||
|
|
||||||
|
y1 = fract * y1;
|
||||||
|
y3 = fract * y3;
|
||||||
|
|
||||||
|
/* Calculation of sine value */
|
||||||
|
*pSinVal = y0 + y1;
|
||||||
|
|
||||||
|
/* Calculation of cosine value */
|
||||||
|
*pCosVal = y2 + y3;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of SinCos group
|
||||||
|
*/
|
|
@ -0,0 +1,324 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sin_cos_q31.c
|
||||||
|
*
|
||||||
|
* Description: Cosine & Sine calculation for Q31 values.
|
||||||
|
*
|
||||||
|
* 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 groupController
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup SinCos
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Sine Table is generated from following loop
|
||||||
|
* <pre>for(i = 0; i < 360; i++)
|
||||||
|
* {
|
||||||
|
* sinTable[i]= sin((i-180) * PI/180.0);
|
||||||
|
* } </pre>
|
||||||
|
* Convert above coefficients to fixed point 1.31 format.
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const int32_t sinTableQ31[360] = {
|
||||||
|
|
||||||
|
0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2,
|
||||||
|
0xf06695da,
|
||||||
|
0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9,
|
||||||
|
0xe108b40d, 0xdedf047d,
|
||||||
|
0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0,
|
||||||
|
0xd00ce422, 0xcdfc85bb,
|
||||||
|
0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224,
|
||||||
|
0xc0000000, 0xbe133b7c,
|
||||||
|
0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af,
|
||||||
|
0xb1320139, 0xaf726def,
|
||||||
|
0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666,
|
||||||
|
0xa3ecac65, 0xa263007d,
|
||||||
|
0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5,
|
||||||
|
0x98722192, 0x9726069c,
|
||||||
|
0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621,
|
||||||
|
0x8efb92c2, 0x8df37f8b,
|
||||||
|
0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4,
|
||||||
|
0x87b826f7, 0x86f93f50,
|
||||||
|
0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b,
|
||||||
|
0x82cc0f36, 0x825a0a5b,
|
||||||
|
0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130,
|
||||||
|
0x804fd23a, 0x802ce84c,
|
||||||
|
0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c,
|
||||||
|
0x804fd23a, 0x807cb130,
|
||||||
|
0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b,
|
||||||
|
0x82cc0f36, 0x8347d77b,
|
||||||
|
0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50,
|
||||||
|
0x87b826f7, 0x88806fc4,
|
||||||
|
0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b,
|
||||||
|
0x8efb92c2, 0x900c7621,
|
||||||
|
0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c,
|
||||||
|
0x98722192, 0x99c64fc5,
|
||||||
|
0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d,
|
||||||
|
0xa3ecac65, 0xa57d8666,
|
||||||
|
0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def,
|
||||||
|
0xb1320139, 0xb2f7b9af,
|
||||||
|
0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c,
|
||||||
|
0xc0000000, 0xc1f1c224,
|
||||||
|
0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb,
|
||||||
|
0xd00ce422, 0xd220ffc0,
|
||||||
|
0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d,
|
||||||
|
0xe108b40d, 0xe334cdc9,
|
||||||
|
0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da,
|
||||||
|
0xf29ecfb2, 0xf4d814a4,
|
||||||
|
0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632,
|
||||||
|
0x6b2f1d2,
|
||||||
|
0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68,
|
||||||
|
0x163a1a7e, 0x186c6ddd,
|
||||||
|
0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f,
|
||||||
|
0x278dde6e, 0x29ac37a0,
|
||||||
|
0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee,
|
||||||
|
0x381c8bb5, 0x3a1c5c57,
|
||||||
|
0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e,
|
||||||
|
0x4793a210, 0x496af3e2,
|
||||||
|
0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05,
|
||||||
|
0x55a6125c, 0x574bb8e6,
|
||||||
|
0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3,
|
||||||
|
0x620dbe8b, 0x637984d4,
|
||||||
|
0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3,
|
||||||
|
0x6c8cd70b, 0x6db7a87a,
|
||||||
|
0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1,
|
||||||
|
0x74ef0ebc, 0x75d31a61,
|
||||||
|
0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba,
|
||||||
|
0x7b0a9f8d, 0x7ba3751d,
|
||||||
|
0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251,
|
||||||
|
0x7ec11aa5, 0x7f0bc097,
|
||||||
|
0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260,
|
||||||
|
0x7fffffff, 0x7ffb0260,
|
||||||
|
0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097,
|
||||||
|
0x7ec11aa5, 0x7e6c9251,
|
||||||
|
0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d,
|
||||||
|
0x7b0a9f8d, 0x7a6831ba,
|
||||||
|
0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61,
|
||||||
|
0x74ef0ebc, 0x7401e4c1,
|
||||||
|
0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a,
|
||||||
|
0x6c8cd70b, 0x6b598ea3,
|
||||||
|
0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4,
|
||||||
|
0x620dbe8b, 0x609a52d3,
|
||||||
|
0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6,
|
||||||
|
0x55a6125c, 0x53f9be05,
|
||||||
|
0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2,
|
||||||
|
0x4793a210, 0x45b6bb5e,
|
||||||
|
0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57,
|
||||||
|
0x381c8bb5, 0x36185aee,
|
||||||
|
0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0,
|
||||||
|
0x278dde6e, 0x256c6f9f,
|
||||||
|
0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd,
|
||||||
|
0x163a1a7e, 0x14060b68,
|
||||||
|
0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2,
|
||||||
|
0x4779632, 0x23be165,
|
||||||
|
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Cosine Table is generated from following loop
|
||||||
|
* <pre>for(i = 0; i < 360; i++)
|
||||||
|
* {
|
||||||
|
* cosTable[i]= cos((i-180) * PI/180.0);
|
||||||
|
* } </pre>
|
||||||
|
* \par
|
||||||
|
* Convert above coefficients to fixed point 1.31 format.
|
||||||
|
*/
|
||||||
|
static const int32_t cosTableQ31[360] = {
|
||||||
|
0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130,
|
||||||
|
0x80b381ac, 0x80f43f69,
|
||||||
|
0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b,
|
||||||
|
0x83cd5982, 0x845c8ae3,
|
||||||
|
0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4,
|
||||||
|
0x89520a1a, 0x8a2ce59f,
|
||||||
|
0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621,
|
||||||
|
0x9126145f, 0x92485786,
|
||||||
|
0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5,
|
||||||
|
0x9b2276b0, 0x9c867b2c,
|
||||||
|
0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666,
|
||||||
|
0xa7156f3c, 0xa8b4471a,
|
||||||
|
0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af,
|
||||||
|
0xb4c373ee, 0xb6950c1e,
|
||||||
|
0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224,
|
||||||
|
0xc3e85b18, 0xc5e3a3a9,
|
||||||
|
0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0,
|
||||||
|
0xd438af17, 0xd653c860,
|
||||||
|
0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9,
|
||||||
|
0xe5632654, 0xe7939223,
|
||||||
|
0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4,
|
||||||
|
0xf7123849, 0xf94d0e2e,
|
||||||
|
0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7,
|
||||||
|
0xb27eb5c,
|
||||||
|
0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd,
|
||||||
|
0x1a9cd9ac, 0x1ccb3237,
|
||||||
|
0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0,
|
||||||
|
0x2bc750e9, 0x2ddf0040,
|
||||||
|
0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57,
|
||||||
|
0x3c17a4e8, 0x3e0e3ddc,
|
||||||
|
0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2,
|
||||||
|
0x4b3c8c12, 0x4d084651,
|
||||||
|
0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6,
|
||||||
|
0x58ea90c4, 0x5a82799a,
|
||||||
|
0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4,
|
||||||
|
0x64dd8950, 0x6639b03b,
|
||||||
|
0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a,
|
||||||
|
0x6ed9eba1, 0x6ff389df,
|
||||||
|
0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61,
|
||||||
|
0x76adf5e6, 0x777f903c,
|
||||||
|
0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d,
|
||||||
|
0x7c32a67e, 0x7cb82885,
|
||||||
|
0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097,
|
||||||
|
0x7f4c7e54, 0x7f834ed0,
|
||||||
|
0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260,
|
||||||
|
0x7fec09e3, 0x7fd317b4,
|
||||||
|
0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251,
|
||||||
|
0x7e0e2e32, 0x7da5f5a5,
|
||||||
|
0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba,
|
||||||
|
0x79bc384d, 0x7906c0b0,
|
||||||
|
0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1,
|
||||||
|
0x730baeed, 0x720c8075,
|
||||||
|
0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3,
|
||||||
|
0x6a1de737, 0x68d9f964,
|
||||||
|
0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3,
|
||||||
|
0x5f1f5ea1, 0x5d9cff83,
|
||||||
|
0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05,
|
||||||
|
0x5246dd49, 0x508d9211,
|
||||||
|
0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e,
|
||||||
|
0x43d464fb, 0x41ecc484,
|
||||||
|
0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee,
|
||||||
|
0x340ff242, 0x32037a45,
|
||||||
|
0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f,
|
||||||
|
0x234815ba, 0x2120fb83,
|
||||||
|
0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68,
|
||||||
|
0x11d06c97, 0xf996a26,
|
||||||
|
0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0,
|
||||||
|
0xfdc41e9b,
|
||||||
|
0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da,
|
||||||
|
0xee2f9369, 0xebf9f498,
|
||||||
|
0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d,
|
||||||
|
0xdcb7ea46, 0xda939061,
|
||||||
|
0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb,
|
||||||
|
0xcbf00dbe, 0xc9e7a512,
|
||||||
|
0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c,
|
||||||
|
0xbc2b9b05, 0xba4944a2,
|
||||||
|
0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def,
|
||||||
|
0xadb922b7, 0xac0641fb,
|
||||||
|
0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d,
|
||||||
|
0xa0e0a15f, 0x9f65ad2d,
|
||||||
|
0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c,
|
||||||
|
0x95e218c9, 0x94a6715d,
|
||||||
|
0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b,
|
||||||
|
0x8cf45113, 0x8bfe1b3f,
|
||||||
|
0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50,
|
||||||
|
0x8643c7b3, 0x8597ce46,
|
||||||
|
0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b,
|
||||||
|
0x81f1d1ce, 0x81936daf,
|
||||||
|
0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c,
|
||||||
|
0x8013f61d, 0x8004fda0,
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 sin_cos function.
|
||||||
|
* @param[in] theta scaled input value in degrees
|
||||||
|
* @param[out] *pSinVal points to the processed sine output.
|
||||||
|
* @param[out] *pCosVal points to the processed cosine output.
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179].
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
void arm_sin_cos_q31(
|
||||||
|
q31_t theta,
|
||||||
|
q31_t * pSinVal,
|
||||||
|
q31_t * pCosVal)
|
||||||
|
{
|
||||||
|
q31_t x0; /* Nearest input value */
|
||||||
|
q31_t y0, y1; /* Nearest output values */
|
||||||
|
q31_t xSpacing = INPUT_SPACING; /* Spaing between inputs */
|
||||||
|
int32_t i; /* Index */
|
||||||
|
q31_t oneByXSpacing; /* 1/ xSpacing value */
|
||||||
|
q31_t out; /* temporary variable */
|
||||||
|
uint32_t sign_bits; /* No.of sign bits */
|
||||||
|
uint32_t firstX = 0x80000000; /* First X value */
|
||||||
|
|
||||||
|
/* Calculation of index */
|
||||||
|
i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing;
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(i < 0)
|
||||||
|
{
|
||||||
|
i = 0;
|
||||||
|
}
|
||||||
|
else if(i >= 359)
|
||||||
|
{
|
||||||
|
i = 358;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Calculation of first nearest input value */
|
||||||
|
x0 = (q31_t) firstX + ((q31_t) i * xSpacing);
|
||||||
|
|
||||||
|
/* Reading nearest sine output values from table */
|
||||||
|
y0 = sinTableQ31[i];
|
||||||
|
y1 = sinTableQ31[i + 1u];
|
||||||
|
|
||||||
|
/* Calculation of 1/(x1-x0) */
|
||||||
|
/* (x1-x0) is xSpacing which is fixed value */
|
||||||
|
sign_bits = 8u;
|
||||||
|
oneByXSpacing = 0x5A000000;
|
||||||
|
|
||||||
|
/* Calculation of (theta - x0)/(x1-x0) */
|
||||||
|
out =
|
||||||
|
(((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits);
|
||||||
|
|
||||||
|
/* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
|
||||||
|
*pSinVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
|
||||||
|
|
||||||
|
/* Reading nearest cosine output values from table */
|
||||||
|
y0 = cosTableQ31[i];
|
||||||
|
y1 = cosTableQ31[i + 1u];
|
||||||
|
|
||||||
|
/* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */
|
||||||
|
*pCosVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30)));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of SinCos group
|
||||||
|
*/
|
|
@ -0,0 +1,280 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_cos_f32.c
|
||||||
|
*
|
||||||
|
* Description: Fast cosine calculation for floating-point values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup cos Cosine
|
||||||
|
*
|
||||||
|
* Computes the trigonometric cosine function using a combination of table lookup
|
||||||
|
* and cubic interpolation. There are separate functions for
|
||||||
|
* Q15, Q31, and floating-point data types.
|
||||||
|
* The input to the floating-point version is in radians while the
|
||||||
|
* fixed-point Q15 and Q31 have a scaled input with the range
|
||||||
|
* [0 +0.9999] mapping to [0 2*pi), Where range excludes 2*pi.
|
||||||
|
*
|
||||||
|
* The implementation is based on table lookup using 256 values together with cubic interpolation.
|
||||||
|
* The steps used are:
|
||||||
|
* -# Calculation of the nearest integer table index
|
||||||
|
* -# Fetch the four table values a, b, c, and d
|
||||||
|
* -# Compute the fractional portion (fract) of the table index.
|
||||||
|
* -# Calculation of wa, wb, wc, wd
|
||||||
|
* -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>
|
||||||
|
*
|
||||||
|
* where
|
||||||
|
* <pre>
|
||||||
|
* a=Table[index-1];
|
||||||
|
* b=Table[index+0];
|
||||||
|
* c=Table[index+1];
|
||||||
|
* d=Table[index+2];
|
||||||
|
* </pre>
|
||||||
|
* and
|
||||||
|
* <pre>
|
||||||
|
* wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;
|
||||||
|
* wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;
|
||||||
|
* wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;
|
||||||
|
* wd=(1/6)*fract.^3 - (1/6)*fract;
|
||||||
|
* </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup cos
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* <b>Example code for Generation of Cos Table:</b>
|
||||||
|
* tableSize = 256;
|
||||||
|
* <pre>for(n = -1; n < (tableSize + 2); n++)
|
||||||
|
* {
|
||||||
|
* cosTable[n+1]= cos(2*pi*n/tableSize);
|
||||||
|
* } </pre>
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const float32_t cosTable[260] = {
|
||||||
|
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
|
||||||
|
0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
|
||||||
|
0.992479562759399410f, 0.989176511764526370f,
|
||||||
|
0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
|
||||||
|
0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
|
||||||
|
0.949528157711029050f, 0.941544055938720700f,
|
||||||
|
0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
|
||||||
|
0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
|
||||||
|
0.870086967945098880f, 0.857728600502014160f,
|
||||||
|
0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
|
||||||
|
0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
|
||||||
|
0.757208824157714840f, 0.740951120853424070f,
|
||||||
|
0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
|
||||||
|
0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
|
||||||
|
0.615231573581695560f, 0.595699310302734380f,
|
||||||
|
0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
|
||||||
|
0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
|
||||||
|
0.449611335992813110f, 0.427555084228515630f,
|
||||||
|
0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
|
||||||
|
0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
|
||||||
|
0.266712754964828490f, 0.242980182170867920f,
|
||||||
|
0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
|
||||||
|
0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
|
||||||
|
0.073564566671848297f, 0.049067676067352295f,
|
||||||
|
0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f,
|
||||||
|
-0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
|
||||||
|
-0.122410677373409270f, -0.146730467677116390f,
|
||||||
|
-0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
|
||||||
|
-0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
|
||||||
|
-0.313681751489639280f, -0.336889863014221190f,
|
||||||
|
-0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
|
||||||
|
-0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
|
||||||
|
-0.492898195981979370f, -0.514102756977081300f,
|
||||||
|
-0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
|
||||||
|
-0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
|
||||||
|
-0.653172850608825680f, -0.671558976173400880f,
|
||||||
|
-0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
|
||||||
|
-0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
|
||||||
|
-0.788346409797668460f, -0.803207516670227050f,
|
||||||
|
-0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
|
||||||
|
-0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
|
||||||
|
-0.893224298954010010f, -0.903989315032958980f,
|
||||||
|
-0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
|
||||||
|
-0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
|
||||||
|
-0.963776051998138430f, -0.970031261444091800f,
|
||||||
|
-0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
|
||||||
|
-0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
|
||||||
|
-0.997290432453155520f, -0.998795449733734130f,
|
||||||
|
-0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
|
||||||
|
-0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
|
||||||
|
-0.992479562759399410f, -0.989176511764526370f,
|
||||||
|
-0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
|
||||||
|
-0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
|
||||||
|
-0.949528157711029050f, -0.941544055938720700f,
|
||||||
|
-0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
|
||||||
|
-0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
|
||||||
|
-0.870086967945098880f, -0.857728600502014160f,
|
||||||
|
-0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
|
||||||
|
-0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
|
||||||
|
-0.757208824157714840f, -0.740951120853424070f,
|
||||||
|
-0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
|
||||||
|
-0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
|
||||||
|
-0.615231573581695560f, -0.595699310302734380f,
|
||||||
|
-0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
|
||||||
|
-0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
|
||||||
|
-0.449611335992813110f, -0.427555084228515630f,
|
||||||
|
-0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
|
||||||
|
-0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
|
||||||
|
-0.266712754964828490f, -0.242980182170867920f,
|
||||||
|
-0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
|
||||||
|
-0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
|
||||||
|
-0.073564566671848297f, -0.049067676067352295f,
|
||||||
|
-0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f,
|
||||||
|
0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
|
||||||
|
0.122410677373409270f, 0.146730467677116390f,
|
||||||
|
0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
|
||||||
|
0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
|
||||||
|
0.313681751489639280f, 0.336889863014221190f,
|
||||||
|
0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
|
||||||
|
0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
|
||||||
|
0.492898195981979370f, 0.514102756977081300f,
|
||||||
|
0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
|
||||||
|
0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
|
||||||
|
0.653172850608825680f, 0.671558976173400880f,
|
||||||
|
0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
|
||||||
|
0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
|
||||||
|
0.788346409797668460f, 0.803207516670227050f,
|
||||||
|
0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
|
||||||
|
0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
|
||||||
|
0.893224298954010010f, 0.903989315032958980f,
|
||||||
|
0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
|
||||||
|
0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
|
||||||
|
0.963776051998138430f, 0.970031261444091800f,
|
||||||
|
0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
|
||||||
|
0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
|
||||||
|
0.997290432453155520f, 0.998795449733734130f,
|
||||||
|
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
|
||||||
|
0.998795449733734130f
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric cosine function for floating-point data.
|
||||||
|
* @param[in] x input value in radians.
|
||||||
|
* @return cos(x).
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
float32_t arm_cos_f32(
|
||||||
|
float32_t x)
|
||||||
|
{
|
||||||
|
float32_t cosVal, fract, in;
|
||||||
|
int32_t index;
|
||||||
|
uint32_t tableSize = (uint32_t) TABLE_SIZE;
|
||||||
|
float32_t wa, wb, wc, wd;
|
||||||
|
float32_t a, b, c, d;
|
||||||
|
float32_t *tablePtr;
|
||||||
|
int32_t n;
|
||||||
|
float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
|
||||||
|
float32_t oneminusfractby2;
|
||||||
|
float32_t frby2xfrsq, frby6xfrsq;
|
||||||
|
|
||||||
|
/* input x is in radians */
|
||||||
|
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
|
||||||
|
in = x * 0.159154943092f;
|
||||||
|
|
||||||
|
/* Calculation of floor value of input */
|
||||||
|
n = (int32_t) in;
|
||||||
|
|
||||||
|
/* Make negative values towards -infinity */
|
||||||
|
if(x < 0.0f)
|
||||||
|
{
|
||||||
|
n = n - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Map input value to [0 1] */
|
||||||
|
in = in - (float32_t) n;
|
||||||
|
|
||||||
|
/* Calculation of index of the table */
|
||||||
|
index = (uint32_t) (tableSize * in);
|
||||||
|
|
||||||
|
/* fractional value calculation */
|
||||||
|
fract = ((float32_t) tableSize * in) - (float32_t) index;
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (float32_t *) & cosTable[index];
|
||||||
|
|
||||||
|
/* Read four nearest values of input value from the cos table */
|
||||||
|
a = tablePtr[0];
|
||||||
|
b = tablePtr[1];
|
||||||
|
c = tablePtr[2];
|
||||||
|
d = tablePtr[3];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
fractsq = fract * fract;
|
||||||
|
fractby2 = fract * 0.5f;
|
||||||
|
fractby6 = fract * 0.166666667f;
|
||||||
|
fractby3 = fract * 0.3333333333333f;
|
||||||
|
fractsqby2 = fractsq * 0.5f;
|
||||||
|
frby2xfrsq = (fractby2) * fractsq;
|
||||||
|
frby6xfrsq = (fractby6) * fractsq;
|
||||||
|
oneminusfractby2 = 1.0f - fractby2;
|
||||||
|
wb = fractsqby2 - fractby3;
|
||||||
|
wc = (fractsqby2 + fract);
|
||||||
|
wa = wb - frby6xfrsq;
|
||||||
|
wb = frby2xfrsq - fractsq;
|
||||||
|
cosVal = wa * a;
|
||||||
|
wc = wc - frby2xfrsq;
|
||||||
|
wd = (frby6xfrsq) - fractby6;
|
||||||
|
wb = wb + oneminusfractby2;
|
||||||
|
|
||||||
|
/* Calculate cos value */
|
||||||
|
cosVal = (cosVal + (b * wb)) + ((c * wc) + (d * wd));
|
||||||
|
|
||||||
|
/* Return the output value */
|
||||||
|
return (cosVal);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of cos group
|
||||||
|
*/
|
|
@ -0,0 +1,205 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_cos_q15.c
|
||||||
|
*
|
||||||
|
* Description: Fast cosine calculation for Q15 values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup cos
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Table Values are in Q15(1.15 Fixed point format) and generation is done in three steps
|
||||||
|
* \par
|
||||||
|
* First Generate cos values in floating point:
|
||||||
|
* tableSize = 256;
|
||||||
|
* <pre>for(n = -1; n < (tableSize + 1); n++)
|
||||||
|
* {
|
||||||
|
* cosTable[n+1]= cos(2*pi*n/tableSize);
|
||||||
|
* }</pre>
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
* \par
|
||||||
|
* Secondly Convert Floating point to Q15(Fixed point):
|
||||||
|
* (cosTable[i] * pow(2, 15))
|
||||||
|
* \par
|
||||||
|
* Finally Rounding to nearest integer is done
|
||||||
|
* cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const q15_t cosTableQ15[259] = {
|
||||||
|
0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
|
||||||
|
0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
|
||||||
|
0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
|
||||||
|
0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
|
||||||
|
0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
|
||||||
|
0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
|
||||||
|
0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
|
||||||
|
0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
|
||||||
|
0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
|
||||||
|
0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
|
||||||
|
0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
|
||||||
|
0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
|
||||||
|
0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
|
||||||
|
0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
|
||||||
|
0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
|
||||||
|
0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
|
||||||
|
0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
|
||||||
|
0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
|
||||||
|
0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
|
||||||
|
0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
|
||||||
|
0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
|
||||||
|
0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
|
||||||
|
0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
|
||||||
|
0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
|
||||||
|
0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
|
||||||
|
0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
|
||||||
|
0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
|
||||||
|
0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
|
||||||
|
0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
|
||||||
|
0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
|
||||||
|
0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
|
||||||
|
0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
|
||||||
|
0x7ff6, 0x7fff, 0x7ff6
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric cosine function for Q15 data.
|
||||||
|
* @param[in] x Scaled input value in radians.
|
||||||
|
* @return cos(x).
|
||||||
|
*
|
||||||
|
* The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.
|
||||||
|
*/
|
||||||
|
|
||||||
|
q15_t arm_cos_q15(
|
||||||
|
q15_t x)
|
||||||
|
{
|
||||||
|
q31_t cosVal; /* Temporary variable for output */
|
||||||
|
q15_t *tablePtr; /* Pointer to table */
|
||||||
|
q15_t in, in2; /* Temporary variables for input */
|
||||||
|
q31_t wa, wb, wc, wd; /* Cubic interpolation coefficients */
|
||||||
|
q15_t a, b, c, d; /* Four nearest output values */
|
||||||
|
q15_t fract, fractCube, fractSquare; /* Variables for fractional value */
|
||||||
|
q15_t oneBy6 = 0x1555; /* Fixed point value of 1/6 */
|
||||||
|
q15_t tableSpacing = TABLE_SPACING_Q15; /* Table spacing */
|
||||||
|
int32_t index; /* Index variable */
|
||||||
|
|
||||||
|
in = x;
|
||||||
|
|
||||||
|
/* Calculate the nearest index */
|
||||||
|
index = (int32_t) in / tableSpacing;
|
||||||
|
|
||||||
|
/* Calculate the nearest value of input */
|
||||||
|
in2 = (q15_t) index *tableSpacing;
|
||||||
|
|
||||||
|
/* Calculation of fractional value */
|
||||||
|
fract = (in - in2) << 8;
|
||||||
|
|
||||||
|
/* fractSquare = fract * fract */
|
||||||
|
fractSquare = (q15_t) ((fract * fract) >> 15);
|
||||||
|
|
||||||
|
/* fractCube = fract * fract * fract */
|
||||||
|
fractCube = (q15_t) ((fractSquare * fract) >> 15);
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (q15_t *) & cosTableQ15[index];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
/* Calculation of wa */
|
||||||
|
/* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */
|
||||||
|
wa = (q31_t) oneBy6 *fractCube;
|
||||||
|
wa += (q31_t) 0x2AAA *fract;
|
||||||
|
wa = -(wa >> 15);
|
||||||
|
wa += (fractSquare >> 1u);
|
||||||
|
|
||||||
|
/* Read first nearest value of output from the cos table */
|
||||||
|
a = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal = a * wa */
|
||||||
|
cosVal = a * wa;
|
||||||
|
|
||||||
|
/* Calculation of wb */
|
||||||
|
wb = (((fractCube >> 1u) - fractSquare) - (fract >> 1u)) + 0x7FFF;
|
||||||
|
|
||||||
|
/* Read second nearest value of output from the cos table */
|
||||||
|
b = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += b*wb */
|
||||||
|
cosVal += b * wb;
|
||||||
|
|
||||||
|
/* Calculation of wc */
|
||||||
|
wc = -(q31_t) fractCube + fractSquare;
|
||||||
|
wc = (wc >> 1u) + fract;
|
||||||
|
|
||||||
|
/* Read third nearest value of output from the cos table */
|
||||||
|
c = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += c*wc */
|
||||||
|
cosVal += c * wc;
|
||||||
|
|
||||||
|
/* Calculation of wd */
|
||||||
|
/* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
|
||||||
|
fractCube = fractCube - fract;
|
||||||
|
wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
|
||||||
|
|
||||||
|
/* Read fourth nearest value of output from the cos table */
|
||||||
|
d = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += d*wd; */
|
||||||
|
cosVal += d * wd;
|
||||||
|
|
||||||
|
/* Convert output value in 1.15(q15) format and saturate */
|
||||||
|
cosVal = __SSAT((cosVal >> 15), 16);
|
||||||
|
|
||||||
|
/* Return the output value in 1.15(q15) format */
|
||||||
|
return ((q15_t) cosVal);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of cos group
|
||||||
|
*/
|
|
@ -0,0 +1,239 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_cos_q31.c
|
||||||
|
*
|
||||||
|
* Description: Fast cosine calculation for Q31 values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup cos
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Table Values are in Q31(1.31 Fixed point format) and generation is done in three steps
|
||||||
|
* First Generate cos values in floating point:
|
||||||
|
* tableSize = 256;
|
||||||
|
* <pre>for(n = -1; n < (tableSize + 1); n++)
|
||||||
|
* {
|
||||||
|
* cosTable[n+1]= cos(2*pi*n/tableSize);
|
||||||
|
* } </pre>
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
* \par
|
||||||
|
* Secondly Convert Floating point to Q31(Fixed point):
|
||||||
|
* (cosTable[i] * pow(2, 31))
|
||||||
|
* \par
|
||||||
|
* Finally Rounding to nearest integer is done
|
||||||
|
* cosTable[i] += (cosTable[i] > 0 ? 0.5 :-0.5);
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
static const q31_t cosTableQ31[259] = {
|
||||||
|
0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f,
|
||||||
|
0x7f0991c4, 0x7e9d55fc,
|
||||||
|
0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b,
|
||||||
|
0x798a23b1, 0x78848414,
|
||||||
|
0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6,
|
||||||
|
0x6f5f02b2, 0x6dca0d14,
|
||||||
|
0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac,
|
||||||
|
0x60ec3830, 0x5ed77c8a,
|
||||||
|
0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94,
|
||||||
|
0x4ebfe8a5, 0x4c3fdff4,
|
||||||
|
0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70,
|
||||||
|
0x398cdd32, 0x36ba2014,
|
||||||
|
0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e,
|
||||||
|
0x2223a4c5, 0x1f19f97b,
|
||||||
|
0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e,
|
||||||
|
0x96a9049, 0x647d97c,
|
||||||
|
0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5,
|
||||||
|
0xed37ef91,
|
||||||
|
0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2,
|
||||||
|
0xd7d946d8, 0xd4e0cb15,
|
||||||
|
0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590,
|
||||||
|
0xc0e8b648, 0xbe31e19b,
|
||||||
|
0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c,
|
||||||
|
0xac64d510, 0xaa0a5b2e,
|
||||||
|
0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54,
|
||||||
|
0x9b1776da, 0x99307ee0,
|
||||||
|
0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a,
|
||||||
|
0x8daad37b, 0x8c4a142f,
|
||||||
|
0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5,
|
||||||
|
0x84a2fc62, 0x83d60412,
|
||||||
|
0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971,
|
||||||
|
0x8058c94c, 0x80277872,
|
||||||
|
0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971,
|
||||||
|
0x80f66e3c, 0x8162aa04,
|
||||||
|
0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5,
|
||||||
|
0x8675dc4f, 0x877b7bec,
|
||||||
|
0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a,
|
||||||
|
0x90a0fd4e, 0x9235f2ec,
|
||||||
|
0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54,
|
||||||
|
0x9f13c7d0, 0xa1288376,
|
||||||
|
0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c,
|
||||||
|
0xb140175b, 0xb3c0200c,
|
||||||
|
0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590,
|
||||||
|
0xc67322ce, 0xc945dfec,
|
||||||
|
0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2,
|
||||||
|
0xdddc5b3b, 0xe0e60685,
|
||||||
|
0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2,
|
||||||
|
0xf6956fb7, 0xf9b82684,
|
||||||
|
0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b,
|
||||||
|
0x12c8106f,
|
||||||
|
0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e,
|
||||||
|
0x2826b928, 0x2b1f34eb,
|
||||||
|
0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70,
|
||||||
|
0x3f1749b8, 0x41ce1e65,
|
||||||
|
0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94,
|
||||||
|
0x539b2af0, 0x55f5a4d2,
|
||||||
|
0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac,
|
||||||
|
0x64e88926, 0x66cf8120,
|
||||||
|
0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6,
|
||||||
|
0x72552c85, 0x73b5ebd1,
|
||||||
|
0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b,
|
||||||
|
0x7b5d039e, 0x7c29fbee,
|
||||||
|
0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f,
|
||||||
|
0x7fa736b4, 0x7fd8878e,
|
||||||
|
0x7ff62182, 0x7fffffff, 0x7ff62182
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric cosine function for Q31 data.
|
||||||
|
* @param[in] x Scaled input value in radians.
|
||||||
|
* @return cos(x).
|
||||||
|
*
|
||||||
|
* The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.
|
||||||
|
*/
|
||||||
|
|
||||||
|
q31_t arm_cos_q31(
|
||||||
|
q31_t x)
|
||||||
|
{
|
||||||
|
q31_t cosVal, in, in2; /* Temporary variables for input, output */
|
||||||
|
q31_t wa, wb, wc, wd; /* Cubic interpolation coefficients */
|
||||||
|
q31_t a, b, c, d; /* Four nearest output values */
|
||||||
|
q31_t *tablePtr; /* Pointer to table */
|
||||||
|
q31_t fract, fractCube, fractSquare; /* Temporary values for fractional values */
|
||||||
|
q31_t oneBy6 = 0x15555555; /* Fixed point value of 1/6 */
|
||||||
|
q31_t tableSpacing = TABLE_SPACING_Q31; /* Table spacing */
|
||||||
|
q31_t temp; /* Temporary variable for intermediate process */
|
||||||
|
int32_t index; /* Index variable */
|
||||||
|
|
||||||
|
in = x;
|
||||||
|
|
||||||
|
/* Calculate the nearest index */
|
||||||
|
index = in / tableSpacing;
|
||||||
|
|
||||||
|
/* Calculate the nearest value of input */
|
||||||
|
in2 = ((q31_t) index) * tableSpacing;
|
||||||
|
|
||||||
|
/* Calculation of fractional value */
|
||||||
|
fract = (in - in2) << 8;
|
||||||
|
|
||||||
|
/* fractSquare = fract * fract */
|
||||||
|
fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32));
|
||||||
|
fractSquare = fractSquare << 1;
|
||||||
|
|
||||||
|
/* fractCube = fract * fract * fract */
|
||||||
|
fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32));
|
||||||
|
fractCube = fractCube << 1;
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (q31_t *) & cosTableQ31[index];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
/* Calculation of wa */
|
||||||
|
/* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */
|
||||||
|
wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
|
||||||
|
temp = 0x2AAAAAAA;
|
||||||
|
wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32);
|
||||||
|
wa = -(wa << 1u);
|
||||||
|
wa += (fractSquare >> 1u);
|
||||||
|
|
||||||
|
/* Read first nearest value of output from the cos table */
|
||||||
|
a = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal = a*wa */
|
||||||
|
cosVal = ((q31_t) (((q63_t) a * wa) >> 32));
|
||||||
|
|
||||||
|
/* q31(1.31) Fixed point value of 1 */
|
||||||
|
temp = 0x7FFFFFFF;
|
||||||
|
|
||||||
|
/* Calculation of wb */
|
||||||
|
wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp;
|
||||||
|
/* Read second nearest value of output from the cos table */
|
||||||
|
b = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += b*wb */
|
||||||
|
cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) b * (wb))) >> 32);
|
||||||
|
|
||||||
|
/* Calculation of wc */
|
||||||
|
wc = -fractCube + fractSquare;
|
||||||
|
wc = (wc >> 1u) + fract;
|
||||||
|
/* Read third nearest values of output value from the cos table */
|
||||||
|
c = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += c*wc */
|
||||||
|
cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) c * (wc))) >> 32);
|
||||||
|
|
||||||
|
/* Calculation of wd */
|
||||||
|
/* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
|
||||||
|
fractCube = fractCube - fract;
|
||||||
|
wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
|
||||||
|
wd = (wd << 1u);
|
||||||
|
|
||||||
|
/* Read fourth nearest value of output from the cos table */
|
||||||
|
d = *tablePtr++;
|
||||||
|
|
||||||
|
/* cosVal += d*wd; */
|
||||||
|
cosVal = (q31_t) ((((q63_t) cosVal << 32) + ((q63_t) d * (wd))) >> 32);
|
||||||
|
|
||||||
|
|
||||||
|
/* convert cosVal in 2.30 format to 1.31 format */
|
||||||
|
return (__QADD(cosVal, cosVal));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of cos group
|
||||||
|
*/
|
|
@ -0,0 +1,281 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sin_f32.c
|
||||||
|
*
|
||||||
|
* Description: Fast sine calculation for floating-point values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup sin Sine
|
||||||
|
*
|
||||||
|
* Computes the trigonometric sine function using a combination of table lookup
|
||||||
|
* and cubic interpolation. There are separate functions for
|
||||||
|
* Q15, Q31, and floating-point data types.
|
||||||
|
* The input to the floating-point version is in radians while the
|
||||||
|
* fixed-point Q15 and Q31 have a scaled input with the range
|
||||||
|
* [0 +0.9999] mapping to [0 2*pi), Where range excludes 2*pi.
|
||||||
|
*
|
||||||
|
* The implementation is based on table lookup using 256 values together with cubic interpolation.
|
||||||
|
* The steps used are:
|
||||||
|
* -# Calculation of the nearest integer table index
|
||||||
|
* -# Fetch the four table values a, b, c, and d
|
||||||
|
* -# Compute the fractional portion (fract) of the table index.
|
||||||
|
* -# Calculation of wa, wb, wc, wd
|
||||||
|
* -# The final result equals <code>a*wa + b*wb + c*wc + d*wd</code>
|
||||||
|
*
|
||||||
|
* where
|
||||||
|
* <pre>
|
||||||
|
* a=Table[index-1];
|
||||||
|
* b=Table[index+0];
|
||||||
|
* c=Table[index+1];
|
||||||
|
* d=Table[index+2];
|
||||||
|
* </pre>
|
||||||
|
* and
|
||||||
|
* <pre>
|
||||||
|
* wa=-(1/6)*fract.^3 + (1/2)*fract.^2 - (1/3)*fract;
|
||||||
|
* wb=(1/2)*fract.^3 - fract.^2 - (1/2)*fract + 1;
|
||||||
|
* wc=-(1/2)*fract.^3+(1/2)*fract.^2+fract;
|
||||||
|
* wd=(1/6)*fract.^3 - (1/6)*fract;
|
||||||
|
* </pre>
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup sin
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Example code for Generation of Floating-point Sin Table:
|
||||||
|
* tableSize = 256;
|
||||||
|
* <pre>for(n = -1; n < (tableSize + 1); n++)
|
||||||
|
* {
|
||||||
|
* sinTable[n+1]=sin(2*pi*n/tableSize);
|
||||||
|
* }</pre>
|
||||||
|
* \par
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const float32_t sinTable[259] = {
|
||||||
|
-0.024541229009628296f, 0.000000000000000000f, 0.024541229009628296f,
|
||||||
|
0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
|
||||||
|
0.122410677373409270f, 0.146730467677116390f,
|
||||||
|
0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
|
||||||
|
0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
|
||||||
|
0.313681751489639280f, 0.336889863014221190f,
|
||||||
|
0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
|
||||||
|
0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
|
||||||
|
0.492898195981979370f, 0.514102756977081300f,
|
||||||
|
0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
|
||||||
|
0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
|
||||||
|
0.653172850608825680f, 0.671558976173400880f,
|
||||||
|
0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
|
||||||
|
0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
|
||||||
|
0.788346409797668460f, 0.803207516670227050f,
|
||||||
|
0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
|
||||||
|
0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
|
||||||
|
0.893224298954010010f, 0.903989315032958980f,
|
||||||
|
0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
|
||||||
|
0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
|
||||||
|
0.963776051998138430f, 0.970031261444091800f,
|
||||||
|
0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
|
||||||
|
0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
|
||||||
|
0.997290432453155520f, 0.998795449733734130f,
|
||||||
|
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
|
||||||
|
0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
|
||||||
|
0.992479562759399410f, 0.989176511764526370f,
|
||||||
|
0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
|
||||||
|
0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
|
||||||
|
0.949528157711029050f, 0.941544055938720700f,
|
||||||
|
0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
|
||||||
|
0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
|
||||||
|
0.870086967945098880f, 0.857728600502014160f,
|
||||||
|
0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
|
||||||
|
0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
|
||||||
|
0.757208824157714840f, 0.740951120853424070f,
|
||||||
|
0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
|
||||||
|
0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
|
||||||
|
0.615231573581695560f, 0.595699310302734380f,
|
||||||
|
0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
|
||||||
|
0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
|
||||||
|
0.449611335992813110f, 0.427555084228515630f,
|
||||||
|
0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
|
||||||
|
0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
|
||||||
|
0.266712754964828490f, 0.242980182170867920f,
|
||||||
|
0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
|
||||||
|
0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
|
||||||
|
0.073564566671848297f, 0.049067676067352295f,
|
||||||
|
0.024541229009628296f, 0.000000000000000122f, -0.024541229009628296f,
|
||||||
|
-0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
|
||||||
|
-0.122410677373409270f, -0.146730467677116390f,
|
||||||
|
-0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
|
||||||
|
-0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
|
||||||
|
-0.313681751489639280f, -0.336889863014221190f,
|
||||||
|
-0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
|
||||||
|
-0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
|
||||||
|
-0.492898195981979370f, -0.514102756977081300f,
|
||||||
|
-0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
|
||||||
|
-0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
|
||||||
|
-0.653172850608825680f, -0.671558976173400880f,
|
||||||
|
-0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
|
||||||
|
-0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
|
||||||
|
-0.788346409797668460f, -0.803207516670227050f,
|
||||||
|
-0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
|
||||||
|
-0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
|
||||||
|
-0.893224298954010010f, -0.903989315032958980f,
|
||||||
|
-0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
|
||||||
|
-0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
|
||||||
|
-0.963776051998138430f, -0.970031261444091800f,
|
||||||
|
-0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
|
||||||
|
-0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
|
||||||
|
-0.997290432453155520f, -0.998795449733734130f,
|
||||||
|
-0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
|
||||||
|
-0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
|
||||||
|
-0.992479562759399410f, -0.989176511764526370f,
|
||||||
|
-0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
|
||||||
|
-0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
|
||||||
|
-0.949528157711029050f, -0.941544055938720700f,
|
||||||
|
-0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
|
||||||
|
-0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
|
||||||
|
-0.870086967945098880f, -0.857728600502014160f,
|
||||||
|
-0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
|
||||||
|
-0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
|
||||||
|
-0.757208824157714840f, -0.740951120853424070f,
|
||||||
|
-0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
|
||||||
|
-0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
|
||||||
|
-0.615231573581695560f, -0.595699310302734380f,
|
||||||
|
-0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
|
||||||
|
-0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
|
||||||
|
-0.449611335992813110f, -0.427555084228515630f,
|
||||||
|
-0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
|
||||||
|
-0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
|
||||||
|
-0.266712754964828490f, -0.242980182170867920f,
|
||||||
|
-0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
|
||||||
|
-0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
|
||||||
|
-0.073564566671848297f, -0.049067676067352295f,
|
||||||
|
-0.024541229009628296f, -0.000000000000000245f, 0.024541229009628296f
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric sine function for floating-point data.
|
||||||
|
* @param[in] x input value in radians.
|
||||||
|
* @return sin(x).
|
||||||
|
*/
|
||||||
|
|
||||||
|
float32_t arm_sin_f32(
|
||||||
|
float32_t x)
|
||||||
|
{
|
||||||
|
float32_t sinVal, fract, in; /* Temporary variables for input, output */
|
||||||
|
int32_t index; /* Index variable */
|
||||||
|
uint32_t tableSize = (uint32_t) TABLE_SIZE; /* Initialise tablesize */
|
||||||
|
float32_t wa, wb, wc, wd; /* Cubic interpolation coefficients */
|
||||||
|
float32_t a, b, c, d; /* Four nearest output values */
|
||||||
|
float32_t *tablePtr; /* Pointer to table */
|
||||||
|
int32_t n;
|
||||||
|
float32_t fractsq, fractby2, fractby6, fractby3, fractsqby2;
|
||||||
|
float32_t oneminusfractby2;
|
||||||
|
float32_t frby2xfrsq, frby6xfrsq;
|
||||||
|
|
||||||
|
/* input x is in radians */
|
||||||
|
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
|
||||||
|
in = x * 0.159154943092f;
|
||||||
|
|
||||||
|
/* Calculation of floor value of input */
|
||||||
|
n = (int32_t) in;
|
||||||
|
|
||||||
|
/* Make negative values towards -infinity */
|
||||||
|
if(x < 0.0f)
|
||||||
|
{
|
||||||
|
n = n - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Map input value to [0 1] */
|
||||||
|
in = in - (float32_t) n;
|
||||||
|
|
||||||
|
/* Calculation of index of the table */
|
||||||
|
index = (uint32_t) (tableSize * in);
|
||||||
|
|
||||||
|
/* fractional value calculation */
|
||||||
|
fract = ((float32_t) tableSize * in) - (float32_t) index;
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (float32_t *) & sinTable[index];
|
||||||
|
|
||||||
|
/* Read four nearest values of input value from the sin table */
|
||||||
|
a = tablePtr[0];
|
||||||
|
b = tablePtr[1];
|
||||||
|
c = tablePtr[2];
|
||||||
|
d = tablePtr[3];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
fractsq = fract * fract;
|
||||||
|
fractby2 = fract * 0.5f;
|
||||||
|
fractby6 = fract * 0.166666667f;
|
||||||
|
fractby3 = fract * 0.3333333333333f;
|
||||||
|
fractsqby2 = fractsq * 0.5f;
|
||||||
|
frby2xfrsq = (fractby2) * fractsq;
|
||||||
|
frby6xfrsq = (fractby6) * fractsq;
|
||||||
|
oneminusfractby2 = 1.0f - fractby2;
|
||||||
|
wb = fractsqby2 - fractby3;
|
||||||
|
wc = (fractsqby2 + fract);
|
||||||
|
wa = wb - frby6xfrsq;
|
||||||
|
wb = frby2xfrsq - fractsq;
|
||||||
|
sinVal = wa * a;
|
||||||
|
wc = wc - frby2xfrsq;
|
||||||
|
wd = (frby6xfrsq) - fractby6;
|
||||||
|
wb = wb + oneminusfractby2;
|
||||||
|
|
||||||
|
/* Calculate sin value */
|
||||||
|
sinVal = (sinVal + (b * wb)) + ((c * wc) + (d * wd));
|
||||||
|
|
||||||
|
/* Return the output value */
|
||||||
|
return (sinVal);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of sin group
|
||||||
|
*/
|
|
@ -0,0 +1,208 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sin_q15.c
|
||||||
|
*
|
||||||
|
* Description: Fast sine calculation for Q15 values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup sin
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Example code for Generation of Q15 Sin Table:
|
||||||
|
* \par
|
||||||
|
* <pre>tableSize = 256;
|
||||||
|
* for(n = -1; n < (tableSize + 1); n++)
|
||||||
|
* {
|
||||||
|
* sinTable[n+1]=sin(2*pi*n/tableSize);
|
||||||
|
* } </pre>
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
* \par
|
||||||
|
* Convert Floating point to Q15(Fixed point):
|
||||||
|
* (sinTable[i] * pow(2, 15))
|
||||||
|
* \par
|
||||||
|
* rounding to nearest integer is done
|
||||||
|
* sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5);
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
static const q15_t sinTableQ15[259] = {
|
||||||
|
0xfcdc, 0x0, 0x324, 0x648, 0x96b, 0xc8c, 0xfab, 0x12c8,
|
||||||
|
0x15e2, 0x18f9, 0x1c0c, 0x1f1a, 0x2224, 0x2528, 0x2827, 0x2b1f,
|
||||||
|
0x2e11, 0x30fc, 0x33df, 0x36ba, 0x398d, 0x3c57, 0x3f17, 0x41ce,
|
||||||
|
0x447b, 0x471d, 0x49b4, 0x4c40, 0x4ec0, 0x5134, 0x539b, 0x55f6,
|
||||||
|
0x5843, 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e9, 0x66d0,
|
||||||
|
0x68a7, 0x6a6e, 0x6c24, 0x6dca, 0x6f5f, 0x70e3, 0x7255, 0x73b6,
|
||||||
|
0x7505, 0x7642, 0x776c, 0x7885, 0x798a, 0x7a7d, 0x7b5d, 0x7c2a,
|
||||||
|
0x7ce4, 0x7d8a, 0x7e1e, 0x7e9d, 0x7f0a, 0x7f62, 0x7fa7, 0x7fd9,
|
||||||
|
0x7ff6, 0x7fff, 0x7ff6, 0x7fd9, 0x7fa7, 0x7f62, 0x7f0a, 0x7e9d,
|
||||||
|
0x7e1e, 0x7d8a, 0x7ce4, 0x7c2a, 0x7b5d, 0x7a7d, 0x798a, 0x7885,
|
||||||
|
0x776c, 0x7642, 0x7505, 0x73b6, 0x7255, 0x70e3, 0x6f5f, 0x6dca,
|
||||||
|
0x6c24, 0x6a6e, 0x68a7, 0x66d0, 0x64e9, 0x62f2, 0x60ec, 0x5ed7,
|
||||||
|
0x5cb4, 0x5a82, 0x5843, 0x55f6, 0x539b, 0x5134, 0x4ec0, 0x4c40,
|
||||||
|
0x49b4, 0x471d, 0x447b, 0x41ce, 0x3f17, 0x3c57, 0x398d, 0x36ba,
|
||||||
|
0x33df, 0x30fc, 0x2e11, 0x2b1f, 0x2827, 0x2528, 0x2224, 0x1f1a,
|
||||||
|
0x1c0c, 0x18f9, 0x15e2, 0x12c8, 0xfab, 0xc8c, 0x96b, 0x648,
|
||||||
|
0x324, 0x0, 0xfcdc, 0xf9b8, 0xf695, 0xf374, 0xf055, 0xed38,
|
||||||
|
0xea1e, 0xe707, 0xe3f4, 0xe0e6, 0xdddc, 0xdad8, 0xd7d9, 0xd4e1,
|
||||||
|
0xd1ef, 0xcf04, 0xcc21, 0xc946, 0xc673, 0xc3a9, 0xc0e9, 0xbe32,
|
||||||
|
0xbb85, 0xb8e3, 0xb64c, 0xb3c0, 0xb140, 0xaecc, 0xac65, 0xaa0a,
|
||||||
|
0xa7bd, 0xa57e, 0xa34c, 0xa129, 0x9f14, 0x9d0e, 0x9b17, 0x9930,
|
||||||
|
0x9759, 0x9592, 0x93dc, 0x9236, 0x90a1, 0x8f1d, 0x8dab, 0x8c4a,
|
||||||
|
0x8afb, 0x89be, 0x8894, 0x877b, 0x8676, 0x8583, 0x84a3, 0x83d6,
|
||||||
|
0x831c, 0x8276, 0x81e2, 0x8163, 0x80f6, 0x809e, 0x8059, 0x8027,
|
||||||
|
0x800a, 0x8000, 0x800a, 0x8027, 0x8059, 0x809e, 0x80f6, 0x8163,
|
||||||
|
0x81e2, 0x8276, 0x831c, 0x83d6, 0x84a3, 0x8583, 0x8676, 0x877b,
|
||||||
|
0x8894, 0x89be, 0x8afb, 0x8c4a, 0x8dab, 0x8f1d, 0x90a1, 0x9236,
|
||||||
|
0x93dc, 0x9592, 0x9759, 0x9930, 0x9b17, 0x9d0e, 0x9f14, 0xa129,
|
||||||
|
0xa34c, 0xa57e, 0xa7bd, 0xaa0a, 0xac65, 0xaecc, 0xb140, 0xb3c0,
|
||||||
|
0xb64c, 0xb8e3, 0xbb85, 0xbe32, 0xc0e9, 0xc3a9, 0xc673, 0xc946,
|
||||||
|
0xcc21, 0xcf04, 0xd1ef, 0xd4e1, 0xd7d9, 0xdad8, 0xdddc, 0xe0e6,
|
||||||
|
0xe3f4, 0xe707, 0xea1e, 0xed38, 0xf055, 0xf374, 0xf695, 0xf9b8,
|
||||||
|
0xfcdc, 0x0, 0x324
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric sine function for Q15 data.
|
||||||
|
* @param[in] x Scaled input value in radians.
|
||||||
|
* @return sin(x).
|
||||||
|
*
|
||||||
|
* The Q15 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.
|
||||||
|
*/
|
||||||
|
|
||||||
|
q15_t arm_sin_q15(
|
||||||
|
q15_t x)
|
||||||
|
{
|
||||||
|
q31_t sinVal; /* Temporary variables output */
|
||||||
|
q15_t *tablePtr; /* Pointer to table */
|
||||||
|
q15_t fract, in, in2; /* Temporary variables for input, output */
|
||||||
|
q31_t wa, wb, wc, wd; /* Cubic interpolation coefficients */
|
||||||
|
q15_t a, b, c, d; /* Four nearest output values */
|
||||||
|
q15_t fractCube, fractSquare; /* Temporary values for fractional value */
|
||||||
|
q15_t oneBy6 = 0x1555; /* Fixed point value of 1/6 */
|
||||||
|
q15_t tableSpacing = TABLE_SPACING_Q15; /* Table spacing */
|
||||||
|
int32_t index; /* Index variable */
|
||||||
|
|
||||||
|
in = x;
|
||||||
|
|
||||||
|
/* Calculate the nearest index */
|
||||||
|
index = (int32_t) in / tableSpacing;
|
||||||
|
|
||||||
|
/* Calculate the nearest value of input */
|
||||||
|
in2 = (q15_t) ((index) * tableSpacing);
|
||||||
|
|
||||||
|
/* Calculation of fractional value */
|
||||||
|
fract = (in - in2) << 8;
|
||||||
|
|
||||||
|
/* fractSquare = fract * fract */
|
||||||
|
fractSquare = (q15_t) ((fract * fract) >> 15);
|
||||||
|
|
||||||
|
/* fractCube = fract * fract * fract */
|
||||||
|
fractCube = (q15_t) ((fractSquare * fract) >> 15);
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (q15_t *) & sinTableQ15[index];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
/* Calculation of wa */
|
||||||
|
/* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAA)*fract; */
|
||||||
|
wa = (q31_t) oneBy6 *fractCube;
|
||||||
|
wa += (q31_t) 0x2AAA *fract;
|
||||||
|
wa = -(wa >> 15);
|
||||||
|
wa += ((q31_t) fractSquare >> 1u);
|
||||||
|
|
||||||
|
/* Read first nearest value of output from the sin table */
|
||||||
|
a = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal = a * wa */
|
||||||
|
sinVal = a * wa;
|
||||||
|
|
||||||
|
/* Calculation of wb */
|
||||||
|
wb = (((q31_t) fractCube >> 1u) - (q31_t) fractSquare) -
|
||||||
|
(((q31_t) fract >> 1u) - 0x7FFF);
|
||||||
|
|
||||||
|
/* Read second nearest value of output from the sin table */
|
||||||
|
b = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += b*wb */
|
||||||
|
sinVal += b * wb;
|
||||||
|
|
||||||
|
|
||||||
|
/* Calculation of wc */
|
||||||
|
wc = -(q31_t) fractCube + fractSquare;
|
||||||
|
wc = (wc >> 1u) + fract;
|
||||||
|
|
||||||
|
/* Read third nearest value of output from the sin table */
|
||||||
|
c = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += c*wc */
|
||||||
|
sinVal += c * wc;
|
||||||
|
|
||||||
|
/* Calculation of wd */
|
||||||
|
/* wd = (oneBy6)*fractCube - (oneBy6)*fract; */
|
||||||
|
fractCube = fractCube - fract;
|
||||||
|
wd = ((q15_t) (((q31_t) oneBy6 * fractCube) >> 15));
|
||||||
|
|
||||||
|
/* Read fourth nearest value of output from the sin table */
|
||||||
|
d = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += d*wd; */
|
||||||
|
sinVal += d * wd;
|
||||||
|
|
||||||
|
/* Convert output value in 1.15(q15) format and saturate */
|
||||||
|
sinVal = __SSAT((sinVal >> 15), 16);
|
||||||
|
|
||||||
|
/* Return the output value in 1.15(q15) format */
|
||||||
|
return ((q15_t) sinVal);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of sin group
|
||||||
|
*/
|
|
@ -0,0 +1,240 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sin_q31.c
|
||||||
|
*
|
||||||
|
* Description: Fast sine calculation for Q31 values.
|
||||||
|
*
|
||||||
|
* 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 groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup sin
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* \par
|
||||||
|
* Tables generated are in Q31(1.31 Fixed point format)
|
||||||
|
* Generation of sin values in floating point:
|
||||||
|
* <pre>tableSize = 256;
|
||||||
|
* for(n = -1; n < (tableSize + 1); n++)
|
||||||
|
* {
|
||||||
|
* sinTable[n+1]= sin(2*pi*n/tableSize);
|
||||||
|
* } </pre>
|
||||||
|
* where pi value is 3.14159265358979
|
||||||
|
* \par
|
||||||
|
* Convert Floating point to Q31(Fixed point):
|
||||||
|
* (sinTable[i] * pow(2, 31))
|
||||||
|
* \par
|
||||||
|
* rounding to nearest integer is done
|
||||||
|
* sinTable[i] += (sinTable[i] > 0 ? 0.5 :-0.5);
|
||||||
|
*/
|
||||||
|
|
||||||
|
static const q31_t sinTableQ31[259] = {
|
||||||
|
0xfcdbd541, 0x0, 0x3242abf, 0x647d97c, 0x96a9049, 0xc8bd35e, 0xfab272b,
|
||||||
|
0x12c8106f,
|
||||||
|
0x15e21445, 0x18f8b83c, 0x1c0b826a, 0x1f19f97b, 0x2223a4c5, 0x25280c5e,
|
||||||
|
0x2826b928, 0x2b1f34eb,
|
||||||
|
0x2e110a62, 0x30fbc54d, 0x33def287, 0x36ba2014, 0x398cdd32, 0x3c56ba70,
|
||||||
|
0x3f1749b8, 0x41ce1e65,
|
||||||
|
0x447acd50, 0x471cece7, 0x49b41533, 0x4c3fdff4, 0x4ebfe8a5, 0x5133cc94,
|
||||||
|
0x539b2af0, 0x55f5a4d2,
|
||||||
|
0x5842dd54, 0x5a82799a, 0x5cb420e0, 0x5ed77c8a, 0x60ec3830, 0x62f201ac,
|
||||||
|
0x64e88926, 0x66cf8120,
|
||||||
|
0x68a69e81, 0x6a6d98a4, 0x6c242960, 0x6dca0d14, 0x6f5f02b2, 0x70e2cbc6,
|
||||||
|
0x72552c85, 0x73b5ebd1,
|
||||||
|
0x7504d345, 0x7641af3d, 0x776c4edb, 0x78848414, 0x798a23b1, 0x7a7d055b,
|
||||||
|
0x7b5d039e, 0x7c29fbee,
|
||||||
|
0x7ce3ceb2, 0x7d8a5f40, 0x7e1d93ea, 0x7e9d55fc, 0x7f0991c4, 0x7f62368f,
|
||||||
|
0x7fa736b4, 0x7fd8878e,
|
||||||
|
0x7ff62182, 0x7fffffff, 0x7ff62182, 0x7fd8878e, 0x7fa736b4, 0x7f62368f,
|
||||||
|
0x7f0991c4, 0x7e9d55fc,
|
||||||
|
0x7e1d93ea, 0x7d8a5f40, 0x7ce3ceb2, 0x7c29fbee, 0x7b5d039e, 0x7a7d055b,
|
||||||
|
0x798a23b1, 0x78848414,
|
||||||
|
0x776c4edb, 0x7641af3d, 0x7504d345, 0x73b5ebd1, 0x72552c85, 0x70e2cbc6,
|
||||||
|
0x6f5f02b2, 0x6dca0d14,
|
||||||
|
0x6c242960, 0x6a6d98a4, 0x68a69e81, 0x66cf8120, 0x64e88926, 0x62f201ac,
|
||||||
|
0x60ec3830, 0x5ed77c8a,
|
||||||
|
0x5cb420e0, 0x5a82799a, 0x5842dd54, 0x55f5a4d2, 0x539b2af0, 0x5133cc94,
|
||||||
|
0x4ebfe8a5, 0x4c3fdff4,
|
||||||
|
0x49b41533, 0x471cece7, 0x447acd50, 0x41ce1e65, 0x3f1749b8, 0x3c56ba70,
|
||||||
|
0x398cdd32, 0x36ba2014,
|
||||||
|
0x33def287, 0x30fbc54d, 0x2e110a62, 0x2b1f34eb, 0x2826b928, 0x25280c5e,
|
||||||
|
0x2223a4c5, 0x1f19f97b,
|
||||||
|
0x1c0b826a, 0x18f8b83c, 0x15e21445, 0x12c8106f, 0xfab272b, 0xc8bd35e,
|
||||||
|
0x96a9049, 0x647d97c,
|
||||||
|
0x3242abf, 0x0, 0xfcdbd541, 0xf9b82684, 0xf6956fb7, 0xf3742ca2, 0xf054d8d5,
|
||||||
|
0xed37ef91,
|
||||||
|
0xea1debbb, 0xe70747c4, 0xe3f47d96, 0xe0e60685, 0xdddc5b3b, 0xdad7f3a2,
|
||||||
|
0xd7d946d8, 0xd4e0cb15,
|
||||||
|
0xd1eef59e, 0xcf043ab3, 0xcc210d79, 0xc945dfec, 0xc67322ce, 0xc3a94590,
|
||||||
|
0xc0e8b648, 0xbe31e19b,
|
||||||
|
0xbb8532b0, 0xb8e31319, 0xb64beacd, 0xb3c0200c, 0xb140175b, 0xaecc336c,
|
||||||
|
0xac64d510, 0xaa0a5b2e,
|
||||||
|
0xa7bd22ac, 0xa57d8666, 0xa34bdf20, 0xa1288376, 0x9f13c7d0, 0x9d0dfe54,
|
||||||
|
0x9b1776da, 0x99307ee0,
|
||||||
|
0x9759617f, 0x9592675c, 0x93dbd6a0, 0x9235f2ec, 0x90a0fd4e, 0x8f1d343a,
|
||||||
|
0x8daad37b, 0x8c4a142f,
|
||||||
|
0x8afb2cbb, 0x89be50c3, 0x8893b125, 0x877b7bec, 0x8675dc4f, 0x8582faa5,
|
||||||
|
0x84a2fc62, 0x83d60412,
|
||||||
|
0x831c314e, 0x8275a0c0, 0x81e26c16, 0x8162aa04, 0x80f66e3c, 0x809dc971,
|
||||||
|
0x8058c94c, 0x80277872,
|
||||||
|
0x8009de7e, 0x80000000, 0x8009de7e, 0x80277872, 0x8058c94c, 0x809dc971,
|
||||||
|
0x80f66e3c, 0x8162aa04,
|
||||||
|
0x81e26c16, 0x8275a0c0, 0x831c314e, 0x83d60412, 0x84a2fc62, 0x8582faa5,
|
||||||
|
0x8675dc4f, 0x877b7bec,
|
||||||
|
0x8893b125, 0x89be50c3, 0x8afb2cbb, 0x8c4a142f, 0x8daad37b, 0x8f1d343a,
|
||||||
|
0x90a0fd4e, 0x9235f2ec,
|
||||||
|
0x93dbd6a0, 0x9592675c, 0x9759617f, 0x99307ee0, 0x9b1776da, 0x9d0dfe54,
|
||||||
|
0x9f13c7d0, 0xa1288376,
|
||||||
|
0xa34bdf20, 0xa57d8666, 0xa7bd22ac, 0xaa0a5b2e, 0xac64d510, 0xaecc336c,
|
||||||
|
0xb140175b, 0xb3c0200c,
|
||||||
|
0xb64beacd, 0xb8e31319, 0xbb8532b0, 0xbe31e19b, 0xc0e8b648, 0xc3a94590,
|
||||||
|
0xc67322ce, 0xc945dfec,
|
||||||
|
0xcc210d79, 0xcf043ab3, 0xd1eef59e, 0xd4e0cb15, 0xd7d946d8, 0xdad7f3a2,
|
||||||
|
0xdddc5b3b, 0xe0e60685,
|
||||||
|
0xe3f47d96, 0xe70747c4, 0xea1debbb, 0xed37ef91, 0xf054d8d5, 0xf3742ca2,
|
||||||
|
0xf6956fb7, 0xf9b82684,
|
||||||
|
0xfcdbd541, 0x0, 0x3242abf
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Fast approximation to the trigonometric sine function for Q31 data.
|
||||||
|
* @param[in] x Scaled input value in radians.
|
||||||
|
* @return sin(x).
|
||||||
|
*
|
||||||
|
* The Q31 input value is in the range [0 +0.9999] and is mapped to a radian value in the range [0 2*pi), Here range excludes 2*pi.
|
||||||
|
*/
|
||||||
|
|
||||||
|
q31_t arm_sin_q31(
|
||||||
|
q31_t x)
|
||||||
|
{
|
||||||
|
q31_t sinVal, in, in2; /* Temporary variables for input, output */
|
||||||
|
int32_t index; /* Index variables */
|
||||||
|
q31_t wa, wb, wc, wd; /* Cubic interpolation coefficients */
|
||||||
|
q31_t a, b, c, d; /* Four nearest output values */
|
||||||
|
q31_t *tablePtr; /* Pointer to table */
|
||||||
|
q31_t fract, fractCube, fractSquare; /* Temporary values for fractional values */
|
||||||
|
q31_t oneBy6 = 0x15555555; /* Fixed point value of 1/6 */
|
||||||
|
q31_t tableSpacing = TABLE_SPACING_Q31; /* Table spacing */
|
||||||
|
q31_t temp; /* Temporary variable for intermediate process */
|
||||||
|
|
||||||
|
in = x;
|
||||||
|
|
||||||
|
/* Calculate the nearest index */
|
||||||
|
index = (uint32_t) in / (uint32_t) tableSpacing;
|
||||||
|
|
||||||
|
/* Calculate the nearest value of input */
|
||||||
|
in2 = (q31_t) index *tableSpacing;
|
||||||
|
|
||||||
|
/* Calculation of fractional value */
|
||||||
|
fract = (in - in2) << 8;
|
||||||
|
|
||||||
|
/* fractSquare = fract * fract */
|
||||||
|
fractSquare = ((q31_t) (((q63_t) fract * fract) >> 32));
|
||||||
|
fractSquare = fractSquare << 1;
|
||||||
|
|
||||||
|
/* fractCube = fract * fract * fract */
|
||||||
|
fractCube = ((q31_t) (((q63_t) fractSquare * fract) >> 32));
|
||||||
|
fractCube = fractCube << 1;
|
||||||
|
|
||||||
|
/* Checking min and max index of table */
|
||||||
|
if(index < 0)
|
||||||
|
{
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
else if(index > 256)
|
||||||
|
{
|
||||||
|
index = 256;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Initialise table pointer */
|
||||||
|
tablePtr = (q31_t *) & sinTableQ31[index];
|
||||||
|
|
||||||
|
/* Cubic interpolation process */
|
||||||
|
/* Calculation of wa */
|
||||||
|
/* wa = -(oneBy6)*fractCube + (fractSquare >> 1u) - (0x2AAAAAAA)*fract; */
|
||||||
|
wa = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
|
||||||
|
temp = 0x2AAAAAAA;
|
||||||
|
wa = (q31_t) ((((q63_t) wa << 32) + ((q63_t) temp * fract)) >> 32);
|
||||||
|
wa = -(wa << 1u);
|
||||||
|
wa += (fractSquare >> 1u);
|
||||||
|
|
||||||
|
/* Read first nearest value of output from the sin table */
|
||||||
|
a = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal = a*wa */
|
||||||
|
sinVal = ((q31_t) (((q63_t) a * wa) >> 32));
|
||||||
|
|
||||||
|
/* q31(1.31) Fixed point value of 1 */
|
||||||
|
temp = 0x7FFFFFFF;
|
||||||
|
|
||||||
|
/* Calculation of wb */
|
||||||
|
wb = ((fractCube >> 1u) - (fractSquare + (fract >> 1u))) + temp;
|
||||||
|
|
||||||
|
/* Read second nearest value of output from the sin table */
|
||||||
|
b = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += b*wb */
|
||||||
|
sinVal = (q31_t) ((((q63_t) sinVal << 32) + (q63_t) b * (wb)) >> 32);
|
||||||
|
|
||||||
|
/* Calculation of wc */
|
||||||
|
wc = -fractCube + fractSquare;
|
||||||
|
wc = (wc >> 1u) + fract;
|
||||||
|
|
||||||
|
/* Read third nearest value of output from the sin table */
|
||||||
|
c = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += c*wc */
|
||||||
|
sinVal = (q31_t) ((((q63_t) sinVal << 32) + ((q63_t) c * wc)) >> 32);
|
||||||
|
|
||||||
|
/* Calculation of wd */
|
||||||
|
/* wd = (oneBy6) * fractCube - (oneBy6) * fract; */
|
||||||
|
fractCube = fractCube - fract;
|
||||||
|
wd = ((q31_t) (((q63_t) oneBy6 * fractCube) >> 32));
|
||||||
|
wd = (wd << 1u);
|
||||||
|
|
||||||
|
/* Read fourth nearest value of output from the sin table */
|
||||||
|
d = *tablePtr++;
|
||||||
|
|
||||||
|
/* sinVal += d*wd; */
|
||||||
|
sinVal = (q31_t) ((((q63_t) sinVal << 32) + ((q63_t) d * wd)) >> 32);
|
||||||
|
|
||||||
|
/* convert sinVal in 2.30 format to 1.31 format */
|
||||||
|
return (__QADD(sinVal, sinVal));
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of sin group
|
||||||
|
*/
|
|
@ -0,0 +1,131 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2011 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sqrt_q15.c
|
||||||
|
*
|
||||||
|
* Description: Q15 square root 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.0 2011/03/08
|
||||||
|
* Alpha release.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2011/09/30
|
||||||
|
* Beta release.
|
||||||
|
*
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
#include "arm_math.h"
|
||||||
|
#include "arm_common_tables.h"
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup SQRT
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q15 square root function.
|
||||||
|
* @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
|
||||||
|
* @param[out] *pOut square root of input value.
|
||||||
|
* @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
|
||||||
|
* <code>in</code> is negative value and returns zero output for negative values.
|
||||||
|
*/
|
||||||
|
|
||||||
|
arm_status arm_sqrt_q15(
|
||||||
|
q15_t in,
|
||||||
|
q15_t * pOut)
|
||||||
|
{
|
||||||
|
q15_t number, temp1, var1, signBits1, half;
|
||||||
|
q31_t bits_val1;
|
||||||
|
float32_t temp_float1;
|
||||||
|
|
||||||
|
number = in;
|
||||||
|
|
||||||
|
/* If the input is a positive number then compute the signBits. */
|
||||||
|
if(number > 0)
|
||||||
|
{
|
||||||
|
signBits1 = __CLZ(number) - 17;
|
||||||
|
|
||||||
|
/* Shift by the number of signBits1 */
|
||||||
|
if((signBits1 % 2) == 0)
|
||||||
|
{
|
||||||
|
number = number << signBits1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
number = number << (signBits1 - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Calculate half value of the number */
|
||||||
|
half = number >> 1;
|
||||||
|
/* Store the number for later use */
|
||||||
|
temp1 = number;
|
||||||
|
|
||||||
|
/*Convert to float */
|
||||||
|
temp_float1 = number * 3.051757812500000e-005f;
|
||||||
|
/*Store as integer */
|
||||||
|
bits_val1 = *(int *) &temp_float1;
|
||||||
|
/* Subtract the shifted value from the magic number to give intial guess */
|
||||||
|
bits_val1 = 0x5f3759df - (bits_val1 >> 1); // gives initial guess
|
||||||
|
/* Store as float */
|
||||||
|
temp_float1 = *(float *) &bits_val1;
|
||||||
|
/* Convert to integer format */
|
||||||
|
var1 = (q31_t) (temp_float1 * 16384);
|
||||||
|
|
||||||
|
/* 1st iteration */
|
||||||
|
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
|
||||||
|
((q15_t)
|
||||||
|
((((q15_t)
|
||||||
|
(((q31_t) var1 * var1) >> 15)) *
|
||||||
|
(q31_t) half) >> 15))) >> 15)) << 2;
|
||||||
|
/* 2nd iteration */
|
||||||
|
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
|
||||||
|
((q15_t)
|
||||||
|
((((q15_t)
|
||||||
|
(((q31_t) var1 * var1) >> 15)) *
|
||||||
|
(q31_t) half) >> 15))) >> 15)) << 2;
|
||||||
|
/* 3rd iteration */
|
||||||
|
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
|
||||||
|
((q15_t)
|
||||||
|
((((q15_t)
|
||||||
|
(((q31_t) var1 * var1) >> 15)) *
|
||||||
|
(q31_t) half) >> 15))) >> 15)) << 2;
|
||||||
|
|
||||||
|
/* Multiply the inverse square root with the original value */
|
||||||
|
var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
|
||||||
|
|
||||||
|
/* Shift the output down accordingly */
|
||||||
|
if((signBits1 % 2) == 0)
|
||||||
|
{
|
||||||
|
var1 = var1 >> (signBits1 / 2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
var1 = var1 >> ((signBits1 - 1) / 2);
|
||||||
|
}
|
||||||
|
*pOut = var1;
|
||||||
|
|
||||||
|
return (ARM_MATH_SUCCESS);
|
||||||
|
}
|
||||||
|
/* If the number is a negative number then store zero as its square root value */
|
||||||
|
else
|
||||||
|
{
|
||||||
|
*pOut = 0;
|
||||||
|
return (ARM_MATH_ARGUMENT_ERROR);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of SQRT group
|
||||||
|
*/
|
|
@ -0,0 +1,129 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2011 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_sqrt_q31.c
|
||||||
|
*
|
||||||
|
* Description: Q31 square root 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.0 2011/03/08
|
||||||
|
* Alpha release.
|
||||||
|
*
|
||||||
|
* Version 1.0.1 2011/09/30
|
||||||
|
* Beta release.
|
||||||
|
*
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
#include "arm_math.h"
|
||||||
|
#include "arm_common_tables.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupFastMath
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup SQRT
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Q31 square root function.
|
||||||
|
* @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
|
||||||
|
* @param[out] *pOut square root of input value.
|
||||||
|
* @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
|
||||||
|
* <code>in</code> is negative value and returns zero output for negative values.
|
||||||
|
*/
|
||||||
|
|
||||||
|
arm_status arm_sqrt_q31(
|
||||||
|
q31_t in,
|
||||||
|
q31_t * pOut)
|
||||||
|
{
|
||||||
|
q31_t number, temp1, bits_val1, var1, signBits1, half;
|
||||||
|
float32_t temp_float1;
|
||||||
|
|
||||||
|
number = in;
|
||||||
|
|
||||||
|
/* If the input is a positive number then compute the signBits. */
|
||||||
|
if(number > 0)
|
||||||
|
{
|
||||||
|
signBits1 = __CLZ(number) - 1;
|
||||||
|
|
||||||
|
/* Shift by the number of signBits1 */
|
||||||
|
if((signBits1 % 2) == 0)
|
||||||
|
{
|
||||||
|
number = number << signBits1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
number = number << (signBits1 - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Calculate half value of the number */
|
||||||
|
half = number >> 1;
|
||||||
|
/* Store the number for later use */
|
||||||
|
temp1 = number;
|
||||||
|
|
||||||
|
/*Convert to float */
|
||||||
|
temp_float1 = number * 4.6566128731e-010f;
|
||||||
|
/*Store as integer */
|
||||||
|
bits_val1 = *(int *) &temp_float1;
|
||||||
|
/* Subtract the shifted value from the magic number to give intial guess */
|
||||||
|
bits_val1 = 0x5f3759df - (bits_val1 >> 1); // gives initial guess
|
||||||
|
/* Store as float */
|
||||||
|
temp_float1 = *(float *) &bits_val1;
|
||||||
|
/* Convert to integer format */
|
||||||
|
var1 = (q31_t) (temp_float1 * 1073741824);
|
||||||
|
|
||||||
|
/* 1st iteration */
|
||||||
|
var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
|
||||||
|
((q31_t)
|
||||||
|
((((q31_t)
|
||||||
|
(((q63_t) var1 * var1) >> 31)) *
|
||||||
|
(q63_t) half) >> 31))) >> 31)) << 2;
|
||||||
|
/* 2nd iteration */
|
||||||
|
var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
|
||||||
|
((q31_t)
|
||||||
|
((((q31_t)
|
||||||
|
(((q63_t) var1 * var1) >> 31)) *
|
||||||
|
(q63_t) half) >> 31))) >> 31)) << 2;
|
||||||
|
/* 3rd iteration */
|
||||||
|
var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
|
||||||
|
((q31_t)
|
||||||
|
((((q31_t)
|
||||||
|
(((q63_t) var1 * var1) >> 31)) *
|
||||||
|
(q63_t) half) >> 31))) >> 31)) << 2;
|
||||||
|
|
||||||
|
/* Multiply the inverse square root with the original value */
|
||||||
|
var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
|
||||||
|
|
||||||
|
/* Shift the output down accordingly */
|
||||||
|
if((signBits1 % 2) == 0)
|
||||||
|
{
|
||||||
|
var1 = var1 >> (signBits1 / 2);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
var1 = var1 >> ((signBits1 - 1) / 2);
|
||||||
|
}
|
||||||
|
*pOut = var1;
|
||||||
|
|
||||||
|
return (ARM_MATH_SUCCESS);
|
||||||
|
}
|
||||||
|
/* If the number is a negative number then store zero as its square root value */
|
||||||
|
else
|
||||||
|
{
|
||||||
|
*pOut = 0;
|
||||||
|
return (ARM_MATH_ARGUMENT_ERROR);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of SQRT group
|
||||||
|
*/
|
|
@ -0,0 +1,105 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_biquad_cascade_df1_32x64_init_q31.c
|
||||||
|
*
|
||||||
|
* Description: High precision Q31 Biquad cascade filter 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.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupFilters
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BiquadCascadeDF1_32x64
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @details
|
||||||
|
*
|
||||||
|
* @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
|
||||||
|
* @param[in] numStages number of 2nd order stages in the filter.
|
||||||
|
* @param[in] *pCoeffs points to the filter coefficients.
|
||||||
|
* @param[in] *pState points to the state buffer.
|
||||||
|
* @param[in] postShift Shift to be applied after the accumulator. Varies according to the coefficients format.
|
||||||
|
* @return none
|
||||||
|
*
|
||||||
|
* <b>Coefficient and State Ordering:</b>
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The coefficients are stored in the array <code>pCoeffs</code> in the following order:
|
||||||
|
* <pre>
|
||||||
|
* {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
|
||||||
|
* </pre>
|
||||||
|
* where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
|
||||||
|
* <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
|
||||||
|
* and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The <code>pState</code> points to state variables array and size of each state variable is 1.63 format.
|
||||||
|
* Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
|
||||||
|
* The state variables are arranged in the state array as:
|
||||||
|
* <pre>
|
||||||
|
* {x[n-1], x[n-2], y[n-1], y[n-2]}
|
||||||
|
* </pre>
|
||||||
|
* The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
|
||||||
|
* The state array has a total length of <code>4*numStages</code> values.
|
||||||
|
* The state variables are updated after each block of data is processed; the coefficients are untouched.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_biquad_cas_df1_32x64_init_q31(
|
||||||
|
arm_biquad_cas_df1_32x64_ins_q31 * S,
|
||||||
|
uint8_t numStages,
|
||||||
|
q31_t * pCoeffs,
|
||||||
|
q63_t * pState,
|
||||||
|
uint8_t postShift)
|
||||||
|
{
|
||||||
|
/* Assign filter stages */
|
||||||
|
S->numStages = numStages;
|
||||||
|
|
||||||
|
/* Assign postShift to be applied to the output */
|
||||||
|
S->postShift = postShift;
|
||||||
|
|
||||||
|
/* Assign coefficient pointer */
|
||||||
|
S->pCoeffs = pCoeffs;
|
||||||
|
|
||||||
|
/* Clear state buffer and size is always 4 * numStages */
|
||||||
|
memset(pState, 0, (4u * (uint32_t) numStages) * sizeof(q63_t));
|
||||||
|
|
||||||
|
/* Assign state pointer */
|
||||||
|
S->pState = pState;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BiquadCascadeDF1_32x64 group
|
||||||
|
*/
|
|
@ -0,0 +1,553 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_biquad_cascade_df1_32x64_q31.c
|
||||||
|
*
|
||||||
|
* Description: High precision Q31 Biquad cascade filter processing 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.
|
||||||
|
*
|
||||||
|
* Version 0.0.7 2010/06/10
|
||||||
|
* Misra-C changes done
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupFilters
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BiquadCascadeDF1_32x64 High Precision Q31 Biquad Cascade Filter
|
||||||
|
*
|
||||||
|
* This function implements a high precision Biquad cascade filter which operates on
|
||||||
|
* Q31 data values. The filter coefficients are in 1.31 format and the state variables
|
||||||
|
* are in 1.63 format. The double precision state variables reduce quantization noise
|
||||||
|
* in the filter and provide a cleaner output.
|
||||||
|
* These filters are particularly useful when implementing filters in which the
|
||||||
|
* singularities are close to the unit circle. This is common for low pass or high
|
||||||
|
* pass filters with very low cutoff frequencies.
|
||||||
|
*
|
||||||
|
* The function operates on blocks of input and output data
|
||||||
|
* and each call to the function processes <code>blockSize</code> samples through
|
||||||
|
* the filter. <code>pSrc</code> and <code>pDst</code> points to input and output arrays
|
||||||
|
* containing <code>blockSize</code> Q31 values.
|
||||||
|
*
|
||||||
|
* \par Algorithm
|
||||||
|
* Each Biquad stage implements a second order filter using the difference equation:
|
||||||
|
* <pre>
|
||||||
|
* y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* </pre>
|
||||||
|
* A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.
|
||||||
|
* \image html Biquad.gif "Single Biquad filter stage"
|
||||||
|
* Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
|
||||||
|
* Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
|
||||||
|
* Pay careful attention to the sign of the feedback coefficients.
|
||||||
|
* Some design tools use the difference equation
|
||||||
|
* <pre>
|
||||||
|
* y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
|
||||||
|
* </pre>
|
||||||
|
* In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Higher order filters are realized as a cascade of second order sections.
|
||||||
|
* <code>numStages</code> refers to the number of second order stages used.
|
||||||
|
* For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
|
||||||
|
* \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"
|
||||||
|
* A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The <code>pState</code> points to state variables array .
|
||||||
|
* Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code> and each state variable in 1.63 format to improve precision.
|
||||||
|
* The state variables are arranged in the array as:
|
||||||
|
* <pre>
|
||||||
|
* {x[n-1], x[n-2], y[n-1], y[n-2]}
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
|
||||||
|
* The state array has a total length of <code>4*numStages</code> values of data in 1.63 format.
|
||||||
|
* The state variables are updated after each block of data is processed; the coefficients are untouched.
|
||||||
|
*
|
||||||
|
* \par Instance Structure
|
||||||
|
* The coefficients and state variables for a filter are stored together in an instance data structure.
|
||||||
|
* A separate instance structure must be defined for each filter.
|
||||||
|
* Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
|
||||||
|
*
|
||||||
|
* \par Init Function
|
||||||
|
* There is also an associated initialization function which performs the following operations:
|
||||||
|
* - Sets the values of the internal structure fields.
|
||||||
|
* - Zeros out the values in the state buffer.
|
||||||
|
* \par
|
||||||
|
* Use of the initialization function is optional.
|
||||||
|
* However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
|
||||||
|
* To place an instance structure into a const data section, the instance structure must be manually initialized.
|
||||||
|
* Set the values in the state buffer to zeros before static initialization.
|
||||||
|
* For example, to statically initialize the filter instance structure use
|
||||||
|
* <pre>
|
||||||
|
* arm_biquad_cas_df1_32x64_ins_q31 S1 = {numStages, pState, pCoeffs, postShift};
|
||||||
|
* </pre>
|
||||||
|
* where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer;
|
||||||
|
* <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied which is described in detail below.
|
||||||
|
* \par Fixed-Point Behavior
|
||||||
|
* Care must be taken while using Biquad Cascade 32x64 filter function.
|
||||||
|
* Following issues must be considered:
|
||||||
|
* - Scaling of coefficients
|
||||||
|
* - Filter gain
|
||||||
|
* - Overflow and saturation
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Filter coefficients are represented as fractional values and
|
||||||
|
* restricted to lie in the range <code>[-1 +1)</code>.
|
||||||
|
* The processing function has an additional scaling parameter <code>postShift</code>
|
||||||
|
* which allows the filter coefficients to exceed the range <code>[+1 -1)</code>.
|
||||||
|
* At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
|
||||||
|
* \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"
|
||||||
|
* This essentially scales the filter coefficients by <code>2^postShift</code>.
|
||||||
|
* For example, to realize the coefficients
|
||||||
|
* <pre>
|
||||||
|
* {1.5, -0.8, 1.2, 1.6, -0.9}
|
||||||
|
* </pre>
|
||||||
|
* set the Coefficient array to:
|
||||||
|
* <pre>
|
||||||
|
* {0.75, -0.4, 0.6, 0.8, -0.45}
|
||||||
|
* </pre>
|
||||||
|
* and set <code>postShift=1</code>
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The second thing to keep in mind is the gain through the filter.
|
||||||
|
* The frequency response of a Biquad filter is a function of its coefficients.
|
||||||
|
* It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.
|
||||||
|
* This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.
|
||||||
|
* To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The third item to consider is the overflow and saturation behavior of the fixed-point Q31 version.
|
||||||
|
* This is described in the function specific documentation below.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BiquadCascadeDF1_32x64
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @details
|
||||||
|
|
||||||
|
* @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter.
|
||||||
|
* @param[in] *pSrc points to the block of input data.
|
||||||
|
* @param[out] *pDst points to the block of output data.
|
||||||
|
* @param[in] blockSize number of samples to process.
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The function is implemented using an internal 64-bit accumulator.
|
||||||
|
* The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
|
||||||
|
* Thus, if the accumulator result overflows it wraps around rather than clip.
|
||||||
|
* In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).
|
||||||
|
* After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to
|
||||||
|
* 1.31 format by discarding the low 32 bits.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Two related functions are provided in the CMSIS DSP library.
|
||||||
|
* <code>arm_biquad_cascade_df1_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q63 accumulator.
|
||||||
|
* <code>arm_biquad_cascade_df1_fast_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q31 accumulator.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_biquad_cas_df1_32x64_q31(
|
||||||
|
const arm_biquad_cas_df1_32x64_ins_q31 * S,
|
||||||
|
q31_t * pSrc,
|
||||||
|
q31_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
q31_t *pIn = pSrc; /* input pointer initialization */
|
||||||
|
q31_t *pOut = pDst; /* output pointer initialization */
|
||||||
|
q63_t *pState = S->pState; /* state pointer initialization */
|
||||||
|
q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
|
||||||
|
q63_t acc; /* accumulator */
|
||||||
|
q31_t Xn1, Xn2; /* Input Filter state variables */
|
||||||
|
q63_t Yn1, Yn2; /* Output Filter state variables */
|
||||||
|
q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
|
||||||
|
q31_t Xn; /* temporary input */
|
||||||
|
int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
|
||||||
|
uint32_t sample, stage = S->numStages; /* loop counters */
|
||||||
|
q31_t acc_l, acc_h; /* temporary output */
|
||||||
|
uint32_t uShift = ((uint32_t) S->postShift + 1u);
|
||||||
|
uint32_t lShift = 32u - uShift; /* Shift to be applied to the output */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
do
|
||||||
|
{
|
||||||
|
/* Reading the coefficients */
|
||||||
|
b0 = *pCoeffs++;
|
||||||
|
b1 = *pCoeffs++;
|
||||||
|
b2 = *pCoeffs++;
|
||||||
|
a1 = *pCoeffs++;
|
||||||
|
a2 = *pCoeffs++;
|
||||||
|
|
||||||
|
/* Reading the state values */
|
||||||
|
Xn1 = (q31_t) (pState[0]);
|
||||||
|
Xn2 = (q31_t) (pState[1]);
|
||||||
|
Yn1 = pState[2];
|
||||||
|
Yn2 = pState[3];
|
||||||
|
|
||||||
|
/* Apply loop unrolling and compute 4 output values simultaneously. */
|
||||||
|
/* The variable acc hold output value that is being computed and
|
||||||
|
* stored in the destination buffer
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
*/
|
||||||
|
|
||||||
|
sample = 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(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc = (q63_t) Xn *b0;
|
||||||
|
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc += (q63_t) Xn1 *b1;
|
||||||
|
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn2 *b2;
|
||||||
|
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn1, a1);
|
||||||
|
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn2, a2);
|
||||||
|
|
||||||
|
/* The result is converted to 1.63 , Yn2 variable is reused */
|
||||||
|
Yn2 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
*pOut = acc_h;
|
||||||
|
|
||||||
|
/* Read the second input into Xn2, to reuse the value */
|
||||||
|
Xn2 = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc = (q63_t) Xn *b1;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc += (q63_t) Xn2 *b0;
|
||||||
|
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn1 *b2;
|
||||||
|
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn2, a1);
|
||||||
|
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn1, a2);
|
||||||
|
|
||||||
|
/* The result is converted to 1.63, Yn1 variable is reused */
|
||||||
|
Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Read the third input into Xn1, to reuse the value */
|
||||||
|
Xn1 = *pIn++;
|
||||||
|
|
||||||
|
/* The result is converted to 1.31 */
|
||||||
|
/* Store the output in the destination buffer. */
|
||||||
|
*(pOut + 1u) = acc_h;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc = (q63_t) Xn1 *b0;
|
||||||
|
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc += (q63_t) Xn2 *b1;
|
||||||
|
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn *b2;
|
||||||
|
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn1, a1);
|
||||||
|
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn2, a2);
|
||||||
|
|
||||||
|
/* The result is converted to 1.63, Yn2 variable is reused */
|
||||||
|
Yn2 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
*(pOut + 2u) = acc_h;
|
||||||
|
|
||||||
|
/* Read the fourth input into Xn, to reuse the value */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc = (q63_t) Xn *b0;
|
||||||
|
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc += (q63_t) Xn1 *b1;
|
||||||
|
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn2 *b2;
|
||||||
|
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn2, a1);
|
||||||
|
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn1, a2);
|
||||||
|
|
||||||
|
/* The result is converted to 1.63, Yn1 variable is reused */
|
||||||
|
Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
*(pOut + 3u) = acc_h;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
|
||||||
|
/* update output pointer */
|
||||||
|
pOut += 4u;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
sample = (blockSize & 0x3u);
|
||||||
|
|
||||||
|
while(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc = (q63_t) Xn *b0;
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc += (q63_t) Xn1 *b1;
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn2 *b2;
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn1, a1);
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn2, a2);
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
Yn2 = Yn1;
|
||||||
|
/* The result is converted to 1.63, Yn1 variable is reused */
|
||||||
|
Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
*pOut++ = acc_h;
|
||||||
|
//Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
// *pOut++ = (q31_t) (acc >> (32 - shift));
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* The first stage output is given as input to the second stage. */
|
||||||
|
pIn = pDst;
|
||||||
|
|
||||||
|
/* Reset to destination buffer working pointer */
|
||||||
|
pOut = pDst;
|
||||||
|
|
||||||
|
/* Store the updated state variables back into the pState array */
|
||||||
|
/* Store the updated state variables back into the pState array */
|
||||||
|
*pState++ = (q63_t) Xn1;
|
||||||
|
*pState++ = (q63_t) Xn2;
|
||||||
|
*pState++ = Yn1;
|
||||||
|
*pState++ = Yn2;
|
||||||
|
|
||||||
|
} while(--stage);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
do
|
||||||
|
{
|
||||||
|
/* Reading the coefficients */
|
||||||
|
b0 = *pCoeffs++;
|
||||||
|
b1 = *pCoeffs++;
|
||||||
|
b2 = *pCoeffs++;
|
||||||
|
a1 = *pCoeffs++;
|
||||||
|
a2 = *pCoeffs++;
|
||||||
|
|
||||||
|
/* Reading the state values */
|
||||||
|
Xn1 = pState[0];
|
||||||
|
Xn2 = pState[1];
|
||||||
|
Yn1 = pState[2];
|
||||||
|
Yn2 = pState[3];
|
||||||
|
|
||||||
|
/* The variable acc hold output value that is being computed and
|
||||||
|
* stored in the destination buffer
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
*/
|
||||||
|
|
||||||
|
sample = blockSize;
|
||||||
|
|
||||||
|
while(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
/* acc = b0 * x[n] */
|
||||||
|
acc = (q63_t) Xn *b0;
|
||||||
|
/* acc += b1 * x[n-1] */
|
||||||
|
acc += (q63_t) Xn1 *b1;
|
||||||
|
/* acc += b[2] * x[n-2] */
|
||||||
|
acc += (q63_t) Xn2 *b2;
|
||||||
|
/* acc += a1 * y[n-1] */
|
||||||
|
acc += mult32x64(Yn1, a1);
|
||||||
|
/* acc += a2 * y[n-2] */
|
||||||
|
acc += mult32x64(Yn2, a2);
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
Yn2 = Yn1;
|
||||||
|
|
||||||
|
/* The result is converted to 1.63, Yn1 variable is reused */
|
||||||
|
Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Calc lower part of acc */
|
||||||
|
acc_l = acc & 0xffffffff;
|
||||||
|
|
||||||
|
/* Calc upper part of acc */
|
||||||
|
acc_h = (acc >> 32) & 0xffffffff;
|
||||||
|
|
||||||
|
/* Apply shift for lower part of acc and upper part of acc */
|
||||||
|
acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
*pOut++ = acc_h;
|
||||||
|
|
||||||
|
//Yn1 = acc << shift;
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer in 1.31 format. */
|
||||||
|
//*pOut++ = (q31_t) (acc >> (32 - shift));
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* The first stage output is given as input to the second stage. */
|
||||||
|
pIn = pDst;
|
||||||
|
|
||||||
|
/* Reset to destination buffer working pointer */
|
||||||
|
pOut = pDst;
|
||||||
|
|
||||||
|
/* Store the updated state variables back into the pState array */
|
||||||
|
*pState++ = (q63_t) Xn1;
|
||||||
|
*pState++ = (q63_t) Xn2;
|
||||||
|
*pState++ = Yn1;
|
||||||
|
*pState++ = Yn2;
|
||||||
|
|
||||||
|
} while(--stage);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BiquadCascadeDF1_32x64 group
|
||||||
|
*/
|
|
@ -0,0 +1,421 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_biquad_cascade_df1_f32.c
|
||||||
|
*
|
||||||
|
* Description: Processing function for the
|
||||||
|
* floating-point Biquad cascade DirectFormI(DF1) filter.
|
||||||
|
*
|
||||||
|
* 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 groupFilters
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @defgroup BiquadCascadeDF1 Biquad Cascade IIR Filters Using Direct Form I Structure
|
||||||
|
*
|
||||||
|
* This set of functions implements arbitrary order recursive (IIR) filters.
|
||||||
|
* The filters are implemented as a cascade of second order Biquad sections.
|
||||||
|
* The functions support Q15, Q31 and floating-point data types.
|
||||||
|
* Fast version of Q15 and Q31 also supported on CortexM4 and Cortex-M3.
|
||||||
|
*
|
||||||
|
* The functions operate on blocks of input and output data and each call to the function
|
||||||
|
* processes <code>blockSize</code> samples through the filter.
|
||||||
|
* <code>pSrc</code> points to the array of input data and
|
||||||
|
* <code>pDst</code> points to the array of output data.
|
||||||
|
* Both arrays contain <code>blockSize</code> values.
|
||||||
|
*
|
||||||
|
* \par Algorithm
|
||||||
|
* Each Biquad stage implements a second order filter using the difference equation:
|
||||||
|
* <pre>
|
||||||
|
* y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* </pre>
|
||||||
|
* A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.
|
||||||
|
* \image html Biquad.gif "Single Biquad filter stage"
|
||||||
|
* Coefficients <code>b0, b1 and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
|
||||||
|
* Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
|
||||||
|
* Pay careful attention to the sign of the feedback coefficients.
|
||||||
|
* Some design tools use the difference equation
|
||||||
|
* <pre>
|
||||||
|
* y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
|
||||||
|
* </pre>
|
||||||
|
* In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Higher order filters are realized as a cascade of second order sections.
|
||||||
|
* <code>numStages</code> refers to the number of second order stages used.
|
||||||
|
* For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
|
||||||
|
* \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"
|
||||||
|
* A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The <code>pState</code> points to state variables array.
|
||||||
|
* Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
|
||||||
|
* The state variables are arranged in the <code>pState</code> array as:
|
||||||
|
* <pre>
|
||||||
|
* {x[n-1], x[n-2], y[n-1], y[n-2]}
|
||||||
|
* </pre>
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
|
||||||
|
* The state array has a total length of <code>4*numStages</code> values.
|
||||||
|
* The state variables are updated after each block of data is processed, the coefficients are untouched.
|
||||||
|
*
|
||||||
|
* \par Instance Structure
|
||||||
|
* The coefficients and state variables for a filter are stored together in an instance data structure.
|
||||||
|
* A separate instance structure must be defined for each filter.
|
||||||
|
* Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
|
||||||
|
* There are separate instance structure declarations for each of the 3 supported data types.
|
||||||
|
*
|
||||||
|
* \par Init Functions
|
||||||
|
* There is also an associated initialization function for each data type.
|
||||||
|
* The initialization function performs following operations:
|
||||||
|
* - Sets the values of the internal structure fields.
|
||||||
|
* - Zeros out the values in the state buffer.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Use of the initialization function is optional.
|
||||||
|
* However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
|
||||||
|
* To place an instance structure into a const data section, the instance structure must be manually initialized.
|
||||||
|
* Set the values in the state buffer to zeros before static initialization.
|
||||||
|
* The code below statically initializes each of the 3 different data type filter instance structures
|
||||||
|
* <pre>
|
||||||
|
* arm_biquad_casd_df1_inst_f32 S1 = {numStages, pState, pCoeffs};
|
||||||
|
* arm_biquad_casd_df1_inst_q15 S2 = {numStages, pState, pCoeffs, postShift};
|
||||||
|
* arm_biquad_casd_df1_inst_q31 S3 = {numStages, pState, pCoeffs, postShift};
|
||||||
|
* </pre>
|
||||||
|
* where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer;
|
||||||
|
* <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied.
|
||||||
|
*
|
||||||
|
* \par Fixed-Point Behavior
|
||||||
|
* Care must be taken when using the Q15 and Q31 versions of the Biquad Cascade filter functions.
|
||||||
|
* Following issues must be considered:
|
||||||
|
* - Scaling of coefficients
|
||||||
|
* - Filter gain
|
||||||
|
* - Overflow and saturation
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* <b>Scaling of coefficients: </b>
|
||||||
|
* Filter coefficients are represented as fractional values and
|
||||||
|
* coefficients are restricted to lie in the range <code>[-1 +1)</code>.
|
||||||
|
* The fixed-point functions have an additional scaling parameter <code>postShift</code>
|
||||||
|
* which allow the filter coefficients to exceed the range <code>[+1 -1)</code>.
|
||||||
|
* At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
|
||||||
|
* \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"
|
||||||
|
* This essentially scales the filter coefficients by <code>2^postShift</code>.
|
||||||
|
* For example, to realize the coefficients
|
||||||
|
* <pre>
|
||||||
|
* {1.5, -0.8, 1.2, 1.6, -0.9}
|
||||||
|
* </pre>
|
||||||
|
* set the pCoeffs array to:
|
||||||
|
* <pre>
|
||||||
|
* {0.75, -0.4, 0.6, 0.8, -0.45}
|
||||||
|
* </pre>
|
||||||
|
* and set <code>postShift=1</code>
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* <b>Filter gain: </b>
|
||||||
|
* The frequency response of a Biquad filter is a function of its coefficients.
|
||||||
|
* It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.
|
||||||
|
* This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.
|
||||||
|
* To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* <b>Overflow and saturation: </b>
|
||||||
|
* For Q15 and Q31 versions, it is described separately as part of the function specific documentation below.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BiquadCascadeDF1
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @param[in] *S points to an instance of the floating-point Biquad cascade structure.
|
||||||
|
* @param[in] *pSrc points to the block of input data.
|
||||||
|
* @param[out] *pDst points to the block of output data.
|
||||||
|
* @param[in] blockSize number of samples to process per call.
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_biquad_cascade_df1_f32(
|
||||||
|
const arm_biquad_casd_df1_inst_f32 * S,
|
||||||
|
float32_t * pSrc,
|
||||||
|
float32_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
float32_t *pIn = pSrc; /* source pointer */
|
||||||
|
float32_t *pOut = pDst; /* destination pointer */
|
||||||
|
float32_t *pState = S->pState; /* pState pointer */
|
||||||
|
float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
|
||||||
|
float32_t acc; /* Simulates the accumulator */
|
||||||
|
float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
|
||||||
|
float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */
|
||||||
|
float32_t Xn; /* temporary input */
|
||||||
|
uint32_t sample, stage = S->numStages; /* loop counters */
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_CM0
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M4 and Cortex-M3 */
|
||||||
|
|
||||||
|
do
|
||||||
|
{
|
||||||
|
/* Reading the coefficients */
|
||||||
|
b0 = *pCoeffs++;
|
||||||
|
b1 = *pCoeffs++;
|
||||||
|
b2 = *pCoeffs++;
|
||||||
|
a1 = *pCoeffs++;
|
||||||
|
a2 = *pCoeffs++;
|
||||||
|
|
||||||
|
/* Reading the pState values */
|
||||||
|
Xn1 = pState[0];
|
||||||
|
Xn2 = pState[1];
|
||||||
|
Yn1 = pState[2];
|
||||||
|
Yn2 = pState[3];
|
||||||
|
|
||||||
|
/* Apply loop unrolling and compute 4 output values simultaneously. */
|
||||||
|
/* The variable acc hold output values that are being computed:
|
||||||
|
*
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
*/
|
||||||
|
|
||||||
|
sample = 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(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the first input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
Yn2 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = Yn2;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
|
||||||
|
/* Read the second input */
|
||||||
|
Xn2 = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
Yn1 = (b0 * Xn2) + (b1 * Xn) + (b2 * Xn1) + (a1 * Yn2) + (a2 * Yn1);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = Yn1;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
|
||||||
|
/* Read the third input */
|
||||||
|
Xn1 = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
Yn2 = (b0 * Xn1) + (b1 * Xn2) + (b2 * Xn) + (a1 * Yn1) + (a2 * Yn2);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = Yn2;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
|
||||||
|
/* Read the forth input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
Yn1 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn2) + (a2 * Yn1);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = Yn1;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 4, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
sample = blockSize & 0x3u;
|
||||||
|
|
||||||
|
while(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = acc;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
Yn2 = Yn1;
|
||||||
|
Yn1 = acc;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Store the updated state variables back into the pState array */
|
||||||
|
*pState++ = Xn1;
|
||||||
|
*pState++ = Xn2;
|
||||||
|
*pState++ = Yn1;
|
||||||
|
*pState++ = Yn2;
|
||||||
|
|
||||||
|
/* The first stage goes from the input buffer to the output buffer. */
|
||||||
|
/* Subsequent numStages occur in-place in the output buffer */
|
||||||
|
pIn = pDst;
|
||||||
|
|
||||||
|
/* Reset the output pointer */
|
||||||
|
pOut = pDst;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
stage--;
|
||||||
|
|
||||||
|
} while(stage > 0u);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
/* Run the below code for Cortex-M0 */
|
||||||
|
|
||||||
|
do
|
||||||
|
{
|
||||||
|
/* Reading the coefficients */
|
||||||
|
b0 = *pCoeffs++;
|
||||||
|
b1 = *pCoeffs++;
|
||||||
|
b2 = *pCoeffs++;
|
||||||
|
a1 = *pCoeffs++;
|
||||||
|
a2 = *pCoeffs++;
|
||||||
|
|
||||||
|
/* Reading the pState values */
|
||||||
|
Xn1 = pState[0];
|
||||||
|
Xn2 = pState[1];
|
||||||
|
Yn1 = pState[2];
|
||||||
|
Yn2 = pState[3];
|
||||||
|
|
||||||
|
/* The variables acc holds the output value that is computed:
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
*/
|
||||||
|
|
||||||
|
sample = blockSize;
|
||||||
|
|
||||||
|
while(sample > 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
Xn = *pIn++;
|
||||||
|
|
||||||
|
/* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
|
||||||
|
acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
|
||||||
|
|
||||||
|
/* Store the result in the accumulator in the destination buffer. */
|
||||||
|
*pOut++ = acc;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
Xn2 = Xn1;
|
||||||
|
Xn1 = Xn;
|
||||||
|
Yn2 = Yn1;
|
||||||
|
Yn1 = acc;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Store the updated state variables back into the pState array */
|
||||||
|
*pState++ = Xn1;
|
||||||
|
*pState++ = Xn2;
|
||||||
|
*pState++ = Yn1;
|
||||||
|
*pState++ = Yn2;
|
||||||
|
|
||||||
|
/* The first stage goes from the input buffer to the output buffer. */
|
||||||
|
/* Subsequent numStages occur in-place in the output buffer */
|
||||||
|
pIn = pDst;
|
||||||
|
|
||||||
|
/* Reset the output pointer */
|
||||||
|
pOut = pDst;
|
||||||
|
|
||||||
|
/* decrement the loop counter */
|
||||||
|
stage--;
|
||||||
|
|
||||||
|
} while(stage > 0u);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_CM0 */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BiquadCascadeDF1 group
|
||||||
|
*/
|
|
@ -0,0 +1,283 @@
|
||||||
|
/* ----------------------------------------------------------------------
|
||||||
|
* Copyright (C) 2010 ARM Limited. All rights reserved.
|
||||||
|
*
|
||||||
|
* $Date: 15. February 2012
|
||||||
|
* $Revision: V1.1.0
|
||||||
|
*
|
||||||
|
* Project: CMSIS DSP Library
|
||||||
|
* Title: arm_biquad_cascade_df1_fast_q15.c
|
||||||
|
*
|
||||||
|
* Description: Fast processing function for the
|
||||||
|
* Q15 Biquad cascade filter.
|
||||||
|
*
|
||||||
|
* Target Processor: Cortex-M4/Cortex-M3
|
||||||
|
*
|
||||||
|
* 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.9 2010/08/16
|
||||||
|
* Initial version
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* -------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @ingroup groupFilters
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @addtogroup BiquadCascadeDF1
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @details
|
||||||
|
* @param[in] *S points to an instance of the Q15 Biquad cascade structure.
|
||||||
|
* @param[in] *pSrc points to the block of input data.
|
||||||
|
* @param[out] *pDst points to the block of output data.
|
||||||
|
* @param[in] blockSize number of samples to process per call.
|
||||||
|
* @return none.
|
||||||
|
*
|
||||||
|
* <b>Scaling and Overflow Behavior:</b>
|
||||||
|
* \par
|
||||||
|
* This fast version uses a 32-bit accumulator with 2.30 format.
|
||||||
|
* The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
|
||||||
|
* Thus, if the accumulator result overflows it wraps around and distorts the result.
|
||||||
|
* In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25).
|
||||||
|
* The 2.30 accumulator is then shifted by <code>postShift</code> bits and the result truncated to 1.15 format by discarding the low 16 bits.
|
||||||
|
*
|
||||||
|
* \par
|
||||||
|
* Refer to the function <code>arm_biquad_cascade_df1_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion. Both the slow and the fast versions use the same instance structure.
|
||||||
|
* Use the function <code>arm_biquad_cascade_df1_init_q15()</code> to initialize the filter structure.
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
|
void arm_biquad_cascade_df1_fast_q15(
|
||||||
|
const arm_biquad_casd_df1_inst_q15 * S,
|
||||||
|
q15_t * pSrc,
|
||||||
|
q15_t * pDst,
|
||||||
|
uint32_t blockSize)
|
||||||
|
{
|
||||||
|
q15_t *pIn = pSrc; /* Source pointer */
|
||||||
|
q15_t *pOut = pDst; /* Destination pointer */
|
||||||
|
q31_t in; /* Temporary variable to hold input value */
|
||||||
|
q31_t out; /* Temporary variable to hold output value */
|
||||||
|
q31_t b0; /* Temporary variable to hold bo value */
|
||||||
|
q31_t b1, a1; /* Filter coefficients */
|
||||||
|
q31_t state_in, state_out; /* Filter state variables */
|
||||||
|
q31_t acc; /* Accumulator */
|
||||||
|
int32_t shift = (int32_t) (15 - S->postShift); /* Post shift */
|
||||||
|
q15_t *pState = S->pState; /* State pointer */
|
||||||
|
q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
|
||||||
|
uint32_t sample, stage = S->numStages; /* Stage loop counter */
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
do
|
||||||
|
{
|
||||||
|
|
||||||
|
/* Read the b0 and 0 coefficients using SIMD */
|
||||||
|
b0 = *__SIMD32(pCoeffs)++;
|
||||||
|
|
||||||
|
/* Read the b1 and b2 coefficients using SIMD */
|
||||||
|
b1 = *__SIMD32(pCoeffs)++;
|
||||||
|
|
||||||
|
/* Read the a1 and a2 coefficients using SIMD */
|
||||||
|
a1 = *__SIMD32(pCoeffs)++;
|
||||||
|
|
||||||
|
/* Read the input state values from the state buffer: x[n-1], x[n-2] */
|
||||||
|
state_in = *__SIMD32(pState)++;
|
||||||
|
|
||||||
|
/* Read the output state values from the state buffer: y[n-1], y[n-2] */
|
||||||
|
state_out = *__SIMD32(pState)--;
|
||||||
|
|
||||||
|
/* Apply loop unrolling and compute 2 output values simultaneously. */
|
||||||
|
/* The variable acc hold output values that are being computed:
|
||||||
|
*
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
|
||||||
|
*/
|
||||||
|
sample = blockSize >> 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(sample > 0u)
|
||||||
|
{
|
||||||
|
|
||||||
|
/* Read the input */
|
||||||
|
in = *__SIMD32(pIn)++;
|
||||||
|
|
||||||
|
/* out = b0 * x[n] + 0 * 0 */
|
||||||
|
out = __SMUAD(b0, in);
|
||||||
|
/* acc = b1 * x[n-1] + acc += b2 * x[n-2] + out */
|
||||||
|
acc = __SMLAD(b1, state_in, out);
|
||||||
|
/* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
|
||||||
|
acc = __SMLAD(a1, state_out, acc);
|
||||||
|
|
||||||
|
/* The result is converted from 3.29 to 1.31 and then saturation is applied */
|
||||||
|
out = __SSAT((acc >> shift), 16);
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
/* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
|
||||||
|
/* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
state_in = __PKHBT(in, state_in, 16);
|
||||||
|
state_out = __PKHBT(out, state_out, 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
state_in = __PKHBT(state_in >> 16, (in >> 16), 16);
|
||||||
|
state_out = __PKHBT(state_out >> 16, (out), 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* out = b0 * x[n] + 0 * 0 */
|
||||||
|
out = __SMUADX(b0, in);
|
||||||
|
/* acc0 = b1 * x[n-1] , acc0 += b2 * x[n-2] + out */
|
||||||
|
acc = __SMLAD(b1, state_in, out);
|
||||||
|
/* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
|
||||||
|
acc = __SMLAD(a1, state_out, acc);
|
||||||
|
|
||||||
|
/* The result is converted from 3.29 to 1.31 and then saturation is applied */
|
||||||
|
out = __SSAT((acc >> shift), 16);
|
||||||
|
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer. */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
*__SIMD32(pOut)++ = __PKHBT(state_out, out, 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
*__SIMD32(pOut)++ = __PKHBT(out, state_out >> 16, 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
/* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
|
||||||
|
/* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
state_in = __PKHBT(in >> 16, state_in, 16);
|
||||||
|
state_out = __PKHBT(out, state_out, 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
state_in = __PKHBT(state_in >> 16, in, 16);
|
||||||
|
state_out = __PKHBT(state_out >> 16, out, 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
sample--;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* If the blockSize is not a multiple of 2, compute any remaining output samples here.
|
||||||
|
** No loop unrolling is used. */
|
||||||
|
|
||||||
|
if((blockSize & 0x1u) != 0u)
|
||||||
|
{
|
||||||
|
/* Read the input */
|
||||||
|
in = *pIn++;
|
||||||
|
|
||||||
|
/* out = b0 * x[n] + 0 * 0 */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
out = __SMUAD(b0, in);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
out = __SMUADX(b0, in);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
/* acc = b1 * x[n-1], acc += b2 * x[n-2] + out */
|
||||||
|
acc = __SMLAD(b1, state_in, out);
|
||||||
|
/* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
|
||||||
|
acc = __SMLAD(a1, state_out, acc);
|
||||||
|
|
||||||
|
/* The result is converted from 3.29 to 1.31 and then saturation is applied */
|
||||||
|
out = __SSAT((acc >> shift), 16);
|
||||||
|
|
||||||
|
/* Store the output in the destination buffer. */
|
||||||
|
*pOut++ = (q15_t) out;
|
||||||
|
|
||||||
|
/* Every time after the output is computed state should be updated. */
|
||||||
|
/* The states should be updated as: */
|
||||||
|
/* Xn2 = Xn1 */
|
||||||
|
/* Xn1 = Xn */
|
||||||
|
/* Yn2 = Yn1 */
|
||||||
|
/* Yn1 = acc */
|
||||||
|
/* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
|
||||||
|
/* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
|
||||||
|
|
||||||
|
#ifndef ARM_MATH_BIG_ENDIAN
|
||||||
|
|
||||||
|
state_in = __PKHBT(in, state_in, 16);
|
||||||
|
state_out = __PKHBT(out, state_out, 16);
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
state_in = __PKHBT(state_in >> 16, in, 16);
|
||||||
|
state_out = __PKHBT(state_out >> 16, out, 16);
|
||||||
|
|
||||||
|
#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/* The first stage goes from the input buffer to the output buffer. */
|
||||||
|
/* Subsequent (numStages - 1) occur in-place in the output buffer */
|
||||||
|
pIn = pDst;
|
||||||
|
|
||||||
|
/* Reset the output pointer */
|
||||||
|
pOut = pDst;
|
||||||
|
|
||||||
|
/* Store the updated state variables back into the state array */
|
||||||
|
*__SIMD32(pState)++ = state_in;
|
||||||
|
*__SIMD32(pState)++ = state_out;
|
||||||
|
|
||||||
|
|
||||||
|
/* Decrement the loop counter */
|
||||||
|
stage--;
|
||||||
|
|
||||||
|
} while(stage > 0u);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @} end of BiquadCascadeDF1 group
|
||||||
|
*/
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue