Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: akmakd.cpp
- Revision:
- 9:6fa3e7b17c27
- Parent:
- 7:e269411568c9
- Child:
- 10:5c69b067d88a
diff -r 8fa4b81db50b -r 6fa3e7b17c27 akmakd.cpp --- a/akmakd.cpp Sat Jun 11 00:16:17 2016 +0000 +++ b/akmakd.cpp Thu Jun 16 18:36:26 2016 +0000 @@ -31,7 +31,11 @@ AkmECompass::SensorType type; AK099XX* ak099xx; AK8963* ak8963; - + + mode = AkmECompass::MODE_POWER_DOWN; + nsf = AkmECompass::NSF_DISABLE; + sdr = AkmECompass::SDR_LOW_POWER_DRIVE; + if(primaryId == AKM_PRIMARY_ID_AKD_I2C){ drdy = new InterruptIn(I2C_DRDY); @@ -182,20 +186,36 @@ } AkmSensor::Status AkmAkd::startSensor(){ + // read once to clear DRDY pin + AkmECompass::MagneticVector mag; + AkmECompass::Status status = compass->getMagneticVector(&mag); + // Puts the device into continuous measurement mode. - if(compass->setOperationMode((AkmECompass::OperationMode)0x02) != AkmECompass::SUCCESS) { - return AkmSensor::ERROR; + if(compass->getSensorType() == AkmECompass::AK09915){ + if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) { + return AkmSensor::ERROR; + } + } + else if(compass->getSensorType() == AkmECompass::AK09912){ + if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) { + return AkmSensor::ERROR; + } + } + else{ + if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) { + return AkmSensor::ERROR; + } } // DRDY interrupt enable for AK8963/AK09912/AK09915 with I2C interface - if(primaryId == AKM_PRIMARY_ID_AKD_I2C && + if( primaryId == AKM_PRIMARY_ID_AKD_I2C && (compass->getSensorType() == AkmECompass::AK8963 || compass->getSensorType() == AkmECompass::AK09912 || compass->getSensorType() == AkmECompass::AK09915) ){ drdy->rise(this, &AkmAkd::detectDRDY); } else{ - ticker.attach(this, &AkmAkd::checkDRDY, 0.01); // 100Hz + ticker.attach(this, &AkmAkd::checkDRDY, 0.005); // 200Hz } return AkmSensor::SUCCESS; } @@ -234,6 +254,96 @@ } AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){ + AkmSensor::Status status = AkmSensor::SUCCESS; + Message::Command cmd = in->getCommand(); + out->setCommand(cmd); + + switch(cmd){ + case Message::CMD_COMPASS_GET_OPERATION_MODE: + { + if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error set operation mode.\n"); + } + out->setArgument(0,mode); + break; + } + case Message::CMD_COMPASS_SET_OPERATION_MODE: + { + mode = (AkmECompass::OperationMode)in->getArgument(0); + if(compass->getSensorType() == AkmECompass::AK09915){ + nsf = (AkmECompass::Nsf)in->getArgument(1); + sdr = (AkmECompass::Sdr)in->getArgument(2); + if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error set operation mode.\n"); + } + } + else if(compass->getSensorType() == AkmECompass::AK09912){ + nsf = (AkmECompass::Nsf)in->getArgument(1); + if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error set operation mode.\n"); + } + } + else{ + if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error set operation mode.\n"); + } + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_REG_WRITEN: + { + char address = in->getArgument(0); + int len = (int)in->getArgument(1); + char data[len]; + for(int i=0; i<len; i++){ + data[i] = in->getArgument(i); + } + if( compass->write(address, data, len) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error register write.\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_REG_READ: + { + char address = in->getArgument(0); + int len = (int)in->getArgument(1); + char data; + if( compass->read(address, &data, len) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error register read.\n"); + } + out->setArgument(0,data); + break; + } + case Message::CMD_REG_READN: + { + char address = in->getArgument(0); + int len = (int)in->getArgument(1); + char data[len]; + if( compass->read(address, data, len) != AkmECompass::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error register read.\n"); + } + for(int i=0; i<len; i++){ + out->setArgument(i, data[i]); + } + break; + } + default: + { + MSG("#Error no command.\n"); + status = AkmSensor::ERROR; + break; + } + } + return AkmSensor::SUCCESS; }