Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7451ctrl.cpp@49:c8f8946129b6, 2018-05-01 (annotated)

Committer:
tkstreet
Date:
Tue May 01 21:31:15 2018 +0000
Revision:
49:c8f8946129b6
Parent:
47:221ec4b404ec 
Modified for Rev.E. compatibility.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 11:cef8dc1cf010 1 #include "ak7451ctrl.h"
masahikofukasawa 11:cef8dc1cf010 2
masahikofukasawa 11:cef8dc1cf010 3 /**
masahikofukasawa 11:cef8dc1cf010 4 * Constructor.
masahikofukasawa 11:cef8dc1cf010 5 *
masahikofukasawa 11:cef8dc1cf010 6 */
masahikofukasawa 27:41aa9fb23a2f 7 Ak7451Ctrl::Ak7451Ctrl() : AkmSensor(){
masahikofukasawa 11:cef8dc1cf010 8 ak7451 = NULL;
masahikofukasawa 11:cef8dc1cf010 9 }
masahikofukasawa 11:cef8dc1cf010 10
masahikofukasawa 11:cef8dc1cf010 11 /**
masahikofukasawa 11:cef8dc1cf010 12 * Destructor.
masahikofukasawa 11:cef8dc1cf010 13 *
masahikofukasawa 11:cef8dc1cf010 14 */
masahikofukasawa 11:cef8dc1cf010 15 Ak7451Ctrl::~Ak7451Ctrl(){
masahikofukasawa 11:cef8dc1cf010 16 if (ak7451) delete ak7451;
masahikofukasawa 11:cef8dc1cf010 17 }
masahikofukasawa 11:cef8dc1cf010 18
masahikofukasawa 11:cef8dc1cf010 19 AkmSensor::Status Ak7451Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 11:cef8dc1cf010 20 primaryId = id;
masahikofukasawa 11:cef8dc1cf010 21 subId = subid;
masahikofukasawa 11:cef8dc1cf010 22
masahikofukasawa 11:cef8dc1cf010 23 if(subId == SUB_ID_AK7451){
tkstreet 25:76c11ab5060e 24 SPI* mSpi; // SPI connection
tkstreet 25:76c11ab5060e 25 DigitalOut* mCs; // Chip select pin
tkstreet 25:76c11ab5060e 26
tkstreet 25:76c11ab5060e 27 // Initialize SPI connection
masahikofukasawa 11:cef8dc1cf010 28 AK7451::Status status = AK7451::ERROR;
masahikofukasawa 11:cef8dc1cf010 29 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 11:cef8dc1cf010 30 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 11:cef8dc1cf010 31 mSpi->frequency(1000000);
tkstreet 25:76c11ab5060e 32
tkstreet 25:76c11ab5060e 33 // Initialize chip select pin
masahikofukasawa 11:cef8dc1cf010 34 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 11:cef8dc1cf010 35 ak7451 = new AK7451();
masahikofukasawa 11:cef8dc1cf010 36 ak7451->begin(mSpi, mCs);
masahikofukasawa 13:d008249f0359 37 sensorName = "AK7451";
masahikofukasawa 11:cef8dc1cf010 38
tkstreet 25:76c11ab5060e 39 // Set to User Mode
masahikofukasawa 11:cef8dc1cf010 40 status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 11:cef8dc1cf010 41 if( status != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 42 MSG("#Error: AK7451 user mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 43 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 44 }
masahikofukasawa 11:cef8dc1cf010 45 MSG("#AK7451 user mode succeed.\r\n");
masahikofukasawa 30:5a241d9b3262 46
masahikofukasawa 30:5a241d9b3262 47 // E_RD 0x1 clockwise rotation
masahikofukasawa 30:5a241d9b3262 48 // E_Z_MODE 0x0 normal output
masahikofukasawa 30:5a241d9b3262 49 // E_ABZ_E 0x1 ABZ Output enable
masahikofukasawa 30:5a241d9b3262 50 // E_ABZ_HYS 0x2 ABZ Hysteresis=1LSB
masahikofukasawa 30:5a241d9b3262 51 // ABZ_RES 0x0 ABZ phase resolution = 1024ppr
masahikofukasawa 30:5a241d9b3262 52 char data[2] = {0x02,0xA0};
tkstreet 25:76c11ab5060e 53
masahikofukasawa 11:cef8dc1cf010 54 status = ak7451->writeEEPROM(0x07,data); // set clockwise
masahikofukasawa 11:cef8dc1cf010 55 if( status != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 56 MSG("#Error: AK7451 write EEPROM failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 57 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 58 }
masahikofukasawa 39:3821886c902e 59 MSG("#Error: AK7451 write EEPROM succeed.\r\n");
tkstreet 34:1ea3357c8d9a 60
masahikofukasawa 30:5a241d9b3262 61 /*
tkstreet 25:76c11ab5060e 62 // Set to Normal Mode
masahikofukasawa 11:cef8dc1cf010 63 status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 11:cef8dc1cf010 64 if( status != AK7451::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 65 MSG("#AK7451 normal mode failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 66 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 67 }
masahikofukasawa 11:cef8dc1cf010 68 MSG("#AK7451 normal mode succeed.\r\n");
masahikofukasawa 30:5a241d9b3262 69 */
masahikofukasawa 11:cef8dc1cf010 70 interval = SENSOR_SAMPLING_RATE; // 10Hz
masahikofukasawa 11:cef8dc1cf010 71 }
masahikofukasawa 11:cef8dc1cf010 72 else{
masahikofukasawa 11:cef8dc1cf010 73 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 74 }
masahikofukasawa 11:cef8dc1cf010 75
masahikofukasawa 11:cef8dc1cf010 76 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 77 }
masahikofukasawa 11:cef8dc1cf010 78
masahikofukasawa 11:cef8dc1cf010 79 AkmSensor::Status Ak7451Ctrl::startSensor(){
masahikofukasawa 30:5a241d9b3262 80 AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 30:5a241d9b3262 81 if( status != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 82 MSG("#Error: AK7451 normal mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 83 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 84 }
masahikofukasawa 29:b488d2c89fba 85 ticker.attach(callback(this, &AkmSensor::setEvent), interval);
masahikofukasawa 11:cef8dc1cf010 86 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 87 }
masahikofukasawa 11:cef8dc1cf010 88
masahikofukasawa 11:cef8dc1cf010 89 AkmSensor::Status Ak7451Ctrl::startSensor(const float sec){
masahikofukasawa 30:5a241d9b3262 90 AK7451::Status status = ak7451->setOperationMode(AK7451::AK7451_NORMAL_MODE);
masahikofukasawa 30:5a241d9b3262 91 if( status != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 92 MSG("#Error: AK7451 normal mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 93 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 94 }
masahikofukasawa 30:5a241d9b3262 95
masahikofukasawa 11:cef8dc1cf010 96 interval = sec;
masahikofukasawa 29:b488d2c89fba 97 ticker.attach(callback(this, &AkmSensor::setEvent), interval);
masahikofukasawa 29:b488d2c89fba 98 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 11:cef8dc1cf010 99 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 100 }
masahikofukasawa 11:cef8dc1cf010 101
masahikofukasawa 11:cef8dc1cf010 102 AkmSensor::Status Ak7451Ctrl::stopSensor(){
masahikofukasawa 30:5a241d9b3262 103 AK7451::Status status;
masahikofukasawa 30:5a241d9b3262 104 status = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 30:5a241d9b3262 105 if( status != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 106 MSG("#Error: AK7451 user mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 107 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 108 }
masahikofukasawa 11:cef8dc1cf010 109 ticker.detach();
masahikofukasawa 29:b488d2c89fba 110 AkmSensor::clearEvent();
masahikofukasawa 11:cef8dc1cf010 111 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 112 }
masahikofukasawa 11:cef8dc1cf010 113
masahikofukasawa 11:cef8dc1cf010 114 AkmSensor::Status Ak7451Ctrl::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 115 AkmSensor::clearEvent();
masahikofukasawa 29:b488d2c89fba 116
masahikofukasawa 11:cef8dc1cf010 117 char angle[2] = {0x00,0x00};
masahikofukasawa 11:cef8dc1cf010 118 AK7451::Status status = ak7451->readAngle(angle);
masahikofukasawa 11:cef8dc1cf010 119
masahikofukasawa 11:cef8dc1cf010 120 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 11:cef8dc1cf010 121 msg->setArgument( 0, status );
masahikofukasawa 11:cef8dc1cf010 122 msg->setArgument( 1, angle[0] );
masahikofukasawa 11:cef8dc1cf010 123 msg->setArgument( 2, angle[1] );
masahikofukasawa 11:cef8dc1cf010 124
masahikofukasawa 47:221ec4b404ec 125 if( status != AK7451::SUCCESS){
masahikofukasawa 11:cef8dc1cf010 126 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 127 }
masahikofukasawa 11:cef8dc1cf010 128 return AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 129 }
masahikofukasawa 11:cef8dc1cf010 130
masahikofukasawa 11:cef8dc1cf010 131 AkmSensor::Status Ak7451Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 11:cef8dc1cf010 132 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 133
masahikofukasawa 11:cef8dc1cf010 134 Message::Command cmd = in->getCommand();
masahikofukasawa 11:cef8dc1cf010 135
masahikofukasawa 11:cef8dc1cf010 136 switch(cmd){
masahikofukasawa 30:5a241d9b3262 137 case Message::CMD_ANGLE_READ:
masahikofukasawa 30:5a241d9b3262 138 {
masahikofukasawa 30:5a241d9b3262 139 // read angle
masahikofukasawa 30:5a241d9b3262 140 char angle[2] = {0x00,0x00};
masahikofukasawa 30:5a241d9b3262 141 char density = 0x00;
masahikofukasawa 30:5a241d9b3262 142 char abnormal = 0x00;
masahikofukasawa 30:5a241d9b3262 143
masahikofukasawa 30:5a241d9b3262 144 if( ak7451->readAngleMeasureCommand(angle, &density, &abnormal) != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 145 MSG("#Error: read angle\r\n");
masahikofukasawa 30:5a241d9b3262 146 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 147 }
masahikofukasawa 30:5a241d9b3262 148 out->setCommand(Message::CMD_ANGLE_READ);
masahikofukasawa 30:5a241d9b3262 149 out->setArgument( 0, (abnormal != 0x03)|(status == AkmSensor::ERROR) ? 0x01 : 0x00 );
masahikofukasawa 30:5a241d9b3262 150 out->setArgument( 1, angle[0] );
masahikofukasawa 30:5a241d9b3262 151 out->setArgument( 2, angle[1] );
masahikofukasawa 30:5a241d9b3262 152 out->setArgument( 3, density );
masahikofukasawa 30:5a241d9b3262 153 break;
masahikofukasawa 30:5a241d9b3262 154 }
masahikofukasawa 11:cef8dc1cf010 155 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 11:cef8dc1cf010 156 {
masahikofukasawa 30:5a241d9b3262 157 AK7451::Status st;
masahikofukasawa 30:5a241d9b3262 158 /*
masahikofukasawa 30:5a241d9b3262 159 AK7451::OperationMode mode = AK7451::AK7451_USER_MODE;
masahikofukasawa 11:cef8dc1cf010 160
masahikofukasawa 30:5a241d9b3262 161 // check the mode
masahikofukasawa 30:5a241d9b3262 162 char data[1];
masahikofukasawa 30:5a241d9b3262 163 if( ak7451->readRegister(0x00, data) != AK7451::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 164 MSG("#NORMAL MODE detected. Set into USER mode.\r\n");
masahikofukasawa 30:5a241d9b3262 165 mode = AK7451::AK7451_NORMAL_MODE;
masahikofukasawa 30:5a241d9b3262 166 st = ak7451->setOperationMode(AK7451::AK7451_USER_MODE);
masahikofukasawa 30:5a241d9b3262 167 if( st != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 168 MSG("#Error: when set user mode\r\n");
masahikofukasawa 30:5a241d9b3262 169 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 170 }
masahikofukasawa 11:cef8dc1cf010 171 }
masahikofukasawa 30:5a241d9b3262 172 */
masahikofukasawa 11:cef8dc1cf010 173 st = ak7451->setAngleZero(); // reset ZP data
masahikofukasawa 11:cef8dc1cf010 174 if( st != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 175 MSG("#Error: setAngleZero: code=%d\r\n",st);
masahikofukasawa 11:cef8dc1cf010 176 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 177 }
masahikofukasawa 30:5a241d9b3262 178 /*
masahikofukasawa 30:5a241d9b3262 179 st = ak7451->setOperationMode(mode);
masahikofukasawa 11:cef8dc1cf010 180 if( st != AK7451::SUCCESS ){
masahikofukasawa 39:3821886c902e 181 MSG("#Error: when set mode:%d\r\n",mode);
masahikofukasawa 11:cef8dc1cf010 182 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 183 }
masahikofukasawa 30:5a241d9b3262 184 */
masahikofukasawa 11:cef8dc1cf010 185 if( status == AkmSensor::ERROR ){
masahikofukasawa 11:cef8dc1cf010 186 out->setArgument(0,1);
masahikofukasawa 11:cef8dc1cf010 187 }else{
masahikofukasawa 11:cef8dc1cf010 188 out->setArgument(0,0);
masahikofukasawa 11:cef8dc1cf010 189 }
masahikofukasawa 11:cef8dc1cf010 190 break;
masahikofukasawa 11:cef8dc1cf010 191 }
masahikofukasawa 30:5a241d9b3262 192 case Message::CMD_REG_WRITE:
masahikofukasawa 30:5a241d9b3262 193 case Message::CMD_REG_WRITEN:
masahikofukasawa 30:5a241d9b3262 194 {
masahikofukasawa 30:5a241d9b3262 195 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 196 const int len = in->getArgument(1);
masahikofukasawa 30:5a241d9b3262 197 if(len != 2){
masahikofukasawa 39:3821886c902e 198 MSG("#Error: length=%d. Only support 2byte length\r\n",len);
masahikofukasawa 30:5a241d9b3262 199 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 200 return status;
masahikofukasawa 30:5a241d9b3262 201 }
masahikofukasawa 30:5a241d9b3262 202 if(in->getArgNum() != len+2){
masahikofukasawa 39:3821886c902e 203 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 30:5a241d9b3262 204 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 205 out->setArgument(0,(char)status);
masahikofukasawa 30:5a241d9b3262 206 return status;
masahikofukasawa 30:5a241d9b3262 207 }
masahikofukasawa 30:5a241d9b3262 208
masahikofukasawa 30:5a241d9b3262 209 char data[len];
masahikofukasawa 30:5a241d9b3262 210 for(int i=0; i<len; i++){
masahikofukasawa 30:5a241d9b3262 211 data[i] = in->getArgument(i+2);
masahikofukasawa 30:5a241d9b3262 212 }
masahikofukasawa 30:5a241d9b3262 213 if( ak7451->writeRegister(address, data) != AK7451::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 214 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 215 MSG("#Error: register write.\r\n");
masahikofukasawa 30:5a241d9b3262 216 }
masahikofukasawa 30:5a241d9b3262 217 if( ak7451->writeEEPROM(address, data) != AK7451::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 218 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 219 MSG("#Error: EEPROM write.\r\n");
masahikofukasawa 30:5a241d9b3262 220 }
masahikofukasawa 30:5a241d9b3262 221 out->setArgument(0,(char)status);
masahikofukasawa 30:5a241d9b3262 222 break;
masahikofukasawa 30:5a241d9b3262 223 }
masahikofukasawa 30:5a241d9b3262 224 case Message::CMD_REG_READ:
masahikofukasawa 30:5a241d9b3262 225 case Message::CMD_REG_READN:
masahikofukasawa 30:5a241d9b3262 226 {
masahikofukasawa 30:5a241d9b3262 227 if(in->getArgNum() != 2){
masahikofukasawa 39:3821886c902e 228 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 30:5a241d9b3262 229 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 230 return status;
masahikofukasawa 30:5a241d9b3262 231 }
masahikofukasawa 30:5a241d9b3262 232
masahikofukasawa 30:5a241d9b3262 233 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 234 const int len = in->getArgument(1);
masahikofukasawa 30:5a241d9b3262 235 if(len != 2){
masahikofukasawa 39:3821886c902e 236 MSG("#Error: length=%d. Only support 2byte length\r\n",len);
masahikofukasawa 30:5a241d9b3262 237 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 238 return status;
masahikofukasawa 30:5a241d9b3262 239 }
masahikofukasawa 30:5a241d9b3262 240 char data[len];
masahikofukasawa 30:5a241d9b3262 241 if( ak7451->readRegister(address, data) != AK7451::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 242 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 243 MSG("#Error: register read.\r\n");
masahikofukasawa 30:5a241d9b3262 244 }
masahikofukasawa 30:5a241d9b3262 245 for(int i=0; i<len; i++){
masahikofukasawa 30:5a241d9b3262 246 out->setArgument(i, data[i]);
masahikofukasawa 30:5a241d9b3262 247 }
masahikofukasawa 30:5a241d9b3262 248 break;
masahikofukasawa 30:5a241d9b3262 249 }
masahikofukasawa 11:cef8dc1cf010 250 default:
masahikofukasawa 11:cef8dc1cf010 251 {
masahikofukasawa 39:3821886c902e 252 MSG("#Error: no command.\r\n");
masahikofukasawa 11:cef8dc1cf010 253 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 254 break;
masahikofukasawa 11:cef8dc1cf010 255 }
masahikofukasawa 11:cef8dc1cf010 256 }
masahikofukasawa 11:cef8dc1cf010 257
masahikofukasawa 11:cef8dc1cf010 258 return status;
masahikofukasawa 11:cef8dc1cf010 259 }
masahikofukasawa 11:cef8dc1cf010 260