Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@19:8dcc4f323bdc, 2017-02-22 (annotated)

Committer:
masahikofukasawa
Date:
Wed Feb 22 18:47:01 2017 +0000
Revision:
19:8dcc4f323bdc
Parent:
18:b7182d5ad8d5
Child:
20:2fca76521680
Modified compass. cont...

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