Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@20:2fca76521680, 2017-02-22 (annotated)

Committer:
masahikofukasawa
Date:
Wed Feb 22 21:56:05 2017 +0000
Revision:
20:2fca76521680
Parent:
19:8dcc4f323bdc
Child:
27:41aa9fb23a2f
Debugged for AK09915 when stop sensor.

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 20:2fca76521680 27 AkmSensor::Status AkmAkd::checkSensor( const uint8_t primaryid, const uint8_t subid, AkmECompass::DeviceId* devid){
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 20:2fca76521680 34 *devid = 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 20:2fca76521680 40 *devid = 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 20:2fca76521680 48 *devid = 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 20:2fca76521680 54 *devid = 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 20:2fca76521680 60 *devid = 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 20:2fca76521680 66 *devid = 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 20:2fca76521680 74 *devid = 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 20:2fca76521680 82 *devid = 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 20:2fca76521680 95 AkmSensor::Status AkmAkd::init(const uint8_t primaryid, const uint8_t subid){
masahikofukasawa 20:2fca76521680 96 primaryId = primaryid;
masahikofukasawa 0:7a00359e701e 97 subId = subid;
masahikofukasawa 0:7a00359e701e 98
masahikofukasawa 20:2fca76521680 99 AkmECompass::DeviceId devid;
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 20:2fca76521680 113 if( checkSensor(primaryId,subId,&devid) != 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 20:2fca76521680 125 compass->init(i2c, slaveAddr[i], devid);
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 20:2fca76521680 143 if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
masahikofukasawa 19:8dcc4f323bdc 144 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 145 }
masahikofukasawa 7:e269411568c9 146
masahikofukasawa 20:2fca76521680 147 compass->init(spi, cs, devid);
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 0:7a00359e701e 193 AkmSensor::Status AkmAkd::startSensor(){
masahikofukasawa 11:cef8dc1cf010 194 // read one data to clear DRDY
masahikofukasawa 9:6fa3e7b17c27 195 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 196 compass->getMagneticVector(&mag);
masahikofukasawa 9:6fa3e7b17c27 197
masahikofukasawa 19:8dcc4f323bdc 198 AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
masahikofukasawa 11:cef8dc1cf010 199 // enable interrupt
masahikofukasawa 19:8dcc4f323bdc 200 if( int_mode == AkmAkd::INTERRUPT_ENABLED_PP ){
masahikofukasawa 10:5c69b067d88a 201 // Push-Pull DRDY
masahikofukasawa 16:d85be9bafb80 202 drdy->rise(callback(this, &AkmAkd::detectDRDY));
masahikofukasawa 0:7a00359e701e 203 }
masahikofukasawa 19:8dcc4f323bdc 204 else if( int_mode == AkmAkd::INTERRUPT_ENABLED_OD ){
masahikofukasawa 10:5c69b067d88a 205 // Open Drain DRDY
masahikofukasawa 16:d85be9bafb80 206 drdy->fall(callback(this, &AkmAkd::detectDRDY));
masahikofukasawa 10:5c69b067d88a 207 }
masahikofukasawa 0:7a00359e701e 208 else{
masahikofukasawa 10:5c69b067d88a 209 // No DRDY
masahikofukasawa 20:2fca76521680 210 ticker.attach(callback(this, &AkmAkd::checkDRDY), 1.0/AKDP_POLLING_FREQUENCY);
masahikofukasawa 0:7a00359e701e 211 }
masahikofukasawa 11:cef8dc1cf010 212
masahikofukasawa 11:cef8dc1cf010 213 // set operation mode
masahikofukasawa 11:cef8dc1cf010 214 if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
masahikofukasawa 11:cef8dc1cf010 215 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 216 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 217 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 218 }
masahikofukasawa 11:cef8dc1cf010 219 }
masahikofukasawa 11:cef8dc1cf010 220 else if( subId == AkmAkd::SUB_ID_AK09912C ){
masahikofukasawa 11:cef8dc1cf010 221 if(compass->setOperationMode(mode,nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 222 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 223 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 224 }
masahikofukasawa 11:cef8dc1cf010 225 }
masahikofukasawa 11:cef8dc1cf010 226 else{
masahikofukasawa 11:cef8dc1cf010 227 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 228 MSG("#Start sensor failed.\r\n");
masahikofukasawa 11:cef8dc1cf010 229 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 230 }
masahikofukasawa 11:cef8dc1cf010 231 }
masahikofukasawa 11:cef8dc1cf010 232
masahikofukasawa 11:cef8dc1cf010 233 MSG("#Start sensor succceeded.\r\n");
masahikofukasawa 0:7a00359e701e 234 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 235 }
masahikofukasawa 0:7a00359e701e 236
masahikofukasawa 0:7a00359e701e 237 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 238 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 239 }
masahikofukasawa 0:7a00359e701e 240
masahikofukasawa 0:7a00359e701e 241 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 242 ticker.detach();
masahikofukasawa 11:cef8dc1cf010 243 event = false;
masahikofukasawa 11:cef8dc1cf010 244
masahikofukasawa 19:8dcc4f323bdc 245 AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
masahikofukasawa 19:8dcc4f323bdc 246 if( int_mode == AkmAkd::INTERRUPT_ENABLED_PP ){
masahikofukasawa 19:8dcc4f323bdc 247 // Push-Pull DRDY
masahikofukasawa 11:cef8dc1cf010 248 drdy->rise(NULL);
masahikofukasawa 11:cef8dc1cf010 249 }
masahikofukasawa 19:8dcc4f323bdc 250 else if( int_mode == AkmAkd::INTERRUPT_ENABLED_OD ){
masahikofukasawa 11:cef8dc1cf010 251 // Open Drain DRDY
masahikofukasawa 11:cef8dc1cf010 252 drdy->fall(NULL);
masahikofukasawa 11:cef8dc1cf010 253 }
masahikofukasawa 19:8dcc4f323bdc 254 else{
masahikofukasawa 19:8dcc4f323bdc 255 // No DRDY
masahikofukasawa 19:8dcc4f323bdc 256 }
masahikofukasawa 20:2fca76521680 257
masahikofukasawa 0:7a00359e701e 258 // Puts the device into power down mode.
masahikofukasawa 20:2fca76521680 259 if( subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D ){
masahikofukasawa 20:2fca76521680 260 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 20:2fca76521680 261 return AkmSensor::ERROR;
masahikofukasawa 20:2fca76521680 262 }
masahikofukasawa 20:2fca76521680 263 }else{
masahikofukasawa 20:2fca76521680 264 if(compass->setOperationMode(AkmECompass::MODE_POWER_DOWN) != AkmECompass::SUCCESS) {
masahikofukasawa 20:2fca76521680 265 return AkmSensor::ERROR;
masahikofukasawa 20:2fca76521680 266 }
masahikofukasawa 0:7a00359e701e 267 }
masahikofukasawa 20:2fca76521680 268
masahikofukasawa 11:cef8dc1cf010 269 // read one data to clear DRDY
masahikofukasawa 11:cef8dc1cf010 270 AkmECompass::MagneticVector mag;
masahikofukasawa 11:cef8dc1cf010 271 compass->getMagneticVector(&mag);
masahikofukasawa 11:cef8dc1cf010 272
masahikofukasawa 0:7a00359e701e 273 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 274 }
masahikofukasawa 0:7a00359e701e 275
masahikofukasawa 0:7a00359e701e 276 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 0:7a00359e701e 277 event = false;
masahikofukasawa 0:7a00359e701e 278
masahikofukasawa 0:7a00359e701e 279 AkmECompass::MagneticVector mag;
masahikofukasawa 0:7a00359e701e 280 AkmECompass::Status status = compass->getMagneticVector(&mag);
masahikofukasawa 0:7a00359e701e 281 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 282 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 283 }
masahikofukasawa 0:7a00359e701e 284 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 0:7a00359e701e 285 msg->setArgument( 0, (mag.isOverflow ? 1 : 0) );
masahikofukasawa 20:2fca76521680 286 msg->setArgument( 1,(char)((int32_t)(mag.mx/AKDP_MAG_SENSITIVITY) >> 8));
masahikofukasawa 20:2fca76521680 287 msg->setArgument( 2, (char)((int32_t)(mag.mx/AKDP_MAG_SENSITIVITY) & 0x00FF) );
masahikofukasawa 20:2fca76521680 288 msg->setArgument( 3, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) >> 8) );
masahikofukasawa 20:2fca76521680 289 msg->setArgument( 4, (char)((int32_t)(mag.my/AKDP_MAG_SENSITIVITY) & 0x00FF) );
masahikofukasawa 20:2fca76521680 290 msg->setArgument( 5, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) >> 8) );
masahikofukasawa 20:2fca76521680 291 msg->setArgument( 6, (char)((int32_t)(mag.mz/AKDP_MAG_SENSITIVITY) & 0x00FF) );
masahikofukasawa 0:7a00359e701e 292 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 293 }
masahikofukasawa 0:7a00359e701e 294
masahikofukasawa 0:7a00359e701e 295 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 296 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 297 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 298 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 299
masahikofukasawa 9:6fa3e7b17c27 300 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 301 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 302 {
masahikofukasawa 18:b7182d5ad8d5 303 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 18:b7182d5ad8d5 304 if(compass->getOperationMode(&mode, &nsf, &sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 305 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 306 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 307 }else{
masahikofukasawa 18:b7182d5ad8d5 308 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 309 out->setArgument(1,nsf);
masahikofukasawa 18:b7182d5ad8d5 310 out->setArgument(2,sdr);
masahikofukasawa 18:b7182d5ad8d5 311 }
masahikofukasawa 18:b7182d5ad8d5 312
masahikofukasawa 18:b7182d5ad8d5 313 }else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 18:b7182d5ad8d5 314 if(compass->getOperationMode(&mode, &nsf) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 315 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 316 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 317 }else{
masahikofukasawa 18:b7182d5ad8d5 318 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 319 out->setArgument(1,nsf);
masahikofukasawa 18:b7182d5ad8d5 320 }
masahikofukasawa 15:1238993fd75f 321 }else{
masahikofukasawa 18:b7182d5ad8d5 322 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 18:b7182d5ad8d5 323 status = AkmSensor::ERROR;
masahikofukasawa 18:b7182d5ad8d5 324 MSG("#Error set operation mode.\r\n");
masahikofukasawa 18:b7182d5ad8d5 325 }else{
masahikofukasawa 18:b7182d5ad8d5 326 out->setArgument(0,mode);
masahikofukasawa 18:b7182d5ad8d5 327 }
masahikofukasawa 9:6fa3e7b17c27 328 }
masahikofukasawa 9:6fa3e7b17c27 329 break;
masahikofukasawa 9:6fa3e7b17c27 330 }
masahikofukasawa 9:6fa3e7b17c27 331 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 332 {
masahikofukasawa 9:6fa3e7b17c27 333 mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 334 if(subId == AkmAkd::SUB_ID_AK09915C || subId == AkmAkd::SUB_ID_AK09915D){
masahikofukasawa 9:6fa3e7b17c27 335 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 336 sdr = (AkmECompass::Sdr)in->getArgument(2);
masahikofukasawa 9:6fa3e7b17c27 337 if(compass->setOperationMode(mode,nsf,sdr) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 338 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 339 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 340 }
masahikofukasawa 9:6fa3e7b17c27 341 }
masahikofukasawa 10:5c69b067d88a 342 else if(subId == AkmAkd::SUB_ID_AK09912C){
masahikofukasawa 9:6fa3e7b17c27 343 nsf = (AkmECompass::Nsf)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 344 if(compass->setOperationMode(mode,(AkmECompass::Nsf)nsf) != 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 else{
masahikofukasawa 9:6fa3e7b17c27 350 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 351 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 352 MSG("#Error set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 353 }
masahikofukasawa 9:6fa3e7b17c27 354 }
masahikofukasawa 9:6fa3e7b17c27 355 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 356 break;
masahikofukasawa 15:1238993fd75f 357 }
masahikofukasawa 15:1238993fd75f 358 case Message::CMD_REG_WRITE:
masahikofukasawa 9:6fa3e7b17c27 359 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 360 {
masahikofukasawa 9:6fa3e7b17c27 361 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 362 int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 363 if(in->getArgNum() != len+2){
masahikofukasawa 15:1238993fd75f 364 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 365 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 366 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 367 return status;
masahikofukasawa 15:1238993fd75f 368 }
masahikofukasawa 15:1238993fd75f 369
masahikofukasawa 9:6fa3e7b17c27 370 char data[len];
masahikofukasawa 9:6fa3e7b17c27 371 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 372 data[i] = in->getArgument(i+2);
masahikofukasawa 9:6fa3e7b17c27 373 }
masahikofukasawa 9:6fa3e7b17c27 374 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 375 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 376 MSG("#Error register write.\r\n");
masahikofukasawa 9:6fa3e7b17c27 377 }
masahikofukasawa 9:6fa3e7b17c27 378 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 379 break;
masahikofukasawa 15:1238993fd75f 380 }
masahikofukasawa 9:6fa3e7b17c27 381 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 382 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 383 {
masahikofukasawa 15:1238993fd75f 384 if(in->getArgNum() != 2){
masahikofukasawa 15:1238993fd75f 385 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 386 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 387 return status;
masahikofukasawa 15:1238993fd75f 388 }
masahikofukasawa 15:1238993fd75f 389
masahikofukasawa 9:6fa3e7b17c27 390 char address = in->getArgument(0);
masahikofukasawa 9:6fa3e7b17c27 391 int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 392 char data[len];
masahikofukasawa 9:6fa3e7b17c27 393 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 394 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 395 MSG("#Error register read.\r\n");
masahikofukasawa 9:6fa3e7b17c27 396 }
masahikofukasawa 9:6fa3e7b17c27 397 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 398 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 399 }
masahikofukasawa 9:6fa3e7b17c27 400 break;
masahikofukasawa 9:6fa3e7b17c27 401 }
masahikofukasawa 9:6fa3e7b17c27 402 default:
masahikofukasawa 9:6fa3e7b17c27 403 {
masahikofukasawa 11:cef8dc1cf010 404 MSG("#Error no command.\r\n");
masahikofukasawa 9:6fa3e7b17c27 405 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 406 break;
masahikofukasawa 9:6fa3e7b17c27 407 }
masahikofukasawa 9:6fa3e7b17c27 408 }
masahikofukasawa 15:1238993fd75f 409 return status;
masahikofukasawa 0:7a00359e701e 410 }
masahikofukasawa 0:7a00359e701e 411
masahikofukasawa 0:7a00359e701e 412 char* AkmAkd::getSensorName(){
masahikofukasawa 0:7a00359e701e 413 return sensorName;
masahikofukasawa 0:7a00359e701e 414 }