From aae0e562c67841f1be16e99b133758f658a3c313 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 2 Aug 2012 01:53:55 -0500 Subject: [PATCH] Create a sine lookup table that is cached in flash and make relay tuning start ot use this. --- flight/CopterControl/Makefile | 2 + flight/Libraries/math/sin_lookup.c | 99 +++++++++++++++++++ flight/Libraries/math/sin_lookup.h | 39 ++++++++ .../Modules/Stabilization/inc/relay_tuning.h | 1 - flight/Modules/Stabilization/relay_tuning.c | 44 +-------- flight/Modules/Stabilization/stabilization.c | 3 - 6 files changed, 143 insertions(+), 45 deletions(-) create mode 100644 flight/Libraries/math/sin_lookup.c create mode 100644 flight/Libraries/math/sin_lookup.h diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 721ec1896..957089c12 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -268,6 +268,7 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/math/sin_lookup.c SRC += $(FLIGHTLIB)/taskmonitor.c ## CMSIS for STM32 @@ -369,6 +370,7 @@ EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(PIOS) EXTRAINCDIRS += $(PIOSINC) EXTRAINCDIRS += $(FLIGHTLIBINC) +EXTRAINCDIRS += $(FLIGHTLIB)/math EXTRAINCDIRS += $(PIOSSTM32F10X) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) diff --git a/flight/Libraries/math/sin_lookup.c b/flight/Libraries/math/sin_lookup.c new file mode 100644 index 000000000..575f171d3 --- /dev/null +++ b/flight/Libraries/math/sin_lookup.c @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "math.h" +#include "stdint.h" + + // This is a precomputed sin lookup table over 90 degrees that + const float sin_table[] = { + 0.000000f,0.017452f,0.034899f,0.052336f,0.069756f,0.087156f,0.104528f,0.121869f,0.139173f,0.156434f, + 0.173648f,0.190809f,0.207912f,0.224951f,0.241922f,0.258819f,0.275637f,0.292372f,0.309017f,0.325568f, + 0.342020f,0.358368f,0.374607f,0.390731f,0.406737f,0.422618f,0.438371f,0.453990f,0.469472f,0.484810f, + 0.500000f,0.515038f,0.529919f,0.544639f,0.559193f,0.573576f,0.587785f,0.601815f,0.615661f,0.629320f, + 0.642788f,0.656059f,0.669131f,0.681998f,0.694658f,0.707107f,0.719340f,0.731354f,0.743145f,0.754710f, + 0.766044f,0.777146f,0.788011f,0.798636f,0.809017f,0.819152f,0.829038f,0.838671f,0.848048f,0.857167f, + 0.866025f,0.874620f,0.882948f,0.891007f,0.898794f,0.906308f,0.913545f,0.920505f,0.927184f,0.933580f, + 0.939693f,0.945519f,0.951057f,0.956305f,0.961262f,0.965926f,0.970296f,0.974370f,0.978148f,0.981627f, + 0.984808f,0.987688f,0.990268f,0.992546f,0.994522f,0.996195f,0.997564f,0.998630f,0.999391f,0.999848f + }; + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * to save flash this cheats and uses trig functions to only save + * 90 values + * @param[in] angle Angle in degrees + * @returns sin(angle) +*/ +float sin_lookup_deg(float angle) +{ + int i_ang = ((int32_t) angle) % 360; + if(i_ang > 270) // for 270 to 360 deg + return -sin_table[360 - i_ang]; + else if (i_ang > 180) // for 180 to 270 deg + return -sin_table[i_ang - 180]; + else if (i_ang > 90) // for 90 to 170 deg + return sin_table[180 - i_ang]; + else // for 0 to 90 deg + return sin_table[i_ang]; + + return 0; +} + +/** + * Get cos(angle) using the sine lookup table + * @param[in] angle Angle in degrees + * @returns cos(angle) + */ +float cos_lookup_deg(float angle) +{ + return sin_lookup_deg(angle + 90); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns sin(angle) + */ +float sin_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return sin_lookup_deg(degrees); +} + +/** + * Use the lookup table to return sine(angle) where angle is in radians + * @param[in] angle Angle in radians + * @returns cos(angle) + */ +float cos_lookup_rad(float angle) +{ + int degrees = angle * 180.0f / M_PI; + return cos_lookup_deg(degrees); +} \ No newline at end of file diff --git a/flight/Libraries/math/sin_lookup.h b/flight/Libraries/math/sin_lookup.h new file mode 100644 index 000000000..0e0edb274 --- /dev/null +++ b/flight/Libraries/math/sin_lookup.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilot Math Utilities + * @{ + * @addtogroup Sine and cosine methods that use a cached lookup table + * @{ + * + * @file sin_lookup.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Sine lookup table from flash with 1 degree resolution + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef SIN_LOOKUP_H +#define SIN_LOOKUP_H + +float sin_lookup_deg(float angle); +float cos_lookup_deg(float angle); +float sin_lookup_rad(float angle); +float cos_lookup_rad(float angle); + +#endif \ No newline at end of file diff --git a/flight/Modules/Stabilization/inc/relay_tuning.h b/flight/Modules/Stabilization/inc/relay_tuning.h index d788ae62e..9044986c5 100644 --- a/flight/Modules/Stabilization/inc/relay_tuning.h +++ b/flight/Modules/Stabilization/inc/relay_tuning.h @@ -34,7 +34,6 @@ #ifndef RELAY_TUNING_H #define RELAY_TUNING_H -int stabilization_relay_init(); int stabilization_relay_rate(float err, float *output, int axis, bool reinit); #endif \ No newline at end of file diff --git a/flight/Modules/Stabilization/relay_tuning.c b/flight/Modules/Stabilization/relay_tuning.c index ad4c7e064..16b4351fd 100644 --- a/flight/Modules/Stabilization/relay_tuning.c +++ b/flight/Modules/Stabilization/relay_tuning.c @@ -32,40 +32,15 @@ */ #include "openpilot.h" -#include "stabilization.h" -#include "stabilizationsettings.h" -#include "actuatordesired.h" -#include "ratedesired.h" #include "relaytuning.h" #include "relaytuningsettings.h" -#include "stabilizationdesired.h" -#include "attitudeactual.h" -#include "gyros.h" -#include "flightstatus.h" -#include "manualcontrol.h" // Just to get a macro -#include "CoordinateConversions.h" +#include "sin_lookup.h" //! Private variables -static float *sin_lookup; // TODO: Move this to flash static const int SIN_RESOLUTION = 180; -//! Private methods -static float sin_l(int angle); - #define MAX_AXES 3 -int stabilization_relay_init() -{ - sin_lookup = (float *) pvPortMalloc(sizeof(float) * SIN_RESOLUTION); - if (sin_lookup == NULL) - return -1; - - for(uint32_t i = 0; i < 180; i++) - sin_lookup[i] = sinf((float)i * 2 * M_PI / 360.0f); - - return 0; -} - /** * Apply a step function for the stabilization controller and monitor the * result @@ -129,8 +104,8 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) uint32_t phase = 360 * dT / relay.Period[axis]; if(phase >= 360) phase = 1; - accum_sin += sin_l(phase) * error; - accum_cos += sin_l(phase + 90) * error; + accum_sin += sin_lookup_deg(phase) * error; + accum_cos += sin_lookup_deg(phase + 90) * error; accumulated ++; // Make sure we've had enough time since last transition then check for a change in the output @@ -164,16 +139,3 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) } -/** - * Uses the lookup table to calculate sine (angle is in degrees) - * @param[in] angle in degrees - * @returns sin(angle) - */ -static float sin_l(int angle) { - angle = angle % 360; - if (angle > 180) - return - sin_lookup[angle-180]; - else - return sin_lookup[angle]; -} - diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index bc6887081..c72348aa8 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -110,9 +110,6 @@ int32_t StabilizationStart() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // This prepares this optional algorithm - stabilization_relay_init(); - // Listen for updates. // AttitudeActualConnectQueue(queue); GyrosConnectQueue(queue);