Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: akmakd.cpp
- Revision:
- 15:1238993fd75f
- Parent:
- 11:cef8dc1cf010
- Child:
- 16:d85be9bafb80
diff -r 21e177fc308a -r 1238993fd75f akmakd.cpp --- a/akmakd.cpp Mon Sep 12 17:24:11 2016 +0000 +++ b/akmakd.cpp Fri Oct 28 21:27:33 2016 +0000 @@ -317,10 +317,11 @@ case Message::CMD_COMPASS_GET_OPERATION_MODE: { if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) { - status = AkmSensor::ERROR; + status = AkmSensor::ERROR; MSG("#Error set operation mode.\r\n"); + }else{ + out->setArgument(0,mode); } - out->setArgument(0,mode); break; } case Message::CMD_COMPASS_SET_OPERATION_MODE: @@ -349,14 +350,22 @@ } out->setArgument(0,(char)status); break; - } + } + case Message::CMD_REG_WRITE: 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( compass->write(address, data, len) != AkmECompass::SUCCESS) { status = AkmSensor::ERROR; @@ -364,21 +373,16 @@ } 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( compass->read(address, &data, len) != AkmECompass::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. Args=%d\r\n",in->getArgNum()); + status = AkmSensor::ERROR; + return status; + } + char address = in->getArgument(0); int len = (int)in->getArgument(1); char data[len]; @@ -398,8 +402,7 @@ break; } } - - return AkmSensor::SUCCESS; + return status; } char* AkmAkd::getSensorName(){