#include <stdio.h>
#define int32_t int
#define int16_t short
#define uint16_t unsigned short
#define uint32_t unsigned int
#define int64_t long
#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))
short steerRateFixdt=0;
short speedRateFixdt=0;
int steerFixdt=0;
int speedFixdt=0;
short steer=0;
short speed=0;
/* =========================== Filtering Functions =========================== */
/* Low pass filter fixed-point 32 bits: fixdt(1,32,16)
* Max: 32767.99998474121
* Min: -32768
* Res: 1.52587890625e-05
*
* Inputs: u = int16 or int32
* Outputs: y = fixdt(1,32,16)
* Parameters: coef = fixdt(0,16,16) = [0,65535U]
*
* Example:
* If coef = 0.8 (in floating point), then coef = 0.8 * 2^16 = 52429 (in fixed-point)
* filtLowPass16(u, 52429, &y);
* yint = (int16_t)(y >> 16); // the integer output is the fixed-point ouput shifted by 16 bits
*/
void filtLowPass32(int32_t u, uint16_t coef, int32_t *y) {
int64_t tmp;
tmp = ((int64_t)((u << 4) - (*y >> 12)) * coef) >> 4;
tmp = CLAMP(tmp, -2147483648LL, 2147483647LL); // Overflow protection: 2147483647LL = 2^31 - 1
*y = (int32_t)tmp + (*y);
}
// Old filter
// Inputs: u = int16
// Outputs: y = fixdt(1,32,20)
// Parameters: coef = fixdt(0,16,16) = [0,65535U]
// yint = (int16_t)(y >> 20); // the integer output is the fixed-point ouput shifted by 20 bits
// void filtLowPass32(int16_t u, uint16_t coef, int32_t *y) {
// int32_t tmp;
// tmp = (int16_t)(u << 4) - (*y >> 16);
// tmp = CLAMP(tmp, -32768, 32767); // Overflow protection
// *y = coef * tmp + (*y);
// }
/* rateLimiter16(int16_t u, int16_t rate, int16_t *y);
* Inputs: u = int16
* Outputs: y = fixdt(1,16,4)
* Parameters: rate = fixdt(1,16,4) = [0, 32767] Do NOT make rate negative (>32767)
*/
void rateLimiter16(int16_t u, int16_t rate, int16_t *y) {
int16_t q0;
int16_t q1;
q0 = (u << 4) - *y;
if (q0 > rate) {
q0 = rate;
} else {
q1 = -rate;
if (q0 < q1) {
q0 = q1;
}
}
*y = q0 + *y;
}
int main () {
//JSRUN引擎2.0,支持多达30种语言在线运行,全仿真在线交互输入输出。
printf("Hello world! - c.jsrun.net.");
// ####### LOW-PASS FILTER #######
for(int i=0;i<40;i++){
rateLimiter16(500, 300, &steerRateFixdt);
rateLimiter16(500, 300, &speedRateFixdt);
filtLowPass32(steerRateFixdt >> 4, 65535, &steerFixdt);
filtLowPass32(speedRateFixdt >> 4, 65535, &speedFixdt);
steer = (int16_t)(steerFixdt >> 16); // convert fixed-point to integer
speed = (int16_t)(speedFixdt >> 16); // convert fixed-point to
printf(" %d %d \r\n",steer,speed);
}
return 0;
}