Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak7451ctrl.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-03-17
- Revision:
- 29:b488d2c89fba
- Parent:
- 27:41aa9fb23a2f
- Child:
- 30:5a241d9b3262
File content as of revision 29:b488d2c89fba:
#include "ak7451ctrl.h" #include "debug.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; DigitalOut* mCs; AK7451::Status status = AK7451::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); ak7451 = new AK7451(); ak7451->begin(mSpi, mCs); sensorName = "AK7451"; status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); if( status != AK7451::SUCCESS ){ MSG("#AK7451 user mode failed.\r\n"); return AkmSensor::ERROR; } MSG("#AK7451 user mode succeed.\r\n"); char data[2] = {0x02,0x00}; // set clockwise rotation status = ak7451->writeEEPROM(0x07,data); // set clockwise if( status != AK7451::SUCCESS ){ MSG("#AK7451 write EEPROM failed.\r\n"); return AkmSensor::ERROR; } MSG("#AK7451 write EEPROM succeed.\r\n"); 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(){ ticker.attach(callback(this, &AkmSensor::setEvent), interval); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){ interval = sec; ticker.attach(callback(this, &AkmSensor::setEvent), interval); MSG("#Start sensor %s.\r\n",sensorName); return AkmSensor::SUCCESS; } AkmSensor::Status Ak7451Ctrl::stopSensor(){ 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_ZERO_RESET: { AK7451::Status st; 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(AK7451::AK7451_NORMAL_MODE); if( st != AK7451::SUCCESS ){ MSG("#Error when set normal mode\r\n"); status = AkmSensor::ERROR; } if( status == AkmSensor::ERROR ){ out->setArgument(0,1); }else{ out->setArgument(0,0); } break; } default: { MSG("#Error no command.\r\n"); status = AkmSensor::ERROR; break; } } return status; }