00001
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 # include <stdio.h>
00051
00052 # include "s_accelerometer.h"
00053 # include "board.h"
00054 # include "gpio.h"
00055 # include "pm.h"
00056 # include "adc.h"
00057
00058
00059
00060
00061
00062
00063
00064
00065 xyz_t g_angle;
00066 volatile avr32_adc_t * adc= (volatile avr32_adc_t *) &AVR32_ADC;
00067
00068 unsigned char adc_channel_x = 1;
00069 unsigned char adc_channel_y = 2;
00070 unsigned char adc_channel_z = 3;
00071
00072
00073 struct S_acc_conv_t {
00074 U16 sin;
00075 U8 angle;
00076 };
00077
00078 struct S_acc_conv_t acc_table []=
00079 { { .sin=(U16)(SIN90 * ACC_1G), .angle=90}
00080 , { .sin=(U16)(SIN85 * ACC_1G), .angle=85}
00081 , { .sin=(U16)(SIN80 * ACC_1G), .angle=80}
00082 , { .sin=(U16)(SIN75 * ACC_1G), .angle=75}
00083 , { .sin=(U16)(SIN70 * ACC_1G), .angle=70}
00084 , { .sin=(U16)(SIN65 * ACC_1G), .angle=65}
00085 , { .sin=(U16)(SIN60 * ACC_1G), .angle=60}
00086 , { .sin=(U16)(SIN55 * ACC_1G), .angle=55}
00087 , { .sin=(U16)(SIN50 * ACC_1G), .angle=50}
00088 , { .sin=(U16)(SIN45 * ACC_1G), .angle=45}
00089 , { .sin=(U16)(SIN40 * ACC_1G), .angle=40}
00090 , { .sin=(U16)(SIN35 * ACC_1G), .angle=35}
00091 , { .sin=(U16)(SIN30 * ACC_1G), .angle=30}
00092 , { .sin=(U16)(SIN25 * ACC_1G), .angle=25}
00093 , { .sin=(U16)(SIN20 * ACC_1G), .angle=20}
00094 , { .sin=(U16)(SIN15 * ACC_1G), .angle=15}
00095 , { .sin=(U16)(SIN10 * ACC_1G), .angle=10}
00096 , { .sin=(U16)(SIN5 * ACC_1G), .angle=5 }
00097 , { .sin=(U16)(SIN0 * ACC_1G), .angle=0 }
00098 };
00099
00100 Avr32_acc_t acc = {
00101
00102 .m = { .x=0 , .y=0 , .z=0 } ,
00103
00104
00105 .left = 0 ,
00106 .right = 0 ,
00107 .up = 0 ,
00108 .down = 0 ,
00109 .topdown = 0 ,
00110
00111
00112 .k = { .x=ACC_ZERO_X , .y=ACC_ZERO_Y , .z=ACC_ZERO_Z } ,
00113 .ak = { .x=0 , .y=0 , .z=0 } ,
00114 .ak2 = 0 ,
00115
00116
00117 .g = { .x=ACC_ZERO_X , .y=ACC_ZERO_Y , .z=ACC_MIN } ,
00118 .ag = { .x=0 , .y=0 , .z=0 } ,
00119 .ag2 = 0 ,
00120 .s = { .x=0 , .y=0 , .z=0 } ,
00121 .s2 = 0
00122 } ;
00123
00124
00125
00126
00127 static void xyz_diff (xyz_t p, xyz_t q, xyz_t* r)
00128 {
00129 r->x = p.x - q.x ;
00130 r->y = p.y - q.y ;
00131 r->z = p.z - q.z ;
00132 }
00133
00134 static int xyz_sumsq (xyz_t p)
00135 {
00136 return ( (p.x*p.x) + (p.y*p.y) + (p.z*p.z) ) ;
00137 }
00138
00139
00140 static void acc_get_value ( volatile avr32_adc_t * adc, unsigned char adc_channel_x, unsigned char adc_channel_y, unsigned char adc_channel_z, xyz_t* val)
00141 {
00142
00143 adc_enable( adc,adc_channel_x);
00144 adc_start( adc) ;
00145 val->x = ( adc_get_value(adc, adc_channel_x) >> ACC_SHIFT ) ;
00146 adc_disable( adc,adc_channel_x);
00147
00148 adc_enable( adc,adc_channel_y);
00149 adc_start( adc) ;
00150 val->y = ( adc_get_value(adc, adc_channel_y) >> ACC_SHIFT ) ;
00151 adc_disable( adc,adc_channel_y);
00152
00153 adc_enable( adc,adc_channel_z);
00154 adc_start( adc) ;
00155 val->z = ( adc_get_value(adc, adc_channel_z) >> ACC_SHIFT ) ;
00156 adc_disable( adc,adc_channel_z);
00157
00158
00159 }
00160
00161
00162
00163
00164
00165 static char acc_slow()
00166 {
00167 return ( (acc.ak2 > ACC_QUIET_LO) && (acc.ak2 < ACC_QUIET_HI) ) ;
00168 }
00169
00170
00171 static void accelerometer_action_x( char* buf )
00172 {
00173 if (acc_slow())
00174 {
00175 U32 i;
00176 for( i=0 ; i<sizeof(acc_table)/sizeof(struct S_acc_conv_t) ; i++)
00177 {
00178 if( abs(acc.ak.x) >= acc_table[i].sin )
00179 {
00180 if( acc.ak.z<0 )
00181 if( acc.ak.x>0 )
00182 {
00183 g_angle.x = acc_table[i].angle ;
00184
00185 }
00186 else
00187 {
00188 g_angle.x = 270 + 90-acc_table[i].angle ;
00189 if( g_angle.x==360 )
00190 g_angle.x=0;
00191
00192 }
00193 else
00194 if( acc.ak.x>0 )
00195 {
00196 g_angle.x = 90 + 90-acc_table[i].angle ;
00197
00198 }
00199 else
00200 {
00201 g_angle.x = 180 + acc_table[i].angle ;
00202
00203 }
00204
00205
00206 break;
00207 }
00208 }
00209 }
00210 }
00211
00212 static void accelerometer_action_y( char* buf )
00213 {
00214 if (acc_slow())
00215 {
00216 U32 i;
00217
00218 for( i=0 ; i<sizeof(acc_table)/sizeof(struct S_acc_conv_t) ; i++)
00219 {
00220 if( abs(acc.ak.y) >= acc_table[i].sin )
00221 {
00222 if( acc.ak.z<0 )
00223 if( acc.ak.y>0 )
00224 {
00225 g_angle.y = acc_table[i].angle ;
00226
00227 }
00228 else
00229 {
00230 g_angle.y = 270 + 90-acc_table[i].angle ;
00231 if( g_angle.y==360 )
00232 g_angle.y=0;
00233
00234 }
00235 else
00236 if( acc.ak.y>0 )
00237 {
00238 g_angle.y = 90 + 90-acc_table[i].angle ;
00239
00240 }
00241 else
00242 {
00243 g_angle.y = 180 + acc_table[i].angle ;
00244
00245 }
00246
00247 break;
00248 }
00249 }
00250 }
00251 }
00252
00253
00254
00258 void accelerometer_init(void)
00259 {
00260
00261 gpio_enable_module_pin(AVR32_ADC_AD_1_PIN, AVR32_ADC_AD_1_FUNCTION);
00262 gpio_enable_module_pin(AVR32_ADC_AD_2_PIN, AVR32_ADC_AD_2_FUNCTION);
00263 gpio_enable_module_pin(AVR32_ADC_AD_3_PIN, AVR32_ADC_AD_3_FUNCTION);
00264
00265
00266
00267 adc_configure(adc);
00268 }
00269
00270
00271
00278 void accelerometer_measure(U32 angle, char* buf)
00279 {
00280
00281 acc_get_value(adc, adc_channel_x, adc_channel_y, adc_channel_z, &acc.m ) ;
00282
00283
00284 xyz_diff( acc.m , acc.k, &acc.ak ) ;
00285
00286 acc.ak2 = xyz_sumsq( acc.ak ) ;
00287
00288 if(!angle)
00289 {
00290 accelerometer_action_x( buf );
00291 sprintf( buf, "%3d\r\n", g_angle.x);
00292 } else {
00293 accelerometer_action_y( buf );
00294 sprintf( buf, "%3d\r\n", g_angle.y);
00295 }
00296
00297
00298 acc.g.x = acc.m.x ;
00299 acc.g.y = acc.m.y ;
00300 acc.g.z = acc.m.z ;
00301 }