Taylor Street / AkmSensor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }