Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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