Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak9752ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2016-07-08
- Revision:
- 10:5c69b067d88a
- Child:
- 11:cef8dc1cf010
File content as of revision 10:5c69b067d88a:
#include "ak9752ctrl.h" #include "debug.h" #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) /** * Constructor. * */ Ak9752Ctrl::Ak9752Ctrl(){ ak9752 = NULL; event = false; interrupt = NULL; } /** * Destructor. * */ Ak9752Ctrl::~Ak9752Ctrl(){ if (ak9752) delete ak9752; if (interrupt) delete interrupt; } AkmSensor::Status Ak9752Ctrl::init(const uint8_t id, const uint8_t subid){ primaryId = id; subId = subid; interrupt = new InterruptIn(I2C_DRDY); I2C* i2c = new I2C(I2C_SDA,I2C_SCL); i2c->frequency(I2C_SPEED_100KHZ); if(subId == SUB_ID_AK9752){ ak9752 = new AK9752(); } else{ return AkmSensor::ERROR; } bool foundSensor = false; AK9752::SlaveAddress slaveAddr[] = { AK9752::SLAVE_ADDR_1 }; for(int i=0; i<sizeof(slaveAddr); i++) { ak9752->init(i2c, slaveAddr[i]); // Checks connectivity if(ak9752->checkConnection() == Ak9752Ctrl::SUCCESS) { // found foundSensor = true; break; } } if(foundSensor != true) return AkmSensor::ERROR; // reset if (ak9752->reset() != AK9752::SUCCESS) { MSG("#Failed to reset AK9752.\n"); } MSG("#Init success AK9752.\n"); return AkmSensor::SUCCESS; } void Ak9752Ctrl::detectINT(){ event = true; } bool Ak9752Ctrl::isEvent(){ return event; } AkmSensor::Status Ak9752Ctrl::startSensor(){ if(ak9752->setOperationMode(mode,fc_tmp, fc_ir) != AK9752::SUCCESS) { MSG("#Error setOperationMode. AK9752.\n"); return AkmSensor::ERROR; } interrupt->fall(this, &Ak9752Ctrl::detectINT); MSG("#Start Sensor success AK9752.\n"); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9752Ctrl::startSensor(const float sec){ interrupt->fall(this, &Ak9752Ctrl::detectINT); return AkmSensor::ERROR; } AkmSensor::Status Ak9752Ctrl::stopSensor(){ event = false; interrupt->fall(0); if(ak9752->setOperationMode(AK9752::MODE_STANDBY, fc_tmp, fc_ir) != AK9752::SUCCESS) { MSG("#Error setOperationMode. AK9752.\n"); return AkmSensor::ERROR; } // read one data to clear INT pin Message msg; Ak9752Ctrl::readSensorData(&msg); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9752Ctrl::readSensorData(Message* msg){ event = false; AK9752::SensorData data; AK9752::Status status = ak9752->getSensorData(&data); if( status != AK9752::SUCCESS){ MSG("#Error getSensorData. AK9752.\n"); return AkmSensor::ERROR; } msg->setCommand(Message::CMD_START_MEASUREMENT); msg->setArgument( 0, data.intStatus.irh); msg->setArgument( 1, data.intStatus.irl); msg->setArgument( 2, data.intStatus.tmph); msg->setArgument( 3, data.intStatus.tmpl); msg->setArgument( 4, data.intStatus.dr); msg->setArgument( 5,(char)((int32_t)(data.ir) >> 8)); msg->setArgument( 6, (char)((int32_t)(data.ir) & 0x00FF) ); msg->setArgument( 7,(char)((int32_t)(data.temperature) >> 8)); msg->setArgument( 8, (char)((int32_t)(data.temperature) & 0x00FF) ); msg->setArgument( 9, data.dor); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9752Ctrl::requestCommand(Message* in, Message* out){ AkmSensor::Status status = AkmSensor::SUCCESS; Message::Command cmd = in->getCommand(); AK9752::Threshold th; AK9752::InterruptStatus interrupt; out->setCommand(cmd); switch(cmd){ case Message::CMD_IR_GET_THRESHOLD: { if (ak9752->getThreshold(&th) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Failed to set threshold to AK9752.\n"); } out->setArgument(0,(char)((int32_t)(th.thirh) >> 8)); out->setArgument(1,(char)((int32_t)(th.thirh) & 0x00FF)); out->setArgument(2,(char)((int32_t)(th.thirl) >> 8)); out->setArgument(3,(char)((int32_t)(th.thirl) & 0x00FF)); out->setArgument(4,(char)((int32_t)(th.thtmph) >> 8)); out->setArgument(5,(char)((int32_t)(th.thtmph) & 0x00FF)); out->setArgument(6,(char)((int32_t)(th.thtmpl) >> 8)); out->setArgument(7,(char)((int32_t)(th.thtmpl) & 0x00FF)); break; } case Message::CMD_IR_SET_THRESHOLD: { th.thirh = CONV16I(in->getArgument(0), in->getArgument(1)); th.thirl = CONV16I(in->getArgument(2), in->getArgument(3)); th.thtmph = CONV16I(in->getArgument(4), in->getArgument(5)); th.thtmpl = CONV16I(in->getArgument(6), in->getArgument(7)); if (ak9752->setThreshold(&th) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Failed to set threshold to AK9752.\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_INTERRUPT: { if (ak9752->getInterruptEnable(&interrupt) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Failed to set hysteresis to AK9752.\n"); } out->setArgument(0, interrupt.irh); out->setArgument(1, interrupt.irl); out->setArgument(2, interrupt.tmph); out->setArgument(3, interrupt.tmpl); out->setArgument(4, interrupt.dr); break; } case Message::CMD_IR_SET_INTERRUPT: { interrupt.irh = in->getArgument(0); interrupt.irl = in->getArgument(1); interrupt.tmph = in->getArgument(2); interrupt.tmpl = in->getArgument(3); interrupt.dr = in->getArgument(4); if (ak9752->setInterruptEnable(&interrupt) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Failed to set hysteresis to AK9752.\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_OPERATION_MODE: { if(ak9752->getOperationMode(&mode, &fc_tmp, &fc_ir) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error setOperationMode. AK9752.\n"); } out->setArgument(0,(char)mode); out->setArgument(1,(char)fc_tmp); out->setArgument(2,(char)fc_ir); break; } case Message::CMD_IR_SET_OPERATION_MODE: { mode = (AK9752::OperationMode)in->getArgument(0); fc_tmp = (AK9752::FcTmp)in->getArgument(1); fc_ir = (AK9752::FcIr)in->getArgument(2); if(ak9752->setOperationMode(mode, fc_tmp, fc_ir) != AK9752::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error setOperationMode. AK9752.\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_REG_WRITE: { char address = in->getArgument(0); int len = (int)in->getArgument(1); const char data = in->getArgument(2); if( ak9752->write(address, &data, len) != AK9752::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( ak9752->write(address, data, len) != AK9752::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( ak9752->read(address, &data, len) != AK9752::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( ak9752->read(address, data, len) != AK9752::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; }