Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@9:6fa3e7b17c27, 2016-06-16 (annotated)

Committer:
masahikofukasawa
Date:
Thu Jun 16 18:36:26 2016 +0000
Revision:
9:6fa3e7b17c27
Parent:
7:e269411568c9
Child:
10:5c69b067d88a
AKDP 002; Modified For E-compass Operation Mode Setting

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 0:7a00359e701e 55 case AkmAkd::SUB_ID_AK09911:
masahikofukasawa 0:7a00359e701e 56 type = AkmECompass::AK09911;
masahikofukasawa 0:7a00359e701e 57 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 58 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 59 AkmAkd::sensorName = "AK09911";
masahikofukasawa 0:7a00359e701e 60 break;
masahikofukasawa 0:7a00359e701e 61 case AkmAkd::SUB_ID_AK09912:
masahikofukasawa 0:7a00359e701e 62 type = AkmECompass::AK09912;
masahikofukasawa 0:7a00359e701e 63 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 64 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 65 AkmAkd::sensorName = "AK09912";
masahikofukasawa 0:7a00359e701e 66 break;
masahikofukasawa 0:7a00359e701e 67 case AkmAkd::SUB_ID_AK09915:
masahikofukasawa 0:7a00359e701e 68 type = AkmECompass::AK09915;
masahikofukasawa 0:7a00359e701e 69 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 70 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 71 AkmAkd::sensorName = "AK09915";
masahikofukasawa 0:7a00359e701e 72 break;
masahikofukasawa 0:7a00359e701e 73 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 0:7a00359e701e 74 type = AkmECompass::AK09916C;
masahikofukasawa 0:7a00359e701e 75 ak099xx = new AK099XX();
masahikofukasawa 0:7a00359e701e 76 compass = ak099xx;
masahikofukasawa 0:7a00359e701e 77 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 0:7a00359e701e 78 break;
masahikofukasawa 0:7a00359e701e 79 default:
masahikofukasawa 0:7a00359e701e 80 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 81 // break;
masahikofukasawa 0:7a00359e701e 82 }
masahikofukasawa 0:7a00359e701e 83
masahikofukasawa 0:7a00359e701e 84 bool foundSensor = false;
masahikofukasawa 0:7a00359e701e 85
masahikofukasawa 0:7a00359e701e 86 AkmECompass::SlaveAddress slaveAddr[]
masahikofukasawa 0:7a00359e701e 87 = { AkmECompass::SLAVE_ADDR_1,
masahikofukasawa 0:7a00359e701e 88 AkmECompass::SLAVE_ADDR_2,
masahikofukasawa 0:7a00359e701e 89 AkmECompass::SLAVE_ADDR_3,
masahikofukasawa 0:7a00359e701e 90 AkmECompass::SLAVE_ADDR_4};
masahikofukasawa 0:7a00359e701e 91
masahikofukasawa 0:7a00359e701e 92 for(int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 0:7a00359e701e 93 {
masahikofukasawa 0:7a00359e701e 94 compass->init(i2c, slaveAddr[i], type);
masahikofukasawa 0:7a00359e701e 95 // Checks connectivity
masahikofukasawa 0:7a00359e701e 96 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 97 // found
masahikofukasawa 0:7a00359e701e 98 foundSensor = true;
masahikofukasawa 0:7a00359e701e 99 break;
masahikofukasawa 0:7a00359e701e 100 }
masahikofukasawa 0:7a00359e701e 101 }
masahikofukasawa 7:e269411568c9 102 if(foundSensor != true){
masahikofukasawa 7:e269411568c9 103 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 104 }
masahikofukasawa 0:7a00359e701e 105
masahikofukasawa 0:7a00359e701e 106 // read a data to reset DRDY
masahikofukasawa 0:7a00359e701e 107 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 108 compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 109
masahikofukasawa 0:7a00359e701e 110 }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
masahikofukasawa 7:e269411568c9 111
masahikofukasawa 7:e269411568c9 112 drdy = new InterruptIn(SPI_DRDY);
masahikofukasawa 7:e269411568c9 113 drdy->rise(0);
masahikofukasawa 0:7a00359e701e 114
masahikofukasawa 7:e269411568c9 115 SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 7:e269411568c9 116 spi->format(8,3); // 8bit, Mode=3
masahikofukasawa 7:e269411568c9 117 spi->frequency(1000000);
masahikofukasawa 7:e269411568c9 118
masahikofukasawa 7:e269411568c9 119 DigitalOut* cs = new DigitalOut(SPI_CS);
masahikofukasawa 7:e269411568c9 120
masahikofukasawa 7:e269411568c9 121 switch(subid){
masahikofukasawa 7:e269411568c9 122 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 7:e269411568c9 123 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 7:e269411568c9 124 type = AkmECompass::AK8963;
masahikofukasawa 7:e269411568c9 125 ak8963 = new AK8963();
masahikofukasawa 7:e269411568c9 126 compass = ak8963;
masahikofukasawa 7:e269411568c9 127 AkmAkd::sensorName = "AK8963";
masahikofukasawa 7:e269411568c9 128 break;
masahikofukasawa 7:e269411568c9 129 case AkmAkd::SUB_ID_AK09911:
masahikofukasawa 7:e269411568c9 130 type = AkmECompass::AK09911;
masahikofukasawa 7:e269411568c9 131 AkmAkd::sensorName = "AK09911";
masahikofukasawa 7:e269411568c9 132 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 133 case AkmAkd::SUB_ID_AK09912:
masahikofukasawa 7:e269411568c9 134 type = AkmECompass::AK09912;
masahikofukasawa 7:e269411568c9 135 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 136 compass = ak099xx;
masahikofukasawa 7:e269411568c9 137 AkmAkd::sensorName = "AK09912";
masahikofukasawa 7:e269411568c9 138 break;
masahikofukasawa 7:e269411568c9 139 case AkmAkd::SUB_ID_AK09915:
masahikofukasawa 7:e269411568c9 140 type = AkmECompass::AK09915;
masahikofukasawa 7:e269411568c9 141 ak099xx = new AK099XX();
masahikofukasawa 7:e269411568c9 142 compass = ak099xx;
masahikofukasawa 7:e269411568c9 143 AkmAkd::sensorName = "AK09915";
masahikofukasawa 7:e269411568c9 144 break;
masahikofukasawa 7:e269411568c9 145 case AkmAkd::SUB_ID_AK09916C:
masahikofukasawa 7:e269411568c9 146 type = AkmECompass::AK09916C;
masahikofukasawa 7:e269411568c9 147 AkmAkd::sensorName = "AK09916C";
masahikofukasawa 7:e269411568c9 148 // doesn't support SPI
masahikofukasawa 7:e269411568c9 149 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 150 default:
masahikofukasawa 7:e269411568c9 151 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 152 // break;
masahikofukasawa 7:e269411568c9 153 }
masahikofukasawa 7:e269411568c9 154
masahikofukasawa 7:e269411568c9 155 bool foundSensor = false;
masahikofukasawa 7:e269411568c9 156 compass->init(spi, cs, type);
masahikofukasawa 7:e269411568c9 157 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 7:e269411568c9 158 foundSensor = true;
masahikofukasawa 7:e269411568c9 159 }
masahikofukasawa 7:e269411568c9 160 if(foundSensor != true){
masahikofukasawa 7:e269411568c9 161 MSG("#failed checkConnetion(SPI). %s\n",AkmAkd::sensorName);
masahikofukasawa 7:e269411568c9 162 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 163 }
masahikofukasawa 7:e269411568c9 164 // read a data to reset DRDY
masahikofukasawa 7:e269411568c9 165 AkmECompass::MagneticVector mag;
masahikofukasawa 7:e269411568c9 166 compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 167 }
masahikofukasawa 0:7a00359e701e 168
masahikofukasawa 0:7a00359e701e 169 MSG("#%s detected.\n", AkmAkd::sensorName);
masahikofukasawa 0:7a00359e701e 170 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 171 }
masahikofukasawa 0:7a00359e701e 172
masahikofukasawa 0:7a00359e701e 173
masahikofukasawa 0:7a00359e701e 174 void AkmAkd::checkDRDY(){
masahikofukasawa 0:7a00359e701e 175 AkmECompass::Status status = compass->isDataReady();
masahikofukasawa 0:7a00359e701e 176 if( status == AkmECompass::DATA_READY ||
masahikofukasawa 0:7a00359e701e 177 status == AkmECompass::DATA_OVER_RUN ) event = true;
masahikofukasawa 0:7a00359e701e 178 }
masahikofukasawa 0:7a00359e701e 179
masahikofukasawa 0:7a00359e701e 180 void AkmAkd::detectDRDY(){
masahikofukasawa 0:7a00359e701e 181 event = true;
masahikofukasawa 0:7a00359e701e 182 }
masahikofukasawa 0:7a00359e701e 183
masahikofukasawa 0:7a00359e701e 184 bool AkmAkd::isEvent(){
masahikofukasawa 0:7a00359e701e 185 return event;
masahikofukasawa 0:7a00359e701e 186 }
masahikofukasawa 0:7a00359e701e 187
masahikofukasawa 0:7a00359e701e 188 AkmSensor::Status AkmAkd::startSensor(){
masahikofukasawa 9:6fa3e7b17c27 189 // read once to clear DRDY pin
masahikofukasawa 9:6fa3e7b17c27 190 AkmECompass::MagneticVector mag;
masahikofukasawa 9:6fa3e7b17c27 191 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 9:6fa3e7b17c27 192
masahikofukasawa 0:7a00359e701e 193 // Puts the device into continuous measurement mode.
masahikofukasawa 9:6fa3e7b17c27 194 if(compass->getSensorType() == AkmECompass::AK09915){
masahikofukasawa 9:6fa3e7b17c27 195 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 196 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 197 }
masahikofukasawa 9:6fa3e7b17c27 198 }
masahikofukasawa 9:6fa3e7b17c27 199 else if(compass->getSensorType() == AkmECompass::AK09912){
masahikofukasawa 9:6fa3e7b17c27 200 if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 201 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 202 }
masahikofukasawa 9:6fa3e7b17c27 203 }
masahikofukasawa 9:6fa3e7b17c27 204 else{
masahikofukasawa 9:6fa3e7b17c27 205 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 206 return AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 207 }
masahikofukasawa 0:7a00359e701e 208 }
masahikofukasawa 0:7a00359e701e 209
masahikofukasawa 7:e269411568c9 210 // DRDY interrupt enable for AK8963/AK09912/AK09915 with I2C interface
masahikofukasawa 9:6fa3e7b17c27 211 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 7:e269411568c9 212 (compass->getSensorType() == AkmECompass::AK8963 ||
masahikofukasawa 7:e269411568c9 213 compass->getSensorType() == AkmECompass::AK09912 ||
masahikofukasawa 7:e269411568c9 214 compass->getSensorType() == AkmECompass::AK09915) ){
masahikofukasawa 0:7a00359e701e 215 drdy->rise(this, &AkmAkd::detectDRDY);
masahikofukasawa 0:7a00359e701e 216 }
masahikofukasawa 0:7a00359e701e 217 else{
masahikofukasawa 9:6fa3e7b17c27 218 ticker.attach(this, &AkmAkd::checkDRDY, 0.005); // 200Hz
masahikofukasawa 0:7a00359e701e 219 }
masahikofukasawa 0:7a00359e701e 220 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 221 }
masahikofukasawa 0:7a00359e701e 222
masahikofukasawa 0:7a00359e701e 223 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 0:7a00359e701e 224 return AkmSensor::ERROR; // doesn't support
masahikofukasawa 0:7a00359e701e 225 }
masahikofukasawa 0:7a00359e701e 226
masahikofukasawa 0:7a00359e701e 227 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 228 ticker.detach();
masahikofukasawa 0:7a00359e701e 229
masahikofukasawa 0:7a00359e701e 230 // Puts the device into power down mode.
masahikofukasawa 0:7a00359e701e 231 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
masahikofukasawa 0:7a00359e701e 232 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 233 }
masahikofukasawa 0:7a00359e701e 234 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 235 }
masahikofukasawa 0:7a00359e701e 236
masahikofukasawa 0:7a00359e701e 237 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 0:7a00359e701e 238 event = false;
masahikofukasawa 0:7a00359e701e 239
masahikofukasawa 0:7a00359e701e 240 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 241 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 242 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 243 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 244 }
masahikofukasawa 0:7a00359e701e 245 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 0:7a00359e701e 246 msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
masahikofukasawa 0:7a00359e701e 247 msg->setArgument( 1,(char)((int32_t)(mag.mx/0.15) >> 8));
masahikofukasawa 0:7a00359e701e 248 msg->setArgument( 2, (char)((int32_t)(mag.mx/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 249 msg->setArgument( 3, (char)((int32_t)(mag.my/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 250 msg->setArgument( 4, (char)((int32_t)(mag.my/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 251 msg->setArgument( 5, (char)((int32_t)(mag.mz/0.15) >> 8) );
masahikofukasawa 0:7a00359e701e 252 msg->setArgument( 6, (char)((int32_t)(mag.mz/0.15) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 253 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 254 }
masahikofukasawa 0:7a00359e701e 255
masahikofukasawa 0:7a00359e701e 256 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 257 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 258 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 259 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 260
masahikofukasawa 9:6fa3e7b17c27 261 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 262 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 263 {
masahikofukasawa 9:6fa3e7b17c27 264 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 265 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 266 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 267 }
masahikofukasawa 9:6fa3e7b17c27 268 out->setArgument(0,mode);
masahikofukasawa 9:6fa3e7b17c27 269 break;
masahikofukasawa 9:6fa3e7b17c27 270 }
masahikofukasawa 9:6fa3e7b17c27 271 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 272 {
masahikofukasawa 9:6fa3e7b17c27 273 mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 274 if(compass->getSensorType() == AkmECompass::AK09915){
masahikofukasawa 9:6fa3e7b17c27 275 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 276 sdr = (AkmECompass::Sdr)in->getArgument(2);
masahikofukasawa 9:6fa3e7b17c27 277 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 278 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 279 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 280 }
masahikofukasawa 9:6fa3e7b17c27 281 }
masahikofukasawa 9:6fa3e7b17c27 282 else if(compass->getSensorType() == AkmECompass::AK09912){
masahikofukasawa 9:6fa3e7b17c27 283 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 284 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 285 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 286 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 287 }
masahikofukasawa 9:6fa3e7b17c27 288 }
masahikofukasawa 9:6fa3e7b17c27 289 else{
masahikofukasawa 9:6fa3e7b17c27 290 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 291 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 292 MSG("#Error set operation mode.\n");
masahikofukasawa 9:6fa3e7b17c27 293 }
masahikofukasawa 9:6fa3e7b17c27 294 }
masahikofukasawa 9:6fa3e7b17c27 295 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 296 break;
masahikofukasawa 9:6fa3e7b17c27 297 }
masahikofukasawa 9:6fa3e7b17c27 298 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 299 {
masahikofukasawa 9:6fa3e7b17c27 300 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 301 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 302 char data[len];
masahikofukasawa 9:6fa3e7b17c27 303 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 304 data[i] = in->getArgument(i);
masahikofukasawa 9:6fa3e7b17c27 305 }
masahikofukasawa 9:6fa3e7b17c27 306 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 307 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 308 MSG("#Error register write.\n");
masahikofukasawa 9:6fa3e7b17c27 309 }
masahikofukasawa 9:6fa3e7b17c27 310 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 311 break;
masahikofukasawa 9:6fa3e7b17c27 312 }
masahikofukasawa 9:6fa3e7b17c27 313 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 314 {
masahikofukasawa 9:6fa3e7b17c27 315 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 316 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 317 char data;
masahikofukasawa 9:6fa3e7b17c27 318 if( compass->read(address, &data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 319 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 320 MSG("#Error register read.\n");
masahikofukasawa 9:6fa3e7b17c27 321 }
masahikofukasawa 9:6fa3e7b17c27 322 out->setArgument(0,data);
masahikofukasawa 9:6fa3e7b17c27 323 break;
masahikofukasawa 9:6fa3e7b17c27 324 }
masahikofukasawa 9:6fa3e7b17c27 325 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 326 {
masahikofukasawa 9:6fa3e7b17c27 327 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 328 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 329 char data[len];
masahikofukasawa 9:6fa3e7b17c27 330 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 331 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 332 MSG("#Error register read.\n");
masahikofukasawa 9:6fa3e7b17c27 333 }
masahikofukasawa 9:6fa3e7b17c27 334 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 335 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 336 }
masahikofukasawa 9:6fa3e7b17c27 337 break;
masahikofukasawa 9:6fa3e7b17c27 338 }
masahikofukasawa 9:6fa3e7b17c27 339 default:
masahikofukasawa 9:6fa3e7b17c27 340 {
masahikofukasawa 9:6fa3e7b17c27 341 MSG("#Error no command.\n");
masahikofukasawa 9:6fa3e7b17c27 342 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 343 break;
masahikofukasawa 9:6fa3e7b17c27 344 }
masahikofukasawa 9:6fa3e7b17c27 345 }
masahikofukasawa 9:6fa3e7b17c27 346
masahikofukasawa 0:7a00359e701e 347 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 348 }
masahikofukasawa 0:7a00359e701e 349
masahikofukasawa 0:7a00359e701e 350 char* AkmAkd::getSensorName(){
masahikofukasawa 0:7a00359e701e 351 return sensorName;
masahikofukasawa 0:7a00359e701e 352 }