vector3c_t accel_ReadXYZ8() {
vector3c_t vec;
vec.x = accel_read(XOUT8);// * accel_scale;
vec.y = accel_read(YOUT8);// * accel_scale;
vec.z = accel_read(ZOUT8);// * accel_scale;
return vec;
}
void accel_get_measurements(int16_t* x, int16_t* y, int16_t* z) {
//read and justify
*x = ((int16_t) (accel_read(XOUT8))) << 8;
//read and justify
*y = ((int16_t) (accel_read(YOUT8))) << 8;
//read and justify
*z = ((int16_t) (accel_read(ZOUT8))) << 8;
}
void task_accel_sensor(void)
{
if (!ti_task)
{
ti_task = 200;
accel_read();
}
}
void *accel_handler(void *arg)
{
int ret;
while (1) {
ret = accel_read(accel_dev, &accel_data);
if (RET_ERR == ret) {
syslog(LOG_ERR, "accel read data failed.\n");
}
handle_accel_data(&accel_data);
usleep(ACCEL_SAMPLE_INTERVAL * 1000);
}
}
vector3i_t accel_ReadXYZ() {
// Read low byte first to ensure high byte is latched
// Note: _OUTH must be read directly after _OUTL
// Result is 2's complement
accel_current.x = (accel_read(XOUTL) | (accel_read(XOUTH) << 8));// * accel_scale;
accel_current.y = (accel_read(YOUTL) | (accel_read(YOUTH) << 8));// * accel_scale;
accel_current.z = (accel_read(ZOUTL) | (accel_read(ZOUTH) << 8));// * accel_scale;
// Sign-extension
if (accel_current.x & 0x0200) accel_current.x |= 0xFC00;
if (accel_current.y & 0x0200) accel_current.y |= 0xFC00;
if (accel_current.z & 0x0200) accel_current.z |= 0xFC00;
return accel_current;
}
static void star_calc(liqcell *sheepdog_flow,STAR *stars,vgraph *graph,liqsketch *sketch, int drawwidth,int drawheight)
{
// pseudostarcount is the count of actual stars, between 0..starscount, based on the size of the box
int pseudostarcount = starcount * drawwidth / 800;
if(pseudostarcount>starcount)pseudostarcount=starcount;
//liqapp_log("stardraw %i,%i :: %i",drawwidth,drawheight,pseudostarcount);
int aax=0;
int aay=0;
int aaz=0;
accel_read(&aax,&aay,&aaz);
#define ff 0.08
float fax=ff * (float)aax;
float fay=ff * (float)aay;
float faz=ff * (float)aaz;
// 10000 == vigerous shake
// 0 == still
float r= 0.01 * ((float)accel_fat);
//liqapp_log("af %5i %3.2f",accel_fat,r);
int size = 4 + (accel_fat/(1000/8));
if(size>24)size=24;
liqcell *backplane = liqcell_child_lookup(sheepdog_flow,"backplane");
liqimage *img = liqcell_getimage(backplane); // grab the background
//liqapp_log("...creating cliprect");
liqcliprect *cr = (img?liqcliprect_newfromimage(img):NULL);
if(cr)
{
vgraph_drawimage(graph,0,0,800,480,img);
}
//liqapp_log("%-4i %-4i %-4i",aax,aay,aaz);
//liqapp_log("%-4.2f %-4.2f %-4.2f",fax,fay,faz);
//liqapp_log("stars calc");
int a;
STAR *sa;
for(a=0;a<pseudostarcount;a++)
{
sa=&stars[a];
//sa->a.x *= 0.3;
//sa->a.y *= 0.3;
sa->a.x = -fax; // start with gravity
sa->a.y = -fay;
//sa->a.x = 0; // start with no acceleration
//sa->a.y = 0;
//sa->a.z = 0;
if(cr)
{
// need to get a sample of points from the ground below
// this is achieved with:
inline void liqcliprect_drawpgetcolor( liqcliprect *self,int x, int y, unsigned char *grey,unsigned char *u,unsigned char *v);
unsigned char gcy[9]={128};
unsigned char gcu=128;
unsigned char gcv=128;
int gx=sa->p.x;
int gy=sa->p.y;
if(gy>0)
{
if(gx>0) liqcliprect_drawpgetcolor( cr, gx-1, gy-1, &gcy[0], &gcu, &gcv );
liqcliprect_drawpgetcolor( cr, gx , gy-1, &gcy[1], &gcu, &gcv );
if(gx<799)liqcliprect_drawpgetcolor( cr, gx+1, gy-1, &gcy[2], &gcu, &gcv );
}
{
if(gx>0) liqcliprect_drawpgetcolor( cr, gx-1, gy , &gcy[3], &gcu, &gcv );
liqcliprect_drawpgetcolor( cr, gx , gy , &gcy[4], &gcu, &gcv );
if(gx<799)liqcliprect_drawpgetcolor( cr, gx+1, gy , &gcy[5], &gcu, &gcv );
}
if(gy<479)
{
if(gx>0) liqcliprect_drawpgetcolor( cr, gx-1, gy+1, &gcy[6], &gcu, &gcv );
liqcliprect_drawpgetcolor( cr, gx , gy+1, &gcy[7], &gcu, &gcv );
if(gx<799)liqcliprect_drawpgetcolor( cr, gx+1, gy+1, &gcy[8], &gcu, &gcv );
}
// now we look for gradients
// relative to 4
//the size of the gradient represents a vector in that other direction
float gax=0;
float gay=0;
void checkvec(int idx,int ox,int oy)
{
int vidx = gcy[idx];
int v4 = gcy[4];
gax += (float)ox * (float)(vidx-v4) ;
gay += (float)oy * (float)(vidx-v4) ;
}
#define rx 1
#define ry 1
checkvec(0, -rx,-ry);
checkvec(1, 0, -ry);
checkvec(2, rx,-ry);
checkvec(3, -rx,0);
//checkvec(0, 0, 0);
checkvec(5, rx,0);
checkvec(6, -rx,ry);
checkvec(7, 0, ry);
checkvec(8, rx,ry);
liqapp_log("g? %i %3.3f,%3.3f %i",a,gax,gay,gcy[4]);
sa->a.x += gax; // start with no acceleration
sa->a.y += gay;
}
}
void accelerometer_read_all(uint16_t* data) {
data[0]=accel_read(ACCEL_X);
data[1]=accel_read(ACCEL_Y);
data[2]=accel_read(ACCEL_Z);
}
void accel_read_xyz(int * data){
data[0] = (int)(signed char)accel_read(XOUT8);
data[1] = (int)(signed char)accel_read(YOUT8);
data[2] = (int)(signed char)accel_read(ZOUT8);
}
static int
accel_ready(void)
{
return (accel_read(0x09) & 1);
}
bool accel_init() {
i2c_init();
// Check WhoAmI register
return accel_read(WHOAMI) == WHOAMI_MAGIC_NUMBER;
}
accel_status_t accel_Status() {
return (accel_status_t)accel_read(STATUS);
}
uint8 accel_ReadTemperature() {
return accel_read(TOUT);
}
void accel_modify(uint8 reg, uint8 value, uint8 mask) {
uint8 curr = accel_read(reg);
curr &= ~mask;
curr |= value & mask;
accel_write(reg, curr);
}
unsigned long Fable_get_accel(unsigned long axis){
return accel_read(axis);
}