compas6

#include <stm32f10x.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "DWLOS_config.h"
#include "DWLOS.h"
#include "i2c_interrupt.h"

#define  __zLSM
#include "main.h"
#include "z_LSM.h"
#include "z_S65.h"



//==============================================================================
// LSM303 init
//==============================================================================
void LSM_init (void)
 {
  int LSM_tmp;

  LOG_ADD('L');

  if (LSM_step == 0 && I2C_IS_BLOCK())
   {
    Set_Timer (LSM_init, 100);
    return;
   }

  I2C_STOP_WAIT(LSM_I2C);
  I2C_BLOCK;

  if (LSM_step < 5)
    LSM_I2C -> M_OutFnc = &LSM_init;
  else
    LSM_I2C -> M_OutFnc = &LSM_init_end;

  LSM_tmp = LSM_step * 3;
  LSM_I2C -> S_adress = LSM_init_arr [LSM_tmp];
  I2C_MEM_ADR_NOT_TX(LSM_I2C);
  I2C_DIRECTION_TX(LSM_I2C);
  *(LSM_I2C -> M_Tx_Buffer + 0) = LSM_init_arr [LSM_tmp + 1];
  *(LSM_I2C -> M_Tx_Buffer + 1) = LSM_init_arr [LSM_tmp + 2];

  LSM_step ++;

  LSM_I2C -> Tx_count = 2;
  I2C_START(LSM_I2C);

 }

void LSM_init_end (void)
 {
  I2C_STOP_WAIT(LSM_I2C);

  if (LSM_step == 6)
    I2C_UNBLOCK;

  LSM_step = 0;
  AX_read_byte = 0;

  LSM_TEST_ON;

  LSM_I2C -> M_OutFnc = &I2C_idle_out;
  Set_Timer (MAG_read, 500);
  Set_Timer (AX_read, 750);
  LOG_ADD('l');
 }




//==============================================================================
// LSM303 read register
//==============================================================================
void MAG_read (void)
 {
  LOG_ADD('M');

  if (I2C_IS_BLOCK())
   {
    Set_Timer (MAG_read, 100);
   }
  else
   {
    I2C_BLOCK;
    LSM_I2C -> M_OutFnc = &MAG_read_end;
    LSM_I2C -> S_adress = I2C_M_ADR;
    I2C_MEM_ADR_TX(LSM_I2C);
    *(LSM_I2C -> I2C_MEM_ADRESS) = LSM303_OUT_X_M;
    I2C_DIRECTION_TX(LSM_I2C);
    LSM_I2C -> Tx_count = 0;
    LSM_I2C -> Rx_count = I2C_M_READ_COUNT;
    I2C_START(LSM_I2C);
   }
 }

void MAG_read_end (void)
 {
  uint16_t tmp;
  float ftmp;

  LOG_ADD('m');
  WDG_reload ();
  Set_Timer (MAG_read, 1000);
  LSM_TEST_ON;

  I2C_READ_END(LSM_I2C);
  I2C_STOP_WAIT(LSM_I2C);
  I2C_UNBLOCK;
  tmp = (int16_t) LSM_I2C -> I2C -> DR;
  tmp ++;

  MAG_X = ((*(LSM_I2C -> M_Rx_Buffer + 0)) << 8) | ((*(LSM_I2C -> M_Rx_Buffer + 1)) & 0xFF);
  MAG_Y = ((*(LSM_I2C -> M_Rx_Buffer + 2)) << 8) | ((*(LSM_I2C -> M_Rx_Buffer + 3)) & 0xFF);
  MAG_Z = ((*(LSM_I2C -> M_Rx_Buffer + 4)) << 8) | ((*(LSM_I2C -> M_Rx_Buffer + 5)) & 0xFF);

/*  S65 -> x = 3;
  S65 -> y = 130;
//  sprintf (S65 -> txt, "%4d %4d %4d", MAG_X, MAG_Y, MAG_Z);
  sprintf (S65 -> txt, "%4d", (signed char)(((MAG_X >> 3) & 0x1FFF) | ((MAG_X & (1 << 15)) >> 8)));
  S65 -> len = strlen (S65 -> txt);
  S65_puts_n ();// */

  S65 -> x = 11;
  S65 -> y = 121;
//  S65 -> txt [0] = (char)(((MAG_X >> 3) & 0x1FFF) | ((MAG_X & (1 << 15)) >> 8));
//  S65 -> txt [1] = (char)(((MAG_Y >> 3) & 0x1FFF) | ((MAG_Y & (1 << 15)) >> 8));

  ftmp = atan2 (MAG_Y, MAG_X);
//  S65 -> txt [0] = (signed char)(20.0*cos (ftmp));
//  S65 -> txt [1] = (signed char)(20.0*sin (ftmp));
  memcpy (S65 -> txt, &ftmp, sizeof (float));
  S65_puts_M ();
 }


