Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak7451ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-12-13
- Revision:
- 46:5938ad2039b0
- Parent:
- 43:45225713cd58
- Child:
- 47:221ec4b404ec
File content as of revision 46:5938ad2039b0:
#include "ak7451ctrl.h" /** * Constructor. * */ Ak7451Ctrl::Ak7451Ctrl() : AkmSensor(){ ak7451 = NULL; } /** * Destructor. * */ Ak7451Ctrl::~Ak7451Ctrl(){ if (ak7451) delete ak7451; } AkmSensor::Status Ak7451Ctrl::init(const uint8_t id, const uint8_t subid){ primaryId = id; subId = subid; if(subId == SUB_ID_AK7451){ SPI* mSpi; // SPI connection DigitalOut* mCs; // Chip select pin // Initialize SPI connection AK7451::Status status = AK7451::ERROR; mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK); mSpi->format(8,1); // 8bit, Mode=1 mSpi->frequency(1000000); // Initialize chip select pin mCs = new DigitalOut(SPI_CS); ak7451 = new AK7451(); ak7451->begin(mSpi, mCs); sensorName = "AK7451"; // Set to User Mode status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); if( status != AK7451::SUCCESS ){ MSG("#Error: AK7451 user mode failed.\r\n"); return AkmSensor::ERROR; } MSG("#AK7451 user mode succeed.\r\n"); // 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("#Error: AK7451 write EEPROM failed.\r\n"); return AkmSensor::ERROR; } MSG("#Error: AK7451 write EEPROM succeed.\r\n"); /* // Set to Normal Mode 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{ return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::startSensor(){ AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE); if( status != AK7451::SUCCESS ){ MSG("#Error: 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("#Error: 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); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::stopSensor(){ AK7451::Status status; status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); if( status != AK7451::SUCCESS ){ MSG("#Error: AK7451 user mode failed.\r\n"); return AkmSensor::ERROR; } ticker.detach(); AkmSensor::clearEvent(); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){ AkmSensor::clearEvent(); char angle[2] = {0x00,0x00}; AK7451::Status status = ak7451->readAngle(angle); msg->setCommand(Message::CMD_START_MEASUREMENT); msg->setArgument( 0, status ); msg->setArgument( 1, angle[0] ); msg->setArgument( 2, angle[1] ); if( status != SUCCESS){ return AkmSensor::ERROR; } return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::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( 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::OperationMode mode = AK7451::AK7451_USER_MODE; // 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(mode); if( st != AK7451::SUCCESS ){ MSG("#Error: when set mode:%d\r\n",mode); 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( 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); 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( 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"); status = AkmSensor::ERROR; break; } } return status; }