2015-02-14 05:11:43 -08:00
# ifndef MATH_H
# define MATH_H
2013-09-23 05:23:34 -07:00
2023-11-05 14:10:08 -08:00
# include <stdint.h>
2023-10-06 00:10:20 -07:00
# include "globals.h"
2022-02-17 19:55:43 -08:00
# ifdef USE_LIBDIVIDE
2023-11-05 14:10:08 -08:00
// We use pre-computed constant parameters with libdivide where possible.
// Using predefined constants saves flash and RAM (.bss) versus calling the
// libdivide generator functions (E.g. libdivide_s32_gen)
// 32-bit constants generated here: https://godbolt.org/z/vP8Kfejo9
2022-02-17 19:55:43 -08:00
# include "src/libdivide/libdivide.h"
2023-11-05 14:10:08 -08:00
# include "src/libdivide/constant_fast_div.h"
2022-02-17 19:55:43 -08:00
# endif
2023-11-05 14:10:08 -08:00
extern uint8_t random1to100 ( void ) ;
/**
* @ defgroup group - rounded - div Rounding integer division
*
* @ brief Integer division returns the quotient . I . e . rounds to zero . This
* code will round the result to nearest integer . Rounding behavior is
* controlled by # DIV_ROUND_BEHAVIOR
*
* @ {
*/
/**
* @ defgroup group - rounded - div - behavior Rounding behavior
* @ {
*/
/** @brief Rounding behavior: always round down */
# define DIV_ROUND_DOWN -1
/** @brief Rounding behavior: always round up */
# define DIV_ROUND_UP 1
/** @brief Rounding behavior: round to nearest
*
* This rounds 0.5 away from zero . This is the same behavior
* as the standard library round ( ) function .
*/
# define DIV_ROUND_NEAREST 0
/** @brief Integer division rounding behavior. */
# define DIV_ROUND_BEHAVIOR DIV_ROUND_NEAREST
// (Unit tests expect DIV_ROUND_NEAREST behavior)
/**
* @ brief Computes the denominator correction for rounding division
* based on our rounding behavior .
*
* @ param d The divisor ( an integer )
* @ param t The type of the result . E . g . uint16_t
*/
# define DIV_ROUND_CORRECT(d, t) ((t)(((d)>>1U)+(t)DIV_ROUND_BEHAVIOR))
///@}
/**
* @ brief Rounded integer division
*
* Integer division returns the quotient . I . e . rounds to zero . This
* macro will round the result to nearest integer . Rounding behavior
* is controlled by # DIV_ROUND_BEHAVIOR
*
* @ warning For performance reasons , this macro does not promote integers .
* So it will overflow if n > MAX ( t ) - ( d / 2 ) .
*
* @ param n The numerator ( dividee ) ( an integer )
* @ param d The denominator ( divider ) ( an integer )
* @ param t The type of the result . E . g . uint16_t
*/
# define DIV_ROUND_CLOSEST(n, d, t) ( \
( ( ( n ) < ( t ) ( 0 ) ) ^ ( ( d ) < ( t ) ( 0 ) ) ) ? \
( ( t ) ( ( n ) - DIV_ROUND_CORRECT ( d , t ) ) / ( t ) ( d ) ) : \
( ( t ) ( ( n ) + DIV_ROUND_CORRECT ( d , t ) ) / ( t ) ( d ) ) )
/**
* @ brief Rounded \ em unsigned integer division
*
2024-03-20 17:37:45 -07:00
* This is slightly faster than the signed version ( DIV_ROUND_CLOSEST ( n , d , t ) )
2023-11-05 14:10:08 -08:00
*
* @ warning For performance reasons , this macro does not promote integers .
* So it will overflow if n > MAX ( t ) - ( d / 2 ) .
*
* @ param n The numerator ( dividee ) ( an \ em unsigned integer )
* @ param d The denominator ( divider ) ( an \ em unsigned integer )
* @ param t The type of the result . E . g . uint16_t
*/
# define UDIV_ROUND_CLOSEST(n, d, t) ((t)((n) + DIV_ROUND_CORRECT(d, t)) / (t)(d))
/**
* @ brief Rounded arithmetic right shift
*
* Right shifting throws away bits . When use for fixed point division , this
2024-03-20 17:37:45 -07:00
* effectively rounds down ( towards zero ) . To round - to - the - nearest - integer
2023-11-05 14:10:08 -08:00
* when right - shifting by S , just add in 2 power S − 1 ( which is the
* fixed - point equivalent of 0.5 ) first
*
* @ param n The value to shift
* @ param s The shift distance in bits
*/
# define RSHIFT_ROUND(n, s) (((n)+(1UL<<(s-1UL)))>>(s))
///@}
/** @brief Test whether the parameter is an integer or not. */
# define IS_INTEGER(d) ((d) == (int32_t)(d))
/**
2024-03-20 17:37:45 -07:00
* @ defgroup group - div100 Optimised integer division by 100
2023-11-05 14:10:08 -08:00
* @ {
*/
static inline uint16_t div100 ( uint16_t n ) {
// As of avr-gcc 5.4.0, the compiler will optimize this to a multiply/shift
// (unlike the signed integer overload, where __divmodhi4 is still called
// see https://godbolt.org/z/c5bs5noT1)
return UDIV_ROUND_CLOSEST ( n , UINT16_C ( 100 ) , uint16_t ) ;
2022-02-17 19:55:43 -08:00
}
2023-11-05 14:10:08 -08:00
static inline int16_t div100 ( int16_t n ) {
# ifdef USE_LIBDIVIDE
// Try faster unsigned path first
if ( n > 0 ) {
return div100 ( ( uint16_t ) n ) ;
}
// Negative values here, so adjust pre-division to get same
// behavior as roundf(float)
return libdivide : : libdivide_s16_do_raw ( n - DIV_ROUND_CORRECT ( UINT16_C ( 100 ) , uint16_t ) , S16_MAGIC ( 100 ) , S16_MORE ( 100 ) ) ;
2022-02-17 19:55:43 -08:00
# else
2023-11-05 14:10:08 -08:00
return DIV_ROUND_CLOSEST ( n , UINT16_C ( 100 ) , int16_t ) ;
2022-02-17 19:55:43 -08:00
# endif
}
2023-11-05 14:10:08 -08:00
static inline uint32_t div100 ( uint32_t n ) {
# ifdef USE_LIBDIVIDE
if ( n < = ( uint32_t ) UINT16_MAX ) {
return div100 ( ( uint16_t ) n ) ;
}
return libdivide : : libdivide_u32_do_raw ( n + DIV_ROUND_CORRECT ( UINT32_C ( 100 ) , uint32_t ) , 2748779070L , 6 ) ;
2022-02-17 19:55:43 -08:00
# else
2023-11-05 14:10:08 -08:00
return UDIV_ROUND_CLOSEST ( n , UINT32_C ( 100 ) , uint32_t ) ;
2022-02-17 19:55:43 -08:00
# endif
}
2023-11-05 14:10:08 -08:00
2024-03-22 04:48:01 -07:00
# if defined(__arm__)
2023-11-05 14:10:08 -08:00
static inline int div100 ( int n ) {
return DIV_ROUND_CLOSEST ( n , 100U , int ) ;
2022-02-17 19:55:43 -08:00
}
# else
2023-11-05 14:10:08 -08:00
static inline int32_t div100 ( int32_t n ) {
2022-02-17 19:55:43 -08:00
# ifdef USE_LIBDIVIDE
2023-11-05 14:10:08 -08:00
if ( n < = INT16_MAX & & n > = INT16_MIN ) {
return div100 ( ( int16_t ) n ) ;
}
return libdivide : : libdivide_s32_do_raw ( n + ( DIV_ROUND_CORRECT ( UINT16_C ( 100 ) , uint32_t ) * ( n < 0 ? - 1 : 1 ) ) , 1374389535L , 5 ) ;
2022-02-17 19:55:43 -08:00
# else
2024-03-20 17:45:10 -07:00
return DIV_ROUND_CLOSEST ( n , INT32_C ( 100 ) , int32_t ) ;
2022-02-17 19:55:43 -08:00
# endif
}
# endif
2023-11-05 14:10:08 -08:00
///@}
2022-02-17 19:55:43 -08:00
2023-11-05 14:10:08 -08:00
/**
* @ brief Optimised integer division by 360
*
* @ param n The numerator ( dividee ) ( an integer )
* @ return uint32_t
*/
static inline uint32_t div360 ( uint32_t n ) {
2022-02-17 19:55:43 -08:00
# ifdef USE_LIBDIVIDE
2023-11-05 14:10:08 -08:00
return libdivide : : libdivide_u32_do_raw ( n + DIV_ROUND_CORRECT ( UINT32_C ( 360 ) , uint32_t ) , 1813430637L , 72 ) ;
# else
return ( uint32_t ) UDIV_ROUND_CLOSEST ( n , UINT32_C ( 360 ) , uint32_t ) ;
# endif
}
/**
* @ brief Integer based percentage calculation .
*
* @ param percent The percent to calculate ( [ 0 , 100 ] )
* @ param value The value to operate on
* @ return uint32_t
*/
2024-01-15 21:22:56 -08:00
static inline uint32_t percentage ( uint8_t percent , uint32_t value )
{
return ( uint32_t ) div100 ( ( uint32_t ) value * ( uint32_t ) percent ) ;
2023-11-05 14:10:08 -08:00
}
2024-01-15 21:22:56 -08:00
2023-11-05 14:10:08 -08:00
/**
* @ brief Integer based half - percentage calculation .
*
* @ param percent The percent to calculate ( [ 0 , 100 ] )
* @ param value The value to operate on
* @ return uint16_t
*/
static inline uint16_t halfPercentage ( uint8_t percent , uint16_t value ) {
uint32_t x200 = ( uint32_t ) percent * ( uint32_t ) value ;
# ifdef USE_LIBDIVIDE
return ( uint16_t ) libdivide : : libdivide_u32_do_raw ( x200 + DIV_ROUND_CORRECT ( UINT32_C ( 200 ) , uint32_t ) , 2748779070L , 7 ) ;
2022-02-17 19:55:43 -08:00
# else
2023-11-05 14:10:08 -08:00
return ( uint16_t ) UDIV_ROUND_CLOSEST ( x200 , UINT16_C ( 200 ) , uint32_t ) ;
2022-02-17 19:55:43 -08:00
# endif
}
2015-02-24 18:13:35 -08:00
2023-11-05 14:10:08 -08:00
/**
* @ brief Make one pass at correcting the value into the range [ min , max )
*
* @ param min Minimum value ( inclusive )
* @ param max Maximum value ( exclusive )
* @ param value Value to nudge
* @ param nudgeAmount Amount to change value by
* @ return int16_t
*/
static inline int16_t nudge ( int16_t min , int16_t max , int16_t value , int16_t nudgeAmount )
{
if ( value < min ) { return value + nudgeAmount ; }
if ( value > max ) { return value - nudgeAmount ; }
return value ;
}
2017-08-20 18:51:05 -07:00
//This is a dedicated function that specifically handles the case of mapping 0-1023 values into a 0 to X range
//This is a common case because it means converting from a standard 10-bit analog input to a byte or 10-bit analog into 0-511 (Eg the temperature readings)
2023-11-05 14:10:08 -08:00
# define fastMap1023toX(x, out_max) ( ((uint32_t)(x) * (out_max)) >> 10)
2020-08-09 15:54:36 -07:00
//This is a new version that allows for out_min
2023-11-05 14:10:08 -08:00
# define fastMap10Bit(x, out_min, out_max) ( ( ((uint32_t)(x) * ((out_max)-(out_min))) >> 10 ) + (out_min))
# if defined(CORE_AVR) || defined(ARDUINO_ARCH_AVR)
static inline bool udiv_is16bit_result ( uint32_t dividend , uint16_t divisor ) {
return divisor > ( uint16_t ) ( dividend > > 16U ) ;
}
# endif
/**
* @ brief Optimised division : uint32_t / uint16_t = > uint16_t
*
* Optimised division of unsigned 32 - bit by unsigned 16 - bit when it is known
* that the result fits into unsigned 16 - bit .
*
* ~ 60 % quicker than raw 32 / 32 = > 32 division on ATMega
*
* @ note Bad things will likely happen if the result doesn ' t fit into 16 - bits .
* @ note Copied from https : //stackoverflow.com/a/66593564
*
* @ param dividend The dividend ( numerator )
* @ param divisor The divisor ( denominator )
* @ return uint16_t
*/
static inline uint16_t udiv_32_16 ( uint32_t dividend , uint16_t divisor )
{
# if defined(CORE_AVR) || defined(ARDUINO_ARCH_AVR)
if ( divisor = = 0U | | ! udiv_is16bit_result ( dividend , divisor ) ) { return UINT16_MAX ; }
# define INDEX_REG "r16"
asm (
" ldi " INDEX_REG " , 16 ; bits = 16 \n \t "
" 0: \n \t "
" lsl %A0 ; shift \n \t "
" rol %B0 ; rem:quot \n \t "
" rol %C0 ; left \n \t "
" rol %D0 ; by 1 \n \t "
" brcs 1f ; if carry out, rem > divisor \n \t "
" cp %C0, %A1 ; is rem less \n \t "
" cpc %D0, %B1 ; than divisor ? \n \t "
" brcs 2f ; yes, when carry out \n \t "
" 1: \n \t "
" sub %C0, %A1 ; compute \n \t "
" sbc %D0, %B1 ; rem -= divisor \n \t "
" ori %A0, 1 ; record quotient bit as 1 \n \t "
" 2: \n \t "
" dec " INDEX_REG " ; bits-- \n \t "
" brne 0b ; until bits == 0 "
: " =d " ( dividend )
: " d " ( divisor ) , " 0 " ( dividend )
: INDEX_REG
) ;
// Lower word contains the quotient, upper word contains the remainder.
return dividend & 0xFFFFU ;
# else
// The non-AVR platforms are all fast enough (or have built in hardware dividers)
// so just fall back to regular 32-bit division.
return dividend / divisor ;
# endif
}
/**
* @ brief Same as udiv_32_16 ( ) , except this will round to nearest integer
* instead of truncating .
*
* Minor performance drop compared to non - rounding version .
* */
static inline uint16_t udiv_32_16_closest ( uint32_t dividend , uint16_t divisor )
{
# if defined(CORE_AVR) || defined(ARDUINO_ARCH_AVR)
dividend = dividend + ( uint32_t ) ( DIV_ROUND_CORRECT ( divisor , uint16_t ) ) ;
return udiv_32_16 ( dividend , divisor ) ;
# else
return ( uint16_t ) UDIV_ROUND_CLOSEST ( dividend , ( uint32_t ) divisor , uint32_t ) ;
# endif
}
2017-08-16 20:51:12 -07:00
2020-08-09 15:54:36 -07:00
# endif