Gyro Sensor Library
util.c@0:da10b9c9db7a, 2012-07-16 (annotated)
- Committer:
- bant62
- Date:
- Mon Jul 16 03:42:01 2012 +0000
- Revision:
- 0:da10b9c9db7a
init revision
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bant62 | 0:da10b9c9db7a | 1 | /** |
bant62 | 0:da10b9c9db7a | 2 | ***************************************************************************** |
bant62 | 0:da10b9c9db7a | 3 | * File Name : util.c |
bant62 | 0:da10b9c9db7a | 4 | * |
bant62 | 0:da10b9c9db7a | 5 | * Title : Utility functions Source File |
bant62 | 0:da10b9c9db7a | 6 | * Revision : 0.1 |
bant62 | 0:da10b9c9db7a | 7 | * Notes : |
bant62 | 0:da10b9c9db7a | 8 | * Target Board : mbed NXP LPC1768 |
bant62 | 0:da10b9c9db7a | 9 | * Tool Chain : ???? |
bant62 | 0:da10b9c9db7a | 10 | * |
bant62 | 0:da10b9c9db7a | 11 | * Revision History: |
bant62 | 0:da10b9c9db7a | 12 | * When Who Description of change |
bant62 | 0:da10b9c9db7a | 13 | * ----------- ----------- ----------------------- |
bant62 | 0:da10b9c9db7a | 14 | * 2012/07/10 Hiroshi M init |
bant62 | 0:da10b9c9db7a | 15 | ***************************************************************************** |
bant62 | 0:da10b9c9db7a | 16 | * |
bant62 | 0:da10b9c9db7a | 17 | * Copyright (C) 2012 Hiroshi M, MIT License |
bant62 | 0:da10b9c9db7a | 18 | * |
bant62 | 0:da10b9c9db7a | 19 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software |
bant62 | 0:da10b9c9db7a | 20 | * and associated documentation files (the "Software"), to deal in the Software without restriction, |
bant62 | 0:da10b9c9db7a | 21 | * including without limitation the rights to use, copy, modify, merge, publish, distribute, |
bant62 | 0:da10b9c9db7a | 22 | * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is |
bant62 | 0:da10b9c9db7a | 23 | * furnished to do so, subject to the following conditions: |
bant62 | 0:da10b9c9db7a | 24 | * |
bant62 | 0:da10b9c9db7a | 25 | * The above copyright notice and this permission notice shall be included in all copies or |
bant62 | 0:da10b9c9db7a | 26 | * substantial portions of the Software. |
bant62 | 0:da10b9c9db7a | 27 | * |
bant62 | 0:da10b9c9db7a | 28 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING |
bant62 | 0:da10b9c9db7a | 29 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
bant62 | 0:da10b9c9db7a | 30 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
bant62 | 0:da10b9c9db7a | 31 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
bant62 | 0:da10b9c9db7a | 32 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
bant62 | 0:da10b9c9db7a | 33 | * |
bant62 | 0:da10b9c9db7a | 34 | **/ |
bant62 | 0:da10b9c9db7a | 35 | |
bant62 | 0:da10b9c9db7a | 36 | /* Includes ----------------------------------------------------------------- */ |
bant62 | 0:da10b9c9db7a | 37 | #include "util.h" |
bant62 | 0:da10b9c9db7a | 38 | |
bant62 | 0:da10b9c9db7a | 39 | /* Private typedef ---------------------------------------------------------- */ |
bant62 | 0:da10b9c9db7a | 40 | /* Private define ----------------------------------------------------------- */ |
bant62 | 0:da10b9c9db7a | 41 | /* Private macro ------------------------------------------------------------ */ |
bant62 | 0:da10b9c9db7a | 42 | /* Private variables -------------------------------------------------------- */ |
bant62 | 0:da10b9c9db7a | 43 | /* Private function prototypes -----------------------------------------------*/ |
bant62 | 0:da10b9c9db7a | 44 | |
bant62 | 0:da10b9c9db7a | 45 | /** |
bant62 | 0:da10b9c9db7a | 46 | * least squares method |
bant62 | 0:da10b9c9db7a | 47 | * |
bant62 | 0:da10b9c9db7a | 48 | */ |
bant62 | 0:da10b9c9db7a | 49 | int lsm(int *y, int x) |
bant62 | 0:da10b9c9db7a | 50 | { |
bant62 | 0:da10b9c9db7a | 51 | int n; |
bant62 | 0:da10b9c9db7a | 52 | static long a0, a1, a2; |
bant62 | 0:da10b9c9db7a | 53 | long s01 = 0, s02 = 0, s11 = 0, s12 = 0; |
bant62 | 0:da10b9c9db7a | 54 | |
bant62 | 0:da10b9c9db7a | 55 | if (x == 0) { |
bant62 | 0:da10b9c9db7a | 56 | for (n = 0; n < N; n++) { |
bant62 | 0:da10b9c9db7a | 57 | s01 += n; |
bant62 | 0:da10b9c9db7a | 58 | s02 += (long)y[n]; |
bant62 | 0:da10b9c9db7a | 59 | s11 += n * n; |
bant62 | 0:da10b9c9db7a | 60 | s12 += n * (long)y[n]; |
bant62 | 0:da10b9c9db7a | 61 | } |
bant62 | 0:da10b9c9db7a | 62 | |
bant62 | 0:da10b9c9db7a | 63 | a0 = (s02 * s11 - s01 * s12) / (N * s11 - s01 * s01); |
bant62 | 0:da10b9c9db7a | 64 | a1 = (N * s12 - s01 * s02); |
bant62 | 0:da10b9c9db7a | 65 | a2 = (N * s11 - s01 * s01 * 2); |
bant62 | 0:da10b9c9db7a | 66 | } |
bant62 | 0:da10b9c9db7a | 67 | return (int)(a0 + a1 * (N + x) / a2); |
bant62 | 0:da10b9c9db7a | 68 | } |
bant62 | 0:da10b9c9db7a | 69 | |
bant62 | 0:da10b9c9db7a | 70 | |
bant62 | 0:da10b9c9db7a | 71 | int xabs(int x,int y) |
bant62 | 0:da10b9c9db7a | 72 | { |
bant62 | 0:da10b9c9db7a | 73 | if(x >= y) { |
bant62 | 0:da10b9c9db7a | 74 | return (x - y); |
bant62 | 0:da10b9c9db7a | 75 | } else { |
bant62 | 0:da10b9c9db7a | 76 | return (y - x); |
bant62 | 0:da10b9c9db7a | 77 | } |
bant62 | 0:da10b9c9db7a | 78 | } |