rtest.cpp

Sample codes to change orientation using 3-Axis accel. chip, SMB380

// rtest.cpp : Defines the entry point for the application.
//

#include "stdafx.h"
#include "rtest.h"
#include <commctrl.h>
#include "smb380.h"
#include "smb380_ioctl.h"
#include "math.h"
#include <celog.h>
#include <afx.h>

#define MAX_LOADSTRING 100
#undef DEBUG

// Global Variables:
HINSTANCE			hInst;			// The current instance
HWND				hwndCB;			// The command bar handle

// Forward declarations of functions included in this code module:
ATOM				MyRegisterClass	(HINSTANCE, LPTSTR);
BOOL				InitInstance	(HINSTANCE, int);
LRESULT CALLBACK	WndProc			(HWND, UINT, WPARAM, LPARAM);
LRESULT CALLBACK	About			(HWND, UINT, WPARAM, LPARAM);


void RotateScreen(smb380acc_t acc_val)
{
  #define ERROR_OFFSET 38
  #define ACC_STABLE_VAL 50
  #define ACC_STABLE_COUNT 2
  static int m_nPrevPicturePosition = DMDO_0;
  static int m_nPicturePosition = DMDO_0;
  static int m_nPrevAccStable = 0;
  static int m_nAccStable = 0;
  static int m_nAccStableCount = 0;

  m_nPrevAccStable = m_nAccStable;
  m_nAccStable = abs(acc_val.x) + abs(acc_val.y) + abs(acc_val.z);

  if(abs(m_nAccStable - m_nPrevAccStable) < ACC_STABLE_VAL)
    m_nAccStableCount++;
  else
    m_nAccStableCount = 0;

  if(m_nAccStableCount < ACC_STABLE_COUNT)
    return;

  if((abs(acc_val.x) - abs(acc_val.y)) < ERROR_OFFSET
    && (abs(acc_val.x) - abs(acc_val.y)) > -ERROR_OFFSET)
    return;

  if(abs(acc_val.x) > abs(acc_val.y))
  {
    if(acc_val.x < -ERROR_OFFSET)
      m_nPicturePosition = DMDO_90;
    
    if(acc_val.x > ERROR_OFFSET)
      m_nPicturePosition = DMDO_270;
  }
  else 
  {
    if(acc_val.y < -ERROR_OFFSET)
      m_nPicturePosition = DMDO_180;
    
    if(acc_val.y > ERROR_OFFSET)
      m_nPicturePosition = DMDO_0;
  }

  if(m_nPicturePosition != m_nPrevPicturePosition)
  {
	  DEVMODE DevMode;
      // Set new orientation
	  memset(&DevMode, 0, sizeof (DevMode));
      DevMode.dmSize               = sizeof (DevMode);
      DevMode.dmFields             = DM_DISPLAYORIENTATION;
      DevMode.dmDisplayOrientation = m_nPicturePosition;

      if (DISP_CHANGE_SUCCESSFUL == ChangeDisplaySettingsEx(NULL, &DevMode, NULL, CDS_RESET, NULL))
      {
         RETAILMSG(1, (L"ChangeDisplaySettingsEx changed rotation angle to %d\n", m_nPicturePosition));
      }
      else
      {
         RETAILMSG(1, (L"ChangeDisplaySettingsEx failed to change the rotation angle to %d\n", m_nPicturePosition));
      }
  }

  m_nPrevPicturePosition = m_nPicturePosition;
}


void InitGSensor(void)
{
	if (smb380_init() == -1)
	{
		RETAILMSG(1, (L"SMB380 device init failed\n"));
	}
	else
	{
		//RETAILMSG(1, (L"SMB380 device init success\n"));
		smb380_write_ee(0x2b, 0x0);
		smb380_write_ee(0x35, 0x80);
		smb380_soft_reset();
		Sleep(50);
	}

	// Get Chip information
	read_reg tmp;
 	tmp.addr = 0x0;
	tmp.len = 1;
	byte data = 0x0;
	smb380_read_reg(&tmp, &data);
	data = (byte)((int)data & (int)(0x07));
	RETAILMSG(1, (TEXT("InitGSensor: Chip ID %x\n"), data));

	tmp.addr = 0x1;
	tmp.len = 1;
	data = 0x0;
	smb380_read_reg(&tmp, &data);
	RETAILMSG(1, (TEXT("InitGSensor: ML Version %x\n"), (byte)((int)data & (int)(0xf))));

	tmp.addr = 0x1;
	tmp.len = 1;
	data = 0x0;
	smb380_read_reg(&tmp, &data);
	RETAILMSG(1, (TEXT("InitGSensor: AL Version %x\n"), (byte)(((int)data & (int)(0xf0))>>4)));
}


