Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: ak7401ctrl.cpp
- Revision:
- 14:21e177fc308a
- Parent:
- 13:d008249f0359
- Child:
- 16:d85be9bafb80
diff -r d008249f0359 -r 21e177fc308a ak7401ctrl.cpp --- a/ak7401ctrl.cpp Fri Aug 12 00:04:48 2016 +0000 +++ b/ak7401ctrl.cpp Mon Sep 12 17:24:11 2016 +0000 @@ -37,25 +37,32 @@ status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); if( status != AK7401::SUCCESS ){ - MSG("#AK7401 user mode failed.\r\n"); + MSG("#AK7401 set USER mode failed.\r\n"); return AkmSensor::ERROR; } - MSG("#AK7401 user mode succeed.\r\n"); - + + // check connection + status = ak7401->checkConnection(); + if( status != AK7401::SUCCESS ){ + MSG("#AK7401 check connection failed.\r\n"); + return AkmSensor::ERROR; + } + + // change rotation direction to CW from CCW char data[2] = {0x00,0x07}; // set clockwise rotation status = ak7401->writeEEPROM(0x05,data); // set clockwise if( status != AK7401::SUCCESS ){ - MSG("#AK7401 write EEPROM failed.\r\n"); + MSG("#AK7401 set rotation failed.\r\n"); return AkmSensor::ERROR; } - MSG("#AK7401 write EEPROM succeed.\r\n"); status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); if( status != AK7401::SUCCESS ){ - MSG("#AK7401 normal mode failed.\r\n"); + MSG("#AK7401 set NORMAL mode failed.\r\n"); return AkmSensor::ERROR; } - MSG("#AK7401 normal mode succeed.\r\n"); + + MSG("#AK7401 init succeed.\r\n"); interval = SENSOR_SAMPLING_RATE; // 10Hz } @@ -109,41 +116,12 @@ AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){ AkmSensor::Status status = AkmSensor::SUCCESS; - Message::Command cmd = in->getCommand(); - switch(cmd){ case Message::CMD_ANGLE_ZERO_RESET: { - AK7401::Status st; - if( Ak7401Ctrl::stopSensor() != AkmSensor::SUCCESS ){ - MSG("#Error stop sensor\r\n"); - status = AkmSensor::ERROR; - } - - st = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); - if( st != AK7401::SUCCESS ){ - MSG("#Error when set user mode\r\n"); - status = AkmSensor::ERROR; - } - char temp[2] = {0x00,0x00}; - st = ak7401->writeRegister(0x06,temp); - if( st != AK7401::SUCCESS ){ - MSG("#Error temp read: code=%d\r\n",st); - status = AkmSensor::ERROR; - } - st = ak7401->setAngleZero(); // reset ZP data - if( st != AK7401::SUCCESS ){ - MSG("#Error setAngleZero: code=%d\r\n",st); - status = AkmSensor::ERROR; - } - st = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); - if( st != AK7401::SUCCESS ){ - MSG("#Error when set normal mode\r\n"); - status = AkmSensor::ERROR; - } - if( Ak7401Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){ - MSG("#Error start sensor\r\n"); + if( ak7401->setAngleZero() != AK7401::SUCCESS ){ + MSG("#Error set angle zero\r\n"); status = AkmSensor::ERROR; } if( status == AkmSensor::ERROR ){