mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 09:13:33 +08:00
Merge commit '4f3b17f53b120cd54112097f4217a90863013c1f' into local/mathlib
This commit is contained in:
px4dev
+4
-2
@@ -101,7 +101,9 @@ INCLUDES += $(APPDIR)
|
||||
|
||||
ASRCS ?= $(wildcard $(SRCDIR)/*.S)
|
||||
CSRCS ?= $(wildcard $(SRCDIR)/*.c)
|
||||
CHDRS ?= $(wildcard $(SRCDIR)/*.h)
|
||||
CXXSRCS ?= $(wildcard $(SRCDIR)/*.cpp)
|
||||
CXXHDRS ?= $(wildcard $(SRCDIR)/*.hpp)
|
||||
|
||||
# if APPNAME is not set, this is a library
|
||||
ifeq ($(APPNAME),)
|
||||
@@ -186,8 +188,8 @@ all: .built
|
||||
#
|
||||
depend: .depend
|
||||
.depend: $(MAKEFILE_LIST) $(SRCS)
|
||||
@$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) >Make.dep
|
||||
@$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) >>Make.dep
|
||||
@$(MKDEP) --dep-path . $(CC) -- $(CFLAGS) -- $(CSRCS) $(CHDRS) >Make.dep
|
||||
@$(MKDEP) --dep-path . $(CXX) -- $(CXXFLAGS) -- $(CXXSRCS) $(CXXHDRS) >>Make.dep
|
||||
@touch $@
|
||||
|
||||
ifneq ($(APPNAME),)
|
||||
|
||||
+28
-2
@@ -48,14 +48,40 @@ CSRCS = err.c \
|
||||
# ppm_decode.c \
|
||||
|
||||
|
||||
CXXSRCS = test/test.cpp \
|
||||
math/Vector.cpp \
|
||||
math/Vector3.cpp \
|
||||
math/EulerAngles.cpp \
|
||||
math/Quaternion.cpp \
|
||||
math/Dcm.cpp \
|
||||
math/Matrix.cpp
|
||||
|
||||
CXXHDRS = test/test.hpp \
|
||||
math/Vector.hpp \
|
||||
math/Vector3.hpp \
|
||||
math/EulerAngles.hpp \
|
||||
math/Quaternion.hpp \
|
||||
math/Dcm.hpp \
|
||||
math/Matrix.hpp
|
||||
|
||||
#
|
||||
# XXX this really should be a CONFIG_* test
|
||||
#
|
||||
ifeq ($(TARGET),px4fmu)
|
||||
INCLUDES += math/arm
|
||||
CSRCS += systemlib.c \
|
||||
pid/pid.c \
|
||||
geo/geo.c
|
||||
pid/pid.c \
|
||||
geo/geo.c
|
||||
CXXSRCS += math/arm/Vector.cpp \
|
||||
math/arm/Matrix.cpp
|
||||
CXXHDRS += math/arm/Vector.hpp \
|
||||
math/arm/Matrix.hpp
|
||||
else
|
||||
INCLUDES += math/generic
|
||||
CXXSRCS += math/generic/Vector.cpp \
|
||||
math/generic/Matrix.cpp
|
||||
CXXHDRS += math/generic/Vector.hpp \
|
||||
math/generic/Matrix.hpp
|
||||
endif
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
||||
@@ -0,0 +1,88 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 test.cpp
|
||||
*
|
||||
* Test library code
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "test.hpp"
|
||||
|
||||
bool __EXPORT equal(float a, float b, float epsilon)
|
||||
{
|
||||
float diff = fabsf(a-b);
|
||||
if (diff>epsilon)
|
||||
{
|
||||
printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
|
||||
return false;
|
||||
}
|
||||
else return true;
|
||||
}
|
||||
|
||||
void __EXPORT float2SigExp(
|
||||
const float & num,
|
||||
float & sig,
|
||||
int & exp)
|
||||
{
|
||||
if (isnan(num) || isinf(num))
|
||||
{
|
||||
sig = 0.0f;
|
||||
exp = -99;
|
||||
return;
|
||||
}
|
||||
if (fabsf(num) < 1.0e-38f)
|
||||
{
|
||||
sig = 0;
|
||||
exp = 0;
|
||||
return;
|
||||
}
|
||||
exp = log10f(fabsf(num));
|
||||
if (exp>0) {
|
||||
exp = ceil(exp);
|
||||
} else {
|
||||
exp = floor(exp);
|
||||
}
|
||||
sig = num;
|
||||
// cheap power since it is integer
|
||||
if (exp>0) {
|
||||
for (int i=0;i<abs(exp);i++) sig /= 10;
|
||||
} else {
|
||||
for (int i=0;i<abs(exp);i++) sig *= 10;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 test.hpp
|
||||
*
|
||||
* Controller library code
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <assert.h>
|
||||
#include <time.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
bool equal(float a, float b, float eps = 1e-5);
|
||||
void float2SigExp(
|
||||
const float & num,
|
||||
float & sig,
|
||||
int & exp);
|
||||
Reference in New Issue
Block a user