Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@38:e865dadfe54d, 2017-06-08 (annotated)

Committer:
masahikofukasawa
Date:
Thu Jun 08 19:09:18 2017 +0000
Revision:
38:e865dadfe54d
Parent:
29:b488d2c89fba
Child:
39:3821886c902e
RevD7_015 release version

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