WebSVN – dashGPS – Blame – /branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_mat_add_f32.c
		4	* Description: Floating-point matrix addition
		5	*
		6	* $Date: 27. January 2017
		7	* $Revision: V.1.5.1
		8	*
		9	* Target Processor: Cortex-M cores
		10	* -------------------------------------------------------------------- */
		11	/*
		12	* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
		13	*
		14	* SPDX-License-Identifier: Apache-2.0
		15	*
		16	* Licensed under the Apache License, Version 2.0 (the License); you may
		17	* not use this file except in compliance with the License.
		18	* You may obtain a copy of the License at
		19	*
		20	* www.apache.org/licenses/LICENSE-2.0
		21	*
		22	* Unless required by applicable law or agreed to in writing, software
		23	* distributed under the License is distributed on an AS IS BASIS, WITHOUT
		24	* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
		25	* See the License for the specific language governing permissions and
		26	* limitations under the License.
		27	*/
		28
		29	#include "arm_math.h"
		30
		31	/**
		32	* @ingroup groupMatrix
		33	*/
		34
		35	/**
		36	* @defgroup MatrixAdd Matrix Addition
		37	*
		38	* Adds two matrices.
		39	* \image html MatrixAddition.gif "Addition of two 3 x 3 matrices"
		40	*
		41	* The functions check to make sure that
		42	* <code>pSrcA</code>, <code>pSrcB</code>, and <code>pDst</code> have the same
		43	* number of rows and columns.
		44	*/
		45
		46	/**
		47	* @addtogroup MatrixAdd
		48	* @{
		49	*/
		50
		51
		52	/**
		53	* @brief Floating-point matrix addition.
		54	* @param[in] *pSrcA points to the first input matrix structure
		55	* @param[in] *pSrcB points to the second input matrix structure
		56	* @param[out] *pDst points to output matrix structure
		57	* @return The function returns either
		58	* <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
		59	*/
		60
		61	arm_status arm_mat_add_f32(
		62	const arm_matrix_instance_f32 * pSrcA,
		63	const arm_matrix_instance_f32 * pSrcB,
		64	arm_matrix_instance_f32 * pDst)
		65	{
		66	float32_t pIn1 = pSrcA->pData; / input data matrix pointer A */
		67	float32_t pIn2 = pSrcB->pData; / input data matrix pointer B */
		68	float32_t pOut = pDst->pData; / output data matrix pointer */
		69
		70	#if defined (ARM_MATH_DSP)
		71
		72	float32_t inA1, inA2, inB1, inB2, out1, out2; /* temporary variables */
		73
		74	#endif // #if defined (ARM_MATH_DSP)
		75
		76	uint32_t numSamples; /* total number of elements in the matrix */
		77	uint32_t blkCnt; /* loop counters */
		78	arm_status status; /* status of matrix addition */
		79
		80	#ifdef ARM_MATH_MATRIX_CHECK
		81	/* Check for matrix mismatch condition */
		82	if ((pSrcA->numRows != pSrcB->numRows) \|\|
		83	(pSrcA->numCols != pSrcB->numCols) \|\|
		84	(pSrcA->numRows != pDst->numRows) \|\| (pSrcA->numCols != pDst->numCols))
		85	{
		86	/* Set status as ARM_MATH_SIZE_MISMATCH */
		87	status = ARM_MATH_SIZE_MISMATCH;
		88	}
		89	else
		90	#endif
		91	{
		92
		93	/* Total number of samples in the input matrix */
		94	numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols;
		95
		96	#if defined (ARM_MATH_DSP)
		97
		98	/* Loop unrolling */
		99	blkCnt = numSamples >> 2U;
		100
		101	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
		102	** a second loop below computes the remaining 1 to 3 samples. */
		103	while (blkCnt > 0U)
		104	{
		105	/* C(m,n) = A(m,n) + B(m,n) */
		106	/* Add and then store the results in the destination buffer. */
		107	/* Read values from source A */
		108	inA1 = pIn1[0];
		109
		110	/* Read values from source B */
		111	inB1 = pIn2[0];
		112
		113	/* Read values from source A */
		114	inA2 = pIn1[1];
		115
		116	/* out = sourceA + sourceB */
		117	out1 = inA1 + inB1;
		118
		119	/* Read values from source B */
		120	inB2 = pIn2[1];
		121
		122	/* Read values from source A */
		123	inA1 = pIn1[2];
		124
		125	/* out = sourceA + sourceB */
		126	out2 = inA2 + inB2;
		127
		128	/* Read values from source B */
		129	inB1 = pIn2[2];
		130
		131	/* Store result in destination */
		132	pOut[0] = out1;
		133	pOut[1] = out2;
		134
		135	/* Read values from source A */
		136	inA2 = pIn1[3];
		137
		138	/* Read values from source B */
		139	inB2 = pIn2[3];
		140
		141	/* out = sourceA + sourceB */
		142	out1 = inA1 + inB1;
		143
		144	/* out = sourceA + sourceB */
		145	out2 = inA2 + inB2;
		146
		147	/* Store result in destination */
		148	pOut[2] = out1;
		149
		150	/* Store result in destination */
		151	pOut[3] = out2;
		152
		153
		154	/* update pointers to process next sampels */
		155	pIn1 += 4U;
		156	pIn2 += 4U;
		157	pOut += 4U;
		158	/* Decrement the loop counter */
		159	blkCnt--;
		160	}
		161
		162	/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
		163	** No loop unrolling is used. */
		164	blkCnt = numSamples % 0x4U;
		165
		166	#else
		167
		168	/* Run the below code for Cortex-M0 */
		169
		170	/* Initialize blkCnt with number of samples */
		171	blkCnt = numSamples;
		172
		173	#endif /* #if defined (ARM_MATH_DSP) */
		174
		175	while (blkCnt > 0U)
		176	{
		177	/* C(m,n) = A(m,n) + B(m,n) */
		178	/* Add and then store the results in the destination buffer. */
		179	pOut++ = (pIn1++) + (*pIn2++);
		180
		181	/* Decrement the loop counter */
		182	blkCnt--;
		183	}
		184
		185	/* set status as ARM_MATH_SUCCESS */
		186	status = ARM_MATH_SUCCESS;
		187
		188	}
		189
		190	/* Return to application */
		191	return (status);
		192	}
		193
		194	/**
		195	* @} end of MatrixAdd group
		196	*/

Subversion Repositories dashGPS

(root)/branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c – Rev 18