Gyro Sensor Library
Diff: util.c
- Revision:
- 0:da10b9c9db7a
diff -r 000000000000 -r da10b9c9db7a util.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/util.c Mon Jul 16 03:42:01 2012 +0000 @@ -0,0 +1,78 @@ +/** + ***************************************************************************** + * File Name : util.c + * + * Title : Utility functions Source File + * Revision : 0.1 + * Notes : + * Target Board : mbed NXP LPC1768 + * Tool Chain : ???? + * + * Revision History: + * When Who Description of change + * ----------- ----------- ----------------------- + * 2012/07/10 Hiroshi M init + ***************************************************************************** + * + * Copyright (C) 2012 Hiroshi M, MIT License + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software + * and associated documentation files (the "Software"), to deal in the Software without restriction, + * including without limitation the rights to use, copy, modify, merge, publish, distribute, + * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or + * substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING + * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + **/ + +/* Includes ----------------------------------------------------------------- */ +#include "util.h" + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function prototypes -----------------------------------------------*/ + +/** + * least squares method + * + */ +int lsm(int *y, int x) +{ + int n; + static long a0, a1, a2; + long s01 = 0, s02 = 0, s11 = 0, s12 = 0; + + if (x == 0) { + for (n = 0; n < N; n++) { + s01 += n; + s02 += (long)y[n]; + s11 += n * n; + s12 += n * (long)y[n]; + } + + a0 = (s02 * s11 - s01 * s12) / (N * s11 - s01 * s01); + a1 = (N * s12 - s01 * s02); + a2 = (N * s11 - s01 * s01 * 2); + } + return (int)(a0 + a1 * (N + x) / a2); +} + + +int xabs(int x,int y) +{ + if(x >= y) { + return (x - y); + } else { + return (y - x); + } +} \ No newline at end of file