Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak7451ctrl.cpp@30:5a241d9b3262, 2017-04-28 (annotated)

Committer:
masahikofukasawa
Date:
Fri Apr 28 20:32:31 2017 +0000
Revision:
30:5a241d9b3262
Parent:
29:b488d2c89fba
Child:
34:1ea3357c8d9a
release RevD7.014. Add register access for AK7401/AK7451. Also for multi sensor control.

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