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

ak7401ctrl.cpp@39:3821886c902e, 2017-07-18 (annotated)

Committer:
masahikofukasawa
Date:
Tue Jul 18 23:20:26 2017 +0000
Revision:
39:3821886c902e
Parent:
30:5a241d9b3262
Child:
43:45225713cd58
For AK09970 Small demo board. Takezawa-san's request.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 10:5c69b067d88a 1 #include "ak7401ctrl.h"
masahikofukasawa 10:5c69b067d88a 2 #include "debug.h"
masahikofukasawa 10:5c69b067d88a 3
masahikofukasawa 10:5c69b067d88a 4 /**
masahikofukasawa 10:5c69b067d88a 5 * Constructor.
masahikofukasawa 10:5c69b067d88a 6 *
masahikofukasawa 10:5c69b067d88a 7 */
masahikofukasawa 27:41aa9fb23a2f 8 Ak7401Ctrl::Ak7401Ctrl() : AkmSensor(){
masahikofukasawa 10:5c69b067d88a 9 ak7401 = NULL;
masahikofukasawa 10:5c69b067d88a 10 }
masahikofukasawa 10:5c69b067d88a 11
masahikofukasawa 10:5c69b067d88a 12 /**
masahikofukasawa 10:5c69b067d88a 13 * Destructor.
masahikofukasawa 10:5c69b067d88a 14 *
masahikofukasawa 10:5c69b067d88a 15 */
masahikofukasawa 10:5c69b067d88a 16 Ak7401Ctrl::~Ak7401Ctrl(){
masahikofukasawa 10:5c69b067d88a 17 if (ak7401) delete ak7401;
masahikofukasawa 10:5c69b067d88a 18 }
masahikofukasawa 10:5c69b067d88a 19
masahikofukasawa 10:5c69b067d88a 20 AkmSensor::Status Ak7401Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 21 primaryId = id;
masahikofukasawa 10:5c69b067d88a 22 subId = subid;
masahikofukasawa 10:5c69b067d88a 23
masahikofukasawa 10:5c69b067d88a 24 if(subId == SUB_ID_AK7401){
masahikofukasawa 10:5c69b067d88a 25 SPI* mSpi;
masahikofukasawa 10:5c69b067d88a 26 DigitalOut* mCs;
masahikofukasawa 10:5c69b067d88a 27 AK7401::Status status = AK7401::ERROR;
masahikofukasawa 10:5c69b067d88a 28 mSpi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 10:5c69b067d88a 29 mSpi->format(8,1); // 8bit, Mode=1
masahikofukasawa 10:5c69b067d88a 30 mSpi->frequency(1000000);
masahikofukasawa 10:5c69b067d88a 31 mCs = new DigitalOut(SPI_CS);
masahikofukasawa 10:5c69b067d88a 32 ak7401 = new AK7401();
masahikofukasawa 10:5c69b067d88a 33 ak7401->begin(mSpi, mCs);
masahikofukasawa 13:d008249f0359 34 sensorName = "AK7401";
masahikofukasawa 10:5c69b067d88a 35
masahikofukasawa 10:5c69b067d88a 36 status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
masahikofukasawa 10:5c69b067d88a 37 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 38 MSG("#AK7401 set USER mode failed.\r\n");
masahikofukasawa 10:5c69b067d88a 39 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 40 }
masahikofukasawa 14:21e177fc308a 41
masahikofukasawa 14:21e177fc308a 42 // check connection
masahikofukasawa 14:21e177fc308a 43 status = ak7401->checkConnection();
masahikofukasawa 14:21e177fc308a 44 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 45 MSG("#AK7401 check connection failed.\r\n");
masahikofukasawa 14:21e177fc308a 46 return AkmSensor::ERROR;
masahikofukasawa 14:21e177fc308a 47 }
masahikofukasawa 14:21e177fc308a 48
masahikofukasawa 14:21e177fc308a 49 // change rotation direction to CW from CCW
masahikofukasawa 10:5c69b067d88a 50 char data[2] = {0x00,0x07}; // set clockwise rotation
masahikofukasawa 10:5c69b067d88a 51 status = ak7401->writeEEPROM(0x05,data); // set clockwise
masahikofukasawa 10:5c69b067d88a 52 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 53 MSG("#AK7401 set rotation failed.\r\n");
masahikofukasawa 10:5c69b067d88a 54 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 55 }
masahikofukasawa 30:5a241d9b3262 56 /*
masahikofukasawa 10:5c69b067d88a 57 status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 10:5c69b067d88a 58 if( status != AK7401::SUCCESS ){
masahikofukasawa 14:21e177fc308a 59 MSG("#AK7401 set NORMAL mode failed.\r\n");
masahikofukasawa 10:5c69b067d88a 60 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 61 }
masahikofukasawa 30:5a241d9b3262 62 */
masahikofukasawa 14:21e177fc308a 63 MSG("#AK7401 init succeed.\r\n");
masahikofukasawa 11:cef8dc1cf010 64
masahikofukasawa 11:cef8dc1cf010 65 interval = SENSOR_SAMPLING_RATE; // 10Hz
masahikofukasawa 10:5c69b067d88a 66 }
masahikofukasawa 10:5c69b067d88a 67 else{
masahikofukasawa 10:5c69b067d88a 68 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 69 }
masahikofukasawa 10:5c69b067d88a 70
masahikofukasawa 10:5c69b067d88a 71 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 72 }
masahikofukasawa 10:5c69b067d88a 73
masahikofukasawa 10:5c69b067d88a 74 AkmSensor::Status Ak7401Ctrl::startSensor(){
masahikofukasawa 30:5a241d9b3262 75 AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 30:5a241d9b3262 76 if( status != AK7401::SUCCESS ){
masahikofukasawa 30:5a241d9b3262 77 MSG("#AK7401 set NORMAL mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 78 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 79 }
masahikofukasawa 29:b488d2c89fba 80 ticker.attach(callback(this, &base::setEvent), interval);
masahikofukasawa 10:5c69b067d88a 81 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 82 }
masahikofukasawa 10:5c69b067d88a 83
masahikofukasawa 10:5c69b067d88a 84 AkmSensor::Status Ak7401Ctrl::startSensor(const float sec){
masahikofukasawa 30:5a241d9b3262 85 AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_NORMAL_MODE);
masahikofukasawa 30:5a241d9b3262 86 if( status != AK7401::SUCCESS ){
masahikofukasawa 30:5a241d9b3262 87 MSG("#AK7401 set NORMAL mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 88 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 89 }
masahikofukasawa 10:5c69b067d88a 90 interval = sec;
masahikofukasawa 29:b488d2c89fba 91 ticker.attach(callback(this, &base::setEvent), interval);
masahikofukasawa 29:b488d2c89fba 92 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 10:5c69b067d88a 93 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 94 }
masahikofukasawa 10:5c69b067d88a 95
masahikofukasawa 10:5c69b067d88a 96 AkmSensor::Status Ak7401Ctrl::stopSensor(){
masahikofukasawa 30:5a241d9b3262 97 AK7401::Status status = ak7401->setOperationMode(AK7401::AK7401_USER_MODE);
masahikofukasawa 30:5a241d9b3262 98 if( status != AK7401::SUCCESS ){
masahikofukasawa 30:5a241d9b3262 99 MSG("#AK7401 set USER mode failed.\r\n");
masahikofukasawa 30:5a241d9b3262 100 return AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 101 }
masahikofukasawa 10:5c69b067d88a 102 ticker.detach();
masahikofukasawa 29:b488d2c89fba 103 AkmSensor::clearEvent();
masahikofukasawa 10:5c69b067d88a 104 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 105 }
masahikofukasawa 10:5c69b067d88a 106
masahikofukasawa 10:5c69b067d88a 107 AkmSensor::Status Ak7401Ctrl::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 108 AkmSensor::clearEvent();
masahikofukasawa 29:b488d2c89fba 109
masahikofukasawa 10:5c69b067d88a 110 char angle[2] = {0x00,0x00};
masahikofukasawa 10:5c69b067d88a 111 AK7401::Status status = ak7401->readAngle(angle);
masahikofukasawa 10:5c69b067d88a 112
masahikofukasawa 10:5c69b067d88a 113 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 114 msg->setArgument( 0, status );
masahikofukasawa 10:5c69b067d88a 115 msg->setArgument( 1, angle[0] );
masahikofukasawa 10:5c69b067d88a 116 msg->setArgument( 2, angle[1] );
masahikofukasawa 10:5c69b067d88a 117
masahikofukasawa 10:5c69b067d88a 118 if( status != SUCCESS){
masahikofukasawa 10:5c69b067d88a 119 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 120 }
masahikofukasawa 10:5c69b067d88a 121 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 122 }
masahikofukasawa 10:5c69b067d88a 123
masahikofukasawa 10:5c69b067d88a 124 AkmSensor::Status Ak7401Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 125 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 126 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 127 switch(cmd){
masahikofukasawa 30:5a241d9b3262 128 case Message::CMD_ANGLE_READ:
masahikofukasawa 30:5a241d9b3262 129 {
masahikofukasawa 30:5a241d9b3262 130 // read angle
masahikofukasawa 30:5a241d9b3262 131 char angle[2] = {0x00,0x00};
masahikofukasawa 30:5a241d9b3262 132 char density = 0x00;
masahikofukasawa 30:5a241d9b3262 133 char abnormal = 0x00;
masahikofukasawa 30:5a241d9b3262 134
masahikofukasawa 30:5a241d9b3262 135 if( ak7401->readAngleMeasureCommand(angle, &density, &abnormal) != AK7401::SUCCESS ){
masahikofukasawa 39:3821886c902e 136 MSG("#Error: Read angle\r\n");
masahikofukasawa 30:5a241d9b3262 137 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 138 }
masahikofukasawa 30:5a241d9b3262 139 out->setCommand(Message::CMD_ANGLE_READ);
masahikofukasawa 30:5a241d9b3262 140 out->setArgument( 0, ((abnormal&0x01)==0x00)|(status==AkmSensor::ERROR) ? 0x01 : 0x00 );
masahikofukasawa 30:5a241d9b3262 141 out->setArgument( 1, angle[0] );
masahikofukasawa 30:5a241d9b3262 142 out->setArgument( 2, angle[1] );
masahikofukasawa 30:5a241d9b3262 143 out->setArgument( 3, density );
masahikofukasawa 30:5a241d9b3262 144 break;
masahikofukasawa 30:5a241d9b3262 145 }
masahikofukasawa 10:5c69b067d88a 146 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 10:5c69b067d88a 147 {
masahikofukasawa 14:21e177fc308a 148 if( ak7401->setAngleZero() != AK7401::SUCCESS ){
masahikofukasawa 39:3821886c902e 149 MSG("#Error: Set angle zero\r\n");
masahikofukasawa 10:5c69b067d88a 150 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 151 }
masahikofukasawa 10:5c69b067d88a 152 if( status == AkmSensor::ERROR ){
masahikofukasawa 10:5c69b067d88a 153 out->setArgument(0,1);
masahikofukasawa 10:5c69b067d88a 154 }else{
masahikofukasawa 10:5c69b067d88a 155 out->setArgument(0,0);
masahikofukasawa 10:5c69b067d88a 156 }
masahikofukasawa 10:5c69b067d88a 157 break;
masahikofukasawa 10:5c69b067d88a 158 }
masahikofukasawa 30:5a241d9b3262 159 case Message::CMD_REG_WRITE:
masahikofukasawa 30:5a241d9b3262 160 case Message::CMD_REG_WRITEN:
masahikofukasawa 30:5a241d9b3262 161 {
masahikofukasawa 30:5a241d9b3262 162 char address = in->getArgument(0);
masahikofukasawa 30:5a241d9b3262 163 int len = in->getArgument(1);
masahikofukasawa 30:5a241d9b3262 164 if(len != 2){
masahikofukasawa 39:3821886c902e 165 MSG("#Error: length=%d. Only support 2byte length\r\n",len);
masahikofukasawa 30:5a241d9b3262 166 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 167 return status;
masahikofukasawa 30:5a241d9b3262 168 }
masahikofukasawa 30:5a241d9b3262 169 if(in->getArgNum() != len+2){
masahikofukasawa 39:3821886c902e 170 MSG("#Error: Argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 30:5a241d9b3262 171 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 172 out->setArgument(0,(char)status);
masahikofukasawa 30:5a241d9b3262 173 return status;
masahikofukasawa 30:5a241d9b3262 174 }
masahikofukasawa 30:5a241d9b3262 175
masahikofukasawa 30:5a241d9b3262 176 char data[len];
masahikofukasawa 30:5a241d9b3262 177 for(int i=0; i<len; i++){
masahikofukasawa 30:5a241d9b3262 178 data[i] = in->getArgument(i+2);
masahikofukasawa 30:5a241d9b3262 179 }
masahikofukasawa 30:5a241d9b3262 180 if( ak7401->writeRegister(address, data) != AK7401::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 181 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 182 MSG("#Error: Register write.\r\n");
masahikofukasawa 30:5a241d9b3262 183 }
masahikofukasawa 30:5a241d9b3262 184 if( ak7401->writeEEPROM(address, data) != AK7401::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 185 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 186 MSG("#Error: EEPROM write.\r\n");
masahikofukasawa 30:5a241d9b3262 187 }
masahikofukasawa 30:5a241d9b3262 188 out->setArgument(0,(char)status);
masahikofukasawa 30:5a241d9b3262 189 break;
masahikofukasawa 30:5a241d9b3262 190 }
masahikofukasawa 30:5a241d9b3262 191 case Message::CMD_REG_READ:
masahikofukasawa 30:5a241d9b3262 192 case Message::CMD_REG_READN:
masahikofukasawa 30:5a241d9b3262 193 {
masahikofukasawa 30:5a241d9b3262 194 if(in->getArgNum() != 2){
masahikofukasawa 39:3821886c902e 195 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 30:5a241d9b3262 196 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 197 return status;
masahikofukasawa 30:5a241d9b3262 198 }
masahikofukasawa 30:5a241d9b3262 199
masahikofukasawa 30:5a241d9b3262 200 char address = in->getArgument(0);
masahikofukasawa 30:5a241d9b3262 201 int len = in->getArgument(1);
masahikofukasawa 30:5a241d9b3262 202 if(len != 2){
masahikofukasawa 39:3821886c902e 203 MSG("#Error: length=%d. Only support 2byte length\r\n",len);
masahikofukasawa 30:5a241d9b3262 204 status = AkmSensor::ERROR;
masahikofukasawa 30:5a241d9b3262 205 return status;
masahikofukasawa 30:5a241d9b3262 206 }
masahikofukasawa 30:5a241d9b3262 207 char data[len];
masahikofukasawa 30:5a241d9b3262 208 if( ak7401->readRegister(address, data) != AK7401::SUCCESS) {
masahikofukasawa 30:5a241d9b3262 209 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 210 MSG("#Error: register read.\r\n");
masahikofukasawa 30:5a241d9b3262 211 }
masahikofukasawa 30:5a241d9b3262 212 for(int i=0; i<len; i++){
masahikofukasawa 30:5a241d9b3262 213 out->setArgument(i, data[i]);
masahikofukasawa 30:5a241d9b3262 214 }
masahikofukasawa 30:5a241d9b3262 215 break;
masahikofukasawa 30:5a241d9b3262 216 }
masahikofukasawa 10:5c69b067d88a 217 default:
masahikofukasawa 10:5c69b067d88a 218 {
masahikofukasawa 39:3821886c902e 219 MSG("#Error: no command.\r\n");
masahikofukasawa 10:5c69b067d88a 220 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 221 break;
masahikofukasawa 10:5c69b067d88a 222 }
masahikofukasawa 10:5c69b067d88a 223 }
masahikofukasawa 10:5c69b067d88a 224
masahikofukasawa 10:5c69b067d88a 225 return status;
masahikofukasawa 10:5c69b067d88a 226 }
masahikofukasawa 10:5c69b067d88a 227
masahikofukasawa 13:d008249f0359 228