Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak7401ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-12-14
- Revision:
- 47:221ec4b404ec
- Parent:
- 46:5938ad2039b0
File content as of revision 47:221ec4b404ec:
#include "ak7401ctrl.h" /** * Constructor. * */ Ak7401Ctrl::Ak7401Ctrl() : AkmSensor(){ ak7401 = NULL; } /** * Destructor. * */ Ak7401Ctrl::~Ak7401Ctrl(){ if (ak7401) delete ak7401; } AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){ primaryId = id; subId = subid; if(subId == SUB_ID_AK7401){ SPI* mSpi; DigitalOut* mCs; AK7401::Status status = AK7401::ERROR; mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK); mSpi->format(8,1); // 8bit, Mode=1 mSpi->frequency(1000000); mCs = new DigitalOut(SPI_CS); ak7401 = new AK7401(); ak7401->begin(mSpi, mCs); sensorName = "AK7401"; status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); if( status != AK7401::SUCCESS ){ MSG("#AK7401 set USER mode failed.\r\n"); return AkmSensor::ERROR; } // 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 set rotation failed.\r\n"); return AkmSensor::ERROR; } /* status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); if( status != AK7401::SUCCESS ){ MSG("#AK7401 set NORMAL mode failed.\r\n"); return AkmSensor::ERROR; } */ MSG("#AK7401 init succeed.\r\n"); interval = SENSOR_SAMPLING_RATE; // 10Hz } else{ return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak7401Ctrl::startSensor(){ AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); if( status != AK7401::SUCCESS ){ MSG("#AK7401 set NORMAL mode failed.\r\n"); return AkmSensor::ERROR; } ticker.attach(callback(this, &base::setEvent), interval); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){ AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE); if( status != AK7401::SUCCESS ){ MSG("#AK7401 set NORMAL mode failed.\r\n"); return AkmSensor::ERROR; } interval = sec; ticker.attach(callback(this, &base::setEvent), interval); MSG("#Start sensor %s.\r\n",sensorName); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7401Ctrl::stopSensor(){ AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE); if( status != AK7401::SUCCESS ){ MSG("#AK7401 set USER mode failed.\r\n"); return AkmSensor::ERROR; } ticker.detach(); AkmSensor::clearEvent(); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){ AkmSensor::clearEvent(); char angle[2] = {0x00,0x00}; AK7401::Status status = ak7401->readAngle(angle); msg->setCommand(Message::CMD_START_MEASUREMENT); msg->setArgument( 0, status ); msg->setArgument( 1, angle[0] ); msg->setArgument( 2, angle[1] ); if( status != AK7401::SUCCESS){ return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){ AkmSensor::Status status = AkmSensor::SUCCESS; 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( ak7401->readAngleMeasureCommand(angle, &density, &abnormal) != AK7401::SUCCESS ){ MSG("#Error: Read angle\r\n"); status = AkmSensor::ERROR; } out->setCommand(Message::CMD_ANGLE_READ); out->setArgument( 0, ((abnormal&0x01)==0x00)|(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: { if( ak7401->setAngleZero() != AK7401::SUCCESS ){ MSG("#Error: Set angle zero\r\n"); status = AkmSensor::ERROR; } if( status == AkmSensor::ERROR ){ out->setArgument(0,1); }else{ out->setArgument(0,0); } break; } case Message::CMD_REG_WRITE: case Message::CMD_REG_WRITEN: { char address = in->getArgument(0); const 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( ak7401->writeRegister(address, data) != AK7401::SUCCESS) { status = AkmSensor::ERROR; MSG("#Error: Register write.\r\n"); } if( ak7401->writeEEPROM(address, data) != AK7401::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); const 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( ak7401->readRegister(address, data) != AK7401::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"); status = AkmSensor::ERROR; break; } } return status; }