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 "board.h"
00051 #include "gpio.h"
00052 #include "adc.h"
00053 #include "lis3l06al.h"
00054 #include "math.h"
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065 avr32_acc_t acc = {
00066
00067 .m = { .x=0 , .y=0 , .z=0 } ,
00068
00069
00070 .pos.left = 0,
00071 .pos.right = 0,
00072 .pos.up = 0,
00073 .pos.down = 0,
00074 .pos.topdown = 0,
00075
00076
00077 .k = { .x=ACC_ZERO_X , .y=ACC_ZERO_Y , .z=ACC_ZERO_Z } ,
00078 .ak = { .x=0 , .y=0 , .z=0 } ,
00079 .ak2 = 0 ,
00080
00081
00082 .g = { .x=ACC_ZERO_X , .y=ACC_ZERO_Y , .z=ACC_MIN } ,
00083 .ag = { .x=0 , .y=0 , .z=0 } ,
00084 .ag2 = 0 ,
00085 .s = { .x=0 , .y=0 , .z=0 } ,
00086 .s2 = 0
00087 } ;
00088
00089
00090 static void xyz_add (xyz_t p, xyz_t q, xyz_t* r )
00091 {
00092 r->x = p.x + q.x ;
00093 r->y = p.y + q.y ;
00094 r->z = p.z + q.z ;
00095 }
00096
00097 static void xyz_diff (xyz_t p, xyz_t q, xyz_t* r)
00098 {
00099 r->x = p.x - q.x ;
00100 r->y = p.y - q.y ;
00101 r->z = p.z - q.z ;
00102 }
00103
00104 static int round0 (int i, int ceil)
00105 {
00106 return ( (abs(i) < abs(ceil)) ? 0 : i ) ;
00107 }
00108
00109 static void xyz_round0 (xyz_t p ,int ceil, xyz_t* r )
00110 {
00111 r->x = round0( p.x , ceil ) ;
00112 r->y = round0( p.y , ceil ) ;
00113 r->z = round0( p.z , ceil ) ;
00114 }
00115
00116 static int xyz_sumsq (xyz_t p)
00117 {
00118 return ( (p.x*p.x) + (p.y*p.y) + (p.z*p.z) ) ;
00119 }
00120
00121 static void acc_get_value (
00122 volatile avr32_adc_t * adc
00123 , xyz_t* val ) {
00124
00125
00126 adc_enable(adc, ADC_CHANNEL_X);
00127 adc_start(adc) ;
00128 val->x = ( adc_get_value(adc, ADC_CHANNEL_X) >> ACC_SHIFT ) ;
00129 adc_disable(adc, ADC_CHANNEL_X);
00130
00131 adc_enable(adc, ADC_CHANNEL_Y);
00132 adc_start(adc) ;
00133 val->y = ( adc_get_value(adc, ADC_CHANNEL_Y) >> ACC_SHIFT ) ;
00134 adc_disable(adc, ADC_CHANNEL_Y);
00135
00136 adc_enable(adc, ADC_CHANNEL_Z);
00137 adc_start(adc) ;
00138 val->z = ( adc_get_value(adc, ADC_CHANNEL_Z) >> ACC_SHIFT ) ;
00139 adc_disable(adc, ADC_CHANNEL_Z);
00140 }
00141
00142
00143
00144
00145
00146
00147 Bool is_acc_slow()
00148 {
00149 return ( (acc.ak2 > ACC_QUIET_LO) && (acc.ak2 < ACC_QUIET_HI) ) ;
00150 }
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170 Bool is_acc_left()
00171 {
00172 if ( (acc.pos.left==0) && (acc.m.x > (int)(1.01*ACC_TRIG_X1)) ) acc.pos.left = 1 ;
00173 else if ( (acc.pos.left==1) && (acc.m.x < (int)(0.99*ACC_TRIG_X1)) ) acc.pos.left = 0 ;
00174 return acc.pos.left ;
00175 }
00176 Bool is_acc_right()
00177 {
00178 if ( (acc.pos.right==0) && (acc.m.x < (int)(0.99*ACC_TRIG_X0)) ) acc.pos.right = 1 ;
00179 else if ( (acc.pos.right==1) && (acc.m.x > (int)(1.01*ACC_TRIG_X0)) ) acc.pos.right = 0 ;
00180 return acc.pos.right ;
00181 }
00182
00183 Bool is_acc_up()
00184 {
00185 if ( (acc.pos.up==0) && (acc.m.y < (int)(0.99*ACC_TRIG_Y0)) ) acc.pos.up = 1 ;
00186 else if ( (acc.pos.up==1) && (acc.m.y > (int)(1.01*ACC_TRIG_Y0)) ) acc.pos.up = 0 ;
00187 return acc.pos.up ;
00188 }
00189 Bool is_acc_down()
00190 {
00191 if ( (acc.pos.down==0) && (acc.m.y > (int)(1.01*ACC_TRIG_Y1)) ) acc.pos.down = 1 ;
00192 else if ( (acc.pos.down==1) && (acc.m.y < (int)(0.99*ACC_TRIG_Y1)) ) acc.pos.down = 0 ;
00193 return acc.pos.down ;
00194 }
00195
00196 Bool is_acc_topdown()
00197 {
00198 if ( (acc.pos.topdown==0) && (acc.m.z > (int)(1.01*ACC_TRIG_Z_BOT)) ) acc.pos.topdown = 1 ;
00199 else if ( (acc.pos.topdown==1) && (acc.m.z < (int)(0.99*ACC_TRIG_Z_BOT)) ) acc.pos.topdown = 0 ;
00200 return acc.pos.topdown ;
00201 }
00202
00203 Bool is_acc_meuh(char stop)
00204 {
00205 static char topdown = 0 ;
00206 static char meuh = 0 ;
00207 if ( stop ) meuh = 0 ;
00208 if ( (topdown==0) && (acc.m.z > ACC_TRIG_Z_BOT) ) topdown = 1 ;
00209 else if ( (topdown==1) && (acc.m.z < ACC_TRIG_Z_TOP) ) { topdown = 0 ; meuh = 1 ; }
00210 return meuh ;
00211 }
00212
00213 void acc_init(void)
00214 {
00215 volatile avr32_adc_t *adc = &AVR32_ADC;
00216 static const gpio_map_t ADC_GPIO_MAP =
00217 {
00218 {AVR32_ADC_AD_3_PIN, AVR32_ADC_AD_3_FUNCTION},
00219 {AVR32_ADC_AD_1_PIN, AVR32_ADC_AD_1_FUNCTION},
00220 {AVR32_ADC_AD_2_PIN, AVR32_ADC_AD_2_FUNCTION}
00221 };
00222
00223
00224 gpio_enable_module(ADC_GPIO_MAP,
00225 sizeof(ADC_GPIO_MAP) / sizeof(ADC_GPIO_MAP[0]));
00226
00227
00228 adc_configure(adc);
00229 }
00230
00231 void acc_update( void )
00232 {
00233 volatile avr32_adc_t *adc = &AVR32_ADC;
00234
00235
00236 acc_get_value(adc, &acc.m) ;
00237
00238
00239 xyz_diff( acc.m , acc.k, &acc.ak ) ;
00240 acc.ak2 = xyz_sumsq( acc.ak ) ;
00241
00242 if ( is_acc_slow() ) {
00243
00244 acc.g.x = acc.m.x ;
00245 acc.g.y = acc.m.y ;
00246 acc.g.z = acc.m.z ;
00247 }
00248 else {
00249 xyz_diff( acc.m , acc.g, &acc.ag ) ;
00250 xyz_round0( acc.ag , 16, &acc.ag ) ;
00251 xyz_add( acc.s , acc.ag, &acc.s ) ;
00252
00253
00254
00255
00256
00257
00258
00260
00261 }
00262 }
00263
00264 #define DEG_2_RAD(deg) ((double)(deg)*3.14/180)
00265 signed long is_acc_abs_angle_x( unsigned long abs_ang )
00266 {
00267 if( abs(acc.ak.x) > (int)(sin(DEG_2_RAD(abs_ang)) * ACC_1G) )
00268 {
00269 if ( acc.ak.x > 0 ) return 1;
00270 else return -1;
00271 }
00272 return 0;
00273 }
00274
00275 signed long is_acc_abs_angle_y( unsigned long abs_ang )
00276 {
00277 if( abs(acc.ak.y) > (int)(sin(DEG_2_RAD(abs_ang)) * ACC_1G) )
00278 {
00279 if ( acc.ak.y > 0 ) return 1;
00280 else return -1;
00281 }
00282 return 0;
00283 }
00284
00285 unsigned long acc_get_m_x( void )
00286 {
00287 return acc.m.x;
00288 }
00289
00290 unsigned long acc_get_m_y( void )
00291 {
00292 return acc.m.y;
00293 }
00294
00295 unsigned long acc_get_m_z( void )
00296 {
00297 return acc.m.z;
00298 }