Hiroshi M / GyroSensor

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?

UserRevisionLine numberNew 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 }