calibrate-mag.c

#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <ncurses.h>

#include "edison-9dof-i2c.h"

#define MAX_MAG_ERROR 100
#define BIAS_FILENAME "mag.bias"

typedef struct {
  Triplet min;
  Triplet max;
  Triplet bias;
} MagDistribution;

void update_mag_bias (Triplet data, MagDistribution *m_dist)
{
  if (data.x < m_dist->min.x) {
    m_dist->min.x = data.x;
    m_dist->bias.x = (m_dist->max.x + m_dist->min.x) / 2;
  } else if (data.x > m_dist->max.x) {
    m_dist->max.x = data.x;
    m_dist->bias.x = (m_dist->max.x + m_dist->min.x) / 2;
  }
  if (data.y < m_dist->min.y) {
    m_dist->min.y = data.y;
    m_dist->bias.y = (m_dist->max.y + m_dist->min.y) / 2;
  } else if (data.y > m_dist->max.y) {
    m_dist->max.y = data.y;
    m_dist->bias.y = (m_dist->max.y + m_dist->min.y) / 2;
  }
  if (data.z < m_dist->min.z) {
    m_dist->min.z = data.z;
    m_dist->bias.z = (m_dist->max.z + m_dist->min.z) / 2;
  } else if (data.z > m_dist->max.z) {
    m_dist->max.z = data.z;
    m_dist->bias.z = (m_dist->max.z + m_dist->min.z) / 2;
  }
}

int get_col(int width, float f)
{
  /* assume values are in [-10000 , +10000] */
  return width/2 + (int)((width / 2) * f / 10000);
}

void print_distribution (Triplet min, Triplet max, Triplet data, int width, char limit, char sensor)
{
  mvaddch(3, get_col (width, min.x), limit);
  mvaddch(3, get_col (width, data.x), sensor);
  mvaddch(3, get_col (width, max.x), limit);
  mvaddch(4, get_col (width, min.y), limit);
  mvaddch(4, get_col (width, data.y), sensor);
  mvaddch(4, get_col (width, max.y), limit);
  mvaddch(5, get_col (width, min.z), limit);
  mvaddch(5, get_col (width, data.z), sensor);
  mvaddch(5, get_col (width, max.z), limit);
}

int calibrate(int file, int width, Triplet *bias, FTriplet *scale)
{
  Triplet data = {0};
  MagDistribution m_dist = {{0},{0},{0}};
  Triplet diameters;
  int avg_diameter;

  while (1) {
    print_distribution (m_dist.min, m_dist.max, data, width, ' ', ' ');

    read_triplet (file, XM_ADDRESS, OUT_X_L_M, &data);
    update_mag_bias (data, &m_dist);

    print_distribution (m_dist.min, m_dist.max, data, width, '|', '*');

    move (6, 0);
    refresh ();
    usleep (20000);
    if (getch () != ERR)
      break;
  }

  bias->x = m_dist.bias.x;
  bias->y = m_dist.bias.y;
  bias->z = m_dist.bias.z;

  /* Try to scale the axes (crude transform from ellipse to sphere) */
  diameters.x = abs (m_dist.max.x - m_dist.min.x);
  diameters.y = abs (m_dist.max.y - m_dist.min.y);
  diameters.z = abs (m_dist.max.z - m_dist.min.z);
  avg_diameter = (diameters.x + diameters.y + diameters.z) / 3;
  scale->x = avg_diameter / (float)diameters.x;
  scale->y = avg_diameter / (float)diameters.y;
  scale->z = avg_diameter / (float)diameters.z;

  return 1;
}

int main (int argc, char **argv)
{
  FILE *output;
  int file, row, col;
  Triplet bias;
  FTriplet scale;

  initscr ();
  nodelay (stdscr, 1);
  noecho ();
  cbreak ();
  curs_set (0);
  getmaxyx (stdscr, row, col);
  (void)row;

  file = init_device (I2C_DEV_NAME);
  if (file == 0)
    return 1;

  init_mag (file, MAG_SCALE_2GS);

  output = fopen(BIAS_FILENAME, "w");
  if (!output){
    printf("Error opening '"BIAS_FILENAME"' for output.\n");
    return 1;
  }
  mvprintw (0, 0, "Calibrating: Turn the Edison around all axes.");
  mvprintw (1, 0, "Try to find the absolute furthest limits of each axis.");
  mvaddch (3, 0, 'X');
  mvaddch (4, 0, 'Y');
  mvaddch (5, 0, 'Z');

  mvprintw (7, 0, "Press any key when done.");

  calibrate (file, col, &bias, &scale);

  endwin ();

  fprintf(output, "m_bias %d %d %d\n", bias.x, bias.y, bias.z);
  fprintf(output, "m_scale %f %f %f\n", scale.x, scale.y, scale.z);
  fclose (output);

  printf ("Calibration succesful, wrote '"BIAS_FILENAME"'.\n");
  return 0;
}