void AX_read (void)
 {
  LOG_ADD('A');

  if (I2C_IS_BLOCK())
   {
    Set_Timer (AX_read, 100);
   }
  else
   {
    I2C_BLOCK;
    LSM_I2C -> M_OutFnc = &AX_read_end;
    LSM_I2C -> S_adress = I2C_A_ADR;
    I2C_MEM_ADR_TX(LSM_I2C);
    *(LSM_I2C -> I2C_MEM_ADRESS) = LSM303_OUT_X_A + AX_read_byte;
    I2C_DIRECTION_TX(LSM_I2C);
    LSM_I2C -> Tx_count = 0;
    LSM_I2C -> Rx_count = 1;
    I2C_START(LSM_I2C);
   }
 }

void AX_read_end (void)
 {
  uint16_t tmp;
  float ftmp;

  static int16_t AX_X, AX_Y, AX_Z;
  static int16_t AX_X_prev, AX_Y_prev, AX_Z_prev;

  static float AX_I1_X, AX_I1_Y, AX_I1_Z;
  static float AX_I1_X_prev, AX_I1_Y_prev, AX_I1_Z_prev;

  static float AX_I2_X, AX_I2_Y, AX_I2_Z;

  LOG_ADD('a');
  LSM_TEST_ON;

  I2C_READ_END(LSM_I2C);
  I2C_STOP_WAIT(LSM_I2C);
  I2C_UNBLOCK;

  tmp = (int16_t) LSM_I2C -> I2C -> DR;
  tmp ++;

  if (AX_read_byte < 6)
   {
    AX_Buffer [AX_read_byte] = *(LSM_I2C -> M_Rx_Buffer);
    AX_read_byte ++;

    Set_Task (AX_read);
   }
  else
   {
    WDG_reload ();
    AX_read_byte = 0;
    Set_Timer (AX_read, 100);


    LOG_ADD('P');

    AX_X = (int16_t)((AX_Buffer [1] << 8) | (AX_Buffer [0] & 0xFF));
    AX_Y = (int16_t)((AX_Buffer [3] << 8) | (AX_Buffer [2] & 0xFF));
    AX_Z = (int16_t)((AX_Buffer [5] << 8) | (AX_Buffer [4] & 0xFF));

    if (AX_X < 500 && AX_X > -500) AX_X = 0;
    if (AX_Y < 500 && AX_Y > -500) AX_Y = 0;

    ftmp = AX_X * 0.00006;
    AX_I1_X += (AX_X_prev + (ftmp)) * 0.1 / 2;
    AX_X_prev = ftmp;

    ftmp = AX_Y * 0.00006;
    AX_I1_Y += (AX_Y_prev + (ftmp)) * 0.1 / 2;
    AX_Y_prev = ftmp;

    AX_I2_X += (AX_I1_X_prev + AX_I1_X) * 0.1 / 2;
    AX_I2_Y += (AX_I1_Y_prev + AX_I1_Y) * 0.1 / 2;

    S65 -> x = 1;
    S65 -> y = 147;
//    sprintf (S65 -> txt, " %4d %4d", AX_X, AX_Y);
    sprintf (S65 -> txt, "_%5.3f %5.3f", AX_I1_X, AX_I2_X);
    S65 -> len = strlen (S65 -> txt);
    S65_puts_n ();// */

/*    S65 -> x = 71;
    S65 -> y = 130;
    sprintf (S65 -> txt, " %5d", AX_X);
    S65 -> len = strlen (S65 -> txt);
    S65_puts_n ();// */

    S65 -> x = 71;
    S65 -> y = 121;
    S65 -> txt [0] = (signed char)(AX_X * 0.00076);
    S65 -> txt [1] = (signed char)(AX_Y * 0.00076);
    S65_puts_A ();

    AX_I1_X_prev = AX_I1_X;
    AX_I1_Y_prev = AX_I1_Y;
    AX_I1_Z_prev = AX_I1_Z;
   }
 }