Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7451ctrl.cpp@29:b488d2c89fba, 2017-03-17 (annotated)

Committer:
masahikofukasawa
Date:
Fri Mar 17 23:29:20 2017 +0000
Revision:
29:b488d2c89fba
Parent:
27:41aa9fb23a2f
Child:
30:5a241d9b3262
Modified for multi sensor demo.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 11:cef8dc1cf010 1 #include "ak7451ctrl.h"
masahikofukasawa 11:cef8dc1cf010 2 #include "debug.h"
masahikofukasawa 11:cef8dc1cf010 3
masahikofukasawa 11:cef8dc1cf010 4 /**
masahikofukasawa 11:cef8dc1cf010 5 * Constructor.
masahikofukasawa 11:cef8dc1cf010 6 *
masahikofukasawa 11:cef8dc1cf010 7 */
masahikofukasawa 27:41aa9fb23a2f 8 Ak7451Ctrl::Ak7451Ctrl() : AkmSensor(){
masahikofukasawa 11:cef8dc1cf010 9 ak7451 = NULL;
masahikofukasawa 11:cef8dc1cf010 10 }
masahikofukasawa 11:cef8dc1cf010 11
masahikofukasawa 11:cef8dc1cf010 12 /**
masahikofukasawa 11:cef8dc1cf010 13 * Destructor.
masahikofukasawa 11:cef8dc1cf010 14 *
masahikofukasawa 11:cef8dc1cf010 15 */
masahikofukasawa 11:cef8dc1cf010 16 Ak7451Ctrl::~Ak7451Ctrl(){
masahikofukasawa 11:cef8dc1cf010 17 if (ak7451) delete ak7451;
masahikofukasawa 11:cef8dc1cf010 18 }
masahikofukasawa 11:cef8dc1cf010 19
masahikofukasawa 11:cef8dc1cf010 20 AkmSensor::Status Ak7451Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 11:cef8dc1cf010 21 primaryId = id;
masahikofukasawa 11:cef8dc1cf010 22 subId = subid;
masahikofukasawa 11:cef8dc1cf010 23
masahikofukasawa 11:cef8dc1cf010 24 if(subId == SUB_ID_AK7451){
masahikofukasawa 11:cef8dc1cf010 25 SPI* mSpi;
masahikofukasawa 11:cef8dc1cf010 26 DigitalOut* mCs;
masahikofukasawa 11:cef8dc1cf010 27 AK7451::Status status = AK7451::ERROR;
masahikofukasawa 11:cef8dc1cf010 28 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 11:cef8dc1cf010 29 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 11:cef8dc1cf010 30 mSpi->frequency(1000000);
masahikofukasawa 11:cef8dc1cf010 31 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 11:cef8dc1cf010 32 ak7451 = new AK7451();
masahikofukasawa 11:cef8dc1cf010 33 ak7451->begin(mSpi, mCs);
masahikofukasawa 13:d008249f0359 34 sensorName = "AK7451";
masahikofukasawa 11:cef8dc1cf010 35
masahikofukasawa 11:cef8dc1cf010 36 status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 11:cef8dc1cf010 37 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 38 MSG("#AK7451 user mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 39 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 40 }
masahikofukasawa 11:cef8dc1cf010 41 MSG("#AK7451 user mode succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 42
masahikofukasawa 11:cef8dc1cf010 43 char data[2] = {0x02,0x00}; // set clockwise rotation
masahikofukasawa 11:cef8dc1cf010 44 status = ak7451->writeEEPROM(0x07,data); // set clockwise
masahikofukasawa 11:cef8dc1cf010 45 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 46 MSG("#AK7451 write EEPROM failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 47 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 48 }
masahikofukasawa 11:cef8dc1cf010 49 MSG("#AK7451 write EEPROM succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 50
masahikofukasawa 11:cef8dc1cf010 51 status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 11:cef8dc1cf010 52 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 53 MSG("#AK7451 normal mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 54 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 55 }
masahikofukasawa 11:cef8dc1cf010 56 MSG("#AK7451 normal mode succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 57
masahikofukasawa 11:cef8dc1cf010 58 interval = SENSOR_SAMPLING_RATE; // 10Hz
masahikofukasawa 11:cef8dc1cf010 59 }
masahikofukasawa 11:cef8dc1cf010 60 else{
masahikofukasawa 11:cef8dc1cf010 61 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 62 }
masahikofukasawa 11:cef8dc1cf010 63
masahikofukasawa 11:cef8dc1cf010 64 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 65 }
masahikofukasawa 11:cef8dc1cf010 66
masahikofukasawa 11:cef8dc1cf010 67 AkmSensor::Status Ak7451Ctrl::startSensor(){
masahikofukasawa 29:b488d2c89fba 68 ticker.attach(callback(this, &AkmSensor::setEvent), interval);
masahikofukasawa 11:cef8dc1cf010 69 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 70 }
masahikofukasawa 11:cef8dc1cf010 71
masahikofukasawa 11:cef8dc1cf010 72 AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 73 interval = sec;
masahikofukasawa 29:b488d2c89fba 74 ticker.attach(callback(this, &AkmSensor::setEvent), interval);
masahikofukasawa 29:b488d2c89fba 75 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 11:cef8dc1cf010 76 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 77 }
masahikofukasawa 11:cef8dc1cf010 78
masahikofukasawa 11:cef8dc1cf010 79 AkmSensor::Status Ak7451Ctrl::stopSensor(){
masahikofukasawa 11:cef8dc1cf010 80 ticker.detach();
masahikofukasawa 29:b488d2c89fba 81 AkmSensor::clearEvent();
masahikofukasawa 11:cef8dc1cf010 82 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 83 }
masahikofukasawa 11:cef8dc1cf010 84
masahikofukasawa 11:cef8dc1cf010 85 AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 86 AkmSensor::clearEvent();
masahikofukasawa 29:b488d2c89fba 87
masahikofukasawa 11:cef8dc1cf010 88 char angle[2] = {0x00,0x00};
masahikofukasawa 11:cef8dc1cf010 89 AK7451::Status status = ak7451->readAngle(angle);
masahikofukasawa 11:cef8dc1cf010 90
masahikofukasawa 11:cef8dc1cf010 91 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 11:cef8dc1cf010 92 msg->setArgument( 0, status );
masahikofukasawa 11:cef8dc1cf010 93 msg->setArgument( 1, angle[0] );
masahikofukasawa 11:cef8dc1cf010 94 msg->setArgument( 2, angle[1] );
masahikofukasawa 11:cef8dc1cf010 95
masahikofukasawa 11:cef8dc1cf010 96 if( status != SUCCESS){
masahikofukasawa 11:cef8dc1cf010 97 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 98 }
masahikofukasawa 11:cef8dc1cf010 99 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 100 }
masahikofukasawa 11:cef8dc1cf010 101
masahikofukasawa 11:cef8dc1cf010 102 AkmSensor::Status Ak7451Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 11:cef8dc1cf010 103 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 104
masahikofukasawa 11:cef8dc1cf010 105 Message::Command cmd = in->getCommand();
masahikofukasawa 11:cef8dc1cf010 106
masahikofukasawa 11:cef8dc1cf010 107 switch(cmd){
masahikofukasawa 11:cef8dc1cf010 108 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 11:cef8dc1cf010 109 {
masahikofukasawa 11:cef8dc1cf010 110 AK7451::Status st;
masahikofukasawa 11:cef8dc1cf010 111
masahikofukasawa 11:cef8dc1cf010 112 st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 11:cef8dc1cf010 113 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 114 MSG("#Error when set user mode\r\n");
masahikofukasawa 11:cef8dc1cf010 115 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 116 }
masahikofukasawa 11:cef8dc1cf010 117 st = ak7451->setAngleZero(); // reset ZP data
masahikofukasawa 11:cef8dc1cf010 118 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 119 MSG("#Error setAngleZero: code=%d\r\n",st);
masahikofukasawa 11:cef8dc1cf010 120 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 121 }
masahikofukasawa 11:cef8dc1cf010 122 st = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 11:cef8dc1cf010 123 if( st != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 124 MSG("#Error when set normal mode\r\n");
masahikofukasawa 11:cef8dc1cf010 125 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 126 }
masahikofukasawa 14:21e177fc308a 127
masahikofukasawa 11:cef8dc1cf010 128 if( status == AkmSensor::ERROR ){
masahikofukasawa 11:cef8dc1cf010 129 out->setArgument(0,1);
masahikofukasawa 11:cef8dc1cf010 130 }else{
masahikofukasawa 11:cef8dc1cf010 131 out->setArgument(0,0);
masahikofukasawa 11:cef8dc1cf010 132 }
masahikofukasawa 11:cef8dc1cf010 133 break;
masahikofukasawa 11:cef8dc1cf010 134 }
masahikofukasawa 11:cef8dc1cf010 135 default:
masahikofukasawa 11:cef8dc1cf010 136 {
masahikofukasawa 11:cef8dc1cf010 137 MSG("#Error no command.\r\n");
masahikofukasawa 11:cef8dc1cf010 138 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 139 break;
masahikofukasawa 11:cef8dc1cf010 140 }
masahikofukasawa 11:cef8dc1cf010 141 }
masahikofukasawa 11:cef8dc1cf010 142
masahikofukasawa 11:cef8dc1cf010 143 return status;
masahikofukasawa 11:cef8dc1cf010 144 }
masahikofukasawa 11:cef8dc1cf010 145