Taylor Street / AkmSensor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of AkmSensor by AKM Development Platform

ak7451ctrl.cpp@46:5938ad2039b0, 2017-12-13 (annotated)

Committer:
masahikofukasawa
Date:
Wed Dec 13 23:35:23 2017 +0000
Revision:
46:5938ad2039b0
Parent:
43:45225713cd58
Child:
47:221ec4b404ec
debugged for warnings

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 11:cef8dc1cf010 125 if( status != 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