roboforum.ru

void lsm303dlh_funct(void)
{
static bool start_lsm=1;
if(start_lsm)
  {
  start_lsm=0;
  i2c_initialization();
  start_lsm303dlh();
  }
read_data();
}


void start_lsm303dlh(void) 
{   
//enable accelerometer
    i2c_on();
    i2c_write_byte(0x30); // write acc
    i2c_write_byte(0x20); // CTRL_REG1_A
    i2c_write_byte(0x27); // normal power mode, 50 Hz data rate, all axes enabled
    i2c_off();

    //enable magnetometer
    i2c_on();
    i2c_write_byte(0x3C); // write mag
    i2c_write_byte(0x02); // MR_REG_M
    i2c_write_byte(0x00); // continuous conversion mode
   i2c_off();
   
}


void read_data(void)
{
    // read accelerometer values
    tx_write(6);  //compass funct
    i2c_on();
    i2c_write_byte(0x30); // write acc
    i2c_write_byte(0xA8); //i2c_write_byte(0xA8); OUT_X_L_A, MSB set to enable auto-increment
    i2c_on();          // repeated start
    i2c_write_byte(0x31); // read acc
    tx_write(i2c_read_byte());
    tx_write(i2c_read_byte());
    tx_write(i2c_read_byte());
    tx_write(i2c_read_byte());
    tx_write(i2c_read_byte());
    tx_write(i2c_read_last_byte());
    i2c_off();

    // read magnetometer values
    i2c_on();
    i2c_write_byte(0x3C); // write mag
    i2c_write_byte(0x03); // OUTXH_M
   i2c_on();        // repeated start
   i2c_write_byte(0x3D); // read mag
   tx_write(i2c_read_byte());
   tx_write(i2c_read_byte());
   tx_write(i2c_read_byte());
   tx_write(i2c_read_byte());
   tx_write(i2c_read_byte());
   tx_write(i2c_read_last_byte());
   i2c_off();
}

//Функции i2c



void i2c_initialization(void)
{
TWSR=0x00; 
TWBR=0x07; //TWBR=0x1D;
TWCR=0x04;
}

void i2c_on(void)
{ 
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN); // send start condition 
while (!(TWCR & (1 << TWINT))); 
} 
 
void i2c_write_byte(signed char byte)
{ 
TWDR = byte;             
TWCR = (1 << TWINT) | (1 << TWEN); // start address transmission 
while (!(TWCR & (1 << TWINT))); 
} 
 
signed char i2c_read_byte(void)
{ 
TWCR = (1 << TWINT) | (1 << TWEA) | (1 << TWEN); // start data reception, transmit ACK 
while (!(TWCR & (1 << TWINT))); 
return TWDR; 
} 

signed char i2c_read_last_byte(void)
{ 
TWCR = (1 << TWINT) | (1 << TWEN); // start data reception
while (!(TWCR & (1 << TWINT))); 
return TWDR; 
} 
 
void i2c_off(void)
{ 
TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN); // send stop condition 
} 

//---------------------------------------------------------------------------
void show_deg(void)
{
static char count=0;
read_data(&a,&m);
lsm303dlh_ack=1;
if(!count){
a_avg.x = 0;
a_avg.y = 0;
a_avg.z  = 0;
m_avg.x  = 0;
m_avg.y  = 0;
m_avg.z  = 0;}

a_avg.x += a.x;
a_avg.y += a.y;
a_avg.z += a.z;
m_avg.x += m.x;
m_avg.y += m.y;
m_avg.z += m.z;
count++;

if(count>4){
   a_avg.x /= 5;
   a_avg.y /= 5;
   a_avg.z /= 5;
   m_avg.x /= 5;
   m_avg.y /= 5;
   m_avg.z /= 5;

int heading = get_heading(&a_avg, &m_avg, &p);
int degr=atan2(m.y,m.x);
Form1->StatusBar1->Panels->Items[0]->Text= IntToStr(heading);}//+IntToStr(degr);
}
//---------------------------------------------------------------------------
void read_data_raw(vector *a, vector *m)
{

   unsigned char axl = read_rx();
        unsigned char axh = read_rx();
   unsigned char ayl = read_rx();
        unsigned char ayh = read_rx();
   unsigned char azl = read_rx();
   unsigned char azh = read_rx();

        unsigned char mxh = read_rx();
   unsigned char mxl = read_rx();
   unsigned char myh = read_rx();
   unsigned char myl = read_rx();
   unsigned char mzh = read_rx();
   unsigned char mzl = read_rx();


   int ax = (int)(axh << 8) + axl;
   int ay = (int)(ayh << 8) + ayl;
   int az = (int)(azh << 8) + azl;
   int mx = (int)(mxh << 8) + mxl;
   int my = (int)(myh << 8) + myl;
   int mz = (int)(mzh << 8) + mzl;

        a->x = ax;
   a->y = ay;
   a->z = az;
   m->x = mx;
   m->y = my;
   m->z = mz;



}
//---------------------------------------------------------------------------
void read_data(vector *a, vector *m)
{
read_data_raw(a, m);
// shift and scale
m->x = (m->x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
m->y = (m->y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
m->z = (m->z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
}
//---------------------------------------------------------------------------
int get_heading(const vector *a, const vector *m, const vector *p)
{
   vector E;
   vector N;

   // cross magnetic vector (magnetic north + inclination) with "down" (acceleration vector) to produce "east"
   vector_cross(m, a, &E);
   vector_normalize(&E);

   // cross "down" with "east" to produce "north" (parallel to the ground)
   vector_cross(a, &E, &N);
   vector_normalize(&N);

   // compute heading
   int heading = atan2(vector_dot(&E, p), vector_dot(&N, p) * 180 / M_PI);
   if (heading < 0) heading += 360;
   return heading;
}
//---------------------------------------------------------------------------
void vector_cross(const vector *a, const vector *b, vector *out)
{
   out->x = a->y * b->z - a->z * b->y;
   out->y = a->z * b->x - a->x * b->z;
   out->z = a->x * b->y - a->y * b->x;
}

float vector_dot(const vector *a, const vector *b)
{
  return a->x * b->x + a->y * b->y + a->z * b->z;
}

void vector_normalize(vector *a)
{
   float mag = sqrt(vector_dot(a, a));
   a->x /= mag;
   a->y /= mag;
   a->z /= mag;
}