Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: ak9750ctrl.cpp
- Revision:
- 10:5c69b067d88a
- Child:
- 11:cef8dc1cf010
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ak9750ctrl.cpp Fri Jul 08 22:26:26 2016 +0000 @@ -0,0 +1,459 @@ +#include "ak9750ctrl.h" +#include "debug.h" + +#define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) + +/** + * Constructor. + * + */ +Ak9750Ctrl::Ak9750Ctrl(){ + ak9750 = NULL; + event = false; + interrupt = NULL; +} + +/** + * Destructor. + * + */ +Ak9750Ctrl::~Ak9750Ctrl(){ + if (ak9750) delete ak9750; + if (interrupt) delete interrupt; +} + +AkmSensor::Status Ak9750Ctrl::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_AK9750){ + ak9750 = new AK9750(); + } + 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("#Failed to checkConnection AK9750.\n"); + return AkmSensor::ERROR; + } + + // reset + if (ak9750->reset() != AK9750::SUCCESS) { + MSG("#Failed to reset AK9750.\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.\n"); + return AkmSensor::ERROR; + } + // Gets threshold from EEPROM + AK9750::Threshold th; + if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { + MSG("#Failed to get threshold from EEPROM AK9750.\n"); + } + // Gets hysteresis from EEPROM + AK9750::Hysteresis hys; + if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { + MSG("#Failed to get hysteresis from EEPROM AK9750.\n"); + } + // Gets interrupt status from EEPROM + AK9750::InterruptStatus intStatus; + if ((ak9750->getInterruptEnableFromEEPROM(&intStatus)) != AK9750::SUCCESS) { + MSG("#Failed to get interrupts of AK9750 from EEPROM.\n"); + } + + // Gets operation mode from EEPROM + if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) { + MSG("#Failed to get operation mode of AK9750 from EEPROM.\n"); + } + MSG("#Operation Mode:(0x%02X,0x%02X)\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.\n"); + return AkmSensor::ERROR; + } + + // Sets threshold from the red EEPROM values + if (ak9750->setThreshold(&th) != AK9750::SUCCESS) { + MSG("#Failed to set threshold to AK9750.\n"); + } + MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l); + + // Sets hysteresis from the red EEPROM values + if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) { + MSG("#Failed to set hysteresis to AK9750.\n"); + } + MSG("#Hysteresis:(0x%02X,0x%02X)\n",hys.ehys13,hys.ehys24); + + // Sets interruput status from the red EEPROM values + AK9750::Status status = ak9750->setInterruptEnable(&intStatus); + if (status != AK9750::SUCCESS) { + MSG("#Failed to set interrupts of AK9750. Status = 0x%02X\n", status); + } + MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\n",intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l,intStatus.drdy); + + MSG("#Init success AK9750.\n"); + return AkmSensor::SUCCESS; +} + +void Ak9750Ctrl::detectINT(){ + event = true; +} + +bool Ak9750Ctrl::isEvent(){ + return event; +} + +AkmSensor::Status Ak9750Ctrl::startSensor(){ + if(ak9750->setOperationMode(mode,filter) != AK9750::SUCCESS) { + MSG("#Error setOperationMode. AK9750.\n"); + return AkmSensor::ERROR; + } + + interrupt->fall(this, &Ak9750Ctrl::detectINT); + MSG("#Start Sensor success AK9750.\n"); + return AkmSensor::SUCCESS; +} + +AkmSensor::Status Ak9750Ctrl::startSensor(const float sec){ + interrupt->fall(this, &Ak9750Ctrl::detectINT); + return AkmSensor::ERROR; +} + +AkmSensor::Status Ak9750Ctrl::stopSensor(){ + event = false; + interrupt->fall(0); + if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) { + MSG("#Error setOperationMode. AK9750.\n"); + return AkmSensor::ERROR; + } + + // read one data to clear INT pin + Message msg; + Ak9750Ctrl::readSensorData(&msg); + + return AkmSensor::SUCCESS; +} + +AkmSensor::Status Ak9750Ctrl::readSensorData(Message* msg){ + event = false; + + AK9750::SensorData data; + AK9750::Status status = ak9750->getSensorData(&data); + if( status != AK9750::SUCCESS){ + MSG("#Error getSensorData. AK9750.\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) { + status = AkmSensor::ERROR; + MSG("#Failed to set threshold to AK9750.\n"); + } + 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("#Failed to set threshold to AK9750.\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_HYSTERESIS: + { + if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Failed to set hysteresis to AK9750.\n"); + } + 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("#Failed to set hysteresis to AK9750.\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_INTERRUPT: + { + if (ak9750->getInterruptEnable(&interrupt) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Failed to set hysteresis to AK9750.\n"); + } + 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("#Failed to set hysteresis to AK9750.\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_OPERATION_MODE: + { + if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error setOperationMode. AK9750.\n"); + } + 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.\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_THRESHOLD_EEPROM: + { + if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Failed to set threshold to AK9750(EEPROM).\n"); + } + 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("#Failed to set threshold to AK9750(EEPROM).\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_HYSTERESIS_EEPROM: + { + if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Failed to set hysteresis to AK9750(EEPROM).\n"); + } + 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("#Failed to set hysteresis to AK9750(EEPROM).\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_INTERRUPT_EEPROM: + { + if (ak9750->getInterruptEnableFromEEPROM(&interrupt) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Failed to set interrupt to AK9750(EEPROM).\n"); + } + 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("#Failed to set interrupt to AK9750(EEPROM).\n"); + } + out->setArgument(0,(char)status); + break; + } + case Message::CMD_IR_GET_OPERATION_MODE_EEPROM: + { + if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error getOperationMode. AK9750(EEPROM).\n"); + } + 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).\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( ak9750->write(address, &data, len) != AK9750::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( ak9750->write(address, data, len) != AK9750::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( ak9750->read(address, &data, len) != AK9750::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( ak9750->read(address, data, len) != AK9750::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; +}