void GoGSensor(void)
{
	static double acc_range = 2.0;

	double PI = 3.1415926535;

	smb380_set_range(SMB380_RANGE_2G); 

	/* set bandwidth to 25 HZ */
	smb380_set_bandwidth(SMB380_BW_25HZ);

	smb380acc_t acctmp;
	acctmp.x = 0;
	acctmp.y = 0;
	acctmp.z = 0;

	if (smb380_read_accel_xyzt(&acctmp) == -1)
	{
		RETAILMSG(1, (L"SMB380 device get xyz failed\n"));
	}
	else
	{
		double roll = 0;
		double pitch = 0;
		double yaw = 0;

		double tmpx = (((double)acctmp.x) * acc_range) / (double)512;
		double tmpy = (((double)acctmp.y) * acc_range) / (double)512;
		double tmpz = (((double)acctmp.z) * acc_range) / (double)512;
#ifdef DEBUG
		RETAILMSG(1, (L"========== acc.x = %d\n", acctmp.x));
		RETAILMSG(1, (L"========== acc.y = %d\n", acctmp.y));
		RETAILMSG(1, (L"========== acc.z = %d\n", acctmp.z));
#endif

		roll = atan(tmpx / tmpz);
		if (tmpz < 0.0)
		{
			roll += (double)PI * ((tmpx > 0.0) ? 1 : -1);
		}
		roll *= -1;

		pitch = atan(tmpy / tmpz);
		if (tmpz < 0.0)
		{
			pitch += (double)PI * ((tmpy > 0.0) ? 1 : -1);
		}
		pitch *= -1;

#ifdef DEBUG
		char tdata[16];
		CString cs_out;

		sprintf(tdata, "%f", pitch); cs_out = tdata;
		RETAILMSG(1, (L"========== pitch = %s\n", cs_out));
		sprintf(tdata, "%f", roll); cs_out = tdata;
		RETAILMSG(1, (L"========== roll = %s\n", cs_out));

		sprintf(tdata, "%f", tmpx); cs_out = tdata;
		RETAILMSG(1, (L"========== tmpx = %s\n", cs_out));
		sprintf(tdata, "%f", tmpy); cs_out = tdata;
		RETAILMSG(1, (L"========== tmpy = %s\n", cs_out));
		sprintf(tdata, "%f", tmpz); cs_out = tdata;
		RETAILMSG(1, (L"========== tmpz = %s\n", cs_out));
		sprintf(tdata, "%f", sqrt(tmpx*tmpx + tmpy*tmpy + tmpz*tmpz)); cs_out = tdata;
		RETAILMSG(1, (L"========== tmpG = %s\n", cs_out));
#endif
		RotateScreen(acctmp);
	}

	byte temperaturetmp = 0;
	if (smb380_read_temperature(&temperaturetmp) == -1)
	{
		RETAILMSG(1, (L"SMB380 device get tmeperature failed\n"));
	}
	else
	{
		double tmp = (double) (temperaturetmp) * 0.5 - 30.0;
#ifdef DEBUG
		char tdata[16];
		CString cs_out;
		sprintf(tdata, "%f", tmp); cs_out = tdata;
		RETAILMSG(1, (L"========== Temperature = %d\n", temperaturetmp));
		RETAILMSG(1, (L"========== Temp = %s\n", cs_out));
#endif
	}
}


int WINAPI WinMain(	HINSTANCE hInstance,
					HINSTANCE hPrevInstance,
					LPTSTR    lpCmdLine,
					int       nCmdShow)
{
	MSG msg;
	HACCEL hAccelTable;

	// Perform application initialization:
	if (!InitInstance (hInstance, nCmdShow)) 
	{
		return FALSE;
	}

	hAccelTable = LoadAccelerators(hInstance, (LPCTSTR)IDC_RTEST);

	InitGSensor();

	// Main message loop:
	while (GetMessage(&msg, NULL, 0, 0)) 
	{
		if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg)) 
		{
			TranslateMessage(&msg);
			DispatchMessage(&msg);
		}
	}

	return msg.wParam;
}

//
//  FUNCTION: MyRegisterClass()
//
//  PURPOSE: Registers the window class.
//
//  COMMENTS:
//
//    It is important to call this function so that the application 
//    will get 'well formed' small icons associated with it.
//
ATOM MyRegisterClass(HINSTANCE hInstance, LPTSTR szWindowClass)
{
	WNDCLASS	wc;

    wc.style			= CS_HREDRAW | CS_VREDRAW;
    wc.lpfnWndProc		= (WNDPROC) WndProc;
    wc.cbClsExtra		= 0;
    wc.cbWndExtra		= 0;
    wc.hInstance		= hInstance;
    wc.hIcon			= LoadIcon(hInstance, MAKEINTRESOURCE(IDI_RTEST));
    wc.hCursor			= 0;
    wc.hbrBackground	= (HBRUSH) GetStockObject(WHITE_BRUSH);
    wc.lpszMenuName		= 0;
    wc.lpszClassName	= szWindowClass;

	return RegisterClass(&wc);
}

