Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: ak7451ctrl.cpp
- Revision:
- 11:cef8dc1cf010
- Child:
- 13:d008249f0359
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ak7451ctrl.cpp Fri Jul 22 22:54:11 2016 +0000 @@ -0,0 +1,164 @@ +#include "ak7451ctrl.h" +#include "debug.h" + +/** + * Constructor. + * + */ +Ak7451Ctrl::Ak7451Ctrl(){ + ak7451 = NULL; + event = false; +} + +/** + * 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); + + 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; +} + +void Ak7451Ctrl::eventCallback(){ + event = true; +} + +bool Ak7451Ctrl::isEvent(){ + return event; +} + +AkmSensor::Status Ak7451Ctrl::startSensor(){ + ticker.attach(this, &Ak7451Ctrl::eventCallback, interval); + return AkmSensor::SUCCESS; +} + +AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){ + interval = sec; + ticker.attach(this, &Ak7451Ctrl::eventCallback, interval); + return AkmSensor::SUCCESS; +} + +AkmSensor::Status Ak7451Ctrl::stopSensor(){ + ticker.detach(); + event = false; + return AkmSensor::SUCCESS; +} + +AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){ + event = false; + 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; + if( Ak7451Ctrl::stopSensor() != AkmSensor::SUCCESS ){ + MSG("#Error stop sensor\r\n"); + status = AkmSensor::ERROR; + } + + st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE); + if( st != AK7451::SUCCESS ){ + MSG("#Error when set user mode\r\n"); + status = AkmSensor::ERROR; + } + char temp[2] = {0x00,0x00}; + st = ak7451->writeRegister(0x06,temp); + if( st != AK7451::SUCCESS ){ + MSG("#Error temp read: code=%d\r\n",st); + 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( Ak7451Ctrl::startSensor(interval) != AkmSensor::SUCCESS ){ + MSG("#Error start sensor\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; +} +