Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak09970ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-12-14
- Revision:
- 47:221ec4b404ec
- Parent:
- 46:5938ad2039b0
File content as of revision 47:221ec4b404ec:
#include "ak09970ctrl.h" #ifndef PI #define PI 3.14159265358979323846 #endif #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) /** * Constructor. * */ Ak09970Ctrl::Ak09970Ctrl() : AkmSensor(){ ak09970 = NULL; } /** * Destructor. * */ Ak09970Ctrl::~Ak09970Ctrl(){ if (ak09970) delete ak09970; } 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){ I2C* i2c = new I2C(I2C_SDA,I2C_SCL); i2c->frequency(I2C_SPEED); 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(unsigned 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){ 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("#Error: Failed checkConnetion(SPI). %s\r\n",Ak09970Ctrl::sensorName); return AkmSensor::ERROR; } // software reset ak09970->reset(); } else{ return AkmSensor::ERROR; } MSG("#%s detected.\r\n", Ak09970Ctrl::sensorName); return AkmSensor::SUCCESS; } void Ak09970Ctrl::setEvent(){ // MSG("#setEvent() in %s.\r\n",sensorName); AK09970::Status status = ak09970->isDataReady(); if( status == AK09970::DATA_READY ) base::setEvent(); } AkmSensor::Status Ak09970Ctrl::startSensor(){ // read one data to clear switch AK09970::SwitchStatus sw_status; ak09970->getSwitchStatus(&sw_status, readConfig); // set operation mode if(ak09970->setOperationMode(mode,sensorDriveMode,sensorMeasurementRange) != AK09970::SUCCESS) { MSG("#Error: Start sensor %s.\r\n", sensorName); return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak09970Ctrl::startSensor(const float sec){ MSG("#Error: Start sensor %s.\r\n", sensorName); return AkmSensor::ERROR; } AkmSensor::Status Ak09970Ctrl::stopSensor(){ AkmSensor::clearEvent(); if(ak09970->setOperationMode(AK09970::MODE_POWER_DOWN, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) { MSG("#Error: Stop sensor %s.\r\n", sensorName); return AkmSensor::ERROR; } // read one data to clear switch AK09970::SwitchStatus sw_status; ak09970->getSwitchStatus(&sw_status, readConfig); return AkmSensor::SUCCESS; } AkmSensor::Status Ak09970Ctrl::readSensorData(Message* msg){ AkmSensor::clearEvent(); AK09970::SwitchStatus sw_status; if( ak09970->getSwitchStatus(&sw_status, readConfig) != AK09970::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\r\n",in->getArgNum()); return AkmSensor::ERROR; } AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0); AK09970::Threshold th; if( ak09970->getThreshold(axis, &th) != AK09970::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\r\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) != AK09970::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\r\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\r\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\r\n",in->getArgNum()); return AkmSensor::ERROR; } if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) { MSG("#Error: getSwitchConfig. AK09970.\r\n"); return AkmSensor::ERROR; } 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\r\n",in->getArgNum()); for(int i=0; i<in->getArgNum(); i++){ MSG("#%d = %02X\r\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.\r\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\r\n",in->getArgNum()); return AkmSensor::ERROR; } if(ak09970->getOperationMode(&mode, &sensorDriveMode, &sensorMeasurementRange) != AK09970::SUCCESS) { MSG("#Error: getOperationMode. AK09970.\r\n"); return AkmSensor::ERROR; } 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\r\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\r\n",mode,sensorDriveMode,sensorMeasurementRange); } out->setArgument(0,(char)status); break; } case Message::CMD_REG_WRITE: case Message::CMD_REG_WRITEN: { char address = in->getArgument(0); const int len = (int)in->getArgument(1); if(in->getArgNum() != len+2){ MSG("#Error: Argument num. Args=%d\r\n",in->getArgNum()); status = AkmSensor::ERROR; out->setArgument(0,(char)status); return status; } char data[len]; for(int i=0; i<len; i++){ data[i] = in->getArgument(i+2); } if( ak09970->write(address, data, len) != AK09970::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Register write.\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_REG_READ: case Message::CMD_REG_READN: { if(in->getArgNum() != 2){ MSG("#Error: Argument num. Args=%d\r\n",in->getArgNum()); return AkmSensor::ERROR; } char address = in->getArgument(0); const int len = (int)in->getArgument(1); char data[len]; if( ak09970->read(address, data, len) != AK09970::SUCCESS) { MSG("#Error: Register read.\r\n"); return AkmSensor::ERROR; } for(int i=0; i<len; i++){ out->setArgument(i, data[i]); } break; } default: { MSG("#Error: No command.\r\n"); status = AkmSensor::ERROR; break; } } return status; }