//
//  FUNCTION: InitInstance(HANDLE, int)
//
//  PURPOSE: Saves instance handle and creates main window
//
//  COMMENTS:
//
//    In this function, we save the instance handle in a global variable and
//    create and display the main program window.
//
BOOL InitInstance(HINSTANCE hInstance, int nCmdShow)
{
	HWND	hWnd;
	TCHAR	szTitle[MAX_LOADSTRING];			// The title bar text
	TCHAR	szWindowClass[MAX_LOADSTRING];		// The window class name

	hInst = hInstance;		// Store instance handle in our global variable
	// Initialize global strings
	LoadString(hInstance, IDC_RTEST, szWindowClass, MAX_LOADSTRING);
	MyRegisterClass(hInstance, szWindowClass);

	LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
	//hWnd = CreateWindow(szWindowClass, szTitle, WS_VISIBLE,
	//	CW_USEDEFAULT, CW_USEDEFAULT, CW_USEDEFAULT, CW_USEDEFAULT, NULL, NULL, hInstance, NULL);
	hWnd = CreateWindow(szWindowClass, szTitle, WS_VISIBLE,
		50, 50, 250, 150, NULL, NULL, hInstance, NULL);

	if (!hWnd)
	{	
		return FALSE;
	}

	ShowWindow(hWnd, SW_HIDE);
	UpdateWindow(hWnd);
	if (hwndCB)
		CommandBar_Show(hwndCB, TRUE);

	return TRUE;
}

//
//  FUNCTION: WndProc(HWND, unsigned, WORD, LONG)
//
//  PURPOSE:  Processes messages for the main window.
//
//  WM_COMMAND	- process the application menu
//  WM_PAINT	- Paint the main window
//  WM_DESTROY	- post a quit message and return
//
//
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
	HDC hdc;
	int wmId, wmEvent;
	PAINTSTRUCT ps;
	TCHAR szHello[MAX_LOADSTRING];

	switch (message) 
	{
		case WM_COMMAND:
			wmId    = LOWORD(wParam); 
			wmEvent = HIWORD(wParam); 
			// Parse the menu selections:
			switch (wmId)
			{
				case IDM_HELP_ABOUT:
				   DialogBox(hInst, (LPCTSTR)IDD_ABOUTBOX, hWnd, (DLGPROC)About);
				   break;
				case IDM_FILE_EXIT:
				   DestroyWindow(hWnd);
				   break;
				default:
				   return DefWindowProc(hWnd, message, wParam, lParam);
			}
			break;
		case WM_CREATE:
			hwndCB = CommandBar_Create(hInst, hWnd, 1);			
			CommandBar_InsertMenubar(hwndCB, hInst, IDM_MENU, 0);
			CommandBar_AddAdornments(hwndCB, 0, 0);
			// Set timer to read sensor data, 25Hz sample rate
			SetTimer(hWnd, 1, 40, NULL);
			break;
		case WM_TIMER:
			GoGSensor();
			break;
		case WM_PAINT:
			RECT rt;
			hdc = BeginPaint(hWnd, &ps);
			GetClientRect(hWnd, &rt);
			LoadString(hInst, IDS_HELLO, szHello, MAX_LOADSTRING);
			DrawText(hdc, szHello, _tcslen(szHello), &rt, 
				DT_SINGLELINE | DT_VCENTER | DT_CENTER);
			EndPaint(hWnd, &ps);
			break;
		case WM_DESTROY:
			CommandBar_Destroy(hwndCB);
			PostQuitMessage(0);
			break;
		default:
			return DefWindowProc(hWnd, message, wParam, lParam);
   }
   return 0;
}

// Mesage handler for the About box.
LRESULT CALLBACK About(HWND hDlg, UINT message, WPARAM wParam, LPARAM lParam)
{
	RECT rt, rt1;
	int DlgWidth, DlgHeight;	// dialog width and height in pixel units
	int NewPosX, NewPosY;

	switch (message)
	{
		case WM_INITDIALOG:
			// trying to center the About dialog
			if (GetWindowRect(hDlg, &rt1)) {
				GetClientRect(GetParent(hDlg), &rt);
				DlgWidth	= rt1.right - rt1.left;
				DlgHeight	= rt1.bottom - rt1.top ;
				NewPosX		= (rt.right - rt.left - DlgWidth)/2;
				NewPosY		= (rt.bottom - rt.top - DlgHeight)/2;
				
				// if the About box is larger than the physical screen 
				if (NewPosX < 0) NewPosX = 0;
				if (NewPosY < 0) NewPosY = 0;
				SetWindowPos(hDlg, 0, NewPosX, NewPosY,
					0, 0, SWP_NOZORDER | SWP_NOSIZE);
			}
			return TRUE;

		case WM_COMMAND:
			if ((LOWORD(wParam) == IDOK) || (LOWORD(wParam) == IDCANCEL))
			{
				EndDialog(hDlg, LOWORD(wParam));
				return TRUE;
			}
			break;
	}
    return FALSE;
}