Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak9750ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-12-13
- Revision:
- 46:5938ad2039b0
- Parent:
- 43:45225713cd58
- Child:
- 47:221ec4b404ec
File content as of revision 46:5938ad2039b0:
#include "ak9750ctrl.h" #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) /** * Constructor. * */ Ak9750Ctrl::Ak9750Ctrl() : AkmSensor(){ ak9750 = NULL; } /** * Destructor. * */ Ak9750Ctrl::~Ak9750Ctrl(){ if (ak9750) delete ak9750; } AkmSensor::Status Ak9750Ctrl::init(const uint8_t id, const uint8_t subid){ primaryId = id; subId = subid; I2C* i2c = new I2C(I2C_SDA,I2C_SCL); i2c->frequency(I2C_SPEED); if(subId == SUB_ID_AK9750){ ak9750 = new AK9750(); sensorName = "AK9750"; } else if(subId == SUB_ID_AK9753){ ak9750 = new AK9750(); sensorName = "AK9753"; } else{ return AkmSensor::ERROR; } bool foundSensor = false; AK9750::SlaveAddress slaveAddr[] = { AK9750::SLAVE_ADDR_1, AK9750::SLAVE_ADDR_2, AK9750::SLAVE_ADDR_3}; for(int i=0; i<sizeof(slaveAddr); i++) { ak9750->init(i2c, slaveAddr[i]); // Checks connectivity if(ak9750->checkConnection() == Ak9750Ctrl::SUCCESS) { // found foundSensor = true; break; } } if(foundSensor != true){ MSG("#Error: Failed to checkConnection AK9750.\r\n"); return AkmSensor::ERROR; } // reset if (ak9750->reset() != AK9750::SUCCESS) { MSG("#Error: Failed to reset AK9750.\r\n"); } /* // Set to EEPROM mode to EEPROM access if(ak9750->setOperationMode(AK9750::MODE_EEPROM_ACCESS, AK9750::DF_0P3HZ) != AK9750::SUCCESS) { MSG("#Error: setOperationMode to EEPROM mode. AK9750.\r\n"); return AkmSensor::ERROR; } */ // Gets threshold from EEPROM AK9750::Threshold th; if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { MSG("#Error: Failed to get threshold from EEPROM AK9750.\r\n"); } MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\r\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l); // Gets hysteresis from EEPROM AK9750::Hysteresis hys; if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { MSG("#Error: Failed to get hysteresis from EEPROM AK9750.\r\n"); } MSG("#Hysteresis:(0x%02X,0x%02X)\r\n",hys.ehys13,hys.ehys24); // Gets interrupt status from EEPROM AK9750::InterruptStatus intStatus; if ((ak9750->getInterruptEnableFromEEPROM(&intStatus)) != AK9750::SUCCESS) { MSG("#Error: Failed to get interrupts of AK9750 from EEPROM.\r\n"); } MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\r\n",intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l,intStatus.drdy); // Gets operation mode from EEPROM if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) { MSG("#Error: Failed to get operation mode of AK9750 from EEPROM.\r\n"); } MSG("#Operation Mode:(0x%02X,0x%02X)\r\n",mode, filter); /* // Back to Stand-by Mode for register access if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) { MSG("#Error: setOperationMode to stand-by mode of AK9750.\r\n"); return AkmSensor::ERROR; } */ // Sets threshold from the red EEPROM values if (ak9750->setThreshold(&th) != AK9750::SUCCESS) { MSG("#Error: Failed to set threshold to AK9750.\r\n"); } // Sets hysteresis from the red EEPROM values if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) { MSG("#Error: Failed to set hysteresis to AK9750.\r\n"); } // Sets interruput status from the red EEPROM values AK9750::Status status = ak9750->setInterruptEnable(&intStatus); if (status != AK9750::SUCCESS) { MSG("#Error: Failed to set interrupts of AK9750. Status = 0x%02X\r\n", status); } if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) { MSG("#Error: setOperationMode of AK9750.\r\n"); return AkmSensor::ERROR; } MSG("#Init success AK9750.\r\n"); return AkmSensor::SUCCESS; } void Ak9750Ctrl::setEvent(){ AK9750::Status status = ak9750->isDataReady(); if( status == AK9750::DATA_READY ) base::setEvent(); } AkmSensor::Status Ak9750Ctrl::startSensor(){ // read one data to clear INT pin AK9750::SensorData data; ak9750->getSensorData(&data); // set operation mode if(ak9750->setOperationMode(mode,filter) != AK9750::SUCCESS) { MSG("#Error: Start sensor failed %s\r\n", sensorName); return AkmSensor::ERROR; } MSG("#Start sensor %s.\r\n",sensorName); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9750Ctrl::startSensor(const float sec){ return AkmSensor::ERROR; } AkmSensor::Status Ak9750Ctrl::stopSensor(){ AkmSensor::clearEvent(); if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) { MSG("#Error: setOperationMode. AK9750.\r\n"); return AkmSensor::ERROR; } // read one data to clear INT pin AK9750::SensorData data; ak9750->getSensorData(&data); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9750Ctrl::readSensorData(Message* msg){ AkmSensor::clearEvent(); AK9750::SensorData data; AK9750::Status status = ak9750->getSensorData(&data); if( status != AK9750::SUCCESS){ MSG("#Error: getSensorData. AK9750.\r\n"); return AkmSensor::ERROR; } msg->setCommand(Message::CMD_START_MEASUREMENT); msg->setArgument( 0, data.intStatus.ir13h); msg->setArgument( 1, data.intStatus.ir13l); msg->setArgument( 2, data.intStatus.ir24h); msg->setArgument( 3, data.intStatus.ir24l); msg->setArgument( 4, data.intStatus.drdy); msg->setArgument( 5,(char)((int32_t)(data.ir1) >> 8)); msg->setArgument( 6, (char)((int32_t)(data.ir1) & 0x00FF) ); msg->setArgument( 7,(char)((int32_t)(data.ir2) >> 8)); msg->setArgument( 8, (char)((int32_t)(data.ir2) & 0x00FF) ); msg->setArgument( 9,(char)((int32_t)(data.ir3) >> 8)); msg->setArgument( 10, (char)((int32_t)(data.ir3) & 0x00FF) ); msg->setArgument( 11,(char)((int32_t)(data.ir4) >> 8)); msg->setArgument( 12, (char)((int32_t)(data.ir4) & 0x00FF) ); msg->setArgument( 13,(char)((int32_t)(data.temperature) >> 8)); msg->setArgument( 14, (char)((int32_t)(data.temperature) & 0x00FF) ); msg->setArgument( 15, data.dor); return AkmSensor::SUCCESS; } AkmSensor::Status Ak9750Ctrl::requestCommand(Message* in, Message* out){ AkmSensor::Status status = AkmSensor::SUCCESS; Message::Command cmd = in->getCommand(); AK9750::Threshold th; AK9750::Hysteresis hys; AK9750::InterruptStatus interrupt; out->setCommand(cmd); switch(cmd){ case Message::CMD_IR_GET_THRESHOLD: { if (ak9750->getThreshold(&th) != AK9750::SUCCESS) { MSG("#Error: Failed to get threshold of AK9750.\r\n"); return AkmSensor::ERROR; } out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8)); out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF)); out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8)); out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF)); out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8)); out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF)); out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8)); out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF)); break; } case Message::CMD_IR_SET_THRESHOLD: { th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1)); th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3)); th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5)); th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7)); if (ak9750->setThreshold(&th) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set threshold to AK9750.\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_HYSTERESIS: { if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) { MSG("#Error: Failed to get hysteresis of AK9750.\r\n"); return AkmSensor::ERROR; } out->setArgument(0,hys.ehys13); out->setArgument(1,hys.ehys24); break; } case Message::CMD_IR_SET_HYSTERESIS: { hys.ehys13 = in->getArgument(0); hys.ehys24 = in->getArgument(1); if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set hysteresis to AK9750.\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_INTERRUPT: { if (ak9750->getInterruptEnable(&interrupt) != AK9750::SUCCESS) { MSG("#Error: Failed to get interrupt of AK9750.\r\n"); return AkmSensor::ERROR; } out->setArgument(0, interrupt.ir13h); out->setArgument(1, interrupt.ir13l); out->setArgument(2, interrupt.ir24h); out->setArgument(3, interrupt.ir24l); out->setArgument(4, interrupt.drdy); break; } case Message::CMD_IR_SET_INTERRUPT: { interrupt.ir13h = in->getArgument(0); interrupt.ir13l = in->getArgument(1); interrupt.ir24h = in->getArgument(2); interrupt.ir24l = in->getArgument(3); interrupt.drdy = in->getArgument(4); if (ak9750->setInterruptEnable(&interrupt) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set interrupt to AK9750.\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_OPERATION_MODE: { if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) { MSG("#Error: getOperationMode. AK9750.\r\n"); return AkmSensor::ERROR; } out->setArgument(0,(char)mode); out->setArgument(1,(char)filter); break; } case Message::CMD_IR_SET_OPERATION_MODE: { mode = (AK9750::OperationMode)in->getArgument(0); filter = (AK9750::DigitalFilter)in->getArgument(1); if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: setOperationMode. AK9750.\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_THRESHOLD_EEPROM: { if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { MSG("#Error: Failed to get threshold of AK9750(EEPROM).\r\n"); return AkmSensor::ERROR; } out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8)); out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF)); out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8)); out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF)); out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8)); out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF)); out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8)); out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF)); break; } case Message::CMD_IR_SET_THRESHOLD_EEPROM: { th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1)); th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3)); th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5)); th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7)); if (ak9750->setThresholdToEEPROM(&th) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set threshold to AK9750(EEPROM).\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_HYSTERESIS_EEPROM: { if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { MSG("#Error: Failed to get hysteresis of AK9750(EEPROM).\r\n"); return AkmSensor::ERROR; } out->setArgument(0,hys.ehys13); out->setArgument(1,hys.ehys24); break; } case Message::CMD_IR_SET_HYSTERESIS_EEPROM: { hys.ehys13 = in->getArgument(0); hys.ehys24 = in->getArgument(1); if (ak9750->setHysteresisToEEPROM(&hys) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set hysteresis to AK9750(EEPROM).\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_INTERRUPT_EEPROM: { if (ak9750->getInterruptEnableFromEEPROM(&interrupt) != AK9750::SUCCESS) { MSG("#Error: Failed to get interrupt of AK9750(EEPROM).\r\n"); return AkmSensor::ERROR; } out->setArgument(0, interrupt.ir13h); out->setArgument(1, interrupt.ir13l); out->setArgument(2, interrupt.ir24h); out->setArgument(3, interrupt.ir24l); out->setArgument(4, interrupt.drdy); break; } case Message::CMD_IR_SET_INTERRUPT_EEPROM: { interrupt.ir13h = in->getArgument(0); interrupt.ir13l = in->getArgument(1); interrupt.ir24h = in->getArgument(2); interrupt.ir24l = in->getArgument(2); interrupt.drdy = in->getArgument(4); if (ak9750->setInterruptEnableToEEPROM(&interrupt) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Failed to set interrupt to AK9750(EEPROM).\r\n"); } out->setArgument(0,(char)status); break; } case Message::CMD_IR_GET_OPERATION_MODE_EEPROM: { if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) { MSG("#Error: getOperationMode. AK9750(EEPROM).\r\n"); return AkmSensor::ERROR; } out->setArgument(0,(char)mode); out->setArgument(1,(char)filter); break; } case Message::CMD_IR_SET_OPERATION_MODE_EEPROM: { mode = (AK9750::OperationMode)in->getArgument(0); filter = (AK9750::DigitalFilter)in->getArgument(1); if(ak9750->setOperationModeToEEPROM(mode, filter) != AK9750::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: setOperationMode. AK9750(EEPROM).\r\n"); } 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( ak9750->write(address, data, len) != AK9750::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( ak9750->read(address, data, len) != AK9750::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; }