From cccb32bb49220db266135d84dc4994d43e96d6b4 Mon Sep 17 00:00:00 2001 From: "J. Camilo G.C." Date: Thu, 14 Nov 2024 22:55:33 -0500 Subject: [PATCH 1/2] sync dev (#29) * Update README.md * Create .gitattributes * Update README.md * doc fixes * fix on qfp16 fun.naming for rad<>deg conversions * minor doc fixes * doc typo fixes and others * qcrc added macros with selected algorithms * added qFIS_StoreAggregatedRegion to qfis * fix qfis doc" * added toc to some dox * minor fix to qpid autotune. Doc fixes * doc typo fixes * typo fixes to qtlisys.h doc * fix doc fcn parameters for some fcns * minor fixes and updated figures * minor doc fixes * minor doc fix * minor doc fixes * Update qfis.dox * fixed bumpless transfer on qpid. added derivative kick removal * fixed initialization * centered formulas on doc * minor changes on qpid. Doc fixes * fixed doc for qpid * qpid doc update * additive mrac anti-windup * qpid doc fixes * doc update * fix formula sintax doc qpid * minor addons to qpid * minor addon to qpid * qpid rev to v1.15 * fix figure on qfis.dox * Update mainpage.dox * added typegeneric and fvector libs * doc fix on typegenerics * minor doc fixes * doc fixes * changes on doc * minor doc fixes * minor changes * updated doc * addons to qfmathex and qtypegeneric * doc fixes y minor changes * updated readme * added qffmath : fast floating-point math * added qfis dep list * minor doc fix * doc group fix * minor doc fixes * math kernel can be selected by using QLIBS_USE_STD_MATH * added cbrt and rcbrt to qffmath * minor adjustments * readme update * fixes to qssmoother * addons to qssmoother and qffmath * doc typo fixes * minor doc typo fixes * added CMakeLists.txt and minor doc fixes" * minor fixes * minor changes * fix doc for qfis * doc typos and text redaction. added static_analisys workflow * SA fixes * added SA deps for misra * SA minor adjustments * SA rev 3 * SA rev 4 * SA rev 5 * SA rev 6 * SA rev 7 * SA rev 8 * SA rev 9 * Update stylesheet * Update doxygen_gen.yml * fix typo in qffmath.h * fix typo and math core selection * minor fixes * minor fixes * updated ver on cmakelists.txt * added qLTISys_SetInitStates. doc update * doc update * doc fixes * fixe to qltisys * Update README.md * fix grammar and typos * minor doc fixes * Create .deepsource.toml * Update stylesheet * fix doxygen version on doc * fixed overflow check on qfp * fix to wrapto180 on qfp16 * fixes to qpid, qnuma & some improvements * bump to 1.2.7 * update readem * sa rev * sa rev * sa fixes * sa rev * sa rev again * fix to qpid * move ffmathex to ffmath. Fix tuning rule for PID. added more functions to ffmath * update qfis.list * update doc stylesheet and doxygen file * update workflows * Update Doxygen file --------- Co-authored-by: camilo --- .deepsource.toml | 4 + .gitattributes | 6 + .github/workflows/doxygen_gen.yml | 14 +- .github/workflows/static-analisys.yml | 33 + .vscode/settings.json | 15 + CMakeLists.txt | 34 + README.md | 437 +--------- check/misra.json | 7 + check/misra_c2012_Rules.txt | 497 ++++++++++++ dep/qfis.list | 4 + doc/Doxyfile | 14 +- doc/mainpage.dox | 32 +- doc/qbitfield.dox | 49 +- doc/qcrc.dox | 7 +- doc/qfis.dox | 315 +++++--- doc/qfp16.dox | 54 +- doc/qltisys.dox | 61 +- doc/qpid.dox | 258 ++++-- doc/qrms.dox | 39 +- doc/qssmoother.dox | 291 ++++--- doc/qtdl.dox | 2 +- doc/stylesheet | 2 +- include/qbitfield.h | 8 +- include/qcrc.h | 230 +++++- include/qffmath.h | 719 +++++++++++++++++ include/qfis.h | 208 +++-- include/qfp16.h | 55 +- include/qltisys.h | 51 +- include/qnuma.h | 48 +- include/qpid.h | 196 +++-- include/qrms.h | 5 +- include/qssmoother.h | 95 ++- include/qtdl.h | 3 +- include/qtypegeneric.h | 246 ++++++ include/qvfloat.h | 273 +++++++ qbitfield.c | 67 +- qcrc.c | 38 +- qffmath.c | 1071 +++++++++++++++++++++++++ qfis.c | 363 +++++---- qfp16.c | 217 ++--- qltisys.c | 72 +- qnuma.c | 76 +- qpid.c | 375 ++++++--- qrms.c | 34 +- qssmoother.c | 304 +++++-- qtdl.c | 8 +- qtypegeneric.c | 374 +++++++++ qvfloat.c | 518 ++++++++++++ 48 files changed, 6249 insertions(+), 1580 deletions(-) create mode 100644 .deepsource.toml create mode 100644 .gitattributes create mode 100644 .github/workflows/static-analisys.yml create mode 100644 .vscode/settings.json create mode 100644 CMakeLists.txt create mode 100644 check/misra.json create mode 100644 check/misra_c2012_Rules.txt create mode 100644 dep/qfis.list create mode 100644 include/qffmath.h create mode 100644 include/qtypegeneric.h create mode 100644 include/qvfloat.h create mode 100644 qffmath.c create mode 100644 qtypegeneric.c create mode 100644 qvfloat.c diff --git a/.deepsource.toml b/.deepsource.toml new file mode 100644 index 0000000..d5af0ce --- /dev/null +++ b/.deepsource.toml @@ -0,0 +1,4 @@ +version = 1 + +[[analyzers]] +name = "cxx" diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..750b8b7 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,6 @@ +.gitattributes export-ignore +.gitignore export-ignore +.gitmodules export-ignore +README.md export-ignore +doc export-ignore +.github export-ignore diff --git a/.github/workflows/doxygen_gen.yml b/.github/workflows/doxygen_gen.yml index f58e5d8..5e92c81 100644 --- a/.github/workflows/doxygen_gen.yml +++ b/.github/workflows/doxygen_gen.yml @@ -11,19 +11,21 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout repository and submodules - uses: actions/checkout@v2 + uses: actions/checkout@v4 with: - submodules: recursive + submodules: recursive - name: create dirs run: mkdir docout + - name: set repo url on corner + run: sed -i 's,https://github.com/jothepro/doxygen-awesome-css,https://github.com/kmilo17pet/qlibs,g' doc/stylesheet/doxygen-custom/header.html - name: Doxygen Action - uses: mattnotmitt/doxygen-action@v1.9.2 + uses: mattnotmitt/doxygen-action@edge with: doxyfile-path: './doc/Doxyfile' - name: rm gitignore - run: rm .gitignore + run: rm .gitignore - name: Deploy doc - uses: peaceiris/actions-gh-pages@v3 + uses: peaceiris/actions-gh-pages@v4 with: github_token: ${{ secrets.GITHUB_TOKEN }} - publish_dir: ./doxyout/html + publish_dir: ./doxyout/html diff --git a/.github/workflows/static-analisys.yml b/.github/workflows/static-analisys.yml new file mode 100644 index 0000000..7a5ca0c --- /dev/null +++ b/.github/workflows/static-analisys.yml @@ -0,0 +1,33 @@ +name: Static-Analysis + +on: + workflow_dispatch: + push: + paths-ignore: + - 'dep/**' + - 'doc/**' + +jobs: + analize: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Update OS packages list + run: | + sudo apt-get update -yq + - name: Install analyzer + run: | + sudo apt-get install -y cppcheck python3 + mkdir sa_results + - name: General checks + run: cppcheck --enable=all --inline-suppr --inconclusive --std=c99 ./ -I ./include --output-file=./sa_results/general.txt --suppress=missingIncludeSystem --suppress=unmatchedSuppression:{} + - name: CERT checks + run: cppcheck --addon=cert.py --inline-suppr --inconclusive --std=c99 ./ -I ./include --output-file=./sa_results/cert.txt --suppress=missingIncludeSystem --suppress=unmatchedSuppression:{} + - name: MISRA checks + run: cppcheck --addon=./check/misra.json --inline-suppr --inconclusive --std=c99 ./ -I ./include --output-file=./sa_results/misra.txt --suppress=missingIncludeSystem --suppress=unmatchedSuppression:{} + - uses: actions/upload-artifact@v4 + with: + name: Static_Analisys_Results + path: sa_results diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..1e0c909 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,15 @@ +{ + "files.associations": { + "algorithm": "c", + "qffmath.h": "c", + "float.h": "c", + "functional": "c", + "math.h": "c", + "qfis.h": "c", + "qpid.h": "c", + "qltisys.h": "c", + "cmath": "c", + "string.h": "c", + "random": "c" + } +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..56a4dae --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,34 @@ +# qLibs CMakeLists.txt file +# Copyright (C) 2012 Eng. Juan Camilo Gómez Cadavid MSc. All Rights Reserved. +# +# To make use of the qLibs libraries on your solution just add the following +# directives to the top-level CMakeLists.txt file : +# +# add_subdirectory( ) +# target_link_libraries( ${PROJECT_NAME} qlibs ) +# +# This file is part of the qLibs distribution. + +cmake_minimum_required( VERSION 3.2 ) +project( qlibs + VERSION 1.2.8 + DESCRIPTION "A collection of useful libraries for embedded systems" + LANGUAGES C + ) + +add_library( ${PROJECT_NAME} + qbitfield.c + qcrc.c + qffmath.c + qfis.c + qfp16.c + qltisys.c + qnuma.c + qpid.c + qrms.c + qssmoother.c + qtdl.c + qtypegeneric.c + qvfloat.c + ) +target_include_directories( ${PROJECT_NAME} PUBLIC include ) diff --git a/README.md b/README.md index 691bc15..ddb2e39 100644 --- a/README.md +++ b/README.md @@ -1,435 +1,62 @@ [![Built for](https://img.shields.io/badge/built%20for-microcontrollers-lightgrey?logo=WhiteSource)](https://github.com/kmilo17pet/qTools) [![CodeFactor](https://www.codefactor.io/repository/github/kmilo17pet/qlibs/badge)](https://www.codefactor.io/repository/github/kmilo17pet/qlibs) [![Codacy Badge](https://app.codacy.com/project/badge/Grade/14d566939d2e4d4181088cc1c6666fa3)](https://www.codacy.com/gh/kmilo17pet/qTools/dashboard?utm_source=github.com&utm_medium=referral&utm_content=kmilo17pet/qTools&utm_campaign=Badge_Grade) -[![CodeInspectorScore](https://api.codiga.io/project/27197/score/svg)](https://app.codiga.io/project/27197/dashboard) -[![CodeInspectorGrade](https://api.codiga.io/project/27197/status/svg)](https://app.codiga.io/project/27197/dashboard) -[![Softacheck](https://softacheck.com/app/repository/kmilo17pet/qlibs/badge)](https://softacheck.com/app/repository/kmilo17pet/qlibs/issues) -[![Documentation](https://softacheck.com/app/repository/kmilo17pet/qlibs/documentation/badge)](https://softacheck.com/app/docs/kmilo17pet/qlibs/) +[![DeepSource](https://app.deepsource.com/gh/kmilo17pet/qlibs.svg/?label=active+issues&show_trend=true&token=sM8bC9gGb5PdO1ronKdoDhUh)](https://app.deepsource.com/gh/kmilo17pet/qlibs/) +[![documentation](https://github.com/kmilo17pet/qlibs/actions/workflows/doxygen_gen.yml/badge.svg)](https://kmilo17pet.github.io/qlibs/) +[![GitHub release (latest by date)](https://img.shields.io/github/v/release/kmilo17pet/qlibs?logo=webpack)](https://github.com/kmilo17pet/qlibs/releases) [![MISRAC2012](https://img.shields.io/badge/MISRAC2012-Compliant-blue.svg?logo=c)](https://en.wikipedia.org/wiki/MISRA_C) [![CERT](https://img.shields.io/badge/CERT-Compliant-blue.svg?logo=c)](https://wiki.sei.cmu.edu/confluence/display/seccode/SEI+CERT+Coding+Standards) [![C Standard](https://img.shields.io/badge/STD-C99-green.svg?logo=c)](https://en.wikipedia.org/wiki/C99) [![Maintenance](https://img.shields.io/badge/Maintained%3F-yes-green.svg)](https://github.com/kmilo17pet/qTools/graphs/commit-activity) -[![License](https://img.shields.io/github/license/kmilo17pet/qTools)](https://github.com/kmilo17pet/qTools/blob/main/LICENSE) - +[![License](https://img.shields.io/github/license/kmilo17pet/qTools)](https://github.com/kmilo17pet/qTools/blob/main/LICENSE) +[![Hits](https://hits.seeyoufarm.com/api/count/incr/badge.svg?url=https%3A%2F%2Fgithub.com%2Fkmilo17pet%2Fqlibs&count_bg=%2379C83D&title_bg=%23555555&icon=cliqz.svg&icon_color=%23E7E7E7&title=hits&edge_flat=false)](https://github.com/kmilo17pet/qlibs) ![qlibs_logo_small](https://user-images.githubusercontent.com/11412210/192115666-a5e3b615-b635-47bb-a30a-fb9107a53b48.png) # qlibs : A collection of useful libraries for embedded systems -* Read the API Reference [here](https://kmilo17pet.github.io/qlibs/) -
qFP16 : Q16.16 Fixed-point math - - - Basic operations - - Trigonometric functions - - Exponential functions -
+* Download the latest release [here](https://github.com/kmilo17pet/qlibs/releases) +* Documentation and API Reference [here](https://kmilo17pet.github.io/qlibs/) -
qSSmoother : Filters to smooth noisy signals - - - `LPF1`: _Low Pass Filter Order 1_ - - `LPF2`: _Low Pass Filter Order 2_ - - `MWM1`: _Moving Window Median O(n)_ - - `MWM2`: _Moving Window Median O(1): With TDL(works efficient for large windows)_ - - `MOR1`: _Moving Outlier Removal O(n)_ - - `MOR2`: _Moving Outlier Removal O(1): With TDL(works efficient for large windows)_ - - `GMWF`: _Gaussian filter_ - - `KLMN`: _Scalar Kalman filter_ - - `EXPW`: _Exponential weighting filter_ -
-
qPID : Closed Loop PID Controller - +Below is the list of the modules provided and their features: + +- qSSmoother : Filters to smooth noisy signals + - `LPF1`: _Low Pass Filter Order 1_ + - `LPF2`: _Low Pass Filter Order 2_ + - `MWM1`: _Moving Window Median O(n)_ + - `MWM2`: _Moving Window Median O(1): With TDL(works efficient for large windows)_ + - `MOR1`: _Moving Outlier Removal O(n)_ + - `MOR2`: _Moving Outlier Removal O(1): With TDL(works efficient for large windows)_ + - `GMWF`: _Gaussian filter_ + - `KLMN`: _Scalar Kalman filter_ + - `EXPW`: _Exponential weighting filter_ +- qPID : Closed Loop PID Controller - Derivative filter - Anti-Windup - Tracking Mode - - Auto-tunning + - Auto-tunning - Additive MRAC -
- -
qLTISys : Recursive LTI systems evaluation by transfer functions - +- qLTISys : Recursive LTI systems evaluation by transfer functions - Continuous - Discrete -
- -
qFIS : Fuzzy Inference System - +- qFIS : Fuzzy Inference System Engine - Mamdani - Sugeno - Tsukamoto -
- -
qCRC : Generic Cyclic Redundancy Check (CRC) calculator - +- qFP16 : Q16.16 Fixed-point math + - Basic operations + - Trigonometric functions + - Exponential functions +- qCRC : Generic Cyclic Redundancy Check (CRC) calculator - CRC8 - CRC16 - CRC32 -
- - qBitField: A bit-field manipulation library -- qTDL : Tapped Delay Line in O(1). +- qTDL : Tapped Delay Line in O(1). - qRMS : Recursive Root Mean Square(RMS) calculation of a signal. +- Type-generic array operations +- Single precision floating-point vector(1D-Array) operations +- Fast single precision floating-point math -## Draft examples - -### Working with a bitfield -```c -#include -#include -#include -#include "qbitfield.h" - -int main( int argc, char *argv[] ) -{ - qBitField_t vPort; /*create the bitfield instance*/ - uint8_t vPortArea[ QBITFIELD_SIZE(48) ] = { 0 }; /*Create the bitfield storage area to hold 48bits*/ - uint16_t rWord; - - qBitField_Setup( &vPort, vPortArea, sizeof(vPortArea) ); - qBitField_ClearAll( &vPort); - /*we are going to write the following value in the bitfield = 0x8177AA55FF88*/ - qBitField_WriteUINTn( &vPort, 0, 0xAA55FF88, 32 ); /*write the first 32 bits*/ - qBitField_WriteUINTn( &vPort, 32, 0x77, 8 ); /*write one of the last two bytes*/ - qBitField_WriteBit( &vPort, 47, 1 ); /*write the last bit of the last byte*/ - qBitField_WriteBit( &vPort, 40, 1 ); /*write the first bit of the last byte*/ - rWord = (uint16_t)qBitField_ReadUINTn( &vPort, 20, 16 ); /*read a word at offset 24*/ - printf("%02X %02X %02X %02X %02X %02X\r\n", vPortArea[0], vPortArea[1], vPortArea[2], vPortArea[3], vPortArea[4], vPortArea[5]); - return EXIT_SUCCESS; -} -``` - - -### A simple fixed-point calculation - -![ec1](https://latex.codecogs.com/svg.latex?\Large&space;x=\frac{-b+\sqrt{b^2-4ac}}{2a}) - - -```c -#include -#include -#include "qfp16.h" - -int main( int argc, char *argv[] ) -{ - qFP16_t a = qFP16_Constant( 1.5f ); - qFP16_t b = qFP16_Constant( 5.2f ); - qFP16_t c = qFP16_Constant( 4.0f ); - qFP16_t tmp; - char ans[ 10 ]; - - tmp = qFP16_Mul( qFP16_IntToFP( 4 ), qFP16_Mul( a, c ) ); - tmp = qFP16_Add( -b, qFP16_Sqrt( qFP16_Sub( qFP16_Mul( b, b ), tmp ) ) ); - tmp = qFP16_Div( tmp, qFP16_Mul( qFP16_IntToFP( 2 ), a ) ); - printf( " result = %s \r\n" , qFP16_FPToA( tmp, ans, 4 ) ); - return EXIT_SUCCESS; -} -``` - -### Simulating a 3rd-order continuous transfer function - -![ec2](https://latex.codecogs.com/svg.latex?\Large&space;G_p(s)=\frac{2s^2+3s+6}{s^3+6s^2+11s+16}) - - -```c -#include -#include -#include "bsp.h" -#include "qltisys.h" - -#define SYS_ORDER ( 3 ) -const TickType_t dt = 50; /*50mS time-step*/ -qLTISys_t system; -float num[ SYS_ORDER+1 ] = { 0.0f, 2.0f, 3.0f, 6.0f }; -float den[ SYS_ORDER+1 ] = { 1.0f, 6.0f, 11.0f, 16.0f }; -qLTISys_ContinuosX_t x[ SYS_ORDER ] = { { { 0.0f } }, { { 0.0f } },{ { 0.0f } } }; - -void xTaskSystemSimulate( void *arg ) -{ - qLTISys_Setup( &system, num, den, x, 0, SYS_ORDER+1, (float)dt/1000.0f ) ); - float ut, yt; - for( ;; ) { - ut = BSP_InputGet(); - yt = qLTISys_Excite( &system, ut ); - vTaskDelay( dt / portTICK_RATE_MS) ; - printf( "u(t) = %0.2f y(t) = %0.2f \r\n", ut, yt ); - } -} -``` - -### Simulating a 2nd-order discrete transfer function - -![ec2](https://latex.codecogs.com/svg.latex?\Large&space;G_p(z^{-1})=\frac{0.1+0.2z^{-1}+0.3z^{-2}}{1-0.85z^{-1}+0.02z^{-2}}) - - -```c -#include -#include -#include "bsp.h" -#include "qltisys.h" - -#define NB ( 3 ) -#define NA ( 2 ) -const TickType_t Ts = 100; /*100mS sample time*/ -qLTISys_t system; -float num[ NB ] = { 0.1f, 0.2f, 0.3f }; -float den[ NA+1 ] = { 1.0f, -0.85f, 0.02f }; -qLTISys_DiscreteX_t xk[ NB ] = { 0.0f, 0.0f, 0.0f }; - -void xTaskSystemSimulate( void *arg ) -{ - qLTISys_Setup( &system, num, den, xk, NB, NA, QLTISYS_DISCRETE ); - float uk, yk; - for( ;; ) { - uk = BSP_InputGet(); - yk = qLTISys_Excite( &system, uk ); - vTaskDelay( Ts / portTICK_RATE_MS) ; - printf( "u(k) = %0.2f y(k) = %0.2f \r\n", uk, yk ); - } -} -``` - -### A speed control using the PID algorithm - -```c -#include -#include -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "bsp.h" -#include "qpid.h" - -const TickType_t dt = 50; /*50mS time-step*/ -void xTaskPIDspeedControl( void *arg ); - -void xTaskPIDspeedControl( void *arg ) -{ - qPID_controller_t *controller = (qPID_controller_t *)arg; - float processMeasurement; - float controlOutput; - float setpoint = 300.0f; /*desired speed 200rpm*/; - - for ( ;; ) { - processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) ); - controlOutput = qPID_Control( controller, setpoint, processMeasurement ); - BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) ); - vTaskDelay( dt / portTICK_RATE_MS) ; - } -} - -int main( int argc, char *argv[] ) -{ - qPID_controller_t speedControl; - int ret; - - BSP_SystemInit( ); - ret = qPID_Setup( &speedControl, 1, 0.1, 0, (float)dt/1000.0f ); - if ( 0 == ret ) { - puts( "ERROR: Cant configure PID controller" ); - } - qPID_SetSaturation( &speedControl, 0.0f, 100.0f ); - /* Create the task that handles the speed control. */ - xTaskCreate( xTaskPIDspeedControl, "speedControl", 1024, &speedControl, configMAX_PRIORITIES - 1 ,NULL ); - vTaskStartScheduler(); - for( ;; ); - return EXIT_SUCCESS; -} -``` - -### Smoothing a noisy signal by using a gaussian filter -```c -#include -#include -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "bsp.h" -#include "qssmoother.h" -#include "AppSignalProcessing.h" - -#define SMOOTHER_WINDOW_SIZE ( 10 ) -#define SMOOTHER_SAMPLE_TIME ( 100 ) - -void xTaskSignalProcessing( void *arg ) -{ - qSSmoother_GMWF_t *smoother = (qSSmoother_GMWF_t *)arg; - float noisySignal; - float smoothedSignal; - for ( ;; ) { - noisySignal = BSP_ScaletoSomething( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) ); - smoothedSignal = qSSmoother_Perform( smoother, noisySignal ); - AppSignalProcess( smoothedSignal ); - vTaskDelay( SMOOTHER_SAMPLE_TIME / portTICK_RATE_MS ) ; - } -} - -int main( int argc, char *argv[] ) -{ - qSSmoother_GMWF_t smoother; - float win_kernel[ 2*SMOOTHER_WINDOW_SIZE ]; /*storage for the window and the kernel*/ - float params[ 2 ] = { 0.5f, SMOOTHER_WINDOW_SIZE/2.0f }; /*sigma and offset*/ - int ret; - - BSP_SystemInit( ); - ret = qSSmoother_Setup( &smoother, QSSMOOTHER_TYPE_GMWF, params, win_kernel, sizeof(win_kernel)/sizeof(win_kernel[0]) ); - if ( 0 == ret ) { - puts( "ERROR: Cant configure the smoother filter" ); - } - xTaskCreate( xTaskSignalProcessing, "signalProcessing", 512, &smoother, 2, NULL ); - vTaskStartScheduler(); - for( ;; ); - return EXIT_SUCCESS; -} -``` - - -### A Mamdani Fuzzy Inference System example -FlexNav system taken from here: https://www.researchgate.net/publication/3955309_FLEXnav_fuzzy_logic_expert_rule-based_position_estimation_for_mobile_robots_on_rugged_terrain - -flexnav_fis.c - -```c -/* Fuzzy inference system generated by qfiscgen.m for the qFIS engine*/ -/* Generated : 23-Sep-2022 15:26:35 */ -/* Note: Obtain the qFIS engine from https://github.com/kmilo17pet/qlibs */ -#include "flexnav_fis.h" -#include "qfis.h" - -/* FIS Object */ -static qFIS_t flexnav; -/* I/O Fuzzy Objects */ -static qFIS_Input_t flexnav_inputs[ 4 ]; -static qFIS_Output_t flexnav_outputs[ 2 ]; -/* I/O Membership Objects */ -static qFIS_MF_t MFin[ 12 ], MFout[ 6 ]; -/* I/O Names */ -enum { wt, dax, day, ae }; -enum { phit, thetat }; -/* I/O Membership functions tags */ -enum { wt_SLOW, wt_MED, wt_FAST, dax_LOW, dax_MED, dax_HIGH, day_LOW, day_MED, day_HIGH, ae_LOW, ae_MED, ae_HIGH }; -enum { phit_GYRO, phit_BOTH, phit_ACCEL, thetat_GYRO, thetat_BOTH, thetat_ACCEL }; -/* Rules of the inference system*/ -static const qFIS_Rules_t rules[] = { - QFIS_RULES_BEGIN - IF wt IS_NOT wt_SLOW THEN phit IS phit_GYRO AND thetat IS thetat_GYRO END - IF dax IS dax_HIGH THEN thetat IS thetat_GYRO END - IF day IS day_HIGH THEN thetat IS thetat_GYRO END - IF ae IS ae_HIGH THEN phit IS phit_GYRO AND thetat IS thetat_GYRO END - IF wt IS wt_SLOW AND dax IS dax_LOW AND ae IS ae_LOW THEN phit IS phit_ACCEL END - IF wt IS wt_SLOW AND day IS day_LOW AND ae IS ae_LOW THEN thetat IS thetat_ACCEL END - IF wt IS wt_SLOW AND dax IS dax_LOW AND ae IS ae_MED THEN phit IS phit_BOTH END - IF wt IS wt_SLOW AND day IS day_LOW AND ae IS ae_MED THEN thetat IS thetat_BOTH END - IF wt IS wt_SLOW AND dax IS dax_MED AND ae IS ae_LOW THEN phit IS phit_BOTH END - IF wt IS wt_SLOW AND day IS day_MED AND ae IS ae_LOW THEN thetat IS thetat_BOTH END - IF wt IS wt_MED AND dax IS dax_LOW AND ae IS ae_LOW THEN phit IS phit_BOTH END - IF wt IS wt_MED AND day IS day_LOW AND ae IS ae_LOW THEN thetat IS thetat_BOTH END - IF wt IS wt_MED AND dax IS_NOT dax_LOW THEN phit IS phit_GYRO END - IF wt IS wt_MED AND day IS_NOT day_LOW THEN thetat IS thetat_GYRO END - IF wt IS wt_MED AND ae IS_NOT ae_LOW THEN phit IS phit_GYRO AND thetat IS thetat_GYRO END - QFIS_RULES_END -}; -/*Rule strengths*/ -float rStrength[ 15 ] = { 0.0f}; - -/*Parameters of the membership functions*/ -static const float wt_SLOW_p[] = { -0.2000f, 0.0000f, 0.2000f }; -static const float wt_MED_p[] = { 0.1000f, 0.2500f, 0.4000f }; -static const float wt_FAST_p[] = { 0.3000f, 0.5000f, 0.7000f }; -static const float dax_LOW_p[] = { -1.0000f, 0.0000f, 2.0000f }; -static const float dax_MED_p[] = { 1.0000f, 2.5000f, 4.0000f }; -static const float dax_HIGH_p[] = { 3.0000f, 5.0000f, 7.0000f }; -static const float day_LOW_p[] = { -2.0000f, 0.0000f, 2.0000f }; -static const float day_MED_p[] = { 1.0000f, 2.5000f, 4.0000f }; -static const float day_HIGH_p[] = { 3.0000f, 5.0000f, 7.0000f }; -static const float ae_LOW_p[] = { -8.0000f, 0.0000f, 8.0000f }; -static const float ae_MED_p[] = { 5.0000f, 10.0000f, 15.0000f }; -static const float ae_HIGH_p[] = { 12.0000f, 20.0000f, 28.0000f }; -static const float phit_GYRO_p[] = { -0.4000f, 0.0000f, 0.4000f }; -static const float phit_BOTH_p[] = { 0.2000f, 0.5000f, 0.8000f }; -static const float phit_ACCEL_p[] = { 0.6000f, 1.0000f, 1.4000f }; -static const float thetat_GYRO_p[] = { -0.4000f, 0.0000f, 0.4000f }; -static const float thetat_BOTH_p[] = { 0.2000f, 0.5000f, 0.8000f }; -static const float thetat_ACCEL_p[] = { 0.6000f, 1.0000f, 1.4000f }; - -void flexnav_init( void ){ - /* Add inputs */ - qFIS_InputSetup( flexnav_inputs, wt, 0.0000f, 0.5000f ); - qFIS_InputSetup( flexnav_inputs, dax, 0.0000f, 5.0000f ); - qFIS_InputSetup( flexnav_inputs, day, 0.0000f, 5.0000f ); - qFIS_InputSetup( flexnav_inputs, ae, 0.0000f, 20.0000f ); - qFIS_OutputSetup( flexnav_outputs, phit, 0.0000f, 1.0000f ); - qFIS_OutputSetup( flexnav_outputs, thetat, 0.0000f, 1.0000f ); - /* Add membership functions to the inputs */ - qFIS_SetMF( MFin, wt, wt_SLOW, trimf, NULL, wt_SLOW_p, 1.0f ); - qFIS_SetMF( MFin, wt, wt_MED, trimf, NULL, wt_MED_p, 1.0f ); - qFIS_SetMF( MFin, wt, wt_FAST, trimf, NULL, wt_FAST_p, 1.0f ); - qFIS_SetMF( MFin, dax, dax_LOW, trimf, NULL, dax_LOW_p, 1.0f ); - qFIS_SetMF( MFin, dax, dax_MED, trimf, NULL, dax_MED_p, 1.0f ); - qFIS_SetMF( MFin, dax, dax_HIGH, trimf, NULL, dax_HIGH_p, 1.0f ); - qFIS_SetMF( MFin, day, day_LOW, trimf, NULL, day_LOW_p, 1.0f ); - qFIS_SetMF( MFin, day, day_MED, trimf, NULL, day_MED_p, 1.0f ); - qFIS_SetMF( MFin, day, day_HIGH, trimf, NULL, day_HIGH_p, 1.0f ); - qFIS_SetMF( MFin, ae, ae_LOW, trimf, NULL, ae_LOW_p, 1.0f ); - qFIS_SetMF( MFin, ae, ae_MED, trimf, NULL, ae_MED_p, 1.0f ); - qFIS_SetMF( MFin, ae, ae_HIGH, trimf, NULL, ae_HIGH_p, 1.0f ); - /* Add membership functions to the outputs */ - qFIS_SetMF( MFout, phit, phit_GYRO, trimf, NULL, phit_GYRO_p, 1.0f ); - qFIS_SetMF( MFout, phit, phit_BOTH, trimf, NULL, phit_BOTH_p, 1.0f ); - qFIS_SetMF( MFout, phit, phit_ACCEL, trimf, NULL, phit_ACCEL_p, 1.0f ); - qFIS_SetMF( MFout, thetat, thetat_GYRO, trimf, NULL, thetat_GYRO_p, 1.0f ); - qFIS_SetMF( MFout, thetat, thetat_BOTH, trimf, NULL, thetat_BOTH_p, 1.0f ); - qFIS_SetMF( MFout, thetat, thetat_ACCEL, trimf, NULL, thetat_ACCEL_p, 1.0f ); - - /* Configure the Inference System */ - qFIS_Setup( &flexnav, Mamdani, - flexnav_inputs, sizeof(flexnav_inputs), - flexnav_outputs, sizeof(flexnav_outputs), - MFin, sizeof(MFin), MFout, sizeof(MFout), - rules, rStrength, 15u ); -} - -void flexnav_run( float *inputs, float *outputs ) { - /* Set the crips inputs */ - qFIS_SetInput( flexnav_inputs, wt, inputs[ wt ] ); - qFIS_SetInput( flexnav_inputs, dax, inputs[ dax ] ); - qFIS_SetInput( flexnav_inputs, day, inputs[ day ] ); - qFIS_SetInput( flexnav_inputs, ae, inputs[ ae ] ); - - qFIS_Fuzzify( &flexnav ); - if ( qFIS_Inference( &flexnav ) > 0 ) { - qFIS_DeFuzzify( &flexnav ); - } - else { - /* Error! */ - } - - /* Get the crips outputs */ - outputs[ phit ] = qFIS_GetOutput( flexnav_outputs, phit ); - outputs[ thetat ] = qFIS_GetOutput( flexnav_outputs, thetat ); -} - -``` - -flexnav_fis.h - -```c -/* Fuzzy inference system generated by qfiscgen.m for the qFIS engine*/ -/* Generated : 23-Sep-2022 15:26:35 */ -/* Note: Obtain the qFIS engine from https://github.com/kmilo17pet/qlibs */ -#ifndef FLEXNAV_FIS_H -#define FLEXNAV_FIS_H - -#ifdef __cplusplus - extern "C" { -#endif - - void flexnav_init( void ); - void flexnav_run( float *inputs, float *outputs ); - -#ifdef __cplusplus - } -#endif - -``` -### Generate C code from the MATLAB Fuzzy Logic Toolbox -Download the `qfiscgen` tool from -[![View qfiscgen ( Fuzzy C-code generator for embedded systems) on File Exchange](https://www.mathworks.com/matlabcentral/images/matlab-file-exchange.svg)](https://la.mathworks.com/matlabcentral/fileexchange/117465-qfiscgen-fuzzy-c-code-generator-for-embedded-systems) diff --git a/check/misra.json b/check/misra.json new file mode 100644 index 0000000..7603ec5 --- /dev/null +++ b/check/misra.json @@ -0,0 +1,7 @@ +{ + "script": "misra.py", + "args": [ + "--rule-texts=${GITHUB_WORKSPACE}/check/misra_c2012_Rules.txt", + "--suppress-rules 12.3,13.3,17.8,18.4,20.10,21.1" + ] +} \ No newline at end of file diff --git a/check/misra_c2012_Rules.txt b/check/misra_c2012_Rules.txt new file mode 100644 index 0000000..aa5ec85 --- /dev/null +++ b/check/misra_c2012_Rules.txt @@ -0,0 +1,497 @@ +Appendix A Summary of guidelines + + +Rule 1.1 Required +The program shall contain no violations of the standard C syntax and constraints, and shall not exceed the implementation’s translation limits + +Rule 1.2 Advisory +Language extensions should not be used + +Rule 1.3 Required +There shall be no occurrence of undefined or critical unspecified behaviour + +Rule 2.1 Required +A project shall not contain unreachable code + +Rule 2.2 Required +There shall be no dead code + +Rule 2.3 Advisory +A project should not contain unused type declarations + +Rule 2.4 Advisory +A project should not contain unused tag declarations + +Rule 2.5 Advisory +A project should not contain unused macro declarations + +Rule 2.6 Advisory +A function should not contain unused label declarations + +Rule 2.7 Advisory +There should be no unused parameters in functions + +Rule 3.1 Required +The character sequences /* and // shall not be used within a comment + +Rule 3.2 Required +Line-splicing shall not be used in // comments + +Rule 4.1 Required +Octal and hexadecimal escape sequences shall be terminated + +Rule 4.2 Advisory +Trigraphs should not be used + +Dir 4.3 Required +Assembly language shall be encapsulated and isolated + +Dir 4.4 Advisory +Sections of code should not be “commented out” + +Dir 4.5 Advisory +Identifiers in the same name space with overlapping visibility should be typographically unambiguous + +Dir 4.6 Advisory +typedefs that indicate size and signedness should be used in place of the basic numerical types + +Dir 4.7 Required +If a function returns error information, then that error information shall be tested + +Dir 4.8 Advisory +If a pointer to a structure or union is never dereferenced within a translation unit, then the implementation of the object should be hidden + +Dir 4.9 Advisory +A function should be used in preference to a function-like macro where they are interchangeable + +Dir 4.10 Required +Precautions shall be taken in order to prevent the contents of a header file being included more than once + +Dir 4.11 Required +The validity of values passed to library functions shall be checked + +Dir 4.12 Required +Dynamic memory allocation shall not be used + +Dir 4.13 Advisory +Functions which are designed to provide operations on a resource should be called in an appropriate sequence + +Rule 5.1 Required +External identifiers shall be distinct + +Rule 5.2 Required +Identifiers declared in the same scope and name space shall be distinct + +Rule 5.3 Required +An identifier declared in an inner scope shall not hide an identifier declared in an outer scope + +Rule 5.4 Required +Macro identifiers shall be distinct + +Rule 5.5 Required +Identifiers shall be distinct from macro names + +Rule 5.6 Required +A typedef name shall be a unique identifier + +Rule 5.7 Required +A tag name shall be a unique identifier + +Rule 5.8 Required +Identifiers that define objects or functions with external linkage shall be unique + +Rule 5.9 Advisory +Identifiers that define objects or functions with internal linkage should be unique + +Rule 6.1 Required +Bit-fields shall only be declared with an appropriate type + +Rule 6.2 Required +Single-bit named bit fields shall not be of a signed type + +Rule 7.1 Required +Octal constants shall not be used + +Rule 7.2 Required +A “u” or “U” suffix shall be applied to all integer constants that are represented in an unsigned type + +Rule 7.3 Required +The lowercase character “l” shall not be used in a literal suffix + +Rule 7.4 Required +A string literal shall not be assigned to an object unless the object’s type is “pointer to const-qualified char” + +Rule 8.1 Required +Types shall be explicitly specified + +Rule 8.2 Required +Function types shall be in prototype form with named parameters + +Rule 8.3 Required +All declarations of an object or function shall use the same names and type qualifiers + +Rule 8.4 Required +A compatible declaration shall be visible when an object or function with external linkage is defined + +Rule 8.5 Required +An external object or function shall be declared once in one and only one file + +Rule 8.6 Required +An identifier with external linkage shall have exactly one external definition + +Rule 8.7 Advisory +Functions and objects should not be defined with external linkage if they are referenced in only one translation unit + +Rule 8.8 Required +The static storage class specifier shall be used in all declarations of objects and functions that have internal linkage + +Rule 8.9 Advisory +An object should be defined at block scope if its identifier only appears in a single function + +Rule 8.10 Required +An inline function shall be declared with the static storage class + +Rule 8.11 Advisory +When an array with external linkage is declared, its size should be explicitly specified + +Rule 8.13 Advisory +A pointer should point to a const-qualified type whenever possible + +Rule 8.14 Required +The restrict type qualifier shall not be used + +Rule 9.1 Mandatory +The value of an object with automatic storage duration shall not be read before it has been set + +Rule 9.2 Required +The initializer for an aggregate or union shall be enclosed in braces + +Rule 9.3 Required +Arrays shall not be partially initialized + +Rule 9.4 Required +An element of an object shall not be initialized more than once + +Rule 9.5 Required +Where designated initializers are used to initialize an array object the size of the array shall be specified explicitly + +Rule 10.1 Required +Operands shall not be of an inappropriate essential type + +Rule 10.2 Required +Expressions of essentially character type shall not be used inappropriately in addition and subtraction operations + +Rule 10.3 Required +The value of an expression shall not be assigned to an object with a narrower essential type or of a different essential type category + +Rule 10.4 Required +Both operands of an operator in which the usual arithmetic conversions are performed shall have the same essential type category + +Rule 10.5 Advisory +The value of an expression should not be cast to an inappropriate essential type + +Rule 10.6 Required +The value of a composite expression shall not be assigned to an object with wider essential type + +Rule 10.7 Required +If a composite expression is used as one operand of an operator in which the usual arithmetic conversions are performed then the other operand shall not have wider essential type + +Rule 10.8 Required +The value of a composite expression shall not be cast to a different essential type category or a wider essential type + +Rule 11.1 Required +Conversions shall not be performed between a pointer to a function and any other type + +Rule 11.2 Required +Conversions shall not be performed between a pointer to an incomplete type and any other type + +Rule 11.3 Required +A cast shall not be performed between a pointer to object type and a pointer to a different object type + +Rule 11.4 Advisory +A conversion should not be performed between a pointer to object and an integer type + +Rule 11.5 Advisory +A conversion should not be performed from pointer to void into pointer to object + +Rule 11.6 Required +A cast shall not be performed between pointer to void and an arithmetic type + +Rule 11.7 Required +A cast shall not be performed between pointer to object and a non-integer arithmetic type + +Rule 11.8 Required +A cast shall not remove any const or volatile qualification from the type pointed to by a pointer + +Rule 11.9 Required +The macro NULL shall be the only permitted form of integer null pointer constant + +Rule 12.1 Advisory +The precedence of operators within expressions should be made explicit + +Rule 12.2 Required +The right hand operand of a shift operator shall lie in the range zero to one less than the width in bits of the essential type of the left hand operand + +Rule 12.3 Advisory +The comma operator should not be used + +Rule 12.4 Advisory +Evaluation of constant expressions should not lead to unsigned integer wrap-around + +Rule 12.5 Mandatory +The sizeof operator shall not have an operand which is a function parameter declared as “array of type” + +Rule 13.1 Required +Initializer lists shall not contain persistent side effects + +Rule 13.2 Required +The value of an expression and its persistent side effects shall be the same under all permitted evaluation orders + +Rule 13.3 Advisory +A full expression containing an increment (++) or decrement (--) operator should have no other potential side effects other than that caused by the increment or decrement operator + +Rule 13.4 Advisory +The result of an assignment operator should not be used + +Rule 13.5 Required +The right hand operand of a logical && or || operator shall not contain persistent side effects + +Rule 13.6 Mandatory +The operand of the sizeof operator shall not contain any expression which has potential side effects + +Rule 14.1 Required +A loop counter shall not have essentially floating type + +Rule 14.2 Required +A for loop shall be well-formed + +Rule 14.3 Required +Controlling expressions shall not be invariant + +Rule 14.4 Required +The controlling expression of an if statement and the controlling expression of an iteration-statement shall have essentially Boolean type + +Rule 15.1 Advisory +The goto statement should not be used + +Rule 15.2 Required +The goto statement shall jump to a label declared later in the same function + +Rule 15.3 Required +Any label referenced by a goto statement shall be declared in the same block, or in any block enclosing the goto statement + +Rule 15.4 Advisory +There should be no more than one break or goto statement used to terminate any iteration statement + +Rule 15.5 Advisory +A function should have a single point of exit at the end + +Rule 15.6 Required +The body of an iteration-statement or a selection-statement shall be a compound-statement + +Rule 15.7 Required +All if … else if constructs shall be terminated with an else statement + +Rule 16.1 Required +All switch statements shall be well-formed + +Rule 16.2 Required +A switch label shall only be used when the most closely-enclosing compound statement is the body of a switch statement + +Rule 16.3 Required +An unconditional break statement shall terminate every switch-clause + +Rule 16.4 Required +Every switch statement shall have a default label + +Rule 16.5 Required +A default label shall appear as either the first or the last switch label of a switch statement + +Rule 16.6 Required +Every switch statement shall have at least two switch-clauses + +Rule 16.7 Required +A switch-expression shall not have essentially Boolean type + +Rule 17.1 Required +The features of shall not be used + +Rule 17.2 Required +Functions shall not call themselves, either directly or indirectly + +Rule 17.3 Mandatory +A function shall not be declared implicitly + +Rule 17.4 Mandatory +All exit paths from a function with non-void return type shall have an explicit return statement with an expression + +Rule 17.5 Advisory +The function argument corresponding to a parameter declared to have an array type shall have an appropriate number of elements + +Rule 17.6 Mandatory +The declaration of an array parameter shall not contain the static keyword between the [ ] + +Rule 17.7 Required +The value returned by a function having non-void return type shall be used + +Rule 17.8 Advisory +A function parameter should not be modified + +Rule 18.1 Required +A pointer resulting from arithmetic on a pointer operand shall address an element of the same array as that pointer operand + +Rule 18.2 Required +Subtraction between pointers shall only be applied to pointers that address elements of the same array + +Rule 18.3 Required +The relational operators >, >=, < and <= shall not be applied to objects of pointer type except where they point into the same object + +Rule 18.4 Advisory +The +, -, += and -= operators should not be applied to an expression of pointer type + +Rule 18.5 Advisory +Declarations should contain no more than two levels of pointer nesting + +Rule 18.6 Required +The address of an object with automatic storage shall not be copied to another object that persists after the first object has ceased to exist + +Rule 18.7 Required +Flexible array members shall not be declared + +Rule 18.8 Required +Variable-length array types shall not be used + +Rule 19.1 Mandatory +An object shall not be assigned or copied to an overlapping object + +Rule 19.2 Advisory +The union keyword should not be used + +Rule 20.1 Advisory +#include directives should only be preceded by preprocessor directives or comments + +Rule 20.2 Required +The ', " or \ characters and the /* or // character sequences shall not occur in a header file name + +Rule 20.3 Required +The #include directive shall be followed by either a or "filename" sequence + +Rule 20.4 Required +A macro shall not be defined with the same name as a keyword + +Rule 20.5 Advisory +#undef should not be used + +Rule 20.7 Required +Expressions resulting from the expansion of macro parameters shall be enclosed in parentheses + +Rule 20.8 Required +The controlling expression of a #if or #elif preprocessing directive shall evaluate to 0 or 1 + +Rule 20.9 Required +All identifiers used in the controlling expression of #if or #elif preprocessing directives shall be #define’d before evaluation + +Rule 20.10 Advisory +The # and ## preprocessor operators should not be used + +Rule 20.11 Required +A macro parameter immediately following a # operator shall not immediately be followed by a ## operator + +Rule 20.12 Required +A macro parameter used as an operand to the # or ## operators, which is itself subject to further macro replacement, shall only be used as an operand to these operators + +Rule 20.13 Required +A line whose first token is # shall be a valid preprocessing directive + +Rule 20.14 Required +All #else, #elif and #endif preprocessor directives shall reside in the same file as the #if, #ifdef or #ifndef directive to which they are related + +Rule 21.1 Required +#define and #undef shall not be used on a reserved identifier or reserved macro name + +Rule 21.2 Required +A reserved identifier or macro name shall not be declared + +Rule 21.3 Required +The memory allocation and deallocation functions of shall not be used + +Rule 21.4 Required +The standard header file shall not be used + +Rule 21.5 Required +The standard header file shall not be used + +Rule 21.6 Required +The Standard Library input/output functions shall not be used + +Rule 21.7 Required +The atof, atoi, atol and atoll functions of shall not be used + +Rule 21.8 Required +The library functions abort, exit, getenv and system of shall not be used + +Rule 21.9 Required +The library functions bsearch and qsort of shall not be used + +Rule 21.10 Required +The Standard Library time and date functions shall not be used + +Rule 21.11 Required +The standard header file shall not be used + +Rule 21.12 Advisory +The exception handling features of should not be used + +Rule 21.13 Mandatory +Any value passed to a function in shall be representable as an unsigned char or be the value EOF + +Rule 21.14 Required +The Standard Library function memcmp shall not be used to compare null terminated strings + +Rule 21.15 Required +The pointer arguments to the Standard Library functions memcpy, memmove and memcmp shall be pointers to qualified or unqualified versions of compatible types + +Rule 21.16 Required +The pointer arguments to the Standard Library function memcmp shall point to either a pointer type, an essentially signed type, an essentially unsigned type, an essentially Boolean type or an essentially enum type + +Rule 21.17 Mandatory +Use of the string handling functions from shall not result in accesses beyond the bounds of the objects referenced by their pointer parameters + +Rule 21.18 Mandatory +The size_t argument passed to any function in shall have an appropriate value + +Rule 21.19 Mandatory +The pointers returned by the Standard Library functions localeconv, getenv, setlocale or, strerror shall only be used as if they have pointer to const-qualified type + +Rule 22.1 Required +All resources obtained dynamically by means of Standard Library functions shall be explicitly released + +Rule 22.2 Mandatory +A block of memory shall only be freed if it was allocated by means of a Standard Library function + +Rule 21.20 Mandatory +The pointer returned by the Standard Library functions asctime, ctime, gmtime, localtime, localeconv, getenv, setlocale or strerror shall not be used following a subsequent call to the same function + +Rule 22.3 Required +The same file shall not be open for read and write access at the same time on different streams + +Rule 22.4 Mandatory +There shall be no attempt to write to a stream which has been opened as read-only + +Rule 22.5 Mandatory +A pointer to a FILE object shall not be dereferenced + +Rule 22.6 Mandatory +The value of a pointer to a FILE shall not be used after the associated stream has been closed + +Rule 22.7 Required +The macro EOF shall only be compared with the unmodified return value from any Standard Library function capable of returning EOF + +Rule 22.8 Required +The value of errno shall be set to zero prior to a call to an errno-setting-function + +Rule 22.9 Required +The value of errno shall be tested against zero after calling an errno-setting-function + +Rule 22.10 Required +The value of errno shall only be tested when the last function to be called was an errno-setting-function diff --git a/dep/qfis.list b/dep/qfis.list new file mode 100644 index 0000000..7d78d49 --- /dev/null +++ b/dep/qfis.list @@ -0,0 +1,4 @@ +include/qfis.h +qfis.c +include/qffmath.h +qffmath.c \ No newline at end of file diff --git a/doc/Doxyfile b/doc/Doxyfile index 764c149..2161934 100644 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -32,19 +32,19 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "API Reference" +PROJECT_NAME = "Documentation" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = v1.2 +PROJECT_NUMBER = # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a # quick idea about the purpose of the project. Keep the description short. -PROJECT_BRIEF = "A collection of useful libraries for embedded systems" +PROJECT_BRIEF = "Tools for embedded systems" # With the PROJECT_LOGO tag one can specify a logo or an icon that is included # in the documentation. The maximum height of the logo should not exceed 55 @@ -1290,6 +1290,8 @@ HTML_EXTRA_STYLESHEET = doc/stylesheet/doxygen-awesome.css \ doc/stylesheet/doxygen-awesome-sidebar-only-darkmode-toggle.css \ doc/stylesheet/doxygen-custom/custom-alternative.css \ +HTML_COLORSTYLE = LIGHT + # The HTML_EXTRA_FILES tag can be used to specify one or more extra images or # other source files which should be copied to the HTML output directory. Note # that these files will be copied to the base HTML output directory. Use the @@ -1313,7 +1315,7 @@ HTML_EXTRA_FILES = doc/stylesheet/doxygen-awesome-darkmode-toggle.js \ # Minimum value: 0, maximum value: 359, default value: 220. # This tag requires that the tag GENERATE_HTML is set to YES. -HTML_COLORSTYLE_HUE = 183 +HTML_COLORSTYLE_HUE = 209 # The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors # in the HTML output. For a value of 0 the output will use grayscales only. A @@ -1321,7 +1323,7 @@ HTML_COLORSTYLE_HUE = 183 # Minimum value: 0, maximum value: 255, default value: 100. # This tag requires that the tag GENERATE_HTML is set to YES. -HTML_COLORSTYLE_SAT = 110 +HTML_COLORSTYLE_SAT = 255 # The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the # luminance component of the colors in the HTML output. Values below 100 @@ -1332,7 +1334,7 @@ HTML_COLORSTYLE_SAT = 110 # Minimum value: 40, maximum value: 240, default value: 80. # This tag requires that the tag GENERATE_HTML is set to YES. -HTML_COLORSTYLE_GAMMA = 197 +HTML_COLORSTYLE_GAMMA = 113 # If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML # page will contain the date and time when the page was generated. Setting this diff --git a/doc/mainpage.dox b/doc/mainpage.dox index f0c1bf6..84ba033 100644 --- a/doc/mainpage.dox +++ b/doc/mainpage.dox @@ -1,6 +1,9 @@ /** @mainpage qLibs -* A collection of useful libraries written in C for embedded systems. qLibs -* is developed using a formal and rigorous process framed in compliance with +* @image html https://user-images.githubusercontent.com/11412210/199044031-0e37f39d-5a57-4a68-9da1-fe3cdbc7c0fa.jpg +* A collection of useful libraries written in C for embedded systems. +* +* @remark +* qLibs is developed using a formal and rigorous process framed in compliance with * the MISRA C 2012 and CERT coding standard guidelines and complemented with * multiple static-analysis checks targeted to safe critical applications. * @@ -13,27 +16,30 @@ * -# Gaussian filter * -# Exponential weighting filter * -# Scalar Kalman filter -* - @subpage qfp16_desc "qFP16 : Fixed-Point Q16.16 math library" -* -# Basic operations -* -# Trigonometric functions -* -# Exponential functions * - @subpage qfis_desc "qFIS : Fuzzy Inference System Engine" * -# Mamdani * -# Sugeno * -# Tsukamoto -* - @subpage qpid_desc "qPID : Close-Loop PID Controller" +* - @subpage qpid_desc "qPID : PID Controller" * -# Derivative filter * -# Anti-windup * -# Tracking mode * -# Auto-tunning * -# Additive MRAC -* - @subpage qlitisys_desc "qLTISys : Recursive LTI systems evaluation by transfer functions" +* - @subpage qltisys_desc "qLTISys : Recursive LTI systems evaluation by transfer functions" * -# Continuos * -# Discrete +* - @subpage qfp16_desc "qFP16 : Fixed-Point Q16.16 math library" +* -# Basic operations +* -# Trigonometric functions +* -# Exponential functions * - @subpage qbitfield_desc "qBitField : Bit-Field manipulation library" * - @subpage qrms_desc "qRMS : Recursive Root Mean Square (RMS) calculation" * - @subpage qcrc_desc "qCRC : Generic Cyclic Redundancy Check (CRC) calculator" * - @subpage qtdl_desc "qTDL : Tapped Delay Line in O(1)" +* - Type-generic array operations +* - Single precision floating-point vector(1D-Array) operations +* - Fast single-precision floating-point math */ /** @@ -48,12 +54,12 @@ /** * @defgroup qpid qPID -* This is the second group +* For a brief description of this module, please read \ref qpid_desc */ /** * @defgroup qltisys qLTISys -* This is the second group +* For a brief description of this module, please read \ref qltisys_desc */ /** @@ -63,15 +69,15 @@ /** * @defgroup qbitfield qBitField -* This is the second group +* For a brief description of this module, please read \ref qbitfield_desc */ /** * @defgroup qrms qRMS -* This is the second group +* For a brief description of this module, please read \ref qrms_desc */ /** * @defgroup qcrc qCRC -* This is the second group +* For a brief description of this module, please read \ref qcrc_desc */ diff --git a/doc/qbitfield.dox b/doc/qbitfield.dox index 7d6d94c..a9719ab 100644 --- a/doc/qbitfield.dox +++ b/doc/qbitfield.dox @@ -1,15 +1,46 @@ /*! @page qbitfield_desc Bit-Field manipulation library -* \ref qbitfield bitfield is a library of functions for creating and manipulate -* bit fields (or bit arrays), i.e. series of zeroes and ones spread across an -* array of storage units (unsigned long integers). +* @tableofcontents +* \ref qbitfield is a library of functions for creating and manipulating +* bit fields also known as bit arrays. These are series of zeroes and ones +* spread across an array of storage units, such as unsigned long integers. * -* Structure +* @section qbitfield_structure Structure * * Bit arrays are stored in data structures of type \ref qBitField_t. This structure -* has two elements: an array of unsigned long integers 'field' for storing the -* bits and an integer 'size' for storing the number of bits in the array. +* has two elements: an array of unsigned long integers called @c field for storing the +* bits and an integer @c size for storing the number of bits in the array. * -* Working with bitfields +* +* @section qbitfield_functions Functions +* +* @subsection qbitfield_setup Instance setup +* +* - \ref #QBITFIELD_SIZE Use it to determine the number of bytes required for a BitField. +* - \ref qBitField_Setup() Setup a initialize a BitField instance. +* +* @subsection qbitfield_sbit Single-bit +* +* - \ref qBitField_SetBit() Sets one bit in a BitField. +* - \ref qBitField_ClearBit() Clears one bit in a BitField. +* - \ref qBitField_ToggleBit() Toggles (i.e. reverses the state of) a bit in a BitField. +* - \ref qBitField_ReadBit() Retrieve the state of a bit in a bitfield. +* - \ref qBitField_WriteBit() Writes one bit in a bitfield. +* +* @subsection qbitfield_pattern Multiple-bits +* +* - \ref qBitField_ClearAll() Clear all the bits in the BitField. +* - \ref qBitField_SetAll() Set all the bits in the BitField. +* - \ref qBitField_ReadUINTn() Reads an unsigned 32-bit value from the BitField. +* - \ref qBitField_WriteUINTn() Writes an unsigned n-bit value from the BitField. +* - \ref qBitField_ReadFloat() Reads a 32-bit floating point value from the BitField. +* - \ref qBitField_WriteFloat() Writes a 32-bit floating point value to the BitField. +* +* @subsection qbitfield_dump Dump +* +* - \ref qBitField_Dump() Copies n bytes from the bit-field instance to a designed memory area. +* +* +* @section qbitfield_workingwith Working with bitfields * * @code{.c} * #include @@ -17,12 +48,12 @@ * #include * #include "qbitfield.h" * -* int main( int argc, char *argv[] ) +* int main( int argc, char *argv[] ) * { * qBitField_t vPort; // create the bitfield instance * uint8_t vPortArea[ QBITFIELD_SIZE(48) ] = { 0 }; // Create the bitfield storage area to hold 48bits * uint16_t rWord; -* +* * qBitField_Setup( &vPort, vPortArea, sizeof(vPortArea) ); * qBitField_ClearAll( &vPort); * //we are going to write the following value in the bitfield = 0x8177AA55FF88 diff --git a/doc/qcrc.dox b/doc/qcrc.dox index 677daf4..7a4be3b 100644 --- a/doc/qcrc.dox +++ b/doc/qcrc.dox @@ -23,10 +23,11 @@ * * The calculator \ref qcrc provided here, uses the brute force method. CRC math can * be accomplished in software by shifting the data or shifting the polynomial -* key, then performing the computations. Supported CRCs includes 8, 16 and 32-bit +* key, then performing the computations. Supported CRCs include 8, 16 and 32-bit * in a generic way, this means that you can specify the polynomial key, the CRC -* initial value, I/O reflection and XOR the final output value. in this way, -* the user can implement any variant of CRC only by adjusting these parameters. +* initial value, I/O reflection and XOR the final output value. This generic +* approach allows the user to implement any variant of CRC(8,16,32) only by adjusting +* these parameters. * * Example: A code snippet to compute the CRC-16/KERMIT. * diff --git a/doc/qfis.dox b/doc/qfis.dox index 62491de..25a912f 100644 --- a/doc/qfis.dox +++ b/doc/qfis.dox @@ -1,46 +1,77 @@ /*! @page qfis_desc Fuzzy Inference System Engine -* The \ref qfis Fuzzy Inference System engine provides an API for building and -* evaluation of type-1 fuzzy logic systems. You can tune membership functions -* and rules easily. +* @tableofcontents +* A Fuzzy Inference System (FIS) is a key component of any fuzzy logic system. It +* uses fuzzy set theory, IF-THEN rules and fuzzy reasoning process to find the +* output corresponding to crisp inputs. Predicates in @c IF-THEN rules are connected +* using @c AND or @c OR logical connectives. +* +* Fuzzy inference works by sending any process's crisp input to the fuzzifier, +* which applies a fuzzy membership function and maps the actual readings into +* fuzzy values. The inference engine applies fuzzy rules from the knowledge base +* and produces the fuzzy output. This output can not be used directly in any +* process or system. It needs to be mapped into the original domain. For this, +* the defuzzifier is used, which is the inverse process of fuzzification. It +* converts the fuzzy output into crisp output, which can be fed to the process. +* +* The \ref qfis Fuzzy Inference System engine provides an API for building and +* evaluation of type-1 fuzzy logic inference systems. * -* The types of inferences supported by qFIS are listed in the \ref qFIS_Type_t and -* are detailed below: +* The types of inferences supported by qFIS are listed in the \ref qFIS_Type_t +* and are detailed below: * -* - Mamdani Fuzzy Inference Systems +* @section qfis_mamdani Mamdani * -* In a Mamdani system, the output of each rule is a fuzzy set. Since Mamdani -* systems have more intuitive and easier to understand rule bases, they are +* In a Mamdani system, the output of each rule is a fuzzy set. Since Mamdani +* systems have more intuitive and easier to understand rule bases, they are * well-suited to expert system applications where the rules are created -* from human expert knowledge. +* from human expert knowledge. * * The output of each rule is a fuzzy set derived from the output membership * function and the implication method of the FIS. These output fuzzy sets are -* combined into a single fuzzy set using the aggregation method of the FIS. +* combined into a single fuzzy set using the aggregation method of the FIS. * Then, to compute a final crisp output value, the combined output fuzzy set is * defuzzified using one of the methods described in Defuzzification Methods. -* -* - Sugeno Fuzzy Inference Systems +* To specify a FIS of this type use the ::Mamdani enum definition when calling +* qFIS_Setup(). +* +*
+* @htmlonly +* +* +* +* mamdanifis +* +* +*
+* +* +* +* @endhtmlonly +* Mamdani inference system +*
+* +* @section qfis_sugeno Sugeno * * Sugeno fuzzy inference, also referred to as Takagi-Sugeno-Kang fuzzy inference, * uses singleton output membership functions that are either constant or a linear -* function of the input values. The defuzzification process for a Sugeno system -* is more computationally efficient compared to that of a Mamdani system, since -* it uses a weighted average or weighted sum of a few data points rather than +* function of the input values. The defuzzification process for a Sugeno system +* is more computationally efficient compared to that of a Mamdani system, since +* it uses a weighted average or weighted sum of a few data points rather than * computing the centroid of a two-dimensional area. * * Each rule generates two values: * -* \f$z_i\f$ Rule output level, which is either a constant value or a linear +* \f$u_i\f$ Rule output level, which is either a constant value or a linear * function of the input values. * -* and \f$w_i\f$, the rule firing strength, is determined by the rule antecedent +* and \f$w_i\f$, the rule firing strength, that is determined by the rule antecedent * -* The output of each rule is the weighted output level, which is the product of -* \f$w_i\f$ and \f$z_i\f$. +* The output of each rule is the weighted output level, which is the product of +* \f$w_i\f$ and \f$u_i\f$. * * The final output of the system is the weighted average/sum over all rule outputs: * -* \f$out= \frac{ \sum_{i=1}^{N}w_{i}z_{i} }{ \sum_{i=1}^{N}w_{i}} \f$ +*
\f$out= \frac{ \sum_{i=1}^{N}w_{i}u_{i} }{ \sum_{i=1}^{N}w_{i}} \f$
* * where \f$N\f$ is the number of rules. * @@ -49,124 +80,171 @@ * controllers that are to be applied, respectively, to different operating * conditions of a dynamic nonlinear system. * -* -* - Tsukamoto Fuzzy Inference Systems -* -* In the Tsukamoto inference system, the consequent of each fuzzy if-then rule -* is represented by a fuzzy set with a monotonical membership function, As a result, -* the inferred output of each rule is defined as a crisp value induced by the +* To specificy a FIS of this type, use the ::Sugeno enum definition when calling qFIS_Setup(). +* +*
+* @htmlonly +* +* +* +* sugenofis +* +* +*
+* +* +* +* @endhtmlonly +* Sugeno inference system +*
+* +* @section qfis_tsukamoto Tsukamoto +* +* In the Tsukamoto inference system, the consequent of each fuzzy if-then rule +* is represented by a fuzzy set with a monotonical membership function, As a result, +* the inferred output of each rule is defined as a crisp value induced by the * rule's firing strength. * * The overall output is taken as the weighted average of each rule's output. -* Since each rule infers a crisp output, the Tsukamoto fuzzy model aggregates -* each rule's output by the method of weighted average and thus avoids the +* Since each rule infers a crisp output, the Tsukamoto fuzzy model aggregates +* each rule's output by the method of weighted average and thus avoids the * time-consuming process of defuzzification. * -* -* Comparision among supported Inference Systems -* -* \image html https://user-images.githubusercontent.com/11412210/192664066-ab40cb04-a36f-4b49-9c14-cc34a4683f43.jpg width=50% -* -* -* Defuzzification Methods +* To specify a FIS of this type, use the ::Tsukamoto enum definition when +* calling qFIS_Setup(). +* +*
+* @htmlonly +* +* +* +* tsukamotofis +* +* +*
+* +* +* +* @endhtmlonly +* Tsukamoto inference system +*
+* +* @section qfis_defuzz Defuzzification Methods * -* qFIS supports five different methods, as listed in the \ref qFIS_DeFuzz_Method_t type +* qFIS supports five different methods, as listed in the \ref qFIS_DeFuzz_Method_t type * for computing a single crisp output value for such a fuzzy set. * -* -# Centroid : this method applies only to Mamdani systems and returns the -* center of gravity of the fuzzy set along the x-axis. If you think of the area -* as a plate with uniform thickness and density, the centroid is the point along -* the x-axis about which the fuzzy set would balance. The centroid is computed -* using the following formula, where \f$\mu(x)\f$ is the membership value for +* -# ::centroid (default): this method applies only to ::Mamdani systems and returns the +* center of gravity of the fuzzy set along the x-axis. If you think of the area +* as a plate with uniform thickness and density, the centroid is the point along +* the x-axis about which the fuzzy set would balance. The centroid is computed +* using the following formula, where \f$\mu(x)\f$ is the membership value for * point \f$x_i\f$ in the universe of discourse. * * -* \f$\text{centroid}= \frac{ \sum_{i=1}\mu(x_{i})x_{i} }{ \sum_{i=1}\mu(x_{i})} \f$ +*
\f$\text{centroid}= \frac{ \sum_{i=1}\mu(x_{i})x_{i} }{ \sum_{i=1}\mu(x_{i})} \f$
* -* -# Bisector : this method applies only for Mamdani systems and finds the +* -# ::bisector : this method applies only for ::Mamdani systems and finds the * vertical line that divides the fuzzy set into two sub-regions of equal area. * It is sometimes, but not always, coincident with the centroid line. * -* -# MOM : Middle of Maximum. Only for Mamdani systems. -* -* -# SOM : Smallest of Maximum. Only for Mamdani systems. +* -# ::mom : Middle of Maximum. Only for ::Mamdani systems. * -* -# LOM : Largest of Maximum. Only for Mamdani systems. +* -# ::som : Smallest of Maximum. Only for ::Mamdani systems. * -* -# WtAver: Weighted average of all rule outputs. this method applies only -* for Sugeno and Tsukamoto systems. +* -# ::lom : Largest of Maximum. Only for ::Mamdani systems. * -* -# WtSum: Weighted sum of all rule outputs. This method applies only for -* Sugeno and Tsukamoto systems. +* -# ::wtaver (default): Weighted average of all rule outputs. this method applies only +* for ::Sugeno and ::Tsukamoto systems. * +* -# ::wtsum: Weighted sum of all rule outputs. This method applies only for +* ::Sugeno and ::Tsukamoto systems. * -* Building a Mamdani FIS +* @note The defuzzification method is selected by default when setting up the +* FIS instance with qFIS_Setup(). However, the user can later change the default +* method using the qFIS_SetDeFuzzMethod() function. * -* To carry the FIS building process in a more friendly way, we are going to explain -* it with a particular example. +* @section qfis_buildfis Building a Mamdani FIS * -* This example goal is to solve the tipping problem, and its described as follows: +* To carry out the FIS building process in a more friendly manner, we will explain +* it using a specific example. The goal is to solve the tipping problem, which +* is described as follows: * -* Given a number from 0 through 10 that represents the quality of service at a -* restaurant, where 10 is excellent, and another number from 0 through 10 that +* Given a number from 0 through 10 that represents the quality of service at a +* restaurant, where 10 is excellent, and another number from 0 through 10 that * represents the quality of the food, where 10 is delicious, what should the tip be? -* -* Tipping behavior varies depending on local traditions and personal preferences. -* In this example, the problem is based on tipping as it is typically practiced -* in the United States. An average tip for a meal in the US is 15%. A generous +* Tipping behavior varies depending on local traditions and personal preferences. +* In this example, the problem is based on tipping as it is typically practiced +* in the United States. An average tip for a meal in the US is 15%. A generous * tip could be as high as 25% and a cheap tip could be 5%. * * The actual amount of the tip can vary depending on the quality of the service -* and food. -* -* To solve this problem using fuzzy logic, first capture the essentials of the -* desired tipping behavior, leaving aside all the factors that could be arbitrary. -* -* After analyzing this problem, the tipping behavior is defined using the following +* and food. For this problem, tipping behavior is defined using the following * three rules. * -* -# If the service is poor or the food is rancid, then the tip is cheap. -* -# If the service is good, then the tip is average. -* -# If the service is excellent or the food is delicious, then the tip is generous. -* -* This leads to a system with 2 inputs -* -* -# service -* -# food -* -* and 1 output: tip +* -# IF the service IS poor OR the food IS rancid, THEN the tip IS cheap. +* -# IF the service IS good, THEN the tip IS average. +* -# IF the service IS excellent OR the food IS delicious, THEN the tip IS generous. * +* This leads to a system with 2 inputs : @c service and @c food and 1 output: @c tip * -* \image html https://www.mathworks.com/help/examples/fuzzy/win64/BuildMamdaniSystemsAtTheCommandLineExample_01.png +*
+* @htmlonly +* +* +* +* fistipper +* +* +*
+* +* +* +* @endhtmlonly +* Tipper FIS +*
* * 5 membership functions for the inputs * -* -# (service)poor : A gaussian membership function with spread of 1.5 and center on 0 -* -# (service)good : A gaussian membership function with spread of 1.5 and center on 5 -* -# (service)excelent : A gaussian membership function with spread of 1.5 and center on 10 -* -# (food)rancid : A trapezoidal membership function with points located on [0 0 1 3] -* -# (food)delicious : A trapezoidal membership function with points located on [7 9 10 10] -* -* \image html https://www.mathworks.com/help/examples/fuzzy/win64/BuildMamdaniSystemsAtTheCommandLineExample_02.png +* -# (service)poor : A @a gaussian membership function with spread of @c 1.5 and center on @c 0 +* -# (service)good : A @a gaussian membership function with spread of @c 1.5 and center on @c 5 +* -# (service)excelent : A @a gaussian membership function with spread of @c 1.5 and center on @c 10 +* -# (food)rancid : A @a trapezoidal membership function with points located on [0 0 1 3] +* -# (food)delicious : A @a trapezoidal membership function with points located on [7 9 10 10] * * and 3 membership functions for the output * -* -# (tip)cheap : A triangular membership function with points located on [0 5 10] -* -# (tip)average : A triangular membership function with points located on [10 15 20] -* -# (tip)generous : A triangular membership function with points located on [20 25 30] -* -* \image html https://www.mathworks.com/help/examples/fuzzy/win64/BuildMamdaniSystemsAtTheCommandLineExample_03.png -* -* To build a fuzzy system, you must first instantiate an abstract object of -* type \ref qFIS_t that represents the fuzzy inference system, then the input and -* output vectors, fuzzy set vectors for inputs and outputs, and enumerations with +* -# (tip)cheap : A @a triangular membership function with points located on [0 5 10] +* -# (tip)average : A @a triangular membership function with points located on [10 15 20] +* -# (tip)generous : A @a triangular membership function with points located on [20 25 30] +* +*
+* @htmlonly +* +* +* +* tippermfs +* +* +*
+* +* +* +* @endhtmlonly +* Membership functions for the tipper FIS +*
+* +* To build the fuzzy system, you must first instantiate an abstract object of +* type \ref qFIS_t that represents the fuzzy inference system, then the input and +* output vectors, fuzzy set vectors for inputs and outputs, and enumerations with * the tags for all of them. Let's take a look : * * @code{.c} * // FIS Object * static qFIS_t tipper; * // I/O Fuzzy Objects -* static qFIS_IO_t tipper_inputs[2], tipper_outputs[1]; +* static qFIS_Input_t tipper_inputs[ 2 ]; +* static qFIS_Output_t tipper_outputs[ 1 ]; * // I/O Membership Objects * static qFIS_MF_t MFin[5], MFout[3]; * // I/O Names @@ -177,8 +255,11 @@ * enum { tip_cheap, tip_average, tip_generous}; * @endcode * -* Please note that all tag names are unique. Then we will define the rules of -* the fuzzy system using the previously defined tags. Rules should be defined through +* @attention +* Please note that all tag names are unique. +* +* Then, we will define the rules of +* the fuzzy system using the previously defined tags. Rules should be defined as * an array of type \ref qFIS_Rules_t and the contents should be rules constructed * with the provided statements: * @@ -211,7 +292,7 @@ * @endcode * * Once all the necessary elements have been defined, we can proceed to the -* construction of the system. First we must configure the inputs and outputs by +* construction of the fuzzy system. First, we must configure the inputs and outputs by * setting the ranges of each. For this, we will use the \ref qFIS_InputSetup() * and qFIS_OutputSetup() functions as follows: * @@ -235,9 +316,10 @@ * static const float tip_generous_p[] = { 20.0f, 25.0f, 30.0f } * @endcode * -* Note: The number of parameters may vary depending on the shape of the membership function. +* @note +* The number of parameters may vary depending on the shape of the membership function. * -* Then, let's proceed to configure the membership functions by relating I/O, +* the next step is to configure the membership functions by relating I/O, * tags, shape and parameters one by one by using the \ref qFIS_SetMF() API as follows: * * @code{.c} @@ -265,7 +347,7 @@ * rules, rStrength, 3u ); * @endcode * -* Evaluating the Fuzzy Inference System +* @section qfis_eval Evaluating a Fuzzy Inference System * * If we already have a fuzzy system configured with \ref qFIS_Setup(), we can * evaluate it by using \ref qFIS_Fuzzify(), \ref qFIS_Inference() and @@ -274,8 +356,8 @@ * obtained with \ref qFIS_GetOutput() * * To show its use, first we are going to put everything together in a single -* code snippet, and we are going to create a function tipper_run() that will be in -* charge of evaluating the fuzzy system by invoking the previously mentioned functions. +* code snippet and we are going to create two functions, @c tipper_init() and @c tipper_run() that will be in +* charge of setting up the fuzzy inference system and evaluating it respectively. * * @code{.c} * #include "tipper_fis.h" @@ -315,7 +397,8 @@ * static const float tip_average_p[] = { 10.0f, 15.0f, 20.0f }; * static const float tip_generous_p[] = { 20.0f, 25.0f, 30.0f }; * -* void tipper_init( void ){ +* void tipper_init( void ) +* { * // Set inputs * qFIS_InputSetup( tipper_inputs, service, 0.0f, 10.0f ); * qFIS_InputSetup( tipper_inputs, food, 0.0f, 10.0f ); @@ -339,7 +422,8 @@ * rules, rStrength, 3u ); * } * -* void tipper_run( float *inputs, float *outputs ) { +* void tipper_run( float *inputs, float *outputs ) +* { * // Set the crips inputs * qFIS_SetInput( tipper_inputs, service, inputs[ service ] ); * qFIS_SetInput( tipper_inputs, food, inputs[ food ] ); @@ -357,18 +441,21 @@ * } * @endcode * -* Building a fuzzy system in code can become tedious, especially when you want -* to tune membership function parameters, where it would be appropriate to have -* a graphical tool that reflects the changes made by each tweak. * -* Although the qFIS engine does not provide such a tool, a well-known tool can -* be used to build the fuzzy system and generate qFIS-compatible C code. The -* tool we are talking about is MATLAB's Fuzzy Logic Toolbox and qLibs provides -* a MATLAB command that can be used to take a fis object generated from MATLAB -* and generate C code based on the qFIS engine. +* @section qfis_codegen Code generation +* +* Building a fuzzy system in code can become tedious, especially when you want +* to tune membership function parameters. It would be appropriate to have a +* graphical tool that reflects the changes made by each tweak. +* +* Although the qFIS engine does not provide such a tool, a well-known tool, +* MATLAB's Fuzzy Logic Toolbox, can be used to build the fuzzy system and +* generate qFIS-compatible C code. qLibs provides a MATLAB command that can be +* used to take a FIS object generated by that tool and generate C code based +* on the qFIS engine. * * Download the C-Code generator here: * * * -*/ \ No newline at end of file +*/ diff --git a/doc/qfp16.dox b/doc/qfp16.dox index aa789fb..bd1bf0a 100644 --- a/doc/qfp16.dox +++ b/doc/qfp16.dox @@ -1,46 +1,46 @@ /*! @page qfp16_desc Fixed-Point Q16.16 library -* qLibs provide \ref qfp16 , which is a small fixed point number library -* designed for embedded use. -* It implements all of your favorite transcendental functions, plus only the -* best basic operators, selected for your calculating pleasure. +* @tableofcontents +* qFP16 is a compact fixed-point number library intended for use in embedded +* systems. It includes a variety of transcendental functions and essential +* operators, carefully chosen for optimal performance. * -* The format is a signed Q16.16, which is good enough for most purposes. +* The format is a signed @c Q16.16, which is good enough for most purposes. * -* Datatype limits +* @section qfp16_datatype_limits Datatype limits * -* The maximum representable value is 32767.999 985. The minimum value is -32768.0. +* The maximum representable value is @c 32767.999985. The minimum value is @c -32768.0 * -* The minimum value is also used to represent fp16.overflow for overflow detection, +* The minimum value is also used to represent @c fp16.overflow for overflow detection, * so for some operations it cannot be determined whether it overflowed or the * result was the smallest possible value. In practice, this does not matter much. * -* The smallest unit (machine precision) of the datatype is 1/65536 = 0.000 015. +* The smallest unit (machine precision) of the datatype is @c 1/65536=0.000015259. * -* Fixed-point functions +* @section qfp16_functions Fixed-point functions * -* All the provided functions operate on 32-bit numbers, \ref qFP16_t, which have 16 -* bit integer part and 16-bit fractional part. +* All the provided functions operate on 32-bit numbers, \ref qFP16_t, which have +* 16-bit integer part and 16-bit fractional part. * -* Conversion functions +* @subsection qfp16_conv_functions Conversion functions * * Conversion from integers and floating-point values. These conversions retain * the numeric value and perform rounding where necessary. * -* - \ref qFP16_IntToFP() Simply multiplies a by qFP16.one = 65536 +* - \ref qFP16_IntToFP() Simply multiplies a by @c qFP16.one=65536 * - \ref qFP16_FPToInt() Divides by \ref qFP16_t and rounds to nearest integer. * - \ref qFP16_FloatToFP() Multiplies by \ref qFP16_t and rounds to nearest value. -* - \ref qFP16_FPToFloat() Divides by qFP16.one. Rounding is according to the -* current floating point mode +* - \ref qFP16_FPToFloat() Divides by qFP16.one. Rounding is according to the +* current floating-point mode * - \ref qFP16_DoubleToFP() Multiplies by \ref qFP16 and rounds to nearest value. -* - \ref qFP16_FPToDouble() Divides by qFP16.one. All \ref qFP16_t values fit into +* - \ref qFP16_FPToDouble() Divides by qFP16.one. All \ref qFP16_t values fit into * a double, so no rounding happens. * - \ref qFP16_FPToA() Converts from \ref qFP16_t to string. * - \ref qFP16_AToFP() Converts from string to \ref qFP16_t. * -* Basic arithmetic +* @subsection qfp16_basic_arithmetic Basic arithmetic * * These functions perform rounding and detect overflows. When overflow is -* detected, they return qfp16.overflow as a marker value. +* detected, they return @c qfp16.overflow as a marker value. * * - \ref qFP16_Add() Addition * - \ref qFP16_Sub() Subtraction @@ -48,22 +48,22 @@ * - \ref qFP16_Div() Division * - \ref qFP16_Mod() Modulo * -* Exponential and transcendental functions +* @subsection qfp16_exp_functions Exponential and transcendental functions * * Roots, exponents & similar. * * - \ref qFP16_Sqrt() Square root. Performs rounding and is accurate to \ref qFP16 limits. -* - \ref qFP16_Exp() Exponential function using power series approximation. -* Accuracy depends on range, worst case +-40 absolute for negative inputs and +* - \ref qFP16_Exp() Exponential function using power series approximation. +* Accuracy depends on range, worst case +-40 absolute for negative inputs and * +-0.003% for positive inputs. Average error is +-1 for neg and +-0.0003% for pos. * - \ref qFP16_Log() Natural logarithm using Newton approximation and \ref qFP16_Exp(). * Worst case error +-3 absolute, average error less than 1 unit. * - \ref qFP16_Log2() Logarithm base 2. -* - \ref qFP16_IPow() Computes the integer-power of a \ref qFP16_t number +* - \ref qFP16_IPow() Computes the integer-power of a \ref qFP16_t number * - \ref qFP16_Pow() Modulo * * -* Trigonometric functions and helpers +* @subsection qfp16_trig_functions Trigonometric functions and helpers * * - \ref qFP16_Sin() Sine for angle in radians * - \ref qFP16_Cos() Cosine for angle in radians @@ -75,13 +75,13 @@ * - \ref qFP16_Sinh() Hyperbolic sine * - \ref qFP16_Cosh() Hyperbolic cosine * - \ref qFP16_Tanh() Hyperbolic tangent -* - \ref fp16_RadToDeg() Converts angle units from radians to degrees. -* - \ref fp16_DegToRad() Converts angle units from degrees to radians +* - \ref qFP16_RadToDeg() Converts angle units from radians to degrees. +* - \ref qFP16_DegToRad() Converts angle units from degrees to radians * - \ref qFP16_WrapToPi() Wrap the fixed-point angle in radians to [−pi pi] * - \ref qFP16_WrapTo180() Wrap the fixed-point angle in degrees to [−180 180] * * -* Example: Solution of the quadratic equation +* @section qfp16_example Example: Solution of the quadratic equation * * This draft example computes one solution of the quadratic equation * by using the fixed point format. Equation is given by: diff --git a/doc/qltisys.dox b/doc/qltisys.dox index a724291..3301f44 100644 --- a/doc/qltisys.dox +++ b/doc/qltisys.dox @@ -1,32 +1,37 @@ -/*! @page qlitisys_desc Recursive LTI Systems Evaluation by transfer functions. -* \ref qltisys is a class to evaluate real-valued transfer function SISO models. -* In a nutshell, a transfer function is a model that describes the frequency-dependent -* response of a linear time-invariant system and can be either continuous-time -* or discrete-time. This means that you can use qLTISys both to simulate dynamic -* systems and to easily implement filters, compensators, or controllers. +/*! @page qltisys_desc Recursive LTI Systems Evaluation by transfer functions. +* @tableofcontents +* \ref qltisys is a class that evaluates single-input, single-output (SISO) +* transfer function models in real-valued systems. +* A transfer function is a model that describes the frequency-dependent response +* of a linear time-invariant system, and this class can handle both continuous-time +* and discrete-time systems. \ref qltisys can be used for simulating dynamic +* systems and implementing filters, compensators, or controllers. * -* Continous-time transfer functions +* @section qltisys_cont Continous-time transfer functions * * Here, the transfer function \f$ G(s) \f$ is the linear mapping of the Laplace * transform of the input, \f$ U(s)= \mathcal{L}[u(t)]\f$, to the Laplace transform * of the output \f$ Y(s)= \mathcal{L}[y(t)]\f$. * +* * \f$ G(s) = \frac{N(s)}{D(s)} = \frac{ b_{0}s^{n} + b_{1}s^{n-1} + b_{2}s^{n-2} + ... + b_{n} }{ s^{n} + a_{1}s^{n-1} + a_{2}s^{n-2} + ... + a_{n} } \f$ * * \f$ N(s) \f$ and \f$ D(s) \f$ are the numerator and denominator polynomials in * descending powers of \f$s\f$, respectively. * -* To instantiate a continuous transfer function, you should define a variable -* of type \ref qLTISys_t, two arrays of N+1 elements with the coefficients of the -* polynomials for both, numerator and denominator, and finally, an array of type -* \ref qLTISys_ContinuosX_t with the N initial states of the system. -* Then you can call \ref qLTISys_Setup() to construct the system and initial -* conditions. Subsequently, now you can evaluate the system with a given input -* signal by just calling \ref qLTISys_Excite(). The user must ensure that the -* evaluation of the system is executed periodically at the defined time step. -* +* To instantiate a continuous transfer function, you should define a variable +* of type \ref qLTISys_t, two arrays of N+1 elements with the coefficients of the +* polynomials for both, the numerator and denominator, and finally, an array of type +* \ref qLTISys_ContinuosX_t to hold the N states of the system. +* Then, you can call \ref qLTISys_Setup() to construct the system and set initial +* conditions. Subsequently, you can evaluate the system with a given input-signal +* by just calling \ref qLTISys_Excite(). +* +* @attention +* The user must ensure that the +* evaluation of the system is executed periodically at the required time step. * -* Example: Evaluate the given continous transfer function +* @subsection qltisys_ex1 Example: Evaluate the given continuous transfer function * * \f$ G(s) = \frac{ 2s^{2} + 3s + 6 }{ s^{3} + 6s^{2} + 11s + 16 } \f$ * @@ -42,7 +47,7 @@ * qLTISys_t system; * float num[ SYS_ORDER+1 ] = { 0.0f, 2.0f, 3.0f, 6.0f }; * float den[ SYS_ORDER+1 ] = { 1.0f, 6.0f, 11.0f, 16.0f }; -* qLTISys_ContinuosX_t x[ SYS_ORDER ] = { { { 0.0f } }, { { 0.0f } },{ { 0.0f } } }; +* qLTISys_ContinuosX_t x[ SYS_ORDER ]; * * void xTaskSystemSimulate( void *arg ) * { @@ -57,23 +62,23 @@ * } * @endcode * -* Discrete-time transfer functions +* @section qltisys_disc Discrete-time transfer functions * -* The z-transform is used in discrete-time systems to deal with the relationship -* between an input signal \f$ u(t) \f$ and an output signal \f$ y(t) \f$ , so the -* transfer function is similarly written as \f$ G(z^{-1}) \f$ and is often -* referred to as the pulse-transfer function.  +* The z-transform is used in discrete-time systems to deal with the relationship +* between an input signal \f$ u(t) \f$ and an output signal \f$ y(t) \f$ , so the +* transfer function is similarly written as \f$ G(z^{-1}) \f$ and is often +* referred to as the pulse-transfer function. * * \f$ G(z^{-1}) = \frac{N(z^{-1})}{D(z^{-1})} = \frac{ b_{0} + b_{1}z^{-1} + b_{2}z^{-2} + ... + b_{m}z^{-m} }{ 1 + a_{1}z^{-1} + a_{2}z^{-2} + ... + a_{n}z^{-n} } \f$ * Discrete systems are instantiated in a similar way to continuous systems, -* but there are some differences. Initial states are defined by using the -* \ref qLTISys_DiscreteX_t. The size of polynomials can vary according to +* but there are some differences. States are should be stored in an array of type +* \ref qLTISys_DiscreteX_t. The size of polynomials can vary according to * their order, and you should pass the \ref #QLTISYS_DISCRETE directive on \ref -* qLTISys_Setup() in order to differentiate the system when it is being evaluated. +* qLTISys_Setup() in order to differentiate the system when it is being evaluated. * Please take a look at the following example: * -* Example: Evaluate the given discrete transfer function +* @subsection qltisys_ex2 Example: Evaluate the given discrete transfer function * * \f$ G(z^{-1}) = \frac{ 0.1 + 0.2z^{-1} + 0.3z^{-2} }{ 1 - 0.85z^{-1} + 0.02z^{-2} } \f$ * @@ -89,7 +94,7 @@ * qLTISys_t system; * float num[ NB ] = { 0.1f, 0.2f, 0.3f }; * float den[ NA+1 ] = { 1.0f, -0.85f, 0.02f }; -* qLTISys_DiscreteX_t xk[ NB ] = { 0.0f, 0.0f, 0.0f }; +* qLTISys_DiscreteX_t xk[ NB ]; * * void xTaskSystemSimulate( void *arg ) * { diff --git a/doc/qpid.dox b/doc/qpid.dox index ff0d579..526df53 100644 --- a/doc/qpid.dox +++ b/doc/qpid.dox @@ -1,52 +1,100 @@ -/*! @page qpid_desc Closed-loop PID Controller -* A PID controller seeks to keep some input variable close to a desired setpoint -* by adjusting an output. The way in which it does this can be 'tuned' by -* adjusting three parameters (Proportinal,Integral,Derivative). +/*! @page qpid_desc PID Controller +* @tableofcontents +* A Proportional Integral Derivative (PID) controller is an automatically +* optimized and accurate control system responsible for ensuring that a +* process remains as close to the desired value as possible regardless of +* various disruptions. The controller compares the measured process variable +* \f$ y(t)\f$ and a desired Setpoint \f$ r(t)\f$ (desired outcome). Based on +* that comparison, the controller calculates the Proportional, Integral and +* Derivative terms and adds them to obtain the output signal \f$ u(t) \f$ that +* attempts to minimize the error. This output signal is then used to command +* the Final Control Element (e.g an actuator) that is in charge of operating +* the process. +* +* Proportional is used to find out the error between the desired value and +* actual value and is responsible for the corrective response. Integral is +* applied to calculate all the past values of error and then integrate them to +* find out the Integral term. When error is expelled from the system this +* integral stops increasing. The derivative is used to predict the expected +* error values in the future based on the present values. Controlling effect can be +* increased if the system has a rapid rate of change, which is also based on +* Derivative. Combining all these three operations gives the name Proportional +* Integral Derivative (PID) Controller. * * qLibs provides the \ref qpid implementation, which, apart from the simplified -* overview representation of the PID controller, also addresses some practical -* issues and includes the additional features: +* overview representation of the PID controller, it also addresses some practical +* issues and includes additional features as listed below: * -* - Noisy Derivative Contributions (Derivative filter) -* - Automatic Anti-windup control -* - Tracking mode (bumpless transfer) -* - Auto-tunning +* - Derivative filter +* - Anti-windup +* - Bumpless transfer +* - SetPoint weighting +* - Auto-tuning * - Additive MRAC * +* @section qpid_approach PID Controller approach * -* PID Controller approach +* The mathematical model and practical loop use a direct control action for all +* the PID terms, which means an increasing positive error results in an +* increasing positive control output correction. * -* The mathematical model and practical loop above both use a direct control -* action for all the PID terms, which means an increasing positive error -* results in an increasing positive control output correction. +* The overall control function implemented in ::qPID_Automatic mode is +* given by: * -* The overall control function is given by: +*
+* \f$ v(t)= \psi(t)r(t) + [ K_{c}e_{b}(t) + K_{i}\int [ e(t) + c(t-1) ]dt + K_{d}f_{d}(t) ] \f$ * -* \f$ v(t)= \psi(t)r(t) + [ K_{c}e(t) + K_{i}\int [ e(t) + c(t-1) ]dt + K_{d}f_{d}(t) ] \f$ * * \f$ u(t) = \text{Sat}[ v(t), u_{min}, u_{max} ]\f$ * -* where \f$r(t)\f$ the set-point, \f$ e(t) = r(t) - y(t) \f$ its the error -* \f$y(t)\f$ the process output, \f$K_{c},K_{i},K_{d}\f$ the PID gain respectively -* and \f$u(t)\f$ the control action. -* -* \f$\psi(t)\f$ the adaptive gain from the additive MRAC (later explained) -* -* As shown above, the derivative term \f$ f_{d}(t)\f$ is the output of a -* low-pass filter of the error derivative. -* -* \f$ f_{d}(t) = \text{LPF}[ \frac{de(t)}{dt} ]\f$ -* -* and \f$ c(t)\f$, the Anti-Windup and tracking mode feedback, with \f$K_{w}\f$ -* and \f$K_{T}\f$ as the ajustment parameters respectively. -* -* \f$ c(t) = K_{w}[ u(t) - v(t) ] + K_{T}[ r_{t}(t) - u(t) ]\f$ -* -* \image html https://user-images.githubusercontent.com/11412210/193475586-80d41932-d7f3-4567-8820-2ab423261354.png -* -* Controller instantiation -* -* Before you start using a controller, it must first be instantiated first +*
+* where \f$r(t)\f$ is the set-point, \f$y(t)\f$ the process output, \f$ e(t) \f$ +* its the error \f$K_{c},K_{i},K_{d}\f$ the PID gains respectively and +* \f$u(t)\f$ the control action. +* +* \f$\psi(t)\f$ is the adaptive gain from the additive MRAC (later explained) +* +* As shown above, the derivative term \f$f_{d}(t)\f$ is the output of a +* low-pass filter that takes the raw derivative as input. +* +*
\f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ]\f$
+* +* and \f$ c(t)\f$, the saturation feedback for the anti-windup, with \f$K_{w}\f$ +* as the adjustment parameter. +* +*
\f$ c(t) = K_{w}[ u(t) - v(t) ] \f$
+* +*
+* @htmlonly +* +* +* +* qpid +* +* +*
+* +* +* +* @endhtmlonly +* PID Implementation +*
+* +* @section qpid_maninput Manual Mode +* When the operation mode is changed to ::qPID_Manual, the feedback is disconnected and +* the controller works in open loop being commanded by the manual input \f$ m(t) \f$. +* Since the controller is a dynamic system, it is necessary to make sure that +* the state of the system is correct when switching the controller between manual +* and automatic mode. When the system is in manual mode, the control algorithm +* produces a control signal that may be different from the manually generated +* control signal. It is necessary to make sure that the two outputs coincide at +* the time of switching. This is called bumpless transfer and it's given by +* +*
\f$ v(t) = \int [ K_{T}m(t) + c(t-1) ]dt\f$
+* +* @section qpid_create Creating a PID Controller +* +* Before you start using a controller, it must be instantiated first * via the \ref qPID_controller_t type and then configured using the \ref qPID_Setup() API, * where you can define the PID gains and time step. * @@ -64,17 +112,17 @@ * @endcode * * -* Usign the controller +* @section qfpid_usage Using the controller * * The following example illustrates how a PID controller can be used to regulate -* the speed of a DC motor. PID control operates on a separate task running -* periodically at a rate of 50mS. The speed measurement is being read through +* the speed of a DC motor. PID control operates on a separate task running +* periodically at a rate of 50mS. The speed measurement is read through * an analog input and then scaled to the appropriate units (RPM). The \ref qPID_Control() -* function will be in charge of computing the control action and updating the internal -* states of the controller. Then, the control output gets scaled-back in order to +* function will be in charge of computing the control action and updating the internal +* states of the controller. Then, the control output gets scaled back in order to * send the control command by using the PWM(Pulse Width Modulation) module. * -* Example: Speed control using a PID controller: +* @subsection qpid_ex1 Example: Speed control using a PID controller: * @code{.c} * #include * #include @@ -105,7 +153,7 @@ * qPID_controller_t speedControl; * int ret; * -* BSP_SystemInit( ); +* BSP_SystemInit(); * ret = qPID_Setup( &speedControl, 1, 0.1, 0, (float)dt/1000.0f ); * if ( 0 == ret ) { * puts( "ERROR: Cant configure PID controller" ); @@ -120,19 +168,20 @@ * @endcode * * -* Additive MRAC (Model Reference Adaptive Control) +* @section qpid_mrac Additive MRAC (Model Reference Adaptive Control) * * Model Reference Adaptive Control (MRAC) is a strategy where the controller * of the closed-loop is adapted by an adjustment mechanism, which takes \f$ y_{m}(t) \f$ * from a reference model as input and tries to adjust the controller output \f$v(t)\f$. -* The adjustment mechanism used by qPID is the enhanced MIT-rule approach, -* which adapts a feed-forward gain by the error between the system \f$ y(t) \f$ -* and the model \f$ y_{m}(t) \f$ and therefore is the so-called Gradient approach. -* +* The adjustment mechanism used by \ref qpid is the enhanced MIT-rule approach, +* which adapts a feed-forward gain by the error between the system \f$ y(t) \f$ +* and a reference model \f$ y_{m}(t) \f$, therefore is the so-called Gradient approach. * +*
* \f$ e_{m}(t) = y(t) - y_{m}(t)\f$ * * \f$ \delta(t) = -\gamma \frac{ e_{m}(t) y_{m}(t) }{ \beta + y_{m}^{2}(t) } \f$ +*
* * where \f$\gamma(t)\f$ is the adaptation gain and \f$\beta(t)\f$ is introduced * to remove the problem of the possible division by zero if \f$y_{m}^{2}(t) \f$ @@ -141,10 +190,10 @@ * The MRAC adaptation is then computed by integrating the \f$\delta(t)\f$ term * as follows: * -* \f$ \psi(t) = \int \delta(t) dt \f$ +*
\f$ \psi(t) = \int [ \delta(t) + c(t) ] dt \f$
* * This method can be used to adapt to slower changes but can become unstable for -* abrupt changes to the system. Therefore, this implementation uses a so called +* abrupt changes to the system. Therefore, this implementation uses a so-called * modified MRAC (M-MRAC) where the adaptation is later added to a PID controller, * being the MRAC the additive loop. * @@ -157,7 +206,7 @@ * then, enable the MRAC by using the \ref qPID_SetMRAC() API. Here, you must provide * a pointer to the output of the reference model and the adaptation gain \f$\gamma(t)\f$ * -* Example: Speed control with PID controller with an additive MRAC: +* @subsection qpid_ex2 Example: Speed control with PID controller with an additive MRAC: * @code{.c} * #include * #include @@ -186,10 +235,10 @@ * qLTISys_Setup( &ref_model, num, den, x, 0, REF_MODEL_ORDER+1, (float)dt/1000.0f ) ); * qPID_SetMRAC( controller, &refmodel_output, 0.01f ); * for ( ;; ) { - refmodel_output = qLTISys_Excite( &ref_model, controlOutput ); +* refmodel_output = qLTISys_Excite( &ref_model, controlOutput ); * processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) ); * controlOutput = qPID_Control( controller, SetPoint, processMeasurement ); -* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) ); +* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) ); * vTaskDelay( dt / portTICK_RATE_MS) ; * } * } @@ -213,7 +262,7 @@ * } * @endcode * -* Autotunning +* @section qpid_autotune Autotuning * * Autotuning can eliminate much of the trial and error of a manual tuning * approach, especially if you do not have a lot of loop tuning experience. @@ -222,13 +271,14 @@ * the tuning parameters to their optimal values. * * The Autotune feature for the controller will only run for a limited amount of time -* each time it is enabled. In other words, autotuning does not run continuously +* after it gets enabled. In other words, autotuning does not run continuously * during operation. Whenever there is a substantial change in the process * dynamics, the tuning process will need to be repeated in order to derive new * gains required for optimal control. * -* Autotunning is performed by using the following recursive algorithm: +* Autotuning is performed by using the following recursive algorithm: * +*
* \f$ L(t) = \frac{ P(t-1)\phi }{ \lambda + \phi^{T}P(t-1)\phi } \f$ * * \f$\theta(t) = \theta(t-1) + L[ y(t) - \phi^{T}\theta(t) ] \f$ @@ -247,20 +297,104 @@ * \f$ K_{i}(t) = \alpha \frac{ g(t)[ 0.54 + 0.33r_{1} ] }{ r_{2}dt } \f$ * * \f$ K_{c}(t) = \alpha \frac{ K_{c}(t)dt }{ r_{2} } \f$ +*
* * where \f$ r_{1} = dt/\tau(t) \f$ and \f$ r_{2} = 1.35 + 0.25r_{1} \f$ * * and the remaining parameters \f$\mu\f$, \f$\alpha\f$, \f$\lambda\f$ are internally * computed for best performance. * -* Autotunning Usage +* @subsection qpid_autotune_usage Autotuning Usage * -* In order to use the autotuner, you must first instantiate the \ref qPID_AutoTunning_t +* In order to use the autotuner, you must first instantiate the \ref qPID_AutoTuning_t * object and bind it to a previously configured PID controller by using the -* \ref qPID_BindAutoTunning(). +* \ref qPID_BindAutoTuning(). * -* After this, you can enable auto tuning via \ref qPID_EnableAutoTunning() +* After this, you can enable autotuning via \ref qPID_EnableAutoTuning() * for a defined number of intervals. When the autotune ends, the resulting PID -* gains will be applied to the binded controller +* gains will be applied to the bounded controller. * +* @subsection qpid_ex3 Example: Speed control with PID controller and autotuning: +* This example takes advantage of the FreeRTOS task notifications to enable the +* autotuning when a rising edge is detected on a certain digital input. Autotuning +* is enabled for 5 sec. Note that the setpoint is briefly modified during this +* process to stimulate the process. Upon completion, the setpoint is restored to +* its original value. +* +* @code{.c} +* #include +* #include +* #include "freertos/FreeRTOS.h" +* #include "freertos/task.h" +* #include "bsp.h" +* #include "qpid.h" +* +* #define REF_MODEL_ORDER ( 1 ) +* +* const TickType_t dt = 50; //50mS time-step +* void xTaskPIDspeedControl( void *arg ); +* TaskHandle_t pid_task; +* +* void gpio_Int_Handler( void ) +* { +* BaseType_t xHigherPriorityTaskWoken = pdFALSE; +* +* vTaskNotifyGiveFromISR( pid_task, &xHigherPriorityTaskWoken); +* portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +* HAL_GPIO_ClearStatusFlag(); +* } +* +* void xTaskPIDspeedControl( void *arg ) +* { +* qPID_controller_t *controller = (qPID_controller_t *)arg; +* float processMeasurement; +* float SetPoint = 300.0f; // desired motor speed 300rpm +* float tmpSetPoint = 320.0f; +* float *p_setpoint = &SetPoint; +* float refmodel_output = 0.0f; +* float controlOutput = 0.0f; +* uint32_t notification_autotune_enable; +* +* qLTISys_Setup( &ref_model, num, den, x, 0, REF_MODEL_ORDER+1, (float)dt/1000.0f ) ); +* qPID_SetMRAC( controller, &refmodel_output, 0.01f ); +* for ( ;; ) { +* notification_autotune_enable = ulTaskNotifyTake( pdTRUE, 0 ); // dont wait +* if ( notification_autotune_enable ) { +* p_setpoint = &tmpSetPoint; +* qPID_EnableAutoTuning( controller, 100 ); //enable for 5 seconds ( 100*50mS ) +* } +* if ( qPID_AutoTuningComplete( controller) ) { +* p_setpoint = &SetPoint; +* qPID_EnableAutoTuning( controller, 0uL ); //disable +* } +* processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) ); +* controlOutput = qPID_Control( controller, *p_setpoint, processMeasurement ); +* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) ); +* vTaskDelay( dt / portTICK_RATE_MS) ; +* } +* } +* +* int main( int argc, char *argv[] ) +* { +* qPID_controller_t speedControl; +* qPID_AutoTuning_t at; +* int ret; +* +* BSP_SystemInit( ); +* HAL_GPIO_Enable( GPIO12, GPIO_INPUT ); +* HAL_GPIO_SetInterruptMode( GPIO12, RISING_EDGE ); +* HAL_GPIO_EnableInterrupts( ); +* ret = qPID_Setup( &speedControl, 1, 0.1, 0, (float)dt/1000.0f ); +* if ( 0 == ret ) { +* puts( "ERROR: Cant configure PID controller" ); +* } +* qPID_SetSaturation( &speedControl, 0.0f, 100.0f ); +* qPID_BindAutoTuning( &speedControl, &at ); +* // Create the task that handles the speed control at the defined rate +* xTaskCreate( xTaskPIDspeedControl, "speedControl", 1024, &speedControl, configMAX_PRIORITIES - 1 , &pid_task ); +* vTaskStartScheduler(); +* for( ;; ); +* return EXIT_SUCCESS; +* } +* @endcode */ \ No newline at end of file diff --git a/doc/qrms.dox b/doc/qrms.dox index e1a844a..e27ed9f 100644 --- a/doc/qrms.dox +++ b/doc/qrms.dox @@ -1,23 +1,23 @@ /*! @page qrms_desc Recursive Root Mean Square estimator -* Real-time digital systems often require the calculation of a root-mean, such -* as a root-mean square (RMS) level or average magnitude of a complex signal. -* While averaging can be efficiently implemented by most microprocessors, -* obtaining an efficient and rapidly converging algorithm is surely a -* time-consuming task between adjustments and optimizations that need to be -* performed. By the other hand, the square root may not be–especially with -* low-cost hardware. If the processor doesn't implement a fast square root -* function, it must be implemented in software; although this yields accurate -* results, it may not be efficient. +* Real-time digital systems often require the calculation of a root-mean, such +* as a root-mean-square (RMS) level or average magnitude of a complex signal. +* While averaging can be efficiently implemented by most microprocessors, +* obtaining an efficient and rapidly converging algorithm is surely a +* time-consuming task due to the adjustments and optimizations that need to be +* performed. On the other hand, calculating the square root can be challenging, +* especially with low-cost hardware. If the processor does not have a built-in +* fast square root function, it must be implemented in software. While this +* can provide accurate results, it may not be as efficient. * * \ref qrms provides a super-efficient method to compute the Recursive Root -* Mean Square (RMS). The implementation uses 3-stage filtering and Newton's -* method to obtain the square root. The filter parameters are perfectly tuned -* and optimized to converge in around 16 cycles operating at a sampling rate -* of 1mS. This means that you must configure your embedded system to meet those -* specifications, either by configuring a timer interrupt or by delegating +* Mean Square (RMS). The implementation uses 3-stage filtering and Newton's +* method to obtain the square root. The filter parameters are perfectly tuned +* and optimized to converge in around 16 cycles, operating at a sampling rate +* of 1mS. This means that you must configure your embedded system to meet those +* specifications, either by configuring a timer interrupt or by delegating * responsibility to a real-time task. * -* Example : By using a timer interrupt +* @section qrms_ex1 Example : By using a timer interrupt * * rms_compute.c * @code{.c} @@ -31,12 +31,14 @@ * static volatile float rms_value = 0.0f; * static float window[ RMS_WIN_SIZE ] = { 0.0f }; * -* void TimerA0_Int_Handler( void ) { //interrupt tick at 1mS +* void TimerA0_Int_Handler( void ) //interrupt tick at 1mS +* { * rms_value = qRMS_Update( &rms_instance, get_sample() ); * HAL_TimerA0_ClearStatusFlag(); * } * -* void rms_setup( void ) { +* void rms_setup( void ) +* { * qRMS_Setup( &rms_instance, window, RMS_WIN_SIZE ); * HAL_TimerA0_Init(); * HAL_TimerA0_SetPeriod_mS( 1 ); @@ -44,7 +46,8 @@ * HAL_TimerA0_Enable(); * } * -* float rms_getvalue( void ) { +* float rms_getvalue( void ) +* { * return rms_value; * } * diff --git a/doc/qssmoother.dox b/doc/qssmoother.dox index f3385b4..42e731d 100644 --- a/doc/qssmoother.dox +++ b/doc/qssmoother.dox @@ -1,34 +1,36 @@ /*! @page qssmoother_desc Filters for signal smoothing -* Smoothing is how we discover important patterns in a signal while leaving -* out things that are unimportant (i.e. noise). This smoothing is accomplished -* through the use of filtering. The goal of smoothing is to produce slow changes -* in value so that it's easier to see trends in our signal. -* @a qLibs provides a collection of smoothing filters to improve our signals, -* the @ref qssmoother module. +* @tableofcontents +* In many embedded systems applications, signals are captured and may be +* subject to "noise." To reconstruct the original signal, a "smoothing filter" +* is applied to "clear" the signal of this noise. The filter attenuates high +* frequency components and emphasizes slow changes in values, making trends +* easier to discern. The qLibs library offers the qSSMoother module, which +* includes a collection of commonly used and efficient smoothing filters for +* easy signal processing. * * Below is the list of filters supported by @ref qssmoother : * -* - First Order Low Pass Filter +* @section qssmoother_lpf1 First Order Low Pass Filter * -* A first order low-pass filter is a filter that passes signals with a frequency -* lower than a selected cutoff frequency and attenuates signals with frequencies -* higher than the cutoff frequency. Low-pass filters provide a smoother form of a -* signal, removing the short-term fluctuations and leaving the longer-term trend. -* The discrete-time ecuation of this filter is defined as follows, where \f$w\f$ +* A first-order low-pass filter passes signals with a frequency lower than a +* chosen cutoff frequency and reduces signals with frequencies higher than the +* cutoff. This results in a smoother signal by removing short-term fluctuations +* and highlighting longer-term trends. +* The discrete-time equation of this filter is defined as follows, where \f$w\f$ * is the cut-off frequency given in \f$rad/seg\f$ * -* The difference-ecuation of the output of this filter is given by: +* The difference-equation of the output of this filter is given by: * -* \f$y(t)=x(t)+\alpha[y(t-1)+x(t)]\f$ +*
\f$y(t)=x(t)+\alpha[y(t-1)+x(t)]\f$
* * where, * -* \f$\alpha=e^{-T_{m}w}\f$ +*
\f$\alpha=e^{-T_{m}w}\f$
* -* \f$ 0 < \alpha < 1\f$ +*
\f$ 0 < \alpha < 1\f$
* * -* Example: setting up a 1st order low-pass filter: +* @subsection qssmoother_ex1 Example: setting up a 1st order low-pass filter: * @code{.c} * qSSmoother_LPF1_t smoother; * float alpha = 0.85f; @@ -40,27 +42,28 @@ * } * @endcode * -* - Second Order Low Pass Filter +* @section qssmoother_lpf2 Second Order Low Pass Filter * * This filter has similar properties to the 1st order low pass filter. The main -* difference between a 1st and 2nd order low pass filter is that the stop -* band roll-off will be twice the 1st order filters at 40dB/decade (12dB/octave) -* as the operating frequency increases above the cut-off frequency \f$w\f$. +* difference is that the stop band roll-off will be twice the 1st order filters +* at 40dB/decade (12dB/octave) as the operating frequency increases above the +* cut-off frequency \f$w\f$. * -* The difference-ecuation of the output of this filter is given by: +* The difference-equation of the output of this filter is given by: * -* \f$y(t)=kx(t)+b_{1}x(t-1)+kx(t-2)-a_{1}y(t-1)-a_{2}y(t-2)\f$ +*
\f$y(t)=kx(t)+b_{1}x(t-1)+kx(t-2)-a_{1}y(t-1)-a_{2}y(t-2)\f$
* * where, * +*
* \f$ 0 < \alpha < 1\f$ * * \f$p=\sqrt{2\alpha}\f$, \f$r=1+p+\alpha^{2}\f$, \f$k=\frac{\alpha^{2}}{r}\f$ * * \f$a_1=\frac{ 2(\alpha^{2}-1)}{r}\f$, \f$a_2=\frac{ 1-p+\alpha^{2} }{r}\f$, \f$b_1=2k\f$ +*
* -* -* Example: setting up a 2nd order low-pass filter: +* @subsection qssmoother_ex2 Example: setting up a 2nd order low-pass filter: * @code{.c} * qSSmoother_LPF2_t smoother; * float alpha = 0.85f; @@ -73,25 +76,25 @@ * @endcode * * -* - Moving Window Median Filter \f$O(n)\f$ +* @section qssmoother_mwm1 Moving Window Median Filter O(n) * -* This is a form of low-pass that computes the moving median of the input signal -* over time. The object uses the sliding window method to compute the moving -* median. In this method, a window of a specified length is moved, sample by -* sample, and the object computes the median of the data in the window. +* This low-pass filter calculates the moving median of the input signal over +* time using the sliding window method. A window of a specified length is moved +* sample by sample and the median of the data in the window is computed. * -* The output for each input sample is the median of the current sample and the -* \f$N - 1\f$ previous samples. \f$N\f$ is the length of the window in samples. -* To compute the first \f$N - 1\f$ outputs, when the window does not have enough -* data yet, the algorithm fills the window with the first current sample. +* For each input sample, the output is the median of the current sample and the +* previous \f$N - 1\f$ samples. \f$N\f$ represents the length of the window in +* samples +* To compute the first \f$N - 1\f$ outputs, he algorithm fills the window with +* the first current sample when there is not enough data yet. * -* The difference-ecuation of the output of this filter is given by: +* The difference-equation of the output of this filter is given by: * -* \f$ \frac{1}{N}\sum_{i=0}^{N-1}y(t-i)=\frac{1}{N}[ y(t) + y(t-1) + ... + y(t-N-1) ] \f$ +*
\f$ \frac{1}{N}\sum_{i=0}^{N-1}y(t-i)=\frac{1}{N}[ y(t) + y(t-1) + ... + y(t-N-1) ] \f$
* * This filter has a time complexity of \f$O(n)\f$ * -* Example: setting up a moving window median filter: +* @subsection qssmoother_ex3 Example: setting up a moving window median filter: * @code{.c} * #define N_MWM ( 20u ) * @@ -106,23 +109,24 @@ * @endcode * * -* - Moving Window Median Filter \f$O(1)\f$ +* @section qssmoother_mwm2 Moving Window Median Filter O(1) * -* This filter has the same principle as described in the previous filter. The -* big difference here is the way the sliding window is treated. The window is -* considered as a TDL (Tapped Delay Line) based on a circular queue. -* This type of structure has a line of delays with \f$N - 1\f$ taps, and each tap -* extracts the signal at some fixed integer delay relative to the input. +* This filter operates on the same principle as the previous filter. The main +* difference is the treatment of the sliding window, which is implemented as a +* TDL (Tapped Delay Line) using a circular queue. +* This structure has \f$N - 1\f$ taps in a line of delays, each tap extracts +* the signal at a fixed integer delay relative to the input. * * The circular queue approach prevents the shift of all the \f$N - 1\f$ samples -* stored in the sliding-window. +* stored in the sliding window. * -* The implementation comes with a huge benefit in terms of performance because -* the time complexity is constant \f$O(1)\f$ , compared to the previous filter, -* where the time complexity grows in direct proportion to the size of the window. -* Use this kind of filter when you need to consider large sliding windows. +* @remark +* This implementation offers a significant performance benefit as it has a +* constant time complexity of \f$O(1)\f$ , compared to the previous filter +* where time complexity grows proportionally to the size of the window. Use +* this filter when dealing with large sliding windows. * -* Example: Setting up a moving window median filter: +* @subsection qssmoother_ex4 Example: Setting up a moving window median filter: * @code{.c} * #define N_MWM ( 150u ) * @@ -137,42 +141,45 @@ * @endcode * * -* - Moving Outliar Removal Filter \f$O(n)\f$ +* @section qssmoother_mor1 Moving Outlier Removal Filter O(n) * -* This filter is used to detect and remove outliers in the input signal. For -* each sample of input signal, the filter computes the median of the moving -* window. If a new incoming sample deviates from the median by a specified -* margin \f$\alpha\f$, it is replaced by the moving median \f$\overline{m(k)} \f$. +* This filter detects and removes outliers in the input signal by computing the +* median of a moving window for each sample. If a new incoming sample deviates +* from the median by a specified margin \f$\alpha\f$, it is replaced by the +* moving median \f$\overline{m(k)} \f$. * * This filter uses the following criteria for outlier removal: * +*
* \f$ y(k) = \overline{m(k)} \rightarrow \text{if} \rightarrow | \overline{m(k)} - x(k) | > \alpha|\overline{m(k)}| \f$ * * \f$ \text{else}=x(k) \f$ +*
* * This filter has a time complexity of \f$O(n)\f$ * -* Example: setting up an outlier removal filter: +* @subsection qssmoother_ex5 Example: setting up an outlier removal filter: * @code{.c} -* #define N_MWM ( 10u ) +* #define N_MOR ( 10u ) * * qSSmoother_MOR1_t smoother; * float alpha = 0.8f; -* float m_window[ N_MWM ] = { 0.0f }; +* float m_window[ N_MOR ] = { 0.0f }; * int ret; * -* ret = qSSmoother_Setup( &smoother, QSSMOOTHER_TYPE_MOR1, &alpha, m_window, N_MWM ); +* ret = qSSmoother_Setup( &smoother, QSSMOOTHER_TYPE_MOR1, &alpha, m_window, N_MOR ); * if ( 0 == ret ) { * // error, smooth filter cant be configured * } * @endcode * * -* - Moving Outliar Removal Filter \f$O(1)\f$ +* @section qssmoother_mor2 Moving Outlier Removal Filter O(1) * * Similar to the previous filter, but with a constant time complexity of  \f$O(1)\f$ +* by using a \ref qtdl data structure. * -* Example: setting up an outlier removal filter: +* @subsection qssmoother_ex6 Example: setting up an outlier removal filter: * @code{.c} * #define N_MWM ( 10u ) * @@ -188,24 +195,24 @@ * @endcode * * -* - Gaussian Filter +* @section qssmoother_gmwf Gaussian Filter * * This filter uses a kernel for smoothing, which defines the shape of the function * that is used to take the average of the neighboring points. A Gaussian kernel -* is a kernel with the shape of a Gaussian curve with a normal distribution. +* has the shape of a Gaussian curve with a normal distribution. * * The coefficients of the kernel are computed from the following equation: * -* \f$ w(n)=e^{\frac{1}{2}( \alpha\frac{ n }{ (L-1)/2} )^2} = e^{-n^{2}/2\sigma^2 }\f$ +*
\f$ w(n)=e^{\frac{1}{2}( \alpha\frac{ n }{ (L-1)/2} )^2} = e^{-n^{2}/2\sigma^2 }\f$
* -* were \f$ -(L-1)/2 \leq n \leq (L-1)/2 \f$ and \f$\alpha\f$ is inversely proportional -* to the standard deviation, \f$\sigma\f$, of a Gaussian random variable. The exact -* correspondence with the standard deviation of a Gaussian probability density +* were \f$ -(L-1)/2 \leq n \leq (L-1)/2 \f$ and \f$\alpha\f$ is inversely proportional +* to the standard deviation, \f$\sigma\f$, of a Gaussian random variable. The exact +* correspondence with the standard deviation of a Gaussian probability density * function is \f$ \sigma = (L – 1)/(2\alpha)\f$ . * -* This filter has a time complexity of \f$O(n)\f$ +* This filter has a time complexity of \f$O(n)\f$ * -* Example: setting up a gaussian filter: +* @subsection qssmoother_ex7 Example: setting up a gaussian filter: * @code{.c} * #define SMOOTHER_WINDOW_SIZE ( 10 ) * #define SMOOTHER_SAMPLE_TIME ( 100 ) @@ -223,32 +230,27 @@ * @endcode * * -* - Exponential Weighting filter +* @section qssmoother_expw Exponential Weighting filter * -* The algorithm of this filter computes a weighting factor and applies it to the input -* signal recursively. As the age of the data increases, the magnitude of the -* weighting factor decreases exponentially and never reaches zero. In other words, -* the recent data has more influence on the statistic at the current sample than -* the older data. +* This filter uses a weighting factor that is computed and applied to the input +* signal in a recursive manner. The weighting factor decreases exponentially as +* the age of the data increases, but never reaches zero, meaning more recent +* data has more influence on the current sample than older data * * The value of the forgetting factor \f$\lambda\f$ determines the rate of change -* of the weighting factor. A forgetting factor of 0.9 gives more weight to the -* older data than a forgetting factor of 0.1. To give more weight to the -* recent data, move the forgetting factor closer to 0. For detecting small -* shifts in rapidly varying data, a smaller value (below 0.5) is more suitable. -* A forgetting factor of 1.0 indicates infinite memory. All the previous samples -* are given equal weight. The optimal value for the forgetting factor depends -* on the data stream. -* -* The moving average algorithm updates the weight and computes the moving +* of the weighting factor. A smaller value (below 0.5) gives more weight to +* recent data, while a value of 1.0 indicates infinite memory. The optimal +* value for the forgetting factor depends on the data stream. +* +* The moving average algorithm updates the weight and computes the moving * average recursively for each data sample that comes in by using the following -* recursive equations. +* recursive equations: * -* \f$ w(t) = \lambda w(t-1) + 1 \f$ +*
\f$ w(t) = \lambda w(t-1) + 1 \f$
* -* \f$ y(t) = ( 1-\frac{1}{w(t)} )y(t-1) + \frac{1}{w(t)}x(t)\f$ +*
\f$ y(t) = ( 1-\frac{1}{w(t)} )y(t-1) + \frac{1}{w(t)}x(t)\f$
* -* Example: setting up an exponential weighting filter: +* @subsection qssmoother_ex8 Example: setting up an exponential weighting filter: * @code{.c} * qSSmoother_EXPW_t smoother; * float lambda = 0.8f; @@ -261,12 +263,12 @@ * @endcode * * -* - Scalar Kalman filter +* @section qssmoother_klmn Scalar Kalman filter * * The Kalman Filter is an efficient optimal estimator that provides a recursive -* computational methodology for estimating the state of a data from measurements -* that are typically noisy, while providing an estimate of the uncertainty of -* the estimate. Here, the scalar version (one-dimensional) is provided +* computational methodology for getting the state of a signal from measurements +* that are typically noisy, while providing an estimate of the uncertainty of +* the estimate. Here, the scalar or one-dimensional version is provided * and only three design parameters are required, the initial covariance \f$P(0)\f$, * the signal noise covariance \f$Q\f$ and the measurement uncertainty \f$r\f$. * @@ -274,27 +276,27 @@ * * predict * -* \f$ x(k) = Ax(k-1)\f$ +*
\f$ x(k) = Ax(k-1)\f$
* -* \f$ P(k) = A^{2}P(k-1) + Q\f$ +*
\f$ P(k) = A^{2}P(k-1) + Q\f$
* * compute the Kalman gain * -* \f$ K(k) = \frac{P(k)H}{r + H P(k) H} \f$ +*
\f$ K(k) = \frac{P(k)H}{r + H P(k) H} \f$
* * update the state and the state uncertainty * -* \f$ x(k) = x(k-1) + K(k)( u(k) - H x(k) ) \f$ +*
\f$ x(k) = x(k-1) + K(k)( u(k) - H x(k) ) \f$
* -* \f$ P(k) = ( 1 - K(k)H )P(k-1) \f$ +*
\f$ P(k) = ( 1 - K(k)H )P(k-1) \f$
* * Where * -* \f$A\f$ is the state transition model, \f$H\f$ the observation model, \f$x(k)\f$ -* the filter output (state estimation), \f$P(k)\f$ the covariance of the predicted -* state, \f$u(k)\f$ the signal measurement and \f$K(k)\f$ the kalman gain. +* \f$A\f$ is the state transition model, \f$H\f$ the observation model, \f$x(k)\f$ +* the filter output (state estimation), \f$P(k)\f$ the covariance of the predicted +* state, \f$u(k)\f$ the signal measurement and \f$K(k)\f$ the kalman gain. * -* Example: setting up a scalar kalman filter: +* @subsection qssmoother_ex9 Example: setting up a scalar Kalman filter: * @code{.c} * qSSmoother_KLMN_t smoother; * float param[] = { 100.0f, 100.0f, 9.0f }; // [initial covariance, noise covariance, measurement uncertainty] @@ -307,16 +309,85 @@ * @endcode * * -* * Example of signal smoothing +* @section qssmoother_desf Double Exponential Smoothing +* +* Is a time series forecasting technique used to analyze and predict trends in +* data. It is an extension of simple exponential smoothing that incorporates +* trend information into the forecast. +* +* The double exponential smoothing filter calculates two smoothing coefficients, +* one for the level of the series and one for the trend. These coefficients are +* used to give more weight to recent observations and dampen the effect of +* older observations. The level and trend are then updated using the following +* equations: +* +* Level: +*
\f$ L(t) = \alpha x(t) + ( 1 - \alpha ) [ L(t-1) + T(t-1) ] \f$
+* +* Trend: +*
\f$ T(t) = \beta [ L(t-1) + T(t-1) ] + ( 1 - \beta ) T(t-1) \f$
+* +* where \f$\alpha\f$ and \f$\beta\f$ are the smoothing coefficients for the +* level and trend, respectively. +* +* The double exponential smoothing filter can be used to generate forecasts by +* extrapolating the level and trend components into the future. The forecast for +* time \f$t+k\f$ is given by: +* +*
\f$ F(t+k) = L(t) + kT(t) \f$
+* +* Where \f$k\f$ is the number of time periods into the future. +* +* Overall, double exponential smoothing is a simple but effective technique +* for forecasting time series data with trends. It can be easily implemented 3 +* and provides accurate forecasts for short-term predictions. However, it may +* not perform well for longer-term forecasts or for data with complex seasonal +* patterns. +* +* @section qssmoother_alnf Adaptive Linear Filter +* +* It is a digital filter that adjusts its parameters based on the +* characteristics of the input signal and the desired output signal. +* +* The adaptive linear filter works by using an algorithm to iteratively adjust +* the filter coefficients in response to changes in the input signal \f$x(t)\f$. +* The algorithm seeks to minimize the difference between the filter output and +* the desired output, known as the error signal \f$ e(t)\f$. This is done by +* adjusting the filter coefficients \f$w_i(t)\f$ in the direction that reduces +* the error signal. For this, the Least Mean Squares (LMS) algorithm is being +* used. The LMS algorithm updates the filter coefficients by multiplying the +* error signal by the input signal and a small step size parameter, and then +* adding this product to the existing filter coefficients. +* +* It is particularly useful in situations where the characteristics of the +* input signal are unknown or change over time, as it can adapt to these changes +* and continue to provide accurate filtering or modeling. +* +* One potential drawback of the adaptive linear filter is that it can be +* computationally intensive, especially if the input signal is large or the +* filter has many coefficients. Additionally, the performance of the filter +* can be sensitive to the choice of step size parameter, which must be +* carefully chosen to balance convergence speed with stability. +* +* The adaptation rule for every filter coefficient is given by +*
\f$ \Delta w(t) = \alpha e(t) x(t) + \mu \Delta w(t-1) \f$
+* +*
\f$ w(t) = w(t-1) x(t) + \Delta w(t) \f$
+* +* The equation also includes a momentum term that adds a fraction of the +* previous coefficient update to the current coefficient update. This can help +* to reduce oscillations in the filter output and improve convergence speed. +* +* @section qssmoother_ex10 Example of signal smoothing * -* The example below is a rough draft of how a smoothing filter might be used. -* The example consists of smoothing a speed sensor signal using a Gaussian -* filter to improve quality and remove noise from it. The filter is initially -* configured by defining the instance and design parameters, then the filter is -* configured. To execute the filter, a task is launched at a sampling rate of -* 100mS. In this routine, the signal is sampled and the filter is executed. -* The enhanced signal is then properly processed without having to worry about -* noise and disturbances. +* The following is an example of how a smoothing filter can be implemented. In +* this scenario, a Gaussian filter is used to smooth a speed sensor signal, +* reducing noise and improving overall signal quality. The filter is set up by +* defining the necessary parameters and instantiating the filter. The filter is +* then executed at a sampling rate of 100 milliseconds. This process involves +* sampling the signal and running it through the filter, resulting in a cleaned +* and enhanced signal that can be further processed without the need to account +* for noise and disturbances. * * @code{.c} * #include @@ -343,7 +414,7 @@ * } * } * -* int main( int argc, char *argv[] ) +* int main( int argc, char *argv[] ) * { * qSSmoother_GMWF_t smoother; * float win_kernel[ 2*SMOOTHER_WINDOW_SIZE ]; //storage for the window and the kernel diff --git a/doc/qtdl.dox b/doc/qtdl.dox index ec11fa0..e1ffa9d 100644 --- a/doc/qtdl.dox +++ b/doc/qtdl.dox @@ -23,7 +23,7 @@ * Given the applications of this structure, the \ref qtdl class is used as the * base component of some aspects of \ref qssmoother and \ref qltisys. * -* Example : Code snipped to instantiate a TDL to hold up to 256 delays. +* @section qtdl_ex1 Example : Code snippet to instantiate a TDL to hold up to 256 delays. * * @code{.c} * #define MAX_DELAYS ( 256 ) diff --git a/doc/stylesheet b/doc/stylesheet index a3c119b..af1d903 160000 --- a/doc/stylesheet +++ b/doc/stylesheet @@ -1 +1 @@ -Subproject commit a3c119b4797be2039761ec1fa0731f038e3026f6 +Subproject commit af1d9030b3ffa7b483fa9997a7272fb12af6af4c diff --git a/include/qbitfield.h b/include/qbitfield.h index 75637c5..f7a0d46 100644 --- a/include/qbitfield.h +++ b/include/qbitfield.h @@ -14,10 +14,10 @@ extern "C" { #endif #include - #include + #include /** @addtogroup qbitfield A Bit-Field manipulation library - * @brief API for the Bit-Field manipulation libraryr + * @brief API for the Bit-Field manipulation library * @{ */ @@ -38,7 +38,7 @@ extern "C" { /** * @brief Use to determine the uint8_t array-size for a BitField. */ - #define QBITFIELD_SIZE( NBITS ) ( 4* ( ( ( NBITS-1 ) / 32 ) + 1 ) ) + #define QBITFIELD_SIZE( NBITS ) ( 4 * ( ( ( (NBITS)-1 ) / 32 ) + 1 ) ) /** * @brief Setup a initialize a BitField instance. @@ -165,7 +165,7 @@ extern "C" { * @param[in] dst Pointer to the destination array where the content is to * be copied. * @param[in] n Number of bytes to copy. - * @return Destination is returned on success, otherwise NULL. + * @return Destination is returned on success, otherwise @c NULL. */ void* qBitField_Dump( const qBitField_t * const b, void * const dst, diff --git a/include/qcrc.h b/include/qcrc.h index 1a4a8c3..f34053d 100644 --- a/include/qcrc.h +++ b/include/qcrc.h @@ -1,9 +1,9 @@ /*! * @file qcrc.h * @author J. Camilo Gomez C. - * @version 1.03 + * @version 1.04 * @note This file is part of the qLibs distribution. - * @brief CRC calculation library. + * @brief CRC calcULation library. **/ #ifndef QCRC_H @@ -16,32 +16,32 @@ extern "C" { #include #include - /** @addtogroup qcrc Generic Cyclic Redundancy Check (CRC) calculator - * @brief API for the qCRC Generic Cyclic Redundancy Check (CRC) calculator + /** @addtogroup qcrc Generic Cyclic Redundancy Check (CRC) calcULator + * @brief API for the qCRC Generic Cyclic Redundancy Check (CRC) calcULator * @{ */ /** * @brief Enumeration with all the supported cyclic redundancy checks - */ + */ typedef enum { QCRC8 = 0, /*!< 8-Bit Cyclic Redundancy Check*/ QCRC16, /*!< 16-Bit Cyclic Redundancy Check*/ - QCRC32, /*!< 32-Bit Cyclic Redundancy Check*/ + QCRC32 /*!< 32-Bit Cyclic Redundancy Check*/ } qCRC_Mode_t; /** - * @brief Calculates in one pass the common @a width bit CRC value for a + * @brief CalcULates in one pass the common @a width bit CRC value for a * block of data that is passed to the function together with a parameter * indicating the @a length. - * @param[in] mode To select the CRC calculation mode. Only the following - * values are supported: QCRC8, QCRC16 and QCRC32. + * @param[in] mode To select the CRC calcULation mode. Only the following + * values are supported: ::QCRC8, ::QCRC16 and ::QCRC32. * @param[in] pData A pointer to the block of data. * @param[in] length The number of bytes in @a data. * @param[in] poly CRC polynomial value. * @param[in] init CRC initial value. * @param[in] refIn If true, the input data is reflected before processing. - * @param[in] refOut If true, the CRC result is reflected before output. + * @param[in] refOut If true, the CRC resULt is reflected before output. * @param[in] xorOut The final XOR value. * @return The CRC value for @a data. */ @@ -54,6 +54,216 @@ extern "C" { const uint8_t refOut, uint32_t xorOut ); + /** @brief CRC-8 with poly = 0x07 init = 0x00 refIn = false refOut = false + * xorOut= 0x00 */ + #define qCRC8( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x07UL, 0x00UL, 0U, 0U, 0x00UL ); \ + + /** @brief CRC-8/CDMA2000 with poly = 0x9B init = 0xFF refIn = false + * refOut = false xorOut= 0x00 */ + #define qCRC8_CDMA2000( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x9BUL, 0xFFUL, 0U, 0U, 0x00UL ); \ + + /** @brief CRC-8/CDMA2000 with poly = 0x39 init = 0x00 refIn = true + * refOut = true xorOut= 0x00 */ + #define qCRC8_DARC( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x39UL, 0x00UL, 1U, 1U, 0x00UL ); \ + + /** @brief CRC-8/CDMA2000 with poly = 0xD5 init = 0x00 refIn = false + * refOut = false xorOut= 0x00 */ + #define qCRC8_DVB_S2( pData, length ) \ + qCRCx( QCRC8, pData, length, 0xD5UL, 0x00UL, 0U, 0U, 0x00UL ); \ + + /** @brief CRC-8/EBU with poly = 0x1D init = 0xFF refIn = true refOut = true + * xorOut= 0x00 */ + #define qCRC8_EBU( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x1DUL, 0xFFUL, 1U, 1U, 0x00UL ); \ + + /** @brief CRC-8/I-CODE with poly = 0x1D init = 0xFD refIn = false + * refOut = false xorOut= 0x00 */ + #define qCRC8_I_CODE( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x1DUL, 0xFDUL, 0U, 0U, 0x00UL ); \ + + /** @brief CRC-8/ITU with poly = 0x07 init = 0x00 refIn = false + * refOut = false xorOut= 0x55 */ + #define qCRC8_ITU( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x07UL, 0x00UL, 0U, 0U, 0x55UL ); \ + + /** @brief CRC-8/MAXIM with poly = 0x31 init = 0x00 refIn = true + * refOut = true xorOut= 0x00 */ + #define qCRC8_MAXIM( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x31UL, 0x00UL, 1U, 1U, 0x00UL ); \ + + /** @brief CRC-8/ROHC with poly = 0x07 init = 0xFF refIn = true + * refOut = true xorOut= 0x00 */ + #define qCRC8_ROHC( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x07UL, 0xFFUL, 1U, 1U, 0x00UL ); \ + + /** @brief CRC-8/WCDMA with poly = 0x9B init = 0x00 refIn = true + * refOut = true xorOut= 0x00 */ + #define qCRC8_WCDMA( pData, length ) \ + qCRCx( QCRC8, pData, length, 0x9BUL, 0x00UL, 1U, 1U, 0x00UL ); \ + + /** @brief CRC-16/CCITT-FALSE with poly = 0x1021 init = 0xFFFF + * refIn = false refOut = false xorOut= 0x0000 */ + #define qCRC16_CCITT_FALSE( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xFFFFUL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/ARC with poly = 0x8005 init = 0x0000 refIn = true r + * efOut = true xorOut= 0x0000 */ + #define qCRC16_ARC( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0x0000UL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/AUG-CCITT with poly = 0x1021 init = 0x1D0F refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_AUG_CCITT( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0x1D0FUL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/BUYPASS with poly = 0x8005 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_BUYPASS( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0x0000UL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/CDMA2000 with poly = 0xC867 init = 0xFFFF refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_CDMA2000( pData, length ) \ + qCRCx( QCRC16, pData, length, 0xC867UL, 0xFFFFUL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/DDS-110 with poly = 0x8005 init = 0x800D refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_DDS_110( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0x800DUL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/DECT-R with poly = 0x0589 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0001 */ + #define qCRC16_DECT_R( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x0589UL, 0x0000UL, 0U, 0U, 0x0001UL ); \ + + /** @brief CRC-16/DECT-X with poly = 0x0589 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_DECT_X( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x0589UL, 0x0000UL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/DNP with poly = 0x3D98 init = 0x0000 refIn = true + * refOut = true xorOut= 0xFFFF */ + #define qCRC16_DNP( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x3D65UL, 0x0000UL, 1U, 1U, 0xFFFFUL ); \ + + /** @brief CRC-16/EN-13757 with poly = 0x3D65 init = 0x0000 refIn = false + * refOut = false xorOut= 0xFFFF */ + #define qCRC16_EN_13757( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x03D65UL, 0x0000UL, 0U, 0U, 0xFFFFUL ); \ + + /** @brief CRC-16/GENIBUS with poly = 0x1021 init = 0xFFFF refIn = false + * refOut = false xorOut= 0xFFFF */ + #define qCRC16_GENIBUS( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xFFFFUL, 0U, 0U, 0xFFFFUL ); \ + + /** @brief CRC-16/MAXIM with poly = 0x8005 init = 0x0000 refIn = true + * refOut = true xorOut= 0xFFFF */ + #define qCRC16_MAXIM( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0x0000UL, 1U, 1U, 0xFFFFUL ); \ + + /** @brief CRC-16/MCRF4XX with poly = 0x1021 init = 0xFFFF refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_MCRF4XX( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xFFFFUL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/RIELLO with poly = 0x1021 init = 0xB2AA refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_RIELLO( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xB2AAUL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/DECT-X with poly = 0x8bb7 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_T10_DIF( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8BB7UL, 0x0000UL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/TELEDISK with poly = 0xA097 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_TELEDISK( pData, length ) \ + qCRCx( QCRC16, pData, length, 0xA097UL, 0x0000UL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-16/TMS37157 with poly = 0x1021 init = 0x89EC refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_TMS37157( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0x89ECUL, 1U, 1U, 0x0000UL ); \ + +/** @brief CRC-16/USB with poly = 0x8005 init = 0xFFFF refIn = true + * refOut = true xorOut= 0xFFFF */ + #define qCRC16_USB( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0xFFFFUL, 1U, 1U, 0xFFFFUL ); \ + + /** @brief CRC-A with poly = 0x1021 init = 0xC6C6 refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_A( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xC6C6UL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/KERMIT with poly = 0x1021 init = 0x0000 refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_KERMIT( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0x0000UL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/MODBUS with poly = 0x8005 init = 0xFFFF refIn = true + * refOut = true xorOut= 0x0000 */ + #define qCRC16_MODBUS( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x8005UL, 0xFFFFUL, 1U, 1U, 0x0000UL ); \ + + /** @brief CRC-16/X-25 with poly = 0x1021 init = 0xFFFF refIn = true + * refOut = true xorOut= 0xFFFF */ + #define qCRC16_X_25( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0xFFFFUL, 1U, 1U, 0xFFFFUL ); \ + + /** @brief CRC-16/XMODEM with poly = 0x1021 init = 0x0000 refIn = false + * refOut = false xorOut= 0x0000 */ + #define qCRC16_XMODEM( pData, length ) \ + qCRCx( QCRC16, pData, length, 0x1021UL, 0x0000UL, 0U, 0U, 0x0000UL ); \ + + /** @brief CRC-32 with poly = 0x04C11DB7 init = 0xFFFFFFFF refIn = true + * refOut = true xorOut= 0xFFFFFFFF */ + #define qCRC32( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x04C11DB7UL, 0xFFFFFFFFUL, 1U, 1U, 0xFFFFFFFFUL ); \ + + /** @brief CRC-32/BZIP2 with poly = 0x04C11DB7 init = 0xFFFFFFFF + * refIn = false refOut = false xorOut= 0xFFFFFFFF */ + #define qCRC32_BZIP2( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x04C11DB7UL, 0xFFFFFFFFUL, 0U, 0U, 0xFFFFFFFFUL ); \ + + /** @brief CRC-32C with poly = 0x1EDC6F41 init = 0xFFFFFFFF refIn = true + * refOut = true xorOut= 0xFFFFFFFF */ + #define qCRC32_C( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x1EDC6F41UL, 0xFFFFFFFFUL, 1U, 1U, 0xFFFFFFFFUL ); \ + + /** @brief CRC-32D with poly = 0xA833982B init = 0xFFFFFFFF refIn = true + * refOut = true xorOut= 0xFFFFFFFF */ + #define qCRC32_D( pData, length ) \ + qCRCx( QCRC32, pData, length, 0xA833982BUL, 0xFFFFFFFFUL, 1U, 1U, 0xFFFFFFFFUL ); \ + + /** @brief CRC-32/JAMCRC with poly = 0x04C11DB7 init = 0xFFFFFFFF + * refIn = true refOut = true xorOut= 0x00000000 */ + #define qCRC32_JAMCRC( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x04C11DB7UL, 0xFFFFFFFFUL, 1U, 1U, 0x00000000UL ); \ + + /** @brief CRC-32/MPEG2 with poly = 0x04C11DB7 init = 0xFFFFFFFF + * refIn = false refOut = false xorOut= 0x00000000 */ + #define qCRC32_MPEG2( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x04C11DB7UL, 0xFFFFFFFFUL, 0U, 0U, 0x00000000UL ); \ + + /** @brief CRC-32/POSIX with poly = 0x04C11DB7 init = 0x00000000 + * refIn = false refOut = false xorOut= 0xFFFFFFFF */ + #define qCRC32_POSIX( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x04C11DB7UL, 0x00000000UL, 0U, 0U, 0xFFFFFFFFUL ); \ + + /** @brief CRC-32Q with poly = 0x814141AB init = 0x00000000 + * refIn = false refOut = false xorOut= 0x00000000 */ + #define qCRC32_Q( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x814141ABUL, 0x00000000UL, 0U, 0U, 0x00000000UL ); \ + + /** @brief CRC-32/XFER with poly = 0x000000AF init = 0x00000000 + * refIn = false refOut = false xorOut= 0x00000000 */ + #define qCRC32_XFER( pData, length ) \ + qCRCx( QCRC32, pData, length, 0x000000AFUL, 0x00000000UL, 0U, 0U, 0x00000000UL ); \ + /** @}*/ #ifdef __cplusplus diff --git a/include/qffmath.h b/include/qffmath.h new file mode 100644 index 0000000..1a006ce --- /dev/null +++ b/include/qffmath.h @@ -0,0 +1,719 @@ +/*! + * @file qffmath.h + * @author J. Camilo Gomez C. + * @version 1.06 + * @note This file is part of the qLibs distribution. + * @brief Fast floating-point math library for applications where speed is more + * important than accuracy + **/ + +#ifndef QFFMATH_H +#define QFFMATH_H + +#ifdef __cplusplus +extern "C" { +#endif + + #include + #include + #include + +#ifdef QLIBS_USE_STD_MATH + /*! @cond */ + #include + #define QLIB_ABS fabsf + #define QLIB_COS cosf + #define QLIB_POW powf + #define QLIB_EXP expf + #define QLIB_LOG logf + #define QLIB_SQRT sqrtf + #define QLIB_ISNAN isnan + #define QLIB_ISINF isinf + #define QLIB_MAX fmaxf + #define QLIB_MIN fminf + #define QLIB_NAN NAN + #define QLIB_MOD fmodf + #define QLIB_ROUND roundf + /*! @endcond */ +#else + /*! @cond */ + #define QLIB_ABS qFFMath_Abs + #define QLIB_COS qFFMath_Cos + #define QLIB_POW qFFMath_Pow + #define QLIB_EXP qFFMath_Exp + #define QLIB_LOG qFFMath_Log + #define QLIB_SQRT qFFMath_Sqrt + #define QLIB_ISNAN qFFMath_IsNaN + #define QLIB_ISINF qFFMath_IsInf + #define QLIB_MAX qFFMath_Max + #define QLIB_MIN qFFMath_Min + #define QLIB_NAN QFFM_NAN + #define QLIB_FMOD qFFMath_Mod + #define QLIB_ROUND qFFMath_Round + /*! @endcond */ + + /** @addtogroup qffmath qFFMath + * @brief Fast floating-point math library for applications where speed is + * more important than accuracy + * @{ + */ + + /*! @cond */ + float _qFFMath_GetAbnormal( const int i ); //skipcq: CXX-E2000 + /*! @endcond */ + + /** @brief The base of natural logarithms ( e ) given as a single-precision floating-point number*/ + #define QFFM_E ( 2.7182818284590452354F ) + /** @brief The base 2 logarithm of e ( log_2 e ) given as a single-precision floating-point number */ + #define QFFM_LOG2E ( 1.4426950408889634074F ) + /** @brief The base 10 logarithm of e ( log_10 e ) given as a single-precision floating-point number */ + #define QFFM_LOG10E ( 0.43429448190325182765F ) + /** @brief The natural logarithm of 2 ( ln 2 ) given as a single-precision floating-point number */ + #define QFFM_LN2 ( 0.69314718055994530942F ) + /** @brief The natural logarithm of 10 ( ln 10 ) given as a single-precision floating-point number */ + #define QFFM_LN10 ( 2.30258509299404568402F ) + /** @brief The circumference of a circle with diameter 1, ( π ) given as a single-precision floating-point number */ + #define QFFM_PI ( 3.14159265358979323846F ) + /** @brief Half of π ( π/2 ) given as a single-precision floating-point number */ + #define QFFM_PI_2 ( 1.57079632679489661923F ) + /** @brief A quarter of π ( π/4 ) given as a single-precision floating-point number */ + #define QFFM_PI_4 ( 0.78539816339744830962F ) + /** @brief The inverse of π ( 1/π ) given as a single-precision floating-point number */ + #define QFFM_1_PI ( 0.31830988618379067154F ) + /** @brief Twice the inverse of π ( 2/π ) given as a single-precision floating-point number */ + #define QFFM_2_PI ( 0.63661977236758134308F ) + /** @brief The inverse of the square root of π ( 2/√π ) given as a single-precision floating-point number */ + #define QFFM_2_SQRTPI ( 1.12837916709551257390F ) + /** @brief The square root of 2 ( √2 ) given as a single-precision floating-point number */ + #define QFFM_SQRT2 ( 1.41421356237309504880F ) + /** @brief The inverse of square root of 2 ( 1/√2 ) given as a single-precision floating-point number */ + #define QFFM_SQRT1_2 ( 0.70710678118654752440F ) + /** @brief The natural logarithm of the square root of 2π given as a single-precision floating-point number */ + #define QFFM_LN_SQRT_2PI ( 0.918938533204672669540968854562379419F ) + + /** @brief The maximum value of a non-infinite single-precision floating-point number */ + #define QFFM_MAXFLOAT ( 3.40282347e+38F ) + /** @brief Positive infinity given as a single-precision floating-point number */ + #define QFFM_INFINITY _qFFMath_GetAbnormal( 0 ) + /** @brief Not a Number (NaN) given as a single-precision floating-point number */ + #define QFFM_NAN _qFFMath_GetAbnormal( 1 ) + + /** @brief Indicates that the value is positive or negative zero */ + #define QFFM_FP_ZERO ( 0 ) + + /** @brief Indicates that the value is subnormal */ + #define QFFM_FP_SUBNORMAL ( 1 ) + + /** @brief Indicates that the value is normal, i.e. not an infinity, + * subnormal, not-a-number or zero*/ + #define QFFM_FP_NORMAL ( 2 ) + + /** @brief Indicates that the value is not representable by the underlying + * type, positive or negative infinity*/ + #define QFFM_FP_INFINITE ( 3 ) + + /** @brief Indicates that the value is not-a-number NaN */ + #define QFFM_FP_NAN ( 4 ) + + /** + * @brief Categorizes the floating-point number @a x. This function + * determines whether its argument is a normal floating-point number, or one + * of several special categories of values, including NaN (not a number), + * infinity, subnormal floating-point values or zero. To determine what + * category the argument belongs to, compare the return value with the + * any of the following number classification macros: + * - #QFFM_FP_ZERO + * - #QFFM_FP_SUBNORMAL + * - #QFFM_FP_NORMAL + * - #QFFM_FP_INFINITE + * - #QFFM_FP_NAN + * @param[in] f The number you want to test. + * @return One of #QFFM_FP_INFINITE, #QFFM_FP_NAN, #QFFM_FP_NORMAL, + * #QFFM_FP_SUBNORMAL, or #QFFM_FP_ZERO, specifying the category of @a x. + */ + int qFFMath_FPClassify( const float f ); + + /** + * @brief Determine if @a x is Not-A-Number (NaN) aka #QFFM_NAN. + * @param[in] x The number you want to test. + * @return true if the value of @a x is (NaN) aka #QFFM_NAN, otherwise + * returns false. + */ + bool qFFMath_IsNaN( const float x ); + + /** + * @brief Determine if @a x is Infinity. + * @param[in] x The number you want to test. + * @return true if the value of @a x is ±Infinity, otherwise returns false. + */ + bool qFFMath_IsInf( const float x ); + + /** + * @brief Determines if the given floating point number @a x has finite + * value i.e. it is normal, subnormal or zero, but not infinite or NaN. + * @param[in] x The number you want to test. + * @return true if @a x has a finite value, false otherwise + */ + bool qFFMath_IsFinite( const float x ); + + /** + * @brief Determines if the given floating point number @a x is normal, i.e. + * is neither zero, subnormal, infinite, or NaN. + * @param[in] x The number you want to test. + * @return true if @a x has a normal value, false otherwise + */ + bool qFFMath_IsNormal( const float x ); + + /*! @cond */ + bool qFFMath_IsEqual( const float a, + const float b ); + + bool qFFMath_IsAlmostEqual( const float a, + const float b, + const float tol ); + /*! @endcond */ + + /** + * @brief Computes the absolute value of a floating point value @a x. + * @param[in] x The floating point value + * @return The absolute value of @a x + */ + float qFFMath_Abs( float x ); + + /** + * @brief Computes the multiplicative inverse or reciprocal for the value + * @a x, denoted by @c 1/x or x^(−1) + * @param[in] x The floating point value + * @return The reciprocal value of @a x + */ + float qFFMath_Recip( float x ); + + /** + * @brief Computes the square-root of @a x + * @param[in] x The floating point value + * @return If no errors occur, square root of @a x, is returned. If a domain + * error occurs #QFFM_NAN is returned + */ + float qFFMath_Sqrt( float x ); + + /** + * @brief Computes the reciprocal square-root of @a x denoted as + * 1/sqrt(x) + * @param[in] x The floating point value + * @return If no errors occur, the reciprocal square root of @a x, is + * returned. If a domain error occurs #QFFM_NAN is returned + */ + float qFFMath_RSqrt( float x ); + + /** + * @brief Computes the cubic-root of @a x + * @param[in] x The floating point value + * @return If no errors occur, cubic root of @a x, is returned. If a domain + * error occurs #QFFM_NAN is returned + */ + float qFFMath_Cbrt( float x ); + + /** + * @brief Computes the reciprocal cubic-root of @a x denoted as + * 1/cbrt(x) + * @param[in] x The floating point value + * @return If no errors occur, the reciprocal cubic root of @a x, is + * returned. If a domain error occurs #QFFM_NAN is returned + */ + float qFFMath_RCbrt( float x ); + + /** + * @brief Computes the nearest integer value to @a x (in floating-point + * format), rounding halfway cases away from zero. + * @param[in] x The floating point value + * @return The nearest integer value to @a x, rounding halfway cases away + * from zero + */ + float qFFMath_Round( float x ); + + /** + * @brief Computes the largest integer value not greater than @a x. + * @param[in] x The floating point value + * @return The largest integer value not greater than @a x + */ + float qFFMath_Floor( float x ); + + /** + * @brief Computes the smallest integer value not less than @a x. + * @param[in] x The floating point value + * @return The smallest integer value not less than @a x + */ + float qFFMath_Ceil( float x ); + + /** + * @brief Computes the nearest integer not greater in magnitude than @a x. + * @param[in] x The floating point value + * @return The nearest integer value not greater in magnitude than @a x + * (in other words, @a x rounded towards zero) + */ + float qFFMath_Trunc( float x ); + + /** + * @brief Obtain the fractional part of @a x. + * @param[in] x The floating point value + * @return The fractional part of @a x + */ + float qFFMath_Frac( float x ); + + /** + * @brief Computes the IEEE remainder of the floating point division + * operation @c x/y + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If successful, returns the IEEE floating-point remainder of the + * division @c x/y. If a domain error occurs, a #QFFM_NAN value is returned. + */ + float qFFMath_Remainder( float x, + float y ); + + /** + * @brief Computes the floating-point remainder of the division operation + * @c x/y + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If successful, returns the floating-point remainder of the + * division @c x/y. If a domain error occurs, a #QFFM_NAN value is returned. + */ + float qFFMath_Mod( float x, + float y ); + + /** + * @brief Computes the sine of @a x (measured in radians). + * @param[in] x The floating point value + * @return If no errors occur, the sine of @a x @c sin(x) in the range + * [-1 ; +1], is returned. If a domain error occurs, a #QFFM_NAN value is + * returned. + */ + float qFFMath_Sin( float x ); + + /** + * @brief Computes the cosine of @a x (measured in radians). + * @param[in] x The floating point value + * @return If no errors occur, the cosine of @a x @c cos(x) in the range + * [-1 ; +1], is returned. If a domain error occurs, a #QFFM_NAN value is + * returned. + */ + float qFFMath_Cos( float x ); + + /** + * @brief Computes the tangent of @a x (measured in radians). + * @param[in] x The floating point value + * @return If no errors occur, the tangent of @a x @c tan(x) is returned. If + * a domain error occurs, a #QFFM_NAN value is returned. + */ + float qFFMath_Tan( float x ); + + /** + * @brief Computes the principal value of the arc sine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the arc sine of @a x @c arcsin(x) in the range + * [-pi/2 ; pi/2]. is returned.If a domain error occurs, a #QFFM_NAN value is + * returned. + */ + float qFFMath_ASin( float x ); + + /** + * @brief Computes the principal value of the arc cosine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the arc cosine of @a x @c arccos(x) in the + * range [0 ; pi]. is returned.If a domain error occurs, a #QFFM_NAN value is + * returned. + */ + float qFFMath_ACos( float x ); + + /** + * @brief Computes the principal value of the arc tangent of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the arc sine of @a x @c arctan(x) in the range + * [-pi/2 ; pi/2]. is returned.If a domain error occurs, a #QFFM_NAN value is + * returned. + */ + float qFFMath_ATan( float x ); + + /** + * @brief Computes the arc tangent of y/x using the signs of arguments to + * determine the correct quadrant. + * @param[in] y The floating point value + * @param[in] x The floating point value + * @return If no errors occur, the arc tangent of @c y/x arctan(y/x) + * in the range [-pi ; +pi] radians, is returned. If a domain error occurs, + * a #QFFM_NAN value is returned. + */ + float qFFMath_ATan2( float y, + float x ); + + /** + * @brief Computes 2 raised to the given power @a x. + * @param[in] x The floating point value + * @return If no errors occur, the base-2 exponential of @a x 2^x is + * returned. If a range error due to overflow occurs, #QFFM_INFINITY is + * returned. + */ + float qFFMath_Exp2( float x ); + + /** + * @brief Computes the base 2 logarithm of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the base-2 logarithm of @a x @c log_2(x) is + * returned. If a domain error occurs, a #QFFM_NAN value is returned. If a + * pole error occurs, -#QFFM_INFINITY is returned. + */ + float qFFMath_Log2( float x ); + + /** + * @brief Computes the e (Euler's number, 2.7182818) raised to the given + * power @a x. + * @param[in] x The floating point value + * @return If no errors occur, the base-e exponential of @a x e^(x) + * is returned. If a range error due to overflow occurs, +#QFFM_INFINITY is + * returned. + */ + float qFFMath_Exp( float x ); + + /** + * @brief Computes the value of 10 raised to the power of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the base-e exponential of @a x 10^(x) + * is returned. If a range error due to overflow occurs, ± #QFFM_INFINITY or + * NAN is returned. + */ + float qFFMath_Exp10( float x ); + + /** + * @brief Computes the natural (base e) logarithm of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the natural (base-e) logarithm of @a x + * @c ln(x) is returned. If a domain error occurs, a #QFFM_NAN value is + * returned. If a pole error occurs, -#QFFM_INFINITY is returned. + */ + float qFFMath_Log( float x ); + + /** + * @brief Computes the common (base-10) logarithm of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the common (base-10) logarithm of @a x + * @c log_10(x) is returned. If a domain error occurs, a #QFFM_NAN value is + * returned. If a pole error occurs, -#QFFM_INFINITY is returned. + */ + float qFFMath_Log10( float x ); + + /** + * @brief Computes the value of @a b raised to the power @a e. + * @param[in] b Base as floating point value + * @param[in] e Exponent as floating point value + * @return If no errors occur, @a b raised to the power of @a e @c b^e is + * returned. If a domain error occurs, a #QFFM_NAN value is returned. If a + * pole error or a range error due to overflow occurs, ± #QFFM_INFINITY is + * returned. + */ + float qFFMath_Pow( float b, + float e ); + + /** + * @brief Computes hyperbolic sine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the hyperbolic sine of @a x @c sinh(x) is + * returned. If a range error occurs, a ± #QFFM_INFINITY is value is + * returned. + */ + float qFFMath_Sinh( float x ); + + /** + * @brief Computes hyperbolic cosine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the hyperbolic cosine of @a x @c cosh(x) is + * returned. If a range error occurs, a #QFFM_INFINITY value is returned. + */ + float qFFMath_Cosh( float x ); + + /** + * @brief Computes hyperbolic tangent of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the hyperbolic tangent of @a x @c tanh(x) is + * returned. + */ + float qFFMath_Tanh( float x ); + + /** + * @brief Computes the inverse hyperbolic sine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the inverse hyperbolic sine of @a x + * sinh^-1(x) is returned. + */ + float qFFMath_ASinh( float x ); + + /** + * @brief Computes the inverse hyperbolic cosine of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the inverse hyperbolic cosine of @a x + * cosh^-1(x) is returned. + */ + float qFFMath_ACosh( float x ); + + /** + * @brief Computes the inverse hyperbolic tangent of @a x. + * @param[in] x The floating point value + * @return If no errors occur, the inverse hyperbolic tangent of @a x + * tanh^-1(x) is returned. If a domain error occurs, a #QFFM_NAN + * value is returned. If a pole error occurs, ± #QFFM_INFINITY is returned. + */ + float qFFMath_ATanh( float x ); + + /** + * @brief Computes the error function of @a x. + * @param[in] x The floating point value + * @return If no errors occur, value the error function is returned. + */ + float qFFMath_Erf( float x ); + + /** + * @brief Computes the complementary error function of @a x. + * @param[in] x The floating point value + * @return If no errors occur, value the complementary error function is + * returned. + */ + float qFFMath_Erfc( float x ); + + /** + * @brief Returns the larger of two floating point arguments. + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If successful, returns the larger of two floating point values + */ + float qFFMath_Max( float x, + float y ); + + /** + * @brief Returns the smaller of two floating point arguments. + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If successful, returns the smaller of two floating point values + */ + float qFFMath_Min( float x, + float y ); + + /** + * @brief Decomposes given floating point value @a x into a normalized + * fraction and an integral power of two. + * @param[in] x The floating point value + * @param[in] pw2 Pointer to integer value to store the exponent to + * @return If @a x is zero, returns zero and stores zero in @a pw2. Otherwise + * (if @a x is not zero), if no errors occur, returns the value @a y in the + * range (-1;-0.5], [0.5; 1) and stores an integer value in @a pw2 such that + * y×2^(pw2) = x . If the value to be stored in @a pw2 is outside + * the range of an @c int, the behavior is unspecified. If @a x is not a + * floating-point number, the behavior is unspecified. + */ + float qFFMath_RExp( float x, + int32_t *pw2 ); + + /** + * @brief Multiplies a floating point value @a x by the number 2 raised to + * the @a pw2 power. + * @param[in] x The floating point value + * @param[in] pw2 Integer value + * @return If no errors occur, @a x multiplied by 2 to the power of @a pw2 + * x×2^pwd is returned. If a range error due to overflow occurs, + * ± #QFFM_INFINITY is returned. If a range error due to underflow occurs, + * the correct result (after rounding) is returned. + */ + float qFFMath_LDExp( float x, + int32_t pw2 ); + + /** + * @brief Computes the square root of the sum of the squares of @a x and @a y, + * without undue overflow or underflow at intermediate stages of the + * computation. + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If no errors occur, the hypotenuse of a right-angled triangle, + * sqrt(x^2 + y^2), is returned. If a range error due to overflow + * occurs, +#QFFM_INFINITY is returned. If a range error due to underflow + * occurs, the correct result (after rounding) is returned. + */ + float qFFMath_Hypot( float x, + float y ); + + /** + * @brief Returns the next representable value of @a x in the direction of + * @a y. If @a x equals to @a y, @a y is returned. + * @param[in] x The floating point value + * @param[in] y The floating point value + * @return If no errors occur, the next representable value of @a x in the + * direction of @a y is returned. If @a x equals @a y, then @a yis returned. + * If a range error due to overflow occurs, ± #QFFM_INFINITY is returned + * (with the same sign as @a x). If a range error occurs due to underflow, + * the correct result is returned. + */ + float qFFMath_NextAfter( float x, + float y ); + + /** + * @brief Computes the midpoint of the floating-points @a a and @a b. + * @param a A floating point value + * @param b A floating point value + * @return Half the sum of @a a and @a b. No overflow occurs. A at most + * one inexact operation occurs. + */ + float qFFMath_Midpoint( float a, + float b ); + + /** + * @brief Computes the linear interpolation between @a a and @a b, if + * the parameter t is inside [0, 1] (the linear extrapolation otherwise), + * i.e. the result of a+t*(b-a) with accounting for floating-point + * calculation imprecision. + * @param a A floating point value + * @param b A floating point value + * @param t A floating point value + * @return Return a+t*(b-a) . When both @a a and @a b are finite, the + * following properties are guaranteed: + * If @c t==0, the result is equal to @a a. + * If @c t==1, the result is equal to @a b. + * If @c t>=0 and @c t<=1, the result is finite. + * If @a t is finite and @c a==b, the result is equal to @a a. + * If @a t is finite or (a-b)!=0 with @a t infinity, the result + * is not @c nan. + * Let @c CMP(x,y) be @c 1 if @c x>y, @c -1 if @c x CMP(lerp(a,b,t2), + * lerp(a,b,t1), CMP(t2,t1) , and @c CMP(b,a) is non-negative. + * (That is, lerp() is monotonic.). + */ + float qFFMath_Lerp( float a, + float b, + float t ); + + /** + * @brief Normalize the given input @a x in value range given by @a xMin and + * @a xMax to value range between 0 and 1. + * @param[in] x Input + * @param[in] xMin Input minimum value for range + * @param[in] xMax Input maximum value for range + * @return The scaled value in range [0 - 1]. + */ + float qFFMath_Normalize( const float x, + const float xMin, + const float xMax ); + + /** + * @brief Scales the given input @a x in value range given by @a xMin and + * @a xMax to value range specified by the @a yMin and @a yMax. + * @param[in] x Input + * @param[in] xMin Input minimum value for range + * @param[in] xMax Input maximum value for range + * @param[in] yMin Output minimum value for range + * @param[in] yMax Output maximum value for range + * @return The scaled value in range @a yMin and @a yMax. + */ + float qFFMath_Map( const float x, + const float xMin, + const float xMax, + const float yMin, + const float yMax ); + + /** + * @brief Determines if the value pointed by @a x falls within a range + * specified by the upper limit and lower limit inputs and coerces the value + * to fall within the range + * @param[in,out] x Input + * @param[in] lowerL Lower limit. + * @param[in] upperL Upper limit. + * @return @c true when the value falls within the specified range, otherwise + * false + */ + bool qFFMath_InRangeCoerce( float * const x, + const float lowerL, + const float upperL ); + + /** + * @brief Determines if the point at ( @a x, @a y ) is inside the polygon + * given by the set of points on @a px and @a py. + * @param[in] x Point x-coordinate + * @param[in] y Point y-coordinate + * @param[in] px x-coordinate points of the polygon + * @param[in] py y-coordinate points of the polygon + * @param[in] p Number of points that represent the polygon + * @return @c true when the given point is inside the polygon + */ + bool qFFMath_InPolygon( const float x, + const float y, + const float * const px, + const float * const py, + const size_t p ); + + /** + * @brief Determines if the point at ( @a x, @a y) is inside the circle + * with radius @a r located at @a cx and @a cy. + * @param[in] x Point x-coordinate + * @param[in] y Point y-coordinate + * @param[in] cx X-location of the circle + * @param[in] cy Y-location of the circle + * @param[in] r Radio of the circle + * @return @c true when the given point is inside the circle + */ + bool qFFMath_InCircle( const float x, + const float y, + const float cx, + const float cy, + const float r ); + + /** + * @brief Computes the gamma function of @a x + * @param[in] x The floating point value + * @return Upon successful completion, this function shall return @c Gamma(x). + * If @a x is a negative integer, a @c inf value shall be returned. If the + * correct value would cause overflow, qFFMath_TGamma() shall return @c ±Inf, + * with the same sign as the correct value of the function. + * If @a x is @c nan, a @c nan shall be returned. + * If @a x is @c +inf, @a x shall be returned. + * If @a x is @c ±0, tgamma() shall return @c ±Inf. + * If @a x is @c -inf, a @c nan value shall be returned. + * For IEEE Std 754-1985 float, overflow happens when 0 < x < 1/FLT_MAX, + * and 171.7 < x. + */ + float qFFMath_TGamma( float x ); + + /** + * @brief Computes the natural logarithm of the absolute value of the + * gamma function of @a x + * @note The argument @a x need not be a non-positive integer ( is defined + * over the reals, except the non-positive integers). + * @param[in] x The floating point value + * @return Upon successful completion, this function shall return the + * logarithmic gamma of @a x. If @a x is a non-positive integer, qFFMath_TGamma() shall + * return @c +inf. If the correct value would cause overflow, qFFMath_TGamma() shall + * return @c ±inf having the same sign as the correct value. + * If @a x is @c nan, a @c nan shall be returned. + * If @a x is @c 1 or @c 2, @c +0 shall be returned. + * If @a x is @c ±inf, @c +inf shall be returned. + */ + float qFFMath_LGamma( float x ); + + /** + * @brief Return the factorial of the integer part of @a x. + * @note The argument @a x needs to be positive + * @warning For @a x values greater than @c 14, result is imprecise because of + * the limited precision of the 32-bit floating point data-type. + * With @a x values greater than @c 35, this function overflows. + * @param[in] x The floating point value + * @return Upon successful completion, this function shall return the + * factorial of the integer part of @a x. If @a x is non-positive, qFFMath_Factorial() shall + * return @c nan. If the correct value would cause overflow, qFFMath_Factorial() shall + * return @c +inf. + */ + float qFFMath_Factorial( float x ); + +#endif /*#ifdef QLIBS_USE_STD_MATH*/ + + /** @}*/ + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/include/qfis.h b/include/qfis.h index 6ea4f6b..681cd3f 100644 --- a/include/qfis.h +++ b/include/qfis.h @@ -1,7 +1,7 @@ /*! * @file qfis.h * @author J. Camilo Gomez C. - * @version 1.4.2 + * @version 1.4.4 * @note This file is part of the qLibs distribution. * @brief Fuzzy Inference System (FIS) Engine **/ @@ -14,7 +14,6 @@ extern "C" { #endif #include - #include #include #include @@ -47,16 +46,16 @@ extern "C" { linzmf, /*!< Linear z-shaped saturation membership function f(a,b)*/ rectmf, /*!< Rectangle Membership Function f(s,e)*/ cosmf, /*!< Cosine Membership Function f(c,w)*/ - constantmf, /*!< Constant membership function f(a) [Only for ::Sugeno FIS]*/ - linearmf, /*!< Linear membership function f(...) [Only for ::Sugeno FIS]*/ - tlinsmf, /*!< Tsukamoto s-shaped saturation membership function f(a,b) [Only for ::Tsukamoto FIS]*/ - tlinzmf, /*!< Tsukamoto linzmf membership function f(a,b) [Only for ::Tsukamoto FIS]*/ - tconcavemf, /*!< Tsukamoto z-shaped saturation membership function f(i,e) [Only for ::Tsukamoto FIS]*/ - tsigmf, /*!< Tsukamoto Sigmoid membership function f(a,c) [Only for ::Tsukamoto FIS]*/ - tsmf, /*!< Tsukamoto S-Shape membership function f(a,b) [Only for ::Tsukamoto FIS]*/ - tzmf, /*!< Tsukamoto Z-Shape membership function f(a,b) [Only for ::Tsukamoto FIS]*/ + constantmf, /*!< Constant membership function f(a) [Only for ::Sugeno FIS ]*/ + linearmf, /*!< Linear membership function f(...) [Only for ::Sugeno FIS ]*/ + tlinsmf, /*!< Tsukamoto s-shaped saturation membership function f(a,b) [Only for ::Tsukamoto FIS ]*/ + tlinzmf, /*!< Tsukamoto linzmf membership function f(a,b) [ Only for ::Tsukamoto FIS ]*/ + tconcavemf, /*!< Tsukamoto z-shaped saturation membership function f(i,e) [Only for ::Tsukamoto FIS ]*/ + tsigmf, /*!< Tsukamoto Sigmoid membership function f(a,c) [ Only for ::Tsukamoto FIS ]*/ + tsmf, /*!< Tsukamoto S-Shape membership function f(a,b) [ Only for ::Tsukamoto FIS ]*/ + tzmf, /*!< Tsukamoto Z-Shape membership function f(a,b) [ Only for ::Tsukamoto FIS ]*/ /*! @cond */ - _NUM_MFS /*!< Number of supported membership functions*/ + _NUM_MFS /*!< Number of supported membership functions*/ //skipcq: CXX-E2000 /*! @endcond */ } qFIS_MF_Name_t; @@ -64,15 +63,15 @@ extern "C" { * @brief An enum with all the possible de-Fuzzyfication methods. */ typedef enum { - centroid = 0, /*!< Center of gravity of the fuzzy set along the x-axis [ Only for ::Mamdani FIS]*/ - bisector, /*!< Vertical line that divides the fuzzy set into two sub-regions of equal area [ Only for ::Mamdani FIS]**/ - mom, /*!< Middle of Maximum [ Only for ::Mamdani FIS]**/ - lom, /*!< Largest of Maximum [ Only for ::Mamdani FIS]**/ - som, /*!< Smallest of Maximum [ Only for ::Mamdani FIS]**/ - wtaver, /*!< Weighted average of all rule outputs [ Only for ::Sugeno and ::Tsukamoto FIS]*/ - wtsum, /*!< Weighted sum of all rule outputs [ Only for ::Sugeno FIS]*/ + centroid = 0, /*!< Center of gravity of the fuzzy set along the x-axis [ Only for ::Mamdani FIS ]*/ + bisector, /*!< Vertical line that divides the fuzzy set into two sub-regions of equal area [ Only for ::Mamdani FIS ]**/ + mom, /*!< Middle of Maximum [ Only for ::Mamdani FIS ]**/ + lom, /*!< Largest of Maximum [ Only for ::Mamdani FIS ]**/ + som, /*!< Smallest of Maximum [ Only for ::Mamdani FIS ]**/ + wtaver, /*!< Weighted average of all rule outputs [ Only for ::Sugeno and ::Tsukamoto FIS ]*/ + wtsum, /*!< Weighted sum of all rule outputs [ Only for ::Sugeno FIS ]*/ /*! @cond */ - _NUM_DFUZZ /*!< Number of supported defuzzification methods*/ + _NUM_DFUZZ /*!< Number of supported defuzzification methods*/ //skipcq: CXX-E2000 /*! @endcond */ } qFIS_DeFuzz_Method_t; @@ -107,13 +106,13 @@ extern "C" { Tsukamoto /*!< Mamdani inference system. The output of each rule its a fuzzy logic set represented with a monotonic membership function.*/ } qFIS_Type_t; - /*! @cond */ + /*! @cond */ typedef enum { DeFuzz_Init, DeFuzz_Compute, - DeFuzz_End, + DeFuzz_End } qFIS_DeFuzzState_t; - /*! @endcond */ + /*! @endcond */ /*! @cond */ typedef struct @@ -143,8 +142,9 @@ extern "C" { { /*! @cond */ qFIS_IO_Base_t b; - float res, x, y, data[ 4 ]; void *owner; + float *xag, *yag; + float res, x, y, data[ 4 ]; /*! @endcond */ } qFIS_Output_t; @@ -167,26 +167,54 @@ extern "C" { /*! @endcond */ } qFIS_MF_t; - /** - * @brief Type definition to instantiate a set of fuzzy rules - * @details Rules are defined by combining I/O and membership function tags - * with the following statements: - * - * #QFIS_RULES_BEGIN #IF, #IS, #IS_NOT, #AND, #OR, #THEN, #END and - * #QFIS_RULES_END - * - * Example: - * @code{.c} - * static const qFIS_Rules_t rules[] = { - * QFIS_RULES_BEGIN - * IF service IS service_poor OR food IS food_rancid THEN tip IS tip_cheap END - * IF service IS service_good THEN tip IS tip_average END - * IF service IS service_excellent OR food IS food_delicious THEN tip IS tip_generous END - * QFIS_RULES_END - * }; - * @endcode - */ - typedef int16_t qFIS_Rules_t; + #define FIS_RULE_ITEM_SIZE 1 + + #if ( FIS_RULE_ITEM_SIZE == 1 ) + + /** + * @brief Type definition to instantiate a set of fuzzy rules + * @details Rules are defined by combining I/O and membership function tags + * with the following statements: + * + * #QFIS_RULES_BEGIN, #IF, #IS, #IS_NOT, #AND, #OR, #THEN, #END and + * #QFIS_RULES_END + * + * Example: + * @code{.c} + * static const qFIS_Rules_t rules[] = { + * QFIS_RULES_BEGIN + * IF service IS service_poor OR food IS food_rancid THEN tip IS tip_cheap END + * IF service IS service_good THEN tip IS tip_average END + * IF service IS service_excellent OR food IS food_delicious THEN tip IS tip_generous END + * QFIS_RULES_END + * }; + * @endcode + */ + typedef int8_t qFIS_Rules_t; + #define FIS_RULES_MIN_VALUE INT8_MIN + #else + /** + * @brief Type definition to instantiate a set of fuzzy rules + * @details Rules are defined by combining I/O and membership function tags + * with the following statements: + * + * #QFIS_RULES_BEGIN, #IF, #IS, #IS_NOT, #AND, #OR, #THEN, #END and + * #QFIS_RULES_END + * + * Example: + * @code{.c} + * static const qFIS_Rules_t rules[] = { + * QFIS_RULES_BEGIN + * IF service IS service_poor OR food IS food_rancid THEN tip IS tip_cheap END + * IF service IS service_good THEN tip IS tip_average END + * IF service IS service_excellent OR food IS food_delicious THEN tip IS tip_generous END + * QFIS_RULES_END + * }; + * @endcode + */ + typedef int16_t qFIS_Rules_t; + #define FIS_RULES_MIN_VALUE INT16_MIN + #endif /*! @cond */ typedef int qFIS_Tag_t; @@ -198,7 +226,7 @@ extern "C" { * @details The instance should be initialized using the qFIS_Setup() API. * @note Do not access any member of this structure directly. */ - typedef struct _qFIS_s + typedef struct _qFIS_s //skipcq: CXX-E2000 { /*! @cond */ qFIS_Input_t *input; @@ -210,7 +238,6 @@ extern "C" { float (*aggregate)( const float a, const float b ); size_t (*inferenceState)( struct _qFIS_s * const f, size_t i ); size_t (*aggregationState)( struct _qFIS_s * const f, size_t i ); - //void (*typeDeFuzz)( struct _qFIS_s * const f ); qFIS_DeFuzz_Fcn_t deFuzz; float *ruleWeight; float *wi; @@ -227,11 +254,18 @@ extern "C" { } qFIS_t; /*! @cond */ - #define _QFIS_RULES_END ( INT16_MIN + 1 ) - #define _QFIS_AND ( INT16_MIN + 2 ) - #define _QFIS_OR ( INT16_MIN + 3 ) - #define _QFIS_THEN ( INT16_MIN + 4 ) - #define _QFIS_RULE_END ( INT16_MIN + 5 ) + #define _QFIS_RULES_END ( FIS_RULES_MIN_VALUE + 1 ) //skipcq: CXX-E2000 + #define _QFIS_AND ( FIS_RULES_MIN_VALUE + 2 ) //skipcq: CXX-E2000 + #define _QFIS_OR ( FIS_RULES_MIN_VALUE + 3 ) //skipcq: CXX-E2000 + #define _QFIS_THEN ( FIS_RULES_MIN_VALUE + 4 ) //skipcq: CXX-E2000 + + #define _QFIS_IF_STATEMENT , //skipcq: CXX-E2000 + #define _QFIS_AND_STATEMENT +1),_QFIS_AND, //skipcq: CXX-E2000 + #define _QFIS_OR_STATEMENT +1),_QFIS_OR, //skipcq: CXX-E2000 + #define _QFIS_THEN_STATEMENT +1),_QFIS_THEN, //skipcq: CXX-E2000 + #define _QFIS_IS_STATEMENT ,( //skipcq: CXX-E2000 + #define _QFIS_IS_NOT_STATEMENT ,-( //skipcq: CXX-E2000 + #define _QFIS_END_STATEMENT +1) //skipcq: CXX-E2000 /*! @endcond */ /*Rules build keywords*/ @@ -243,33 +277,33 @@ extern "C" { * #QFIS_RULES_END declare the end of the FIS rule set. * @see #QFIS_RULES_END * @warning Only one segment is allowed inside a fuzzy rule set. - * @note It must always be used together with a matching #QFIS_RULES_END + * @note It must always be used together with a matching #QFIS_RULES_END * statement. * Example: * @code{.c} - * static const qFIS_Rules_t rules[] = { + * static const qFIS_Rules_t rules[] = { * QFIS_RULES_BEGIN - * + * * QFIS_RULES_END * }; * @endcode */ - #define QFIS_RULES_BEGIN ( INT16_MIN ) + #define QFIS_RULES_BEGIN ( FIS_RULES_MIN_VALUE ) /** * @brief Ends a Fuzzy rule set. - * The #QFIS_RULES_END statement is used to finalize the declaration of a + * The #QFIS_RULES_END statement is used to finalize the declaration of a * FIS rule set. It should be placed at the end of the rules enumeration. * #QFIS_RULES_BEGIN declare the start point of the FIS rule set. * @see #QFIS_RULES_BEGIN * @warning Only one segment is allowed inside a fuzzy rule set. - * @note It must always be used together with a matching #QFIS_RULES_BEGIN + * @note It must always be used together with a matching #QFIS_RULES_BEGIN * statement. * Example: * @code{.c} - * static const qFIS_Rules_t rules[] = { + * static const qFIS_Rules_t rules[] = { * QFIS_RULES_BEGIN - * + * * QFIS_RULES_END * }; * @endcode @@ -277,19 +311,19 @@ extern "C" { #define QFIS_RULES_END ,_QFIS_RULES_END /** @brief Rule statement to begin a rule sentence */ - #define IF , + #define IF _QFIS_IF_STATEMENT /** @brief Rule statement to represent the AND connector */ - #define AND +1),_QFIS_AND, + #define AND _QFIS_AND_STATEMENT /** @brief Rule statement to represent the OR connector */ - #define OR +1),_QFIS_OR, + #define OR _QFIS_OR_STATEMENT /** @brief Rule statement to represent the implication */ - #define THEN +1),_QFIS_THEN, + #define THEN _QFIS_THEN_STATEMENT /** @brief Rule statement to represent a premise*/ - #define IS ,( + #define IS _QFIS_IS_STATEMENT /** @brief Rule statement to represent a negated premise */ - #define IS_NOT ,-( + #define IS_NOT _QFIS_IS_NOT_STATEMENT /** @brief Rule statement to end a rule sentence */ - #define END +1) + #define END _QFIS_END_STATEMENT /** * @brief Set parameters of the FIS instance. @@ -303,7 +337,7 @@ extern "C" { const qFIS_ParamValue_t x ); /** - * @brief Change the default deFuzzification method of the FIS instance. + * @brief Change the default de-Fuzzification method of the FIS instance. * @param[in] f A pointer to the Fuzzy Inference System instance. * @param[in] m The de-fuzzification method: use one of the following : * ::centroid, ::bisector, ::mom, ::lom, ::som, ::wtaver, ::wtsum @@ -327,7 +361,7 @@ extern "C" { * @param[in] ni The number of bytes used by @a inputs. Use the sizeof operator. * @param[in] outputs An array with all the system outputs as qFIS_Output_t * objects. - * @param[in] no The number of bytes used by @a inputs. Use the sizeof operator. + * @param[in] no The number of bytes used by @a outputs. Use the sizeof operator. * @param[in] mf_inputs An array with all the membership functions related to * the inputs. This should be an array of qFIS_MF_t objects. * @param[in] nmi The number of bytes used by @a mf_inputs. Use the sizeof @@ -372,7 +406,7 @@ extern "C" { /** * @brief Setup the output with the specified tag and set limits for it * @note limits do not apply in ::Sugeno outputs - * @param[in] v An array with the FIS outputs inputs as a qFIS_Output_t array. + * @param[in] v An array with the FIS outputs as a qFIS_Output_t array. * @param[in] t The output tag * @param[in] min Minimum allowed value for this output * @param[in] max Max allowed value for this output @@ -410,7 +444,7 @@ extern "C" { * objects. * @param[in] io The I/O tag related with this membership function * @param[in] mf The user-defined tag for this membership function - * @param[in] s The wanted shape/form for this membership function, cam + * @param[in] s The wanted shape/form for this membership function, can * be one of the following: ::trimf, ::trapmf, ::gbellmf, ::gaussmf, * ::gauss2mf, ::sigmf, ::dsigmf, ::psigmf, ::pimf, ::smf, ::zmf, * ::singletonmf, ::concavemf, ::spikemf, ::linsmf, ::linzmf, ::rectmf, @@ -418,16 +452,16 @@ extern "C" { * @note For ::Sugeno FIS, an output membership function should be one of the * following: ::constantmf, ::linearmf. * @note For ::Tsukamoto FIS, an output membership function should be one the - * following monotonic functions : ::tlinsmf, ::tlinzmf, ::tsmf, ::tzmf, + * following monotonic functions : ::tlinsmf, ::tlinzmf, ::tsmf, ::tzmf, * ::tconcavemf * @note To set a custom user-defined membership function, set this argument * as ::custommf and pass a pointer to the desired function on the * @a custom_mf argument. * @param[in] custom_mf Custom user-defined membership function. To ignore - * pass NULL as argument. + * pass @c NULL as argument. * @param[in] cp Points or coefficients of the membership function. * @param[in] h Height of the membership function. - * @note Heigth parameter @a h does not apply for output membership functions + * @note Height parameter @a h does not apply for output membership functions * on ::Sugeno and ::Tsukamoto inference systems. [ 0 <= h <= 1] * @return 1 on success, otherwise return 0. */ @@ -439,9 +473,33 @@ extern "C" { const float *cp, const float h ); + /** + * @brief Set location to store the aggregated region for supplied FIS output + * @note This feature only applies to Mamdani systems. + * @warning Array size of @a x and @a y should be greater or equal to the + * number of evaluation points of the FIS instance. + * @pre The FIS instance and the output should be previously configured + * initialized with qFIS_Setup() and qFIS_OutputSetup() respectively. + * @param[in] o An array of type qFIS_Output_t with the FIS outputs. + * @param[in] t The output tag + * @param[in] x Array where the x-axis points of the aggregated output will be + * stored + * @param[in] y Array where the y-axis points of the aggregated output will be + * stored + * @param[in] n Number of elements in @a x or @a y. + * @return 1 on success, otherwise return 0. + */ + int qFIS_StoreAggregatedRegion( qFIS_Output_t * const o, + const qFIS_Tag_t t, + float *x, + float *y, + const size_t n ); + /** * @brief Perform the fuzzification operation over the crisp inputs on the * requested FIS object + * @pre I/Os and fuzzy sets must be previously initialized by qFIS_InputSetup(), + * qFIS_OutputSetup(), qFIS_SetMF() and qFIS_Setup() respectively. * @param[in] f A pointer to the Fuzzy Inference System instance. * @return 1 on success, otherwise return 0. */ @@ -449,6 +507,8 @@ extern "C" { /** * @brief Perform the inference process on the requested FIS object + * @pre The instance should have already invoked the fuzzification operation + * successfully with qFIS_Fuzzify() * @param[in] f A pointer to the Fuzzy Inference System instance. * @return 1 on success, otherwise return 0. */ @@ -456,6 +516,8 @@ extern "C" { /** * @brief Perform the de-Fuzzification operation to compute the crisp outputs. + * @pre The instance should have already invoked the inference process + * successfully with qFIS_Inference() * @note By default, this function, this API uses the Centroid method on * ::Mamdani type FIS and weight-average on ::Sugeno type FIS. To change * the default settings use the qFIS_SetDeFuzzMethod() function. @@ -466,6 +528,8 @@ extern "C" { /** * @brief Set weights to the rules of the inference system. + * @pre I/Os and fuzzy sets must be previously initialized by qFIS_InputSetup(), + * qFIS_OutputSetup(), qFIS_SetMF() and qFIS_Setup() respectively. * @param[in] f A pointer to the Fuzzy Inference System instance. * @param[in] rWeights An array with the values of every rule weight; * @return 1 on success, otherwise return 0. diff --git a/include/qfp16.h b/include/qfp16.h index 16424a8..3a5cbf7 100644 --- a/include/qfp16.h +++ b/include/qfp16.h @@ -1,7 +1,7 @@ /*! * @file qfp16.h * @author J. Camilo Gomez C. - * @version 1.07 + * @version 1.08 * @note This file is part of the qLibs distribution. * @brief Fixed-Point math Q16.16 with rounding and saturated arithmetic. **/ @@ -15,7 +15,6 @@ extern "C" { #include #include - #include /** @addtogroup qfp16 Q16.16 Fixed-point math * @brief API for the qFP16 Fixed-point math library @@ -26,7 +25,7 @@ extern "C" { typedef int32_t qFP16_t; /*! @cond */ - struct _qFP16_const_s + struct _qFP16_const_s //skipcq: CXX-E2000 { const qFP16_t f_e, /* [ e ] The base of natural logarithms, e.*/ @@ -58,7 +57,7 @@ extern "C" { /** * @brief Fixed-point Q16.16 constants */ - extern const struct _qFP16_const_s qFP16; + extern const struct _qFP16_const_s qFP16; //skipcq: CXX-E2000 /** * @brief A macro for defining a fixed-point constant value. @@ -68,8 +67,8 @@ extern "C" { * @return The literal argument @a x converted to fixed-point(qFP16_t). */ #define qFP16_Constant(x) \ - ( (qFP16_t)( ( (x) >= 0 ) ? ( (x) * 65536.0f + 0.5f ) \ - : ( (x) * 65536.0f - 0.5f ) ) ) \ + ( (qFP16_t)( ( (x) >= 0 ) ? ( (x) * 65536.0F + 0.5F ) \ + : ( (x) * 65536.0F - 0.5F ) ) ) \ /** * @brief A Q16.16 fixed-point settings object @@ -102,8 +101,8 @@ extern "C" { /** * @brief Select the provided setting instance to perform fixed-point * operations. - * @param[in] instance A pointer to the fixed-point settings instance. Pass NULL - * to use the default settings. + * @param[in] instance A pointer to the fixed-point settings instance. Pass + * @c NULL to use the default settings. * @return none. */ void qFP16_SettingsSelect( qFP16_Settings_t * const instance ); @@ -130,7 +129,7 @@ extern "C" { qFP16_t qFP16_FloatToFP( const float x ); /** - * @brief Returns the fixed-point value @ x converted to float. + * @brief Returns the fixed-point value @a x converted to float. * @param[in] x The fixed-point(q16.16) value. * @return This function returns @a x converted to float. */ @@ -144,7 +143,7 @@ extern "C" { qFP16_t qFP16_DoubleToFP( const double x ); /** - * @brief Returns the fixed-point value @ x converted to double. + * @brief Returns the fixed-point value @a x converted to double. * @param[in] x The fixed-point(q16.16) value. * @return This function returns @a x converted to double. */ @@ -183,7 +182,7 @@ extern "C" { * @brief Returns the fixed-point addition @a x + @a y. * @param[in] X The fixed-point(q16.16) value. * @param[in] Y The fixed-point(q16.16) value. - * @return This function returns the addition operation x+y. QFP16_OVERFLOW + * @return This function returns the addition operation x+y. @c qFP16.overflow * when an operation overflow is detected. */ qFP16_t qFP16_Add( const qFP16_t X, @@ -193,7 +192,7 @@ extern "C" { * @brief Returns the fixed-point subtraction @a x - @a y. * @param[in] X The fixed-point(q16.16) value. * @param[in] Y The fixed-point(q16.16) value. - * @return This function returns the subtraction operation x-y. QFP16_OVERFLOW + * @return This function returns the subtraction operation x-y. @c qFP16.overflow * when an operation overflow is detected. */ qFP16_t qFP16_Sub( const qFP16_t X, @@ -203,7 +202,7 @@ extern "C" { * @brief Returns the fixed-point product operation @a x * @a y. * @param[in] x The fixed-point(q16.16) value. * @param[in] y The fixed-point(q16.16) value. - * @return This function returns the product operation x*y. QFP16_OVERFLOW + * @return This function returns the product operation x*y. @c qFP16.overflow * when an operation overflow is detected. */ qFP16_t qFP16_Mul( const qFP16_t x, @@ -213,7 +212,7 @@ extern "C" { * @brief Returns the fixed-point division operation @a x / @a y. * @param[in] x The fixed-point(q16.16) value. * @param[in] y The fixed-point(q16.16) value. - * @return This function returns the product operation x/y. QFP16_OVERFLOW + * @return This function returns the product operation x/y. @c qFP16.overflow * when an operation overflow is detected. */ qFP16_t qFP16_Div( const qFP16_t x, @@ -232,14 +231,14 @@ extern "C" { * @brief Returns the fixed-point square root of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns the square root of @a x. For negative - * numbers, returns QFP16_OVERFLOW. + * numbers, returns @c qFP16.overflow . */ qFP16_t qFP16_Sqrt( qFP16_t x ); /** * @brief Returns the fixed-point value of e raised to the xth power. * @param[in] x The fixed-point(q16.16) value. - * @return This function returns the exponential value of x. QFP16_OVERFLOW + * @return This function returns the exponential value of x. @c qFP16.overflow * when an operation overflow is detected. */ qFP16_t qFP16_Exp( qFP16_t x ); @@ -248,7 +247,7 @@ extern "C" { * @brief Returns the fixed-point natural logarithm (base-e logarithm) of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns natural logarithm of @a x. For negative - * values returns QFP16_OVERFLOW + * values returns@c qFP16.overflow */ qFP16_t qFP16_Log( qFP16_t x ); @@ -256,7 +255,7 @@ extern "C" { * @brief Returns the fixed-point log base 2 of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns log base 2 of @a x. For negative values - * returns QFP16_OVERFLOW + * returns @c qFP16.overflow */ qFP16_t qFP16_Log2( const qFP16_t x ); @@ -266,7 +265,7 @@ extern "C" { * in radians. * @return This function returns the angle converted in degrees. */ - qFP16_t fp16_RadToDeg( const qFP16_t x ); + qFP16_t qFP16_RadToDeg( const qFP16_t x ); /** * @brief Converts angle units from degrees to radians. @@ -274,7 +273,7 @@ extern "C" { * in degrees. * @return This function returns the angle converted in radians. */ - qFP16_t fp16_DegToRad( const qFP16_t x ); + qFP16_t qFP16_DegToRad( const qFP16_t x ); /** * @brief Wrap the fixed-point angle in radians to [−pi pi] @@ -357,7 +356,7 @@ extern "C" { * @brief Computes the fixed-point hyperbolic cosine of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns hyperbolic cosine of @a x. If overflow - * detected returns QFP16_OVERFLOW. If the function saturates, returns + * detected returns @c qFP16.overflow. If the function saturates, returns * QFP16_EXP_MAX or QFP16_EXP_MIN. */ qFP16_t qFP16_Cosh( qFP16_t x ); @@ -366,7 +365,7 @@ extern "C" { * @brief Computes the fixed-point hyperbolic sine of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns hyperbolic sine of @a x. If overflow - * detected returns QFP16_OVERFLOW. If the function saturates, returns + * detected returns @c qFP16.overflow. If the function saturates, returns * QFP16_EXP_MAX or QFP16_EXP_MIN. */ qFP16_t qFP16_Sinh( qFP16_t x ); @@ -375,7 +374,7 @@ extern "C" { * @brief Computes the fixed-point hyperbolic tangent of @a x. * @param[in] x The fixed-point(q16.16) value. * @return This function returns hyperbolic tangent of @a x. If overflow - * detected returns QFP16_OVERFLOW. If the function saturates, returns + * detected returns @c qFP16.overflow. If the function saturates, returns * QFP16_EXP_MAX or QFP16_EXP_MIN. */ qFP16_t qFP16_Tanh( qFP16_t x ); @@ -389,7 +388,7 @@ extern "C" { * @param[in] n The number of elements of the fixed-point array @a p. * @param[in] x The fixed-point(q16.16) value to evaluate the polynomial. * @return This function returns the polynomial evaluation p(x). If overflow - * detected returns QFP16_OVERFLOW. + * detected returns @c qFP16.overflow. */ qFP16_t qFP16_Polyval( const qFP16_t * const p, const size_t n, @@ -401,7 +400,7 @@ extern "C" { * @param[in] y The fixed-point(q16.16) power value. Only the integer part is * taken * @return This function returns the result of raising @a x to the power @a y. - * QFP16_OVERFLOW when an operation overflow is detected. + * @c qFP16.overflow when an operation overflow is detected. */ qFP16_t qFP16_IPow( const qFP16_t x, const qFP16_t y ); @@ -411,7 +410,7 @@ extern "C" { * @param[in] x The fixed-point(q16.16) base value. * @param[in] y The fixed-point(q16.16) power value. * @return This function returns the result of raising @a x to the power @a y. - * QFP16_OVERFLOW when an operation overflow is detected. + * @c qFP16.overflow when an operation overflow is detected. */ qFP16_t qFP16_Pow( const qFP16_t x, const qFP16_t y ); @@ -443,9 +442,9 @@ extern "C" { * floating-point number. * @return On success, the function returns the converted floating point * number as a fixed-point value. If no valid conversion could be performed, - * the function returns zero (0.0) or QFP16_OVERFLOW. If the converted value + * the function returns zero (0.0) or @c qFP16.overflow. If the converted value * would be out of the range of representable values by a fixed-point Q16.16, - * the function returns QFP16_OVERFLOW. + * the function returns @c qFP16.overflow . */ qFP16_t qFP16_AToFP( const char *s ); diff --git a/include/qltisys.h b/include/qltisys.h index 65c45f8..e6cf66e 100644 --- a/include/qltisys.h +++ b/include/qltisys.h @@ -1,7 +1,7 @@ /*! * @file qltisys.h * @author J. Camilo Gomez C. - * @version 1.07 + * @version 1.09 * @note This file is part of the qTools distribution. * @brief API to simulate continuous and discrete LTI systems. **/ @@ -15,7 +15,6 @@ extern "C" { #include #include - #include #include "qtdl.h" #include "qnuma.h" @@ -28,24 +27,28 @@ extern "C" { /** * @brief Macro to specify that the system is time-discrete */ - #define QLTISYS_DISCRETE ( -1.0f ) + #define QLTISYS_DISCRETE ( -1.0F ) + + /*cstat -MISRAC2012-Rule-2.3*/ /** - * @brief Type to specify continuos states + * @brief Type to specify continuos states */ typedef qNumA_state_t qLTISys_ContinuosX_t; /** - * @brief Type to specify continuos states + * @brief Type to specify continuos states */ typedef float qLTISys_DiscreteX_t; + /*cstat +MISRAC2012-Rule-2.3*/ + /** * @brief A LTI system object * @details The instance should be initialized using the qLTISys_Setup() API. * @note Do not access any member of this structure directly. */ - typedef struct _qLTISys_s + typedef struct _qLTISys_s //skipcq: CXX-E2000 { /*! @cond */ float (*sysUpdate)( struct _qLTISys_s *sys, float u ); @@ -61,6 +64,7 @@ extern "C" { /** * @brief Drives the LTI system recursively using the input signal provided + * @pre Instance must be previously initialized by qLTISys_Setup() * @note The user must ensure that this function is executed in the time * specified in @a dt either by using a HW or SW timer, a real time task, * or a timing service. @@ -102,6 +106,16 @@ extern "C" { */ int qLTISys_IsInitialized( const qLTISys_t * const sys ); + /** + * @brief Set the initial states for the given system + * @pre System should be previously initialized by using qLTISys_Setup() + * @param[in] sys A pointer to the LTI system instance + * @param[in] xi An array of n-elements with the initial state values. User + * can pass @c NULL as argument to set initial conditions equal to zero. + * @return 1 on success, otherwise return 0. + */ + int qLTISys_SetInitStates( qLTISys_t * const sys, const float * const xi ); + /** * @brief Setup and initialize an instance of a LTI system. * @param[in] sys A pointer to the continuous LTI system instance @@ -114,23 +128,26 @@ extern "C" { * Coefficients should be given in descending powers of the n or nb-degree * polynomial. Coefficients will be normalized internally. * @param[in,out] x Initial conditions of the system. For a continuos system, - * an array of type qNumA_state_t with n elements. - * For a discrete system, an array of type float with max(na,nb) elements + * an array of type qLTISys_ContinuosX_t with n elements. + * For a discrete system, an array of type qLTISys_DiscreteX_t with + * max(na,nb) elements * For both cases, the supplied array will be updated on every invocation of * qLTISys_Excite(). * @param[in] nb The order of polynomial @a num + 1 (Only for discrete systems). - * example: b0 + b1*z^-1 + b2*z^-2 + b3*z^-3 , nb = 4 + * example: \f$ b_{0}+b_{1}z^{-1}+b_{2}z^{-2}+b_{3}z^{-3}, nb = 4 \f$ * @note If the system is continuous, pass 0 as argument. * @param[in] na The order of polynomial @a den. (if system is discrete). For * continuous system the number of elements of @a num and @a den. - * - * example 1: a0 + a1*z^-1 + a2*z^-2 + a3*z^-3 , na = 3 - * - * example 2: num = b0*s^2 + b1*s + b2 , den = a0*s^2 + a1*s + a2 , na = 3 + * + * example 1: \f$ a_{0}+a_{1}z^{-1}+a_{2}z^{-2}+a_{3}z^{-3}, na = 3 \f$ + * + * example 2: \f$ \frac{ b_{0}s^{2}+b_{1}s+b_{2} }{ a_{0}s^{2} + a_{1}s + a_{2} }, na = 3 \f$ * @note For continuous systems, size of @a num and @a den should be equal. * @param[in] dt The time-step of the continuos system. For discrete systems * pass #QLTISYS_DISCRETE as argument * @return 1 on success, otherwise return 0. + * @note By default, initial conditions are set to zero. To change the initial + * conditions to the desired values, use the qLTISys_SetInitStates() function. */ int qLTISys_Setup( qLTISys_t * const sys, float *num, @@ -149,7 +166,7 @@ extern "C" { * the delay lines of @a x. * @param[in] c An array of @a wsize elements with the coefficients of the * FIR filter. Coefficients should be given in descending powers of the - * nth-degree polynomial. To ignore pass NULL. + * nth-degree polynomial. To ignore pass @c NULL. * @param[in] wsize The number of elements of @a w. * @param[in] x A sample of the input signal. * @return If @a c is provided, returns the evaluation of the FIR filter. @@ -165,11 +182,11 @@ extern "C" { * @param[in] sys A pointer to the continuous LTI system instance * @param[in] im The desired integration method. Use one of the following: * - * qNumA_IntegralRe : Integrate using the Rectangular rule. + * @c qNumA_IntegralRe : Integrate using the Rectangular rule. * - * qNumA_IntegralTr : (default) Integrate using the Trapezoidal rule. + * @c qNumA_IntegralTr : (default) Integrate using the Trapezoidal rule. * - * qNumA_IntegralSi : Integrate using the Simpson's 1/3 rule. + * @c qNumA_IntegralSi : Integrate using the Simpson's 1/3 rule. * * @return 1 on success, otherwise return 0. */ diff --git a/include/qnuma.h b/include/qnuma.h index fb6d82d..fdfaa4d 100644 --- a/include/qnuma.h +++ b/include/qnuma.h @@ -13,7 +13,9 @@ extern "C" { #endif - typedef struct _qNumA_state_s + #include + + typedef struct _qNumA_state_s //skipcq: CXX-E2000 { /*! @cond */ float x[ 3 ]; @@ -23,7 +25,8 @@ extern "C" { /*! @cond */ typedef float (*qNumA_IntegrationMethod_t)( qNumA_state_t *x, const float s, - const float dt ); + const float dt, + const bool bUpdate ); /*! @endcond */ /** @@ -47,7 +50,8 @@ extern "C" { */ float qNumA_IntegralRe( qNumA_state_t *x, const float s, - const float dt ); + const float dt, + const bool bUpdate ); /** * @brief Perform a numerical integration step by using the trapezoidal rule. @@ -58,7 +62,8 @@ extern "C" { */ float qNumA_IntegralTr( qNumA_state_t *x, const float s, - const float dt ); + const float dt, + const bool bUpdate ); /** * @brief Perform a numerical integration step by using the Simpson's rule. @@ -69,7 +74,8 @@ extern "C" { */ float qNumA_IntegralSi( qNumA_state_t *x, const float s, - const float dt ); + const float dt, + const bool bUpdate); /** * @brief Perform a numerical derivation step by using the delta rule. @@ -78,10 +84,36 @@ extern "C" { * @param[in] dt The time-step given in seconds. * @return The current value of the derivation step. */ - float qNumA_Derivative( qNumA_state_t *x, - const float s, - const float dt ); + float qNumA_Derivative2p( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ); + /** + * @brief Perform a numerical derivation step by using the three-point + * backward-difference. + * @param[in] x A pointer to the numerical state instance + * @param[in] s The input signal + * @param[in] dt The time-step given in seconds. + * @return The current value of the derivation step. + */ + float qNumA_DerivativeBa( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ); + + /** + * @brief Perform a numerical derivation step by using the three-point + * forward-difference. + * @param[in] x A pointer to the numerical state instance + * @param[in] s The input signal + * @param[in] dt The time-step given in seconds. + * @return The current value of the derivation step. + */ + float qNumA_DerivativeFo( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ); #ifdef __cplusplus } diff --git a/include/qpid.h b/include/qpid.h index 1a24a46..226b7cc 100644 --- a/include/qpid.h +++ b/include/qpid.h @@ -1,7 +1,7 @@ /*! * @file qpid.h * @author J. Camilo Gomez C. - * @version 1.08 + * @version 1.16 * @note This file is part of the qLibs distribution. * @brief API to control systems using the PID algorithm. This controller * features anti-windup, tracking mode, and derivative filter. @@ -15,9 +15,7 @@ extern "C" { #endif #include - #include #include - #include #include "qnuma.h" /** @addtogroup qpid Closed Loop PID Controller library @@ -26,9 +24,45 @@ extern "C" { */ /** - * @brief A PID Auto-tunning object + * @brief PID Gains structure + */ + typedef struct { + float Kc; /*!< Proportional gain */ + float Ki; /*!< Integral gain */ + float Kd; /*!< Derivative gain */ + } qPID_Gains_t; + + /** + * @brief Enumeration class with the operational modes for the PID controller + */ + typedef enum { + qPID_TYPE_P, /*!< Proportional controller */ + qPID_TYPE_PD, /*!< Proportional-Integral controller */ + qPID_TYPE_PI, /*!< Proportional-Derivative controller */ + qPID_TYPE_PID /*!< Proportional-Integral-Derivative controller */ + } qPID_Type_t; + + /** + * @brief Enumeration with the operational modes for the PID controller + */ + typedef enum { + qPID_Automatic, /*!< Fully operational closed-loop PID controller */ + qPID_Manual /*!< Open-loop with manual input*/ + } qPID_Mode_t; + + + /** + * @brief Direction modes of the PID controller + */ + typedef enum { + qPID_Forward, /*!< Forward control action */ + qPID_Backward /*!< Reverse the control action*/ + } qPID_Direction_t; + + /** + * @brief A PID Auto-tuning object * @details The instance should be bound to a configured PID controller by - * using the qPID_BindAutoTunning() API + * using the qPID_BindAutoTuning() API */ typedef struct { @@ -41,12 +75,12 @@ extern "C" { float mu, speed; /*fine adjustments [ 0 < mu < speed ] [ 0 < speed < 1 ]*/ uint32_t it; /*enable time*/ /*! @endcond */ - } qPID_AutoTunning_t; + } qPID_AutoTuning_t; /** * @brief Macro to keep the auto-tuner enabled indefinitely */ - #define QPID_AUTOTUNNING_UNDEFINED ( 0xFFFFFFFEuL ) + #define QPID_AUTOTUNING_UNDEFINED ( 0xFFFFFFFEUL ) /** * @brief A PID controller object @@ -56,14 +90,18 @@ extern "C" { typedef struct { /*! @cond */ - float kc, ki, kd, dt, min, max, epsilon, kw, kt, D, u1, beta; - float *uEF; /*external feedback for tracking mode*/ + float kc, ki, kd, b, c, dt, min, max, epsilon, kw, kt, D, u1, beta, uSat; + float m, mInput; const float *yr; float alfa, gamma; /*MRAC additive controller parameters*/ qNumA_state_t c_state; /*controller integral & derivative state*/ qNumA_state_t m_state; /*MRAC additive controller state*/ - qPID_AutoTunning_t *adapt; + qNumA_state_t b_state; /*Bumples-transfer state*/ + qPID_AutoTuning_t *adapt; qNumA_IntegrationMethod_t integrate; + qPID_Mode_t mode; + qPID_Direction_t dir; + qPID_Type_t type; uint8_t init; /*! @endcond */ } qPID_controller_t; @@ -83,6 +121,29 @@ extern "C" { const float kd, const float dt ); + /** + * @brief Set the PID control action direction. + * @param[in] c A pointer to the PID controller instance. + * @param[in] d Desired output direction. + * @return 1 on success, otherwise return 0. + */ + int qPID_SetDirection( qPID_controller_t * const c, + const qPID_Direction_t d ); + + /** + * @brief Set/Change the PID controller gains by using the [Kc, Ti Td ] + * triplet. + * @param[in] c A pointer to the PID controller instance. + * @param[in] kc Proportional Gain. + * @param[in] ti Integral time. + * @param[in] td Derivative time. + * @return 1 on success, otherwise return 0. + */ + int qPID_SetParams( qPID_controller_t * const c, + const float kc, + const float ti, + const float td ); + /** * @brief Set/Change the PID controller gains. * @param[in] c A pointer to the PID controller instance. @@ -96,6 +157,18 @@ extern "C" { const float ki, const float kd ); + /** + * @brief Set/Change extra PID controller gains. + * @param[in] c A pointer to the PID controller instance. + * @param[in] kw Saturation feedback gain. Used for antiWindup and bumpless + * transfer. A zero value disables these features. + * @param[in] kt Manual input gain. + * @return 1 on success, otherwise return 0. + */ + int qPID_SetExtraGains( qPID_controller_t * const c, + const float kw, + const float kt ); + /** * @brief Reset the internal PID controller calculations. * @param[in] c A pointer to the PID controller instance. @@ -108,14 +181,11 @@ extern "C" { * @param[in] c A pointer to the PID controller instance. * @param[in] min The minimal value allowed for the output. * @param[in] max The maximal value allowed for the output. - * @param[in] kw Anti-windup feedback gain. A zero value disables the - * anti-windup feature. * @return 1 on success, otherwise return 0. */ int qPID_SetSaturation( qPID_controller_t * const c, const float min, - const float max, - const float kw ); + const float max ); /** * @brief Convert the controller gains to conform the series or interacting @@ -135,27 +205,54 @@ extern "C" { const float eps ); /** - * @brief Set the tunning parameter for the derivative filter. + * @brief Set the tuning parameter for the derivative filter. * @param[in] c A pointer to the PID controller instance. - * @param[in] beta The tunning parameter. [ 0 < beta < 1 ] + * @param[in] beta The tuning parameter. [ 0 < beta < 1 ] * @return 1 on success, otherwise return 0. */ int qPID_SetDerivativeFilter( qPID_controller_t * const c, const float beta ); /** - * @brief Set the PID tracking mode. This allows the PID controller to adjust - * its internal state by changing its integrator output so that the output - * tracks a prescribed signal feeding this extra input. Can be used to - * achieve bumpless control transfer. + * @brief Set the PID Reference(Set-Point) Weighting. This value is used in + * order to avoid the increase of the rise time due to the smoothing of the + * reference signal applied to the closed-loop system + * @note A value close to zero en @a gc can be used to eliminate the + * reduce the effect of the phenomena called Derivative Kick + * @param[in] c A pointer to the PID controller instance. + * @param[in] gb The reference weight value for the proportional element. + * @param[in] gc The reference weight value for the derivative element. + * @return 1 on success, otherwise return 0. + */ + int qPID_SetReferenceWeighting( qPID_controller_t * const c, + const float gb, + const float gc ); + + /** + * @brief Set the PID manual input mode. This value will be used + * as the manual input when the controller it set into the ::qPID_Manual + * mode. Bumpless-transfer is guaranteed. + * @param[in] c A pointer to the PID controller instance. + * @param[in] manualInput The value of the manual input. + * @return 1 on success, otherwise return 0. + */ + int qPID_SetManualInput( qPID_controller_t * const c, + const float manualInput ); + + /** + * @brief Change the controller operational mode. + * In ::qPID_Automatic mode, the computed output of the PID controller + * will be used as the control signal for the process. In + * ::qPID_Manual mode, the manual input will be used as the control + * signal for the process and the PID controller loop will continue + * operating to guarantee the bumpless-transfer when a switch to + * the ::qPID_Automatic mode its performed; * @param[in] c A pointer to the PID controller instance. - * @param[in] var A pointer to the external feedback variable. - * @param[in] kt Tracking gain. + * @param[in] m The desired operational mode. * @return 1 on success, otherwise return 0. */ - int qPID_SetTrackingMode( qPID_controller_t * const c, - float *var, - const float kt ); + int qPID_SetMode( qPID_controller_t * const c, + const qPID_Mode_t m ); /** * @brief Enable the additive MRAC(Model Reference Adaptive Control) feature. @@ -165,11 +262,12 @@ extern "C" { * @return 1 on success, otherwise return 0. */ int qPID_SetMRAC( qPID_controller_t * const c, - const float *modelRef, - const float gamma ); + const float *modelRef, + const float gamma ); /** * @brief Computes the control action for given PID controller instance. + * @pre Instance must be previously initialized by qPID_Setup() * @note The user must ensure that this function is executed in the time * specified in @a dt either by using a HW or SW timer, a real time task, * or a timing service. @@ -185,55 +283,58 @@ extern "C" { /** * @brief Binds the specified instance to enable the PID controller auto * tuning algorithm. - * @note To unbind the auto tunning algorithm, pass NULL as argument. + * @note To unbind the auto-tuning algorithm, pass @c NULL as argument. * @param[in] c A pointer to the PID controller instance. - * @param[in] at A pointer to the PID auto tunning instance. + * @param[in] at A pointer to the PID auto tuning instance. * @return 1 on success, otherwise return 0. */ - int qPID_BindAutoTunning( qPID_controller_t * const c, - qPID_AutoTunning_t * const at ); + int qPID_BindAutoTuning( qPID_controller_t * const c, + qPID_AutoTuning_t * const at ); /** * @brief Set the number of time steps where the auto tuner algorithm will - * modify the controller gains. + * modify the controller gains. + * @pre Controller must have an qPID_AutoTuning_t object already binded wih + * qPID_BindAutoTuning() + * @note To disable auto-tuning pass a 0uL value to the @a tEnable argument. * @param[in] c A pointer to the PID controller instance. * @param[in] tEnable The number of time steps. To keep the auto tuner - * enabled indefinitely pass #QPID_AUTOTUNNING_UNDEFINED as argument. + * enabled indefinitely pass #QPID_AUTOTUNING_UNDEFINED as argument. * @return 1 on success, otherwise return 0. */ - int qPID_EnableAutoTunning( qPID_controller_t * const c, - const uint32_t tEnable ); + int qPID_EnableAutoTuning( qPID_controller_t * const c, + const uint32_t tEnable ); /** - * @brief Verifies that the auto tuning process has finished with new gains + * @brief Verifies that the auto tuning process has finished with new gains * set on the controller * @param[in] c A pointer to the PID controller instance. - * @return 1 if auto-tunning its complete, otherwise return 0. + * @return 1 if auto-tuning its complete, otherwise return 0. */ - int qPID_AutoTunningComplete( const qPID_controller_t * const c ); + int qPID_AutoTuningComplete( const qPID_controller_t * const c ); /** - * @brief Change parameters of the auto-tunning algorithm. + * @brief Change parameters of the auto-tuning algorithm. * @param[in] c A pointer to the PID controller instance. * @param[in] mu Algorithm momentum. [ 0 <= alfa <= 1 ]. * @param[in] alfa Final controller speed adjustment. [ 0 < alfa <= 1 ]. * @param[in] lambda Algorithm forgetting factor [ 0.8 <= lambda <= 1 ]. * @return 1 on success, 0 on failure. */ - int qPID_AutoTunningSetParameters( qPID_controller_t * const c, - const float mu, - const float alfa, - const float lambda ); + int qPID_AutoTuningSetParameters( qPID_controller_t * const c, + const float mu, + const float alfa, + const float lambda ); /** * @brief Set integration method for the PID controller. * @param[in] c A pointer to the PID controller instance. * @param[in] im The desired integration method. Use one of the following: * - * qNumA_IntegralRe : Integrate using the Rectangular rule. + * @c qNumA_IntegralRe : Integrate using the Rectangular rule. * - * qNumA_IntegralTr : (default) Integrate using the Trapezoidal rule. + * @c qNumA_IntegralTr : (default) Integrate using the Trapezoidal rule. * - * qNumA_IntegralSi : Integrate using the Simpson's 1/3 rule. + * @c qNumA_IntegralSi : Integrate using the Simpson's 1/3 rule. * * @return 1 on success, otherwise return 0. */ @@ -241,6 +342,9 @@ extern "C" { qNumA_IntegrationMethod_t im ); + bool qPID_AutoTunningControllerType( qPID_controller_t *c, + const qPID_Type_t t ); + /** @}*/ #ifdef __cplusplus diff --git a/include/qrms.h b/include/qrms.h index 4f31658..df041b9 100644 --- a/include/qrms.h +++ b/include/qrms.h @@ -1,7 +1,7 @@ /*! * @file qrms.h * @author J. Camilo Gomez C. - * @version 1.03 + * @version 1.04 * @note This file is part of the qLibs distribution. * @brief Computes the RMS (Root Mean Square) of a signal using a 2-step * recursive average specially designed for micro-controllers with FPU. @@ -20,10 +20,9 @@ extern "C" { */ #include - #include - #include #include "qssmoother.h" + /** * @brief RMS calculator instance */ diff --git a/include/qssmoother.h b/include/qssmoother.h index 9328840..0d5fc02 100644 --- a/include/qssmoother.h +++ b/include/qssmoother.h @@ -1,7 +1,7 @@ /*! * @file qssmoother.h * @author J. Camilo Gomez C. - * @version 1.06 + * @version 1.09 * @note This file is part of the qLibs distribution. * @brief API to smooth noisy signals. **/ @@ -15,11 +15,9 @@ extern "C" { #include #include - #include - #include #include "qtdl.h" - /** @addtogroup qssmoother qSSMoother : Filters to smooth noisy signals + /** @addtogroup qssmoother qSSMoother : Filters to smooth noisy signals * @brief API for the qSSmoother library * @{ */ @@ -37,16 +35,18 @@ extern "C" { QSSMOOTHER_TYPE_GMWF, /*!< Gaussian Filter*/ QSSMOOTHER_TYPE_KLMN, /*!< Kalman Filter*/ QSSMOOTHER_TYPE_EXPW, /*!< Exponential weighting filter*/ + QSSMOOTHER_TYPE_DESF, /*!< Double Exponential Smoother*/ + QSSMOOTHER_TYPE_ALNF, /*!< Adaptive Noise Cancellation*/ } qSSmoother_Type_t; /*! @cond */ #define qSSmootherPtr_t void /*abstract class*/ - typedef struct _qSSmoother_s + typedef struct _qSSmoother_s //skipcq: CXX-E2000 { void *vt; uint8_t init; - } _qSSmoother_t; + } _qSSmoother_t; //skipcq: CXX-E2000 /*! @endcond */ /** @@ -157,13 +157,38 @@ extern "C" { float x; /* state */ float A; /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */ float H; /* z(n)=H*x(n)+w(n),w(n)~N(0,r) */ - float q; /* process(predict) noise convariance */ - float r; /* measure noise convariance */ - float p; /* estimated error convariance */ + float q; /* process(predict) noise covariance */ + float r; /* measure noise covariance */ + float p; /* estimated error covariance */ float gain; /*! @endcond */ } qSSmoother_KLMN_t; + /** + * @brief Double exponential smoothing (Holt’s Method) + */ + typedef struct + { + /*! @cond */ + _qSSmoother_t f; + float alpha, beta; + float lt, bt, n; + /*! @endcond */ + } qSSmoother_DESF_t; + + /** + * @brief Adaptive Filter LMS + */ + typedef struct + { + /*! @cond */ + _qSSmoother_t f; + float alpha, mu; + float *w, *w_1, *x; + size_t n; + /*! @endcond */ + } qSSmoother_ALNF_t; + /** * @brief Check if the smoother filter is initialized. * @param[in] s A pointer to the signal smoother instance. @@ -180,6 +205,7 @@ extern "C" { /** * @brief Perform the smooth operation recursively for the input signal @a x. + * @pre Instance must be previously initialized by qSSmoother_Setup() * @param[in] s A pointer to the signal smoother instance. * @param[in] x A sample of the input signal. * @return The smoothed output. @@ -210,38 +236,52 @@ extern "C" { * * - ::QSSMOOTHER_TYPE_EXPW * + * - ::QSSMOOTHER_TYPE_DESF + * + * - ::QSSMOOTHER_TYPE_ALNF + * * @param[in] param The smoother parameters. Depends of the type selected: * - * if ::QSSMOOTHER_TYPE_LPF1 a pointer to a value between [ 0 < alpha < 1 ] + * if ::QSSMOOTHER_TYPE_LPF1 a pointer to a value between [ 0 < @a alpha < 1 ] * - * if ::QSSMOOTHER_TYPE_LPF2 a pointer to a value between [ 0 < alpha < 1 ] + * if ::QSSMOOTHER_TYPE_LPF2 a pointer to a value between [ 0 < @a alpha < 1 ] * - * if ::QSSMOOTHER_TYPE_MWM1 can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_MWM1 can be ignored. Pass @c NULL as argument. * - * if ::QSSMOOTHER_TYPE_MWM2 can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_MWM2 can be ignored. Pass @c NULL as argument. * - * if ::QSSMOOTHER_TYPE_MOR1 a pointer to a value between [ 0 < alpha < 1 ] + * if ::QSSMOOTHER_TYPE_MOR1 a pointer to a value between [ 0 < @a alpha < 1 ] * - * if ::QSSMOOTHER_TYPE_MOR2 a pointer to a value between [ 0 < alpha < 1 ] + * if ::QSSMOOTHER_TYPE_MOR2 a pointer to a value between [ 0 < @a alpha < 1 ] * * if ::QSSMOOTHER_TYPE_GMWF, an array with two values. The first element - * with the Standard deviation [ sigma > 0 ]. The second element with the - * offset of the gaussian center. [ 0 < pos < (wsize-1) ]. + * with the Standard deviation [ @a sigma > 0 ]. The second element with the + * offset of the gaussian center. [ 0 < @a offset < ( @a wsize - 1 ) ]. * * if ::QSSMOOTHER_TYPE_KLMN, an array with three values. The first element - * with the initial estimated error convariance. The second element with the - * process(predict) noise convariance. The third element with the measure + * with the initial estimated error covariance. The second element with the + * process(predict) noise covariance. The third element with the measure * noise convariance * - * if ::QSSMOOTHER_TYPE_EXPW, a pointer to a value between [ 0 < lambda < 1 ] + * if ::QSSMOOTHER_TYPE_EXPW, a pointer to a value between [ 0 < @a lambda < 1 ] * that represents the forgetting factor. * + * if ::QSSMOOTHER_TYPE_DESF, an array with three values. The first element + * [ 0 < @a alpha < 1 ] that represents the weight for the level, the second, + * [ 0 < @a beta < 1 ] weight for the trend. The third element with the number + * of step for the forecast, should be an integer value greater or equal to + * zero. + * + * if ::QSSMOOTHER_TYPE_ALNF, an array with two values. The first element + * with learning rate [ 0 < @a alpha < 1 ]. The second element with the + * momentum [ 0 < @a mu < 1 ]. + * * @param[in] window The filter window and coefficients. Depends of the type * selected: * - * if ::QSSMOOTHER_TYPE_LPF1, can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_LPF1, can be ignored. Pass @c NULL as argument. * - * if ::QSSMOOTHER_TYPE_LPF2, can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_LPF2, can be ignored. Pass @c NULL as argument. * * if ::QSSMOOTHER_TYPE_MWM1, An array of @a wsize elements. * @@ -251,12 +291,17 @@ extern "C" { * * if ::QSSMOOTHER_TYPE_MOR2, An array of @a wsize elements. * - * if ::QSSMOOTHER_TYPE_GMWF, An array of 2x @a wsize to hold both, the + * if ::QSSMOOTHER_TYPE_GMWF, An array of 2* @a wsize to hold both, the * window and the gaussian kernel coefficients. * - * if ::QSSMOOTHER_TYPE_KLMN, can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_KLMN, can be ignored. Pass @c NULL as argument. + * + * if ::QSSMOOTHER_TYPE_EXPW, can be ignored. Pass @c NULL as argument. + * + * if ::QSSMOOTHER_TYPE_DESF, can be ignored. Pass @c NULL as argument. * - * if ::QSSMOOTHER_TYPE_EXPW, can be ignored. Pass NULL as argument. + * if ::QSSMOOTHER_TYPE_ALNF, An array of 2* @a wsize elements when momentum + * is set to zero, otherwise an array of 3* @a wsize elements. * * @param[in] wsize If used, the number of elements in @a window, otherwise * pass 0uL as argument. diff --git a/include/qtdl.h b/include/qtdl.h index 6efc6fc..794f95a 100644 --- a/include/qtdl.h +++ b/include/qtdl.h @@ -44,7 +44,7 @@ extern "C" { * @brief Setup and initialize a Tapped Delay Line (TDL) instance by setting * the default optimal parameters. * @param[in] q A pointer to the TDL instance. - * @param[in] area An array of size @ n where delays will be stored + * @param[in] area An array of size @a n where delays will be stored * @param[in] n The number of elements on @a area. * @param[in] initVal The value with which all TDL delays will be initialized * @return none @@ -89,6 +89,7 @@ extern "C" { /** * @brief Insert a new sample to the TDL removing the oldest sample * @param[in] q A pointer to the TDL instance. + * @param[in] sample The new sample to insert. * @return none */ void qTDL_InsertSample( qTDL_t * const q, diff --git a/include/qtypegeneric.h b/include/qtypegeneric.h new file mode 100644 index 0000000..6af5e59 --- /dev/null +++ b/include/qtypegeneric.h @@ -0,0 +1,246 @@ +/*! + * @file qtypegeneric.h + * @author J. Camilo Gomez C. + * @version 1.01 + * @note This file is part of the qLibs distribution. + * @brief Type-Generic utilities + **/ + +#ifndef QTYPEGENERIC_H +#define QTYPEGENERIC_H + +#ifdef __cplusplus +extern "C" { +#endif + + #include + #include + #include + + + /** @addtogroup qtypegeneric qTypeGeneric + * @brief Type-generic utilities + * @{ + */ + + /*! @cond */ + typedef int (*qTypeGeneric_CompareFcn_t)( const void *, const void *, void * ); + typedef int (*qTypeGeneric_ForEachFcn_t)( int, void *, void * ); + /*! @endcond */ + + /** + * @brief Swaps the data pointed by @a x and @a y + * @param[in,out] x Pointer to data to be swapped. + * @param[in,out] y Pointer to data to be swapped. + * @param[in] n The size of the data to be swapped. + * @return none. + */ + void qTypeGeneric_Swap( void * const x, + void * const y, + size_t n ); + + /** + * @brief Sorts the given array pointed to by @a pbase in ascending order. + * The array contains @a n elements of @a size bytes. Function pointed to + * by @a cmp is used for object comparison. + * @remark This algorithm uses a non-recursive variant of the quicksort + * algorithm. + * @param[in,out] pbase Pointer to the array to sort. + * @param[in] n Number of elements in the array. + * @param[in] size size of each element in the array in bytes + * @param[in] cmp Comparison function which returns ​a negative integer value + * if the first argument is less than the second, a positive integer value + * if the first argument is greater than the second and zero if the arguments + * are equivalent. + * The signature of the comparison function should be equivalent to the + * following: + * @code{.c} + * int cmp( const void *a, const void *b, void *arg ); + * @endcode + * The function must not modify the objects passed to it and must return + * consistent results when called for the same objects, regardless of their + * positions in the array. + * @param[in] arg Additional information (e.g., collating sequence), passed + * to @c cmp as the third argument + * @return none. + */ + void qTypeGeneric_Sort( void * const pbase, + size_t n, + size_t size, + qTypeGeneric_CompareFcn_t cmp, + void *arg ); + + /** + * @brief Reverse the given array pointed to by @a pbase. + * Operation takes place on the portion of the array that starts at position + * @a init to position @a end. + * @param[in,out] pbase Pointer to the array to reverse. + * @param[in] size Size of each element in the array in bytes + * @param[in] init Position of the first element. + * @param[in] end Position of the last element. + * @return none. + */ + void qTypeGeneric_Reverse( void * const pbase, + const size_t size, + const size_t init, + const size_t end ); + + /** + * @brief Rotates @a k elements of the array pointed to by @a pbase. The array + * contains @a n elements of @a size bytes. Rotation direction is determined + * by the sign of @a k, the means a positive value performs a right-rotation + * and a negative value a left-rotation. + * @param[in,out] pbase Pointer to the array to reverse. + * @param[in] size Size of each element in the array in bytes + * @param[in] n Number of elements in the array. + * @param[in] k Positions to rotate. + * @return none. + */ + void qTypeGeneric_Rotate( void * const pbase, + const size_t size, + const size_t n, + const int k ); + + /** + * @brief Set the data pointed by @a ref to every element of the array + * pointed by @a pbase. The array contains @a n elements of @a size bytes + * @param[in,out] pbase Pointer to the array to reverse. + * @param[in] size Size of each element in the array in bytes + * @param[in] n Number of elements in the array. + * @param[in] ref Pointer to the value to be set. + * @return This function returns a pointer to the memory area @a pbase + */ + void* qTypeGeneric_Set( void * const pbase, + const size_t size, + const size_t n, + const void * const ref ); + + /** + * @brief Performs a linear search over an array of @a n elements + * pointed to by @a pbase for an element that matches the object pointed to + * by @a key. + * The size of each element is specified by @a size. The array contents + * should be sorted in ascending order according to the @a compar + * comparison function. This routine should take two arguments pointing to + * the @a key and to an array element, in that order, and should return an + * integer less than, equal to, or greater than zero if the @a key object + * is respectively less than, matching, or greater than the array element. + * @param[in] key This is the pointer to the object that serves as key for + * the search, type-casted as a @c void*. + * @param[in] pbase This is the pointer to the first object of the array + * where the search is performed, type-casted as a @c void*. + * @param[in] n This is the number of elements in the array pointed by + * @a pbase. + * @param[in] size This is the size in bytes of each element in the array. + * @param[in] compar This is the function that compares two elements. + * The signature of the comparison function should be equivalent to the + * following: + * @code{.c} + * int compar( const void *a, const void *b, void *arg ); + * @endcode + * Comparison function which returns ​a negative integer value if the first + * argument is less than the second, a positive integer value if the first + * argument is greater than the second and zero if the arguments are + * equivalent. @a key is passed as the first argument, an element from the + * array as the second. + * The function must not modify the objects passed to it and must return + * consistent results when called for the same objects, regardless of their + * positions in the array. + * @param[in] arg Additional information (e.g., collating sequence), passed + * to @a compar as the third argument + * @return This function returns a pointer to an entry in the array that + * matches the search key. If key is not found, a @c NULL pointer is returned. + */ + void* qTypeGeneric_LSearch( const void *key, + const void *pbase, + const size_t n, + const size_t size, + qTypeGeneric_CompareFcn_t compar, + void *arg ); + + /** + * @brief Performs a binary search over an array of @a n elements + * pointed to by @a pbase for an element that matches the object pointed to + * by @a key. + * The size of each element is specified by @a size. The array contents + * should be sorted in ascending order according to the @a compar + * comparison function. This routine should take two arguments pointing to + * the @a key and to an array element, in that order, and should return an + * integer less than, equal to, or greater than zero if the @a key object + * is respectively less than, matching, or greater than the array element. + * @param[in] key This is the pointer to the object that serves as key for + * the search, type-casted as a @c void*. + * @param[in] pbase This is the pointer to the first object of the array + * where the search is performed, type-casted as a @c void*. + * @param[in] n This is the number of elements in the array pointed by + * @a base. + * @param[in] size This is the size in bytes of each element in the array. + * @param[in] compar This is the function that compares two elements. + * The signature of the comparison function should be equivalent to the + * following: + * @code{.c} + * int compar( const void *a, const void *b, void *arg ); + * @endcode + * Comparison function which returns ​a negative integer value if the first + * argument is less than the second, a positive integer value if the first + * argument is greater than the second and zero if the arguments are + * equivalent. @a key is passed as the first argument, an element from the + * array as the second. + * The function must not modify the objects passed to it and must return + * consistent results when called for the same objects, regardless of their + * positions in the array. + * @param[in] arg Additional information (e.g., collating sequence), passed + * to @a compar as the third argument + * @return This function returns a pointer to an entry in the array that + * matches the search key. If key is not found, a @c NULL pointer is returned. + */ + void* qTypeGeneric_BSearch( const void *key, + const void *pbase, + const size_t n, + const size_t size, + qTypeGeneric_CompareFcn_t compar, + void *arg ); + /** + * @brief Iterates @a n elements of the array pointed to by @a pbase. + * The size of each element is specified by @a size. Every element should be + * handled by function @a f. The iteration loop can be aborted by returning 1. + * @param[in] pbase This is the pointer to the first object of the array + * type-casted as a @c void*. + * @param[in] n This is the number of elements in the array pointed by + * @a pbase. + * @param[in] size This is the size in bytes of each element in the array. + * @param[in] f The function that will handle each element of the array + * The signature of this handling function should be equivalent to the + * following: + * @code{.c} + * int iterFcn( int i, void *element, void *arg ); + * @endcode + * The argument @a i is used to keep track of the iteration in which the loop + * is. + * - i < 0 Loop its a about to start (pre-loop invocation). The + * @a element argument at this stage its passed as @c NULL and should not be + * dereferenced. + * - [ 0 <= i < n ] Loop its iterating and the @a element argument + * is pointing to the array at index @a i. + * - i == n Loop has ended (pos-loop invocation). The @a element + * argument at this stage its passed as @c NULL and should not be dereferenced. + * @param[in] dir Pass @c true to iterate the array backwards. + * @param[in] arg Additional information (e.g., collating sequence), passed + * to @a f as the third argument + * @return This function returns 1 if the iteration loop is aborted otherwise + * returns 0. + */ + int qTypeGeneric_ForEach( void *pbase, + const size_t size, + const size_t n, + qTypeGeneric_ForEachFcn_t f, + const bool dir, + void *arg ); + + /** @}*/ + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/include/qvfloat.h b/include/qvfloat.h new file mode 100644 index 0000000..97ef043 --- /dev/null +++ b/include/qvfloat.h @@ -0,0 +1,273 @@ +/*! + * @file qvfloat.h + * @author J. Camilo Gomez C. + * @version 1.05 + * @note This file is part of the qLibs distribution. + * @brief Floating-point Vector(1D-Array) operations + **/ + +#ifndef QVFLOAT_H +#define QVFLOAT_H + +#ifdef __cplusplus +extern "C" { +#endif + + #include + #include + + + /** @addtogroup qvfloat qFVector + * @brief Floating-point vector(1D-Array) operations + * @{ + */ + + /** + * @brief Supported operators on vectors + */ + typedef enum { + VFLOAT_ADD = 0, /*!< Addition operator*/ + VFLOAT_MUL, /*!< Multiplication operator*/ + VFLOAT_DIV /*!< Division operator*/ + } qVFloat_Operation_t; + + /** + * @brief Metrics returned by qVFloat_Moment() + */ + typedef struct { + float mean; /*!< Mean */ + float avgDev; /*!< Average Deviation */ + float stdDev; /*!< Standard Deviation */ + float var; /*!< Variance */ + float skew; /*!< Skewness */ + float curt; /*!< Kurtosis */ + } qVFloat_Moment_t; + + /** + * @brief Metrics returned by qVFloat_MinMax() + */ + typedef struct { + float min; /*!< Minimal value on vector */ + float max; /*!< Maximal value on vector */ + size_t index_min; /*!< Index where minimum value is found*/ + size_t index_max; /*!< Index where maximum value is found*/ + } qVFloat_MinMax_t; + + /** + * @brief Computes one of the following 1D-vector operation : + * - dst = a*x [o] b*y if both @a x and @a y are supplied. + * - dst = a*x [o] b if the @a y argument is @c NULL. + * + * Here, [o] corresponds to the operator that will be applied + * @note If @a y or @a dst are used, they must have the same length as @a x. + * @param[out] dst The pointer to the destination array where the result + * will be stored. To ignore pass @c NULL as argument. + * @param[in] o The desired operator, should be one of the following: + * - ::VFLOAT_ADD : Addition + * - ::VFLOAT_MUL : Multiplication + * - ::VFLOAT_DIV : Division + * @param[in] a Value to scale the vector @a x. + * @param[in] x A vector as an 1D float array + * @param[in] b Value to scale the vector @a y. + * @param[in] y A vector as an 1D float array. To ignore pass @c NULL as + * argument. + * @param[in] n The number of elements of vector @a x and @a y + * @return The sum of all elements on @a dst after the operation. + */ + float qVFloat_Operate( float * const dst, + qVFloat_Operation_t o, + const float a, + const float * const x, + const float b, + const float * const y, + const size_t n ); + + /** + * @brief Apply one the supplied function ( @a fx1 or @a fx2 ) to the input + * vector(s). + * - dst = a*fx1( x ) if @a fx2 is @c NULL + * - dst = a*fx2( x, y ) if @a fx1 is @c NULL + * - dst = a*fx2( x, b ) if @a fx1 and @a y are @c NULL + * @note If @a y or @a dst are used, they must have the same length as @a x. + * @remark If both function are supplied, only @a fx1 will be applied + * @param[out] dst The pointer to the destination vector where the result + * will be stored. To ignore pass @c NULL as argument. + * @param[in] fx1 Function that will be applied to each element of @a x. This + * function should take one parameter following this signature: + * @code{.c} + * float fx1( float p ); + * @endcode + * To ignore pass @c NULL as argument. + * @param[in] fx2 Function that will be applied to each element of @a x. This + * function should take 2 parameters following this signature: + * @code{.c} + * float fx1( float p1, float p2 ); + * @endcode + * To ignore pass @c NULL as argument. + * @param[in] x A vector as an 1D float array + * @param[in] y A vector as an 1D float array. To ignore pass @c NULL as + * argument. + * @param[in] a A value to scale the result of @a fx1 and @a fx2. + * @param[in] b An scalar value used as second argument on @a fx2 if @a y + * is ignored. + * @param[in] n The number of elements of vectors @a x and @a y + * @return The sum of all elements on @a dst after the operation. + */ + float qVFloat_ApplyFx( float *dst, + float (*fx1)(float), + float (*fx2)(float, float), + float * const x, + float * const y, + const float a, + const float b, + const size_t n ); + + /** + * @brief Computes the moment of a distribution for a given set of data + * including : Mean, Variance, Average deviation, standard deviation, + * skewness and kurtosis. + * @param[out] m A structure of type qVFloat_Moment_t where the metrics will + * be stored. + * @param[in] x A vector of data as an 1D float array + * @param[in] n The number of elements of vectors @a x and @a y + * @return 1 on success, otherwise returns 0. + */ + int qVFloat_Moment( qVFloat_Moment_t * const m, + const float * const x, + const size_t n ); + + /** + * @brief Set the value given on @a c to all the elements of the vector + * pointed by @a x + * @param[in] x A pointer to the input vector + * @param[in] c The value to set + * @param[in] n Number of elements of the vector + * @return A pointer to the vector, same as @a x on success. + */ + float* qVFloat_Set( float * const x, + const float c, + const size_t n ); + + /** + * @brief Copies all the values on vector pointed by @a src to the vector + * pointed by @a dst. + * @param[in] dst Pointer to the destination vector + * @param[in] src Pointer to the source vector + * @param[in] n Number of elements of the vector + * @return A pointer to the vector, same as @a dst. + */ + float* qVFloat_Copy( float * const dst, + const float * const src, + const size_t n ); + + /** + * @brief Takes the input vector @a p as a polynomial p and evaluates it for + * @a x. The argument p is a vector of length @a n+1 whose + * elements are the coefficients (in descending powers) of an nth-degree + * polynomial. + * @param[in] p The polynomial coefficients, specified as a vector. + * @param[in] x Value to evaluate the polynomial + * @param[in] n Number of coefficients on polynomial @a p + * @return A pointer to @a dst. + */ + float qVFloat_PolyVal( const float * const p, + const float x, + size_t n ); + + /** + * @brief Generate linearly spaced vector of @n points. The spacing between + * the points is (x2-x1)/(n-1) + * @param[out] dst The pointer to the destination vector where the result + * will be stored. + * @param[in] x1 Point interval + * @param[in] x2 Point interval + * @param[in] n Number of points + * @return A pointer to @a dst. + */ + float* qVFloat_LinSpace( float * const dst, + const float x1, + const float x2, + const size_t n ); + + /** + * @brief Returns the Euclidean distance between vectors @a x and @a y + * @param[in] x Pointer to the input vector. + * @param[in] y Pointer to the input vector + * @param[in] n Number of elements of the vector + * @return The Euclidean distance value. + */ + float qVFloat_Distance( const float * const x, + const float * const y, + const size_t n ); + /** + * @brief Reverse the given vector pointed by @a src. Operation takes place + * on the portion of the vector that starts at position @a init to position + * @a end. + * @remark If the @a dst argument is ignored, the operation will take place + * over the @a src argument itself. + * @param[out] dst The pointer to the destination vector where the result + * will be stored. To ignore pass @c NULL as argument. + * @param[in,out] src The input vector to reverse. + * @param[in] init Position of the first element. + * @param[in] end Position of the last element. + * @return A pointer to the reversed vector. + */ + float* qVFloat_Reverse( float * const dst, + float * const src, + const size_t init, + const size_t end ); + + /** + * @brief Rotates the elements of vector pointed by @a src the number of + * places and in the direction indicated by @a k. + * @remark If the @a dst argument is ignored, the operation will take place + * over the @a src argument itself. + * @param[out] dst The pointer to the destination vector where the result + * will be stored. To ignore pass @c NULL as argument. + * @param[in,out] src The input vector to rotate. + * @param[in] k Position of the first element. + * @param[in] n Number of elements of the vector + * @return A pointer to the rotated vector. + */ + float* qVFloat_Rotate( float * const dst, + float * const src, + const int k, + const size_t n ); + + /** + * @brief Find the smallest and largest elements of vector pointed by @a x. + * @param[out] o The pointer where the outcome of this function will be stored + * @param[in] x The input vector to. + * @param[in] n Number of places to rotate + * @return 1 on success, otherwise return 0. + */ + int qVFloat_MinMax( qVFloat_MinMax_t * const o, + const float * const x, + const size_t n ); + + /** + * @brief Returns a sorted version of unsorted vector pointed by @a src with + * the elements arranged in ascending or descending order according the value + * or @a dir. + * @note This function is a wrapper to qTypeGeneric_Sort() + * @remark If the @a dst argument is ignored, the operation will take place + * over the @a src argument itself. + * @param[out] dst The pointer to the destination vector where the result + * will be stored. To ignore pass @c NULL as argument. + * @param[in,out] src The input vector to rotate. + * @param[in] dir Pass false to sort in ascending order, otherwise pass true. + * @param[in] n Number of elements of the vector + * @return A pointer to the sorted vector. + */ + float* qVFloat_Sort( float * const dst, + float * const src, + const bool dir, + size_t n ); + + /** @}*/ + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/qbitfield.c b/qbitfield.c index 45de87c..41671a5 100644 --- a/qbitfield.c +++ b/qbitfield.c @@ -1,11 +1,12 @@ /*! * @file qbitfield.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qbitfield.h" +#include -static const size_t qBitField_LBit = (size_t)( sizeof(uint32_t) * 8uL ); +static const size_t qBitField_LBit = (size_t)( sizeof(uint32_t) * 8U ); static uint32_t qBitField_Mask( const size_t index ); static size_t qBitField_BitSlot( const size_t index ); @@ -30,7 +31,7 @@ static void qBitField_Write_uint32( qBitField_t *b, /*============================================================================*/ static uint32_t qBitField_Mask( const size_t index ) { - return (uint32_t)1uL << ( index % qBitField_LBit ); + return (uint32_t)1U << ( index % qBitField_LBit ); } /*============================================================================*/ static size_t qBitField_BitSlot( const size_t index ) @@ -43,7 +44,7 @@ static uint32_t qBitField_BitGet( const qBitField_t * const b, { const size_t slot = qBitField_BitSlot( index ); - return ( b->field[ slot ] >> ( index % qBitField_LBit ) ) & 1uL; + return ( b->field[ slot ] >> ( index % qBitField_LBit ) ) & 1U; } /*============================================================================*/ static void qBitField_BitSet( qBitField_t * const b, @@ -81,7 +82,7 @@ static uint32_t qBitField_SafeMask( const uint32_t val, /*============================================================================*/ static size_t qBitField_Offset( const size_t index ) { - return index & (size_t)31u; + return index & (size_t)31U; } /*============================================================================*/ static uint32_t qBitField_MaskMerge( const uint32_t w, @@ -97,11 +98,12 @@ int qBitField_Setup( qBitField_t * const b, { int retValue = 0; - if ( ( NULL != b ) && ( NULL != area ) && ( area_size > 0u ) ) { + if ( ( NULL != b ) && ( NULL != area ) && ( area_size > 0U ) ) { /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ b->field = (uint32_t *)area; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ - b->size = area_size*8u; + b->size = area_size*8U; b->nSlots = area_size/sizeof(uint32_t); retValue = 1; } @@ -114,7 +116,7 @@ int qBitField_ClearAll( qBitField_t * const b ) int retValue = 0; if ( NULL != b ) { - (void)memset( b->field, 0, b->size/8u ); + (void)memset( b->field, 0, b->size/8U ); retValue = 1; } @@ -126,7 +128,7 @@ int qBitField_SetAll( qBitField_t * const b ) int retValue = 0; if ( NULL != b ) { - (void)memset( b->field, 0xFF, b->size/8u ); + (void)memset( b->field, 0xFF, b->size/8U ); retValue = 1; } @@ -175,10 +177,10 @@ int qBitField_ToggleBit( qBitField_t * const b, uint8_t qBitField_ReadBit( const qBitField_t * const b, const size_t index ) { - uint8_t retValue = 0u; + uint8_t retValue = 0U; if ( NULL != b ) { - retValue = ( 0uL != qBitField_BitGet( b, index ) )? 1u : 0u; + retValue = ( 0U != qBitField_BitGet( b, index ) )? 1U : 0U; } return retValue; @@ -191,7 +193,7 @@ int qBitField_WriteBit( qBitField_t * const b, int retValue = 0; if ( NULL != b ) { - if ( 0u != value ) { + if ( 0U != value ) { qBitField_BitSet( b, index ); } else { @@ -207,18 +209,18 @@ uint32_t qBitField_ReadUINTn( const qBitField_t * const b, const size_t index, size_t xBits ) { - uint32_t retValue = 0uL; + uint32_t retValue = 0U; - if ( ( NULL != b ) && ( xBits <= 32u ) ) { - if ( 1u == xBits ) { + if ( ( NULL != b ) && ( xBits <= 32U ) ) { + if ( 1U == xBits ) { retValue = (uint32_t)qBitField_ReadBit( b, index ); } - else if ( 32u == xBits ) { + else if ( 32U == xBits ) { retValue = qBitField_Read_uint32( b, index ); } else { retValue = qBitField_Read_uint32( b, index ); - retValue &= qBitField_SafeMask( 0xFFFFFFFFuL, 32u, xBits ); + retValue &= qBitField_SafeMask( 0xFFFFFFFFU, 32U, xBits ); } } @@ -232,20 +234,20 @@ int qBitField_WriteUINTn( qBitField_t * const b, { int retValue = 0; - if ( ( NULL != b ) && ( xBits <= 32u ) ) { + if ( ( NULL != b ) && ( xBits <= 32U ) ) { uint32_t w, mask; - if ( 1u == xBits ) { + if ( 1U == xBits ) { (void)qBitField_WriteBit( b, index, (uint8_t)value ); } - else if ( 32u == xBits ) { + else if ( 32U == xBits ) { qBitField_Write_uint32( b, index, value ); } else { w = qBitField_Read_uint32( b, index ); - value &= qBitField_SafeMask( 0xFFFFFFFFuL, 32u, xBits ); + value &= qBitField_SafeMask( 0xFFFFFFFFU, 32U, xBits ); /*cstat -ATH-overflow*/ - mask = (uint32_t)0xFFFFFFFFuL << xBits; + mask = (uint32_t)0xFFFFFFFFU << (uint32_t)xBits; /*cstat +ATH-overflow*/ qBitField_Write_uint32( b, index, qBitField_MaskMerge( w, value, mask ) ); } @@ -258,11 +260,13 @@ int qBitField_WriteUINTn( qBitField_t * const b, float qBitField_ReadFloat( const qBitField_t * const b, const size_t index ) { - float retValue = 0.0f; + float retValue = 0.0F; if ( NULL != b ) { uint32_t rval; + rval = qBitField_Read_uint32( b, index ); + /*cppcheck-suppress misra-c2012-21.15 */ (void)memcpy( &retValue, &rval, sizeof(float) ); } @@ -276,7 +280,8 @@ int qBitField_WriteFloat( qBitField_t * const b, int retValue = 0; if ( NULL != b ) { - uint32_t fval = 0uL; + uint32_t fval = 0U; + /*cppcheck-suppress misra-c2012-21.15 */ (void)memcpy( &fval, &value, sizeof(float) ); qBitField_Write_uint32( b, index, fval ); retValue = 1; @@ -292,7 +297,7 @@ void* qBitField_Dump( const qBitField_t * const b, void *retValue = NULL; if ( ( NULL != b ) && ( NULL != dst ) ) { - if ( n <= ( b->size/8u ) ) { + if ( n <= ( b->size/8U ) ) { retValue = memcpy( dst, (const void*)b->field, n ); } } @@ -309,10 +314,10 @@ static uint32_t qBitField_Read_uint32( const qBitField_t * const b, slot = qBitField_BitSlot( index ); of = qBitField_Offset( index ); result = b->field[ slot ] >> of; - bits_taken = 32u - of; - if ( ( 0u != of ) && ( ( index + bits_taken ) < b->size ) ) { + bits_taken = 32U - of; + if ( ( 0U != of ) && ( ( index + bits_taken ) < b->size ) ) { /*cstat -CERT-INT30-C_a -ATH-shift-bounds -MISRAC2012-Rule-12.2 -CERT-INT34-C_b*/ - result |= b->field[ slot + 1u ] << (uint32_t)bits_taken; + result |= b->field[ slot + 1U ] << (uint32_t)bits_taken; /*cstat +CERT-INT30-C_a +ATH-shift-bounds +MISRAC2012-Rule-12.2 +CERT-INT34-C_b*/ } @@ -328,14 +333,14 @@ static void qBitField_Write_uint32( qBitField_t *b, slot = qBitField_BitSlot( index ); of = qBitField_Offset( index ); - if ( 0u == of ) { + if ( 0U == of ) { b->field[ slot ] = value; } else { - mask = qBitField_SafeMask( 0xFFFFFFFFuL, qBitField_LBit, of ); + mask = qBitField_SafeMask( 0xFFFFFFFFU, qBitField_LBit, of ); b->field[ slot ] = ( value << of ) | ( b->field[ slot ] & mask ); if ( ++slot < b->nSlots ) { - b->field[ slot ] = qBitField_SafeMask( value, 32u, of ) | + b->field[ slot ] = qBitField_SafeMask( value, 32U, of ) | ( b->field[ slot ] & ( ~mask ) ); } } diff --git a/qcrc.c b/qcrc.c index 8559122..88c52e0 100644 --- a/qcrc.c +++ b/qcrc.c @@ -1,7 +1,7 @@ /*! * @file qcrc.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qcrc.h" @@ -16,14 +16,15 @@ static uint32_t qCRCx_Reflect( uint32_t xData, uint32_t r = 0; uint8_t xBit; /*Reflect the data about the center bit*/ - for ( xBit= 0u ; xBit < nBits ; ++xBit ) { + for ( xBit= 0U ; xBit < nBits ; ++xBit ) { /*if the LSB bit is set, set the reflection of it*/ - if ( 0u != ( xData & 0x01u ) ) { + if ( 0U != ( xData & 0x01U ) ) { /*cstat -MISRAC2012-Rule-10.8 -ATH-shift-bounds -MISRAC2012-Rule-12.2 -CERT-INT34-C_b*/ - r |= (uint32_t)( 1u << ( ( nBits - 1u ) - xBit ) ); + /*cppcheck-suppress misra-c2012-10.8 */ + r |= (uint32_t)( 1U << ( ( nBits - 1U ) - xBit ) ); /*cstat +MISRAC2012-Rule-10.8 +ATH-shift-bounds +MISRAC2012-Rule-12.2 +CERT-INT34-C_b*/ } - xData >>= 1u; + xData >>= 1U; } return r; @@ -38,37 +39,38 @@ uint32_t qCRCx( const qCRC_Mode_t mode, const uint8_t refOut, uint32_t xorOut ) { - uint32_t crc = 0uL; + uint32_t crc = 0U; /*cstat -ATH-cmp-unsign-pos*/ - if ( ( NULL != pData ) && ( length > 0u ) && ( mode >= QCRC8 ) && ( mode <= QCRC32 ) ) { + if ( ( NULL != pData ) && ( length > 0U ) && ( mode >= QCRC8 ) && ( mode <= QCRC32 ) ) { /*cstat +ATH-cmp-unsign-pos*/ size_t i; uint8_t xBit; - const uint32_t widthValues[ 3 ] = { 8uL, 16uL, 32uL }; + const uint32_t widthValues[ 3 ] = { 8UL, 16UL, 32UL }; const uint32_t width = widthValues[ mode ]; /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ uint8_t const * const msg = pData; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ - const uint32_t wd1 = (uint32_t)width - 8u; - const uint32_t topBit = 1uL << ( width - 1uL ); - const uint32_t bitMask = ( 0xFFFFFFFFuL >> ( 32uL - width ) ); + const uint32_t wd1 = (uint32_t)width - 8U; + const uint32_t topBit = (uint32_t)1U << ( width - 1U ); + const uint32_t bitMask = ( 0xFFFFFFFFU >> ( 32U - width ) ); poly &= bitMask; xorOut &= bitMask; crc = init; /*Perform modulo-2 division, a byte at a time. */ - for ( i = 0u ; i < length ; ++i ) { + for ( i = 0U ; i < length ; ++i ) { /*cstat -CERT-INT34-C_a*/ - crc ^= ( 0u != refIn ) ? - ( qCRCx_Reflect( (uint32_t)msg[ i ], 8u ) << wd1 ) : + crc ^= ( 0U != refIn ) ? + ( qCRCx_Reflect( (uint32_t)msg[ i ], 8U ) << wd1 ) : ( (uint32_t)msg[ i ] << wd1 ); /*cstat +CERT-INT34-C_a*/ - for ( xBit = 8u ; xBit > 0u ; --xBit ) { + for ( xBit = 8U ; xBit > 0U ; --xBit ) { /*try to divide the current data bit*/ - crc = ( 0u != ( crc & topBit ) ) ? ( ( crc << 1u ) ^ poly ) - : ( crc << 1u ); + crc = ( 0U != ( crc & topBit ) ) ? ( ( crc << 1U ) ^ poly ) + : ( crc << 1U ); } } - crc = ( 0u != refOut ) ? ( qCRCx_Reflect( crc, (uint8_t)width )^xorOut ) + crc = ( 0U != refOut ) ? ( qCRCx_Reflect( crc, (uint8_t)width )^xorOut ) : ( crc^xorOut ); crc &= bitMask; } diff --git a/qffmath.c b/qffmath.c new file mode 100644 index 0000000..e675f6b --- /dev/null +++ b/qffmath.c @@ -0,0 +1,1071 @@ +/*! + * @file qffmath.c + * @author J. Camilo Gomez C. + * @note This file is part of the qLibs distribution. + **/ + +#include "qffmath.h" + +#ifndef QLIBS_USE_STD_MATH +#include +#include +#include + +/*cppcheck-suppress misra-c2012-20.7 */ +#define cast_reinterpret( dst, src, dst_type ) \ +(void)memcpy( &dst, &src, sizeof(dst_type) ) \ + +static float qFFMath_CalcCbrt( float x , bool r ); +static float lgamma_positive( float x ); + +/*============================================================================*/ +float _qFFMath_GetAbnormal( const int i ) +{ + static const uint32_t u_ab[ 2 ] = { 0x7F800000U, 0x7FBFFFFFU }; + static float f_ab[ 2 ] = { 0.0F, 0.0F }; + static bool init = true; + + if ( init ) { + /*cppcheck-suppress misra-c2012-21.15 */ + (void)memcpy( f_ab, u_ab, sizeof(f_ab) ); + init = false; + } + + return f_ab[ i ]; +} +/*============================================================================*/ +int qFFMath_FPClassify( const float f ) +{ + uint32_t u = 0U; + int retVal; + + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( u, f, uint32_t ); + u &= 0x7FFFFFFFU; + + if ( 0U == u ) { + retVal = QFFM_FP_ZERO; + } + else if ( u < 0x00800000U ) { + retVal = QFFM_FP_SUBNORMAL; + } + else if ( u < 0x7F800000U ) { + retVal = QFFM_FP_NORMAL; + } + else if ( 0x7F800000U == u ) { + retVal = QFFM_FP_INFINITE; + } + else { + retVal = QFFM_FP_NAN; + } + + return retVal; +} +/*============================================================================*/ +bool qFFMath_IsNaN( const float x ) +{ + return ( QFFM_FP_NAN == qFFMath_FPClassify( x ) ); +} +/*============================================================================*/ +bool qFFMath_IsInf( const float x ) +{ + return ( QFFM_FP_INFINITE == qFFMath_FPClassify( x ) ); +} +/*============================================================================*/ +bool qFFMath_IsFinite( const float x ) +{ + return ( qFFMath_FPClassify( x ) < QFFM_FP_INFINITE ); +} +/*============================================================================*/ +bool qFFMath_IsNormal( const float x ) +{ + return ( qFFMath_FPClassify( x ) == QFFM_FP_NORMAL ); +} +/*============================================================================*/ +bool qFFMath_IsAlmostEqual( const float a, + const float b, + const float tol ) +{ + return ( qFFMath_Abs( a - b ) <= qFFMath_Abs( tol ) ); +} +/*============================================================================*/ +bool qFFMath_IsEqual( const float a, + const float b ) +{ + return ( qFFMath_Abs( a - b ) <= FLT_MIN ); +} +/*============================================================================*/ +float qFFMath_Abs( float x ) +{ + return ( x < 0.0F ) ? -x : x; +} +/*============================================================================*/ +float qFFMath_Recip( float x ) +{ + uint32_t y = 0U; + float z = 0.0F; + + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( y, x, uint32_t ); + y = 0x7EF311C7U - y; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( z, y, float ); + + return z*( 2.0F - ( x*z ) ); +} +/*============================================================================*/ +float qFFMath_Sqrt( float x ) +{ + float retVal; + + if ( x < 0.0F ) { + retVal = QFFM_NAN; + } + else if ( QFFM_FP_ZERO == qFFMath_FPClassify( x ) ) { + retVal = 0.0F; + } + else { + uint32_t y = 0U; + float z = 0.0F; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( y, x, uint32_t ); + y = ( ( y - 0x00800000U ) >> 1U ) + 0x20000000U; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( z, y, float ); + z = ( ( x/z ) + z ) * 0.5F; + retVal = 0.5F*( ( x/z ) + z ); + } + + return retVal; +} +/*============================================================================*/ +float qFFMath_RSqrt( float x ) +{ + float retVal; + + if ( x < 0.0F ) { + retVal = QFFM_NAN; + } + else if ( QFFM_FP_ZERO == qFFMath_FPClassify( x ) ) { + retVal = QFFM_INFINITY; + } + else { + uint32_t y = 0U; + float z = 0.5F*x; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( y, x, uint32_t ); + y = 0x5F375A86U - ( y >> 1U ); + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( x, y, float ); + retVal = x*( 1.5F - ( z*x*x ) ); + } + + return retVal; +} +/*============================================================================*/ +static float qFFMath_CalcCbrt( float x , bool r ) +{ + float retVal, y = 0.0F, c, d; + const float k[ 3 ] = { 1.752319676F, 1.2509524245F, 0.5093818292F }; + uint32_t i = 0U; + bool neg = false; + + if ( x < 0.0F ) { + x = -x; + neg = true; + } + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( i, x, uint32_t ); + i = 0x548C2B4BU - ( i/3U ); + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( y, i, float ); + c = x*y*y*y; + y = y*( k[ 0 ] - ( c*( k[ 1 ] - ( k[ 2 ]*c ) ) ) ); + + d = x*y*y; + c = 1.0F - ( d*y ); + retVal = 1.0F + ( 0.333333333333F*c ); + retVal *= ( r ) ? y : d; + return ( neg )? -retVal : retVal; +} +/*============================================================================*/ +float qFFMath_Cbrt( float x ) +{ + return qFFMath_CalcCbrt( x, false ); +} +/*============================================================================*/ +float qFFMath_RCbrt( float x ) +{ + return ( QFFM_FP_ZERO == qFFMath_FPClassify( x ) ) ? QFFM_INFINITY + : qFFMath_CalcCbrt( x, true ); +} +/*============================================================================*/ +float qFFMath_Round( float x ) +{ + x += 12582912.0F; + x -= 12582912.0F; + return x; +} +/*============================================================================*/ +float qFFMath_Floor( float x ) +{ + return qFFMath_Round( x - 0.5F ); +} +/*============================================================================*/ +float qFFMath_Ceil( float x ) +{ + return qFFMath_Round( x + 0.5F ); +} +/*============================================================================*/ +float qFFMath_Trunc( float x ) +{ + /*cstat -CERT-FLP36-C -CERT-FLP34-C*/ + return (float)( (int32_t)x ); + /*cstat +CERT-FLP36-C +CERT-FLP34-C*/ +} +/*============================================================================*/ +float qFFMath_Frac( float x ) +{ + return x - qFFMath_Trunc( x ); +} +/*============================================================================*/ +float qFFMath_Remainder( float x, + float y ) +{ + return x - ( y*qFFMath_Floor( x/y ) ); +} +/*============================================================================*/ +float qFFMath_Mod( float x, + float y ) +{ + return ( QFFM_FP_ZERO == qFFMath_FPClassify( x )) ? QFFM_NAN + : ( x - ( y*qFFMath_Trunc( x/y ) ) ); +} +/*============================================================================*/ +float qFFMath_Sin( float x ) +{ + float y; + + if ( qFFMath_Abs( x ) <= 0.0066F ) { + y = x; + } + else { + x *= -QFFM_1_PI; + y = x + 25165824.0F; + x -= y - 25165824.0F; + x *= qFFMath_Abs( x ) - 1.0F; + y = x*( ( 3.5841304553896F*qFFMath_Abs( x ) ) + 3.1039673861526F ); + } + return y; +} +/*============================================================================*/ +float qFFMath_Cos( float x ) +{ + float y; + const float abs_x = qFFMath_Abs( x ); + + if ( qFFMath_IsEqual( abs_x, QFFM_PI_2 ) ) { + y = 1.0e-12F; + } + else { + y = qFFMath_Sin( x + QFFM_PI_2 ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Tan( float x ) +{ + return qFFMath_Sin( x )/qFFMath_Cos( x ); +} +/*============================================================================*/ +float qFFMath_ASin( float x ) +{ + x = qFFMath_Sqrt( 1.0F + x ) - qFFMath_Sqrt( 1.0F - x ); + return x*( ( 0.131754508171F*qFFMath_Abs( x ) ) + 0.924391722181F ); +} +/*============================================================================*/ +float qFFMath_ACos( float x ) +{ + return QFFM_PI_2 - qFFMath_ASin( x ); +} +/*============================================================================*/ +float qFFMath_ATan( float x ) +{ + float abs_x; + + x /= qFFMath_Abs( x ) + 1.0F; + abs_x = qFFMath_Abs( x ); + + return x*( ( abs_x*( ( -1.45667498914F*abs_x ) + 2.18501248371F ) ) + 0.842458832225F ); +} +/*============================================================================*/ +float qFFMath_ATan2( float y, float x ) +{ + float t, f; + + t = QFFM_PI - ( ( y < 0.0F ) ? 6.283185307F : 0.0F ); + f = ( qFFMath_Abs( x ) <= FLT_MIN ) ? 1.0F : 0.0F; + y = qFFMath_ATan( y/( x + f ) ) + ( ( x < 0.0F ) ? t : 0.0F ); + + return y + ( f*( ( 0.5F*t ) - y ) ); +} +/*============================================================================*/ +float qFFMath_Exp2( float x ) +{ + float retVal; + + if ( x <= -126.0F ) { + retVal = 0.0F; + } + else if ( x > 128.0F ) { + retVal = QFFM_INFINITY; + } + else { + float ip, fp; + float ep_f = 0.0F; + int32_t ep_i; + + ip = qFFMath_Floor( x + 0.5F ); + fp = x - ip; + /*cstat -MISRAC2012-Rule-10.1_R6 -CERT-FLP34-C*/ + ep_i = ( (int32_t)( ip ) + 127 ) << 23; + /*cstat +MISRAC2012-Rule-10.1_R6 +CERT-FLP34-C*/ + x = 1.535336188319500e-4F; + x = ( x*fp ) + 1.339887440266574e-3F; + x = ( x*fp ) + 9.618437357674640e-3F; + x = ( x*fp ) + 5.550332471162809e-2F; + x = ( x*fp ) + 2.402264791363012e-1F; + x = ( x*fp ) + 6.931472028550421e-1F; + x = ( x*fp ) + 1.0F; + cast_reinterpret( ep_f, ep_i, float ); + retVal = ep_f*x; + } + + return retVal; +} +/*============================================================================*/ +float qFFMath_Log2( float x ) +{ + float retVal; + + if ( x < 0.0F ) { + retVal = QFFM_NAN; + } + else if ( QFFM_FP_ZERO == qFFMath_FPClassify( x ) ) { + retVal = -QFFM_INFINITY; + } + else { + float z, px; + int32_t ip, fp; + int32_t val_i = 0; + + cast_reinterpret( val_i, x, int32_t ); + /*cstat -MISRAC2012-Rule-10.1_R6*/ + fp = val_i & 8388607; + ip = val_i & 2139095040; + fp |= 1065353216; + cast_reinterpret( x, fp, float ); + ip >>= 23; + ip -= 127; + /*cstat +MISRAC2012-Rule-10.1_R6*/ + if ( x > QFFM_SQRT2 ) { + x *= 0.5F; + ++ip; + } + x -= 1.0F; + px = 7.0376836292e-2F; + px = ( px*x ) - 1.1514610310e-1F; + px = ( px*x ) + 1.1676998740e-1F; + px = ( px*x ) - 1.2420140846e-1F; + px = ( px*x ) + 1.4249322787e-1F; + px = ( px*x ) - 1.6668057665e-1F; + px = ( px*x ) + 2.0000714765e-1F; + px = ( px*x ) - 2.4999993993e-1F; + px = ( px*x ) + 3.3333331174e-1F; + z = x*x; + z = ( x*z*px ) - ( 0.5F*z ) + x; + z *= QFFM_LOG2E; + /*cstat -CERT-FLP36-C*/ + retVal = ( (float)ip ) + z; + /*cstat +CERT-FLP36-C*/ + } + + return retVal; +} +/*============================================================================*/ +float qFFMath_Exp( float x ) +{ + return qFFMath_Exp2( QFFM_LOG2E*x ); +} +/*============================================================================*/ +float qFFMath_Exp10( float x ) +{ + return qFFMath_Exp2( 3.32192809F*x ); +} +/*============================================================================*/ +float qFFMath_Log( float x ) +{ + return QFFM_LN2*qFFMath_Log2(x); +} +/*============================================================================*/ +float qFFMath_Log10( float x ) +{ + return 0.301029996F*qFFMath_Log2(x); +} +/*============================================================================*/ +float qFFMath_Pow( float b, + float e ) +{ + return qFFMath_Exp2( e*qFFMath_Log2( b ) ); +} +/*============================================================================*/ +float qFFMath_Sinh( float x ) +{ + const float epx = qFFMath_Exp( x ); + const float enx = 1.0F/epx; + return 0.5F*( epx - enx ); +} +/*============================================================================*/ +float qFFMath_Cosh( float x ) +{ + const float epx = qFFMath_Exp( x ); + const float enx = 1.0F/epx; + return 0.5F*( epx + enx ); +} +/*============================================================================*/ +float qFFMath_Tanh( float x ) +{ + x = qFFMath_Exp( -2.0F*x ); + return ( 1.0F - x )/( 1.0F + x ); +} +/*============================================================================*/ +float qFFMath_ASinh( float x ) +{ + return qFFMath_Log( x + qFFMath_Sqrt( ( x*x ) + 1.0F ) ); +} +/*============================================================================*/ +float qFFMath_ACosh( float x ) +{ + return ( x < 1.0F ) ? QFFM_NAN + : qFFMath_Log( x + qFFMath_Sqrt( ( x*x ) - 1.0F ) ); +} +/*============================================================================*/ +float qFFMath_ATanh( float x ) +{ + return qFFMath_Log( ( 1.0F + x )/( 1.0F - x ) )*0.5F; +} +/*============================================================================*/ +float qFFMath_Erf( float x ) +{ + float retVal; + + if ( x >= 6.912F ) { + retVal = 1.0F; + } + else { + x = qFFMath_Exp( 3.472034176F*x ); + retVal = ( x/( ( qFFMath_Abs( x ) + 1.0F )*2.0F ) ) - 1.0F; + } + + return retVal; +} +/*============================================================================*/ +float qFFMath_Erfc( float x ) +{ + return 1.0F - qFFMath_Erf( x ); +} +/*============================================================================*/ +float qFFMath_Max( float x, + float y ) +{ + return ( x > y ) ? x : y; +} +/*============================================================================*/ +float qFFMath_Min( float x, + float y ) +{ + return ( x < y ) ? x : y; +} +/*============================================================================*/ +float qFFMath_RExp( float x, + int32_t *pw2 ) +{ + uint32_t lu = 0U, iu; + int32_t i = 0; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( lu, x, uint32_t ); + iu = ( lu >> 23U ) & 0x000000FFU; /* Find the exponent (power of 2) */ + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( i, iu, int32_t ); + i -= 0x7E; + pw2[ 0 ] = (int)i; + lu &= 0x807FFFFFU; /* strip all exponent bits */ + lu |= 0x3F000000U; /* mantissa between 0.5 and 1 */ + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( x, lu, float ); + + return x; +} +/*============================================================================*/ +float qFFMath_LDExp( float x, + int32_t pw2 ) +{ + uint32_t lu = 0U, eu; + int32_t e = 0; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( lu, x, uint32_t ); + eu = ( lu >> 23U ) & 0x000000FFU; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( e, eu, int32_t ); + e += pw2; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( eu, e, uint32_t ); + lu = ( ( eu & 0xFFU ) << 23U ) | ( lu & 0x807FFFFFU ); + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( x, lu, float ); + + return x; +} +/*============================================================================*/ +float qFFMath_Hypot( float x, + float y ) +{ + float retVal; + /*cstat -MISRAC2012-Rule-13.5*/ + if ( qFFMath_IsFinite( x ) && qFFMath_IsFinite( y ) ) { + float a, b, an, bn;; + int32_t e = 0; + + if ( x >= y ) { + a = x; + b = y; + } + else { + a = y; + b = x; + } + /* Write a = an * 2^e, b = bn * 2^e with 0 <= bn <= an < 1.*/ + an = qFFMath_RExp( a, &e ); + bn = qFFMath_LDExp( b, -e ); + retVal = qFFMath_Sqrt( ( an*an ) + ( bn*bn ) ); + retVal = qFFMath_LDExp( retVal, e ); + } + else { + retVal = ( qFFMath_IsInf( x ) || qFFMath_IsInf( y ) ) ? QFFM_INFINITY + : QFFM_NAN; + } + /*cstat +MISRAC2012-Rule-13.5*/ + return retVal; +} +/*============================================================================*/ +float qFFMath_NextAfter( float x, + float y ) +{ + float retVal; + uint32_t ax, ay, uxi = 0U, uyi = 0U; + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( uxi, x, uint32_t ); + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( uyi, y, uint32_t ); + /*cstat -MISRAC2012-Rule-13.5*/ + if ( qFFMath_IsNaN( x ) || qFFMath_IsNaN( y ) ) { + /*cstat +MISRAC2012-Rule-13.5*/ + retVal = QFFM_NAN; + } + else if ( uxi == uyi ) { + retVal = y; + } + else { + ax = uxi & 0x7FFFFFFFU; + ay = uyi & 0x7FFFFFFFU; + if ( 0U == ax ) { + uxi = ( 0U == ay ) ? uyi : ( ( uyi & 0x80000000U ) | 1U ); + } + else if ( ( ax > ay ) || ( 0U != ( ( uxi^uyi ) & 0x80000000U ) ) ) { + uxi--; + } + else { + uxi++; + } + /*cppcheck-suppress misra-c2012-21.15 */ + cast_reinterpret( retVal, uxi, float ); + } + + return retVal; +} +/*============================================================================*/ +float qFFMath_Midpoint( float a, + float b ) +{ + float y; + const float lo = 2.0F*FLT_MIN; + const float hi = 0.5F*FLT_MAX; + const float abs_a = qFFMath_Abs( a ); + const float abs_b = qFFMath_Abs( b ); + + if ( ( abs_a <= hi ) && ( abs_b <= hi ) ) { + y = 0.5F*( a + b ); + } + else if ( abs_a < lo ) { + y = a + ( 0.5F*b ); + } + else if ( abs_b < lo) { + y = ( 0.5F*a ) + b; + } + else { + y = ( 0.5F*a ) + ( 0.5F*b ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Lerp( float a, + float b, + float t ) +{ + float y; + + if ( ( ( a <= 0.0F ) && ( b >= 0.0F ) ) || ( ( a >= 0.0F ) && ( b <= 0.0F ) ) ) { + y = ( t*b ) + ( a*( 1.0F - t ) ); + } + else if ( qFFMath_IsEqual( t, 1.0F ) ) { + y = b; + } + else { + const float x = a + ( t*( b - a ) ); + y = ( ( t > 1.0F ) == ( b > a ) ) ? ( ( b < x ) ? x : b ) + : ( ( b > x ) ? x : b ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Normalize( const float x, + const float xMin, + const float xMax ) +{ + return ( x - xMin )/( xMax - xMin ); +} +/*============================================================================*/ +float qFFMath_Map( const float x, + const float xMin, + const float xMax, + const float yMin, + const float yMax ) +{ + return ( ( yMax - yMin )*qFFMath_Normalize( x, xMin, xMax ) ) + yMin; +} +/*============================================================================*/ +bool qFFMath_InRangeCoerce( float * const x, + const float lowerL, + const float upperL ) +{ + bool retVal = false; + + if ( qFFMath_IsNaN( x[ 0 ] ) ) { + x[ 0 ] = lowerL; + } + else { + if ( x[ 0 ] < lowerL ) { + x[ 0 ] = lowerL; + } + else if ( x[ 0 ] > upperL ) { + x[ 0 ] = upperL; + } + else { + retVal = true; + } + } + + return retVal; +} +/*============================================================================*/ +bool qFFMath_InPolygon( const float x, + const float y, + const float * const px, + const float * const py, + const size_t p ) +{ + size_t i; + bool retVal = false; + float max_y = py[ 0 ], max_x = px[ 0 ], min_y = py[ 0 ], min_x = px[ 0 ]; + + for ( i = 0U ; i < p ; ++i ) { + max_y = ( py[ i ] > max_y ) ? py[ i ] : max_y; + max_x = ( px[ i ] > max_x ) ? px[ i ] : max_x; + min_y = ( py[ i ] < min_y ) ? py[ i ] : min_y; + min_x = ( px[ i ] < min_x ) ? px[ i ] : min_x; + } + + if ( ( y >= min_y ) && ( y <= max_y ) && ( x >= min_x ) && ( x <= max_x ) ) { + size_t j = p - 1U; + + for ( i = 0U ; i < p ; ++i ) { + if ( ( px[ i ] > x ) != ( px[ j ] > x ) ) { + const float dx = px[ j ] - px[ i ]; + const float dy = py[ j ] - py[ i ]; + if ( y < ( ( dy*( x - px[ i ] ) )/( dx + py[ i ] ) ) ) { + retVal = !retVal; + } + } + j = i; + } + } + + return retVal; +} +/*============================================================================*/ +bool qFFMath_InCircle( const float x, + const float y, + const float cx, + const float cy, + const float r ) +{ + const float d = ( ( x - cx )*( x - cx ) ) + ( ( y - cy )*( y - cy ) ); + return ( d <= ( r*r ) ); +} +/*============================================================================*/ +float qFFMath_TGamma( float x ) +{ + float result; + + const int fClass = qFFMath_FPClassify( x ); + if ( QFFM_FP_NAN == fClass ) { + result = QFFM_NAN; + } + else if ( QFFM_FP_ZERO == fClass ) { + result = QFFM_INFINITY; /* a huge value */ + } + else if ( QFFM_FP_INFINITE == fClass ) { + result = ( x > 0.0F ) ? QFFM_INFINITY : QFFM_NAN; + } + else { + bool parity = false; + float fact = 1.0F; + float y = x; + float y1; + + if ( y <= 0.0F ) { + float isItAnInt; + + y = -x; + y1 = qFFMath_Trunc( y ); + isItAnInt = y - y1; + if ( !qFFMath_IsEqual( 0.0F, isItAnInt ) ) { + const float tmp = 2.0F*qFFMath_Trunc( y1*0.5F ); + + if ( !qFFMath_IsEqual( y1, tmp ) ) { + parity = true; + } + fact = -QFFM_PI/qFFMath_Sin( QFFM_PI*isItAnInt ); + y += 1.0F; + } + else { + result = QFFM_NAN; + goto EXIT_TGAMMA; + } + } + if ( y < FLT_EPSILON ) { + if ( y >= FLT_MIN ) { + result = 1.0F/y; + } + else { + result = QFFM_INFINITY; + } + } + else if ( y < 12.0F ) { + float num = 0.0F, den = 1.0F, z; + int n = 0; + + y1 = y; + if ( y < 1.0F ) { + z = y; + y += 1.0F; + } + else { + n = (int)y - 1; + /*cstat -CERT-FLP36-C */ + y -= (float)n; + /*cstat +CERT-FLP36-C */ + z = y - 1.0F; + } + + num = z*( num + -1.71618513886549492533811e+0F ); + den = ( den*z ) -3.08402300119738975254353e+1F; + num = z*( num + 2.47656508055759199108314e+1F ); + den = ( den*z ) + 3.15350626979604161529144e+2F; + num = z*( num - 3.79804256470945635097577e+2F ); + den = ( den*z ) - 1.01515636749021914166146e+3F; + num = z*( num + 6.29331155312818442661052e+2F ); + den = ( den*z ) - 3.10777167157231109440444e+3F; + num = z*( num + 8.66966202790413211295064e+2F ); + den = ( den*z ) + 2.25381184209801510330112e+4F; + num = z*( num - 3.14512729688483675254357e+4F ); + den = ( den*z ) + 4.75584627752788110767815e+3F; + num = z*( num - 3.61444134186911729807069e+4F ); + den = ( den*z ) - 1.34659959864969306392456e+5F; + num = z*( num + 6.64561438202405440627855e+4F ); + den = ( den*z ) - 1.15132259675553483497211e+5F; + + result = ( num/den ) + 1.0F; + if ( y1 < y ) { + result /= y1; + } + else if ( y1 > y ) { + for ( int i = 0; i < n ; ++i ) { + result *= y; + y += 1.0F; + } + } + } + else { + if ( x <= 171.624F ) { /* x <= xBig */ + const float yy = y*y; + float sum = 5.7083835261e-03F; + + sum = ( sum/yy ) - 1.910444077728e-03F; + sum = ( sum/yy ) + 8.4171387781295e-04F; + sum = ( sum/yy ) - 5.952379913043012e-04F; + sum = ( sum/yy ) + 7.93650793500350248e-04F; + sum = ( sum/yy ) - 2.777777777777681622553e-03F; + sum = ( sum/yy ) + 8.333333333333333331554247e-02F; + + sum = ( sum /y ) - y + QFFM_LN_SQRT_2PI; + sum += ( y - 0.5F )*qFFMath_Log( y ); + result = qFFMath_Exp( sum ); + } + else { + result = QFFM_INFINITY; + } + } + if ( parity ) { + result = -result; + } + if ( !qFFMath_IsEqual( fact, 1.0F ) ) { + result = fact/result; + } + } + + EXIT_TGAMMA: + return result; +} +/*============================================================================*/ +static float lgamma_positive( float x ) +{ + const float d1 = -5.772156649015328605195174e-1F; + const float d2 = 4.227843350984671393993777e-1F; + const float d4 = 1.791759469228055000094023e+0F; + const float pnt68 = 0.6796875F; + float result; + + if ( x > 171.624F ) { + result = QFFM_INFINITY; + } + else { + float y, corrector, num, den; + + y = x; + if ( y <= FLT_EPSILON ) { + result = -qFFMath_Log( y ); + } + else if ( y <= 1.5F ) { + float xMinus; + + if ( y < pnt68 ) { + corrector = -qFFMath_Log( y ); + xMinus = y; + } + else { + corrector = 0.0F; + xMinus = ( y - 0.5F ) - 0.5F; + } + if ( ( y <= 0.5F ) || ( y >= pnt68 ) ) { + den = 1.0F; + num = 0.0F; + num = ( num*xMinus ) + 4.945235359296727046734888e+0F; + num = ( num*xMinus ) + 2.018112620856775083915565e+2F; + num = ( num*xMinus ) + 2.290838373831346393026739e+3F; + num = ( num*xMinus ) + 1.131967205903380828685045e+4F; + num = ( num*xMinus ) + 2.855724635671635335736389e+4F; + num = ( num*xMinus ) + 3.848496228443793359990269e+4F; + num = ( num*xMinus ) + 2.637748787624195437963534e+4F; + num = ( num*xMinus ) + 7.225813979700288197698961e+3F; + den = ( den*xMinus ) + 6.748212550303777196073036e+1F; + den = ( den*xMinus ) + 1.113332393857199323513008e+3F; + den = ( den*xMinus ) + 7.738757056935398733233834e+3F; + den = ( den*xMinus ) + 2.763987074403340708898585e+4F; + den = ( den*xMinus ) + 5.499310206226157329794414e+4F; + den = ( den*xMinus ) + 6.161122180066002127833352e+4F; + den = ( den*xMinus ) + 3.635127591501940507276287e+4F; + den = ( den*xMinus ) + 8.785536302431013170870835e+3F; + result = corrector + ( xMinus*( d1 + ( xMinus*( num/den ) ) ) ); + } + else { + xMinus = ( y - 0.5F ) - 0.5F; + den = 1.0F; + num = 0.0F; + num = ( num*xMinus ) + 4.974607845568932035012064e+0F; + num = ( num*xMinus ) + 5.424138599891070494101986e+2F; + num = ( num*xMinus ) + 1.550693864978364947665077e+4F; + num = ( num*xMinus ) + 1.847932904445632425417223e+5F; + num = ( num*xMinus ) + 1.088204769468828767498470e+6F; + num = ( num*xMinus ) + 3.338152967987029735917223e+6F; + num = ( num*xMinus ) + 5.106661678927352456275255e+6F; + num = ( num*xMinus ) + 3.074109054850539556250927e+6F; + den = ( den*xMinus ) + 1.830328399370592604055942e+2F; + den = ( den*xMinus ) + 7.765049321445005871323047e+3F; + den = ( den*xMinus ) + 1.331903827966074194402448e+5F; + den = ( den*xMinus ) + 1.136705821321969608938755e+6F; + den = ( den*xMinus ) + 5.267964117437946917577538e+6F; + den = ( den*xMinus ) + 1.346701454311101692290052e+7F; + den = ( den*xMinus ) + 1.782736530353274213975932e+7F; + den = ( den*xMinus ) + 9.533095591844353613395747e+6F; + result = corrector + ( xMinus*( d2 + ( xMinus*( num/den ) ) ) ); + } + } + else if ( y <= 4.0F ) { + const float xMinus = y - 2.0F; + den = 1.0F; + num = 0.0F; + num = ( num*xMinus ) + 4.974607845568932035012064e+0F; + num = ( num*xMinus ) + 5.424138599891070494101986e+2F; + num = ( num*xMinus ) + 1.550693864978364947665077e+4F; + num = ( num*xMinus ) + 1.847932904445632425417223e+5F; + num = ( num*xMinus ) + 1.088204769468828767498470e+6F; + num = ( num*xMinus ) + 3.338152967987029735917223e+6F; + num = ( num*xMinus ) + 5.106661678927352456275255e+6F; + num = ( num*xMinus ) + 3.074109054850539556250927e+6F; + den = ( den*xMinus ) + 1.830328399370592604055942e+2F; + den = ( den*xMinus ) + 7.765049321445005871323047e+3F; + den = ( den*xMinus ) + 1.331903827966074194402448e+5F; + den = ( den*xMinus ) + 1.136705821321969608938755e+6F; + den = ( den*xMinus ) + 5.267964117437946917577538e+6F; + den = ( den*xMinus ) + 1.346701454311101692290052e+7F; + den = ( den*xMinus ) + 1.782736530353274213975932e+7F; + den = ( den*xMinus ) + 9.533095591844353613395747e+6F; + result = xMinus*( d2 + ( xMinus*( num/den ) ) ); + } + else if ( y <= 12.0F ) { + const float xMinus = y - 4.0F; + den = -1.0F; + num = 0.0F; + num = ( num*xMinus ) + 1.474502166059939948905062e+04F; + num = ( num*xMinus ) + 2.426813369486704502836312e+06F; + num = ( num*xMinus ) + 1.214755574045093227939592e+08F; + num = ( num*xMinus ) + 2.663432449630976949898078e+09F; + num = ( num*xMinus ) + 2.940378956634553899906876e+10F; + num = ( num*xMinus ) + 1.702665737765398868392998e+11F; + num = ( num*xMinus ) + 4.926125793377430887588120e+11F; + num = ( num*xMinus ) + 5.606251856223951465078242e+11F; + den = ( den*xMinus ) + 2.690530175870899333379843e+03F; + den = ( den*xMinus ) + 6.393885654300092398984238e+05F; + den = ( den*xMinus ) + 4.135599930241388052042842e+07F; + den = ( den*xMinus ) + 1.120872109616147941376570e+09F; + den = ( den*xMinus ) + 1.488613728678813811542398e+10F; + den = ( den*xMinus ) + 1.016803586272438228077304e+11F; + den = ( den*xMinus ) + 3.417476345507377132798597e+11F; + den = ( den*xMinus ) + 4.463158187419713286462081e+11F; + result = d4 + ( xMinus*( num/den ) ); + } + else { + result = 0.0F; + if ( y <= 4294967296.87842273712158203125F ) { /* y < xBig^(1/4)*/ + const float yy = y*y; + result = 5.7083835261e-03F; + result = ( result/yy ) - 1.910444077728e-03F; + result = ( result/yy ) + 8.4171387781295e-04F; + result = ( result/yy ) - 5.952379913043012e-04F; + result = ( result/yy ) + 7.93650793500350248e-04F; + result = ( result/yy ) - 2.777777777777681622553e-03F; + result = ( result/yy ) + 8.333333333333333331554247e-02F; + } + result /= y; + corrector = qFFMath_Log( y ); + result += QFFM_LN_SQRT_2PI - ( 0.5F*corrector ); + result += y*( corrector - 1.0F ); + } + } + + return result; +} +/*============================================================================*/ +float qFFMath_LGamma( float x ) +{ + float result; + + const int fClass = qFFMath_FPClassify( x ); + if ( QFFM_FP_NAN == fClass ) { + result = QFFM_NAN; + } + else if ( ( QFFM_FP_ZERO == fClass ) || ( QFFM_FP_INFINITE == fClass ) ) { + result = QFFM_INFINITY; + } + else { + if ( x < 0.0F ) { + if ( x <= -4503599627370496.0F ) { /* x < 2^52 */ + result = QFFM_INFINITY; + } + else { + float y, y1, isItAnInt; + + y = -x; + y1 = qFFMath_Trunc( y ); + isItAnInt = y - y1; + if ( qFFMath_IsEqual( 0.0F, isItAnInt ) ) { + result = QFFM_INFINITY; + } + else { + float a; + + a = qFFMath_Sin( QFFM_PI*isItAnInt ); + result = qFFMath_Log( QFFM_PI/qFFMath_Abs( a*x ) ) - lgamma_positive( -x ); + } + } + } + else { + result = lgamma_positive( x ); + } + } + + return result; +} +/*============================================================================*/ +float qFFMath_Factorial( float x ) +{ + static const float ft[ 35 ] = { 1.0F, 1.0F, 2.0F, 6.0F, 24.0F, 120.0F, 720.0F, + 5040.0F, 40320.0F, 362880.0F, 3628800.0F, + 39916800.0F, 479001600.0F, 6227020800.0F, + 87178291200.0F, 1307674368000.0F, + 20922789888000.0F, 355687428096000.0F, + 6402373705728001.0F, 121645100408832000.0F, + 2432902008176640000.0F, 51090942171709440000.0F, + 1124000727777607680000.0F, + 25852016738884978212864.0F, + 620448401733239544217600.0F, + 15511210043330988202786816.0F, + 403291461126605719042260992.0F, + 10888869450418351940239884288.0F, + 304888344611713836734530715648.0F, + 8841761993739700772720181510144.0F, + 265252859812191104246398737973248.0F, + 8222838654177921277277005322125312.0F, + 263130836933693591553328612565319680.0F, + 8683317618811885938715673895318323200.0F, + 295232799039604119555149671006000381952.0F, + }; + float y; + + if ( x > 34.0F ) { + y = QFFM_INFINITY; + } + else if ( x >= 0.0F ) { + y = ft[ (size_t)x ]; + } + else { + y = QFFM_NAN; + } + + return y; +} +#endif /*#ifndef QLIBS_USE_STD_MATH*/ \ No newline at end of file diff --git a/qfis.c b/qfis.c index 4ce0231..75937c0 100644 --- a/qfis.c +++ b/qfis.c @@ -1,11 +1,12 @@ /*! * @file qfis.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qfis.h" -#include +#include "qffmath.h" +#include typedef float (*qFIS_FuzzyOperator_t)( const float a, const float b ); @@ -98,8 +99,8 @@ static float qFIS_ProbOR( const float a, static float qFIS_Sum( const float a, const float b ); static float qFIS_Bound( float y, - const float ymin, - const float ymax ); + const float yMin, + const float yMax ); static void qFIS_EvalInputMFs( qFIS_t * const f ); static void qFIS_TruncateInputs( qFIS_t * const f ); static float qFIS_ParseFuzzValue( qFIS_MF_t * const mfIO, @@ -130,7 +131,7 @@ static float qFIS_DeFuzz_WtSum( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ); static void qFIS_Aggregate( qFIS_t * const f ); -#define QFIS_INFERENCE_ERROR ( 0u ) +#define QFIS_INFERENCE_ERROR ( 0U ) /*============================================================================*/ int qFIS_SetParameter( qFIS_t * const f, @@ -147,24 +148,28 @@ int qFIS_SetParameter( qFIS_t * const f, switch ( p ) { case qFIS_Implication: if ( x <= qFIS_PROD ) { + /*cppcheck-suppress misra-c2012-11.1 */ f->implicate = method[ x ]; retVal = 1; } break; case qFIS_Aggregation: if ( ( x >= qFIS_MAX ) && ( x <= qFIS_SUM ) ) { + /*cppcheck-suppress misra-c2012-11.1 */ f->aggregate = method[ x ]; retVal = 1; } break; case qFIS_AND: if ( x <= qFIS_PROD ) { + /*cppcheck-suppress misra-c2012-11.1 */ f->andOp = method[ x ]; retVal = 1; } break; case qFIS_OR: if ( ( x >= qFIS_MAX ) && ( x <= qFIS_PROBOR ) ) { + /*cppcheck-suppress misra-c2012-11.1 */ f->orOp = method[ x ]; retVal = 1; } @@ -201,6 +206,7 @@ int qFIS_SetDeFuzzMethod( qFIS_t * const f, if ( ( ( Mamdani == f->type ) && ( m <= som ) ) || ( ( Sugeno == f->type ) && ( m >= wtaver ) && ( m <= wtsum ) ) || ( ( Tsukamoto == f->type ) && ( wtaver == m ) )) { + /*cppcheck-suppress misra-c2012-11.1 */ f->deFuzz = method[ m ]; retVal = 1; } @@ -244,11 +250,12 @@ int qFIS_Setup( qFIS_t * const f, retVal += qFIS_SetParameter( f, qFIS_OR, qFIS_MAX ); retVal += qFIS_SetParameter( f, qFIS_Implication, qFIS_MIN ); retVal += qFIS_SetParameter( f, qFIS_Aggregation, qFIS_MAX ); + /*cppcheck-suppress misra-c2012-10.6 */ retVal = ( 5 == retVal ) ? 1 : 0; f->deFuzz = ( Mamdani == t ) ? &qFIS_DeFuzz_Centroid : &qFIS_DeFuzz_WtAverage; f->ruleWeight = NULL; - for ( i = 0 ; i < f->nOutputs ; ++i ) { + for ( i = 0U ; i < f->nOutputs ; ++i ) { /*cstat -CERT-FLP36-C*/ f->output[ i ].res = ( f->output[ i ].b.max - f->output[ i ].b.min )/(float)f->nPoints; /*cstat +CERT-FLP36-C*/ @@ -283,14 +290,16 @@ int qFIS_OutputSetup( qFIS_Output_t * const v, int retVal = 0; if ( ( NULL != v ) && ( t >= 0 ) ) { - v[ t ].data[ 0 ] = 0.0f; - v[ t ].data[ 1 ] = 0.0f; - v[ t ].data[ 2 ] = 0.0f; - v[ t ].data[ 3 ] = 0.0f; - v[ t ].x = 0.0f; - v[ t ].y = 0.0f; + v[ t ].data[ 0 ] = 0.0F; + v[ t ].data[ 1 ] = 0.0F; + v[ t ].data[ 2 ] = 0.0F; + v[ t ].data[ 3 ] = 0.0F; + v[ t ].x = 0.0F; + v[ t ].y = 0.0F; v[ t ].b.min = min; v[ t ].b.max = max; + v[ t ].xag = NULL; + v[ t ].yag = NULL; retVal = 1; } @@ -314,7 +323,7 @@ int qFIS_SetInput( qFIS_Input_t * const v, float qFIS_GetOutput( const qFIS_Output_t * const v, const qFIS_Tag_t t ) { - float retVal = 0.0f; + float retVal = 0.0F; if ( ( NULL != v ) && ( t >= 0 ) ) { retVal = v[ t ].b.value; @@ -351,29 +360,54 @@ int qFIS_SetMF( qFIS_MF_t * const m, m[ mf ].shape = custom_mf; /*user-defined membership function*/ } else { + /*cppcheck-suppress misra-c2012-11.1 */ m[ mf ].shape = fShape[ s ]; } m[ mf ].index = (size_t)io; m[ mf ].points = cp; - m[ mf ].fx = 0.0f; - m[ mf ].h = qFIS_Bound( h, 0.0f, 1.0f ); + m[ mf ].fx = 0.0F; + m[ mf ].h = qFIS_Bound( h, 0.0F, 1.0F ); retVal = 1; } return retVal; } /*============================================================================*/ +int qFIS_StoreAggregatedRegion( qFIS_Output_t * const o, + const qFIS_Tag_t t, + float *x, + float *y, + const size_t n ) +{ + int retVal = 0; + + if ( ( NULL != o ) && ( t >= 0 ) && ( NULL != x ) && ( NULL != y ) ) { + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + qFIS_t *f = (qFIS_t *)o[ t ].owner; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + if ( n >= f->nPoints ) { + o[ t ].xag = x; + o[ t ].yag = y; + retVal = 1; + } + } + + return retVal; +} +/*============================================================================*/ static void qFIS_EvalInputMFs( qFIS_t * const f ) { size_t i; qFIS_MF_t *mf; - for ( i = 0 ; i < f->nMFInputs ; ++i ) { + for ( i = 0U ; i < f->nMFInputs ; ++i ) { mf = &f->inMF[ i ]; /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ mf->fx = mf->h*mf->shape( (qFIS_IO_Base_t*)&f->input[ mf->index ], mf->points, - 1u ); + 1U ); /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ } } @@ -382,7 +416,7 @@ static void qFIS_TruncateInputs( qFIS_t * const f ) { size_t i; - for ( i = 0 ; i < f->nInputs ; ++i ) { + for ( i = 0U ; i < f->nInputs ; ++i ) { f->input[ i ].b.value = qFIS_Bound( f->input[ i ].b.value, f->input[ i ].b.min, f->input[ i ].b.max ); @@ -405,16 +439,17 @@ int qFIS_Fuzzify( qFIS_t * const f ) static float qFIS_ParseFuzzValue( qFIS_MF_t * const mfIO, qFIS_Rules_t index ) { - uint8_t neg = (uint8_t)( index < 0 ); + uint8_t neg = ( index < 0 ) ? 1U : 0U ; float y; - if ( 0u != neg ) { + if ( 0U != neg ) { index = -index; } /*cstat -CERT-INT32-C_a*/ - y = qFIS_Bound( mfIO[ index - 1 ].fx, 0.0f, 1.0f ); + y = qFIS_Bound( mfIO[ index - 1 ].fx, 0.0F, 1.0F ); /*cstat +CERT-INT32-C_a*/ - y = ( 0u != neg ) ? ( 1.0f - y ) : y ; + /*cppcheck-suppress misra-c2012-12.1 */ + y = ( 0U != neg ) ? ( 1.0F - y ) : y ; return y; } @@ -430,7 +465,7 @@ static qFIS_FuzzyOperator_t qFIS_GetFuzzOperator( const qFIS_t * const f ) case _QFIS_OR: oper = f->orOp; break; - default: + default: oper = &qFIS_Sum; break; } @@ -441,12 +476,12 @@ static qFIS_FuzzyOperator_t qFIS_GetFuzzOperator( const qFIS_t * const f ) static size_t qFIS_InferenceAntecedent( struct _qFIS_s * const f, size_t i ) { - int16_t inIndex, MFInIndex, connector; + qFIS_Rules_t inIndex, MFInIndex, connector; qFIS_FuzzyOperator_t op; /*cstat -CERT-INT30-C_a*/ inIndex = f->rules[ i ]; - MFInIndex = f->rules[ i + 1u ]; - connector = f->rules[ i + 2u ]; + MFInIndex = f->rules[ i + 1U ]; + connector = f->rules[ i + 2U ]; /*cstat -CERT-INT30-C_a*/ op = qFIS_GetFuzzOperator( f ); f->rStrength = op( f->rStrength, qFIS_ParseFuzzValue( f->inMF, MFInIndex ) ); @@ -458,11 +493,11 @@ static size_t qFIS_InferenceAntecedent( struct _qFIS_s * const f, if ( ( _QFIS_AND == connector ) || ( _QFIS_OR == connector ) ) { f->lastConnector = connector; f->inferenceState = &qFIS_InferenceAntecedent; - i += 2u; + i += 2U; } else if ( _QFIS_THEN == connector ) { f->inferenceState = &qFIS_InferenceReachEnd; - i += 2u; + i += 2U; } else { i = QFIS_INFERENCE_ERROR; @@ -475,18 +510,18 @@ static size_t qFIS_InferenceAntecedent( struct _qFIS_s * const f, static size_t qFIS_InferenceReachEnd( struct _qFIS_s * const f, size_t i ) { - int16_t connector; + qFIS_Rules_t connector; - connector = ( f->nOutputs > 1u )? f->rules[ i + 2u ] : -1; - i += 2u; + connector = ( f->nOutputs > 1U )? f->rules[ i + 2U ] : -1; + i += 2U; if ( _QFIS_AND != connector ) { f->inferenceState = &qFIS_InferenceAntecedent; f->lastConnector = -1; f->wi[ f->ruleCount ] = f->rStrength; if ( NULL != f->ruleWeight ) { - f->wi[ f->ruleCount ] *= qFIS_Bound( f->ruleWeight[ f->ruleCount ], 0.0f, 1.0f ); + f->wi[ f->ruleCount ] *= qFIS_Bound( f->ruleWeight[ f->ruleCount ], 0.0F, 1.0F ); } - f->rStrength = 0.0f; + f->rStrength = 0.0F; ++f->ruleCount; --i; } @@ -499,40 +534,44 @@ static size_t qFIS_AggregationFindConsequent( struct _qFIS_s * const f, { while ( _QFIS_THEN != f->rules[ i++ ] ) {} f->aggregationState = &qFIS_InferenceConsequent; - - return --i; + /*cstat -MISRAC2012-Rule-2.2_c*/ + return --i; /*!ok*/ + /*cstat +MISRAC2012-Rule-2.2_c*/ } /*============================================================================*/ static size_t qFIS_InferenceConsequent( struct _qFIS_s * const f, size_t i ) { - int16_t outIndex, MFOutIndex, connector; - uint8_t neg = 0u; + qFIS_Rules_t outIndex, MFOutIndex, connector; + uint8_t neg = 0U; outIndex = f->rules[ i ]; - MFOutIndex = f->rules[ i + 1u ]; - connector = ( f->nOutputs > 1u )? f->rules[ i + 2u ] : -1; + MFOutIndex = f->rules[ i + 1U ]; + connector = ( f->nOutputs > 1U )? f->rules[ i + 2U ] : -1; if ( MFOutIndex < 0 ) { MFOutIndex = -MFOutIndex; - neg = 1u; + neg = 1U; } MFOutIndex -= 1; - if ( f->wi[ f->ruleCount ] > 0.0f ) { + if ( f->wi[ f->ruleCount ] > 0.0F ) { qFIS_Output_t *o = &f->output[ outIndex ]; qFIS_MF_t *m = &f->outMF[ MFOutIndex ]; if ( Mamdani == f->type ) { float v; /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ - v = m->h*m->shape( (qFIS_IO_Base_t*)o, m->points, 1u ); + /*cppcheck-suppress misra-c2012-11.3 */ + v = m->h*m->shape( (qFIS_IO_Base_t*)o, m->points, 1U ); /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - v = ( 1u == neg )? ( 1.0f - v ) : v; + /*cppcheck-suppress misra-c2012-12.1 */ + v = ( 1U == neg )? ( 1.0F - v ) : v; o->y = f->aggregate( o->y, f->implicate( f->wi[ f->ruleCount ], v ) ); } else { /* Sugeno and Tsukamoto*/ float zi; /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ zi = m->shape( (qFIS_IO_Base_t*)f->input, m->points, f->nInputs ); /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ o->data[ 0 ] += zi*f->wi[ f->ruleCount ]; @@ -540,7 +579,7 @@ static size_t qFIS_InferenceConsequent( struct _qFIS_s * const f, } } - i += 2u; + i += 2U; if ( _QFIS_AND != connector ) { f->aggregationState = &qFIS_AggregationFindConsequent; ++f->ruleCount; @@ -554,10 +593,10 @@ static void qFIS_Aggregate( qFIS_t * const f ) { if ( NULL != f ) { if ( QFIS_RULES_BEGIN == f->rules[ 0 ] ) { - size_t i = 1u; + size_t i = 1U; f->aggregationState = &qFIS_AggregationFindConsequent; - f->ruleCount = 0u; + f->ruleCount = 0U; while ( ( _QFIS_RULES_END != f->rules[ i ] ) && ( f->ruleCount < f->nRules ) ) { i = f->aggregationState( f, i ); if ( QFIS_INFERENCE_ERROR == i ) { @@ -572,7 +611,7 @@ static void qFIS_Aggregate( qFIS_t * const f ) static float qFIS_DeFuzz_Centroid( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; switch ( stage ) { case DeFuzz_Compute: @@ -580,8 +619,8 @@ static float qFIS_DeFuzz_Centroid( qFIS_Output_t * const o, o->data[ 1 ] += o->y; break; case DeFuzz_Init: - o->data[ 0 ] = 0.0f; /*store sum(y)*/ - o->data[ 1 ] = 0.0f; /*store sum(x*y)*/ + o->data[ 0 ] = 0.0F; /*store sum(y)*/ + o->data[ 1 ] = 0.0F; /*store sum(x*y)*/ break; case DeFuzz_End: d = o->data[ 0 ]/o->data[ 1 ]; /*sum(x*y)/sum(y)*/ @@ -597,7 +636,7 @@ static float qFIS_DeFuzz_Bisector( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { size_t k; - float d = 0.0f; + float d = 0.0F; qFIS_t *f; switch ( stage ) { @@ -605,20 +644,21 @@ static float qFIS_DeFuzz_Bisector( qFIS_Output_t * const o, o->data[ 0 ] += o->y; break; case DeFuzz_Init: - o->data[ 0 ] = 0.0f; /*store sum(y)*/ + o->data[ 0 ] = 0.0F; /*store sum(y)*/ break; case DeFuzz_End: - o->data[ 1 ] = 0.0f; + o->data[ 1 ] = 0.0F; /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ f = (qFIS_t *)o->owner; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ - for ( k = 0u ; k < f->nPoints ; ++k ) { - o->y = 0.0f; + for ( k = 0U ; k < f->nPoints ; ++k ) { + o->y = 0.0F; o->x = qFIS_GetNextX( o->b.min, o->res, k ); o->b.value = o->x; qFIS_Aggregate( f ); o->data[ 1 ] += o->y; - if ( o->data[ 1 ] >= ( 0.5f*o->data[ 0 ] ) ) { + if ( o->data[ 1 ] >= ( 0.5F*o->data[ 0 ] ) ) { break; } } @@ -634,7 +674,7 @@ static float qFIS_DeFuzz_Bisector( qFIS_Output_t * const o, static float qFIS_DeFuzz_LOM( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; switch ( stage ) { case DeFuzz_Compute: @@ -644,7 +684,7 @@ static float qFIS_DeFuzz_LOM( qFIS_Output_t * const o, } break; case DeFuzz_Init: - o->data[ 0 ] = -1.0f; /*yMax*/ + o->data[ 0 ] = -1.0F; /*yMax*/ o->data[ 1 ] = o->b.max; /*xLargest*/ break; case DeFuzz_End: @@ -660,7 +700,7 @@ static float qFIS_DeFuzz_LOM( qFIS_Output_t * const o, static float qFIS_DeFuzz_SOM( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; switch ( stage ) { case DeFuzz_Compute: @@ -670,7 +710,7 @@ static float qFIS_DeFuzz_SOM( qFIS_Output_t * const o, } break; case DeFuzz_Init: - o->data[ 0 ] = -1.0f; /*yMax*/ + o->data[ 0 ] = -1.0F; /*yMax*/ o->data[ 1 ] = o->b.min; /*xSmallest*/ break; case DeFuzz_End: @@ -686,7 +726,7 @@ static float qFIS_DeFuzz_SOM( qFIS_Output_t * const o, static float qFIS_DeFuzz_MOM( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; switch ( stage ) { case DeFuzz_Compute: @@ -694,26 +734,26 @@ static float qFIS_DeFuzz_MOM( qFIS_Output_t * const o, o->data[ 0 ] = o->y; o->data[ 1 ] = o->x; o->data[ 2 ] = o->x; - o->data[ 3 ] = 1.0f; + o->data[ 3 ] = 1.0F; } - else if ( ( o->data[ 3 ] > 0.0f ) && ( fabsf( o->y - o->data[ 0 ] ) <= FLT_MIN ) ) { + else if ( qFFMath_IsEqual( o->y , o->data[ 0 ] ) && ( o->data[ 3 ] > 0.0F ) ) { o->data[ 2 ] = o->x; } else if ( o->y < o->data[ 0 ] ) { - o->data[ 3 ] = -1.0f; + o->data[ 3 ] = -1.0F; } else { /*nothing to do*/ } break; case DeFuzz_Init: - o->data[ 0 ] = -1.0f; /* yMax */ + o->data[ 0 ] = -1.0F; /* yMax */ o->data[ 1 ] = o->b.min; /*xSmallest*/ o->data[ 2 ] = o->b.max; /*xLargest*/ - o->data[ 3 ] = -1.0f; /*sp*/ + o->data[ 3 ] = -1.0F; /*sp*/ break; case DeFuzz_End: - d = 0.5f*( o->data[ 1 ] + o->data[ 2 ] ); + d = 0.5F*( o->data[ 1 ] + o->data[ 2 ] ); break; default: break; @@ -725,7 +765,7 @@ static float qFIS_DeFuzz_MOM( qFIS_Output_t * const o, static float qFIS_DeFuzz_WtAverage( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; if ( DeFuzz_End == stage ) { d = o->data[ 0 ]/o->data[ 1 ]; @@ -737,7 +777,7 @@ static float qFIS_DeFuzz_WtAverage( qFIS_Output_t * const o, static float qFIS_DeFuzz_WtSum( qFIS_Output_t * const o, const qFIS_DeFuzzState_t stage ) { - float d = 0.0f; + float d = 0.0F; if ( DeFuzz_End == stage ) { d = o->data[ 0 ]; @@ -753,39 +793,43 @@ int qFIS_DeFuzzify( qFIS_t * const f ) if ( NULL != f ) { size_t i; - for ( i = 0; i < f->nOutputs ; ++i ) { + for ( i = 0U; i < f->nOutputs ; ++i ) { f->deFuzz( &f->output[ i ] , DeFuzz_Init ); } if ( Mamdani == f->type ) { size_t k; - for ( k = 0u ; k < f->nPoints ; ++k ) { - for ( i = 0; i < f->nOutputs ; ++i ) { /* initialize*/ - f->output[ i ].y = 0.0f; + for ( k = 0U ; k < f->nPoints ; ++k ) { + for ( i = 0U; i < f->nOutputs ; ++i ) { /* initialize*/ + f->output[ i ].y = 0.0F; f->output[ i ].x = qFIS_GetNextX( f->output[ i ].b.min, f->output[ i ].res, k ); f->output[ i ].b.value = f->output[ i ].x; } qFIS_Aggregate( f ); - for ( i = 0; i < f->nOutputs ; ++i ) { + for ( i = 0U; i < f->nOutputs ; ++i ) { f->deFuzz( &f->output[ i ] , DeFuzz_Compute ); + if ( NULL != f->output[ i ].xag ) { /*store aggregated*/ + f->output[ i ].xag[ k ] = f->output[ i ].x; + f->output[ i ].yag[ k ] = f->output[ i ].y; + } } } } else { /*Sugeno and Tsukamoto systems*/ - for ( i = 0; i < f->nOutputs ; ++i ) { /* initialize*/ - f->output[ i ].data[ 0 ] = 0.0f; /*store sum wi*/ - f->output[ i ].data[ 1 ] = 0.0f; /*store sum zi*wi*/ + for ( i = 0U; i < f->nOutputs ; ++i ) { /* initialize*/ + f->output[ i ].data[ 0 ] = 0.0F; /*store sum wi*/ + f->output[ i ].data[ 1 ] = 0.0F; /*store sum zi*wi*/ } qFIS_Aggregate( f ); - for ( i = 0; i < f->nOutputs ; ++i ) { /* initialize*/ + for ( i = 0U; i < f->nOutputs ; ++i ) { /* initialize*/ f->deFuzz( &f->output[ i ] , DeFuzz_Compute ); } } - for ( i = 0; i < f->nOutputs ; ++i ) { + for ( i = 0U; i < f->nOutputs ; ++i ) { f->output[ i ].b.value = f->deFuzz( &f->output[ i ] , DeFuzz_End ); f->output[ i ].b.value = qFIS_Bound( f->output[ i ].b.value, f->output[ i ].b.min, @@ -814,14 +858,14 @@ int qFIS_Inference( qFIS_t * const f ) int retVal = 0; if ( NULL != f ) { - size_t i = 0u; + size_t i = 0U; if ( QFIS_RULES_BEGIN == f->rules[ 0 ] ) { f->inferenceState = &qFIS_InferenceAntecedent; - f->rStrength = 0.0f; + f->rStrength = 0.0F; f->lastConnector = -1; - f->ruleCount = 0u; - i = 1u; + f->ruleCount = 0U; + i = 1U; while ( ( _QFIS_RULES_END != f->rules[ i ] ) && ( f->ruleCount < f->nRules ) ) { i = f->inferenceState( f, i ); if ( QFIS_INFERENCE_ERROR == i ) { @@ -843,7 +887,7 @@ static float qFIS_GetNextX( const float init, const size_t i ) { /*cstat -CERT-FLP36-C*/ - return init + ( ( (float)i + 0.5f )*res ); + return init + ( ( (float)i + 0.5F )*res ); /*cstat +CERT-FLP36-C*/ } /*============================================================================*/ @@ -860,7 +904,7 @@ static float qFIS_TriMF( const qFIS_IO_Base_t * const in, c = p[ 2 ]; tmp = qFIS_Min( ( x - a )/( b - a ) , ( c - x )/( c - b ) ); - return qFIS_Max( tmp , 0.0f ); + return qFIS_Max( tmp , 0.0F ); } /*============================================================================*/ static float qFIS_TrapMF( const qFIS_IO_Base_t * const in, @@ -875,10 +919,10 @@ static float qFIS_TrapMF( const qFIS_IO_Base_t * const in, b = p[ 1 ]; c = p[ 2 ]; d = p[ 3 ]; - tmp = qFIS_Min( ( x - a )/( b - a ) , 1.0f ); + tmp = qFIS_Min( ( x - a )/( b - a ) , 1.0F ); tmp = qFIS_Min( tmp, ( d - x )/( d - c ) ) ; - return qFIS_Max( tmp , 0.0f ); + return qFIS_Max( tmp , 0.0F ); } /*============================================================================*/ static float qFIS_GBellMF( const qFIS_IO_Base_t * const in, @@ -893,7 +937,7 @@ static float qFIS_GBellMF( const qFIS_IO_Base_t * const in, b = p[ 1 ]; c = p[ 2 ]; - return ( 1.0f/( 1.0f + powf( fabsf( ( x - c )/a ) , 2.0f*b ) ) ); + return ( 1.0F/( 1.0F + QLIB_POW( QLIB_ABS( ( x - c )/a ) , 2.0F*b ) ) ); } /*============================================================================*/ static float qFIS_GaussMF( const qFIS_IO_Base_t * const in, @@ -908,7 +952,7 @@ static float qFIS_GaussMF( const qFIS_IO_Base_t * const in, c = p[ 1 ]; tmp = ( x - c )/a; - return expf( -0.5f*tmp*tmp ); + return QLIB_EXP( -0.5F*tmp*tmp ); } /*============================================================================*/ static float qFIS_Gauss2MF( const qFIS_IO_Base_t * const in, @@ -920,8 +964,8 @@ static float qFIS_Gauss2MF( const qFIS_IO_Base_t * const in, c1 = p[ 1 ]; c2 = p[ 3 ]; - f1 = ( x <= c1 ) ? qFIS_GaussMF( in , p, n ) : 1.0f; - f2 = ( x <= c2 ) ? qFIS_GaussMF( in , &p[ 2 ], n ) : 1.0f; + f1 = ( x <= c1 ) ? qFIS_GaussMF( in , p, n ) : 1.0F; + f2 = ( x <= c2 ) ? qFIS_GaussMF( in , &p[ 2 ], n ) : 1.0F; return f1*f2; } @@ -937,7 +981,7 @@ static float qFIS_SigMF( const qFIS_IO_Base_t * const in, a = p[ 0 ]; b = p[ 1 ]; - return 1.0f/( 1.0f + expf( -a*( x - b ) ) ); + return 1.0F/( 1.0F + QLIB_EXP( -a*( x - b ) ) ); } /*============================================================================*/ static float qFIS_TSigMF( const qFIS_IO_Base_t * const in, @@ -952,16 +996,16 @@ static float qFIS_TSigMF( const qFIS_IO_Base_t * const in, a = p[ 0 ]; /*slope*/ b = p[ 1 ]; /*inflection*/ - if ( fabsf( x - 1.0f ) <= FLT_MIN ) { /* x == 1 ? */ - if ( a >= 0.0f ) { + if ( qFFMath_IsEqual( x, 1.0F ) ) { + if ( a >= 0.0F ) { y = max; } else { y = min; } } - else if ( fabsf( x ) <= FLT_MIN ) { /* x == 0 ? */ - if ( a >= 0.0f ) { + else if ( qFFMath_IsEqual( x, 0.0F ) ) { + if ( a >= 0.0F ) { y = min; } else { @@ -970,7 +1014,7 @@ static float qFIS_TSigMF( const qFIS_IO_Base_t * const in, } else { /*cstat -MISRAC2012-Dir-4.11_a*/ - y = b - ( logf( ( 1.0f/x ) - 1.0f )/a ); + y = b - ( QLIB_LOG( ( 1.0F/x ) - 1.0F )/a ); /*cstat +MISRAC2012-Dir-4.11_a*/ } @@ -981,14 +1025,14 @@ static float qFIS_DSigMF( const qFIS_IO_Base_t * const in, const float *p, const size_t n ) { - return fabsf( qFIS_SigMF( in , p, n ) - qFIS_SigMF( in , &p[ 2 ], n ) ); + return QLIB_ABS( qFIS_SigMF( in , p, n ) - qFIS_SigMF( in , &p[ 2 ], n ) ); } /*============================================================================*/ static float qFIS_PSigMF( const qFIS_IO_Base_t * const in, const float *p, const size_t n ) { - return fabsf( qFIS_SigMF( in , p, n )*qFIS_SigMF( in , &p[ 2 ], n ) ); + return QLIB_ABS( qFIS_SigMF( in , p, n )*qFIS_SigMF( in , &p[ 2 ], n ) ); } /*============================================================================*/ static float qFIS_SMF( const qFIS_IO_Base_t * const in, @@ -1002,21 +1046,21 @@ static float qFIS_SMF( const qFIS_IO_Base_t * const in, a = p[ 0 ]; b = p[ 1 ]; if ( x <= a ) { - y = 0.0f; + y = 0.0F; } else if ( x >= b ) { - y = 1.0f; + y = 1.0F; } - else if ( ( x >= a ) && ( x <= ( ( a + b )*0.5f ) ) ) { + else if ( ( x >= a ) && ( x <= ( ( a + b )*0.5F ) ) ) { tmp = ( x - a )/( b - a ); - y = 2.0f*tmp*tmp; + y = 2.0F*tmp*tmp; } - else if ( ( x <= b ) && ( x >= ( ( a + b )*0.5f ) ) ) { + else if ( ( x <= b ) && ( x >= ( ( a + b )*0.5F ) ) ) { tmp = ( x - b )/( b - a ); - y = ( 1.0f - ( 2.0f*tmp*tmp ) ); + y = ( 1.0F - ( 2.0F*tmp*tmp ) ); } else { - y = 0.0f; + y = 0.0F; } return y; @@ -1029,21 +1073,21 @@ static float qFIS_TSMF( const qFIS_IO_Base_t * const in, float diff, a, b, ta, tb, ma, mb; float x = in[ 0 ].value; (void)n; - qFIS_IO_Base_t tmp; + qFIS_IO_Base_t tmp = { 0.0F, 0.0F, 0.0F }; a = p[ 0 ]; /*start*/ b = p[ 1 ]; /*end*/ diff = b - a; - diff = 0.5f*diff*diff; + diff = 0.5F*diff*diff; /*cstat -MISRAC2012-Dir-4.11_a -MISRAC2012-Dir-4.11_b*/ - ta = a + sqrtf( x*diff ); + ta = a + QLIB_SQRT( x*diff ); tmp.value = ta; ma = qFIS_SMF( &tmp, p, n ); - tb = b + sqrtf( -( x - 1.0f )*diff ); + tb = b + QLIB_SQRT( -( x - 1.0F )*diff ); tmp.value = tb; mb = qFIS_SMF( &tmp, p, n ); /*cstat +MISRAC2012-Dir-4.11_a +MISRAC2012-Dir-4.11_b*/ - return ( fabsf( x - ma ) < fabsf( x - mb ) ) ? ta : tb; + return ( QLIB_ABS( x - ma ) < QLIB_ABS( x - mb ) ) ? ta : tb; } /*============================================================================*/ static float qFIS_ZMF( const qFIS_IO_Base_t * const in, @@ -1057,21 +1101,21 @@ static float qFIS_ZMF( const qFIS_IO_Base_t * const in, a = p[ 0 ]; b = p[ 1 ]; if ( x <= a ) { - y = 1.0f; + y = 1.0F; } else if ( x >= b ) { - y = 0.0f; + y = 0.0F; } - else if ( ( x >= a ) && ( x <= ( ( a + b )*0.5f ) ) ) { + else if ( ( x >= a ) && ( x <= ( ( a + b )*0.5F ) ) ) { tmp = ( x - a )/( b - a ); - y = 1.0f - ( 2.0f*tmp*tmp ); + y = 1.0F - ( 2.0F*tmp*tmp ); } - else if ( ( x <= b ) && ( x >= ( ( a + b )*0.5f ) ) ) { + else if ( ( x <= b ) && ( x >= ( ( a + b )*0.5F ) ) ) { tmp = ( x - b )/( b - a ); - y = 2.0f*tmp*tmp; + y = 2.0F*tmp*tmp; } else { - y = 0.0f; + y = 0.0F; } return y; @@ -1089,20 +1133,20 @@ static float qFIS_LinSMF( const qFIS_IO_Base_t * const in, b = p[ 1 ]; if ( a < b ) { if ( x < a ) { - y = 0.0f; + y = 0.0F; } else if ( x > b ) { - y = 1.0f; + y = 1.0F; } else { y = ( x - a )/( b - a ); } } - else if ( fabsf( a - b ) <= FLT_MIN ) { - y = ( x < a ) ? 0.0f : 1.0f; + else if ( qFFMath_IsEqual( a, b ) ) { + y = ( x < a ) ? 0.0F : 1.0F; } else { - y = 0.0f; + y = 0.0F; } return y; @@ -1120,20 +1164,20 @@ static float qFIS_LinZMF( const qFIS_IO_Base_t * const in, b = p[ 1 ]; if ( a < b ) { if ( x < a ) { - y = 1.0f; + y = 1.0F; } else if ( x > b ) { - y = 0.0f; + y = 0.0F; } else { y = ( a - x )/( a - b ); } } - else if ( fabsf( a - b ) <= FLT_MIN ) { - y = ( x < a ) ? 1.0f : 0.0f; + else if ( qFFMath_IsEqual( a, b ) ) { + y = ( x < a ) ? 1.0F : 0.0F; } else { - y = 0.0f; + y = 0.0F; } return y; @@ -1146,28 +1190,28 @@ static float qFIS_TZMF( const qFIS_IO_Base_t * const in, float diff, a, b, ta, tb, ma, mb; float x = in[ 0 ].value; (void)n; - qFIS_IO_Base_t tmp; + qFIS_IO_Base_t tmp = { 0.0F, 0.0F, 0.0F }; a = p[ 0 ]; /*start*/ b = p[ 1 ]; /*end*/ diff = b - a; - diff = 0.5f*diff*diff; + diff = 0.5F*diff*diff; /*cstat -MISRAC2012-Dir-4.11_a -MISRAC2012-Dir-4.11_b*/ - ta = a + sqrtf( -( x - 1.0f )*diff ); + ta = a + QLIB_SQRT( -( x - 1.0F )*diff ); tmp.value = ta; ma = qFIS_SMF( &tmp, p, n ); - tb = b + sqrtf( x*diff ); + tb = b + QLIB_SQRT( x*diff ); tmp.value = tb; mb = qFIS_SMF( &tmp, p, n ); /*cstat +MISRAC2012-Dir-4.11_a +MISRAC2012-Dir-4.11_b*/ - return ( fabsf( x - ma ) < fabsf( x - mb ) ) ? ta : tb; + return ( QLIB_ABS( x - ma ) < QLIB_ABS( x - mb ) ) ? ta : tb; } /*============================================================================*/ static float qFIS_PiMF( const qFIS_IO_Base_t * const in, const float *p, const size_t n ) { - return fabsf( qFIS_SMF( in , p, n )*qFIS_ZMF( in , &p[ 2 ], n ) ); + return QLIB_ABS( qFIS_SMF( in , p, n )*qFIS_ZMF( in , &p[ 2 ], n ) ); } /*============================================================================*/ static float qFIS_SingletonMF( const qFIS_IO_Base_t * const in, @@ -1177,7 +1221,7 @@ static float qFIS_SingletonMF( const qFIS_IO_Base_t * const in, float x = in[ 0 ].value; (void)n; - return ( ( fabsf( x - p[ 0 ] ) <= FLT_MIN ) ? 1.0f : 0.0f ); + return ( qFFMath_IsEqual( x, p[ 0 ] ) ) ? 1.0F : 0.0F; } /*============================================================================*/ static float qFIS_ConcaveMF( const qFIS_IO_Base_t * const in, @@ -1191,13 +1235,13 @@ static float qFIS_ConcaveMF( const qFIS_IO_Base_t * const in, i = p[ 0 ]; e = p[ 1 ]; if ( ( i <= e ) && ( x < e ) ) { - y = ( e - i )/( ( 2.0f*e ) - i -x ); + y = ( e - i )/( ( 2.0F*e ) - i -x ); } else if ( ( i > e ) && ( x > e ) ) { - y = ( i - e )/( -( 2.0f*e ) + i +x ); + y = ( i - e )/( -( 2.0F*e ) + i +x ); } else { - y = 1.0f; + y = 1.0F; } return y; @@ -1212,7 +1256,7 @@ static float qFIS_TConcaveMF( const qFIS_IO_Base_t * const in, i = p[ 0 ]; e = p[ 1 ]; - return ( ( i - e )/qFIS_ConcaveMF( in, p, n ) ) + ( 2.0f*e ) - i; + return ( ( i - e )/qFIS_ConcaveMF( in, p, n ) ) + ( 2.0F*e ) - i; } /*============================================================================*/ static float qFIS_SpikeMF( const qFIS_IO_Base_t * const in, @@ -1226,7 +1270,7 @@ static float qFIS_SpikeMF( const qFIS_IO_Base_t * const in, w = p[ 0 ]; c = p[ 1 ]; - return expf( -fabsf( 10.0f*( x - c )/w ) ); + return QLIB_EXP( -QLIB_ABS( 10.0F*( x - c )/w ) ); } /*============================================================================*/ static float qFIS_TLinSMF( const qFIS_IO_Base_t * const in, @@ -1268,7 +1312,7 @@ static float qFIS_RectangleMF( const qFIS_IO_Base_t * const in, s = p[ 0 ]; e = p[ 1 ]; - return ( ( x >= s ) && ( x <= e ) ) ? 1.0f : 0.0f; + return ( ( x >= s ) && ( x <= e ) ) ? 1.0F : 0.0F; } /*============================================================================*/ static float qFIS_CosineMF( const qFIS_IO_Base_t * const in, @@ -1277,16 +1321,16 @@ static float qFIS_CosineMF( const qFIS_IO_Base_t * const in, { float x = in[ 0 ].value; float c, w, y; - const float pi = 3.14159265358979323846f; + const float pi = 3.14159265358979323846F; (void)n; c = p[ 0 ]; w = p[ 1 ]; - if ( ( x < ( c - ( 0.5*w ) ) ) || ( x > ( c + ( 0.5f*w ) ) ) ) { - y = 0.0f; + if ( ( x < ( c - ( 0.5F*w ) ) ) || ( x > ( c + ( 0.5F*w ) ) ) ) { + y = 0.0F; } else { - y = 0.5f*( 1.0f + cosf( 2.0f/w*pi*( x - c) ) ); + y = 0.5F*( 1.0F + QLIB_COS( 2.0F/w*pi*( x - c) ) ); } return y; @@ -1305,10 +1349,10 @@ static float qFIS_LinearMF( const qFIS_IO_Base_t * const in, const float *p, const size_t n ) { - float px = 0.0f; + float px = 0.0F; size_t i; - for ( i = 0u ; i < n ; ++i ) { + for ( i = 0U ; i < n ; ++i ) { px += in[ i ].value*p[ i ]; } px += p[ i ]; @@ -1319,49 +1363,38 @@ static float qFIS_LinearMF( const qFIS_IO_Base_t * const in, static float qFIS_Min( const float a, const float b ) { - return qFIS_Bound( ( a < b ) ? a : b, 0.0f, 1.0f ); + return qFIS_Bound( ( a < b ) ? a : b, 0.0F, 1.0F ); } /*============================================================================*/ static float qFIS_Max( const float a, const float b ) { - return qFIS_Bound( ( a > b ) ? a : b, 0.0f, 1.0f ); + return qFIS_Bound( ( a > b ) ? a : b, 0.0F, 1.0F ); } /*============================================================================*/ static float qFIS_Prod( const float a, const float b ) { - return qFIS_Bound( a*b, 0.0f, 1.0f ); + return qFIS_Bound( a*b, 0.0F, 1.0F ); } /*============================================================================*/ static float qFIS_ProbOR( const float a, const float b ) { - return qFIS_Bound( a + b - ( a*b ), 0.0f, 1.0f ); + return qFIS_Bound( a + b - ( a*b ), 0.0F, 1.0F ); } /*============================================================================*/ static float qFIS_Sum( const float a, const float b ) { - return qFIS_Bound( a + b, 0.0f, 1.0f ); + return qFIS_Bound( a + b, 0.0F, 1.0F ); } /*============================================================================*/ static float qFIS_Bound( float y, - const float ymin, - const float ymax ) + const float yMin, + const float yMax ) { - if ( 1 == (int)isnan( y ) ) { - y = ymin; - } - else { - if ( y < ymin ) { - y = ymin; - } - - if ( y > ymax ) { - y = ymax; - } - } + (void)qFFMath_InRangeCoerce( &y, yMin, yMax ); return y; } diff --git a/qfp16.c b/qfp16.c index cc33d6b..d86c759 100644 --- a/qfp16.c +++ b/qfp16.c @@ -1,14 +1,16 @@ /*! * @file qfp16.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qfp16.h" +#include +#include /*used only for internal operations*/ /*! @cond */ -struct _qFP16_intern_s +struct _qFP16_intern_s //skipcq: CXX-E2000 { const qFP16_t exp_max, @@ -16,8 +18,7 @@ struct _qFP16_intern_s f_3, /* [ 3 ] */ f_16, /* [ -16 ] */ f_100, /* [ 100 ] */ - f_6_5, /* [ 6.5 ] */ - f_4_pi; /* [ 4/pi ] */ + f_6_5; /* [ 6.5 ] */ const float one_fp16_f; /* [ 1/65536 ] */ const double one_fp16_d;/* [ 1/65536 ] */ const uint32_t @@ -26,22 +27,21 @@ struct _qFP16_intern_s integer_mask; }; -static const struct _qFP16_intern_s intern = { +static const struct _qFP16_intern_s intern = { //skipcq: CXX-E2000 /*exp_max*/ 681391, /*f_2*/ 131072, /*f_3*/ 196608, /*f_16*/ 1048576, /*f_100*/ 6553600, /*f_6_5*/ 425984, - /*f_4_pi*/ 83443, - /*one_fp16_f*/ 0.0000152587890625f, + /*one_fp16_f*/ 0.0000152587890625F, /*one_fp16_d*/ 0.0000152587890625, - /*overflow_mask*/ 0x80000000uL, - /*fraction_mask*/ 0x0000FFFFuL, - /*integer_mask*/ 0xFFFF0000uL + /*overflow_mask*/ 0x80000000U, + /*fraction_mask*/ 0x0000FFFFU, + /*integer_mask*/ 0xFFFF0000U }; -const struct _qFP16_const_s qFP16 = { +const struct _qFP16_const_s qFP16 = { //skipcq: CXX-E2000 /*f_e*/ 178145, /*f_log2e*/ 94548, /*f_log10e*/ 28462, @@ -67,14 +67,11 @@ const struct _qFP16_const_s qFP16 = { /*f_360*/ 23592960, }; -static qFP16_Settings_t fp_default = { -2147483647, 2147483647, 1u, 0u }; -static qFP16_Settings_t *fp = &fp_default; +static qFP16_Settings_t fp_default = { -2147483647, 2147483647, 1U, 0U }; //skipcq: CXX-W2009 +static qFP16_Settings_t *fp = &fp_default; //skipcq: CXX-W2009 /*! @endcond */ -static uint32_t qFP16_OverflowCheck( uint32_t res, - const uint32_t x, - const uint32_t y ); static qFP16_t qFP16_rs( const qFP16_t x ); static qFP16_t qFP16_log2i( qFP16_t x ); static char *qFP16_itoa( char *buf, @@ -96,13 +93,16 @@ int qFP16_SettingsSet( qFP16_Settings_t * const instance, { int retValue = 0; - if ( ( NULL != instance ) && ( max > min ) && ( rounding <= 1u ) && ( saturate <= 1u ) ) { + if ( ( NULL != instance ) && ( max > min ) && ( rounding <= 1U ) && ( saturate <= 1U ) ) { instance->min = min; instance->max = max; instance->rounding = rounding; instance->saturate = saturate; retValue = 1; } + else { + (void)fp_default; + } return retValue; } @@ -121,11 +121,13 @@ int qFP16_FPToInt( const qFP16_t x ) { int retValue; - if ( 1u == fp->rounding ) { + if ( 1U == fp->rounding ) { if ( x >= 0 ) { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( x + ( qFP16.one >> 1 ) ) / qFP16.one; } else { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( x - ( qFP16.one >> 1 ) ) / qFP16.one; } } @@ -147,8 +149,8 @@ qFP16_t qFP16_FloatToFP( const float x ) /*cstat -CERT-FLP36-C*/ retValue = x * (float)qFP16.one; /*cstat +CERT-FLP36-C*/ - if ( 1u == fp->rounding ) { - retValue += ( retValue >= 0.0f) ? 0.5f : -0.5f; + if ( 1U == fp->rounding ) { + retValue += ( retValue >= 0.0F ) ? 0.5F : -0.5F; } return (qFP16_t)retValue; @@ -167,7 +169,7 @@ qFP16_t qFP16_DoubleToFP( const double x ) /*cstat -CERT-FLP36-C*/ retValue = x * (double)qFP16.one; /*cstat +CERT-FLP36-C*/ - if ( 1u == fp->rounding ) { + if ( 1U == fp->rounding ) { retValue += ( retValue >= 0.0 ) ? 0.5 : -0.5; } @@ -197,6 +199,7 @@ qFP16_t qFP16_Abs( const qFP16_t x ) /*============================================================================*/ qFP16_t qFP16_Floor( const qFP16_t x ) { + /*cppcheck-suppress misra-c2012-10.8 */ return (qFP16_t)( (uint32_t)x & intern.integer_mask ); } /*============================================================================*/ @@ -220,7 +223,9 @@ qFP16_t qFP16_Add( const qFP16_t X, uint32_t retValue; retValue = x + y; - retValue = qFP16_OverflowCheck( retValue, x, y ); + if ( ( 0U == ( ( x ^ y ) & intern.overflow_mask ) ) && ( 0U != ( ( x ^ retValue ) & intern.overflow_mask ) ) ) { + retValue = (uint32_t)qFP16.overflow; + } return qFP16_Saturate( (qFP16_t)retValue, X, X ); } @@ -232,7 +237,9 @@ qFP16_t qFP16_Sub( const qFP16_t X, uint32_t retValue; retValue = x - y; - retValue = qFP16_OverflowCheck( retValue, x, y ); + if ( ( 0U != ( ( x ^ y ) & intern.overflow_mask ) ) && ( 0U != ( ( x ^ retValue ) & intern.overflow_mask ) ) ) { + retValue = (uint32_t)qFP16.overflow; + } return qFP16_Saturate( (qFP16_t)retValue, X, X ); } @@ -244,23 +251,29 @@ qFP16_t qFP16_Mul( const qFP16_t x, int32_t a, c, ac, adcb, mulH; uint32_t b, d, bd, tmp, mulL; /*cstat -MISRAC2012-Rule-10.3*/ + /*cppcheck-suppress misra-c2012-10.1 */ a = ( x >> 16 ); + /*cppcheck-suppress misra-c2012-10.1 */ c = ( y >> 16 ); b = ( x & 0xFFFF ); d = ( y & 0xFFFF ); /*cstat +MISRAC2012-Rule-10.3*/ ac = a*c; + /*cppcheck-suppress misra-c2012-10.8 */ adcb = (int32_t)( ( (uint32_t)a*d ) + ( (uint32_t)c*b ) ); bd = b*d; + /*cppcheck-suppress misra-c2012-10.1 */ mulH = ac + ( adcb >> 16 ); tmp = (uint32_t)adcb << 16; mulL = bd + tmp; if ( mulL < bd ) { ++mulH; } + /*cppcheck-suppress misra-c2012-10.6 */ a = ( mulH < 0 ) ? -1 : 0; + /*cppcheck-suppress misra-c2012-10.1 */ if ( a == ( mulH >> 15 ) ) { - if ( 1u == fp->rounding ) { + if ( 1U == fp->rounding ) { uint32_t tmp2; tmp2 = mulL; @@ -269,10 +282,12 @@ qFP16_t qFP16_Mul( const qFP16_t x, if ( mulL > tmp2 ) { --mulH; } + /*cppcheck-suppress misra-c2012-10.1 */ retValue = (qFP16_t)( mulH << 16 ) | (qFP16_t)( mulL >> 16 ); retValue += 1; } else { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = (qFP16_t)( mulH << 16 ) | (qFP16_t)( mulL >> 16 ); } } @@ -286,7 +301,7 @@ qFP16_t qFP16_Div( const qFP16_t x, qFP16_t retValue = fp->min; if ( 0 != y ) { - uint32_t xRem, xDiv, bit = 0x10000uL; + uint32_t xRem, xDiv, bit = 0x10000U; xRem = (uint32_t)( ( x >= 0 ) ? x : -x ); xDiv = (uint32_t)( ( y >= 0 ) ? y : -y ); @@ -297,11 +312,11 @@ qFP16_t qFP16_Div( const qFP16_t x, } retValue = qFP16.overflow; /*cstat -MISRAC2012-Rule-14.3_a*/ - if ( 0uL != bit ) { /*MISRAC2012-Rule-14.3_a false positive*/ + if ( 0U != bit ) { /*MISRAC2012-Rule-14.3_a false positive*/ /*cstat +MISRAC2012-Rule-14.3_a*/ - uint32_t quotient = 0uL; + uint32_t quotient = 0U; - if ( 0uL != ( xDiv & 0x80000000uL ) ) { + if ( 0U != ( xDiv & 0x80000000U ) ) { if ( xRem >= xDiv ) { quotient |= bit; xRem -= xDiv; @@ -310,7 +325,7 @@ qFP16_t qFP16_Div( const qFP16_t x, bit >>= 1; } - while ( ( 0uL != bit ) && ( 0uL != xRem ) ) { + while ( ( 0U != bit ) && ( 0U != xRem ) ) { if ( xRem >= xDiv ) { quotient |= bit; xRem -= xDiv; @@ -318,15 +333,15 @@ qFP16_t qFP16_Div( const qFP16_t x, xRem <<= 1; bit >>= 1; } - if ( 1u == fp->rounding ) { + if ( 1U == fp->rounding ) { if ( xRem >= xDiv ) { ++quotient; } } retValue = (qFP16_t)quotient; - - if ( 0uL != ( (uint32_t)( x ^ y ) & intern.overflow_mask ) ) { + /*cppcheck-suppress misra-c2012-10.8 */ + if ( 0U != ( (uint32_t)( x ^ y ) & intern.overflow_mask ) ) { if ( quotient == (uint32_t)fp->min ) { retValue = qFP16.overflow; } @@ -361,39 +376,46 @@ qFP16_t qFP16_Sqrt( qFP16_t x ) uint8_t n; retValue = 0; - bit = ( 0 != ( x & (qFP16_t)0xFFF00000uL ) ) ? (uint32_t)( 1 << 30 ) - : (uint32_t)( 1 << 18 ); + /*cppcheck-suppress [ cert-INT31-c, misra-c2012-12.2, misra-c2012-12.1 ]*/ + bit = ( 0 != ( x & (qFP16_t)4293918720 ) ) ? ( 1U << 30U ) : ( 1U << 18U ); while ( bit > (uint32_t)x ) { - bit >>= 2; + bit >>= 2U; } - for ( n = 0u ; n < 2u ; ++n ) { - while ( 0u != bit ) { + for ( n = 0U ; n < 2U ; ++n ) { + while ( 0U != bit ) { + /*cppcheck-suppress misra-c2012-10.8 */ if ( x >= (qFP16_t)( (uint32_t)retValue + bit ) ) { + /*cppcheck-suppress misra-c2012-10.8 */ x -= (qFP16_t)( (uint32_t)retValue + bit ); - retValue = (qFP16_t)( ( (uint32_t)retValue >> 1uL ) + bit ); + /*cppcheck-suppress misra-c2012-10.8 */ + retValue = (qFP16_t)( ( (uint32_t)retValue >> 1U ) + bit ); } else { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( retValue >> 1 ); } - bit >>= 2; + bit >>= 2U; } - if ( 0u == n ) { + if ( 0U == n ) { if ( x > 65535 ) { x -= retValue; + /*cppcheck-suppress misra-c2012-10.1 */ x = ( x << 16 ) - qFP16.one_half; + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( retValue << 16 ) + qFP16.one_half; } else { x <<= 16; retValue <<= 16; } - bit = 1 << 14; + /*cppcheck-suppress [ misra-c2012-10.6, misra-c2012-12.2 ] */ + bit = 1U << 14U; } } } - if ( ( 1u == fp->rounding ) && ( x > retValue ) ) { + if ( ( 1U == fp->rounding ) && ( x > retValue ) ) { ++retValue; } @@ -403,7 +425,7 @@ qFP16_t qFP16_Sqrt( qFP16_t x ) qFP16_t qFP16_Exp( qFP16_t x ) { qFP16_t retValue, term; - uint8_t isNegative; + bool isNegative; int i; if ( 0 == x ) { @@ -419,8 +441,8 @@ qFP16_t qFP16_Exp( qFP16_t x ) retValue = 0; } else { - isNegative = (uint8_t)( x < 0 ); - if ( 1u == isNegative ) { + isNegative = ( x < 0 ); + if ( isNegative ) { x = -x; } @@ -436,7 +458,7 @@ qFP16_t qFP16_Exp( qFP16_t x ) } } - if ( 1u == isNegative ) { + if ( isNegative ) { retValue = qFP16_Div( qFP16.one, retValue ); } } @@ -450,7 +472,7 @@ qFP16_t qFP16_Log( qFP16_t x ) static const qFP16_t e4 = 3578144; /*e^4*/ if ( x > 0 ) { - qFP16_t guess = intern.f_2, delta, e; + qFP16_t guess = intern.f_2, delta; int scaling = 0, count = 0; while ( x > intern.f_100 ) { @@ -459,12 +481,13 @@ qFP16_t qFP16_Log( qFP16_t x ) } while ( x < qFP16.one ) { - x = qFP16_Mul(x, e4 ); + x = qFP16_Mul( x, e4 ); scaling -= 4; } do { - e = qFP16_Exp( guess ); + qFP16_t e = qFP16_Exp( guess ); + delta = qFP16_Div( x - e , e ); if ( delta > intern.f_3 ) { @@ -498,7 +521,7 @@ qFP16_t qFP16_Log2( const qFP16_t x ) retValue = qFP16_log2i( x ); } } - if ( 1u == fp->saturate ) { + if ( 1U == fp->saturate ) { if ( qFP16.overflow == retValue ) { retValue = fp->min; } @@ -507,12 +530,12 @@ qFP16_t qFP16_Log2( const qFP16_t x ) return retValue; } /*============================================================================*/ -qFP16_t fp16_RadToDeg( const qFP16_t x ) +qFP16_t qFP16_RadToDeg( const qFP16_t x ) { return qFP16_Mul( qFP16_WrapToPi( x ), qFP16.f_180_pi ); } /*============================================================================*/ -qFP16_t fp16_DegToRad( qFP16_t x ) +qFP16_t qFP16_DegToRad( qFP16_t x ) { return qFP16_Mul( qFP16_WrapTo180( x ), qFP16.f_pi_180 ); } @@ -537,7 +560,7 @@ qFP16_t qFP16_WrapTo180( qFP16_t x ) while ( x > qFP16.f_180 ) { x -= qFP16.f_360; } - while ( x <= -qFP16.f_pi ) { + while ( x <= -qFP16.f_180 ) { x += qFP16.f_360; } } @@ -588,8 +611,8 @@ qFP16_t qFP16_Atan2( const qFP16_t y, const qFP16_t QFP16_0_981689 = 0x0000FB50; const qFP16_t QFP16_0_196289 = 0x00003240; static const qFP16_t f_3pi_div_4 = 154415; /*3*pi/4*/ - - mask = ( y >> ( sizeof(qFP16_t)*7u ) ); + /*cppcheck-suppress misra-c2012-10.1 */ + mask = ( y >> ( sizeof(qFP16_t)*7U ) ); absY = ( y + mask ) ^ mask; if ( x >= 0 ) { r = qFP16_Div( ( x - absY ), ( x + absY ) ); @@ -649,6 +672,7 @@ qFP16_t qFP16_Cosh( qFP16_t x ) enx = qFP16_Exp( -x ); if ( ( qFP16.overflow != epx ) && ( qFP16.overflow != enx ) ) { retValue = epx + enx; + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( retValue >> 1 ); } } @@ -675,6 +699,7 @@ qFP16_t qFP16_Sinh( qFP16_t x ) enx = qFP16_Exp( -x ); if ( ( qFP16.overflow != epx ) && ( qFP16.overflow != enx ) ) { retValue = epx - enx; + /*cppcheck-suppress misra-c2012-10.1 */ retValue = ( retValue >> 1 ); } } @@ -710,12 +735,13 @@ qFP16_t qFP16_Polyval( const qFP16_t * const p, const size_t n, const qFP16_t x ) { - qFP16_t fx, tmp; + qFP16_t fx; size_t i; /*polynomial evaluation using Horner's method*/ fx = p[ 0 ]; - for ( i = 1u ; i < n ; ++i ) { - tmp = qFP16_Mul( fx, x ); + for ( i = 1U ; i < n ; ++i ) { + qFP16_t tmp = qFP16_Mul( fx, x ); + if ( qFP16.overflow == tmp ) { fx = qFP16.overflow; break; @@ -734,6 +760,7 @@ qFP16_t qFP16_IPow( const qFP16_t x, int32_t i; retValue = qFP16.one; + /*cppcheck-suppress misra-c2012-10.1 */ n = y >> 16; if ( 0 == n ) { retValue = qFP16.one; @@ -758,7 +785,7 @@ qFP16_t qFP16_Pow( const qFP16_t x, { qFP16_t retValue = qFP16.overflow; - if ( ( 0uL == ( (uint32_t)y & intern.fraction_mask ) ) && ( y > 0 ) ) { + if ( ( 0U == ( (uint32_t)y & intern.fraction_mask ) ) && ( y > 0 ) ) { /*handle integer exponent explicitly*/ retValue = qFP16_IPow( x, y ); } @@ -794,7 +821,7 @@ char* qFP16_FPToA( const qFP16_t num, str[ 8 ] = '\0'; } else { - const uint32_t iScales[ 6 ] = { 1uL, 10uL, 100uL, 1000uL, 10000uL, 100000uL }; + const uint32_t iScales[ 6 ] = { 1U, 10U, 100U, 1000U, 10000U, 100000U }; uint32_t uValue, fPart, scale; int32_t iPart; @@ -818,11 +845,11 @@ char* qFP16_FPToA( const qFP16_t num, iPart++; fPart -= scale; } - str = qFP16_itoa( str, 10000, (uint32_t)iPart, 1u ); + str = qFP16_itoa( str, 10000, (uint32_t)iPart, 1U ); - if ( 1u != scale ) { + if ( 1U != scale ) { *str++ = '.'; - str = qFP16_itoa( str, scale/10u, fPart, 0u ); + str = qFP16_itoa( str, scale/10U, fPart, 0U ); } *str = '\0'; } @@ -833,10 +860,10 @@ char* qFP16_FPToA( const qFP16_t num, qFP16_t qFP16_AToFP( const char *s ) { uint8_t neg; - uint32_t iPart = 0uL, fPart = 0uL, scale = 1uL, digit; + uint32_t iPart = 0U, fPart = 0U, scale = 1U, digit; int32_t count = 0; qFP16_t retValue = qFP16.overflow; - int point_seen = 0, overflow = 0; + bool point_seen = false, overflow = false, badchr = false; char c; /*cstat -MISRAC2012-Dir-4.11_h*/ @@ -844,68 +871,59 @@ qFP16_t qFP16_AToFP( const char *s ) s++; /*discard whitespaces*/ } - neg = (uint8_t)( '-' == *s ); + neg = ( '-' == *s ) ? 1U : 0U; if ( ( '+' == *s ) || ( '-' == *s ) ) { s++; /*move to the next sign*/ } for ( c = s[ 0 ] ; '\0' != c ; c = s[ 0 ] ) { if ( '.' == c ) { - point_seen = 1; + point_seen = true; } else if ( 0 != isdigit( (int)c ) ) { digit = (uint32_t)c - (uint32_t)'0'; - if ( 1 == point_seen ) { /* Decode the fractional part */ - scale *= 10u; - fPart *= 10u; + if ( point_seen ) { /* Decode the fractional part */ + scale *= 10U; + fPart *= 10U; fPart += digit; } else { /* Decode the decimal part */ - iPart *= 10u; + iPart *= 10U; iPart += digit; ++count; - overflow = (int)( ( 0 == count ) || ( count > 5 ) || - ( iPart > 32768uL ) || - ( ( 0u == neg ) && ( iPart > 32767uL ) ) ); - if ( 1 == overflow ) { - break; - } + overflow = ( ( 0 == count ) || ( count > 5 ) || + ( iPart > 32768U ) || + ( ( 0U == neg ) && ( iPart > 32767U ) ) ); } } else { + badchr = true; + } + if ( overflow || badchr ) { break; } s++; } - if ( 0 == overflow ) { + if ( false == overflow ) { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = (qFP16_t)iPart << 16; retValue += qFP16_Div( (qFP16_t)fPart, (qFP16_t)scale ); - retValue = ( 1u == neg ) ? -retValue : retValue; + retValue = ( 1U == neg ) ? -retValue : retValue; } /*cstat +MISRAC2012-Dir-4.11_h*/ return retValue; } /*============================================================================*/ -static uint32_t qFP16_OverflowCheck( uint32_t res, - const uint32_t x, - const uint32_t y ) -{ - if ( ( 0uL == ( ( x ^ y ) & intern.overflow_mask ) ) && - ( 0uL != ( ( x ^ res ) & intern.overflow_mask ) ) ) { - res = (uint32_t)qFP16.overflow; - } - - return res; -} -/*============================================================================*/ static qFP16_t qFP16_rs( const qFP16_t x ) { qFP16_t retValue; - if ( 1u == fp->rounding ) { - retValue = ( x >> 1u ) + ( x & 1 ); + if ( 1U == fp->rounding ) { + /*cppcheck-suppress misra-c2012-10.1 */ + retValue = ( x >> 1U ) + ( x & 1 ); } else { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = x >> 1; } @@ -922,6 +940,7 @@ static qFP16_t qFP16_log2i( qFP16_t x ) } if ( 0 == x ) { + /*cppcheck-suppress misra-c2012-10.1 */ retValue = retValue << 16; } else { @@ -934,7 +953,7 @@ static qFP16_t qFP16_log2i( qFP16_t x ) x = qFP16_rs( x ); } } - if ( 1u == fp->rounding ) { + if ( 1U == fp->rounding ) { x = qFP16_Mul( x, x ); if ( x >= intern.f_2 ) { ++retValue; @@ -950,16 +969,16 @@ static char *qFP16_itoa( char *buf, uint32_t value, uint8_t skip ) { - while ( 0u != scale ) { + while ( 0U != scale ) { uint32_t digit = ( value / scale ); - if ( ( 0u == skip ) || ( 0u != digit ) || ( 1u == scale ) ) { - skip = 0u; + if ( ( 0U == skip ) || ( 0U != digit ) || ( 1U == scale ) ) { + skip = 0U; /*cstat -MISRAC2012-Rule-10.2 -MISRAC2012-Rule-10.3*/ *buf++ = (char)'0' + (char)digit; /*cstat +MISRAC2012-Rule-10.2 +MISRAC2012-Rule-10.3*/ value %= scale; } - scale /= 10u; + scale /= 10U; } return buf; @@ -971,7 +990,7 @@ static qFP16_t qFP16_Saturate( const qFP16_t nsInput, { qFP16_t retValue = nsInput; - if ( 1u == fp->saturate ) { + if ( 1U == fp->saturate ) { if ( qFP16.overflow == nsInput ) { retValue = ( ( x >= 0 ) == ( y >= 0 ) ) ? fp->max : fp->min; } diff --git a/qltisys.c b/qltisys.c index 6652a71..225615b 100644 --- a/qltisys.c +++ b/qltisys.c @@ -1,10 +1,11 @@ /*! * @file qltisys.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qltisys.h" +#include static float qLTISys_DiscreteUpdate( qLTISys_t * const sys, const float u ); @@ -29,27 +30,27 @@ static float qLTISys_DiscreteUpdate( qLTISys_t * const sys, static float qLTISys_ContinuosUpdate( qLTISys_t * const sys, const float u ) { - float y = 0.0f; - float dx0 = 0.0f; + float y = 0.0F; + float dx0 = 0.0F; - if ( 1u == sys->n ) { + if ( 1U == sys->n ) { dx0 = ( u - ( sys->xc[ 0 ].x[ 0 ]*sys->a[ 0 ] ) ); - (void)sys->integrate( &sys->xc[ 0 ], dx0 , sys->dt ); + (void)sys->integrate( &sys->xc[ 0 ], dx0 , sys->dt, true ); y = ( sys->b[ 0 ] - ( sys->a[ 0 ]*sys->b0 ) )*sys->xc[ 0 ].x[ 0 ]; } else { size_t i; /*compute states of the system by using the controllable canonical form*/ - for ( i = ( sys->n - 1u ) ; i >= 1u ; --i ) { + for ( i = ( sys->n - 1U ) ; i >= 1U ; --i ) { dx0 += sys->a[ i ]*sys->xc[ i ].x[ 0 ]; /*compute the first derivative*/ /*integrate to obtain the remaining states*/ - (void)sys->integrate( &sys->xc[ i ], sys->xc[ i - 1u ].x[ 0 ], sys->dt ); + (void)sys->integrate( &sys->xc[ i ], sys->xc[ i - 1U ].x[ 0 ], sys->dt, true ); /*compute the first part of the output*/ y += ( sys->b[ i ] - ( sys->a[ i ]*sys->b0 ) )*sys->xc[ i ].x[ 0 ]; } /*compute remaining part of the output that depends of the first state*/ dx0 = u - ( dx0 + ( sys->a[ 0 ]*sys->xc[ 0 ].x[ 0 ] ) ); - (void)sys->integrate( &sys->xc[ 0 ], dx0, sys->dt ); /*integrate to get the first state*/ + (void)sys->integrate( &sys->xc[ 0 ], dx0, sys->dt, true ); /*integrate to get the first state*/ /*compute the remaining part of the output*/ y += ( sys->b[ 0 ] - ( sys->a[ 0 ]*sys->b0 ) )*sys->xc[ 0 ].x[ 0 ]; } @@ -60,7 +61,7 @@ static float qLTISys_ContinuosUpdate( qLTISys_t * const sys, float qLTISys_Excite( qLTISys_t * const sys, float u ) { - float y = 0.0f; + float y = 0.0F; if ( 1 == qLTISys_IsInitialized( sys ) ) { if ( NULL != sys->tDelay.head ) { /*check if has delay*/ @@ -119,7 +120,33 @@ int qLTISys_IsInitialized( const qLTISys_t * const sys ) int retValue = 0; if ( NULL != sys ) { - retValue = (int)( ( NULL != sys->sysUpdate ) && ( ( NULL != sys->xc ) || ( NULL != sys->xd ) ) ); + /*cppcheck-suppress misra-c2012-10.6 */ + retValue = ( ( NULL != sys->sysUpdate ) && ( ( NULL != sys->xc ) || ( NULL != sys->xd ) ) ) ? 1 : 0; + } + + return retValue; +} +/*============================================================================*/ +int qLTISys_SetInitStates( qLTISys_t * const sys, const float * const xi ) +{ + int retValue = 0; + + if ( 1 == qLTISys_IsInitialized( sys ) ) { + size_t i; + + for ( i = 0U; i < sys->n ; ++i ) { + const float zero = 0.0F; + const float *iv = ( NULL != xi ) ? &xi[ i ] : &zero; + + if ( sys->dt <= 0.0F ) { + sys->xd[ i ] = iv[ 0 ]; + } + else { + qNumA_StateInit( &sys->xc[ i ], iv[ 0 ], iv[ 0 ], iv[ 0 ] ); + } + } + + retValue = 1; } return retValue; @@ -135,44 +162,47 @@ int qLTISys_Setup( qLTISys_t * const sys, { int retValue = 0; - if ( ( NULL != sys ) && ( NULL != num ) && ( NULL != den ) && ( NULL != x ) && ( na > 0u ) ) { + if ( ( NULL != sys ) && ( NULL != num ) && ( NULL != den ) && ( NULL != x ) && ( na > 0U ) ) { float a0; size_t i; - if ( dt <= 0.0f ) { /*discrete system*/ + if ( dt <= 0.0F ) { /*discrete system*/ sys->b = num; sys->na = na; sys->nb = nb; sys->n = ( na > nb ) ? na : nb; sys->sysUpdate = &qLTISys_DiscreteUpdate; /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ sys->xd = (float*)x; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ sys->xc = NULL; } else { /*continuos system*/ sys->b = &num[ 1 ]; - sys->n = na - 1u; + sys->n = na - 1U; sys->nb = sys->n; sys->sysUpdate = &qLTISys_ContinuosUpdate; /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ sys->xc = (qNumA_state_t*)x; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ sys->xd = NULL; } + sys->dt = dt; sys->integrate = &qNumA_IntegralTr; /*default integration method*/ sys->a = &den[ 1 ]; - sys->dt = dt; /*normalize the transfer function coefficients*/ a0 = den[ 0 ]; - for ( i = 0 ; i < sys->nb ; ++i ) { + for ( i = 0U ; i < sys->nb ; ++i ) { num[ i ] /= a0; } - for ( i = 0 ; i < sys->na ; ++i ) { + for ( i = 0U ; i < sys->na ; ++i ) { den[ i ] /= a0; } sys->b0 = num[ 0 ]; sys->tDelay.head = NULL; + (void)qLTISys_SetInitStates( sys, NULL ); retValue = qLTISys_SetSaturation( sys, -FLT_MAX, FLT_MAX ); } @@ -185,18 +215,18 @@ float qLTISys_DiscreteFIRUpdate( float *w, const float x ) { size_t i; - float y = 0.0f; + float y = 0.0F; if ( NULL != c ) { - for ( i = ( wsize - 1u ) ; i >= 1u ; --i ) { - w[ i ] = w[ i - 1u ]; + for ( i = ( wsize - 1U ) ; i >= 1U ; --i ) { + w[ i ] = w[ i - 1U ]; y += w[ i ]*c[ i ]; } y += c[ 0 ]*x; } else { - for ( i = ( wsize - 1u ) ; i >= 1u ; --i ) { - w[ i ] = w[ i - 1u ]; + for ( i = ( wsize - 1U ) ; i >= 1U ; --i ) { + w[ i ] = w[ i - 1U ]; y += w[ i ]; } y += x; diff --git a/qnuma.c b/qnuma.c index 62e1fec..25ab4b9 100644 --- a/qnuma.c +++ b/qnuma.c @@ -1,11 +1,12 @@ /*! * @file qnuma.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qnuma.h" +/*cppcheck-suppress misra-c2012-20.7 */ #define qNumA_Update( x ) (x)->x[ 2 ] = (x)->x[ 1 ]; \ (x)->x[ 1 ] = s \ @@ -22,41 +23,84 @@ void qNumA_StateInit( qNumA_state_t *x, /*============================================================================*/ float qNumA_IntegralRe( qNumA_state_t *x, const float s, - const float dt ) + const float dt, + const bool bUpdate ) { x->x[ 0 ] += s*dt; - qNumA_Update( x ); + if ( bUpdate ) { + qNumA_Update( x ); + } return x->x[ 0 ]; } /*============================================================================*/ float qNumA_IntegralTr( qNumA_state_t *x, const float s, - const float dt ) + const float dt, + const bool bUpdate ) { - x->x[ 0 ] += 0.5f*( s + x->x[ 1 ] )*dt; - qNumA_Update( x ); + x->x[ 0 ] += 0.5F*( s + x->x[ 1 ] )*dt; + if ( bUpdate ) { + qNumA_Update( x ); + } return x->x[ 0 ]; } /*============================================================================*/ float qNumA_IntegralSi( qNumA_state_t *x, const float s, - const float dt ) + const float dt, + const bool bUpdate ) { - x->x[ 0 ] += ( 1.0f/6.0f )*( s + ( 4.0f*x->x[ 1 ] ) + x->x[ 2 ] )*dt; - qNumA_Update( x ); + x->x[ 0 ] += ( 1.0F/6.0F )*( s + ( 4.0F*x->x[ 1 ] ) + x->x[ 2 ] )*dt; + if ( bUpdate ) { + qNumA_Update( x ); + } return x->x[ 0 ]; } /*============================================================================*/ -float qNumA_Derivative( qNumA_state_t *x, - const float s, - const float dt ) +float qNumA_Derivative2p( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ) { - x->x[ 0 ]= ( s - x->x[ 1 ] )/dt; - qNumA_Update( x ); + float ds; - return x->x[ 0 ]; + ds = ( s - x->x[ 1 ] )/dt; + if ( bUpdate ) { + qNumA_Update( x ); + } + + return ds; +} +/*============================================================================*/ +float qNumA_DerivativeBa( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ) +{ + float ds; + + ds = ( ( 3.0F*s ) - ( 4.0F*x->x[ 1 ] ) + x->x[ 2 ] )/( 2.0F*dt ); + if ( bUpdate ) { + qNumA_Update( x ); + } + + return ds; } -/*============================================================================*/ \ No newline at end of file +/*============================================================================*/ +float qNumA_DerivativeFo( qNumA_state_t *x, + const float s, + const float dt, + const bool bUpdate ) +{ + float ds; + + ds = ( ( 4.0F*x->x[ 1 ] ) - ( 3.0F*x->x[ 2 ] ) - s )/( 2.0F*dt ); + if ( bUpdate ) { + qNumA_Update( x ); + } + + return ds; +} \ No newline at end of file diff --git a/qpid.c b/qpid.c index 800ba52..40ac3d5 100644 --- a/qpid.c +++ b/qpid.c @@ -1,19 +1,26 @@ /*! * @file qpid.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qpid.h" +#include "qffmath.h" +#include static float qPID_Sat( float x, const float min, const float max ); -static void qPID_AdaptGains( qPID_controller_t * const c, - const float u, - const float y ); +static bool qPID_AutoTuningStep( qPID_controller_t *c, + const float u, + const float y ); +static void qPID_AutoTunningAdaptGains( qPID_controller_t *c, + const float u, + const float y ); static int qPID_ATCheck( const float x ); +static qPID_Gains_t qPID_AutoTuningGetEstimates( qPID_controller_t *c ); + /*============================================================================*/ int qPID_Setup( qPID_controller_t * const c, const float kc, @@ -23,20 +30,55 @@ int qPID_Setup( qPID_controller_t * const c, { int retValue = 0; - if ( ( NULL != c ) && ( dt > 0.0f ) ) { + if ( ( NULL != c ) && ( dt > 0.0F ) ) { c->dt = dt; - c->init = 1u; + c->init = 1U; c->adapt = NULL; c->integrate = &qNumA_IntegralTr; /*default integration method*/ - (void)qPID_SetDerivativeFilter( c, 0.98f ); + (void)qPID_SetDerivativeFilter( c, 0.98F ); (void)qPID_SetEpsilon( c, FLT_MIN ); (void)qPID_SetGains( c, kc, ki, kd ); - (void)qPID_SetSaturation( c , 0.0f, 100.0f, 1.0f ); - (void)qPID_SetTrackingMode( c, NULL, 1.0f ); - (void)qPID_SetMRAC( c, NULL, 0.5f ); + (void)qPID_SetSaturation( c , 0.0F, 100.0F ); + (void)qPID_SetMRAC( c, NULL, 0.5F ); + (void)qPID_SetMode( c, qPID_Automatic ); + (void)qPID_SetManualInput( c, 0.0F ); + (void)qPID_SetExtraGains( c, 1.0F, 1.0F ); + (void)qPID_SetDirection( c, qPID_Forward ); + (void)qPID_SetReferenceWeighting( c, 1.0F, 0.0F ); retValue = qPID_Reset( c ); } + return retValue; + +} +/*============================================================================*/ +int qPID_SetDirection( qPID_controller_t * const c, + const qPID_Direction_t d ) +{ + int retValue = 0; + + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->dir = d; + retValue = 1; + } + + return retValue; +} +/*============================================================================*/ +int qPID_SetParams( qPID_controller_t * const c, + const float kc, + const float ti, + const float td ) +{ + int retValue = 0; + + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->kc = kc; + c->ki = kc/ti; + c->kd = kc*td; + retValue = 1; + } + return retValue; } /*============================================================================*/ @@ -47,13 +89,26 @@ int qPID_SetGains( qPID_controller_t * const c, { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init ) ) { + if ( ( NULL != c ) && ( 0U != c->init ) ) { c->kc = kc; c->ki = ki; c->kd = kd; - if ( NULL != c->adapt ) { - /*to be defined*/ - } + retValue = 1; + } + + return retValue; +} + +/*============================================================================*/ +int qPID_SetExtraGains( qPID_controller_t * const c, + const float kw, + const float kt ) +{ + int retValue = 0; + + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->kw = kw; + c->kt = kt; retValue = 1; } @@ -64,10 +119,14 @@ int qPID_Reset( qPID_controller_t * const c ) { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init ) ) { - qNumA_StateInit( &c->c_state, 0.0f, 0.0f, 0.0f ); - c->D = 0.0f; - c->u1 = 0.0f; + if ( ( NULL != c ) && ( 0U != c->init ) ) { + qNumA_StateInit( &c->c_state, 0.0F, 0.0F, 0.0F ); + qNumA_StateInit( &c->m_state, 0.0F, 0.0F, 0.0F ); + qNumA_StateInit( &c->b_state, 0.0F, 0.0F, 0.0F ); + c->D = 0.0F; + c->u1 = 0.0F; + c->m = 0.0F; + c->uSat = 0.0F; retValue = 1; } @@ -76,15 +135,13 @@ int qPID_Reset( qPID_controller_t * const c ) /*============================================================================*/ int qPID_SetSaturation( qPID_controller_t * const c, const float min, - const float max, - const float kw ) + const float max ) { int retValue = 0; - if ( ( NULL != c ) && ( max > min ) && ( 0u != c->init ) && ( kw >= 0.0f ) ) { + if ( ( NULL != c ) && ( max > min ) && ( 0U != c->init ) ) { c->min = min; c->max = max; - c->kw = kw; retValue = 1; } @@ -95,12 +152,12 @@ int qPID_SetSeries( qPID_controller_t * const c ) { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init ) ) { + if ( ( NULL != c ) && ( 0U != c->init ) ) { float ti, td, tmp; ti = c->kc/c->ki; td = c->kd/c->kc; - tmp = 1.0f + ( td/ti ); + tmp = 1.0F + ( td/ti ); c->kc = c->kc*tmp; c->ki = c->kc/( ti*tmp ); c->kd = c->kc*( td/tmp ); @@ -115,7 +172,7 @@ int qPID_SetEpsilon( qPID_controller_t * const c, { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init) && ( eps > 0.0f ) ) { + if ( ( NULL != c ) && ( 0U != c->init) && ( eps > 0.0F ) ) { c->epsilon = eps; retValue = 1; } @@ -128,7 +185,7 @@ int qPID_SetDerivativeFilter( qPID_controller_t * const c, { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init ) && ( beta > 0.0f ) && ( beta < 1.0f ) ) { + if ( ( NULL != c ) && ( 0U != c->init ) && ( beta > 0.0F ) && ( beta < 1.0F ) ) { c->beta = beta; retValue = 1; } @@ -136,15 +193,41 @@ int qPID_SetDerivativeFilter( qPID_controller_t * const c, return retValue; } /*============================================================================*/ -int qPID_SetTrackingMode( qPID_controller_t * const c, - float *var, - const float kt ) +int qPID_SetMode( qPID_controller_t * const c, + const qPID_Mode_t m ) { int retValue = 0; - if ( ( NULL != c ) && ( kt > 0.0f ) && ( 0u != c->init ) ) { - c->kt = kt; - c->uEF = var; + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->mode = m; + retValue = 1; + } + + return retValue; +} +/*============================================================================*/ +int qPID_SetReferenceWeighting( qPID_controller_t * const c, + const float gb, + const float gc ) +{ + int retValue = 0; + + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->b = qPID_Sat( gb, 0.0F, 1.0F ); + c->c = qPID_Sat( gc, 0.0F, 1.0F ); + retValue = 1; + } + + return retValue; +} +/*============================================================================*/ +int qPID_SetManualInput( qPID_controller_t * const c, + const float manualInput ) +{ + int retValue = 0; + + if ( ( NULL != c ) && ( 0U != c->init ) ) { + c->mInput = manualInput; retValue = 1; } @@ -157,9 +240,9 @@ int qPID_SetMRAC( qPID_controller_t * const c, { int retValue = 0; - if ( ( NULL != c ) && ( gamma > 0.0f ) ) { - qNumA_StateInit( &c->m_state, 0.0f, 0.0f, 0.0f ); - c->alfa = 0.01f; + if ( ( NULL != c ) && ( gamma > 0.0F ) ) { + qNumA_StateInit( &c->m_state, 0.0F, 0.0F, 0.0F ); + c->alfa = 0.01F; c->gamma = gamma; c->yr = modelRef; retValue = 1; @@ -168,79 +251,100 @@ int qPID_SetMRAC( qPID_controller_t * const c, return retValue; } /*============================================================================*/ +static float qPID_Error( qPID_controller_t * const c, float w, float y, float k ) +{ + float e = ( k*w ) - y; + + if ( QLIB_ABS( e ) <= c->epsilon ) { + e = 0.0F; + } + + return e; +} +/*============================================================================*/ float qPID_Control( qPID_controller_t * const c, const float w, const float y ) { float u = w; - if ( ( NULL != c ) && ( 0u != c->init ) ) { - float e, v, de, ie; + if ( ( NULL != c ) && ( 0U != c->init ) ) { + float e, v, de, ie, bt, sw, kc, ki, kd; + + kc = c->kc; + ki = c->ki; + kd = c->kd; + if ( qPID_Backward == c->dir ) { + kc = ( kc > 0.0F ) ? -kc : kc; + ki = ( ki > 0.0F ) ? -ki : ki; + kd = ( kd > 0.0F ) ? -kd : kd; + } - e = w - y; - if ( fabs( e ) <= ( c->epsilon ) ) { - e = 0.0f; + e = qPID_Error( c, w, y, 1.0F ); + if ( QLIB_ABS( e ) <= c->epsilon ) { + e = 0.0F; } /*integral with anti-windup*/ - ie = c->integrate( &c->c_state, e + c->u1, c->dt ); - de = qNumA_Derivative( &c->c_state, e, c->dt ); + de = qNumA_Derivative2p( &c->c_state, qPID_Error( c, w, y, c->c ), c->dt, false ); + ie = c->integrate( &c->c_state, e + c->u1, c->dt, true ); c->D = de + ( c->beta*( c->D - de ) ); /*derivative filtering*/ - v = ( c->kc*e ) + ( c->ki*ie ) + ( c->kd*c->D ); /*compute PID action*/ + v = ( kc*qPID_Error( c, w, y, c->b ) ) + ( ki*ie ) + ( kd*c->D ); /*compute PID action*/ if ( NULL != c->yr ) { /*MRAC additive controller using the modified MIT rule*/ - float theta = 0.0f; - if ( ( c->u1*c->u1 ) <= c->epsilon ) { /*additive anti-windup*/ + float theta = 0.0F; + if ( QLIB_ABS( c->u1 ) <= c->epsilon ) { /*additive anti-windup*/ const float em = y - c->yr[ 0 ]; const float delta = -c->gamma*em*c->yr[ 0 ]/ ( c->alfa + ( c->yr[ 0 ]*c->yr[ 0 ] ) ); - theta = c->integrate( &c->m_state, delta, c->dt ); + theta = c->integrate( &c->m_state, delta /*+ c->u1*/, c->dt, true ); } v += w*theta; } - - u = qPID_Sat( v, c->min, c->max ); - c->u1 = c->kw*( u - v ); /*anti-windup feedback*/ - if ( NULL != c->uEF ) { /*tracking mode*/ - c->u1 += c->kt*( c->uEF[ 0 ] - u ); - } - + /*bumpless-transfer*/ + bt = ( c->kt*c->mInput ) + ( c->kw*( c->uSat - c->m ) ); + c->m = c->integrate( &c->b_state, bt ,c->dt, true ); + sw = ( qPID_Automatic == c->mode ) ? v : c->m; + c->uSat = qPID_Sat( sw, c->min, c->max ); + u = c->uSat; + /*anti-windup feedback*/ + c->u1 = c->kw*( u - v ); if ( NULL != c->adapt ) { - qPID_AdaptGains( c, u, y ); + qPID_AutoTunningAdaptGains( c, u, y ); } } return u; } /*============================================================================*/ -int qPID_BindAutoTunning( qPID_controller_t * const c, - qPID_AutoTunning_t * const at ) +int qPID_BindAutoTuning( qPID_controller_t * const c, + qPID_AutoTuning_t * const at ) { int retValue = 0; - if ( ( NULL != c ) && ( 0u != c->init ) ) { + if ( ( NULL != c ) && ( 0U != c->init ) ) { if ( NULL != at ) { float k,T; c->adapt = at; - at->l = 0.9898f; - at->il = 1.0f/at->l; - at->p00 = 1000.0f; - at->p11 = 1000.0f; - at->p01 = 0.0f; - at->p10 = 0.0f; - at->uk = 0.0f; - at->yk = 0.0f; - at->k = 0.0f; - at->tao = 0.0f; - at->it = 100uL; - at->mu = 0.95f; - k = c->kc/0.9f; - T = ( 0.27f*k )/c->ki; - at->a1 = -expf( -c->dt/T ); - at->b1 = k*( 1.0f + at->a1 ); - at->speed = 0.25f; - at->it = QPID_AUTOTUNNING_UNDEFINED; + at->l = 0.9898F; + at->il = 1.0F/at->l; + at->p00 = 1000.0F; + at->p11 = 1000.0F; + at->p01 = 0.0F; + at->p10 = 0.0F; + at->uk = 0.0F; + at->yk = 0.0F; + at->k = 0.0F; + at->tao = 0.0F; + at->it = 100UL; + at->mu = 0.95F; + k = c->kc/0.9F; + T = ( 0.27F*k )/c->ki; + at->a1 = -QLIB_EXP( -c->dt/T ); + at->b1 = k*( 1.0F + at->a1 ); + at->speed = 0.25F; + at->it = QPID_AUTOTUNING_UNDEFINED; } else { c->adapt = NULL; @@ -251,14 +355,14 @@ int qPID_BindAutoTunning( qPID_controller_t * const c, return retValue; } /*============================================================================*/ -int qPID_EnableAutoTunning( qPID_controller_t * const c, - const uint32_t tEnable ) +int qPID_EnableAutoTuning( qPID_controller_t * const c, + const uint32_t tEnable ) { int retValue = 0; if ( NULL != c ) { if ( NULL != c->adapt ) { - c->adapt->it = tEnable; + c->adapt->it = ( 0UL == tEnable ) ? QPID_AUTOTUNING_UNDEFINED : tEnable; retValue = 1; } } @@ -266,11 +370,12 @@ int qPID_EnableAutoTunning( qPID_controller_t * const c, return retValue; } /*============================================================================*/ -static void qPID_AdaptGains( qPID_controller_t *c, - const float u, - const float y ) +static bool qPID_AutoTuningStep( qPID_controller_t *c, + const float u, + const float y ) { - qPID_AutoTunning_t *s = c->adapt; + qPID_AutoTuning_t *s = c->adapt; + bool ready = false; float error , r, l0, l1; float lp00, lp01, lp10, lp11; float k, tao, tmp1, tmp2; @@ -290,58 +395,112 @@ static void qPID_AdaptGains( qPID_controller_t *c, lp01 = s->il*s->p01; lp10 = s->il*s->p10; lp11 = s->il*s->p11; - tmp1 = ( l0*s->uk ) - 1.0f; - tmp2 = ( l1*s->yk ) + 1.0f; - s->p00 = ( l0*lp10*s->yk ) - ( lp00*tmp1 ) + 1e-10f; + tmp1 = ( l0*s->uk ) - 1.0F; + tmp2 = ( l1*s->yk ) + 1.0F; + s->p00 = ( l0*lp10*s->yk ) - ( lp00*tmp1 ) + 1e-10F; s->p01 = ( l0*lp11*s->yk ) - ( lp01*tmp1 ); s->p10 = ( lp10*tmp2 ) - ( l1*lp00*s->uk ); - s->p11 = ( lp11*tmp2 ) - ( l1*lp01*s->uk ) + 1e-10f; + s->p11 = ( lp11*tmp2 ) - ( l1*lp01*s->uk ) + 1e-10F; /*update I/O measurements*/ s->yk = y; s->uk = u; - k = s->b1/( 1.0f + s->a1 ); - tmp1 = ( s->a1 < 0.0f ) ? -s->a1 : s->a1; + k = s->b1/( 1.0F + s->a1 ); + tmp1 = ( s->a1 < 0.0F ) ? -s->a1 : s->a1; /*cstat -MISRAC2012-Dir-4.11_a -MISRAC2012-Rule-13.5*/ - tao = -c->dt/logf( tmp1 ); /*ok, passing absolute value*/ - if ( ( 0 != qPID_ATCheck( tao ) ) && ( 0 != qPID_ATCheck( k ) ) && ( s->it > 0uL ) ) { + tao = -c->dt/QLIB_LOG( tmp1 ); /*ok, passing absolute value*/ + if ( ( 0 != qPID_ATCheck( tao ) ) && ( 0 != qPID_ATCheck( k ) ) && ( s->it > 0UL ) ) { /*cstat +MISRAC2012-Dir-4.11_a +MISRAC2012-Rule-13.5*/ s->k = k + ( s->mu*( s->k - k ) ); s->tao = tao + ( s->mu*( s->tao - tao ) ); - if ( ( 0uL == --s->it ) && ( s->it != QPID_AUTOTUNNING_UNDEFINED ) ) { - tmp1 = c->dt/s->tao; - tmp2 = ( 1.35f + ( 0.25f*tmp1 ) ); - c->kc = ( s->speed*tmp2*s->tao )/( s->k*c->dt ); - c->ki = ( ( s->speed*c->kc )*( 0.54f + ( 0.33f*tmp1 ) ) )/( tmp2*c->dt ); - c->kd = ( 0.5f*s->speed*c->kc*c->dt )/tmp2; + if ( ( 0UL == --s->it ) && ( s->it != QPID_AUTOTUNING_UNDEFINED ) ) { + ready = true; } } + + return ready; +} +/*============================================================================*/ +static qPID_Gains_t qPID_AutoTuningGetEstimates( qPID_controller_t *c ) +{ + qPID_Gains_t gains = { 0.0F, 0.0F, 0.0F }; + qPID_AutoTuning_t *s = c->adapt; + const float td = s->tao*0.1F; + + switch ( c->type ) { + case qPID_TYPE_P: + gains.Kc = s->speed*( 1.03F/s->k )*( ( s->tao/td ) + 0.34F ); + break; + case qPID_TYPE_PD: + gains.Kc = s->speed*( 1.24F/s->k )*( ( s->tao/td ) + 0.129F ); + gains.Kd = gains.Kc*( 0.27F*td )*( s->tao - ( 0.324F*td ) )/( s->tao + ( 0.129F*td ) ); + break; + case qPID_TYPE_PI: + gains.Kc = s->speed*( 0.9F/s->k )*( ( s->tao/td ) + 0.092F ); + gains.Ki = gains.Kc*( s->tao + ( 2.22F*td ) )/( 3.33F*td*( s->tao + ( 0.092F*td ) ) ); + break; + case qPID_TYPE_PID: + gains.Kc = s->speed*( 1.35F/s->k )*( ( s->tao/td ) + 0.185F ); + gains.Ki = gains.Kc*( s->tao + ( 0.611F*td ) )/( 2.5F*td*( s->tao + ( 0.185F*td ) ) ); + gains.Kd = ( 0.37F*gains.Kc*td*s->tao )/( s->tao + ( 0.185F*td ) ); + break; + default: + break; + } + + return gains; +} +/*============================================================================*/ +static void qPID_AutoTunningAdaptGains( qPID_controller_t *c, + const float u, + const float y ) +{ + if ( qPID_AutoTuningStep( c, u, y ) ) { + qPID_Gains_t newGains = qPID_AutoTuningGetEstimates( c ); + c->kc = newGains.Kc; + c->ki = newGains.Ki; + c->kd = newGains.Kd; + } +} +/*============================================================================*/ +bool qPID_AutoTunningControllerType( qPID_controller_t *c, + const qPID_Type_t t ) +{ + bool retVal = false; + + if ( NULL != c ) { + if ( NULL != c->adapt ) { + c->type = t; + retVal = true; + } + } + + return retVal; } /*============================================================================*/ -int qPID_AutoTunningComplete( const qPID_controller_t * const c ) +int qPID_AutoTuningComplete( const qPID_controller_t * const c ) { int retVal = 0; if ( NULL != c ) { if ( NULL != c->adapt ) { - retVal = ( ( 0uL == c->adapt->it ) && - ( c->adapt->it != QPID_AUTOTUNNING_UNDEFINED ) - ) ? 1 : 0; + /*cppcheck-suppress misra-c2012-10.6 */ + retVal = ( ( 0UL == c->adapt->it ) && ( c->adapt->it != QPID_AUTOTUNING_UNDEFINED ) ) ? 1 : 0; } } return retVal; } /*============================================================================*/ -int qPID_AutoTunningSetParameters( qPID_controller_t * const c, - const float mu, - const float alfa, - const float lambda ) +int qPID_AutoTuningSetParameters( qPID_controller_t * const c, + const float mu, + const float alfa, + const float lambda ) { int retVal = 0; - if ( ( NULL != c ) && ( mu > 0.0f ) && ( mu <= 1.0f ) && - ( alfa > 0.0f ) && ( alfa <= 1.0f ) && - ( lambda >= 0.8f ) && ( lambda <= 1.0f ) ) { + if ( ( NULL != c ) && ( mu > 0.0F ) && ( mu <= 1.0F ) && + ( alfa > 0.0F ) && ( alfa <= 1.0F ) && + ( lambda >= 0.8F ) && ( lambda <= 1.0F ) ) { if ( NULL != c->adapt ) { c->adapt->l = lambda; c->adapt->mu = mu; @@ -373,7 +532,7 @@ static float qPID_Sat( float x, static int qPID_ATCheck( const float x ) { /*cstat -MISRAC2012-Rule-13.5 -MISRAC2012-Rule-10.3*/ - return ( 0 == (int)isnan( x ) ) && ( x > 0.0f ) && ( 0 == (int)isinf( x ) ); + return ( 0 == (int)QLIB_ISNAN( x ) ) && ( x > 0.0F ) && ( 0 == (int)QLIB_ISINF( x ) ); /*cstat +MISRAC2012-Rule-13.5 +MISRAC2012-Rule-10.3*/ } /*============================================================================*/ diff --git a/qrms.c b/qrms.c index 07b6673..2d8881b 100644 --- a/qrms.c +++ b/qrms.c @@ -1,13 +1,11 @@ /*! * @file qrms.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qrms.h" - - -static float qRMS_NewtonsFastSqrt( const float x ); +#include "qffmath.h" /*============================================================================*/ int qRMS_Setup( qRMS_t * const q, @@ -16,11 +14,11 @@ int qRMS_Setup( qRMS_t * const q, { int retValue = 0; - if ( ( NULL != q ) && ( NULL != window ) && ( wsize > 0u ) ) { - float defaults[ 2 ] = { 0.99f, 0.75f }; /*default parameters*/ - (void)qSSmoother_Setup( &q->f1, QSSMOOTHER_TYPE_EXPW, &defaults[0], NULL, 0uL ); + if ( ( NULL != q ) && ( NULL != window ) && ( wsize > 0U ) ) { + float defaults[ 2 ] = { 0.99F, 0.75F }; /*default parameters*/ + (void)qSSmoother_Setup( &q->f1, QSSMOOTHER_TYPE_EXPW, &defaults[ 0 ], NULL, 0UL ); (void)qSSmoother_Setup( &q->f2, QSSMOOTHER_TYPE_MWM2, NULL , window, wsize ); - (void)qSSmoother_Setup( &q->f3, QSSMOOTHER_TYPE_LPF1, &defaults[1], NULL, 0uL ); + (void)qSSmoother_Setup( &q->f3, QSSMOOTHER_TYPE_LPF1, &defaults[ 1 ], NULL, 0UL ); retValue = 1; } @@ -30,10 +28,10 @@ int qRMS_Setup( qRMS_t * const q, float qRMS_Update( qRMS_t * const q, const float x ) { - float y = 0.0f; + float y = 0.0F; if ( NULL != q ) { - y = qRMS_NewtonsFastSqrt( qSSmoother_Perform( &q->f1, x*x ) ); + y = QLIB_SQRT( qSSmoother_Perform( &q->f1, x*x ) ); y = qSSmoother_Perform( &q->f2, y ); /*2nd stage moving-window overlap*/ y = qSSmoother_Perform( &q->f3, y ); /*3rd stage low-pass filter*/ } @@ -47,7 +45,7 @@ int qRMS_SetParams( qRMS_t * const q, { int retValue = 0; - if ( ( NULL != q ) && ( lambda > 0.0f ) && ( lambda <= 1.0f ) && ( alpha > 0.0f ) && ( alpha <= 1.0f ) ) { + if ( ( NULL != q ) && ( lambda > 0.0F ) && ( lambda <= 1.0F ) && ( alpha > 0.0F ) && ( alpha <= 1.0F ) ) { q->f1.lambda = lambda; q->f3.alpha = alpha; retValue = 1; @@ -55,18 +53,4 @@ int qRMS_SetParams( qRMS_t * const q, return retValue; } -/*============================================================================*/ -static float qRMS_NewtonsFastSqrt( const float x ) -{ - uint32_t i = 0uL; - float xHalf = 0.5f*x ,y = x; - const uint32_t SQRT_2_RAISED_127 = 0x5F3759DFuL; /* sqrt(2^127) */ - - (void)memcpy( &i, &y, sizeof(uint32_t) ); /* allowed type-punning*/ - i = SQRT_2_RAISED_127 - ( i >> 1 ); /* get the best initial guess*/ - (void)memcpy( &y, &i, sizeof(uint32_t) ); /* allowed type-punning*/ - y = y * ( 1.5f - ( xHalf * y * y ) ); /* 1st iteration*/ - /* multiply the result by x to get sqrt(x)*/ - return y*x; -} /*============================================================================*/ \ No newline at end of file diff --git a/qssmoother.c b/qssmoother.c index 592357d..8d00ce9 100644 --- a/qssmoother.c +++ b/qssmoother.c @@ -1,11 +1,12 @@ /*! * @file qssmoother.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qssmoother.h" #include "qltisys.h" +#include "qffmath.h" /*! @cond */ struct qSmoother_Vtbl_s @@ -15,9 +16,9 @@ struct qSmoother_Vtbl_s }; /*! @endcond */ -static float qSSmoother_Abs( float x ); static void qSSmoother_WindowSet( float *w, - size_t wsize, float x ); + const size_t wsize, + const float x ); static int qSSmoother_Setup_LPF1( _qSSmoother_t * const f, const float * const param, float *window, @@ -54,6 +55,14 @@ static int qSSmoother_Setup_EXPW( _qSSmoother_t * const f, const float * const param, float *window, const size_t wsize ); +static int qSSmoother_Setup_DESF( _qSSmoother_t * const f, + const float * const param, + float *window, + const size_t wsize ); +static int qSSmoother_Setup_ALNF( _qSSmoother_t * const f, + const float * const param, + float *window, + const size_t wsize ); static float qSSmoother_Filter_LPF1( _qSSmoother_t * const f, const float x ); static float qSSmoother_Filter_LPF2( _qSSmoother_t * const f, @@ -72,7 +81,10 @@ static float qSSmoother_Filter_KLMN( _qSSmoother_t * const f, const float x ); static float qSSmoother_Filter_EXPW( _qSSmoother_t * const f, const float x ); - +static float qSSmoother_Filter_DESF( _qSSmoother_t * const f, + const float x ); +static float qSSmoother_Filter_ALNF( _qSSmoother_t * const f, + const float x ); /*============================================================================*/ int qSSmoother_Setup( qSSmootherPtr_t * const s, const qSSmoother_Type_t type, @@ -80,7 +92,7 @@ int qSSmoother_Setup( qSSmootherPtr_t * const s, float *window, const size_t wsize ) { - static struct qSmoother_Vtbl_s qSmoother_Vtbl[ 9 ] = { + static struct qSmoother_Vtbl_s qSmoother_Vtbl[ 11 ] = { { &qSSmoother_Filter_LPF1, &qSSmoother_Setup_LPF1 }, { &qSSmoother_Filter_LPF2, &qSSmoother_Setup_LPF2 }, { &qSSmoother_Filter_MWM1, &qSSmoother_Setup_MWM1 }, @@ -90,12 +102,15 @@ int qSSmoother_Setup( qSSmootherPtr_t * const s, { &qSSmoother_Filter_GMWF, &qSSmoother_Setup_GMWF }, { &qSSmoother_Filter_KLMN, &qSSmoother_Setup_KLMN }, { &qSSmoother_Filter_EXPW, &qSSmoother_Setup_EXPW }, + { &qSSmoother_Filter_DESF, &qSSmoother_Setup_DESF }, + { &qSSmoother_Filter_ALNF, &qSSmoother_Setup_ALNF }, }; int retValue = 0; const size_t maxTypes = sizeof(qSmoother_Vtbl)/sizeof(qSmoother_Vtbl[ 0 ] ); if ( ( s != NULL ) && ( (size_t)type < maxTypes ) ) { /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ _qSSmoother_t * const self = (_qSSmoother_t* const)s; self->vt = &qSmoother_Vtbl[ type ]; retValue = qSmoother_Vtbl[ type ].setup( s, param, window, wsize ); @@ -113,8 +128,9 @@ static int qSSmoother_Setup_LPF1( _qSSmoother_t * const f, int retValue = 0; float alpha = param[ 0 ]; - if ( ( alpha > 0.0f ) && ( alpha < 1.0f ) ) { + if ( ( alpha > 0.0F ) && ( alpha < 1.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_LPF1_t * const s = (qSSmoother_LPF1_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->alpha = alpha; @@ -134,20 +150,23 @@ static int qSSmoother_Setup_LPF2( _qSSmoother_t * const f, int retValue = 0; float alpha = param[ 0 ]; - if ( ( alpha > 0.0f ) && ( alpha < 1.0f ) ) { + if ( ( alpha > 0.0F ) && ( alpha < 1.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_LPF2_t * const s = (qSSmoother_LPF2_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ - float aa, p1, r; + float aa; + float p1; + float r; aa = alpha*alpha; /*cstat -MISRAC2012-Dir-4.11_b*/ - p1 = sqrtf( 2.0f*alpha ); /*arg always positive*/ + p1 = QLIB_SQRT( 2.0F*alpha ); /*arg always positive*/ /*cstat +MISRAC2012-Dir-4.11_b*/ - r = 1.0f + p1 + aa; + r = 1.0F + p1 + aa; s->k = aa/r; - s->a1 = 2.0f*( aa - 1.0f )/r; - s->a2 = ( 1.0f - p1 + aa )/r; - s->b1 = 2.0f*s->k; + s->a1 = 2.0F*( aa - 1.0F )/r; + s->a2 = ( 1.0F - p1 + aa )/r; + s->b1 = 2.0F*s->k; retValue = qSSmoother_Reset( s ); (void)window; (void)wsize; @@ -163,8 +182,9 @@ static int qSSmoother_Setup_MWM1( _qSSmoother_t * const f, { int retValue = 0; - if ( ( NULL != window ) && ( wsize > 0u ) ) { + if ( ( NULL != window ) && ( wsize > 0U ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MWM1_t * const s = (qSSmoother_MWM1_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->w = window; @@ -183,12 +203,13 @@ static int qSSmoother_Setup_MWM2( _qSSmoother_t * const f, { int retValue = 0; - if ( ( NULL != window ) && ( wsize > 0u ) ) { + if ( ( NULL != window ) && ( wsize > 0U ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MWM2_t * const s = (qSSmoother_MWM2_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ - qTDL_Setup( &s->tdl, window, wsize, 0.0f ); - s->sum = 0.0f; + qTDL_Setup( &s->tdl, window, wsize, 0.0F ); + s->sum = 0.0F; retValue = qSSmoother_Reset( s ); (void)param; } @@ -204,8 +225,9 @@ static int qSSmoother_Setup_MOR1( _qSSmoother_t * const f, int retValue = 0; float a = param[ 0 ]; - if ( ( NULL != window ) && ( wsize > 0u ) && ( a > 0.0f ) && ( a < 1.0f ) ) { + if ( ( NULL != window ) && ( wsize > 0U ) && ( a > 0.0F ) && ( a < 1.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MOR1_t * const s = (qSSmoother_MOR1_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->w = window; @@ -225,14 +247,15 @@ static int qSSmoother_Setup_MOR2( _qSSmoother_t * const f, int retValue = 0; float q = param[ 0 ]; - if ( ( NULL != window ) && ( wsize > 0u ) && ( q > 0.0f ) && ( q < 1.0f ) ) { + if ( ( NULL != window ) && ( wsize > 0U ) && ( q > 0.0F ) && ( q < 1.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MOR2_t * const s = (qSSmoother_MOR2_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->alpha = q; - qTDL_Setup( &s->tdl, window, wsize, 0.0f ); - s->sum = 0.0f; - s->m = 0.0f; + qTDL_Setup( &s->tdl, window, wsize, 0.0F ); + s->sum = 0.0F; + s->m = 0.0F; retValue = qSSmoother_Reset( s ); } @@ -249,28 +272,32 @@ static int qSSmoother_Setup_GMWF( _qSSmoother_t * const f, /*cstat -CERT-FLP34-C*/ const size_t c = (size_t)param[ 1 ]; /*cstat +CERT-FLP34-C*/ - const size_t ws = wsize/2u; + const size_t ws = wsize/2U; - if ( ( NULL != window ) && ( wsize > 0u ) && ( c < ws ) && ( sg > 0.0f ) ) { + if ( ( NULL != window ) && ( wsize > 0U ) && ( c < ws ) && ( sg > 0.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_GMWF_t * const s = (qSSmoother_GMWF_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ float *kernel = &window[ ws ]; - float r, sum = 0.0f; + float r; + float sum = 0.0F; size_t i; - float l, center; + float l; + float center; /*cstat -CERT-FLP36-C -MISRAC2012-Rule-10.8*/ - l = (float)( wsize - 1u )/2.0f; + /*cppcheck-suppress misra-c2012-10.8 */ + l = (float)( wsize - 1U )/2.0F; center = (float)c - l; - r = 2.0f*sg*sg; - for ( i = 0u ; i < ws ; ++i ) { + r = 2.0F*sg*sg; + for ( i = 0U ; i < ws ; ++i ) { float d = (float)i - l; /*symmetry*/ d -= center; - kernel[ i ] = expf( -( d*d )/r ); + kernel[ i ] = QLIB_EXP( -( d*d )/r ); sum += kernel[ i ]; } /*cstat +CERT-FLP36-C +MISRAC2012-Rule-10.8*/ - for ( i = 0u ; i < ws ; ++i ) { + for ( i = 0U ; i < ws ; ++i ) { kernel[ i ] /= sum; } s->w = window; @@ -292,15 +319,16 @@ static int qSSmoother_Setup_KLMN( _qSSmoother_t * const f, float q = param[ 1 ]; float r = param[ 2 ]; - if ( ( p > 0.0f ) && ( q > 0.0f ) && ( r > 0.0f ) ) { + if ( ( p > 0.0F ) && ( q > 0.0F ) && ( r > 0.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_KLMN_t * const s = (qSSmoother_KLMN_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->p = p; s->q = q; s->r = r; - s->A = 1.0f; - s->H = 1.0f; + s->A = 1.0F; + s->H = 1.0F; retValue = qSSmoother_Reset( s ); (void)window; (void)wsize; @@ -317,13 +345,14 @@ static int qSSmoother_Setup_EXPW( _qSSmoother_t * const f, int retValue = 0; float lambda = param[ 0 ]; - if ( ( lambda > 0.0f ) && ( lambda < 1.0f ) ) { + if ( ( lambda > 0.0F ) && ( lambda < 1.0F ) ) { /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_EXPW_t * const s = (qSSmoother_EXPW_t* const)f; /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ s->lambda = lambda; - s->m = 0.0f; - s->w = 1.0f; + s->m = 0.0F; + s->w = 1.0F; retValue = qSSmoother_Reset( s ); (void)window; (void)wsize; @@ -332,9 +361,56 @@ static int qSSmoother_Setup_EXPW( _qSSmoother_t * const f, return retValue; } /*============================================================================*/ -static float qSSmoother_Abs( const float x ) +static int qSSmoother_Setup_DESF( _qSSmoother_t * const f, + const float * const param, + float *window, + const size_t wsize ) { - return ( x < 0.0f ) ? -x : x; + int retValue = 0; + float a = param[ 0 ]; + float b = param[ 1 ]; + float n = param[ 2 ]; + + if ( ( n >= 0.0F ) && ( a > 0.0F ) && ( a < 1.0F ) && ( b > 0.0F ) && ( b < 1.0F ) ) { + /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ + qSSmoother_DESF_t * const s = (qSSmoother_DESF_t* const)f; + /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ + s->alpha = a; + s->beta = b; + s->n = QLIB_ROUND( n ); + retValue = qSSmoother_Reset( s ); + (void)window; + (void)wsize; + } + + return retValue; +} +/*============================================================================*/ +static int qSSmoother_Setup_ALNF( _qSSmoother_t * const f, + const float * const param, + float *window, + const size_t wsize ) +{ + int retValue = 0; + float a = param[ 0 ]; + float m = param[ 1 ]; + + if ( ( NULL != window ) && ( wsize > 0U ) && ( a > 0.0F ) && ( a < 1.0F ) && ( m > 0.0F ) && ( m < 1.0F ) ) { + /*cstat -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-EXP36-C_a*/ + /*cppcheck-suppress misra-c2012-11.3 */ + qSSmoother_ALNF_t * const s = (qSSmoother_ALNF_t* const)f; + /*cstat +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-EXP36-C_a*/ + s->alpha = a; + s->mu = m; + s->x = window; + s->w = &window[ wsize ]; + s->w_1 = ( s->mu > 0.0F ) ? &window[ 2U*wsize ] : NULL; + s->n = wsize; + retValue = qSSmoother_Reset( s ); + } + + return retValue; } /*============================================================================*/ static void qSSmoother_WindowSet( float *w, @@ -343,7 +419,7 @@ static void qSSmoother_WindowSet( float *w, { size_t i; - for ( i = 0 ; i < wsize ; ++i ) { + for ( i = 0U ; i < wsize ; ++i ) { w[ i ] = x; } } @@ -354,9 +430,11 @@ int qSSmoother_IsInitialized( const qSSmootherPtr_t * const s ) if ( NULL != s ) { /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ const _qSSmoother_t * const f = (const _qSSmoother_t* const)s; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ - retValue = (int)( NULL != f->vt ); + /*cppcheck-suppress misra-c2012-10.6 */ + retValue = ( NULL != f->vt ) ? 1 : 0; } return retValue; @@ -368,9 +446,10 @@ int qSSmoother_Reset( qSSmootherPtr_t * const s ) if ( NULL != s ) { /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ _qSSmoother_t * const f = (_qSSmoother_t* const)s; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ - f->init = 1u; + f->init = 1U; retValue = 1; } @@ -384,7 +463,9 @@ float qSSmoother_Perform( qSSmootherPtr_t * const s, if ( NULL != s ) { /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ _qSSmoother_t * const f = (_qSSmoother_t* const)s; + /*cppcheck-suppress misra-c2012-11.5 */ struct qSmoother_Vtbl_s *vt = f->vt; /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ if ( NULL != vt->perform ) { @@ -395,16 +476,17 @@ float qSSmoother_Perform( qSSmootherPtr_t * const s, return retValue; } /*============================================================================*/ -static float qSSmoother_Filter_LPF1( _qSSmoother_t *f, +static float qSSmoother_Filter_LPF1( _qSSmoother_t * const f, const float x ) { float y; /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_LPF1_t * const s = (qSSmoother_LPF1_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { s->y1 = x; - f->init = 0u; + f->init = 0U; } y = x + ( s->alpha*( s->y1 - x ) ); s->y1 = y; @@ -412,19 +494,20 @@ static float qSSmoother_Filter_LPF1( _qSSmoother_t *f, return y; } /*============================================================================*/ -static float qSSmoother_Filter_LPF2( _qSSmoother_t *f, +static float qSSmoother_Filter_LPF2( _qSSmoother_t * const f, const float x ) { float y; /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_LPF2_t * const s = (qSSmoother_LPF2_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { s->y1 = x; s->y2 = x; s->x1 = x; s->x2 = x; - f->init = 0u; + f->init = 0U; } y = ( s->k*x ) + ( s->b1*s->x1 ) + ( s->k*s->x2 ) - ( s->a1*s->y1 ) - ( s->a2*s->y2 ); @@ -436,32 +519,34 @@ static float qSSmoother_Filter_LPF2( _qSSmoother_t *f, return y; } /*============================================================================*/ -static float qSSmoother_Filter_MWM1( _qSSmoother_t *f, +static float qSSmoother_Filter_MWM1( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MWM1_t * const s = (qSSmoother_MWM1_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { qSSmoother_WindowSet( s->w, s->wsize, x ); - f->init = 0u; + f->init = 0U; } /*cstat -CERT-FLP36-C*/ return qLTISys_DiscreteFIRUpdate( s->w, NULL, s->wsize, x )/(float)s->wsize; /*cstat +CERT-FLP36-C*/ } /*============================================================================*/ -static float qSSmoother_Filter_MWM2( _qSSmoother_t *f, +static float qSSmoother_Filter_MWM2( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-FLP36-C*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MWM2_t * const s = (qSSmoother_MWM2_t* const)f; float wsize = (float)s->tdl.itemCount; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-FLP36-C*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { qTDL_Flush( &s->tdl, x ); s->sum = x*wsize; - f->init = 0u; + f->init = 0U; } s->sum += x - qTDL_GetOldest( &s->tdl ); qTDL_InsertSample( &s->tdl, x ); @@ -469,22 +554,23 @@ static float qSSmoother_Filter_MWM2( _qSSmoother_t *f, return s->sum/wsize; } /*============================================================================*/ -static float qSSmoother_Filter_MOR1( _qSSmoother_t *f, +static float qSSmoother_Filter_MOR1( _qSSmoother_t * const f, const float x ) { float m; /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MOR1_t * const s = (qSSmoother_MOR1_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { qSSmoother_WindowSet( s->w, s->wsize, x ); s->m = x; - f->init = 0u; + f->init = 0U; } /*shift, sum and compensate*/ m = qLTISys_DiscreteFIRUpdate( s->w, NULL, s->wsize, x ) - x; /*is it an outlier?*/ - if ( qSSmoother_Abs( s->m - x ) > ( s->alpha*qSSmoother_Abs( s->m ) ) ) { + if ( QLIB_ABS( s->m - x ) > ( s->alpha*QLIB_ABS( s->m ) ) ) { s->w[ 0 ] = s->m; /*replace the outlier with the dynamic median*/ } /*cstat -CERT-FLP36-C*/ @@ -494,22 +580,23 @@ static float qSSmoother_Filter_MOR1( _qSSmoother_t *f, return s->w[ 0 ]; } /*============================================================================*/ -static float qSSmoother_Filter_MOR2( _qSSmoother_t *f, +static float qSSmoother_Filter_MOR2( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d -CERT-FLP36-C*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_MOR2_t * const s = (qSSmoother_MOR2_t* const)f; float wsize = (float)s->tdl.itemCount; float xx = x; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d +CERT-FLP36-C*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { qTDL_Flush( &s->tdl, x ); s->sum = wsize*x; s->m = x; - f->init = 0u; + f->init = 0U; } /*is it an outlier?*/ - if ( qSSmoother_Abs( s->m - x ) > ( s->alpha*qSSmoother_Abs( s->m ) ) ) { + if ( QLIB_ABS( s->m - x ) > ( s->alpha*QLIB_ABS( s->m ) ) ) { xx = s->m; /*replace the outlier with the dynamic median*/ } s->sum += xx - qTDL_GetOldest( &s->tdl ); @@ -519,31 +606,33 @@ static float qSSmoother_Filter_MOR2( _qSSmoother_t *f, return x; } /*============================================================================*/ -static float qSSmoother_Filter_GMWF( _qSSmoother_t *f, +static float qSSmoother_Filter_GMWF( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_GMWF_t * const s = (qSSmoother_GMWF_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ - if ( 1u == f->init ) { + if ( 1U == f->init ) { qSSmoother_WindowSet( s->w, s->wsize, x ); - f->init = 0u; + f->init = 0U; } return qLTISys_DiscreteFIRUpdate( s->w, s->k, s->wsize, x ); } /*============================================================================*/ -static float qSSmoother_Filter_KLMN( _qSSmoother_t *f, +static float qSSmoother_Filter_KLMN( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_KLMN_t * const s = (qSSmoother_KLMN_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ float pH; - if ( 1u == f->init ) { + if ( 1U == f->init ) { s->x = x; - f->init = 0u; + f->init = 0U; } /* Predict */ s->x = s->A*s->x; @@ -552,28 +641,91 @@ static float qSSmoother_Filter_KLMN( _qSSmoother_t *f, pH = s->p*s->H; s->gain = pH/( s->r + ( s->H*pH ) ); s->x += s->gain*( x - ( s->H*s->x ) ); - s->p = ( 1.0f - ( s->gain*s->H ) )*s->p; /*covariance update*/ + s->p = ( 1.0F - ( s->gain*s->H ) )*s->p; /*covariance update*/ return s->x; } /*============================================================================*/ -static float qSSmoother_Filter_EXPW( _qSSmoother_t *f, +static float qSSmoother_Filter_EXPW( _qSSmoother_t * const f, const float x ) { /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ qSSmoother_EXPW_t * const s = (qSSmoother_EXPW_t* const)f; /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ float iw; - if ( 1u == f->init ) { + if ( 1U == f->init ) { s->m = x; - s->w = 1.0f; - f->init = 0u; + s->w = 1.0F; + f->init = 0U; } - s->w = ( s->lambda*s->w ) + 1.0f; - iw = 1.0f/s->w; - s->m = ( s->m*( 1.0f - iw ) ) + ( iw*x ); + s->w = ( s->lambda*s->w ) + 1.0F; + iw = 1.0F/s->w; + s->m = ( s->m*( 1.0F - iw ) ) + ( iw*x ); return s->m; } /*============================================================================*/ +static float qSSmoother_Filter_DESF( _qSSmoother_t * const f, + const float x ) +{ + /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ + qSSmoother_DESF_t * const s = (qSSmoother_DESF_t* const)f; + /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ + float lt_1; + + if ( 1U == f->init ) { + s->lt = x; + s->bt = x; + f->init = 0U; + } + lt_1 = s->lt; + s->lt = ( ( 1.0F - s->alpha)*lt_1 ) + ( s->alpha*x ); /*level*/ + s->bt = ( ( 1.0F - s->beta )*s->bt ) + ( s->beta*( s->lt - lt_1 ) ); /*trend*/ + return s->lt + ( s->n*s->bt ); /*model/forecast*/ +} +/*============================================================================*/ +static float qSSmoother_Filter_ALNF( _qSSmoother_t * const f, + const float x ) +{ + /*cstat -CERT-EXP36-C_a -MISRAC2012-Rule-11.3 -CERT-EXP39-C_d*/ + /*cppcheck-suppress misra-c2012-11.3 */ + qSSmoother_ALNF_t * const s = (qSSmoother_ALNF_t* const)f; + /*cstat +CERT-EXP36-C_a +MISRAC2012-Rule-11.3 +CERT-EXP39-C_d*/ + float xe; + size_t i; + + if ( 1U == f->init ) { + /*cstat -CERT-FLP36-C*/ + float np = 1.0F/(float)s->n; + /*cstat +CERT-FLP36-C*/ + qSSmoother_WindowSet( s->x, s->n, x ); + qSSmoother_WindowSet( s->w, s->n, np ); + if ( NULL != s->w_1 ) { + qSSmoother_WindowSet( s->w_1, s->n, np ); + } + f->init = 0U; + } + xe = qLTISys_DiscreteFIRUpdate( s->x, s->w, s->n, x ); + if ( NULL != s->w_1 ) { + float *w_1 = &s->w[ s->n ]; + + for ( i = 0U ; i < s->n ; ++i ) { + float w = s->w[ i ]; + float w1 = w_1[ i ]; + + s->w[ i ] += ( s->alpha*( x - xe )*s->x[ i ] ) + ( s->mu*( w - w1 ) ); + w_1[ i ] = w; + } + } + else { + for ( i = 0U ; i < s->n ; ++i ) { + s->w[ i ] += s->alpha*( x - xe )*s->x[ i ]; + } + } + + return xe; +} +/*============================================================================*/ diff --git a/qtdl.c b/qtdl.c index 956861d..a0630ee 100644 --- a/qtdl.c +++ b/qtdl.c @@ -1,7 +1,7 @@ /*! * @file qtdl.c * @author J. Camilo Gomez C. - * @note This file is part of the qTools distribution. + * @note This file is part of the qLibs distribution. **/ #include "qtdl.h" @@ -28,9 +28,9 @@ void qTDL_Flush( qTDL_t * const q, q->tail = &q->head[ q->itemCount ]; q->wr = q->head; /*cstat -CERT-INT30-C_a*/ - q->rd = &q->head[ q->itemCount - 1u ]; + q->rd = &q->head[ q->itemCount - 1U ]; /*cstat +CERT-INT30-C_a*/ - for ( i = 0u ; i < q->itemCount ; ++i ) { /*initialize the queue*/ + for ( i = 0U ; i < q->itemCount ; ++i ) { /*initialize the queue*/ qTDL_InsertNewest( q, initVal ); /*the queue should be always full*/ } } @@ -54,7 +54,7 @@ static void qTDL_RemoveOldest( qTDL_t * const q ) /*============================================================================*/ float qTDL_GetOldest( const qTDL_t * const q ) { - return ( ( q->rd + 1u ) >= q->tail ) ? q->head[ 0 ] : q->rd[ 1 ]; + return ( ( q->rd + 1U ) >= q->tail ) ? q->head[ 0 ] : q->rd[ 1 ]; } /*============================================================================*/ float qTDL_GetAtIndex( const qTDL_t * const q, diff --git a/qtypegeneric.c b/qtypegeneric.c new file mode 100644 index 0000000..0490a2c --- /dev/null +++ b/qtypegeneric.c @@ -0,0 +1,374 @@ +/*! + * @file qtypegeneric.c + * @author J. Camilo Gomez C. + * @note This file is part of the qLibs distribution. + **/ + +#include "qtypegeneric.h" +#include + +/*! @cond */ +typedef struct { + uint8_t *lo, *hi; +} qTypeGeneric_SortStackNode_t; +/*! @endcond */ + +static void qTypeGeneric_SortStackPush( qTypeGeneric_SortStackNode_t **top, + uint8_t *low, + uint8_t *high ); + +/*============================================================================*/ +void qTypeGeneric_Swap( void * const x, + void * const y, + size_t n ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + uint8_t * const a = (uint8_t * const)x; + uint8_t *b = (uint8_t * const)y; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + size_t i = 0U, j = 0U; + do { + uint8_t tmp = a[ i ]; + /*cstat -CERT-INT30-C_a*/ + a[ i++ ] = b[ j ]; + b[ j++ ] = tmp; + /*cstat +CERT-INT30-C_a*/ + } while( --n > 0U ); +} +/*============================================================================*/ +static void qTypeGeneric_SortStackPush( qTypeGeneric_SortStackNode_t **top, + uint8_t *low, + uint8_t *high ) +{ + top[ 0 ]->lo = low; + top[ 0 ]->hi = high; + ++top[ 0 ]; +} +/*============================================================================*/ +void qTypeGeneric_Sort( void * const pbase, + size_t n, + size_t size, + qTypeGeneric_CompareFcn_t cmp, + void *arg ) +{ + /*cstat -MISRAC2012-Rule-18.4*/ + if ( ( NULL != pbase ) && ( size > 0U ) && ( n > 0U ) && ( NULL != cmp ) ) { + const size_t max_thresh = 4U*size; + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + uint8_t *base_ptr = (uint8_t *)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + uint8_t * const end_ptr = &base_ptr[ size*( n - 1U ) ]; + uint8_t *tmp_ptr = base_ptr, *run_ptr; + uint8_t *thresh = ( end_ptr < ( base_ptr + max_thresh) ) ? end_ptr : ( base_ptr + max_thresh ) ; + + if ( n > 4U ) { + uint8_t *lo = base_ptr, *hi = &lo[ size*( n - 1U ) ]; + qTypeGeneric_SortStackNode_t stack[ 8U*sizeof(size_t) ], *top = stack; + + qTypeGeneric_SortStackPush( &top, NULL, NULL ); + while ( stack < top ) { + uint8_t *left_ptr, *right_ptr; + /*cstat -ATH-div-0-unchk-param -CERT-INT33-C_h*/ + /*cppcheck-suppress misra-c2012-10.8 */ + uint8_t *mid = &lo[ size*( ( (size_t)( hi - lo )/size ) >> 1U ) ]; + /*cstat +ATH-div-0-unchk-param +CERT-INT33-C_h*/ + if ( cmp( mid, lo, arg ) < 0 ) { + qTypeGeneric_Swap( mid, lo, size ); + } + if ( cmp( hi, mid, arg ) < 0 ) { + qTypeGeneric_Swap( mid, hi, size ); + } + else { + /*cppcheck-suppress misra-c2012-15.1 */ + goto jump_over; /*MISRAC deviation allowed*/ + } + if ( cmp( mid, lo, arg ) < 0 ) { + qTypeGeneric_Swap( mid, lo, size ); + } + + jump_over: + left_ptr = lo + size; + right_ptr = hi - size; + do { + while ( cmp( left_ptr, mid, arg ) < 0 ) { + /*cppcheck-suppress misra-c2012-10.3 */ + left_ptr += size; + } + while ( cmp( mid, right_ptr, arg ) < 0 ) { + /*cppcheck-suppress misra-c2012-10.3 */ + right_ptr -= size; + } + + if ( left_ptr < right_ptr ) { + qTypeGeneric_Swap( left_ptr, right_ptr, size ); + if ( mid == left_ptr ) { + mid = right_ptr; + } + else if ( mid == right_ptr ) { + mid = left_ptr; + } + else { + /*nothing to do here*/ + } + /*cppcheck-suppress misra-c2012-10.3 */ + left_ptr += size; + /*cppcheck-suppress misra-c2012-10.3 */ + right_ptr -= size; + } + else if ( left_ptr == right_ptr ) { + /*cppcheck-suppress misra-c2012-10.3 */ + left_ptr += size; + /*cppcheck-suppress misra-c2012-10.3 */ + right_ptr -= size; + break; + } + else { + /*nothing to do here*/ + } + } while (left_ptr <= right_ptr); + + /*cppcheck-suppress misra-c2012-10.8 */ + if ( (size_t)( right_ptr - lo ) <= max_thresh ) { + /*cppcheck-suppress misra-c2012-10.8 */ + if ( (size_t)( hi - left_ptr ) <= max_thresh ) { + --top; /*POP form the stack*/ + lo = top->lo; + hi = top->hi; + } + else { + lo = left_ptr; + } + } + /*cppcheck-suppress misra-c2012-10.8 */ + else if ( (size_t)( hi - left_ptr ) <= max_thresh ) { + hi = right_ptr; + } + else if ( ( right_ptr - lo ) > ( hi - left_ptr ) ) { + qTypeGeneric_SortStackPush( &top, lo, right_ptr ); + lo = left_ptr; + } + else { + qTypeGeneric_SortStackPush( &top, left_ptr, hi ); + hi = right_ptr; + } + } + } + /*cppcheck-suppress misra-c2012-10.3 */ + for ( run_ptr = tmp_ptr + size ; run_ptr <= thresh ; run_ptr += size ) { + if ( cmp( run_ptr, tmp_ptr, arg ) < 0 ) { + tmp_ptr = run_ptr; + } + } + + if ( tmp_ptr != base_ptr ) { + qTypeGeneric_Swap( tmp_ptr, base_ptr, size ); + } + run_ptr = base_ptr + size; + /*cppcheck-suppress misra-c2012-10.3 */ + while ( (run_ptr += size ) <= end_ptr ) { + tmp_ptr = run_ptr - size; + while ( cmp( run_ptr, tmp_ptr, arg ) < 0 ) { + /*cppcheck-suppress misra-c2012-10.3 */ + tmp_ptr -= size; + } + /*cppcheck-suppress misra-c2012-10.3 */ + tmp_ptr += size; + if ( tmp_ptr != run_ptr ) { + uint8_t *tra = run_ptr + size; + while ( --tra >= run_ptr ) { + uint8_t c = *tra; + uint8_t *hi = tra; + uint8_t *lo = tra; + /*cppcheck-suppress misra-c2012-10.3 */ + while ( (lo -= size) >= tmp_ptr ) { + *hi = *lo; + hi = lo; + } + *hi = c; + } + } + } + } + /*cstat +MISRAC2012-Rule-18.4*/ +} +/*============================================================================*/ +void qTypeGeneric_Reverse( void * const pbase, + const size_t size, + const size_t init, + const size_t end ) +{ + if ( ( NULL != pbase ) && ( size > 0U ) && ( end > init ) ) { + size_t s = size*init; + size_t e = size*end; + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + uint8_t *v = (uint8_t*)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + while( s < e ) { + qTypeGeneric_Swap( &v[ s ], &v[ e ], size ); + s += size; + e -= size; + } + } +} +/*============================================================================*/ +void qTypeGeneric_Rotate( void * const pbase, + const size_t size, + const size_t n, + const int k ) +{ + if ( ( NULL != pbase ) && ( 0 != k ) && ( n > 0U ) ) { + size_t r; + + if ( k > 0 ) { + r = (size_t)k; + r %= n; + qTypeGeneric_Reverse( pbase, size, n - r, n - 1U ); + qTypeGeneric_Reverse( pbase, size, 0U, n - r - 1U ); + qTypeGeneric_Reverse( pbase, size, 0U, n - 1U ); + } + else { + r = (size_t)(-k); + r %= n; + qTypeGeneric_Reverse( pbase, size, 0U, r - 1U ); + qTypeGeneric_Reverse( pbase, size, r, n - 1U ); + qTypeGeneric_Reverse( pbase, size, 0U, n - 1U ); + } + } +} +/*============================================================================*/ +void* qTypeGeneric_Set( void * const pbase, + const size_t size, + const size_t n, + const void * const ref ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + uint8_t *p = (uint8_t*)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + void *retVal = NULL; + + if ( ( NULL != pbase ) && ( size > 0U ) && ( n > 0U ) && ( NULL != ref ) ) { + size_t i; + + for ( i = 0U ; i < n ; i++ ) { + retVal = memcpy( &p[ size*i ], ref, size ); + } + } + + return retVal; +} +/*============================================================================*/ +void* qTypeGeneric_LSearch( const void *key, + const void *pbase, + const size_t n, + const size_t size, + qTypeGeneric_CompareFcn_t compar, + void *arg ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + const uint8_t *pb = (const uint8_t *)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + void* retVal = NULL; + size_t i; + + for ( i = 0U ; i < n ; ++i ) { + const uint8_t *element = &pb[ i*size ]; + + if ( 0 == compar( key, element, arg) ) { + /*cstat -MISRAC2012-Rule-11.8*/ + /*cppcheck-suppress [ misra-c2012-11.8, cert-EXP05-C ]*/ + retVal = (void*)element; + /*cstat +MISRAC2012-Rule-11.8*/ + break; + } + } + return retVal; +} +/*============================================================================*/ +void* qTypeGeneric_BSearch( const void *key, + const void *pbase, + const size_t n, + const size_t size, + qTypeGeneric_CompareFcn_t compar, + void *arg ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + const uint8_t *base = (const uint8_t *)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + size_t lim = n; + const uint8_t *p; + void *retVal = NULL; + + while ( 0U != lim ) { + int cmp; + + p = &base[ ( lim >> 1U )*size ]; + cmp = compar( key, p, arg ); + if ( 0 == cmp ) { + /*cstat -MISRAC2012-Rule-11.8*/ + /*cppcheck-suppress [ misra-c2012-11.8, cert-EXP05-C ]*/ + retVal = (void *)p; + /*cstat +MISRAC2012-Rule-11.8*/ + break; + } + else if ( cmp > 0 ) { + base = &p[ size ]; + lim--; + } + else { + /*nothing to do here*/ + } + lim >>= 1U; + } + return retVal; +} +/*============================================================================*/ +int qTypeGeneric_ForEach( void *pbase, + const size_t size, + const size_t n, + qTypeGeneric_ForEachFcn_t f, + const bool dir, + void *arg ) +{ + int retVal = 0; + + if ( ( NULL != pbase ) && ( NULL != f ) && ( n > 0U ) ) { + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + uint8_t *pb = (uint8_t *)pbase; + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ + if ( 1 != f( -1, NULL, arg ) ) { + size_t i; + uint8_t *element; + + if ( false == dir ) { + for ( i = 0U ; i < n ; ++i ) { + element = &pb[ i*size ]; + retVal = f( (int)i , element, arg ); + if ( 1 == retVal ) { + break; + } + } + } + else { + i = n; + while ( i-- > 0U ) { + element = &pb[ i*size ]; + retVal = f( (int)i, element, arg ); + if ( 1 == retVal ) { + break; + } + } + } + } + (void)f( (int)n, NULL, arg ); + } + + return retVal; +} +/*============================================================================*/ \ No newline at end of file diff --git a/qvfloat.c b/qvfloat.c new file mode 100644 index 0000000..a73240f --- /dev/null +++ b/qvfloat.c @@ -0,0 +1,518 @@ +/*! + * @file qvfloat.c + * @author J. Camilo Gomez C. + * @note This file is part of the qLibs distribution. + **/ + +#include "qvfloat.h" +#include "qtypegeneric.h" +#include "qffmath.h" +#include + +typedef float (*qVFloat_VVFcn_t)( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ); +typedef float (*qVFloat_kVFcn_t)( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ); + +static float qVFloat_VV_Add( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ); +static float qVFloat_VV_Mul( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ); +static float qVFloat_VV_Div( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ); +static float qVFloat_kV_Add( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ); +static float qVFloat_kV_Mul( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ); +static float qVFloat_kV_Div( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ); +static int qVFloat_CmpAsc( const void *a, + const void *b, + void *arg ); +static int qVFloat_CmpDes( const void *a, + const void *b, + void *arg ); + +/*============================================================================*/ +static float qVFloat_VV_Add( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ) +{ + size_t i; + float s = 0.0F; + + for ( i = 0U; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = ( a*x[ i ] ) + ( b*y[ i ] ); + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +static float qVFloat_kV_Add( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ) +{ + size_t i; + float s = 0.0F; + + for ( i = 0U ; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = ( a*x[ i ] ) + b; + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +static float qVFloat_VV_Mul( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ) +{ + size_t i; + float s = 0.0F; + const float k = a*b; + + for ( i = 0U ; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = k*x[ i ]*y[ i ]; + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +static float qVFloat_kV_Mul( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ) +{ + size_t i; + float s = 0.0F; + const float k = a*b; + + for ( i = 0U ; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = k*x[ i ]; + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +static float qVFloat_VV_Div( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + const float * const y, + size_t n ) +{ + size_t i; + float s = 0.0F; + const float k = a/b; + + for ( i = 0U ; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = ( k*x[ i ] )/y[ i ]; + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +static float qVFloat_kV_Div( float **dst, + float **pOut, + const float a, + const float * const x, + const float b, + size_t n ) +{ + size_t i; + float s = 0.0F; + const float k = a/b; + + for ( i = 0U ; i < n ; ++i ) { + pOut[ 0 ][ 0 ] = k*x[ i ]; + s += pOut[ 0 ][ 0 ]; + dst[ 0 ]++; + } + + return s; +} +/*============================================================================*/ +float qVFloat_Operate( float * dst, + qVFloat_Operation_t o, + const float a, + const float * const x, + const float b, + const float * const y, + const size_t n ) +{ + /*cppcheck-suppress variableScope */ + static qVFloat_VVFcn_t vv_fcn[ 3 ] = { + &qVFloat_VV_Add, &qVFloat_VV_Mul, &qVFloat_VV_Div + }; + /*cppcheck-suppress variableScope */ + static qVFloat_kVFcn_t kv_fcn[ 3 ] = { + &qVFloat_kV_Add, &qVFloat_kV_Mul, &qVFloat_kV_Div + }; + float retVal = 0.0F; + + if ( ( NULL != x ) && ( n > 0U ) ) { + float t; + float *pt = &t; + float **ppt = ( NULL == dst ) ? &pt : &dst; + float **pdst = &dst; + + retVal = ( NULL != y ) ? vv_fcn[ o ]( pdst, ppt, a, x, b, y, n ) + : kv_fcn[ o ]( pdst, ppt, a, x, b, n ); + } + + return retVal; +} +/*============================================================================*/ +float qVFloat_ApplyFx( float *dst, + float (*fx1)(float), + float (*fx2)(float, float), + float * const x, + float * const y, + const float a, + const float b, + const size_t n ) +{ + size_t i; + float s = 0.0F; + float t; + float *pt = &t; + float **ppt = ( NULL == dst ) ? &pt : &dst; + + if ( NULL != fx1 ) { + for ( i = 0U ; i < n ; ++i ) { + ppt[ 0 ][ 0 ] = a*fx1( x[ i ] ); + s += ppt[ 0 ][ 0 ]; + dst++; + } + } + else if ( NULL != fx2 ) { + if ( NULL != y ) { + for ( i = 0U ; i < n ; ++i ) { + ppt[ 0 ][ 0 ]= a*fx2( x[ i ], y[ i ] ); + s += ppt[ 0 ][ 0 ]; + dst++; + } + } + else { + for ( i = 0U ; i < n ; ++i ) { + ppt[ 0 ][ 0 ] = a*fx2( x[ i ], b ); + s += ppt[ 0 ][ 0 ]; + dst++; + } + } + } + else { + /*nothing to do here*/ + } + + return s; +} +/*============================================================================*/ +int qVFloat_Moment( qVFloat_Moment_t * const m, + const float * const x, + const size_t n ) +{ + int retVal = 0; + + if ( n > 1U ) { + size_t j; + float ep = 0.0F; + float s = 0.0F; + /*cstat -CERT-FLP36-C*/ + const float l = (float)n; + /*cstat +CERT-FLP36-C*/ + (void)memset( m, 0, sizeof(qVFloat_Moment_t) ); + for ( j = 1U ; j <= n ; ++j ) { + s += x[ j ]; + } + m->mean = s/l; + for ( j = 1U ; j <= n ; ++j ) { + float p; + + s = x[ j ] - m->mean; + ep += s; + m->avgDev += QLIB_ABS( s ); + p = s*s; + m->var += p; + p *= s; + m->skew += p; + p *= s; + m->curt += p; + } + m->avgDev /= l; + m->var = ( m->var - ( ( ep*ep )/l ) )/( l - 1.0F ); + /*cstat -MISRAC2012-Dir-4.11_b -CERT-FLP34-C*/ + m->stdDev = ( m->var >= 0.0F ) ? QLIB_SQRT( m->var ) : QLIB_NAN; + /*cstat +MISRAC2012-Dir-4.11_b*/ + if ( false == qFFMath_IsEqual( 0.0F, m->var ) ) { + m->skew /= ( ( m->curt )/( l*m->var*m->var ) ) - 3.0F; + } + else { + m->skew = QLIB_NAN; + } + /*cstat +CERT-FLP34-C*/ + retVal = 1; + } + + return retVal; +} +/*============================================================================*/ +float* qVFloat_Set( float * const x, + const float c, + const size_t n ) +{ + size_t i; + + for ( i = 0U ; i < n ; ++i ) { + x[ i ] = c; + } + + return x; +} +/*============================================================================*/ +float* qVFloat_Copy( float * const dst, + const float * const src, + const size_t n ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b*/ + /*cppcheck-suppress misra-c2012-11.5 */ + return (float*)memcpy( dst, src, n*sizeof(float) ); + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b*/ +} +/*============================================================================*/ +float qVFloat_PolyVal( const float * const p, + const float x, + size_t n ) +{ + size_t i; + float fx = 0.0F; + + for ( i = 0U ; i < n ; ++i ) { + /*cstat -CERT-FLP36-C*/ + fx += p[ n - i - 1U ]*QLIB_POW( x, (float)i ); + /*cstat +CERT-FLP36-C*/ + } + + return fx; +} +/*============================================================================*/ +float* qVFloat_LinSpace( float * const dst, + const float x1, + const float x2, + const size_t n ) +{ + /*cstat -CERT-FLP36-C -MISRAC2012-Rule-10.8*/ + /*cppcheck-suppress misra-c2012-10.8 */ + float step = ( x2 - x1 )/(float)( n - 1U ); + size_t i; + + for ( i = 0U ; i < n ; ++i ) { + dst[ i ] = x1 + ( (float)i*step ); + } + /*cstat +CERT-FLP36-C +MISRAC2012-Rule-10.8*/ + return dst; +} +/*============================================================================*/ +float qVFloat_Distance( const float * const x, + const float * const y, + const size_t n ) +{ + float s = 0.0F; + + if ( ( NULL != x ) && ( NULL != y ) ) { + size_t i; + + for ( i = 0U ; i < n ; ++i ) { + float tmp = x[ i ] - y[ i ]; + + s += tmp*tmp; + } + /*cstat -MISRAC2012-Dir-4.11_b*/ + s = QLIB_SQRT( s ); /*always positive*/ + /*cstat +MISRAC2012-Dir-4.11_b*/ + } + + return s; +} +/*============================================================================*/ +float* qVFloat_Reverse( float * const dst, + float * const src, + const size_t init, + const size_t end ) +{ + float *v = NULL; + + if ( ( NULL != src ) && ( end > init ) ) { + v = ( NULL != dst ) ? qVFloat_Copy( dst, src, end - init ) : src; + qTypeGeneric_Reverse( v, sizeof(float), init, end ); + } + + return v; +} +/*============================================================================*/ +float* qVFloat_Rotate( float * const dst, + float * const src, + const int k, + const size_t n ) +{ + float *v = NULL; + + if ( ( NULL != src ) && ( 0 != k ) && ( n > 0U ) ) { + v = ( NULL != dst ) ? qVFloat_Copy( dst, src, n ) : src; + qTypeGeneric_Rotate( v, sizeof(float), n, k ); + } + + return v; +} +/*============================================================================*/ +int qVFloat_MinMax( qVFloat_MinMax_t * const o, + const float * const x, + const size_t n ) +{ + int retVal = 0; + + if ( ( NULL != x ) && ( NULL != o ) && ( n > 0U ) ) { + if ( 1U == n ) { + o->min = x[ 0 ]; + o->max = x[ 0 ]; + o->index_min = 0U; + o->index_max = 0U; + } + else { + size_t i; + + if ( x[ 0 ] > x[ 1 ] ) { + o->max = x[ 0 ]; + o->min = x[ 1 ]; + o->index_min = 1U; + o->index_max = 0U; + } + else { + o->max = x[ 1 ]; + o->min = x[ 0 ]; + o->index_min = 0U; + o->index_max = 1U; + } + + for ( i = 2U ; i < n ; ++i ) { + if ( x[ i ] > o->max ) { + o->max = x[ i ]; + o->index_max = i; + } + else if ( x[ i ] < o->min ) { + o->min = x[ i ]; + o->index_min = i; + } + else { + /*nothing to do here*/ + } + } + } + retVal = 1; + } + + return retVal; +} +/*============================================================================*/ +static int qVFloat_CmpAsc( const void *a, + const void *b, + void *arg ) +{ + /*cstat -MISRAC2012-Rule-11.5 -CERT-EXP36-C_b -MISRAC2012-Rule-10.1_R3*/ + /*cppcheck-suppress misra-c2012-11.5 */ + const float *fa = (const float*)a; + /*cppcheck-suppress misra-c2012-11.5 */ + const float *fb = (const float*)b; + (void)arg; + /*cppcheck-suppress misra-c2012-10.5 */ + return ( (int)( fa[ 0 ] > fb[ 0 ] ) ) - ( (int)( fa[ 0 ] < fb[ 0 ] ) ); +} +/*============================================================================*/ +static int qVFloat_CmpDes( const void *a, + const void *b, + void *arg ) +{ + /*cppcheck-suppress misra-c2012-11.5 */ + const float *fa = (const float*)a; + /*cppcheck-suppress misra-c2012-11.5 */ + const float *fb = (const float*)b; + (void)arg; + /*cppcheck-suppress misra-c2012-10.5 */ + return (int)( fa[ 0 ] < fb[ 0 ] ) - (int)( fa[ 0 ] > fb[ 0 ] ); + /*cstat +MISRAC2012-Rule-11.5 +CERT-EXP36-C_b +MISRAC2012-Rule-10.1_R3*/ +} +/*============================================================================*/ +float* qVFloat_Sort( float * const dst, + float * const src, + const bool dir, + size_t n ) +{ + float *v = NULL; + + if ( ( src != NULL ) && ( n > 0U ) ) { + v = ( NULL != dst ) ? qVFloat_Copy( dst, src, n ) : src; + qTypeGeneric_Sort( v, n, sizeof(float), ( dir ) ? &qVFloat_CmpAsc : &qVFloat_CmpDes, NULL ); + } + + return v; +} +/*============================================================================*/ \ No newline at end of file From 3d203b1527b67305372fadc7c5da8c1aff8e5e45 Mon Sep 17 00:00:00 2001 From: camilo Date: Fri, 15 Nov 2024 08:43:05 -0500 Subject: [PATCH 2/2] added more special functions --- include/qffmath.h | 302 ++++++++ qffmath.c | 1754 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 2056 insertions(+) diff --git a/include/qffmath.h b/include/qffmath.h index 1a006ce..d5e1a70 100644 --- a/include/qffmath.h +++ b/include/qffmath.h @@ -91,6 +91,13 @@ extern "C" { /** @brief The natural logarithm of the square root of 2π given as a single-precision floating-point number */ #define QFFM_LN_SQRT_2PI ( 0.918938533204672669540968854562379419F ) + /** @brief Constant Euler-Mascheroni given as a single-precision floating-point number*/ + #define QFFM_GAMMA_E ( 0.577215664901532860606512090082402431F ) + /** @brief Twice circumference of a circle with diameter 1, ( 2π ) given as a single-precision floating-point number */ + #define QFFM_2PI ( 6.283185307179586231995926937088370323F ) + /** @brief The natural logarithm of π ( ln(π) ) given as a single-precision floating-point number */ + #define QFFM_LN_PI ( 1.144729885849400163877476188645232468F ) + /** @brief The maximum value of a non-infinite single-precision floating-point number */ #define QFFM_MAXFLOAT ( 3.40282347e+38F ) /** @brief Positive infinity given as a single-precision floating-point number */ @@ -464,6 +471,42 @@ extern "C" { */ float qFFMath_ATanh( float x ); + /** + * @brief Wraps angle @a x, in radians, to the interval [−pi, pi] such that + * @c pi maps to @c pi and @c -pi maps to @c -pi. In general, odd, positive multiples + * of @c pi map to @c pi and odd, negative multiples of @c pi map to @c -pi. + * @param x The angle in radians + * @return The angle @a x wrapped to the [-pi, pi] interval + */ + float qFFMath_WrapToPi( float x ); + + /** + * @brief Wraps angle @a x, in radians, to the interval [0, 2*pi] such that + * @c 0 maps to @c 0 and @c 2*pi and @c 2*pi maps to @c 2*pi. In general, positive multiples + * of @c 2*pi map to @c 2*pi and negative multiples of @c 2*pi map to @c 0. + * @param x The angle in radians + * @return The angle @a x wrapped to the [0, 2*pi] interval + */ + float qFFMath_WrapTo2Pi( float x ); + + /** + * @brief Wraps angle @a x, in degrees, to the interval [–180, 180] such + * that @c 180 maps to @c 180 and @c -180 maps to @c -180. In general, odd, positive + * multiples of @c 180 map to @c 180 and odd, negative multiples of @c 180 map to @c -180. + * @param x The angle in degrees + * @return The angle @a x wrapped to the [-180, 180] interval + */ + float qFFMath_WrapTo180( float x ); + + /** + * @brief Wraps angle @a x, in degrees, to the interval [0, 360] such + * that @c 0 maps to @c 0 and @c 360 maps to @c 360. In general, positive multiples + * of @c 360 map to @c 360 and negative multiples of @c 360 map to zero. + * @param x The angle in degrees + * @return The angle @a x wrapped to the [0, 360] interval + */ + float qFFMath_WrapTo360( float x ); + /** * @brief Computes the error function of @a x. * @param[in] x The floating point value @@ -708,6 +751,265 @@ extern "C" { */ float qFFMath_Factorial( float x ); + /** + * @brief Computes the associated Laguerre polynomials of the degree @a n, + * order @a m, and argument @a x. + * @param[in] n The degree of the polynomial, an unsigned integer value + * @param[in] m The order of the polynomial, an unsigned integer value + * @param[in] x The argument, a floating-point or integer value + * @return Upon successful completion, the value of the associated Laguerre + * polynomial of @a x shall be returned. If the argument is @c nan, a @c nan is + * returned. If @a x is negative, @c nan is returned. If @a n or @a m is greater or + * equal to @c 128, the behavior is implementation-defined. + */ + float qFFMath_Assoc_laguerre( size_t n, + size_t m, + float x ); + + /** + * @brief Computes the associated Legendre polynomials of the degree @a n, + * order @a m, and argument @a x. + * @param[in] n The degree of the polynomial, an unsigned integer value + * @param[in] m The order of the polynomial, an unsigned integer value + * @param[in] x The argument, a floating-point or integer value + * @return Upon successful completion, the value of the associated Legendre + * polynomial of x shall be returned. If the argument is @c nan, a @c nan is + * returned. If |x| > 1, @c nan is returned due the domain error. + * If @a n is greater or equal to @c 128, the behavior is implementation-defined. + */ + float qFFMath_Assoc_legendre( size_t n, + size_t m, + float x ); + + /** + * @brief Computes the Beta function of @a x and @a y. + * @param[in] x The argument, a floating-point or integer value + * @param[in] y The argument, a floating-point or integer value + * @return Upon successful completion, the value of the Beta function of + * @a x and @a y. If the argument is @c nan, @c nan is returned. The function + * is only required to be defined where both @a x and @a y are greater + * than zero, and is allowed to return @c nan otherwise. + */ + float qFFMath_Beta( float x, + float y ); + + /** + * @brief Computes the complete elliptic integral of the first kind of @a k + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @return Upon successful completion, the value of the complete elliptic + * integral of the first kind of @a k. If the argument is @c nan, @c nan is + * returned. If |k| > 1, NaN is returned due the domain error + */ + float qFFMath_Comp_ellint_1( float k ); + + /** + * @brief Computes the complete elliptic integral of the second kind of @a k + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @return Upon successful completion, the value of the complete elliptic + * integral of the second kind of @a k. If the argument is @c nan, @c nan is + * returned. If |k| > 1, @c nan is returned due the domain error + */ + float qFFMath_Comp_ellint_2( float k ); + + /** + * @brief Computes the complete elliptic integral of the third kind of + * @a k and @a nu. + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @param[in] nu Elliptic characteristic as a floating-point value + * @return Upon successful completion, the value of the complete elliptic + * integral of the third kind of @a k and @a nu. If the argument is @c nan, + * @c nan is returned. If |k| > 1, @c nan is returned due the domain error + */ + float qFFMath_Comp_ellint_3( float k, + float nu ); + + /** + * @brief Computes the incomplete elliptic integral of the first kind of + * @a k and @a phi + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @param[in] phi Jacobi amplitude as a floating-point value given in radians + * @return Upon successful completion, the value of the incomplete elliptic + * integral of the first kind of @a k and @a phi. If the argument is @c nan, + * @c nan is returned. If |k| > 1, @c nan is returned due the domain error + */ + float qFFMath_Ellint_1( float k, + float phi ); + + /** + * @brief Computes the incomplete elliptic integral of the second kind of + * @a k and @a phi + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @param[in] phi Jacobi amplitude as a floating-point value given in radians + * @return Upon successful completion, the value of the incomplete elliptic + * integral of the second kind of @a k and @a phi. If the argument is @c nan, + * @c nan is returned. If |k| > 1, @c nan is returned due the domain error + */ + float qFFMath_Ellint_2( float k, + float phi ); + + /** + * @brief Computes the incomplete elliptic integral of the third kind of + * @a k, @a nu and @a phi + * @param[in] k Elliptic modulus or eccentricity as a floating-point value + * @param[in] nu Elliptic characteristic as a floating-point value + * @param[in] phi Jacobi amplitude as a floating-point value given in radians + * @return Upon successful completion, the value of the incomplete elliptic + * integral of the third kind of @a k, @a nu and @a phi. If the argument is @c nan, + * @c nan is returned. If |k| > 1, @c nan is returned due the domain error + */ + float qFFMath_Ellint_3( float k, + float nu, + float phi ); + + /** + * @brief Computes the Exponential integral of @a num + * @param[in] num A floating-point value + * @return Upon successful completion, the value of the exponential integral + * of @a num. If the argument is @c nan, @c nan is returned. If the argument + * is @c ±0, @c -inf is returned. + */ + float qFFMath_Expint( float num ); + + /** + * @brief Computes the (physicist's) Hermite polynomials of the degree + * @a n and argument @a x + * @param[in] n The degree of the polynomial + * @param[in] x The argument, a floating-point value + * @return Upon successful completion, the value of order-n Hermite polynomial + * of @a x. If the argument is @c nan, @c nan is returned. If n>=128, + * the behavior is implementation-defined. + */ + float qFFMath_Hermite( size_t n, + float x ); + + /** + * @brief Computes the non-associated Laguerre polynomials of the degree @a n, + * and argument @a x. + * @param[in] n The degree of the polynomial, an unsigned integer value + * @param[in] x The argument, a floating-point or integer value + * @return Upon successful completion, the value of the non-associated Laguerre + * polynomial of @a x shall be returned. If the argument is @c nan, a @c nan is + * returned. If @a x is negative, @c nan is returned. If @a n is greater or + * equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Laguerre( size_t n, + float x ); + + /** + * @brief Computes the unassociated Legendre polynomials of the degree @a n, + * and argument @a x. + * @param[in] n The degree of the polynomial, an unsigned integer value + * @param[in] x The argument, a floating-point or integer value + * @return Upon successful completion, the value of the unassociated Legendre + * polynomial of @a x shall be returned. If the argument is @c nan, a @c nan is + * returned. The function is not required to be defined for |x| > 1 . + * If @a n is greater or equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Legendre( size_t n, + float x ); + + /** + * @brief Computes the Riemann zeta function of @a s + * @param[in] s A floating-point value + * @return Upon successful completion, the value of the Riemann zeta function + * of @a s. If the argument is @c nan, @c nan is returned. + */ + float qFFMath_Riemann_zeta( float s ); + + /** + * @brief Computes the spherical Bessel function of the first kind @a n, + * and @a x. + * @param[in] n The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the spherical Bessel + * function of the first kind of @a n and @a x. If the argument is @c nan, a @c nan is + * returned. If @a n is greater or equal than @c 128, the behavior is + * implementation-defined. + */ + float qFFMath_Sph_bessel( size_t n, + float x ); + + /** + * @brief Computes the spherical Bessel function of the second kind also + * known as the spherical Neumann function of @a n and @a x. + * @param[in] n The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the spherical Bessel + * function of the second kind (spherical Neumann function) of @a n and + * @a x. If the argument is @c nan, a @c nan is + * returned. If @a n is greater or equal than @c 128, the behavior is + * implementation-defined. + */ + float qFFMath_Sph_neumann( size_t n, + float x ); + + /** + * @brief Computes the regular modified cylindrical Bessel function of + * @a nu and @a x + * @param[in] nu The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the regular modified + * cylindrical Bessel function of @a nu and @a x. If the argument is + * @c nan, a @c nan is returned. If @a nu is greater or equal than @c 128, + * the behavior is implementation-defined. + */ + float qFFMath_Cyl_bessel_i( float nu, + float x ); + + /** + * @brief Computes the cylindrical Bessel function of the first kind of @a nu + * and @a x + * @param[in] nu The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the irregular modified cylindrical Bessel function + * (also known as modified Bessel function of the second kind) of @a nu + * and @a x. If the argument is @c nan, a @c nan is returned. If @a nu is + * greater or equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Cyl_bessel_j( float nu, + float x ); + + /** + * @brief Computes the irregular modified cylindrical Bessel function + * (also known as modified Bessel function of the second kind) of @a nu + * and @a x + * @param[in] nu The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the irregular modified cylindrical Bessel function + * (also known as modified Bessel function of the second kind) of @a nu + * and @a x. If the argument is @c nan, a @c nan is returned. If @a nu is + * greater or equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Cyl_bessel_k( float nu, + float x ); + + /** + * @brief Computes the cylindrical Neumann function ( also known as Bessel + * function of the second kind or Weber function) of @a nu and @a x. + * @param[in] nu The order of the function + * @param[in] x The argument to the function, a floating-point or integer value + * @return Upon successful completion, the value of the cylindrical Neumann + * function ( Bessel function of the second kind) of @a nu + * and @a x. If the argument is @c nan, a @c nan is returned. If @a nu is + * greater or equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Cyl_neumann( float nu, + float x ); + + /** + * @brief 1-3) Computes the spherical associated Legendre function of + * degree @a l, order @a m, and polar angle @a theta + * @param[in] l The degree + * @param[in] m The order + * @param[in] theta Polar angle, measured in radians + * @return Upon successful completion, the value of the spherical associated + * Legendre function (that is, spherical harmonic with sigma = 0) of @a l, + * @a m, and @a theta. If the argument @a theta is @c nan, a @c nan is returned. + * If @a l is greater or equal than @c 128, the behavior is implementation-defined. + */ + float qFFMath_Sph_legendre( size_t l, + size_t m, + float theta ); #endif /*#ifdef QLIBS_USE_STD_MATH*/ /** @}*/ diff --git a/qffmath.c b/qffmath.c index e675f6b..83ef50f 100644 --- a/qffmath.c +++ b/qffmath.c @@ -18,6 +18,67 @@ static float qFFMath_CalcCbrt( float x , bool r ); static float lgamma_positive( float x ); +static float poly_laguerre_recursion( size_t n, + float alpha, + float x ); +static float poly_laguerre_large_n( size_t n, + float alpha, + float x ); +static float poly_laguerre_hyperg( size_t n, + float alpha, + float x ); +static float poly_legendre_p( size_t l, + float x ); +static float ellint_rf( float x, + float y, + float z ); +static float ellint_rd( float x, + float y, + float z ); +static float ellint_rc( float x, + float y ); +static float ellint_rj( float x, + float y, + float z, + float p ); +static float expint_E1_series( float x ); +static float expint_E1_asymp( float x ); +static float expint_Ei_asymp( float x ); +static float expint_E1( float x ); +static float expint_En_cont_frac( size_t n, + float x ); +static float expint_Ei_series( float x ); +static float expint_Ei( float x ); +static float riemann_zeta_glob( float s ); +static float riemann_zeta_product( float s ); +static void gamma_temme( float mu, + float* gam1, + float* gam2, + float* gam_pl, + float* gam_mi); +static void bessel_jn( float nu, + float x, + float* j_nu, + float* n_nu, + float* j_pnu, + float* n_pnu ); +static void sph_bessel_jn( size_t n, + float x, + float* j_n, + float* n_n, + float* jp_n, + float* np_n ); +static void bessel_ik( float nu, + float x, + float* i_nu, + float* k_nu, + float* i_pnu, + float* k_pnu ); +static float cyl_bessel_ij_series( float nu, + float x, + float sgn, + size_t max_iter ); + /*============================================================================*/ float _qFFMath_GetAbnormal( const int i ) { @@ -456,6 +517,26 @@ float qFFMath_ATanh( float x ) return qFFMath_Log( ( 1.0F + x )/( 1.0F - x ) )*0.5F; } /*============================================================================*/ +float qFFMath_WrapToPi( float x ) +{ + return qFFMath_Mod( x + QFFM_PI, QFFM_2PI ) - QFFM_PI; +} +/*============================================================================*/ +float qFFMath_WrapTo2Pi( float x ) +{ + return qFFMath_Mod( x, QFFM_2PI ); +} +/*============================================================================*/ +float qFFMath_WrapTo180( float x ) +{ + return qFFMath_Mod( x + 180.0F, 360.0F ) - 180.0F; +} +/*============================================================================*/ +float qFFMath_WrapTo360( float x ) +{ + return qFFMath_Mod( x, 360.0F ); +} +/*============================================================================*/ float qFFMath_Erf( float x ) { float retVal; @@ -1068,4 +1149,1677 @@ float qFFMath_Factorial( float x ) return y; } +/*============================================================================*/ +static float poly_laguerre_recursion( size_t n, + float alpha, + float x ) +{ + const float l0 = 1.0F; + float y; + + if ( 0U == n ) { + y = l0; + } + else { + const float l1 = -x + 1.0F + alpha; + if ( 1U == n ) { + y = l1; + } + else { + float ln2 = l0; + float ln1 = l1; + float ln = 0.0F; + for ( size_t i = 2U ; i <= n ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float nn = (float)i; + /*cstat +CERT-FLP36-C*/ + ln = ( ( ( 2.0F*nn ) - 1.0F ) + alpha - x )*( ln1/nn ) + - ( ( nn - 1.0F ) + alpha )*( ln2/nn ); + ln2 = ln1; + ln1 = ln; + } + y = ln; + } + } + + return y; +} +/*============================================================================*/ +static float poly_laguerre_large_n( size_t n, + float alpha, + float x ) +{ + const float PI_2_SQ = 2.467401100272339498076235031476244330406188964F; + /*cstat -CERT-FLP36-C*/ + const float m = (float)n; + /*cstat +CERT-FLP36-C*/ + const float a = -m; + const float b = alpha + 1.0F; + const float eta = ( 2.0F*b ) - ( 4.0F*a ); + const float cos2th = x/eta; + const float sin2th = 1.0F - cos2th; + const float th = qFFMath_ACos( qFFMath_Sqrt( cos2th ) ); + const float pre_h = PI_2_SQ*eta*eta*cos2th*sin2th; + const float lg_b = qFFMath_LGamma( b + m ); + const float ln_fact = qFFMath_LGamma( m + 1.0F ); + + const float preTerm1 = 0.5F*( 1.0F - b )*qFFMath_Log( 0.25F*x*eta ); + const float preTerm2 = 0.25F*qFFMath_Log( pre_h ); + const float lnPre = lg_b - ln_fact + ( 0.5F*x ) + preTerm1 - preTerm2; + const float serTerm1 = qFFMath_Sin( QFFM_PI*a ); + const float th2 = 2.0F*th; + const float serTerm2 = qFFMath_Sin( ( 0.25F*eta*( ( th2 ) - qFFMath_Sin( th2 ) ) + QFFM_PI_4 ) ); + + return qFFMath_Exp( lnPre )*( serTerm1 + serTerm2 ); +} +/*============================================================================*/ +static float poly_laguerre_hyperg( size_t n, + float alpha, + float x ) +{ + const float b = alpha + 1.0F; + const float mx = -x; + const float tc_sgn = ( x < 0.0F ) ? 1.0F : ( ( 1 == ( n % 2 ) ) ? -1.0F : 1.0F ); + const float ax = qFFMath_Abs( x ); + float tc = 1.0F; + + for ( size_t i = 1U ; i <= n ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float k = (float)i; + /*cstat +CERT-FLP36-C*/ + tc *= ax/k; + } + + float term = tc*tc_sgn; + float sum = term; + const int N = (int)n; + for ( int i = ( N - 1 ) ; i >= 0; --i ) { + /*cstat -CERT-FLP36-C -MISRAC++2008-5-0-7*/ + const float k = (float)i; + term *= ( b + k )/((float)( N - i ))*( k + 1.0F )/mx; + /*cstat +CERT-FLP36-C +MISRAC++2008-5-0-7*/ + sum += term; + } + + return sum; +} +/*============================================================================*/ +float qFFMath_Assoc_laguerre( size_t n, + size_t m, + float x ) +{ + float y; + /*cstat -CERT-FLP36-C*/ + const float alpha = (float)m; + const float N = (float)n; + /*cstat +CERT-FLP36-C*/ + if ( ( x < 0.0F ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( 0U == n ) { + y = 1.0F; + } + else if ( 1U == n ) { + y = 1.0F + alpha - x; + } + else if ( qFFMath_IsEqual( 0.0F, x ) ) { + float prod = alpha + 1.0F; + for ( size_t i = 2U ; i < n ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float k = (float)i; + /*cstat +CERT-FLP36-C*/ + prod *= ( alpha + k )/k; + } + y = prod; + } + else if ( ( alpha > -1.0F ) && ( x < ( ( 2.0F*( alpha + 1.0F ) ) + ( 4.0F*N ) ) ) ) { + y = poly_laguerre_large_n( n, alpha, x ); + } + else if ( ( ( x > 0.0F ) && ( alpha < -( N + 1.0F ) ) ) ) { + y = poly_laguerre_recursion( n, alpha, x ); + } + else { + y = poly_laguerre_hyperg( n, alpha, x ); + } + + return y; +} +/*============================================================================*/ +static float poly_legendre_p( size_t l, + float x ) +{ + float y; + + if ( qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( -1.0F, x ) ) { + y = ( 1 == ( l % 2 ) ) ? -1.0F : 1.0F; + } + else { + float p_lm2 = 1.0F; + + if ( 0U == l ) { + y = p_lm2; + } + else { + float p_lm1 = x; + + if ( 1U == l ) { + y = p_lm1; + } + else { + float p_l = 0.0F; + + for ( size_t i = 2U ; i <= l ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float ll = (float)i; + /*cstat +CERT-FLP36-C*/ + p_l = ( 2.0F*x* p_lm1 ) - p_lm2 - ( ( x*p_lm1 ) - p_lm2)/ll; + p_lm2 = p_lm1; + p_lm1 = p_l; + } + y = p_l; + } + } + } + + return y; +} +/*============================================================================*/ +float qFFMath_Assoc_legendre( size_t n, + size_t m, + float x ) +{ + float y; + const float phase = 1.0F; + + if ( m > n ) { + y = 0.0F; + } + else if ( qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( 0U == m ) { + y = poly_legendre_p( n, x ); + } + else { + float p_mm = 1.0F; + const float root = qFFMath_Sqrt( 1.0F - x )*qFFMath_Sqrt( 1.0F + x ); + float fact = 1.0F; + + for ( size_t i = 1U ; i <= m ; ++i ) { + p_mm *= phase*fact*root; + fact += 2.0F; + } + + if ( n == m ) { + y = p_mm; + } + else { + /*cstat -CERT-FLP36-C*/ + const float p_mp1m = ( 2.0F*( (float)m ) + 1.0F )*x*p_mm; + /*cstat +CERT-FLP36-C*/ + if ( n == ( m + 1U ) ) { + y = p_mp1m; + } + else { + float p_lm2m = p_mm; + float p_lm1m = p_mp1m; + float p_lm = 0.0F; + /*cstat -CERT-FLP36-C*/ + const float M = (float)m; + for ( size_t i = ( m + 2U ) ; i <= n ; ++i ) { + const float j = (float)i; + /*cstat +CERT-FLP36-C*/ + p_lm = ( ( ( 2.0F*j ) - 1.0F )*x*p_lm1m ) - ( ( j + M - 1.0F )*p_lm2m/( j - M ) ); + p_lm2m = p_lm1m; + p_lm1m = p_lm; + } + y = p_lm; + } + } + } + + return y; +} +/*============================================================================*/ +float qFFMath_Beta( float x, + float y ) +{ + float result; + + if ( qFFMath_IsNaN( x ) || qFFMath_IsNaN( y ) ) { + result = QFFM_NAN; + } + else { + const float bet = qFFMath_LGamma( x ) + qFFMath_LGamma( y ) - qFFMath_LGamma( x + y ); + result = qFFMath_Exp( bet ); + } + + return result; +} +/*============================================================================*/ +static float ellint_rf( float x, + float y, + float z ) +{ + const float loLim = 5.0F*FLT_MIN; + float result; + + + if ( ( x < 0.0F ) || ( y < 0.0F ) || ( z < 0.0F ) || + ( ( x + y ) < loLim ) || ( ( x + z ) < loLim ) || ( ( y + z) < loLim ) + ) { + result = QFFM_NAN; + } + else { + const float c0 = 1.0F/4.0F; + const float c1 = 1.0F/24.0F; + const float c2 = 1.0F/10.0F; + const float c3 = 3.0F/44.0F; + const float c4 = 1.0F/14.0F; + const float errTol = 0.0024607833005759250678823324F; + const float c13 = 1.0F/3.0F; + + float xn = x; + float yn = y; + float zn = z; + float mu, xnDev, ynDev, znDev; + const size_t maxIter = 100U; + float e2, e3, s; + + for ( size_t iter = 0U ; iter < maxIter ; ++iter ) { + float abs_xnDev, abs_ynDev, abs_znDev, lambda, epsilon; + float xRoot, yRoot, zRoot; + + mu = ( xn + yn + zn )*c13; + xnDev = 2.0F - ( mu + xn )/mu; + ynDev = 2.0F - ( mu + yn )/mu; + znDev = 2.0F - ( mu + zn )/mu; + abs_xnDev = qFFMath_Abs( xnDev ); + abs_ynDev = qFFMath_Abs( ynDev ); + abs_znDev = qFFMath_Abs( znDev ); + epsilon = ( abs_xnDev > abs_ynDev ) ? abs_xnDev : abs_ynDev; + epsilon = ( abs_znDev > epsilon ) ? abs_znDev : epsilon; + if ( epsilon < errTol ) { + break; + } + xRoot = qFFMath_Sqrt( xn ); + yRoot = qFFMath_Sqrt( yn ); + zRoot = qFFMath_Sqrt( zn ); + lambda = xRoot*( yRoot + zRoot ) + ( yRoot*zRoot ); + xn = c0*( xn + lambda ); + yn = c0*( yn + lambda ); + zn = c0*( zn + lambda ); + } + e2 = xnDev*ynDev; + e3 = ( e2*znDev ); + e2 = e2 - ( znDev*znDev ); + s = 1.0F + ( ( c1*e2 ) - c2 - ( c3*e3 ) )*e2 + ( c4*e3 ); + result = s/qFFMath_Sqrt( mu ); + } + + return result; +} +/*============================================================================*/ +static float ellint_rd( float x, + float y, + float z ) +{ + const float errTol = 0.0017400365588678507952624663346341549F; + const float loLim = 4.103335708781587555782386855921935e-26F; + float result; + + if ( ( x < 0.0F ) || ( y < 0.0F ) || ( ( x + y ) < loLim ) || ( z < loLim ) ) { + result = QFFM_NAN; + } + else { + const float c0 = 1.0F/4.0F; + const float c1 = 3.0F/14.0F; + const float c2 = 1.0F/6.0F; + const float c3 = 9.0F/22.0F; + const float c4 = 3.0F/26.0F; + float xn = x; + float yn = y; + float zn = z; + float sigma = 0.0F; + float power4 = 1.0F; + float mu, xnDev, ynDev, znDev; + float ea, eb, ec, ed, ef, s1, s2; + const size_t maxIter = 100U; + + for ( size_t iter = 0U ; iter < maxIter ; ++iter ) { + float abs_xnDev, abs_ynDev, abs_znDev, lambda, epsilon; + float xRoot, yRoot, zRoot; + + mu = ( xn + yn + ( 3.0F*zn ) )*0.2F; + xnDev = ( mu - xn )/mu; + ynDev = ( mu - yn )/mu; + znDev = ( mu - zn )/mu; + abs_xnDev = qFFMath_Abs( xnDev ); + abs_ynDev = qFFMath_Abs( ynDev ); + abs_znDev = qFFMath_Abs( znDev ); + epsilon = ( abs_xnDev > abs_ynDev ) ? abs_xnDev : abs_ynDev; + epsilon = ( abs_znDev > epsilon ) ? abs_znDev : epsilon; + if ( epsilon < errTol ) { + break; + } + xRoot = qFFMath_Sqrt( xn ); + yRoot = qFFMath_Sqrt( yn ); + zRoot = qFFMath_Sqrt( zn ); + lambda = xRoot*( yRoot + zRoot ) + ( yRoot*zRoot ); + sigma += power4/( zRoot*( zn + lambda ) ); + power4 *= c0; + xn = c0*( xn + lambda ); + yn = c0*( yn + lambda ); + zn = c0*( zn + lambda ); + } + ea = xnDev*ynDev; + eb = znDev*znDev; + ec = ea - eb; + ed = ea - ( 6.0F*eb ); + ef = ed + ec + ec; + s1 = ed*( -c1 + ( c3*ed/3.0F ) - ( 1.5F*c4*znDev*ef ) ); + s2 = znDev*( ( c2*ef ) + znDev*( -( c3*ec ) - ( znDev*c4 ) - ea ) ); + result = ( 3.0F*sigma ) + power4*qFFMath_RSqrt( mu )*( 1.0F + s1 + s2)/mu; + } + + return result; +} +/*============================================================================*/ +static float ellint_rc( float x, + float y ) +{ + float result; + const float loLim = 5.8774717550000002558112628881984982848919e-38F; + const float errTol = 0.049606282877419791144113503378321F; + + if ( ( x < 0.0F ) || ( y < 0.0F ) || ( y < loLim ) ) { + result = QFFM_NAN; + } + else { + const float c0 = 1.0F/4.0F; + const float c1 = 1.0F/7.0F; + const float c2 = 9.0F/22.0F; + const float c3 = 3.0F/10.0F; + const float c4 = 3.0F/8.0F; + const float c13 = 1.0F/3.0F; + float xn = x; + float yn = y; + const size_t maxIter = 100; + float mu, s, sn; + + for ( size_t iter = 0U ; iter < maxIter ; ++iter ) { + float lambda; + + mu = ( xn + 2.0F*yn )*c13; + sn = ( yn + mu )/mu - 2.0F; + if ( qFFMath_Abs( sn ) < errTol ){ + break; + } + lambda = ( 2.0F*qFFMath_Sqrt( xn )*qFFMath_Sqrt( yn ) ) + yn; + xn = c0*( xn + lambda ); + yn = c0*( yn + lambda ); + } + s = sn*sn*( c3 + sn*( c1 + sn*( c4 + ( sn*c2 ) ) ) ); + result = ( 1.0F + s )*qFFMath_RSqrt( mu ); + } + + return result; +} +/*============================================================================*/ +static float ellint_rj( float x, + float y, + float z, + float p ) +{ + float result; + const float loLim = 4.103335708781587555782386855921935e-26F; + const float errTol = 0.049606282877419791144113503378321F; + + if ( ( x < 0.0F ) || ( y < 0.0F ) || ( z < 0.0F ) || ( ( x + y ) < loLim ) || + ( ( x + z ) < loLim ) || ( ( y + z) < loLim )|| ( p < loLim ) ) { + result = QFFM_NAN; + } + else { + const float c0 = 1.0F/4.0F; + const float c1 = 3.0F/14.0F; + const float c2 = 1.0F/3.0F; + const float c3 = 3.0F/22.0F; + const float c4 = 3.0F/26.0F; + float xn = x; + float yn = y; + float zn = z; + float pn = p; + float sigma = 0.0F; + float power4 = 1.0F; + float mu, xnDev, ynDev, znDev, pnDev; + float ea, eb, ec, e2, e3, s1, s2, s3; + const size_t maxIter = 100; + + for ( size_t iter = 0U ; iter < maxIter ; ++iter ) { + float abs_xnDev, abs_ynDev, abs_znDev, abs_pnDev, lambda, alpha1; + float alpha2, beta, epsilon; + float xRoot, yRoot, zRoot; + mu = 0.2F*( xn + yn + zn + ( 2.0F*pn ) ); + xnDev = ( mu - xn )/mu; + ynDev = ( mu - yn )/mu; + znDev = ( mu - zn )/mu; + pnDev = ( mu - pn )/mu; + abs_xnDev = qFFMath_Abs( xnDev ); + abs_ynDev = qFFMath_Abs( ynDev ); + abs_znDev = qFFMath_Abs( znDev ); + abs_pnDev = qFFMath_Abs( pnDev ); + epsilon = ( abs_xnDev > abs_ynDev ) ? abs_xnDev : abs_ynDev; + epsilon = ( abs_znDev > epsilon ) ? abs_znDev : epsilon; + epsilon = ( abs_pnDev > epsilon ) ? abs_pnDev : epsilon; + if ( epsilon < errTol ) { + break; + } + xRoot = qFFMath_Sqrt( xn ); + yRoot = qFFMath_Sqrt( yn ); + zRoot = qFFMath_Sqrt( zn ); + lambda = xRoot*( yRoot + zRoot ) + ( yRoot*zRoot ); + alpha1 = pn*( xRoot + yRoot + zRoot ) + xRoot*yRoot*zRoot; + alpha2 = alpha1*alpha1; + beta = pn*( pn + lambda )*( pn + lambda ); + sigma += power4*ellint_rc( alpha2, beta ); + power4 *= c0; + xn = c0*( xn + lambda ); + yn = c0*( yn + lambda ); + zn = c0*( zn + lambda ); + pn = c0*( pn + lambda ); + } + ea = xnDev*( ynDev + znDev ) + ynDev*znDev; + eb = xnDev*ynDev*znDev; + ec = pnDev*pnDev; + e2 = ea - 3.0F*ec; + e3 = eb + 2.0F*pnDev*( ea - ec ); + s1 = 1.0F + e2*( -c1 + 0.75F*c3*e2 - 1.5F*c4*e3 ); + s2 = eb*( 0.5F*c2 + pnDev*( -c3 - c3 + pnDev*c4 ) ); + s3 = pnDev*ea*( c2 - ( pnDev*c3 ) ) - ( c2*pnDev*ec ); + result = 3.0F*sigma + power4*( s1 + s2 + s3)/( mu * qFFMath_Sqrt( mu ) ); + } + + return result; +} +/*============================================================================*/ +float qFFMath_Comp_ellint_1( float k ) +{ + float y; + + if ( qFFMath_IsNaN( k ) || ( qFFMath_Abs( k ) >= 1.0F ) ) { + y = QFFM_NAN; + } + else { + y = ellint_rf( 0.0F, 1.0F - ( k*k ), 1.0F ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Comp_ellint_2( float k ) +{ + float y; + const float abs_k = qFFMath_Abs( k ); + + if ( qFFMath_IsNaN( k ) || ( abs_k > 1.0F ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 1.0F, abs_k ) ) { + y = 1.0F; + } + else { + const float kk = k*k; + const float one_m_kk = 1.0F - kk; + const float c13 = 1.0F/3.0F; + + y = ellint_rf( 0.0F, one_m_kk, 1.0F ) - c13*kk*ellint_rd( 0.0F, one_m_kk, 1.0F ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Comp_ellint_3( float k, + float nu ) +{ + float y; + const float abs_k = qFFMath_Abs( k ); + + if ( qFFMath_IsNaN( k ) || qFFMath_IsNaN( nu ) || ( abs_k > 1.0F ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 1.0F, nu ) ) { + y = QFFM_INFINITY; + } + else { + const float kk = k*k; + const float one_m_kk = 1.0F - kk; + const float c13 = 1.0F/3.0F; + + y = ellint_rf( 0.0F, one_m_kk, 1.0F ) + c13*nu*ellint_rj( 0.0F, one_m_kk, 1.0F, 1.0F - nu ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Ellint_1( float k, + float phi ) +{ + float y; + + if ( qFFMath_IsNaN( k ) || qFFMath_IsNaN( phi ) || ( qFFMath_Abs(k) > 1.0F ) ) { + y = QFFM_NAN; + } + else { + const float n = qFFMath_Floor( phi/QFFM_PI + 0.5F ); + const float phi_red = phi - n*QFFM_PI; + const float s = qFFMath_Sin( phi_red ); + const float c = qFFMath_Cos( phi_red ); + const float f = s*ellint_rf( c*c, 1.0F - k*k*s*s, 1.0F ); + if ( QFFM_FP_ZERO == qFFMath_FPClassify( n ) ) { + y = f; + } + else { + y = f + ( 2.0F*n*qFFMath_Comp_ellint_1( k ) ); + } + } + + return y; +} +/*============================================================================*/ +float qFFMath_Ellint_2( float k, + float phi ) +{ + float y; + + if ( qFFMath_IsNaN( k ) || qFFMath_IsNaN( phi ) || ( qFFMath_Abs( k ) > 1.0F ) ) { + y = QFFM_NAN; + } + else { + const float c13 = 1.0F/3.0F; + const float n = qFFMath_Floor( phi/QFFM_PI + 0.5F ); + const float phi_red = phi - n*QFFM_PI; + const float kk = k*k; + const float s = qFFMath_Sin( phi_red ); + const float ss = s*s; + const float sss = ss*s; + const float c = qFFMath_Cos( phi_red ); + const float cc = c*c; + const float tmp = 1.0F - kk*ss; + const float e = s*ellint_rf( cc, tmp, 1.0F ) + - c13*kk*sss*ellint_rd( cc, tmp, 1.0F ); + + if ( QFFM_FP_ZERO == qFFMath_FPClassify( n ) ) { + y = e; + } + else { + y = e + ( 2.0F*n*qFFMath_Comp_ellint_2( k ) ); + } + } + + return y; +} +/*============================================================================*/ +float qFFMath_Ellint_3( float k, + float nu, + float phi ) +{ + float y; + + if ( qFFMath_IsNaN( k ) || qFFMath_IsNaN( nu ) || qFFMath_IsNaN( phi ) || ( qFFMath_Abs( k ) > 1.0F ) ) { + y = QFFM_NAN; + } + else { + const float n = qFFMath_Floor( phi/QFFM_PI + 0.5F ); + const float phi_red = phi - n*QFFM_PI; + const float kk = k*k; + const float s = qFFMath_Sin( phi_red ); + const float ss = s*s; + const float sss = ss*s; + const float c = qFFMath_Cos( phi_red ); + const float cc = c*c; + const float tmp = 1.0F - kk*ss; + const float c13 = 1.0F/3.0F; + const float pi = s*ellint_rf( cc, tmp, 1.0F ) + c13*nu*sss*ellint_rj( cc, tmp, 1.0F, 1.0F - nu*ss ); + + if ( QFFM_FP_ZERO == qFFMath_FPClassify( n ) ) { + y = pi; + } + else { + y = pi + ( 2.0F*n*qFFMath_Comp_ellint_3( k, nu ) ); + } + } + + return y; +} +/*============================================================================*/ +static float expint_E1_series( float x ) +{ + float term = 1.0F; + float eSum = 0.0F; + float oSum = 0.0F; + const size_t maxIter = 1000U; + + for ( size_t i = 1U; i < maxIter; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float j = (float)i; + /*cstat +CERT-FLP36-C*/ + term *= -x/j; + if ( qFFMath_Abs( term ) < FLT_EPSILON ) { + break; + } + if ( term >= 0.0F ) { + eSum += term/j; + } + else { + oSum += term/j; + } + } + return - eSum - oSum - QFFM_GAMMA_E - qFFMath_Log( x ); +} +/*============================================================================*/ +static float expint_E1_asymp( float x ) +{ + float term = 1.0F; + float eSum = 1.0F; + float oSum = 0.0F; + const size_t maxIter = 1000U; + + for ( size_t i = 1U; i < maxIter; ++i ) { + const float prev = term; + /*cstat -CERT-FLP36-C*/ + term *= -( (float)i )/x; + /*cstat +CERT-FLP36-C*/ + if ( qFFMath_Abs( term ) > qFFMath_Abs( prev ) ) { + break; + } + if ( term >= 0.0F ) { + eSum += term; + } + else { + oSum += term; + } + } + return qFFMath_Exp( -x )*( eSum + oSum )/x; +} +/*============================================================================*/ +static float expint_Ei_asymp( float x ) +{ + float term = 1.0F; + float sum = 1.0F; + const size_t maxIter = 1000U; + + for ( size_t i = 1U; i < maxIter; ++i ) { + const float prev = term; + /*cstat -CERT-FLP36-C*/ + term *= -( (float)i )/x; + /*cstat +CERT-FLP36-C*/ + if ( ( term < FLT_EPSILON ) || ( term >= prev ) ) { + break; + } + sum += term; + } + + return qFFMath_Exp( x )*sum/x; +} +/*============================================================================*/ +static float expint_E1( float x ) +{ + float y; + + if ( x < 0.0F ) { + y = -expint_Ei( -x ); + } + else if ( x < 1.0F ) { + y = expint_E1_series( x ); + } + else if ( x < 100.F ) { + y = expint_En_cont_frac( 1, x ); + } + else { + y = expint_E1_asymp( x ); + } + + return y; +} +/*============================================================================*/ +static float expint_En_cont_frac( size_t n, + float x ) +{ + float y = QFFM_NAN; + const int maxIter = 1000; + const int nm1 = (int)n - 1; + /*cstat -CERT-FLP36-C*/ + float b = x + (float)n; + /*cstat +CERT-FLP36-C*/ + float c = 1.0F/FLT_MIN; + float d = 1.0F/b; + float h = d; + + for ( int i = 1; i <= maxIter; ++i ) { + /*cstat -MISRAC++2008-5-0-7 -CERT-FLP36-C*/ + const float a = -( (float)( i*( nm1 + i ) ) ); + /*cstat MISRAC++2008-5-0-7 +CERT-FLP36-C*/ + b += 2.0F; + d = 1.0F/( ( a*d ) + b ); + c = b + ( a/c ); + const float del = c*d; + h *= del; + if ( qFFMath_Abs( del - 1.0F ) < FLT_EPSILON ) { + y = h*qFFMath_Exp( -x ); + break; + } + } + + return y; +} +/*============================================================================*/ +static float expint_Ei_series( float x ) +{ + float term = 1.0F; + float sum = 0.0F; + const size_t maxIter = 1000U; + + for ( size_t i = 1U; i < maxIter; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float j = (float)i; + /*cstat +CERT-FLP36-C*/ + term *= x/j; + sum += term/j; + if ( term < ( FLT_EPSILON*sum ) ) { + break; + } + } + return QFFM_GAMMA_E + sum + qFFMath_Log( x ); +} +/*============================================================================*/ +static float expint_Ei( float x ) +{ + const float logEps = 36.044F; + float y; + + if ( x < 0.0F ) { + y = -expint_E1( -x ); + } + else if ( x < logEps ) { + y = expint_Ei_series( x ); + } + else { + y = expint_Ei_asymp( x ); + } + + return y; +} +/*============================================================================*/ +float qFFMath_Expint( float num ) +{ + return ( qFFMath_IsNaN( num ) ) ? QFFM_NAN : expint_Ei( num ); +} +/*============================================================================*/ +float qFFMath_Hermite( size_t n, + float x ) +{ + float y = 0.0F; + + if ( qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else { + const float H_0 = 1.0F; + + if ( 0U == n ) { + y= H_0; + } + else { + const float H_1 = 2.0F*x; + if ( 1U == n ) { + y = H_1; + } + else { + float H_nm1, H_nm2; + + H_nm2 = H_0; + H_nm1 = H_1; + for ( size_t i = 2U; i <= n; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float j = (float)( i - 1U ); + /*cstat +CERT-FLP36-C*/ + y = 2.0F*( ( x*H_nm1 ) - ( j*H_nm2 ) ); + H_nm2 = H_nm1; + H_nm1 = y; + } + } + } + } + + return y; +} +/*============================================================================*/ +float qFFMath_Laguerre( size_t n, + float x ) +{ + return qFFMath_Assoc_laguerre( n, 0, x ); +} +/*============================================================================*/ +float qFFMath_Legendre( size_t n, + float x ) +{ + float y = 0.0F; + + if ( qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 1.0F, x ) ) { + y = 1.0F; + } + else if ( qFFMath_IsEqual( -1.0F, x ) ) { + y = ( ( n % 2 ) == 1 ) ? -1.0F : 1.0F; + } + else { + float p_lm2 = 1.0F; + + if ( 0 == n ) { + y = p_lm2; + } + else { + float p_lm1 = x; + + if ( 1 == n ) { + y = p_lm1; + } + else { + for ( size_t ll = 2U ; ll <= n; ++ll ) { + /*cstat -CERT-FLP36-C*/ + const float ll_f = (float)ll; + /*cstat +CERT-FLP36-C*/ + y = ( 2.0F*x*p_lm1 ) - p_lm2 - ( x*p_lm1 - p_lm2)/ll_f; + p_lm2 = p_lm1; + p_lm1 = y; + } + } + } + } + + return y; +} +/*============================================================================*/ +static float riemann_zeta_glob( float s ) +{ + const float maxBinCoeff = 86.4982335337F; + float zeta = 0.0F; + const float ss = s; + bool neg = false; + + if ( s < 0.0F ) { + s = 1.0F - s; + neg = true; + } + float num = 0.5F; + const size_t maxIt = 10000U; + /*cstat -MISRAC++2008-6-6-4*/ + for ( size_t i = 0U ; i < maxIt; ++i ) { + bool punt = false; + float sgn = 1.0F; + float term = 0.0F; + for ( size_t j = 0U ; j <= i ; ++j ) { + /*cstat -CERT-FLP36-C*/ + const float ii = (float)i; + const float jj = (float)j; + /*cstat +CERT-FLP36-C*/ + float bin_coeff = qFFMath_LGamma( 1.0F + ii ) - + qFFMath_LGamma( 1.0F + jj ) - + qFFMath_LGamma( 1.0F + ii - jj ); + + if ( bin_coeff > maxBinCoeff ) { + punt = true; + break; + } + bin_coeff = qFFMath_Exp( bin_coeff ); + term += sgn*bin_coeff*qFFMath_Pow( 1.0F + jj, -s ); + sgn *= -1.0F; + } + if ( punt ) { + break; + } + term *= num; + zeta += term; + if ( qFFMath_Abs( term/zeta ) < FLT_EPSILON ) { + break; + } + num *= 0.5F; + } + /*cstat +MISRAC++2008-6-6-4*/ + zeta /= 1.0F - qFFMath_Pow( 2.0F, 1.0F - s ); + + if ( neg ) { + zeta *= qFFMath_Pow( 2.0F*QFFM_PI, ss )* + qFFMath_Sin( QFFM_PI_2*ss )* + qFFMath_Exp( qFFMath_LGamma( s ) )/QFFM_PI; + } + return zeta; +} +/*============================================================================*/ +static float riemann_zeta_product( float s ) +{ + static const uint8_t prime[ 29 ] = { + 2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53, 59, 61, 67, + 71, 73, 79, 83, 89, 97, 101, 103, 107, 109 + }; + static const size_t num_primes = sizeof(prime)/sizeof(float); + float zeta = 1.0F; + + for ( size_t i = 0U ; i < num_primes; ++i ) { + const float f = 1.0F - qFFMath_Pow( (float)prime[ i ], -s ); + + zeta *= f; + if ( ( 1.0F - f ) < FLT_EPSILON ) { + break; + } + } + zeta = 1.0F/zeta; + + return zeta; +} +/*============================================================================*/ +float qFFMath_Riemann_zeta( float s ) +{ + float z; + + if ( qFFMath_IsNaN( s ) ) { + z = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 1.0F, s ) ) { + z = QFFM_INFINITY; + } + else if ( s < -19.0F ) { + z = riemann_zeta_product( 1.0F - s ); + z *= qFFMath_Pow( 2.0F*QFFM_PI, s )* + qFFMath_Sin( QFFM_PI_2*s )* + qFFMath_Exp( qFFMath_LGamma( 1.0F - s ) )/ + QFFM_PI; + } + else if ( s < 20.0F ) { + z = riemann_zeta_glob( s ); + } + else { + z = riemann_zeta_product( s ); + } + + return z; +} +/*============================================================================*/ +static void gamma_temme( float mu, + float* gam1, + float* gam2, + float* gam_pl, + float* gam_mi) +{ + + gam_pl[ 0 ] = 1.0F/qFFMath_TGamma( 1.0F + mu ); + gam_mi[ 0 ] = 1.0F/qFFMath_TGamma( 1.0F - mu ); + + if ( qFFMath_Abs( mu ) < FLT_EPSILON ) { + gam1[ 0 ] = -QFFM_GAMMA_E; + } + else { + gam1[ 0 ] = ( gam_mi[ 0 ] - gam_pl[ 0 ] )/( 2.0F*mu ); + } + gam2[ 0 ] = 0.5F*( gam_mi[ 0 ] + gam_pl[ 0 ] ); +} +/*============================================================================*/ +static void bessel_jn( float nu, + float x, + float* j_nu, + float* n_nu, + float* j_pnu, + float* n_pnu ) +{ + if ( qFFMath_IsEqual( 0.0F, x ) ) { + if ( qFFMath_IsEqual( 0.0F, nu ) ) { + j_nu[ 0 ] = 1.0F; + j_pnu[ 0 ] = 0.0F; + } + else if ( qFFMath_IsEqual( 1.0F, nu ) ) { + j_nu[ 0 ] = 0.0F; + j_pnu[ 0 ] = 0.5F; + } + else { + j_nu[ 0 ] = 0.0F; + j_pnu[ 0 ] = 0.0F; + } + n_nu[ 0 ] = -QFFM_INFINITY; + n_pnu[ 0 ] = QFFM_INFINITY; + } + else { + const float eps = FLT_EPSILON; + const float fp_min = 1.08420217256745973463717809E-19F; + const int max_iter = 15000; + const float x_min = 2.0F; + /*cstat -CERT-FLP34-C*/ + const int tmp_nl = (int)( nu - x + 1.5F ); + const int nl = ( x < x_min ) ? (int)( nu + 0.5F ) + : ( ( tmp_nl > 0 )? tmp_nl : 0 ); + /*cstat +CERT-FLP34-C -CERT-FLP36-C*/ + const float mu = nu - (float)nl; + /*cstat +CERT-FLP36-C*/ + const float mu2 = mu*mu; + const float xi = 1.0F/x; + const float xi2 = 2.0F*xi; + const float w = xi2/QFFM_PI; + float iSign = 1.0F; + float h = nu*xi; + if ( h < fp_min ) { + h = fp_min; + } + float b = xi2*nu; + float d = 0.0F; + float c = h; + for ( int i = 1; i <= max_iter; ++i ) { + b += xi2; + d = b - d; + if ( qFFMath_Abs( d ) < fp_min ) { + d = fp_min; + } + c = b - ( 1.0F/c ); + if ( qFFMath_Abs( c ) < fp_min ) { + c = fp_min; + } + d = 1.0F / d; + + const float del = c * d; + h *= del; + if ( d < 0.0F ) { + iSign = -iSign; + } + if ( qFFMath_Abs( del - 1.0F ) < eps ) { + break; + } + } + float j_nul = iSign*fp_min; + float j_pnu_l = h*j_nul; + const float j_nul1 = j_nul; + const float j_pnu1 = j_pnu_l; + float fact = nu*xi; + + for ( int l = nl; l >= 1; --l ) { + const float j_nu_temp = ( fact*j_nul ) + j_pnu_l; + + fact -= xi; + j_pnu_l = ( fact*j_nu_temp ) - j_nul; + j_nul = j_nu_temp; + } + if ( qFFMath_IsEqual( 0.0F, j_nul ) ) { + j_nul = eps; + } + const float f = j_pnu_l/j_nul; + float n_mu, n_nu1, n_pmu, Jmu; + if ( x < x_min ) { + const float x2 = 0.5F*x; + const float pi_mu = QFFM_PI*mu; + const float fact_l = ( qFFMath_Abs( pi_mu ) < eps ) ? 1.0F : pi_mu/qFFMath_Sin( pi_mu ); + d = -qFFMath_Log( x2 ); + float e = mu*d; + const float fact2 = ( qFFMath_Abs( e ) < eps ) ? 1.0F : qFFMath_Sinh(e)/e; + float gam1, gam2, gam_pl, gam_mi; + + gamma_temme( mu, &gam1, &gam2, &gam_pl, &gam_mi ); + float ff = ( 2.0F/QFFM_PI )*fact_l*( gam1*qFFMath_Cosh( e ) + ( gam2*fact2*d ) ); + e = qFFMath_Exp( e ); + float p = e/( QFFM_PI*gam_pl ); + float q = 1.0F/( e*QFFM_PI*gam_mi ); + const float pi_mu2 = pi_mu / 2.0F; + const float fact3 = ( qFFMath_Abs( pi_mu2 ) < eps ) ? 1.0F : qFFMath_Sin( pi_mu2 )/pi_mu2; + const float r = QFFM_PI*pi_mu2*fact3*fact3; + float sum = ff + ( r*q ); + float sum1 = p; + c = 1.0F; + d = -x2*x2; + for ( int i = 1; i <= max_iter ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float j = (float)i; + /*cstat +CERT-FLP36-C*/ + ff = ( j*ff + p + q )/( j*j - mu2 ); + c *= d/j; + p /= j - mu; + q /= j + mu; + const float del = c*( ff + ( r*q ) ); + sum += del; + const float del1 = ( c*p ) - ( j*del ); + sum1 += del1; + if ( qFFMath_Abs( del ) < ( eps*( 1.0F + qFFMath_Abs( sum ) ) ) ) { + break; + } + } + + n_mu = -sum; + n_nu1 = -sum1*xi2; + n_pmu = ( mu*xi*n_mu ) - n_nu1; + Jmu = w/( n_pmu - ( f*n_mu ) ); + } + else { + float a = 0.25F - mu2; + float q = 1.0F; + float p = -xi*0.5F; + const float br = 2.0F*x; + float bi = 2.0F; + float fact_g = a*xi/( ( p*p ) + ( q*q ) ); + float cr = br + ( q*fact_g ); + float ci = bi + ( p*fact_g ); + float den = ( br*br ) + ( bi*bi ); + float dr = br/den; + float di = -bi/den; + float dlr = ( cr*dr ) - ( ci*di ); + float dli = ( cr*di ) + ( ci*dr ); + float temp = ( p*dlr ) - ( q*dli ); + q = ( p*dli ) + ( q * dlr ); + p = temp; + + for ( int i = 2; i <= max_iter; ++i ) { + a += (float)( 2*( i - 1 ) ); + bi += 2.0F; + dr = ( a*dr ) + br; + di = ( a*di ) + bi; + if ( ( qFFMath_Abs( dr ) + qFFMath_Abs( di ) ) < fp_min ) { + dr = fp_min; + } + fact_g = a/( ( cr*cr ) + ( ci*ci ) ); + cr = br + ( cr*fact_g ); + ci = bi - ( ci*fact_g ); + if ( ( qFFMath_Abs( cr ) + qFFMath_Abs( ci ) ) < fp_min ) { + cr = fp_min; + } + den = ( dr*dr ) + ( di*di ); + dr /= den; + di /= -den; + dlr = ( cr*dr ) - ( ci*di ); + dli = ( cr*di ) + ( ci*dr ); + temp = ( p*dlr ) - ( q*dli ); + q = ( p*dli ) + ( q*dlr ); + p = temp; + if ( qFFMath_Abs( dlr - 1.0F ) + qFFMath_Abs( dli ) < eps ) { + break; + } + } + const float gam = ( p - f )/q; + + Jmu = qFFMath_Sqrt( w/( ( p - f )*gam + q ) ); + if ( ( Jmu*j_nul ) < 0.0F) { + Jmu = -Jmu; + } + n_mu = gam*Jmu; + n_pmu = ( p + ( q/gam ) )*n_mu; + n_nu1 = ( mu*xi*n_mu ) - n_pmu; + } + fact = Jmu/j_nul; + j_nu[ 0 ] = fact*j_nul1; + j_pnu[ 0 ] = fact*j_pnu1; + for ( int i = 1; i <= nl; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float n_nu_temp = ( mu + (float)i )*xi2*n_nu1 - n_mu; + /*cstat +CERT-FLP36-C*/ + n_mu = n_nu1; + n_nu1 = n_nu_temp; + } + n_nu[ 0 ] = n_mu; + n_pnu[ 0 ] = ( nu*xi*n_mu ) - n_nu1; + } +} +/*============================================================================*/ +static void sph_bessel_jn( size_t n, + float x, + float* j_n, + float* n_n, + float* jp_n, + float* np_n ) +{ + /*cstat -CERT-FLP36-C*/ + const float nu = (float)n + 0.5F; + /*cstat +CERT-FLP36-C*/ + const float sqrtpi2 = 1.25331413731550012080617761967005208134651184F; + float j_nu, n_nu, jp_nu, np_nu; + const float factor = sqrtpi2*qFFMath_RSqrt( x ); + const float inv_2x = 1.0F/( 2.0F*x ); + + bessel_jn( nu, x, &j_nu, &n_nu, &jp_nu, &np_nu ); + j_n[ 0 ] = factor*j_nu; + n_n[ 0 ] = factor*n_nu; + jp_n[ 0 ] = ( factor*jp_nu ) - ( j_n[ 0 ]*inv_2x ); + np_n[ 0 ] = ( factor*np_nu ) - ( n_n[ 0 ]*inv_2x ); +} +/*============================================================================*/ +float qFFMath_Sph_bessel( size_t n, + float x ) +{ + float y; + + if ( ( x < 0.0F ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 0.0F, x ) ) { + y = ( 0U == n ) ? 1.0F : 0.0F; + } + else { + float j_n, n_n, jp_n, np_n; + + sph_bessel_jn( n, x, &j_n, &n_n, &jp_n, &np_n ); + y = j_n; + } + + return y; +} +/*============================================================================*/ +float qFFMath_Sph_neumann( size_t n, + float x ) +{ + float y; + + if ( ( x < 0.0F ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( qFFMath_IsEqual( 0.0F, x ) ) { + y = -QFFM_INFINITY; + } + else { + float j_n, n_n, jp_n, np_n; + + sph_bessel_jn( n, x, &j_n, &n_n, &jp_n, &np_n ); + y = n_n; + } + return y; +} +/*============================================================================*/ +static void bessel_ik( float nu, + float x, + float* i_nu, + float* k_nu, + float* i_pnu, + float* k_pnu ) +{ + if ( qFFMath_IsEqual( 0.0F, x ) ) { + if ( qFFMath_IsEqual( 0.0F, nu ) ) { + i_nu[ 0 ] = 1.0F; + i_pnu[ 0 ] = 0.0F; + } + else if ( qFFMath_IsEqual( 1.0F, x ) ) { + i_nu[ 0 ] = 0.0F; + i_pnu[ 0 ] = 0.5F; + } + else { + i_nu[ 0 ] = 0.0F; + i_pnu[ 0 ] = 0.0F; + } + k_nu[ 0 ] = QFFM_INFINITY; + k_pnu[ 0 ] = -QFFM_INFINITY; + } + else { + const float eps = FLT_EPSILON; + const float fp_min = 10.0F*FLT_EPSILON; + const int max_iter = 15000; + const float x_min = 2.0F; + /*cstat -CERT-FLP34-C -CERT-FLP36-C*/ + const int nl = (int)( nu + 0.5F ); + const float mu = nu - (float)nl; + /*cstat +CERT-FLP34-C +CERT-FLP36-C*/ + const float mu2 = mu*mu; + const float xi = 1.0F/x; + const float xi2 = 2.0F*xi; + float h = nu*xi; + + if ( h < fp_min ) { + h = fp_min; + } + float b = xi2*nu; + float d = 0.0F; + float c = h; + + for ( int i = 1; i <= max_iter; ++i ) { + b += xi2; + d = 1.0F/( b + d ); + c = b + ( 1.0F/c ); + const float del = c*d; + h *= del; + if ( qFFMath_Abs( del - 1.0F ) < eps ) { + break; + } + } + + float i_nul = fp_min; + float i_pnu_l = h*i_nul; + const float i_nul1 = i_nul; + const float i_pnu_1 = i_pnu_l; + float fact_m = nu*xi; + + for ( int l = nl ; l >= 1 ; --l ) { + const float i_nu_temp = ( fact_m*i_nul ) + i_pnu_l; + + fact_m -= xi; + i_pnu_l = ( fact_m*i_nu_temp ) + i_nul; + i_nul = i_nu_temp; + } + const float f = i_pnu_l/i_nul; + float Kmu, k_nu1; + + if ( x < x_min ) { + const float x2 = 0.5F*x; + const float pi_mu = QFFM_PI*mu; + const float fact = ( qFFMath_Abs( pi_mu ) < eps ) ? 1.0F : pi_mu/qFFMath_Sin( pi_mu ); + d = -qFFMath_Log( x2 ); + float e = mu*d; + const float fact2 = ( qFFMath_Abs( e ) < eps ) ? 1.0F : qFFMath_Sinh( e )/e ; + float gam1, gam2, gam_pl, gam_mi; + gamma_temme(mu, &gam1, &gam2, &gam_pl, &gam_mi); + float ff = fact*( ( gam1*qFFMath_Cosh( e ) ) + ( gam2*fact2*d ) ); + float sum = ff; + e = qFFMath_Exp( e ); + float p = e/( 2.0F*gam_pl ); + float q = 1.0F/( 2.0F*e*gam_mi ); + float sum1 = p; + c = 1.0F; + d = x2*x2; + + for ( int i = 1; i <= max_iter; ++i ) { + const float j = (float)i; + ff = ( j*ff + p + q )/( j*j - mu2 ); + c *= d/j; + p /= j - mu; + q /= j + mu; + const float del = c*ff; + sum += del; + sum1 += c*( p - ( j*ff ) ); + if ( qFFMath_Abs( del ) < ( eps*qFFMath_Abs( sum ) ) ) { + break; + } + } + + Kmu = sum; + k_nu1 = sum1*xi2; + } + else { + float del_h = d; + float q1 = 0.0F; + float q2 = 1.0F; + const float a1 = 0.25F - mu2; + float q = a1; + float a = -a1; + float s = 1.0F + ( q*del_h ); + + b = 2.0F*( 1.0F + x ); + d = 1.0F/b; + h = d; + c = a1; + for ( int i = 2 ; i <= max_iter; ++i) { + a -= (float)( 2*( i - 1 ) ); + c = -a*c/( (float)i ); + const float q_new = ( q1 - ( b*q2 ) )/a; + q1 = q2; + q2 = q_new; + q += c*q_new; + b += 2.0F; + d = 1.0F/( b + ( a*d ) ); + del_h = ( ( b*d ) - 1.0F )*del_h; + h += del_h; + const float del_s = q*del_h; + s += del_s; + if ( qFFMath_Abs( del_s/s ) < eps ) { + break; + } + } + h = a1*h; + Kmu = qFFMath_Sqrt( QFFM_PI/( 2.0F*x ) )*qFFMath_Exp(-x)/s; + k_nu1 = Kmu*( mu + x + 0.5F - h )*xi; + } + const float k_pmu = ( mu*xi*Kmu ) - k_nu1; + const float i_num_u = xi/( ( f*Kmu ) - k_pmu ); + + i_nu[ 0 ] = i_num_u*i_nul1/i_nul; + i_pnu[ 0 ] = i_num_u*i_pnu_1/i_nul; + for ( int i = 1 ; i <= nl ; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float k_nu_temp = ( ( mu + (float)i )*xi2*k_nu1 ) + Kmu; + /*cstat +CERT-FLP36-C*/ + Kmu = k_nu1; + k_nu1 = k_nu_temp; + } + k_nu[ 0 ] = Kmu; + k_pnu[ 0 ] = ( nu*xi*Kmu ) - k_nu1; + } +} +/*============================================================================*/ +static float cyl_bessel_ij_series( float nu, + float x, + float sgn, + size_t max_iter ) +{ + float y; + + if ( qFFMath_IsEqual( 0.0F, x ) ) { + y = ( qFFMath_IsEqual( 0.0F, nu ) ) ? 1.0F : 0.0F; + } + else { + const float x2 = 0.5F*x; + float fact = nu*qFFMath_Log( x2 ); + float Jn = 1.0F; + float term = 1.0F; + const float xx4 = sgn*x2*x2; + + fact -= qFFMath_LGamma( nu + 1.0F ); + fact = qFFMath_Exp( fact ); + for ( size_t i = 1U ; i < max_iter; ++i ) { + /*cstat -CERT-FLP36-C*/ + const float j = (float)i; + /*cstat +CERT-FLP36-C*/ + term *= xx4/( j*( nu + j ) ); + Jn += term; + if ( qFFMath_Abs( term/Jn ) < FLT_EPSILON ) { + break; + } + } + y = fact*Jn; + } + + return y; +} +/*============================================================================*/ +float qFFMath_Cyl_bessel_i( float nu, + float x ) +{ + float y; + + if ( ( nu < 0.0F ) || ( x < 0.0F ) || qFFMath_IsNaN( nu ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( ( x*x ) < ( 10.0F*( nu + 1.0F ) ) ) { + y = cyl_bessel_ij_series( nu, x, 1.0F, 200U ); + } + else { + float I_nu, K_nu, Ip_nu, Kp_nu; + + bessel_ik( nu, x, &I_nu, &K_nu, &Ip_nu, &Kp_nu ); + y = I_nu; + } + return y; +} +/*============================================================================*/ +static void cyl_bessel_jn_asymp( float nu, + float x, + float * Jnu, + float * Nnu) +{ + const float mu = 4.0F*nu*nu; + const float x8 = 8.0F*x; + float P = 0.0F; + float Q = 0.0F; + float term = 1.0F; + const float eps = FLT_EPSILON; + size_t i = 0U; + + do { + bool epsP, epsQ; + float k2_1; + /*cstat -MISRAC++2008-0-1-2_b*/ + float k = (float)i; + /*cstat +MISRAC++2008-0-1-2_b*/ + k2_1 = 2.0F*k - 1.0F; + term *= ( i == 0U ) ? 1.0F : -( mu - ( k2_1*k2_1 ) )/( k*x8 ); + epsP = qFFMath_Abs( term ) < ( eps*qFFMath_Abs( P ) ); + P += term; + ++i; + k = (float)i; + k2_1 = 2.0F*k - 1.0F; + term *= ( mu - ( k2_1*k2_1 ) )/( k*x8 ); + epsQ = qFFMath_Abs( term ) < ( eps*qFFMath_Abs( Q ) ); + Q += term; + if ( epsP && epsQ && ( k > ( 0.5F*nu ) ) ) { + break; + } + ++i; + } while ( i < 1000U ); + const float chi = x - ( nu + 0.5F )*QFFM_PI_2; + const float c = qFFMath_Cos( chi ); + const float s = qFFMath_Sin( chi ); + const float coeff = qFFMath_Sqrt( 2.0F/( QFFM_PI*x ) ); + Jnu[ 0 ] = coeff*( ( c*P ) - ( s*Q ) ); + Nnu[ 0 ] = coeff*( ( s*P ) + ( c*Q ) );; +} +/*============================================================================*/ +float qFFMath_Cyl_bessel_j( float nu, + float x ) +{ + float y; + + if ( ( nu < 0.0F ) || ( x < 0.0F ) || qFFMath_IsNaN( nu ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( ( x*x ) < ( 10.0F*( nu + 1.0F ) ) ) { + y = cyl_bessel_ij_series( nu, x, -1.0F, 200U ); + } + else if ( x > 1000.0F ) { + float j_nu, n_nu; + + cyl_bessel_jn_asymp( nu, x, &j_nu, &n_nu ); + y = j_nu; + } + else { + float J_nu, N_nu, Jp_nu, Np_nu; + + bessel_jn( nu, x, &J_nu, &N_nu, &Jp_nu, &Np_nu ); + y = J_nu; + } + return y; +} +/*============================================================================*/ +float qFFMath_Cyl_bessel_k( float nu, + float x ) +{ + float y; + + if ( ( nu < 0.0F ) || ( x < 0.0F ) || qFFMath_IsNaN( nu ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else { + float I_nu, K_nu, Ip_nu, Kp_nu; + + bessel_ik( nu, x, &I_nu, &K_nu, &Ip_nu, &Kp_nu ); + y = K_nu; + } + return y; +} +/*============================================================================*/ +float qFFMath_Cyl_neumann( float nu, + float x ) +{ + float y; + + if ( ( nu < 0.0F ) || ( x < 0.0F ) || qFFMath_IsNaN( nu ) || qFFMath_IsNaN( x ) ) { + y = QFFM_NAN; + } + else if ( x > 1000.0F ) { + float J_nu, N_nu; + cyl_bessel_jn_asymp( nu, x, &J_nu, &N_nu ); + y = N_nu; + } + else { + float J_nu, N_nu, Jp_nu, Np_nu; + + bessel_jn( nu, x, &J_nu, &N_nu, &Jp_nu, &Np_nu ); + y = N_nu; + } + return y; +} +/*============================================================================*/ +float qFFMath_Sph_legendre( size_t l, + size_t m, + float theta ) +{ + float y; + + if ( qFFMath_IsNaN( theta ) ) { + y = QFFM_NAN; + } + else { + const float x = qFFMath_Cos( theta ); + const float pi4 = 4.0F*QFFM_PI; + + if ( m > l ) { //skipcq : CXX-W2041 + y = 0.0F; + } + else if ( 0U == m ) { + float P = qFFMath_Legendre( l, x ); + /*cstat -CERT-FLP36-C*/ + const float fact = qFFMath_Sqrt( (float)( 2U*l + 1U )/pi4 ); + /*cstat +CERT-FLP36-C*/ + P *= fact; + y = P; + } + else if ( qFFMath_IsEqual( 1.0F, x ) || qFFMath_IsEqual( -1.0F, x ) ) { + y = 0.0F; + } + else { + /*cstat -CERT-FLP36-C*/ + const float mf = (float)m; + const float y_mp1m_factor = x*qFFMath_Sqrt( (float)( 2U*m + 3U ) ); + /*cstat +CERT-FLP36-C*/ + const float sgn = ( 1U == ( m % 2U ) ) ? -1.0F : 1.0F; + const float ln_circ = qFFMath_Log( 1.0F - ( x*x ) ); + const float ln_poc_h = qFFMath_LGamma( mf + 0.5F ) - qFFMath_LGamma( mf ); + const float ln_pre_val = ( -0.25F*QFFM_LN_PI ) + 0.5F*( ln_poc_h + ( mf*ln_circ ) ); + const float sr = qFFMath_Sqrt( ( 2.0F + ( 1.0F/mf ) )/pi4); + float y_mm = sgn*sr*qFFMath_Exp( ln_pre_val ); + float y_mp1m = y_mp1m_factor*y_mm; + + if ( l == m ) { + y = y_mm; + } + else if ( l == ( m + 1U ) ) { + y = y_mp1m; + } + else { + float y_lm = 0.0F; + + for ( size_t ll = ( m + 2U ) ; ll <= l; ++ll ) { + /*cstat -CERT-FLP36-C*/ + const float ll_m_m = (float)( ll - m ); + const float ll_p_m = (float)( ll + m ); + const float ll2_p_1 = (float)( ( 2U*ll ) + 1U ); + const float ll2_m_1 = (float)( ( 2U*ll ) - 1U ); + const float ll_pm_m1 = (float)( ll + m - 1U ); + const float ll_mm_m1 = (float)( ll - m - 1U ); + /*cstat +CERT-FLP36-C*/ + const float rat1 = ll_m_m/ll_p_m; + const float fact1 = qFFMath_Sqrt( rat1*ll2_p_1*ll2_m_1 ); + /*cstat -CERT-FLP36-C*/ + const float fact2 = qFFMath_Sqrt( rat1*( ll_mm_m1/ll_pm_m1 )*ll2_p_1/(float)( 2U*ll - 3U ) ); + /*cstat -CERT-FLP36-C*/ + y_lm = ( x*y_mp1m*fact1 - ll_pm_m1*y_mm*fact2 )/ll_m_m; + y_mm = y_mp1m; + y_mp1m = y_lm; + } + y = y_lm; + } + } + } + return y; +} #endif /*#ifndef QLIBS_USE_STD_MATH*/ \ No newline at end of file