Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@10:5c69b067d88a, 2016-07-08 (annotated)

Committer:
masahikofukasawa
Date:
Fri Jul 08 22:26:26 2016 +0000
Revision:
10:5c69b067d88a
Parent:
9:6fa3e7b17c27
Child:
11:cef8dc1cf010
RevD with AK09970 Release to Japan.

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 7:e269411568c9 182 MSG("#failed checkConnetion(SPI). %s\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 0:7a00359e701e 190 MSG("#%s detected.\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 10:5c69b067d88a 210
masahikofukasawa 9:6fa3e7b17c27 211 // read once to clear DRDY pin
masahikofukasawa 9:6fa3e7b17c27 212 AkmECompass::MagneticVector mag;
masahikofukasawa 9:6fa3e7b17c27 213 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 9:6fa3e7b17c27 214
masahikofukasawa 10:5c69b067d88a 215 if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
masahikofukasawa 9:6fa3e7b17c27 216 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 217 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 218 }
masahikofukasawa 9:6fa3e7b17c27 219 }
masahikofukasawa 10:5c69b067d88a 220 else if( subId == AkmAkd::SUB_ID_AK09912C ){
masahikofukasawa 9:6fa3e7b17c27 221 if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 222 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 223 }
masahikofukasawa 9:6fa3e7b17c27 224 }
masahikofukasawa 9:6fa3e7b17c27 225 else{
masahikofukasawa 9:6fa3e7b17c27 226 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 227 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 228 }
masahikofukasawa 0:7a00359e701e 229 }
masahikofukasawa 0:7a00359e701e 230
masahikofukasawa 10:5c69b067d88a 231 // DRDY interrupt enable
masahikofukasawa 9:6fa3e7b17c27 232 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 233 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 10:5c69b067d88a 234 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 10:5c69b067d88a 235 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 10:5c69b067d88a 236 subId == AkmAkd::SUB_ID_AK09915C ) ){
masahikofukasawa 10:5c69b067d88a 237 // Push-Pull DRDY
masahikofukasawa 0:7a00359e701e 238 drdy->rise(this, &AkmAkd::detectDRDY);
masahikofukasawa 0:7a00359e701e 239 }
masahikofukasawa 10:5c69b067d88a 240 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 10:5c69b067d88a 241 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 10:5c69b067d88a 242 subId == AkmAkd::SUB_ID_AK09916D ) ){
masahikofukasawa 10:5c69b067d88a 243 // Open Drain DRDY
masahikofukasawa 10:5c69b067d88a 244 drdy->fall(this, &AkmAkd::detectDRDY);
masahikofukasawa 10:5c69b067d88a 245 }
masahikofukasawa 0:7a00359e701e 246 else{
masahikofukasawa 10:5c69b067d88a 247 // No DRDY
masahikofukasawa 9:6fa3e7b17c27 248 ticker.attach(this, &AkmAkd::checkDRDY, 0.005); // 200Hz
masahikofukasawa 0:7a00359e701e 249 }
masahikofukasawa 0:7a00359e701e 250 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 251 }
masahikofukasawa 0:7a00359e701e 252
masahikofukasawa 0:7a00359e701e 253 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 0:7a00359e701e 254 return AkmSensor::ERROR; // doesn't support
masahikofukasawa 0:7a00359e701e 255 }
masahikofukasawa 0:7a00359e701e 256
masahikofukasawa 0:7a00359e701e 257 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 258 ticker.detach();
masahikofukasawa 0:7a00359e701e 259
masahikofukasawa 0:7a00359e701e 260 // Puts the device into power down mode.
masahikofukasawa 0:7a00359e701e 261 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 262 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 263 }
masahikofukasawa 0:7a00359e701e 264 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 265 }
masahikofukasawa 0:7a00359e701e 266
masahikofukasawa 0:7a00359e701e 267 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 0:7a00359e701e 268 event = false;
masahikofukasawa 0:7a00359e701e 269
masahikofukasawa 0:7a00359e701e 270 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 271 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 272 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 273 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 274 }
masahikofukasawa 0:7a00359e701e 275 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 0:7a00359e701e 276 msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
masahikofukasawa 0:7a00359e701e 277 msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
masahikofukasawa 0:7a00359e701e 278 msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 279 msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 280 msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 281 msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 282 msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 283 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 284 }
masahikofukasawa 0:7a00359e701e 285
masahikofukasawa 0:7a00359e701e 286 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 287 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 288 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 289 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 290
masahikofukasawa 9:6fa3e7b17c27 291 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 292 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 293 {
masahikofukasawa 9:6fa3e7b17c27 294 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 295 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 296 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 297 }
masahikofukasawa 9:6fa3e7b17c27 298 out->setArgument(0,mode);
masahikofukasawa 9:6fa3e7b17c27 299 break;
masahikofukasawa 9:6fa3e7b17c27 300 }
masahikofukasawa 9:6fa3e7b17c27 301 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 302 {
masahikofukasawa 9:6fa3e7b17c27 303 mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 304 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 9:6fa3e7b17c27 305 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 306 sdr = (AkmECompass::Sdr)in->getArgument(2);
masahikofukasawa 9:6fa3e7b17c27 307 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 308 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 309 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 310 }
masahikofukasawa 9:6fa3e7b17c27 311 }
masahikofukasawa 10:5c69b067d88a 312 else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 9:6fa3e7b17c27 313 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 314 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 315 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 316 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 317 }
masahikofukasawa 9:6fa3e7b17c27 318 }
masahikofukasawa 9:6fa3e7b17c27 319 else{
masahikofukasawa 9:6fa3e7b17c27 320 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 321 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 322 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 323 }
masahikofukasawa 9:6fa3e7b17c27 324 }
masahikofukasawa 9:6fa3e7b17c27 325 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 326 break;
masahikofukasawa 9:6fa3e7b17c27 327 }
masahikofukasawa 9:6fa3e7b17c27 328 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 329 {
masahikofukasawa 9:6fa3e7b17c27 330 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 331 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 332 char data[len];
masahikofukasawa 9:6fa3e7b17c27 333 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 334 data[i] = in->getArgument(i);
masahikofukasawa 9:6fa3e7b17c27 335 }
masahikofukasawa 9:6fa3e7b17c27 336 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 337 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 338 MSG("#Error register write.\n");
masahikofukasawa 9:6fa3e7b17c27 339 }
masahikofukasawa 9:6fa3e7b17c27 340 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 341 break;
masahikofukasawa 9:6fa3e7b17c27 342 }
masahikofukasawa 9:6fa3e7b17c27 343 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 344 {
masahikofukasawa 9:6fa3e7b17c27 345 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 346 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 347 char data;
masahikofukasawa 9:6fa3e7b17c27 348 if( compass->read(address, &data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 349 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 350 MSG("#Error register read.\n");
masahikofukasawa 9:6fa3e7b17c27 351 }
masahikofukasawa 9:6fa3e7b17c27 352 out->setArgument(0,data);
masahikofukasawa 9:6fa3e7b17c27 353 break;
masahikofukasawa 9:6fa3e7b17c27 354 }
masahikofukasawa 9:6fa3e7b17c27 355 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 356 {
masahikofukasawa 9:6fa3e7b17c27 357 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 358 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 359 char data[len];
masahikofukasawa 9:6fa3e7b17c27 360 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 361 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 362 MSG("#Error register read.\n");
masahikofukasawa 9:6fa3e7b17c27 363 }
masahikofukasawa 9:6fa3e7b17c27 364 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 365 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 366 }
masahikofukasawa 9:6fa3e7b17c27 367 break;
masahikofukasawa 9:6fa3e7b17c27 368 }
masahikofukasawa 9:6fa3e7b17c27 369 default:
masahikofukasawa 9:6fa3e7b17c27 370 {
masahikofukasawa 9:6fa3e7b17c27 371 MSG("#Error no command.\n");
masahikofukasawa 9:6fa3e7b17c27 372 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 373 break;
masahikofukasawa 9:6fa3e7b17c27 374 }
masahikofukasawa 9:6fa3e7b17c27 375 }
masahikofukasawa 9:6fa3e7b17c27 376
masahikofukasawa 0:7a00359e701e 377 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 378 }
masahikofukasawa 0:7a00359e701e 379
masahikofukasawa 0:7a00359e701e 380 char* AkmAkd::getSensorName(){
masahikofukasawa 0:7a00359e701e 381 return sensorName;
masahikofukasawa 0:7a00359e701e 382 }