Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@40:42e48427e4b7, 2017-07-19 (annotated)

Committer:
masahikofukasawa
Date:
Wed Jul 19 23:30:22 2017 +0000
Revision:
40:42e48427e4b7
Parent:
39:3821886c902e
Child:
41:a3ea80c594ec
Changed I2C speed:100kHz --> 400kHz, Serial baud-rate: 115200bps --> 460800bps for faster sampling.

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