rotation_sense.c

来自「low_g sensor ratation code」· C语言 代码 · 共 78 行

/*********************************************************************
 
  (c) copyright Freescale Semiconductor Ltd. 2006
  ALL RIGHTS RESERVED
 
 *********************************************************************
 
 *********************************************************************

  File:				    Rotation_sense.c
 
  Description:		    Photo rotation Algorithm
  
  Date:        		 July 2007
  Author:			    Vincent Ko


 Condition:
 1.5g selection
 CPU Bus = no specific requirement
 Interrupt period = no specific requirement    
 ********************************************************************/
#include "rotation_sense.h"
#include <stdlib.h>

typedef unsigned char   UINT8;      //!<unsigned 8 bit definition 
typedef unsigned int    UINT16;     //!<unsigned 16 bit definition
typedef unsigned long   UINT32;     //!<unsigned 32 bit definition
typedef signed char     INT8;       //!<signed 8 bit definition 
typedef int             INT16;      //!<signed 16 bit definition
typedef long int        INT32;      //!<signed 32 bit definition

#define  ONE_G          63
#define  SIN30          30          // 30 degree
#define  NEG_SIN30     -30          // -30 degree
#define  TRACKINGDELAY  12

typedef struct {
   ORIENTATION gReportOri,gPendingOri;
   UINT8 gTime;
} RotVar;

RotVar *__far gRotPtr;
#define ReportOri       gRotPtr->gReportOri
#define PendingOri      gRotPtr->gPendingOri
#define Time            gRotPtr->gTime

void rotation_init(void *__far ptr) {
   gRotPtr = ptr;
   ReportOri = FLAT;
   PendingOri = FLAT;
   Time = TRACKINGDELAY;  
}

ORIENTATION rotation_sense (signed char x, signed char y, signed char z) {   
   ORIENTATION ThisOri;
   if ((x>SIN30)&&((UINT8)abs(y)<SIN30)) ThisOri = XUP;
   else if ((x<NEG_SIN30)&&((UINT8)abs(y)<SIN30)) ThisOri = XDOWN;
   else if (((UINT8)abs(x)<SIN30)&&(y>SIN30)) ThisOri = YUP;
   else if (((UINT8)abs(x)<SIN30)&&(y<NEG_SIN30)) ThisOri = YDOWN;
   else ThisOri = FLAT;
   
   if ((ThisOri!=ReportOri)&&(Time==TRACKINGDELAY)) PendingOri = ThisOri;
   
   if (ThisOri==PendingOri) {
      Time--;
   } else {
      Time = TRACKINGDELAY;
   }
   
   if (!Time) {
      ReportOri = PendingOri;
      Time = TRACKINGDELAY;
   }
   
   return ReportOri;
}