Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@11:cef8dc1cf010, 2016-07-22 (annotated)

Committer:
masahikofukasawa
Date:
Fri Jul 22 22:54:11 2016 +0000
Revision:
11:cef8dc1cf010
Parent:
10:5c69b067d88a
Child:
15:1238993fd75f
RevD7_004

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 0:7a00359e701e 1 #include "akmakd.h"
masahikofukasawa 0:7a00359e701e 2 #include "ak8963.h"
masahikofukasawa 0:7a00359e701e 3 #include "ak099xx.h"
masahikofukasawa 0:7a00359e701e 4 #include "debug.h"
masahikofukasawa 0:7a00359e701e 5
masahikofukasawa 0:7a00359e701e 6 /**
masahikofukasawa 0:7a00359e701e 7 * Constructor.
masahikofukasawa 0:7a00359e701e 8 *
masahikofukasawa 0:7a00359e701e 9 */
masahikofukasawa 0:7a00359e701e 10 AkmAkd::AkmAkd(){
masahikofukasawa 0:7a00359e701e 11 event = false;
masahikofukasawa 0:7a00359e701e 12 compass = NULL;
masahikofukasawa 0:7a00359e701e 13 drdy = NULL;
masahikofukasawa 0:7a00359e701e 14 sensorName = "";
masahikofukasawa 0:7a00359e701e 15 }
masahikofukasawa 0:7a00359e701e 16
masahikofukasawa 0:7a00359e701e 17 /**
masahikofukasawa 0:7a00359e701e 18 * Destructor.
masahikofukasawa 0:7a00359e701e 19 *
masahikofukasawa 0:7a00359e701e 20 */
masahikofukasawa 0:7a00359e701e 21 AkmAkd::~AkmAkd(){
masahikofukasawa 0:7a00359e701e 22 drdy->rise(0);
masahikofukasawa 0:7a00359e701e 23 if (compass) delete compass;
masahikofukasawa 0:7a00359e701e 24 if (drdy) delete drdy;
masahikofukasawa 0:7a00359e701e 25 }
masahikofukasawa 0:7a00359e701e 26
masahikofukasawa 0:7a00359e701e 27 AkmSensor::Status AkmAkd::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 0:7a00359e701e 28 primaryId = id;
masahikofukasawa 0:7a00359e701e 29 subId = subid;
masahikofukasawa 0:7a00359e701e 30
masahikofukasawa 0:7a00359e701e 31 AkmECompass::SensorType type;
masahikofukasawa 7:e269411568c9 32 AK099XX* ak099xx;
masahikofukasawa 7:e269411568c9 33 AK8963* ak8963;
masahikofukasawa 9:6fa3e7b17c27 34
masahikofukasawa 9:6fa3e7b17c27 35 mode = AkmECompass::MODE_POWER_DOWN;
masahikofukasawa 9:6fa3e7b17c27 36 nsf = AkmECompass::NSF_DISABLE;
masahikofukasawa 9:6fa3e7b17c27 37 sdr = AkmECompass::SDR_LOW_POWER_DRIVE;
masahikofukasawa 9:6fa3e7b17c27 38
masahikofukasawa 0:7a00359e701e 39 if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
masahikofukasawa 0:7a00359e701e 40
masahikofukasawa 7:e269411568c9 41 drdy = new InterruptIn(I2C_DRDY);
masahikofukasawa 7:e269411568c9 42 drdy->rise(0);
masahikofukasawa 7:e269411568c9 43
masahikofukasawa 0:7a00359e701e 44 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 0:7a00359e701e 45 i2c->frequency(I2C_SPEED_100KHZ);
masahikofukasawa 0:7a00359e701e 46
masahikofukasawa 0:7a00359e701e 47 switch(subid){
masahikofukasawa 0:7a00359e701e 48 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 0:7a00359e701e 49 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 0:7a00359e701e 50 type = AkmECompass::AK8963;
masahikofukasawa 0:7a00359e701e 51 ak8963 = new AK8963();
masahikofukasawa 0:7a00359e701e 52 compass = ak8963;
masahikofukasawa 0:7a00359e701e 53 AkmAkd::sensorName = "AK8963";
masahikofukasawa 0:7a00359e701e 54 break;
masahikofukasawa 10:5c69b067d88a 55 case AkmAkd::SUB_ID_AK09911C:
masahikofukasawa 0:7a00359e701e 56 type = AkmECompass::AK09911;
masahikofukasawa 0:7a00359e701e 57 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 58 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 59 AkmAkd::sensorName = "AK09911C";
masahikofukasawa 0:7a00359e701e 60 break;
masahikofukasawa 10:5c69b067d88a 61 case AkmAkd::SUB_ID_AK09912C:
masahikofukasawa 0:7a00359e701e 62 type = AkmECompass::AK09912;
masahikofukasawa 0:7a00359e701e 63 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 64 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 65 AkmAkd::sensorName = "AK09912C";
masahikofukasawa 0:7a00359e701e 66 break;
masahikofukasawa 10:5c69b067d88a 67 case AkmAkd::SUB_ID_AK09915C:
masahikofukasawa 0:7a00359e701e 68 type = AkmECompass::AK09915;
masahikofukasawa 0:7a00359e701e 69 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 70 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 71 AkmAkd::sensorName = "AK09915C";
masahikofukasawa 10:5c69b067d88a 72 break;
masahikofukasawa 10:5c69b067d88a 73 case AkmAkd::SUB_ID_AK09915D:
masahikofukasawa 10:5c69b067d88a 74 type = AkmECompass::AK09915;
masahikofukasawa 10:5c69b067d88a 75 ak099xx = new AK099XX();
masahikofukasawa 10:5c69b067d88a 76 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 77 AkmAkd::sensorName = "AK09915D";
masahikofukasawa 0:7a00359e701e 78 break;
masahikofukasawa 0:7a00359e701e 79 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 0:7a00359e701e 80 type = AkmECompass::AK09916C;
masahikofukasawa 0:7a00359e701e 81 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 82 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 83 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 0:7a00359e701e 84 break;
masahikofukasawa 10:5c69b067d88a 85 case AkmAkd::SUB_ID_AK09916D:
masahikofukasawa 10:5c69b067d88a 86 type = AkmECompass::AK09916D;
masahikofukasawa 10:5c69b067d88a 87 ak099xx = new AK099XX();
masahikofukasawa 10:5c69b067d88a 88 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 89 AkmAkd::sensorName = "AK09916D";
masahikofukasawa 10:5c69b067d88a 90 break;
masahikofukasawa 0:7a00359e701e 91 default:
masahikofukasawa 0:7a00359e701e 92 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 93 // break;
masahikofukasawa 0:7a00359e701e 94 }
masahikofukasawa 0:7a00359e701e 95
masahikofukasawa 0:7a00359e701e 96 bool foundSensor = false;
masahikofukasawa 0:7a00359e701e 97
masahikofukasawa 0:7a00359e701e 98 AkmECompass::SlaveAddress slaveAddr[]
masahikofukasawa 0:7a00359e701e 99 = { AkmECompass::SLAVE_ADDR_1,
masahikofukasawa 0:7a00359e701e 100 AkmECompass::SLAVE_ADDR_2,
masahikofukasawa 0:7a00359e701e 101 AkmECompass::SLAVE_ADDR_3,
masahikofukasawa 0:7a00359e701e 102 AkmECompass::SLAVE_ADDR_4};
masahikofukasawa 0:7a00359e701e 103
masahikofukasawa 0:7a00359e701e 104 for(int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 0:7a00359e701e 105 {
masahikofukasawa 0:7a00359e701e 106 compass->init(i2c, slaveAddr[i], type);
masahikofukasawa 0:7a00359e701e 107 // Checks connectivity
masahikofukasawa 0:7a00359e701e 108 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 109 // found
masahikofukasawa 0:7a00359e701e 110 foundSensor = true;
masahikofukasawa 0:7a00359e701e 111 break;
masahikofukasawa 0:7a00359e701e 112 }
masahikofukasawa 0:7a00359e701e 113 }
masahikofukasawa 7:e269411568c9 114 if(foundSensor != true){
masahikofukasawa 7:e269411568c9 115 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 116 }
masahikofukasawa 0:7a00359e701e 117
masahikofukasawa 0:7a00359e701e 118 // read a data to reset DRDY
masahikofukasawa 0:7a00359e701e 119 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 120 compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 121
masahikofukasawa 0:7a00359e701e 122 }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
masahikofukasawa 7:e269411568c9 123
masahikofukasawa 7:e269411568c9 124 drdy = new InterruptIn(SPI_DRDY);
masahikofukasawa 7:e269411568c9 125 drdy->rise(0);
masahikofukasawa 0:7a00359e701e 126
masahikofukasawa 7:e269411568c9 127 SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 7:e269411568c9 128 spi->format(8,3); // 8bit, Mode=3
masahikofukasawa 7:e269411568c9 129 spi->frequency(1000000);
masahikofukasawa 7:e269411568c9 130
masahikofukasawa 7:e269411568c9 131 DigitalOut* cs = new DigitalOut(SPI_CS);
masahikofukasawa 7:e269411568c9 132
masahikofukasawa 7:e269411568c9 133 switch(subid){
masahikofukasawa 7:e269411568c9 134 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 7:e269411568c9 135 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 7:e269411568c9 136 type = AkmECompass::AK8963;
masahikofukasawa 7:e269411568c9 137 ak8963 = new AK8963();
masahikofukasawa 7:e269411568c9 138 compass = ak8963;
masahikofukasawa 7:e269411568c9 139 AkmAkd::sensorName = "AK8963";
masahikofukasawa 7:e269411568c9 140 break;
masahikofukasawa 10:5c69b067d88a 141 case AkmAkd::SUB_ID_AK09911C:
masahikofukasawa 7:e269411568c9 142 type = AkmECompass::AK09911;
masahikofukasawa 10:5c69b067d88a 143 AkmAkd::sensorName = "AK09911C";
masahikofukasawa 7:e269411568c9 144 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 145 case AkmAkd::SUB_ID_AK09912C:
masahikofukasawa 7:e269411568c9 146 type = AkmECompass::AK09912;
masahikofukasawa 7:e269411568c9 147 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 148 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 149 AkmAkd::sensorName = "AK09912C";
masahikofukasawa 7:e269411568c9 150 break;
masahikofukasawa 10:5c69b067d88a 151 case AkmAkd::SUB_ID_AK09915C:
masahikofukasawa 7:e269411568c9 152 type = AkmECompass::AK09915;
masahikofukasawa 7:e269411568c9 153 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 154 compass = ak099xx;
masahikofukasawa 10:5c69b067d88a 155 AkmAkd::sensorName = "AK09915C";
masahikofukasawa 10:5c69b067d88a 156 break;
masahikofukasawa 10:5c69b067d88a 157 case AkmAkd::SUB_ID_AK09915D:
masahikofukasawa 10:5c69b067d88a 158 type = AkmECompass::AK09915;
masahikofukasawa 10:5c69b067d88a 159 AkmAkd::sensorName = "AK09915D";
masahikofukasawa 7:e269411568c9 160 break;
masahikofukasawa 7:e269411568c9 161 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 7:e269411568c9 162 type = AkmECompass::AK09916C;
masahikofukasawa 7:e269411568c9 163 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 7:e269411568c9 164 // doesn't support SPI
masahikofukasawa 7:e269411568c9 165 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 166 case AkmAkd::SUB_ID_AK09916D:
masahikofukasawa 10:5c69b067d88a 167 type = AkmECompass::AK09916D;
masahikofukasawa 10:5c69b067d88a 168 AkmAkd::sensorName = "AK09916D";
masahikofukasawa 10:5c69b067d88a 169 // doesn't support SPI
masahikofukasawa 10:5c69b067d88a 170 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 171 default:
masahikofukasawa 7:e269411568c9 172 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 173 // break;
masahikofukasawa 7:e269411568c9 174 }
masahikofukasawa 7:e269411568c9 175
masahikofukasawa 7:e269411568c9 176 bool foundSensor = false;
masahikofukasawa 7:e269411568c9 177 compass->init(spi, cs, type);
masahikofukasawa 7:e269411568c9 178 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 7:e269411568c9 179 foundSensor = true;
masahikofukasawa 7:e269411568c9 180 }
masahikofukasawa 7:e269411568c9 181 if(foundSensor != true){
masahikofukasawa 11:cef8dc1cf010 182 MSG("#failed checkConnetion(SPI). %s\r\n",AkmAkd::sensorName);
masahikofukasawa 7:e269411568c9 183 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 184 }
masahikofukasawa 7:e269411568c9 185 // read a data to reset DRDY
masahikofukasawa 7:e269411568c9 186 AkmECompass::MagneticVector mag;
masahikofukasawa 7:e269411568c9 187 compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 188 }
masahikofukasawa 0:7a00359e701e 189
masahikofukasawa 11:cef8dc1cf010 190 MSG("#%s detected.\r\n", AkmAkd::sensorName);
masahikofukasawa 0:7a00359e701e 191 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 192 }
masahikofukasawa 0:7a00359e701e 193
masahikofukasawa 0:7a00359e701e 194
masahikofukasawa 0:7a00359e701e 195 void AkmAkd::checkDRDY(){
masahikofukasawa 0:7a00359e701e 196 AkmECompass::Status status = compass->isDataReady();
masahikofukasawa 0:7a00359e701e 197 if( status == AkmECompass::DATA_READY ||
masahikofukasawa 0:7a00359e701e 198 status == AkmECompass::DATA_OVER_RUN ) event = true;
masahikofukasawa 0:7a00359e701e 199 }
masahikofukasawa 0:7a00359e701e 200
masahikofukasawa 0:7a00359e701e 201 void AkmAkd::detectDRDY(){
masahikofukasawa 0:7a00359e701e 202 event = true;
masahikofukasawa 0:7a00359e701e 203 }
masahikofukasawa 0:7a00359e701e 204
masahikofukasawa 0:7a00359e701e 205 bool AkmAkd::isEvent(){
masahikofukasawa 0:7a00359e701e 206 return event;
masahikofukasawa 0:7a00359e701e 207 }
masahikofukasawa 0:7a00359e701e 208
masahikofukasawa 0:7a00359e701e 209 AkmSensor::Status AkmAkd::startSensor(){
masahikofukasawa 11:cef8dc1cf010 210 // read one data to clear DRDY
masahikofukasawa 9:6fa3e7b17c27 211 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 212 compass->getMagneticVector(&mag);
masahikofukasawa 9:6fa3e7b17c27 213
masahikofukasawa 11:cef8dc1cf010 214 // enable interrupt
masahikofukasawa 9:6fa3e7b17c27 215 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 216 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 10:5c69b067d88a 217 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 10:5c69b067d88a 218 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 10:5c69b067d88a 219 subId == AkmAkd::SUB_ID_AK09915C ) ){
masahikofukasawa 10:5c69b067d88a 220 // Push-Pull DRDY
masahikofukasawa 0:7a00359e701e 221 drdy->rise(this, &AkmAkd::detectDRDY);
masahikofukasawa 0:7a00359e701e 222 }
masahikofukasawa 10:5c69b067d88a 223 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 224 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 10:5c69b067d88a 225 subId == AkmAkd::SUB_ID_AK09916D ) ){
masahikofukasawa 10:5c69b067d88a 226 // Open Drain DRDY
masahikofukasawa 10:5c69b067d88a 227 drdy->fall(this, &AkmAkd::detectDRDY);
masahikofukasawa 10:5c69b067d88a 228 }
masahikofukasawa 0:7a00359e701e 229 else{
masahikofukasawa 10:5c69b067d88a 230 // No DRDY
masahikofukasawa 9:6fa3e7b17c27 231 ticker.attach(this, &AkmAkd::checkDRDY, 0.005); // 200Hz
masahikofukasawa 0:7a00359e701e 232 }
masahikofukasawa 11:cef8dc1cf010 233
masahikofukasawa 11:cef8dc1cf010 234 // set operation mode
masahikofukasawa 11:cef8dc1cf010 235 if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
masahikofukasawa 11:cef8dc1cf010 236 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 237 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 238 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 239 }
masahikofukasawa 11:cef8dc1cf010 240 }
masahikofukasawa 11:cef8dc1cf010 241 else if( subId == AkmAkd::SUB_ID_AK09912C ){
masahikofukasawa 11:cef8dc1cf010 242 if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 243 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 244 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 245 }
masahikofukasawa 11:cef8dc1cf010 246 }
masahikofukasawa 11:cef8dc1cf010 247 else{
masahikofukasawa 11:cef8dc1cf010 248 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 249 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 250 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 251 }
masahikofukasawa 11:cef8dc1cf010 252 }
masahikofukasawa 11:cef8dc1cf010 253
masahikofukasawa 11:cef8dc1cf010 254 MSG("#Start sensor succceeded.\r\n");
masahikofukasawa 0:7a00359e701e 255 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 256 }
masahikofukasawa 0:7a00359e701e 257
masahikofukasawa 0:7a00359e701e 258 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 259 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 260 }
masahikofukasawa 0:7a00359e701e 261
masahikofukasawa 0:7a00359e701e 262 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 263 ticker.detach();
masahikofukasawa 11:cef8dc1cf010 264 event = false;
masahikofukasawa 11:cef8dc1cf010 265
masahikofukasawa 11:cef8dc1cf010 266 // disable DRDY interrupt
masahikofukasawa 11:cef8dc1cf010 267 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 11:cef8dc1cf010 268 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 11:cef8dc1cf010 269 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 11:cef8dc1cf010 270 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 11:cef8dc1cf010 271 subId == AkmAkd::SUB_ID_AK09915C ) ){
masahikofukasawa 11:cef8dc1cf010 272 drdy->rise(NULL);
masahikofukasawa 11:cef8dc1cf010 273 }
masahikofukasawa 11:cef8dc1cf010 274 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 11:cef8dc1cf010 275 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 11:cef8dc1cf010 276 subId == AkmAkd::SUB_ID_AK09916D ) ){
masahikofukasawa 11:cef8dc1cf010 277 // Open Drain DRDY
masahikofukasawa 11:cef8dc1cf010 278 drdy->fall(NULL);
masahikofukasawa 11:cef8dc1cf010 279 }
masahikofukasawa 11:cef8dc1cf010 280
masahikofukasawa 0:7a00359e701e 281 // Puts the device into power down mode.
masahikofukasawa 0:7a00359e701e 282 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 283 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 284 }
masahikofukasawa 11:cef8dc1cf010 285 // read one data to clear DRDY
masahikofukasawa 11:cef8dc1cf010 286 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 287 compass->getMagneticVector(&mag);
masahikofukasawa 11:cef8dc1cf010 288
masahikofukasawa 0:7a00359e701e 289 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 290 }
masahikofukasawa 0:7a00359e701e 291
masahikofukasawa 0:7a00359e701e 292 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 0:7a00359e701e 293 event = false;
masahikofukasawa 0:7a00359e701e 294
masahikofukasawa 0:7a00359e701e 295 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 296 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 297 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 298 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 299 }
masahikofukasawa 0:7a00359e701e 300 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 0:7a00359e701e 301 msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
masahikofukasawa 0:7a00359e701e 302 msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
masahikofukasawa 0:7a00359e701e 303 msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 304 msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 305 msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 306 msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 307 msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 308 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 309 }
masahikofukasawa 0:7a00359e701e 310
masahikofukasawa 0:7a00359e701e 311 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 312 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 313 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 314 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 315
masahikofukasawa 9:6fa3e7b17c27 316 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 317 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 318 {
masahikofukasawa 9:6fa3e7b17c27 319 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 320 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 321 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 322 }
masahikofukasawa 9:6fa3e7b17c27 323 out->setArgument(0,mode);
masahikofukasawa 9:6fa3e7b17c27 324 break;
masahikofukasawa 9:6fa3e7b17c27 325 }
masahikofukasawa 9:6fa3e7b17c27 326 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 327 {
masahikofukasawa 9:6fa3e7b17c27 328 mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 329 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 9:6fa3e7b17c27 330 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 331 sdr = (AkmECompass::Sdr)in->getArgument(2);
masahikofukasawa 9:6fa3e7b17c27 332 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 333 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 334 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 335 }
masahikofukasawa 9:6fa3e7b17c27 336 }
masahikofukasawa 10:5c69b067d88a 337 else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 9:6fa3e7b17c27 338 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 339 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 340 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 341 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 342 }
masahikofukasawa 9:6fa3e7b17c27 343 }
masahikofukasawa 9:6fa3e7b17c27 344 else{
masahikofukasawa 9:6fa3e7b17c27 345 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 346 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 347 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 348 }
masahikofukasawa 9:6fa3e7b17c27 349 }
masahikofukasawa 9:6fa3e7b17c27 350 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 351 break;
masahikofukasawa 9:6fa3e7b17c27 352 }
masahikofukasawa 9:6fa3e7b17c27 353 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 354 {
masahikofukasawa 9:6fa3e7b17c27 355 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 356 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 357 char data[len];
masahikofukasawa 9:6fa3e7b17c27 358 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 359 data[i] = in->getArgument(i);
masahikofukasawa 9:6fa3e7b17c27 360 }
masahikofukasawa 9:6fa3e7b17c27 361 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 362 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 363 MSG("#Error register write.\r\n");
masahikofukasawa 9:6fa3e7b17c27 364 }
masahikofukasawa 9:6fa3e7b17c27 365 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 366 break;
masahikofukasawa 9:6fa3e7b17c27 367 }
masahikofukasawa 9:6fa3e7b17c27 368 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 369 {
masahikofukasawa 9:6fa3e7b17c27 370 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 371 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 372 char data;
masahikofukasawa 9:6fa3e7b17c27 373 if( compass->read(address, &data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 374 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 375 MSG("#Error register read.\r\n");
masahikofukasawa 9:6fa3e7b17c27 376 }
masahikofukasawa 9:6fa3e7b17c27 377 out->setArgument(0,data);
masahikofukasawa 9:6fa3e7b17c27 378 break;
masahikofukasawa 9:6fa3e7b17c27 379 }
masahikofukasawa 9:6fa3e7b17c27 380 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 381 {
masahikofukasawa 9:6fa3e7b17c27 382 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 383 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 384 char data[len];
masahikofukasawa 9:6fa3e7b17c27 385 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 386 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 387 MSG("#Error register read.\r\n");
masahikofukasawa 9:6fa3e7b17c27 388 }
masahikofukasawa 9:6fa3e7b17c27 389 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 390 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 391 }
masahikofukasawa 9:6fa3e7b17c27 392 break;
masahikofukasawa 9:6fa3e7b17c27 393 }
masahikofukasawa 9:6fa3e7b17c27 394 default:
masahikofukasawa 9:6fa3e7b17c27 395 {
masahikofukasawa 11:cef8dc1cf010 396 MSG("#Error no command.\r\n");
masahikofukasawa 9:6fa3e7b17c27 397 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 398 break;
masahikofukasawa 9:6fa3e7b17c27 399 }
masahikofukasawa 9:6fa3e7b17c27 400 }
masahikofukasawa 9:6fa3e7b17c27 401
masahikofukasawa 0:7a00359e701e 402 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 403 }
masahikofukasawa 0:7a00359e701e 404
masahikofukasawa 0:7a00359e701e 405 char* AkmAkd::getSensorName(){
masahikofukasawa 0:7a00359e701e 406 return sensorName;
masahikofukasawa 0:7a00359e701e 407 }