Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak09970ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2016-07-08
- Revision:
- 10:5c69b067d88a
- Child:
- 11:cef8dc1cf010
File content as of revision 10:5c69b067d88a:
#include "ak09970ctrl.h" #include "debug.h" #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) /** * Constructor. * */ Ak09970Ctrl::Ak09970Ctrl(){ event = false; ak09970 = NULL; sw = NULL; sensorName = ""; } /** * Destructor. * */ Ak09970Ctrl::~Ak09970Ctrl(){ sw->rise(0); if (ak09970) delete ak09970; if (sw) delete sw; } AkmSensor::Status Ak09970Ctrl::init(const uint8_t id, const uint8_t subid){ primaryId = id; subId = subid; // initialize readConfig readConfig.enabledReadX = false; readConfig.enabledReadY = false; readConfig.enabledReadZ = false; readConfig.enabledUpperOnly = false; if(primaryId == AKM_PRIMARY_ID_AKD_I2C){ sw = new InterruptIn(I2C_DRDY); sw->rise(0); I2C* i2c = new I2C(I2C_SDA,I2C_SCL); i2c->frequency(I2C_SPEED_100KHZ); switch(subid){ case Ak09970Ctrl::SUB_ID_AK09970: ak09970 = new AK09970(); Ak09970Ctrl::sensorName = "AK09970"; break; default: return AkmSensor::ERROR; } bool foundSensor = false; AK09970::SlaveAddress slaveAddr[] = { AK09970::SLAVE_ADDR_1, AK09970::SLAVE_ADDR_2}; for(int i=0; i<sizeof(slaveAddr); i++) { ak09970->init(i2c, slaveAddr[i]); // Checks connectivity if(ak09970->checkConnection() == AK09970::SUCCESS) { // found foundSensor = true; break; } } if(foundSensor != true){ return AkmSensor::ERROR; } // software reset ak09970->reset(); }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){ sw = new InterruptIn(SPI_DRDY); sw->rise(0); SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK); spi->format(8,3); // 8bit, Mode=3 spi->frequency(1000000); DigitalOut* cs = new DigitalOut(SPI_CS); switch(subid){ case Ak09970Ctrl::SUB_ID_AK09970: ak09970 = new AK09970(); Ak09970Ctrl::sensorName = "AK09970"; break; default: return AkmSensor::ERROR; } bool foundSensor = false; ak09970->init(spi, cs); if(ak09970->checkConnection() == AK09970::SUCCESS) { foundSensor = true; } if(foundSensor != true){ MSG("#failed checkConnetion(SPI). %s\n",Ak09970Ctrl::sensorName); return AkmSensor::ERROR; } // software reset ak09970->reset(); } else{ return AkmSensor::ERROR; } MSG("#%s detected.\n", Ak09970Ctrl::sensorName); return AkmSensor::SUCCESS; } void Ak09970Ctrl::checkINT(){ // AK09970::Status status = ak09970->isDataReady(); // if( status == AK09970::DATA_READY || // status == AK09970::DATA_OVER_RUN ) event = true; } void Ak09970Ctrl::detectINT(){ event = true; } bool Ak09970Ctrl::isEvent(){ return event; } AkmSensor::Status Ak09970Ctrl::startSensor(){ if(ak09970->setOperationMode(mode,sensorDriveMode,sensorMeasurementRange) != AK09970::SUCCESS) { return AkmSensor::ERROR; } sw->rise(this, &Ak09970Ctrl::detectINT); return AkmSensor::SUCCESS; } AkmSensor::Status Ak09970Ctrl::startSensor(const float sec){ return AkmSensor::ERROR; // doesn't support } AkmSensor::Status Ak09970Ctrl::stopSensor(){ if(ak09970->setOperationMode(AK09970::MODE_POWER_DOWN, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) { return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak09970Ctrl::readSensorData(Message* msg){ event = false; AK09970::SwitchStatus sw_status; if( ak09970->getSwitchStatus(&sw_status, readConfig) != AkmSensor::SUCCESS) { return AkmSensor::ERROR; } msg->setCommand(Message::CMD_START_MEASUREMENT); msg->setArgument(0, sensorMeasurementRange); msg->setArgument(1, sw_status.st_hi); msg->setArgument(2, sw_status.st_lo); msg->setArgument(3, (char)(sw_status.mag.mx>>8)); msg->setArgument(4, (char)(sw_status.mag.mx & 0x00FF)); msg->setArgument(5, (char)(sw_status.mag.my>>8)); msg->setArgument(6, (char)(sw_status.mag.my & 0x00FF)); msg->setArgument(7, (char)(sw_status.mag.mz>>8)); msg->setArgument(8, (char)(sw_status.mag.mz & 0x00FF)); return AkmSensor::SUCCESS; } AkmSensor::Status Ak09970Ctrl::requestCommand(Message* in, Message* out){ AkmSensor::Status status = AkmSensor::SUCCESS; Message::Command cmd = in->getCommand(); out->setCommand(cmd); switch(cmd){ case Message::CMD_PROGSW_GET_THRESHOLD: { if(in->getArgNum() != 1){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); return AkmSensor::ERROR; } AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0); AK09970::Threshold th; if( ak09970->getThreshold(axis, &th) != AkmSensor::SUCCESS ){ return AkmSensor::ERROR; } out->setArgument(0, axis); out->setArgument(1,(char)((int32_t)(th.BOP1) >> 8)); out->setArgument(2,(char)((int32_t)(th.BOP1) & 0x00FF)); out->setArgument(3,(char)((int32_t)(th.BRP1) >> 8)); out->setArgument(4,(char)((int32_t)(th.BRP1) & 0x00FF)); out->setArgument(5,(char)((int32_t)(th.BOP2) >> 8)); out->setArgument(6,(char)((int32_t)(th.BOP2) & 0x00FF)); out->setArgument(7,(char)((int32_t)(th.BRP2) >> 8)); out->setArgument(8,(char)((int32_t)(th.BRP2) & 0x00FF)); break; } case Message::CMD_PROGSW_SET_THRESHOLD: { if(in->getArgNum() != 9){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0); AK09970::Threshold th; th.BOP1 = CONV16I(in->getArgument(1),in->getArgument(2)); th.BRP1 = CONV16I(in->getArgument(3),in->getArgument(4)); th.BOP2 = CONV16I(in->getArgument(5),in->getArgument(6)); th.BRP2 = CONV16I(in->getArgument(7),in->getArgument(8)); if( ak09970->setThreshold(axis, th) != AkmSensor::SUCCESS ){ status = AkmSensor::ERROR; } out->setArgument(0,(char)status); break; } case Message::CMD_PROGSW_GET_READ_COFIGURATION: { if(in->getArgNum() != 0){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); return AkmSensor::ERROR; } out->setArgument(0, readConfig.enabledReadX); out->setArgument(1, readConfig.enabledReadY); out->setArgument(2, readConfig.enabledReadZ); out->setArgument(3, readConfig.enabledUpperOnly); break; } case Message::CMD_PROGSW_SET_READ_COFIGURATION: { if(in->getArgNum() != 4){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } readConfig.enabledReadX = in->getArgument(0); readConfig.enabledReadY = in->getArgument(1); readConfig.enabledReadZ = in->getArgument(2); readConfig.enabledUpperOnly = in->getArgument(3); out->setArgument(0,(char)status); break; } case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION: { if(in->getArgNum() != 0){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); return AkmSensor::ERROR; } if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error getSwitchConfig. AK09970.\n"); } out->setArgument(0,switchConfig.enabledODINTEN); out->setArgument(1,switchConfig.enabledINTEN); out->setArgument(2,switchConfig.enabledERRADCEN); out->setArgument(3,switchConfig.enabledERRXYEN); out->setArgument(4,switchConfig.enabledSWZ2EN); out->setArgument(5,switchConfig.enabledSWZ1EN); out->setArgument(6,switchConfig.enabledSWY2EN); out->setArgument(7,switchConfig.enabledSWY1EN); out->setArgument(8,switchConfig.enabledSWX2EN); out->setArgument(9,switchConfig.enabledSWX1EN); out->setArgument(10,switchConfig.enabledDRDYEN); break; } case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION: { if(in->getArgNum() != 11){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); for(int i=0; i<in->getArgNum(); i++){ MSG("#%d = %02X\n",i,in->getArgument(i)); } status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } switchConfig.enabledODINTEN = in->getArgument(0); switchConfig.enabledINTEN = in->getArgument(1); switchConfig.enabledERRADCEN = in->getArgument(2); switchConfig.enabledERRXYEN = in->getArgument(3); switchConfig.enabledSWZ2EN = in->getArgument(4); switchConfig.enabledSWZ1EN = in->getArgument(5); switchConfig.enabledSWY2EN = in->getArgument(6); switchConfig.enabledSWY1EN = in->getArgument(7); switchConfig.enabledSWX2EN = in->getArgument(8); switchConfig.enabledSWX1EN = in->getArgument(9); switchConfig.enabledDRDYEN = in->getArgument(10); if(ak09970->setSwitchConfig(switchConfig) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error setSwitchConfig. AK09970.\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_PROGSW_GET_OPERATION_MODE: { if(in->getArgNum() != 0){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); return AkmSensor::ERROR; } if(ak09970->getOperationMode(&mode, &sensorDriveMode, &sensorMeasurementRange) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error getOperationMode. AK09970.\n"); } out->setArgument(0,(char)sensorMeasurementRange); out->setArgument(1,(char)sensorDriveMode); out->setArgument(2,(char)mode); break; } case Message::CMD_PROGSW_SET_OPERATION_MODE: { if(in->getArgNum() != 3){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } sensorMeasurementRange = (AK09970::SensorMeasurementRange)(in->getArgument(0)); sensorDriveMode = (AK09970::SensorDriveMode)(in->getArgument(1)); mode = (AK09970::OperationMode)(in->getArgument(2)); if(ak09970->setOperationMode(mode, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error setOperationMode. AK09970. %d,%d,%d\n",mode,sensorDriveMode,sensorMeasurementRange); } out->setArgument(0,(char)status); break; } case Message::CMD_REG_WRITE: { if(in->getArgNum() != 3){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } char address = in->getArgument(0); int len = (int)in->getArgument(1); const char data = in->getArgument(2); if( ak09970->write(address, &data, len) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error register write.\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( ak09970->write(address, data, len) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error register write.\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_REG_READ: { if(in->getArgNum() != 2){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); return AkmSensor::ERROR; } char address = in->getArgument(0); int len = (int)in->getArgument(1); char data; if( ak09970->read(address, &data, len) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error register read.\n"); } out->setArgument(0,data); break; } case Message::CMD_REG_READN: { if(in->getArgNum() != 2){ MSG("#Error argument num. AK09970. Args=%d\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } char address = in->getArgument(0); int len = (int)in->getArgument(1); char data[len]; if( ak09970->read(address, data, len) != AK09970::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 status; } char* Ak09970Ctrl::getSensorName(){ return sensorName; }