Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: ak7451ctrl.cpp
- Revision:
- 30:5a241d9b3262
- Parent:
- 29:b488d2c89fba
- Child:
- 34:1ea3357c8d9a
--- a/ak7451ctrl.cpp Fri Mar 17 23:29:20 2017 +0000 +++ b/ak7451ctrl.cpp Fri Apr 28 20:32:31 2017 +0000 @@ -39,22 +39,27 @@ return AkmSensor::ERROR; } MSG("#AK7451 user mode succeed.\r\n"); - - char data[2] = {0x02,0x00}; // set clockwise rotation + + // E_RD 0x1 clockwise rotation + // E_Z_MODE 0x0 normal output + // E_ABZ_E 0x1 ABZ Output enable + // E_ABZ_HYS 0x2 ABZ Hysteresis=1LSB + // ABZ_RES 0x0 ABZ phase resolution = 1024ppr + char data[2] = {0x02,0xA0}; status = ak7451->writeEEPROM(0x07,data); // set clockwise if( status != AK7451::SUCCESS ){ MSG("#AK7451 write EEPROM failed.\r\n"); return AkmSensor::ERROR; } MSG("#AK7451 write EEPROM succeed.\r\n"); - +/* status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); if( status != AK7451::SUCCESS ){ MSG("#AK7451 normal mode failed.\r\n"); return AkmSensor::ERROR; } MSG("#AK7451 normal mode succeed.\r\n"); - +*/ interval = SENSOR_SAMPLING_RATE; // 10Hz } else{ @@ -65,11 +70,22 @@ } AkmSensor::Status Ak7451Ctrl::startSensor(){ + AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); + if( status != AK7451::SUCCESS ){ + MSG("#AK7451 normal mode failed.\r\n"); + return AkmSensor::ERROR; + } ticker.attach(callback(this, &AkmSensor::setEvent), interval); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){ + AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); + if( status != AK7451::SUCCESS ){ + MSG("#AK7451 normal mode failed.\r\n"); + return AkmSensor::ERROR; + } + interval = sec; ticker.attach(callback(this, &AkmSensor::setEvent), interval); MSG("#Start sensor %s.\r\n",sensorName); @@ -77,6 +93,12 @@ } AkmSensor::Status Ak7451Ctrl::stopSensor(){ + AK7451::Status status; + status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); + if( status != AK7451::SUCCESS ){ + MSG("#AK7451 user mode failed.\r\n"); + return AkmSensor::ERROR; + } ticker.detach(); AkmSensor::clearEvent(); return AkmSensor::SUCCESS; @@ -105,26 +127,54 @@ Message::Command cmd = in->getCommand(); switch(cmd){ + case Message::CMD_ANGLE_READ: + { + // read angle + char angle[2] = {0x00,0x00}; + char density = 0x00; + char abnormal = 0x00; + + if( ak7451->readAngleMeasureCommand(angle, &density, &abnormal) != AK7451::SUCCESS ){ + MSG("#Error read angle\r\n"); + status = AkmSensor::ERROR; + } + out->setCommand(Message::CMD_ANGLE_READ); + out->setArgument( 0, (abnormal != 0x03)|(status == AkmSensor::ERROR) ? 0x01 : 0x00 ); + out->setArgument( 1, angle[0] ); + out->setArgument( 2, angle[1] ); + out->setArgument( 3, density ); + break; + } case Message::CMD_ANGLE_ZERO_RESET: { - AK7451::Status st; + AK7451::Status st; +/* + AK7451::OperationMode mode = AK7451::AK7451_USER_MODE; - st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); - if( st != AK7451::SUCCESS ){ - MSG("#Error when set user mode\r\n"); - status = AkmSensor::ERROR; + // check the mode + char data[1]; + if( ak7451->readRegister(0x00, data) != AK7451::SUCCESS) { + MSG("#NORMAL MODE detected. Set into USER mode.\r\n"); + mode = AK7451::AK7451_NORMAL_MODE; + st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); + if( st != AK7451::SUCCESS ){ + MSG("#Error when set user mode\r\n"); + status = AkmSensor::ERROR; + } } +*/ st = ak7451->setAngleZero(); // reset ZP data if( st != AK7451::SUCCESS ){ MSG("#Error setAngleZero: code=%d\r\n",st); status = AkmSensor::ERROR; } - st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); +/* + st = ak7451->setOperationMode(mode); if( st != AK7451::SUCCESS ){ - MSG("#Error when set normal mode\r\n"); + MSG("#Error when set mode:%d\r\n",mode); status = AkmSensor::ERROR; } - +*/ if( status == AkmSensor::ERROR ){ out->setArgument(0,1); }else{ @@ -132,6 +182,64 @@ } break; } + case Message::CMD_REG_WRITE: + case Message::CMD_REG_WRITEN: + { + char address = in->getArgument(0); + int len = in->getArgument(1); + if(len != 2){ + MSG("#Error length=%d. Only support 2byte length\r\n",len); + status = AkmSensor::ERROR; + return status; + } + 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( ak7451->writeRegister(address, data) != AK7451::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error register write.\r\n"); + } + if( ak7451->writeEEPROM(address, data) != AK7451::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error EEPROM 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()); + status = AkmSensor::ERROR; + return status; + } + + char address = in->getArgument(0); + int len = in->getArgument(1); + if(len != 2){ + MSG("#Error length=%d. Only support 2byte length\r\n",len); + status = AkmSensor::ERROR; + return status; + } + char data[len]; + if( ak7451->readRegister(address, data) != AK7451::SUCCESS) { + status = AkmSensor::ERROR; + MSG("#Error register read.\r\n"); + } + for(int i=0; i<len; i++){ + out->setArgument(i, data[i]); + } + break; + } default: { MSG("#Error no command.\r\n");