CROTUS / Mbed 2 deprecated ProjetCasque

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed CROTUS_XBee mbed-rtos Crotus_Com

Magneto.cpp@0:28d5622d1a3e, 2017-03-12 (annotated)

Committer:
libv2001
Date:
Sun Mar 12 14:58:06 2017 +0000
Revision:
0:28d5622d1a3e
Child:
1:3591e7df4ff4
Lecture et conversion des donn?es de l'acc?l?rom?tre et du magn?tom?tre.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
libv2001 0:28d5622d1a3e 1 #include "Magneto.h"
libv2001 0:28d5622d1a3e 2
libv2001 0:28d5622d1a3e 3 #define PI 3.14159265358979323f
libv2001 0:28d5622d1a3e 4
libv2001 0:28d5622d1a3e 5 namespace {
libv2001 0:28d5622d1a3e 6 char cmd[2];
libv2001 0:28d5622d1a3e 7 char buffer[6];
libv2001 0:28d5622d1a3e 8
libv2001 0:28d5622d1a3e 9 Ticker calibrationTicker;
libv2001 0:28d5622d1a3e 10
libv2001 0:28d5622d1a3e 11 int16_t minX = OR_MAX_VALUE; // The maximum value is put in the min so real values will always be smaller
libv2001 0:28d5622d1a3e 12 int16_t maxX = OR_MIN_VALUE; // The minimum value is put in the max so real values will always be bigger
libv2001 0:28d5622d1a3e 13 int16_t minY = OR_MAX_VALUE; // The maximum value is put in the min so real values will always be smaller
libv2001 0:28d5622d1a3e 14 int16_t maxY = OR_MIN_VALUE; // The minimum value is put in the max so real values will always be bigger
libv2001 0:28d5622d1a3e 15
libv2001 0:28d5622d1a3e 16 int16_t offsetX = 0;
libv2001 0:28d5622d1a3e 17 int16_t offsetY = 0;
libv2001 0:28d5622d1a3e 18
libv2001 0:28d5622d1a3e 19 int16_t scaleX = 0;
libv2001 0:28d5622d1a3e 20 int16_t scaleY = 0;
libv2001 0:28d5622d1a3e 21
libv2001 0:28d5622d1a3e 22 void _UpdateScaleAndOffset(){
libv2001 0:28d5622d1a3e 23 offsetX = (maxX + minX)/2;
libv2001 0:28d5622d1a3e 24 scaleX = maxX - offsetX;
libv2001 0:28d5622d1a3e 25
libv2001 0:28d5622d1a3e 26 offsetY = (maxY + minY)/2;
libv2001 0:28d5622d1a3e 27 scaleY = maxY - offsetY;
libv2001 0:28d5622d1a3e 28 }
libv2001 0:28d5622d1a3e 29
libv2001 0:28d5622d1a3e 30 void _UpdateCalibration(int16_t x, int16_t y){
libv2001 0:28d5622d1a3e 31 bool changed = false;
libv2001 0:28d5622d1a3e 32
libv2001 0:28d5622d1a3e 33 if (x < minX){
libv2001 0:28d5622d1a3e 34 minX = x;
libv2001 0:28d5622d1a3e 35 changed = true;
libv2001 0:28d5622d1a3e 36 }
libv2001 0:28d5622d1a3e 37 if (x > maxX){
libv2001 0:28d5622d1a3e 38 maxX = x;
libv2001 0:28d5622d1a3e 39 changed = true;
libv2001 0:28d5622d1a3e 40 }
libv2001 0:28d5622d1a3e 41
libv2001 0:28d5622d1a3e 42 if (y < minY){
libv2001 0:28d5622d1a3e 43 minY = y;
libv2001 0:28d5622d1a3e 44 changed = true;
libv2001 0:28d5622d1a3e 45 }
libv2001 0:28d5622d1a3e 46 if (y > maxY){
libv2001 0:28d5622d1a3e 47 maxY = y;
libv2001 0:28d5622d1a3e 48 changed = true;
libv2001 0:28d5622d1a3e 49 }
libv2001 0:28d5622d1a3e 50
libv2001 0:28d5622d1a3e 51 if(changed || true){
libv2001 0:28d5622d1a3e 52 _UpdateScaleAndOffset();
libv2001 0:28d5622d1a3e 53 }
libv2001 0:28d5622d1a3e 54 }
libv2001 0:28d5622d1a3e 55
libv2001 0:28d5622d1a3e 56 LocalFileSystem local("local");
libv2001 0:28d5622d1a3e 57
libv2001 0:28d5622d1a3e 58 void _LoadCalibration(){
libv2001 0:28d5622d1a3e 59 FILE *fp = fopen("/local/magneto.cfg", "r"); // Open "out.txt" on the local file system for writing
libv2001 0:28d5622d1a3e 60 // File doesn't exist
libv2001 0:28d5622d1a3e 61 if (fp == 0){
libv2001 0:28d5622d1a3e 62 printf("File doesn't exist!!\r\n");
libv2001 0:28d5622d1a3e 63 return;
libv2001 0:28d5622d1a3e 64 }
libv2001 0:28d5622d1a3e 65
libv2001 0:28d5622d1a3e 66 fscanf(fp,"%04X %04X %04X %04X", &minX, &maxX, &minY, &maxY);
libv2001 0:28d5622d1a3e 67 fclose(fp);
libv2001 0:28d5622d1a3e 68
libv2001 0:28d5622d1a3e 69 _UpdateScaleAndOffset();
libv2001 0:28d5622d1a3e 70 }
libv2001 0:28d5622d1a3e 71
libv2001 0:28d5622d1a3e 72 void _SaveCalibration(){
libv2001 0:28d5622d1a3e 73 FILE *fp = fopen("/local/magneto.cfg", "w"); // Open "out.txt" on the local file system for writing
libv2001 0:28d5622d1a3e 74 fprintf(fp, "%04X %04X %04X %04X", minX & 0xFFFF, maxX & 0xFFFF, minY & 0xFFFF, maxY & 0xFFFF);
libv2001 0:28d5622d1a3e 75 fclose(fp);
libv2001 0:28d5622d1a3e 76 }
libv2001 0:28d5622d1a3e 77 };
libv2001 0:28d5622d1a3e 78
libv2001 0:28d5622d1a3e 79 Magneto::Magneto(I2C & i2c) :
libv2001 0:28d5622d1a3e 80 _i2c(&i2c)
libv2001 0:28d5622d1a3e 81 {
libv2001 0:28d5622d1a3e 82 _LoadCalibration();
libv2001 0:28d5622d1a3e 83
libv2001 0:28d5622d1a3e 84 // Every 10 minutes
libv2001 0:28d5622d1a3e 85 calibrationTicker.attach(_SaveCalibration, 6);
libv2001 0:28d5622d1a3e 86 }
libv2001 0:28d5622d1a3e 87
libv2001 0:28d5622d1a3e 88 bool Magneto::Test(){
libv2001 0:28d5622d1a3e 89 cmd[0] = MAGNETO_RA_ID_A;
libv2001 0:28d5622d1a3e 90
libv2001 0:28d5622d1a3e 91 _i2c->write(MAGNETO_ADDRESS, cmd, 1, true);
libv2001 0:28d5622d1a3e 92 _i2c->read(MAGNETO_ADDRESS, buffer, 3);
libv2001 0:28d5622d1a3e 93
libv2001 0:28d5622d1a3e 94 return (buffer[0] == IR_A_VALUE) &&
libv2001 0:28d5622d1a3e 95 (buffer[1] == IR_B_VALUE) &&
libv2001 0:28d5622d1a3e 96 (buffer[2] == IR_C_VALUE);
libv2001 0:28d5622d1a3e 97 }
libv2001 0:28d5622d1a3e 98
libv2001 0:28d5622d1a3e 99 void Magneto::ActivateDevice(){
libv2001 0:28d5622d1a3e 100 cmd[0] = MAGNETO_RA_CONFIG_A;
libv2001 0:28d5622d1a3e 101 cmd[1] = (CRA_SAMPLE_AVG_8 << CRA_SAMPLE_AVG_BIT) | (CRA_DATA_RATE_15 << CRA_DATA_RATE_BIT);
libv2001 0:28d5622d1a3e 102
libv2001 0:28d5622d1a3e 103 _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
libv2001 0:28d5622d1a3e 104
libv2001 0:28d5622d1a3e 105 cmd[0] = MAGNETO_RA_CONFIG_B;
libv2001 0:28d5622d1a3e 106 cmd[1] = CRB_GAIN_130 << CRB_GAIN_BIT;
libv2001 0:28d5622d1a3e 107
libv2001 0:28d5622d1a3e 108 _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
libv2001 0:28d5622d1a3e 109
libv2001 0:28d5622d1a3e 110 cmd[0] = MAGNETO_RA_MODE;
libv2001 0:28d5622d1a3e 111 cmd[1] = MR_MODE_CONTINUOUS << MR_MODE_BIT;
libv2001 0:28d5622d1a3e 112
libv2001 0:28d5622d1a3e 113 _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
libv2001 0:28d5622d1a3e 114 }
libv2001 0:28d5622d1a3e 115
libv2001 0:28d5622d1a3e 116 void Magneto::ReadXZY(int16_t* raw){
libv2001 0:28d5622d1a3e 117 cmd[0] = MAGNETO_RA_X_MSB;
libv2001 0:28d5622d1a3e 118
libv2001 0:28d5622d1a3e 119 _i2c->write(MAGNETO_ADDRESS, cmd, 1, true);
libv2001 0:28d5622d1a3e 120 _i2c->read(MAGNETO_ADDRESS, buffer, 6);
libv2001 0:28d5622d1a3e 121
libv2001 0:28d5622d1a3e 122 for(int i = 0; i < 6; i += 2){
libv2001 0:28d5622d1a3e 123 int16_t value = buffer[i];
libv2001 0:28d5622d1a3e 124 value = (value << 8) | buffer[i+1];
libv2001 0:28d5622d1a3e 125 raw[i >> 1] = value;
libv2001 0:28d5622d1a3e 126 }
libv2001 0:28d5622d1a3e 127 }
libv2001 0:28d5622d1a3e 128
libv2001 0:28d5622d1a3e 129 int16_t Magneto::GetHeadingXY(){
libv2001 0:28d5622d1a3e 130 int16_t data[3] = {0};
libv2001 0:28d5622d1a3e 131
libv2001 0:28d5622d1a3e 132 ReadXZY(data);
libv2001 0:28d5622d1a3e 133
libv2001 0:28d5622d1a3e 134 int16_t x = data[0];
libv2001 0:28d5622d1a3e 135 int16_t y = data[2];
libv2001 0:28d5622d1a3e 136
libv2001 0:28d5622d1a3e 137 _UpdateCalibration(x, y);
libv2001 0:28d5622d1a3e 138
libv2001 0:28d5622d1a3e 139 if (scaleX == 0 || scaleY == 0){
libv2001 0:28d5622d1a3e 140 return HEADING_ERROR;
libv2001 0:28d5622d1a3e 141 }
libv2001 0:28d5622d1a3e 142
libv2001 0:28d5622d1a3e 143 float scaledX = (float)(x - offsetX) / (float) scaleX; // Scaled value between -1 and 1
libv2001 0:28d5622d1a3e 144 float scaledY = (float)(y - offsetY) / (float) scaleY; // Scaled value between -1 and 1
libv2001 0:28d5622d1a3e 145
libv2001 0:28d5622d1a3e 146 int16_t heading = atan2(scaledY, scaledX) * 180.0f / PI;
libv2001 0:28d5622d1a3e 147
libv2001 0:28d5622d1a3e 148 if (heading < 0) {
libv2001 0:28d5622d1a3e 149 heading += 360;
libv2001 0:28d5622d1a3e 150 } else if (heading > 360) {
libv2001 0:28d5622d1a3e 151 heading -= 360;
libv2001 0:28d5622d1a3e 152 }
libv2001 0:28d5622d1a3e 153
libv2001 0:28d5622d1a3e 154 return heading;
libv2001 0:28d5622d1a3e 155 }
libv2001 0:28d5622d1a3e 156
libv2001 0:28d5622d1a3e 157
libv2001 0:28d5622d1a3e 158 void Magneto::ResetDevice(){
libv2001 0:28d5622d1a3e 159 minX = OR_MAX_VALUE;
libv2001 0:28d5622d1a3e 160 maxX = OR_MIN_VALUE;
libv2001 0:28d5622d1a3e 161 minY = OR_MAX_VALUE;
libv2001 0:28d5622d1a3e 162 maxY = OR_MIN_VALUE;
libv2001 0:28d5622d1a3e 163
libv2001 0:28d5622d1a3e 164 offsetX = 0;
libv2001 0:28d5622d1a3e 165 offsetY = 0;
libv2001 0:28d5622d1a3e 166
libv2001 0:28d5622d1a3e 167 scaleX = 0;
libv2001 0:28d5622d1a3e 168 scaleY = 0;
libv2001 0:28d5622d1a3e 169 }