Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: ak09970ctrl.cpp
- Revision:
- 15:1238993fd75f
- Parent:
- 11:cef8dc1cf010
- Child:
- 16:d85be9bafb80
diff -r 21e177fc308a -r 1238993fd75f ak09970ctrl.cpp --- a/ak09970ctrl.cpp Mon Sep 12 17:24:11 2016 +0000 +++ b/ak09970ctrl.cpp Fri Oct 28 21:27:33 2016 +0000 @@ -276,8 +276,8 @@ } if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) { - status = AkmSensor::ERROR; MSG("#Error getSwitchConfig. AK09970.\r\n"); + return AkmSensor::ERROR; } out->setArgument(0,switchConfig.enabledODINTEN); out->setArgument(1,switchConfig.enabledINTEN); @@ -328,11 +328,11 @@ 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) { - status = AkmSensor::ERROR; MSG("#Error getOperationMode. AK09970.\r\n"); + return AkmSensor::ERROR; } out->setArgument(0,(char)sensorMeasurementRange); out->setArgument(1,(char)sensorDriveMode); @@ -359,31 +359,20 @@ break; } case Message::CMD_REG_WRITE: - { - 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; - } - - char address = in->getArgument(0); - int len = (int)in->getArgument(1); - const char data = in->getArgument(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_WRITEN: { char address = in->getArgument(0); 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); + data[i] = in->getArgument(i+2); } if( ak09970->write(address, data, len) != AK09970::SUCCESS) { status = AkmSensor::ERROR; @@ -393,37 +382,19 @@ break; } case Message::CMD_REG_READ: - { - if(in->getArgNum() != 2){ - MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum()); - return AkmSensor::ERROR; - } - - char address = in->getArgument(0); - int len = (int)in->getArgument(1); - char data; - if( ak09970->read(address, &data, len) != AK09970::SUCCESS) { - status = AkmSensor::ERROR; - MSG("#Error register read.\r\n"); - } - out->setArgument(0,data); - break; - } case Message::CMD_REG_READN: { if(in->getArgNum() != 2){ - MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum()); - status = AkmSensor::ERROR; - out->setArgument(0,(char)status); - return status; - } + MSG("#Error argument num. Args=%d\r\n",in->getArgNum()); + return AkmSensor::ERROR; + } char address = in->getArgument(0); int len = (int)in->getArgument(1); char data[len]; if( ak09970->read(address, data, len) != AK09970::SUCCESS) { - status = AkmSensor::ERROR; MSG("#Error register read.\r\n"); + return AkmSensor::ERROR; } for(int i=0; i<len; i++){ out->setArgument(i, data[i]);