Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@18:b7182d5ad8d5, 2017-02-21 (annotated)

Committer:
masahikofukasawa
Date:
Tue Feb 21 23:46:12 2017 +0000
Revision:
18:b7182d5ad8d5
Parent:
16:d85be9bafb80
Child:
19:8dcc4f323bdc
Add AK09918 and debugged for getOperationMode() for e-compasses.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 0:7a00359e701e 1 #include "akmakd.h"
masahikofukasawa 0:7a00359e701e 2 #include "ak8963.h"
masahikofukasawa 0:7a00359e701e 3 #include "ak099xx.h"
masahikofukasawa 0:7a00359e701e 4 #include "debug.h"
masahikofukasawa 0:7a00359e701e 5
masahikofukasawa 0:7a00359e701e 6 /**
masahikofukasawa 0:7a00359e701e 7 * Constructor.
masahikofukasawa 0:7a00359e701e 8 *
masahikofukasawa 0:7a00359e701e 9 */
masahikofukasawa 0:7a00359e701e 10 AkmAkd::AkmAkd(){
masahikofukasawa 0:7a00359e701e 11 event = false;
masahikofukasawa 0:7a00359e701e 12 compass = NULL;
masahikofukasawa 0:7a00359e701e 13 drdy = NULL;
masahikofukasawa 0:7a00359e701e 14 sensorName = "";
masahikofukasawa 0:7a00359e701e 15 }
masahikofukasawa 0:7a00359e701e 16
masahikofukasawa 0:7a00359e701e 17 /**
masahikofukasawa 0:7a00359e701e 18 * Destructor.
masahikofukasawa 0:7a00359e701e 19 *
masahikofukasawa 0:7a00359e701e 20 */
masahikofukasawa 0:7a00359e701e 21 AkmAkd::~AkmAkd(){
masahikofukasawa 0:7a00359e701e 22 drdy->rise(0);
masahikofukasawa 0:7a00359e701e 23 if (compass) delete compass;
masahikofukasawa 0:7a00359e701e 24 if (drdy) delete drdy;
masahikofukasawa 0:7a00359e701e 25 }
masahikofukasawa 0:7a00359e701e 26
masahikofukasawa 0:7a00359e701e 27 AkmSensor::Status AkmAkd::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 0:7a00359e701e 28 primaryId = id;
masahikofukasawa 0:7a00359e701e 29 subId = subid;
masahikofukasawa 0:7a00359e701e 30
masahikofukasawa 0:7a00359e701e 31 AkmECompass::SensorType type;
masahikofukasawa 7:e269411568c9 32 AK099XX* ak099xx;
masahikofukasawa 7:e269411568c9 33 AK8963* ak8963;
masahikofukasawa 9:6fa3e7b17c27 34
masahikofukasawa 9:6fa3e7b17c27 35 mode = AkmECompass::MODE_POWER_DOWN;
masahikofukasawa 9:6fa3e7b17c27 36 nsf = AkmECompass::NSF_DISABLE;
masahikofukasawa 9:6fa3e7b17c27 37 sdr = AkmECompass::SDR_LOW_POWER_DRIVE;
masahikofukasawa 9:6fa3e7b17c27 38
masahikofukasawa 0:7a00359e701e 39 if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
masahikofukasawa 0:7a00359e701e 40
masahikofukasawa 7:e269411568c9 41 drdy = new InterruptIn(I2C_DRDY);
masahikofukasawa 7:e269411568c9 42 drdy->rise(0);
masahikofukasawa 7:e269411568c9 43
masahikofukasawa 0:7a00359e701e 44 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 0:7a00359e701e 45 i2c->frequency(I2C_SPEED_100KHZ);
masahikofukasawa 0:7a00359e701e 46
masahikofukasawa 0:7a00359e701e 47 switch(subid){
masahikofukasawa 0:7a00359e701e 48 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 0:7a00359e701e 49 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 0:7a00359e701e 50 type = AkmECompass::AK8963;
masahikofukasawa 0:7a00359e701e 51 ak8963 = new AK8963();
masahikofukasawa 0:7a00359e701e 52 compass = ak8963;
masahikofukasawa 0:7a00359e701e 53 AkmAkd::sensorName = "AK8963";
masahikofukasawa 0:7a00359e701e 54 break;
masahikofukasawa 10:5c69b067d88a 55 case AkmAkd::SUB_ID_AK09911C:
masahikofukasawa 0:7a00359e701e 56 type = AkmECompass::AK09911;
masahikofukasawa 0:7a00359e701e 57 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 58 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 59 AkmAkd::sensorName = "AK09911C";
masahikofukasawa 0:7a00359e701e 60 break;
masahikofukasawa 10:5c69b067d88a 61 case AkmAkd::SUB_ID_AK09912C:
masahikofukasawa 0:7a00359e701e 62 type = AkmECompass::AK09912;
masahikofukasawa 0:7a00359e701e 63 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 64 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 65 AkmAkd::sensorName = "AK09912C";
masahikofukasawa 0:7a00359e701e 66 break;
masahikofukasawa 10:5c69b067d88a 67 case AkmAkd::SUB_ID_AK09915C:
masahikofukasawa 0:7a00359e701e 68 type = AkmECompass::AK09915;
masahikofukasawa 0:7a00359e701e 69 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 70 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 71 AkmAkd::sensorName = "AK09915C";
masahikofukasawa 10:5c69b067d88a 72 break;
masahikofukasawa 10:5c69b067d88a 73 case AkmAkd::SUB_ID_AK09915D:
masahikofukasawa 10:5c69b067d88a 74 type = AkmECompass::AK09915;
masahikofukasawa 10:5c69b067d88a 75 ak099xx = new AK099XX();
masahikofukasawa 10:5c69b067d88a 76 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 77 AkmAkd::sensorName = "AK09915D";
masahikofukasawa 0:7a00359e701e 78 break;
masahikofukasawa 0:7a00359e701e 79 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 0:7a00359e701e 80 type = AkmECompass::AK09916C;
masahikofukasawa 0:7a00359e701e 81 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 82 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 83 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 0:7a00359e701e 84 break;
masahikofukasawa 10:5c69b067d88a 85 case AkmAkd::SUB_ID_AK09916D:
masahikofukasawa 10:5c69b067d88a 86 type = AkmECompass::AK09916D;
masahikofukasawa 10:5c69b067d88a 87 ak099xx = new AK099XX();
masahikofukasawa 10:5c69b067d88a 88 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 89 AkmAkd::sensorName = "AK09916D";
masahikofukasawa 10:5c69b067d88a 90 break;
masahikofukasawa 18:b7182d5ad8d5 91 case AkmAkd::SUB_ID_AK09918:
masahikofukasawa 18:b7182d5ad8d5 92 type = AkmECompass::AK09918;
masahikofukasawa 18:b7182d5ad8d5 93 ak099xx = new AK099XX();
masahikofukasawa 18:b7182d5ad8d5 94 compass = ak099xx;
masahikofukasawa 18:b7182d5ad8d5 95 AkmAkd::sensorName = "AK09918";
masahikofukasawa 18:b7182d5ad8d5 96 break;
masahikofukasawa 0:7a00359e701e 97 default:
masahikofukasawa 0:7a00359e701e 98 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 99 // break;
masahikofukasawa 0:7a00359e701e 100 }
masahikofukasawa 0:7a00359e701e 101
masahikofukasawa 0:7a00359e701e 102 bool foundSensor = false;
masahikofukasawa 0:7a00359e701e 103
masahikofukasawa 0:7a00359e701e 104 AkmECompass::SlaveAddress slaveAddr[]
masahikofukasawa 0:7a00359e701e 105 = { AkmECompass::SLAVE_ADDR_1,
masahikofukasawa 0:7a00359e701e 106 AkmECompass::SLAVE_ADDR_2,
masahikofukasawa 0:7a00359e701e 107 AkmECompass::SLAVE_ADDR_3,
masahikofukasawa 0:7a00359e701e 108 AkmECompass::SLAVE_ADDR_4};
masahikofukasawa 0:7a00359e701e 109
masahikofukasawa 0:7a00359e701e 110 for(int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 0:7a00359e701e 111 {
masahikofukasawa 0:7a00359e701e 112 compass->init(i2c, slaveAddr[i], type);
masahikofukasawa 0:7a00359e701e 113 // Checks connectivity
masahikofukasawa 0:7a00359e701e 114 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 115 // found
masahikofukasawa 0:7a00359e701e 116 foundSensor = true;
masahikofukasawa 0:7a00359e701e 117 break;
masahikofukasawa 0:7a00359e701e 118 }
masahikofukasawa 0:7a00359e701e 119 }
masahikofukasawa 7:e269411568c9 120 if(foundSensor != true){
masahikofukasawa 7:e269411568c9 121 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 122 }
masahikofukasawa 0:7a00359e701e 123
masahikofukasawa 0:7a00359e701e 124 // read a data to reset DRDY
masahikofukasawa 18:b7182d5ad8d5 125 // AkmECompass::MagneticVector mag;
masahikofukasawa 18:b7182d5ad8d5 126 // compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 127
masahikofukasawa 0:7a00359e701e 128 }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
masahikofukasawa 7:e269411568c9 129
masahikofukasawa 7:e269411568c9 130 drdy = new InterruptIn(SPI_DRDY);
masahikofukasawa 7:e269411568c9 131 drdy->rise(0);
masahikofukasawa 0:7a00359e701e 132
masahikofukasawa 7:e269411568c9 133 SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 7:e269411568c9 134 spi->format(8,3); // 8bit, Mode=3
masahikofukasawa 7:e269411568c9 135 spi->frequency(1000000);
masahikofukasawa 7:e269411568c9 136
masahikofukasawa 7:e269411568c9 137 DigitalOut* cs = new DigitalOut(SPI_CS);
masahikofukasawa 7:e269411568c9 138
masahikofukasawa 7:e269411568c9 139 switch(subid){
masahikofukasawa 7:e269411568c9 140 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 7:e269411568c9 141 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 7:e269411568c9 142 type = AkmECompass::AK8963;
masahikofukasawa 7:e269411568c9 143 ak8963 = new AK8963();
masahikofukasawa 7:e269411568c9 144 compass = ak8963;
masahikofukasawa 7:e269411568c9 145 AkmAkd::sensorName = "AK8963";
masahikofukasawa 7:e269411568c9 146 break;
masahikofukasawa 10:5c69b067d88a 147 case AkmAkd::SUB_ID_AK09911C:
masahikofukasawa 7:e269411568c9 148 type = AkmECompass::AK09911;
masahikofukasawa 10:5c69b067d88a 149 AkmAkd::sensorName = "AK09911C";
masahikofukasawa 7:e269411568c9 150 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 151 case AkmAkd::SUB_ID_AK09912C:
masahikofukasawa 7:e269411568c9 152 type = AkmECompass::AK09912;
masahikofukasawa 7:e269411568c9 153 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 154 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 155 AkmAkd::sensorName = "AK09912C";
masahikofukasawa 7:e269411568c9 156 break;
masahikofukasawa 10:5c69b067d88a 157 case AkmAkd::SUB_ID_AK09915C:
masahikofukasawa 7:e269411568c9 158 type = AkmECompass::AK09915;
masahikofukasawa 7:e269411568c9 159 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 160 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 161 AkmAkd::sensorName = "AK09915C";
masahikofukasawa 10:5c69b067d88a 162 break;
masahikofukasawa 10:5c69b067d88a 163 case AkmAkd::SUB_ID_AK09915D:
masahikofukasawa 10:5c69b067d88a 164 type = AkmECompass::AK09915;
masahikofukasawa 18:b7182d5ad8d5 165 ak099xx = new AK099XX();
masahikofukasawa 18:b7182d5ad8d5 166 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 167 AkmAkd::sensorName = "AK09915D";
masahikofukasawa 7:e269411568c9 168 break;
masahikofukasawa 7:e269411568c9 169 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 7:e269411568c9 170 type = AkmECompass::AK09916C;
masahikofukasawa 7:e269411568c9 171 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 7:e269411568c9 172 // doesn't support SPI
masahikofukasawa 7:e269411568c9 173 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 174 case AkmAkd::SUB_ID_AK09916D:
masahikofukasawa 10:5c69b067d88a 175 type = AkmECompass::AK09916D;
masahikofukasawa 10:5c69b067d88a 176 AkmAkd::sensorName = "AK09916D";
masahikofukasawa 10:5c69b067d88a 177 // doesn't support SPI
masahikofukasawa 10:5c69b067d88a 178 return AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 179 case AkmAkd::SUB_ID_AK09918:
masahikofukasawa 18:b7182d5ad8d5 180 type = AkmECompass::AK09918;
masahikofukasawa 18:b7182d5ad8d5 181 AkmAkd::sensorName = "AK09918";
masahikofukasawa 18:b7182d5ad8d5 182 // doesn't support SPI
masahikofukasawa 18:b7182d5ad8d5 183 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 184 default:
masahikofukasawa 7:e269411568c9 185 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 186 // break;
masahikofukasawa 7:e269411568c9 187 }
masahikofukasawa 7:e269411568c9 188
masahikofukasawa 7:e269411568c9 189 bool foundSensor = false;
masahikofukasawa 7:e269411568c9 190 compass->init(spi, cs, type);
masahikofukasawa 7:e269411568c9 191 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 7:e269411568c9 192 foundSensor = true;
masahikofukasawa 7:e269411568c9 193 }
masahikofukasawa 7:e269411568c9 194 if(foundSensor != true){
masahikofukasawa 11:cef8dc1cf010 195 MSG("#failed checkConnetion(SPI). %s\r\n",AkmAkd::sensorName);
masahikofukasawa 7:e269411568c9 196 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 197 }
masahikofukasawa 7:e269411568c9 198 // read a data to reset DRDY
masahikofukasawa 18:b7182d5ad8d5 199 // AkmECompass::MagneticVector mag;
masahikofukasawa 18:b7182d5ad8d5 200 // compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 201 }
masahikofukasawa 0:7a00359e701e 202
masahikofukasawa 11:cef8dc1cf010 203 MSG("#%s detected.\r\n", AkmAkd::sensorName);
masahikofukasawa 0:7a00359e701e 204 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 205 }
masahikofukasawa 0:7a00359e701e 206
masahikofukasawa 0:7a00359e701e 207
masahikofukasawa 0:7a00359e701e 208 void AkmAkd::checkDRDY(){
masahikofukasawa 0:7a00359e701e 209 AkmECompass::Status status = compass->isDataReady();
masahikofukasawa 0:7a00359e701e 210 if( status == AkmECompass::DATA_READY ||
masahikofukasawa 0:7a00359e701e 211 status == AkmECompass::DATA_OVER_RUN ) event = true;
masahikofukasawa 0:7a00359e701e 212 }
masahikofukasawa 0:7a00359e701e 213
masahikofukasawa 0:7a00359e701e 214 void AkmAkd::detectDRDY(){
masahikofukasawa 0:7a00359e701e 215 event = true;
masahikofukasawa 0:7a00359e701e 216 }
masahikofukasawa 0:7a00359e701e 217
masahikofukasawa 0:7a00359e701e 218 bool AkmAkd::isEvent(){
masahikofukasawa 0:7a00359e701e 219 return event;
masahikofukasawa 0:7a00359e701e 220 }
masahikofukasawa 0:7a00359e701e 221
masahikofukasawa 0:7a00359e701e 222 AkmSensor::Status AkmAkd::startSensor(){
masahikofukasawa 11:cef8dc1cf010 223 // read one data to clear DRDY
masahikofukasawa 9:6fa3e7b17c27 224 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 225 compass->getMagneticVector(&mag);
masahikofukasawa 9:6fa3e7b17c27 226
masahikofukasawa 11:cef8dc1cf010 227 // enable interrupt
masahikofukasawa 9:6fa3e7b17c27 228 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 229 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 10:5c69b067d88a 230 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 10:5c69b067d88a 231 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 10:5c69b067d88a 232 subId == AkmAkd::SUB_ID_AK09915C ) ){
masahikofukasawa 10:5c69b067d88a 233 // Push-Pull DRDY
masahikofukasawa 16:d85be9bafb80 234 drdy->rise(callback(this, &AkmAkd::detectDRDY));
masahikofukasawa 0:7a00359e701e 235 }
masahikofukasawa 10:5c69b067d88a 236 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 237 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 10:5c69b067d88a 238 subId == AkmAkd::SUB_ID_AK09916D ) ){
masahikofukasawa 10:5c69b067d88a 239 // Open Drain DRDY
masahikofukasawa 16:d85be9bafb80 240 drdy->fall(callback(this, &AkmAkd::detectDRDY));
masahikofukasawa 10:5c69b067d88a 241 }
masahikofukasawa 0:7a00359e701e 242 else{
masahikofukasawa 10:5c69b067d88a 243 // No DRDY
masahikofukasawa 16:d85be9bafb80 244 ticker.attach(callback(this, &AkmAkd::checkDRDY), 0.005); // 200Hz
masahikofukasawa 0:7a00359e701e 245 }
masahikofukasawa 11:cef8dc1cf010 246
masahikofukasawa 11:cef8dc1cf010 247 // set operation mode
masahikofukasawa 11:cef8dc1cf010 248 if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
masahikofukasawa 11:cef8dc1cf010 249 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 250 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 251 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 252 }
masahikofukasawa 11:cef8dc1cf010 253 }
masahikofukasawa 11:cef8dc1cf010 254 else if( subId == AkmAkd::SUB_ID_AK09912C ){
masahikofukasawa 11:cef8dc1cf010 255 if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 256 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 257 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 258 }
masahikofukasawa 11:cef8dc1cf010 259 }
masahikofukasawa 11:cef8dc1cf010 260 else{
masahikofukasawa 11:cef8dc1cf010 261 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 262 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 263 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 264 }
masahikofukasawa 11:cef8dc1cf010 265 }
masahikofukasawa 11:cef8dc1cf010 266
masahikofukasawa 11:cef8dc1cf010 267 MSG("#Start sensor succceeded.\r\n");
masahikofukasawa 0:7a00359e701e 268 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 269 }
masahikofukasawa 0:7a00359e701e 270
masahikofukasawa 0:7a00359e701e 271 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 272 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 273 }
masahikofukasawa 0:7a00359e701e 274
masahikofukasawa 0:7a00359e701e 275 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 276 ticker.detach();
masahikofukasawa 11:cef8dc1cf010 277 event = false;
masahikofukasawa 11:cef8dc1cf010 278
masahikofukasawa 11:cef8dc1cf010 279 // disable DRDY interrupt
masahikofukasawa 11:cef8dc1cf010 280 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 11:cef8dc1cf010 281 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 11:cef8dc1cf010 282 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 11:cef8dc1cf010 283 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 11:cef8dc1cf010 284 subId == AkmAkd::SUB_ID_AK09915C ) ){
masahikofukasawa 11:cef8dc1cf010 285 drdy->rise(NULL);
masahikofukasawa 11:cef8dc1cf010 286 }
masahikofukasawa 11:cef8dc1cf010 287 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 11:cef8dc1cf010 288 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 11:cef8dc1cf010 289 subId == AkmAkd::SUB_ID_AK09916D ) ){
masahikofukasawa 11:cef8dc1cf010 290 // Open Drain DRDY
masahikofukasawa 11:cef8dc1cf010 291 drdy->fall(NULL);
masahikofukasawa 11:cef8dc1cf010 292 }
masahikofukasawa 11:cef8dc1cf010 293
masahikofukasawa 0:7a00359e701e 294 // Puts the device into power down mode.
masahikofukasawa 0:7a00359e701e 295 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 296 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 297 }
masahikofukasawa 11:cef8dc1cf010 298 // read one data to clear DRDY
masahikofukasawa 11:cef8dc1cf010 299 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 300 compass->getMagneticVector(&mag);
masahikofukasawa 11:cef8dc1cf010 301
masahikofukasawa 0:7a00359e701e 302 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 303 }
masahikofukasawa 0:7a00359e701e 304
masahikofukasawa 0:7a00359e701e 305 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 0:7a00359e701e 306 event = false;
masahikofukasawa 0:7a00359e701e 307
masahikofukasawa 0:7a00359e701e 308 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 309 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 310 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 311 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 312 }
masahikofukasawa 0:7a00359e701e 313 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 0:7a00359e701e 314 msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
masahikofukasawa 0:7a00359e701e 315 msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
masahikofukasawa 0:7a00359e701e 316 msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 317 msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 318 msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 319 msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 320 msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 321 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 322 }
masahikofukasawa 0:7a00359e701e 323
masahikofukasawa 0:7a00359e701e 324 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 325 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 326 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 327 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 328
masahikofukasawa 9:6fa3e7b17c27 329 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 330 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 331 {
masahikofukasawa 18:b7182d5ad8d5 332 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 18:b7182d5ad8d5 333 if(compass->getOperationMode(&mode, &nsf, &sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 334 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 335 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 336 }else{
masahikofukasawa 18:b7182d5ad8d5 337 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 338 out->setArgument(1,nsf);
masahikofukasawa 18:b7182d5ad8d5 339 out->setArgument(2,sdr);
masahikofukasawa 18:b7182d5ad8d5 340 }
masahikofukasawa 18:b7182d5ad8d5 341
masahikofukasawa 18:b7182d5ad8d5 342 }else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 18:b7182d5ad8d5 343 if(compass->getOperationMode(&mode, &nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 344 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 345 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 346 }else{
masahikofukasawa 18:b7182d5ad8d5 347 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 348 out->setArgument(1,nsf);
masahikofukasawa 18:b7182d5ad8d5 349 }
masahikofukasawa 15:1238993fd75f 350 }else{
masahikofukasawa 18:b7182d5ad8d5 351 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 352 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 353 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 354 }else{
masahikofukasawa 18:b7182d5ad8d5 355 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 356 }
masahikofukasawa 9:6fa3e7b17c27 357 }
masahikofukasawa 9:6fa3e7b17c27 358 break;
masahikofukasawa 9:6fa3e7b17c27 359 }
masahikofukasawa 9:6fa3e7b17c27 360 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 361 {
masahikofukasawa 9:6fa3e7b17c27 362 mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 363 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 9:6fa3e7b17c27 364 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 365 sdr = (AkmECompass::Sdr)in->getArgument(2);
masahikofukasawa 9:6fa3e7b17c27 366 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 367 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 368 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 369 }
masahikofukasawa 9:6fa3e7b17c27 370 }
masahikofukasawa 10:5c69b067d88a 371 else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 9:6fa3e7b17c27 372 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 373 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 374 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 375 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 376 }
masahikofukasawa 9:6fa3e7b17c27 377 }
masahikofukasawa 9:6fa3e7b17c27 378 else{
masahikofukasawa 9:6fa3e7b17c27 379 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 380 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 381 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 382 }
masahikofukasawa 9:6fa3e7b17c27 383 }
masahikofukasawa 9:6fa3e7b17c27 384 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 385 break;
masahikofukasawa 15:1238993fd75f 386 }
masahikofukasawa 15:1238993fd75f 387 case Message::CMD_REG_WRITE:
masahikofukasawa 9:6fa3e7b17c27 388 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 389 {
masahikofukasawa 9:6fa3e7b17c27 390 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 391 int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 392 if(in->getArgNum() != len+2){
masahikofukasawa 15:1238993fd75f 393 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 394 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 395 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 396 return status;
masahikofukasawa 15:1238993fd75f 397 }
masahikofukasawa 15:1238993fd75f 398
masahikofukasawa 9:6fa3e7b17c27 399 char data[len];
masahikofukasawa 9:6fa3e7b17c27 400 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 401 data[i] = in->getArgument(i+2);
masahikofukasawa 9:6fa3e7b17c27 402 }
masahikofukasawa 9:6fa3e7b17c27 403 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 404 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 405 MSG("#Error register write.\r\n");
masahikofukasawa 9:6fa3e7b17c27 406 }
masahikofukasawa 9:6fa3e7b17c27 407 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 408 break;
masahikofukasawa 15:1238993fd75f 409 }
masahikofukasawa 9:6fa3e7b17c27 410 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 411 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 412 {
masahikofukasawa 15:1238993fd75f 413 if(in->getArgNum() != 2){
masahikofukasawa 15:1238993fd75f 414 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 415 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 416 return status;
masahikofukasawa 15:1238993fd75f 417 }
masahikofukasawa 15:1238993fd75f 418
masahikofukasawa 9:6fa3e7b17c27 419 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 420 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 421 char data[len];
masahikofukasawa 9:6fa3e7b17c27 422 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 423 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 424 MSG("#Error register read.\r\n");
masahikofukasawa 9:6fa3e7b17c27 425 }
masahikofukasawa 9:6fa3e7b17c27 426 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 427 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 428 }
masahikofukasawa 9:6fa3e7b17c27 429 break;
masahikofukasawa 9:6fa3e7b17c27 430 }
masahikofukasawa 9:6fa3e7b17c27 431 default:
masahikofukasawa 9:6fa3e7b17c27 432 {
masahikofukasawa 11:cef8dc1cf010 433 MSG("#Error no command.\r\n");
masahikofukasawa 9:6fa3e7b17c27 434 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 435 break;
masahikofukasawa 9:6fa3e7b17c27 436 }
masahikofukasawa 9:6fa3e7b17c27 437 }
masahikofukasawa 15:1238993fd75f 438 return status;
masahikofukasawa 0:7a00359e701e 439 }
masahikofukasawa 0:7a00359e701e 440
masahikofukasawa 0:7a00359e701e 441 char* AkmAkd::getSensorName(){
masahikofukasawa 0:7a00359e701e 442 return sensorName;
masahikofukasawa 0:7a00359e701e 443 }