diff options
Diffstat (limited to 'utility/demo-fw/common/dfm_accelerometer.c')
| -rw-r--r-- | utility/demo-fw/common/dfm_accelerometer.c | 647 |
1 files changed, 647 insertions, 0 deletions
diff --git a/utility/demo-fw/common/dfm_accelerometer.c b/utility/demo-fw/common/dfm_accelerometer.c new file mode 100644 index 0000000..410fe91 --- /dev/null +++ b/utility/demo-fw/common/dfm_accelerometer.c @@ -0,0 +1,647 @@ +/* ----------------------------------------------------------------------------
+ * ATMEL Microcontroller Software Support
+ * ----------------------------------------------------------------------------
+ * Copyright (c) 2008, Atmel Corporation
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * - Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the disclaimer below.
+ *
+ * Atmel's name may not be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
+ * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
+ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ * ----------------------------------------------------------------------------
+ */
+ //------------------------------------------------------------------------------
+/// \unit
+///
+/// This file contains algorithms and drivers for tilt sensing.
+///
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+// Headers
+//------------------------------------------------------------------------------
+#include "dfm_accelerometer.h"
+#include "board.h"
+#include <irq/irq.h>
+#include <pio/pio.h>
+#include <pio/pio_it.h>
+#include <adc/adc12.h>
+#include <lcd/draw.h>
+
+#include <stdio.h>
+
+#include <math.h>
+
+#include "dfm_config.h"
+#include "dfm_cmd.h"
+#include "dfm_lcd_tsd.h"
+#include "dfm_dispboxmgr.h"
+#include "dfm_it.h"
+#include "dfm_init.h"
+
+//------------------------------------------------------------------------------
+// Macros
+//------------------------------------------------------------------------------
+#define BOARD_ADC12_FREQ 5000000 //! ADC clock
+#define ADC12_VREF 3300 //! reference voltage 3.3 * 1000,make sure the jumper setting is right
+#define PINS_ACC_ADC PIN_ADC0_AD2,PIN_ADC0_AD6,PIN_ADC0_AD7
+
+#define TAN30 (37) //! fixed point for tanget tan(45)*0x40
+#define TAN15 17 //! fixed point for tangent tan(15)*0x40
+#define DELAY 100
+#define SAMPLES 1
+
+#if defined(at91sam3u4)
+#define AT91C_ID_ADC_PRESENT AT91C_ID_ADC12B
+#define AT91C_BASE_ADC AT91C_BASE_ADC12B
+#endif
+//------------------------------------------------------------------------------
+// External Variables
+//------------------------------------------------------------------------------
+extern const char * gpActiveBinFile;
+extern unsigned int gLcdRefreshFlag;
+extern int rotation_axes_cpt;
+/// record measurement times
+static int count =0;
+//------------------------------------------------------------------------------
+// Variables
+//------------------------------------------------------------------------------
+
+
+unsigned char ADC12_channel_x = ADC12_CHANNEL_2; //!< X Channel For Accelerometer
+unsigned char ADC12_channel_y = ADC12_CHANNEL_6; //!< Y Channel For Accelerometer
+unsigned char ADC12_channel_z = ADC12_CHANNEL_7; //!< Z Channel For Accelerometer
+/// Indicates that the conversion is finished.
+static volatile unsigned char conversionDone;
+
+acc_t acc = { //!< Accelerometer Instance
+ .m = { .x=0 , .y=0 , .z=0 } ,
+ .k = { .x=ACC_ZERO_X , .y=ACC_ZERO_Y , .z=ACC_ZERO_Z } ,
+ .ak = { .x=0 , .y=0 , .z=0 } ,
+} ;
+
+static const Pin pinsADC[] = {PINS_ACC_ADC};
+
+static xyz_t lastMeasure = {0};
+int gDir = DIR_UNCHANGED;
+int gLastDir = DIR_UNCHANGED;
+
+///push button to configure for the applicatino.
+static Pin pinBP4 = PIN_PUSHBUTTON_1;
+//------------------------------------------------------------------------------
+// Functions
+//------------------------------------------------------------------------------
+int DV_Check_AccStatus();
+
+//------------------------------------------------------------------------------
+/// add position
+//------------------------------------------------------------------------------
+xyz_t xyz_add (xyz_t p, xyz_t q)
+{
+ xyz_t r ;
+ r.x = p.x + q.x ;
+ r.y = p.y + q.y ;
+ r.z = p.z + q.z ;
+ return r ;
+}
+
+//------------------------------------------------------------------------------
+/// sub position
+//------------------------------------------------------------------------------
+xyz_t xyz_diff (xyz_t p, xyz_t q)
+{
+ xyz_t r ;
+ r.x = p.x - q.x ;
+ r.y = p.y - q.y ;
+ r.z = p.z - q.z ;
+ return r ;
+}
+
+//------------------------------------------------------------------------------
+/// Invoke platform api to get converted data
+/// \param pAdc address pointer for ADC12B controller
+/// \param ADC12_channel_x channel number for x axis
+/// \param ADC12_channel_y channel number for y axis
+/// \param ADC12_channel_z channel number for z axis
+/// \return converted data for three channels
+//------------------------------------------------------------------------------
+xyz_t acc_get_value ( AT91S_ADC12B *pAdc , unsigned char ADC12_channel_x, unsigned char ADC12_channel_y, unsigned char ADC12_channel_z)
+{
+ xyz_t val ;
+
+ val.x = ADC12_GetConvertedData(pAdc, ADC12_channel_x);
+ val.y = ADC12_GetConvertedData(pAdc, ADC12_channel_y);
+ val.z = ADC12_GetConvertedData(pAdc, ADC12_channel_z);
+
+ val.x = val.x>>ACC_SHIFT;
+ val.y = val.y>>ACC_SHIFT;
+ val.z = val.z>>ACC_SHIFT;
+
+ return val ;
+}
+
+
+//------------------------------------------------------------------------------
+/// get present acceleration value as 0-g value for x and y, 1-g value for z
+//------------------------------------------------------------------------------
+void DoCalibration()
+{
+ int i = 0,j;
+ xyz_t temp ={0};
+ count = 0;
+ acc.m.x = 0;
+ acc.m.y = 0;
+ acc.m.z = 0;
+ for(i = 0; i < 16 ; i++)
+ {
+ if(DV_Check_AccStatus()){
+ temp = xyz_add(acc.m,temp);
+ for(j=0;j<DELAY;j++);
+ }
+
+ }
+ acc.m.x = temp.x /count;
+ acc.m.y = temp.y /count;
+ acc.m.z = temp.z /count;
+
+ count = 0;
+ acc.k = acc.m;
+
+
+}
+
+//------------------------------------------------------------------------------
+///check if the tilt angles are big enough.If yes,get rotation direction
+///according to the sign of two axes
+/// \return rotation direction
+//------------------------------------------------------------------------------
+int DV_Accelerometer_Tilt()
+{
+ int dir = DV_Accelerometer_Unchanged;
+ int x=0,y=0,z=0;
+
+ int i = 0,j;
+ acc.m.x = 0;
+ acc.m.y = 0;
+ acc.m.z = 0;
+
+ count = 0;
+
+ for(i = 0; i < SAMPLES ; i++)
+ {
+ DV_Check_AccStatus();
+ for(j=0;j<DELAY;j++);
+ }
+ acc.m.x = acc.m.x /count;
+ acc.m.y = acc.m.y /count;
+ acc.m.z = acc.m.z /count;
+ count = 0;
+
+ acc.ak = xyz_diff(acc.m,acc.k);
+
+
+
+ x = acc.ak.x;
+ y = acc.ak.y;
+ z = (acc.ak.z + ACC_1G);
+
+
+ if( (unsigned int)(x*x)<<12 > (TAN15*TAN15)*(y*y+z*z) || (unsigned int)(y*y)<<12 > (TAN15*TAN15)*(x*x+z*z))
+ {
+ if((unsigned int)(x*x)<<12 > (TAN15*TAN15)*(y*y+z*z))
+ {
+ if((unsigned int)(y*y)<<12 > (TAN15*TAN15)*(x*x+z*z))
+ {
+ if(x < 0)
+ {
+ if(y < 0)
+ {
+ dir = DV_Accelerometer_up_right;
+ }
+ else
+ {
+ dir =DV_Accelerometer_down_right;
+ }
+ }
+ else
+ {
+ if(y < 0)
+ {
+ dir = DV_Accelerometer_up_left;
+ }
+ else
+ {
+ dir = DV_Accelerometer_down_left;
+
+ }
+ }
+ }
+ else
+ {
+ if(x < 0)
+ {
+ dir = DV_Accelerometer_right;
+ }
+ else
+ {
+ dir = DV_Accelerometer_left;
+ }
+ }
+
+ }
+ else
+ {
+ if(y < 0)
+ {
+ dir = DV_Accelerometer_up;
+ }
+ else
+ {
+ dir = DV_Accelerometer_down;
+ }
+ }
+ }
+
+ return dir;
+
+}
+
+
+//------------------------------------------------------------------------------
+/// get the direction of the board
+/// \return direction
+//------------------------------------------------------------------------------
+int DV_Accelerometer_Dir()
+{
+
+ int dir = gDir;
+ int x=0,y=0,z=0;
+ xyz_t diff = {0};
+
+ ///measure the difference between the last one and the present one
+ diff = xyz_diff(acc.m,lastMeasure);
+
+ ///check the status,acc.m will be changed after called routine
+ ///if conversion is done,otherwise,return immediately
+ DV_Check_AccStatus();
+
+
+ ///no big change since last measurement
+ if( diff.x*diff.x + diff.y*diff.y + diff.z*diff.z < ((ACC_1G*ACC_1G)>>11))
+ {
+ ///keep the old direction
+ return gDir;
+
+ }
+ ///update lastMeasure to the new one
+ lastMeasure = acc.m;
+
+ ///get acceleration offset
+ acc.ak = xyz_diff(acc.m,acc.k);
+
+
+
+ x = acc.ak.x;
+ y = acc.ak.y;
+ z = (acc.ak.z + ACC_1G);
+ /// filter non-tilting operation
+ if( (x*x + y*y+z*z > ACC_AMPLITUDE_UPPER) ||(x*x + y*y+z*z < ACC_AMPLITUDE_LOWER) ){
+ return gDir;
+ }
+
+ ///the board plane is angled with the horizontal surface
+ if( (unsigned int)(x*x)<<12 > (TAN30*TAN30)*(y*y+z*z) || (unsigned int)(y*y)<<12 > (TAN30*TAN30)*(x*x+z*z))
+ {
+
+ if(x*x > y*y + (y*y>>1))
+ {
+ if(x > 0)
+ {
+ dir = DIR_HORIZIONTAL_UP;
+ }
+ else
+ {
+ dir = DIR_HORIZIONTAL_DOWN;
+ }
+ }
+ else if(x*x + (x*x>>1) < y*y)
+ {
+ if(y > 0)
+ {
+ dir = DIR_VERTICAL_UP;
+ }
+ else
+ {
+ dir = DIR_VERTICAL_DOWN;
+ }
+ }
+ //else dir = previous dir
+ //leave gap enough for stabilization
+ ///renew the direction
+ gDir = dir;
+ }
+
+ return gDir;
+
+}
+//------------------------------------------------------------------------------
+/// Check accelerometer is ready?
+/// if the direction is unchanged since last measurement,return "not ready"
+/// \return 0 not ready, other value means ready and command string length
+//------------------------------------------------------------------------------
+unsigned int ACC_CommandIsReady()
+{
+ int dir = DIR_UNCHANGED;
+ unsigned int ret;
+ dir = DV_Accelerometer_Dir();
+ //no change of direction since last measurement
+ if(gLastDir == dir)
+ {
+ ret = 0;
+ }
+ else
+ {
+ ret = 1;
+ }
+ gLastDir = dir;
+ return ret;
+}
+
+
+//------------------------------------------------------------------------------
+// get flipped ppt page
+//------------------------------------------------------------------------------
+const char * ACC_GetCommand()
+{
+ if(gDir == DIR_HORIZIONTAL_UP || gDir == DIR_HORIZIONTAL_DOWN)
+ {
+ //update to portait image file
+ gpActiveBinFile = LCDSLIDEVIMAGEFILE;
+ gLcdRefreshFlag = 1;
+
+ }
+ else
+ {
+ gpActiveBinFile = LCDSLIDEIMAGEFILE;
+ gLcdRefreshFlag = 1;
+
+ }
+ return NULL;
+}
+
+//an example of ShowPrompt
+int ACC_ShowPrompt()
+{
+// int i;
+// switch(gDir)//gDir
+// {
+//
+// case DIR_HORIZIONTAL_UP:
+// printf("Horizontal Up!\n");
+// //LCDD_DrawRectangle((void *)BOARD_LCD_BASE, x,y,width,height,0xE0E0C0);
+// break;
+// case DIR_HORIZIONTAL_DOWN:
+// printf("Horizontal Down!\n");
+// break;
+// case DIR_VERTICAL_UP:
+// printf("Vertical Up!\n");
+// break;
+// case DIR_VERTICAL_DOWN:
+// printf("Vertical Down!\n");
+// break;
+// default:
+// printf("Unchanged!\n");
+// break;
+// }
+// for(i=0;i<5000;i++);
+ return 0;
+}
+
+//global accelerometer entry variable
+TInputEntry gAccEntry = {0, {NULL,NULL},ACC_ShowPrompt, ACC_CommandIsReady, ACC_GetCommand, NULL};
+//------------------------------------------------------------------------------
+/// Check if conversion is done.If done,read the corresponding parameters and
+///start new measurement
+//------------------------------------------------------------------------------
+int DV_Check_AccStatus()
+{
+ //check if conversation is done
+ //if done, read and measure,and start new conversation
+ //else, return directly
+
+
+ if(conversionDone == ((1<<ADC12_channel_x)|(1<<ADC12_channel_y)|(1<<ADC12_channel_z)))//x,y,z all done
+ {
+ // IRQ_DisableIT(AT91C_ID_ADC_PRESENT);
+ DV_ACCELEROMETER_Measure();
+ //sharing variable for main and isr
+ conversionDone= 0;
+ // record the number of measurement
+ count++;
+
+ // Start new measurement
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_x);//Enable EOCx interrupt
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_y);//Enable EOCx interrupt
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_z);//Enable EOCx interrupt
+ ADC12_StartConversion(AT91C_BASE_ADC);
+ // conversionDone for all 3 channels
+ return 1;
+ }else{
+ return 0;
+ }
+
+
+
+}
+
+//------------------------------------------------------------------------------
+/// Interrupt handler for the ADC. Signals that the conversion is finished by
+/// setting a flag variable.
+//------------------------------------------------------------------------------
+void ADC12B_IrqHandler(void)
+{
+ unsigned int status, i;
+ static unsigned char chns[3];
+ AT91PS_ADC12B padc = AT91C_BASE_ADC;
+ chns[0] = ADC12_channel_x;
+ chns[1] = ADC12_channel_y;
+ chns[2] = ADC12_channel_z;;
+
+
+ status = ADC12_GetStatus(padc);
+ //printf("status =0x%X\n\r", status);
+ //TRACE_DEBUG("ADC12_imr=0x%X\n\r", ADC12_GetInterruptMaskStatus());
+
+ for(i=0;i<sizeof(chns)/sizeof(unsigned char);i++) {
+ if (ADC12_IsChannelInterruptStatusSet(status, chns[i])) {
+
+ //printf("channel %d\n\r", chns[i]);
+ ADC12_DisableIt(AT91C_BASE_ADC, 1<<chns[i]);//disable EOCx interrupt
+ conversionDone |= 1<<chns[i];
+ }
+ }
+}
+
+#if defined(USE_IT_CHAIN_MGR)
+TDFM_ItServList gAccADC12BHandler = {AT91C_ID_ADC_PRESENT, ADC12B_IrqHandler, NULL};
+#endif
+//------------------------------------------------------------------------------
+/// ADC12B and accelerometer initialization
+//------------------------------------------------------------------------------
+int DV_ACCELEROMETER_Init(void)
+{
+ int cali_done = 0;
+ char keyStatus = 0;
+ Pin pinSleep;
+ pinSleep.attribute = PIO_DEFAULT;
+ pinSleep.mask = (1<<13);
+ pinSleep.id = AT91C_ID_PIOC;
+ pinSleep.pio = AT91C_BASE_PIOC;
+ pinSleep.type = PIO_OUTPUT_1;
+
+ ///Normal mode
+ PIO_Configure(&pinSleep,1);
+ PIO_Set(&pinSleep);
+
+#ifdef PINS_ADC
+ PIO_Configure(pinsADC, PIO_LISTSIZE(pinsADC));
+#endif
+ ///ADC12B configuration
+ ADC12_Initialize( AT91C_BASE_ADC,
+ AT91C_ID_ADC_PRESENT,
+ AT91C_ADC12B_MR_TRGEN_DIS,
+ 0,
+ AT91C_ADC12B_MR_SLEEP_NORMAL,
+ AT91C_ADC12B_MR_LOWRES_12_BIT,//for 12bit adc,0 means 12bits,1-10bits
+ BOARD_MCK,
+ BOARD_ADC12_FREQ,
+ 10,
+ 2400);
+ ///Enable 3 channels for x,y and z
+ ADC12_EnableChannel(AT91C_BASE_ADC, ADC12_channel_x);
+ ADC12_EnableChannel(AT91C_BASE_ADC, ADC12_channel_y);
+ ADC12_EnableChannel(AT91C_BASE_ADC, ADC12_channel_z);
+
+ ///Enable adc12b interrupt
+ //IRQ_ConfigureIT(AT91C_ID_ADC_PRESENT, 0, ADCC0_IrqHandler);
+ // IRQ_EnableIT(AT91C_ID_ADC_PRESENT);
+#if defined(USE_IT_CHAIN_MGR)
+ DFM_RegisterItHandler(&gAccADC12BHandler);
+#endif
+
+ IRQ_EnableIT(AT91C_ID_ADC_PRESENT);
+
+ conversionDone = 0;
+
+ ///Enable channel interrupt
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_x);
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_y);
+ ADC12_EnableIt(AT91C_BASE_ADC, 1<<ADC12_channel_z);
+
+ /// Start measurement
+ ADC12_StartConversion(AT91C_BASE_ADC);
+
+ ///if calibration needed here?
+
+ // Do calibration first,ineteract with users
+ LCDD_Fill(((void *)BOARD_LCD_BASE),0xffffff);
+ //LCDD_DrawString((void *)BOARD_LCD_BASE,10,100,"Put the board on \nthe horizontal \nsurface and press \nBP4 to start \ncalibration!",0x0);
+
+ //PIO_InitializeInterrupts(0);
+ //PIO_Configure(&pinBP4,1);
+ //keyStatus = 1;
+ while(cali_done == 0)
+ {
+ // Check if button state has changed
+ //unsigned char isButtonPressed = PIO_Get(&pinBP4);
+ //if (isButtonPressed != keyStatus) {
+
+ // Update button state
+ //if (!isButtonPressed) {
+
+ // Key has been pressed
+ //printf("BP4 has been pressed!\n");
+ //keyStatus = 0;
+
+ DoCalibration();
+ cali_done = 1;
+
+
+ //}
+ //}
+ }
+ return 0;
+
+}
+
+//------------------------------------------------------------------------------
+/// Get immediate acceleration
+//------------------------------------------------------------------------------
+void DV_ACCELEROMETER_Measure(void)
+{
+ acc.m = acc_get_value(AT91C_BASE_ADC, ADC12_channel_x, ADC12_channel_y, ADC12_channel_z) ;
+
+}
+
+
+//------------------------------------------------------------------------------
+///
+///Measure the direction,left,right,forward,backward,then switch rotation axis
+///appopriately
+///
+//------------------------------------------------------------------------------
+void DV_Accelerometer_Turn()
+{
+ int dir = DV_Accelerometer_Tilt();
+ switch(dir)
+ {
+ case DV_Accelerometer_Unchanged:
+ rotation_axes_cpt = 0;
+ break;
+ case DV_Accelerometer_left:
+ rotation_axes_cpt = 4;
+ break;
+ case DV_Accelerometer_right:
+ rotation_axes_cpt = 3;
+ break;
+ case DV_Accelerometer_up:
+ rotation_axes_cpt = 2;
+ break;
+ case DV_Accelerometer_down:
+ rotation_axes_cpt = 1;
+ break;
+ case DV_Accelerometer_up_right:
+ rotation_axes_cpt = 6;
+ break;
+ case DV_Accelerometer_up_left:
+ rotation_axes_cpt = 8;
+ break;
+ case DV_Accelerometer_down_right:
+ rotation_axes_cpt = 5;
+ break;
+ case DV_Accelerometer_down_left:
+ rotation_axes_cpt = 7;
+ break;
+ }
+}
+
+
+//Accelerometer init in level 6
+DFM_INIT(6, DV_ACCELEROMETER_Init);
+
+
|