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@6:2605aa78ef9f, 2017-04-06 (annotated)

Committer:
libv2001
Date:
Thu Apr 06 11:27:04 2017 +0000
Revision:
6:2605aa78ef9f
Parent:
5:8142f455454b 
Underscores!!!!

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