Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@49:c8f8946129b6, 2018-05-01 (annotated)

Committer:
tkstreet
Date:
Tue May 01 21:31:15 2018 +0000
Revision:
49:c8f8946129b6
Parent:
48:427bdb7bf31b 
Modified for Rev.E. compatibility.

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 42:b48b3ab8690e 4 #include "ak09940.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 42:b48b3ab8690e 24 AK09940* ak09940;
masahikofukasawa 19:8dcc4f323bdc 25 AK8963* ak8963;
masahikofukasawa 42:b48b3ab8690e 26 lenOptions = 0;
masahikofukasawa 42:b48b3ab8690e 27
masahikofukasawa 19:8dcc4f323bdc 28 switch(subid){
masahikofukasawa 19:8dcc4f323bdc 29 case AkmAkd::SUB_ID_AK8963N:
masahikofukasawa 19:8dcc4f323bdc 30 case AkmAkd::SUB_ID_AK8963C:
masahikofukasawa 20:2fca76521680 31 *devid = AkmECompass::AK8963;
masahikofukasawa 19:8dcc4f323bdc 32 AkmAkd::sensorName = "AK8963";
masahikofukasawa 19:8dcc4f323bdc 33 ak8963 = new AK8963();
masahikofukasawa 19:8dcc4f323bdc 34 compass = ak8963;
masahikofukasawa 19:8dcc4f323bdc 35 break;
masahikofukasawa 19:8dcc4f323bdc 36 case AkmAkd::SUB_ID_AK09911C:
masahikofukasawa 20:2fca76521680 37 *devid = AkmECompass::AK09911;
masahikofukasawa 19:8dcc4f323bdc 38 AkmAkd::sensorName = "AK09911C";
masahikofukasawa 19:8dcc4f323bdc 39 if(primaryId == AKM_PRIMARY_ID_AKD_SPI)
masahikofukasawa 19:8dcc4f323bdc 40 return AkmSensor::ERROR; // No SPI support
masahikofukasawa 19:8dcc4f323bdc 41 ak099xx = new AK099XX();
masahikofukasawa 19:8dcc4f323bdc 42 compass = ak099xx;
masahikofukasawa 19:8dcc4f323bdc 43 break;
masahikofukasawa 19:8dcc4f323bdc 44 case AkmAkd::SUB_ID_AK09912C:
masahikofukasawa 20:2fca76521680 45 *devid = AkmECompass::AK09912;
masahikofukasawa 19:8dcc4f323bdc 46 AkmAkd::sensorName = "AK09912C";
masahikofukasawa 19:8dcc4f323bdc 47 ak099xx = new AK099XX();
masahikofukasawa 19:8dcc4f323bdc 48 compass = ak099xx;
masahikofukasawa 41:a3ea80c594ec 49 lenOptions = 1;
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 41:a3ea80c594ec 56 lenOptions = 2;
masahikofukasawa 19:8dcc4f323bdc 57 break;
masahikofukasawa 19:8dcc4f323bdc 58 case AkmAkd::SUB_ID_AK09915D:
masahikofukasawa 20:2fca76521680 59 *devid = AkmECompass::AK09915;
masahikofukasawa 19:8dcc4f323bdc 60 AkmAkd::sensorName = "AK09915D";
masahikofukasawa 19:8dcc4f323bdc 61 ak099xx = new AK099XX();
masahikofukasawa 19:8dcc4f323bdc 62 compass = ak099xx;
masahikofukasawa 41:a3ea80c594ec 63 lenOptions = 2;
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 38:e865dadfe54d 89 case AkmAkd::SUB_ID_AK09917:
masahikofukasawa 38:e865dadfe54d 90 *devid = AkmECompass::AK09917;
masahikofukasawa 38:e865dadfe54d 91 AkmAkd::sensorName = "AK09917";
masahikofukasawa 38:e865dadfe54d 92 if(primaryId == AKM_PRIMARY_ID_AKD_SPI)
masahikofukasawa 38:e865dadfe54d 93 return AkmSensor::ERROR; // No SPI support
masahikofukasawa 38:e865dadfe54d 94 ak099xx = new AK099XX();
masahikofukasawa 38:e865dadfe54d 95 compass = ak099xx;
masahikofukasawa 41:a3ea80c594ec 96 lenOptions = 2;
masahikofukasawa 41:a3ea80c594ec 97 break;
masahikofukasawa 41:a3ea80c594ec 98 case AkmAkd::SUB_ID_AK09940:
masahikofukasawa 41:a3ea80c594ec 99 *devid = AkmECompass::AK09940;
masahikofukasawa 41:a3ea80c594ec 100 AkmAkd::sensorName = "AK09940";
masahikofukasawa 42:b48b3ab8690e 101 ak09940 = new AK09940();
masahikofukasawa 42:b48b3ab8690e 102 compass = ak09940;
masahikofukasawa 41:a3ea80c594ec 103 lenOptions = 5;
masahikofukasawa 38:e865dadfe54d 104 break;
masahikofukasawa 19:8dcc4f323bdc 105 default:
masahikofukasawa 19:8dcc4f323bdc 106 return AkmSensor::ERROR;
masahikofukasawa 19:8dcc4f323bdc 107 }
masahikofukasawa 41:a3ea80c594ec 108
masahikofukasawa 41:a3ea80c594ec 109 uint8_t options[lenOptions];
masahikofukasawa 41:a3ea80c594ec 110 mode.options = options;
masahikofukasawa 41:a3ea80c594ec 111
masahikofukasawa 19:8dcc4f323bdc 112 return AkmSensor::SUCCESS;
masahikofukasawa 19:8dcc4f323bdc 113 }
masahikofukasawa 19:8dcc4f323bdc 114
masahikofukasawa 20:2fca76521680 115 AkmSensor::Status AkmAkd::init(const uint8_t primaryid, const uint8_t subid){
masahikofukasawa 20:2fca76521680 116 primaryId = primaryid;
masahikofukasawa 0:7a00359e701e 117 subId = subid;
masahikofukasawa 42:b48b3ab8690e 118
masahikofukasawa 20:2fca76521680 119 AkmECompass::DeviceId devid;
masahikofukasawa 19:8dcc4f323bdc 120
masahikofukasawa 41:a3ea80c594ec 121 mode.mode = AkmECompass::MODE_POWER_DOWN;
masahikofukasawa 9:6fa3e7b17c27 122
masahikofukasawa 0:7a00359e701e 123 if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
masahikofukasawa 0:7a00359e701e 124
masahikofukasawa 0:7a00359e701e 125 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 40:42e48427e4b7 126 i2c->frequency(I2C_SPEED);
masahikofukasawa 0:7a00359e701e 127
masahikofukasawa 20:2fca76521680 128 if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
masahikofukasawa 19:8dcc4f323bdc 129 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 130 }
masahikofukasawa 0:7a00359e701e 131
masahikofukasawa 0:7a00359e701e 132 AkmECompass::SlaveAddress slaveAddr[]
masahikofukasawa 0:7a00359e701e 133 = { AkmECompass::SLAVE_ADDR_1,
masahikofukasawa 0:7a00359e701e 134 AkmECompass::SLAVE_ADDR_2,
masahikofukasawa 0:7a00359e701e 135 AkmECompass::SLAVE_ADDR_3,
masahikofukasawa 0:7a00359e701e 136 AkmECompass::SLAVE_ADDR_4};
masahikofukasawa 0:7a00359e701e 137
masahikofukasawa 47:221ec4b404ec 138 for(unsigned int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 0:7a00359e701e 139 {
masahikofukasawa 42:b48b3ab8690e 140 MSG("#look for e-compass.\r\n");
masahikofukasawa 20:2fca76521680 141 compass->init(i2c, slaveAddr[i], devid);
masahikofukasawa 0:7a00359e701e 142 // Checks connectivity
masahikofukasawa 42:b48b3ab8690e 143 MSG("#checkConnection.\r\n");
masahikofukasawa 0:7a00359e701e 144 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 19:8dcc4f323bdc 145 MSG("#%s detected.\r\n", AkmAkd::sensorName);
masahikofukasawa 19:8dcc4f323bdc 146 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 147 }
masahikofukasawa 0:7a00359e701e 148 }
masahikofukasawa 0:7a00359e701e 149 }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
masahikofukasawa 7:e269411568c9 150
masahikofukasawa 7:e269411568c9 151 SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 7:e269411568c9 152 spi->format(8,3); // 8bit, Mode=3
masahikofukasawa 41:a3ea80c594ec 153 spi->frequency(SPI_SPEED);
masahikofukasawa 7:e269411568c9 154
masahikofukasawa 7:e269411568c9 155 DigitalOut* cs = new DigitalOut(SPI_CS);
masahikofukasawa 19:8dcc4f323bdc 156
masahikofukasawa 20:2fca76521680 157 if( checkSensor(primaryId,subId,&devid) != AkmSensor::SUCCESS ){
masahikofukasawa 19:8dcc4f323bdc 158 return AkmSensor::ERROR;
masahikofukasawa 7:e269411568c9 159 }
masahikofukasawa 7:e269411568c9 160
masahikofukasawa 20:2fca76521680 161 compass->init(spi, cs, devid);
masahikofukasawa 7:e269411568c9 162 if(compass->checkConnection() == AkmECompass::SUCCESS) {
masahikofukasawa 19:8dcc4f323bdc 163 MSG("#%s detected.\r\n", AkmAkd::sensorName);
masahikofukasawa 19:8dcc4f323bdc 164 return AkmSensor::SUCCESS;
masahikofukasawa 7:e269411568c9 165 }
masahikofukasawa 0:7a00359e701e 166 }
masahikofukasawa 42:b48b3ab8690e 167 MSG("#ERROR: couldn't detected.\r\n");
masahikofukasawa 19:8dcc4f323bdc 168 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 169 }
masahikofukasawa 0:7a00359e701e 170
masahikofukasawa 29:b488d2c89fba 171 void AkmAkd::setEvent(){
masahikofukasawa 41:a3ea80c594ec 172 AkmECompass::Status status = compass->isDataReady();
masahikofukasawa 41:a3ea80c594ec 173 if( status == AkmECompass::DATA_READY ){
masahikofukasawa 29:b488d2c89fba 174 base::setEvent();
masahikofukasawa 29:b488d2c89fba 175 }
masahikofukasawa 0:7a00359e701e 176 }
masahikofukasawa 0:7a00359e701e 177
masahikofukasawa 19:8dcc4f323bdc 178 AkmAkd::InterruptMode AkmAkd::getInterrupt(uint8_t primaryId, uint8_t subId){
masahikofukasawa 19:8dcc4f323bdc 179 AkmAkd::InterruptMode ret = AkmAkd::INTERRUPT_DISABLED;
masahikofukasawa 19:8dcc4f323bdc 180
masahikofukasawa 19:8dcc4f323bdc 181 if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 19:8dcc4f323bdc 182 (subId == AkmAkd::SUB_ID_AK8963N ||
masahikofukasawa 19:8dcc4f323bdc 183 subId == AkmAkd::SUB_ID_AK8963C ||
masahikofukasawa 19:8dcc4f323bdc 184 subId == AkmAkd::SUB_ID_AK09912C ||
masahikofukasawa 41:a3ea80c594ec 185 subId == AkmAkd::SUB_ID_AK09915C ||
masahikofukasawa 41:a3ea80c594ec 186 subId == AkmAkd::SUB_ID_AK09940 ) ){
masahikofukasawa 19:8dcc4f323bdc 187 ret = AkmAkd::INTERRUPT_ENABLED_PP;
masahikofukasawa 19:8dcc4f323bdc 188 }
masahikofukasawa 19:8dcc4f323bdc 189 else if( primaryId == AKM_PRIMARY_ID_AKD_I2C &&
masahikofukasawa 19:8dcc4f323bdc 190 (subId == AkmAkd::SUB_ID_AK09915D ||
masahikofukasawa 38:e865dadfe54d 191 subId == AkmAkd::SUB_ID_AK09916D ||
masahikofukasawa 38:e865dadfe54d 192 subId == AkmAkd::SUB_ID_AK09917 ) ){
masahikofukasawa 19:8dcc4f323bdc 193 ret = AkmAkd::INTERRUPT_ENABLED_OD;
masahikofukasawa 19:8dcc4f323bdc 194 }
masahikofukasawa 19:8dcc4f323bdc 195 else{
masahikofukasawa 19:8dcc4f323bdc 196 // No DRDY
masahikofukasawa 19:8dcc4f323bdc 197 // No DRDY use for SPI interface
masahikofukasawa 19:8dcc4f323bdc 198 }
masahikofukasawa 19:8dcc4f323bdc 199 return ret;
masahikofukasawa 19:8dcc4f323bdc 200 }
masahikofukasawa 19:8dcc4f323bdc 201
masahikofukasawa 0:7a00359e701e 202 AkmSensor::Status AkmAkd::startSensor(){
masahikofukasawa 11:cef8dc1cf010 203 // read one data to clear DRDY
masahikofukasawa 41:a3ea80c594ec 204 AkmECompass::MagneticVectorLsb mag;
Masahiko Fukasawa 48:427bdb7bf31b 205 if( compass->getMagneticVectorLsb(&mag) != AkmECompass::SUCCESS)
Masahiko Fukasawa 48:427bdb7bf31b 206 {
Masahiko Fukasawa 48:427bdb7bf31b 207 MSG("#Error: Start sensor failed %s during read LSB.\r\n", sensorName);
Masahiko Fukasawa 48:427bdb7bf31b 208 return AkmSensor::ERROR;
Masahiko Fukasawa 48:427bdb7bf31b 209 }
masahikofukasawa 9:6fa3e7b17c27 210
masahikofukasawa 19:8dcc4f323bdc 211 AkmAkd::InterruptMode int_mode = getInterrupt(primaryId,subId);
masahikofukasawa 39:3821886c902e 212 if( int_mode == AkmAkd::INTERRUPT_DISABLED ){
masahikofukasawa 39:3821886c902e 213 // No DRDY, attach timer and start
masahikofukasawa 29:b488d2c89fba 214 ticker.attach(callback(this, &AkmAkd::setEvent), 1.0/AKDP_POLLING_FREQUENCY);
masahikofukasawa 0:7a00359e701e 215 }
masahikofukasawa 11:cef8dc1cf010 216
masahikofukasawa 11:cef8dc1cf010 217 // set operation mode
masahikofukasawa 41:a3ea80c594ec 218 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 41:a3ea80c594ec 219 MSG("#Error: Start sensor failed %s\r\n", sensorName);
masahikofukasawa 41:a3ea80c594ec 220 return AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 221 }
Masahiko Fukasawa 48:427bdb7bf31b 222
masahikofukasawa 29:b488d2c89fba 223 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 0:7a00359e701e 224 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 225 }
masahikofukasawa 0:7a00359e701e 226
masahikofukasawa 0:7a00359e701e 227 AkmSensor::Status AkmAkd::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 228 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 229 }
masahikofukasawa 0:7a00359e701e 230
masahikofukasawa 0:7a00359e701e 231 AkmSensor::Status AkmAkd::stopSensor(){
masahikofukasawa 0:7a00359e701e 232 ticker.detach();
masahikofukasawa 29:b488d2c89fba 233 AkmSensor::clearEvent();
masahikofukasawa 41:a3ea80c594ec 234
masahikofukasawa 41:a3ea80c594ec 235 AkmECompass::Mode temp;
masahikofukasawa 41:a3ea80c594ec 236 temp.mode = AkmECompass::MODE_POWER_DOWN;
masahikofukasawa 42:b48b3ab8690e 237 uint8_t options[lenOptions];
masahikofukasawa 41:a3ea80c594ec 238 for(int i=0; i<lenOptions; i++){
masahikofukasawa 42:b48b3ab8690e 239 options[i] = mode.options[i];
masahikofukasawa 41:a3ea80c594ec 240 }
masahikofukasawa 42:b48b3ab8690e 241 temp.options = options;
masahikofukasawa 41:a3ea80c594ec 242 if(compass->setOperationMode(temp) != AkmECompass::SUCCESS) {
masahikofukasawa 41:a3ea80c594ec 243 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 244 }
masahikofukasawa 20:2fca76521680 245
masahikofukasawa 11:cef8dc1cf010 246 // read one data to clear DRDY
masahikofukasawa 41:a3ea80c594ec 247 AkmECompass::MagneticVectorLsb mag;
masahikofukasawa 41:a3ea80c594ec 248 compass->getMagneticVectorLsb(&mag);
masahikofukasawa 11:cef8dc1cf010 249
masahikofukasawa 0:7a00359e701e 250 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 251 }
masahikofukasawa 0:7a00359e701e 252
masahikofukasawa 0:7a00359e701e 253 AkmSensor::Status AkmAkd::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 254 AkmSensor::clearEvent();
masahikofukasawa 0:7a00359e701e 255
masahikofukasawa 41:a3ea80c594ec 256 AkmECompass::MagneticVectorLsb lsb;
masahikofukasawa 41:a3ea80c594ec 257 AkmECompass::Status status = compass->getMagneticVectorLsb(&lsb);
masahikofukasawa 0:7a00359e701e 258 if( status != AkmECompass::SUCCESS){
masahikofukasawa 0:7a00359e701e 259 return AkmSensor::ERROR;
masahikofukasawa 0:7a00359e701e 260 }
masahikofukasawa 0:7a00359e701e 261 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 41:a3ea80c594ec 262 msg->setArgument( 0, (lsb.isOverflow ? 1 : 0) );
masahikofukasawa 41:a3ea80c594ec 263 if(subId == AkmAkd::SUB_ID_AK09940)
masahikofukasawa 41:a3ea80c594ec 264 {
masahikofukasawa 41:a3ea80c594ec 265 msg->setArgument( 1,(char)( lsb.lsbX>>16 ) );
masahikofukasawa 41:a3ea80c594ec 266 msg->setArgument( 2,(char)( lsb.lsbX>>8 ) );
masahikofukasawa 41:a3ea80c594ec 267 msg->setArgument( 3,(char)( lsb.lsbX ) );
masahikofukasawa 41:a3ea80c594ec 268 msg->setArgument( 4,(char)( lsb.lsbY>>16 ) );
masahikofukasawa 41:a3ea80c594ec 269 msg->setArgument( 5,(char)( lsb.lsbY>>8 ) );
masahikofukasawa 41:a3ea80c594ec 270 msg->setArgument( 6,(char)( lsb.lsbY ) );
masahikofukasawa 41:a3ea80c594ec 271 msg->setArgument( 7,(char)( lsb.lsbZ>>16 ) );
masahikofukasawa 41:a3ea80c594ec 272 msg->setArgument( 8,(char)( lsb.lsbZ>>8 ) );
masahikofukasawa 41:a3ea80c594ec 273 msg->setArgument( 9,(char)( lsb.lsbZ ) );
masahikofukasawa 41:a3ea80c594ec 274 msg->setArgument(10,(char)( lsb.lsbTemp ) );
masahikofukasawa 41:a3ea80c594ec 275 }
masahikofukasawa 41:a3ea80c594ec 276 else
masahikofukasawa 41:a3ea80c594ec 277 {
masahikofukasawa 41:a3ea80c594ec 278 msg->setArgument( 1,(char)( lsb.lsbX>>8 ) );
masahikofukasawa 41:a3ea80c594ec 279 msg->setArgument( 2,(char)( lsb.lsbX ) );
masahikofukasawa 41:a3ea80c594ec 280 msg->setArgument( 3,(char)( lsb.lsbY>>8 ) );
masahikofukasawa 41:a3ea80c594ec 281 msg->setArgument( 4,(char)( lsb.lsbY ) );
masahikofukasawa 41:a3ea80c594ec 282 msg->setArgument( 5,(char)( lsb.lsbZ>>8 ) );
masahikofukasawa 41:a3ea80c594ec 283 msg->setArgument( 6,(char)( lsb.lsbZ ) );
masahikofukasawa 41:a3ea80c594ec 284 }
masahikofukasawa 0:7a00359e701e 285 return AkmSensor::SUCCESS;
masahikofukasawa 0:7a00359e701e 286 }
masahikofukasawa 0:7a00359e701e 287
masahikofukasawa 0:7a00359e701e 288 AkmSensor::Status AkmAkd::requestCommand(Message* in, Message* out){
masahikofukasawa 9:6fa3e7b17c27 289 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 9:6fa3e7b17c27 290 Message::Command cmd = in->getCommand();
masahikofukasawa 9:6fa3e7b17c27 291 out->setCommand(cmd);
masahikofukasawa 9:6fa3e7b17c27 292
masahikofukasawa 9:6fa3e7b17c27 293 switch(cmd){
masahikofukasawa 9:6fa3e7b17c27 294 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 41:a3ea80c594ec 295 {
masahikofukasawa 41:a3ea80c594ec 296 if(compass->getOperationMode(&mode) != AkmECompass::SUCCESS) {
masahikofukasawa 41:a3ea80c594ec 297 status = AkmSensor::ERROR;
masahikofukasawa 41:a3ea80c594ec 298 MSG("#Error: set operation mode.\r\n");
masahikofukasawa 15:1238993fd75f 299 }else{
masahikofukasawa 41:a3ea80c594ec 300 out->setArgument(0,mode.mode);
masahikofukasawa 41:a3ea80c594ec 301 for(int i=0; i<lenOptions; i++){
masahikofukasawa 41:a3ea80c594ec 302 out->setArgument(i+1,mode.options[i]);
masahikofukasawa 41:a3ea80c594ec 303 }
masahikofukasawa 9:6fa3e7b17c27 304 }
masahikofukasawa 9:6fa3e7b17c27 305 break;
masahikofukasawa 9:6fa3e7b17c27 306 }
masahikofukasawa 9:6fa3e7b17c27 307 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 308 {
masahikofukasawa 41:a3ea80c594ec 309 mode.mode = (AkmECompass::OperationMode)in->getArgument(0);
masahikofukasawa 41:a3ea80c594ec 310 for(int i=0; i<lenOptions; i++){
masahikofukasawa 41:a3ea80c594ec 311 mode.options[i] = in->getArgument(i+1);
masahikofukasawa 9:6fa3e7b17c27 312 }
masahikofukasawa 41:a3ea80c594ec 313 if(compass->setOperationMode(mode) != AkmECompass::SUCCESS) {
masahikofukasawa 41:a3ea80c594ec 314 status = AkmSensor::ERROR;
masahikofukasawa 41:a3ea80c594ec 315 MSG("#Error: set operation mode.\r\n");
masahikofukasawa 9:6fa3e7b17c27 316 }
masahikofukasawa 41:a3ea80c594ec 317 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 318 break;
masahikofukasawa 15:1238993fd75f 319 }
masahikofukasawa 15:1238993fd75f 320 case Message::CMD_REG_WRITE:
masahikofukasawa 9:6fa3e7b17c27 321 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 322 {
masahikofukasawa 9:6fa3e7b17c27 323 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 324 const int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 325 if(in->getArgNum() != len+2){
masahikofukasawa 39:3821886c902e 326 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 327 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 328 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 329 return status;
masahikofukasawa 15:1238993fd75f 330 }
masahikofukasawa 15:1238993fd75f 331
masahikofukasawa 9:6fa3e7b17c27 332 char data[len];
masahikofukasawa 9:6fa3e7b17c27 333 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 334 data[i] = in->getArgument(i+2);
masahikofukasawa 9:6fa3e7b17c27 335 }
masahikofukasawa 9:6fa3e7b17c27 336 if( compass->write(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 337 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 338 MSG("#Error: register write.\r\n");
masahikofukasawa 9:6fa3e7b17c27 339 }
masahikofukasawa 9:6fa3e7b17c27 340 out->setArgument(0,(char)status);
masahikofukasawa 9:6fa3e7b17c27 341 break;
masahikofukasawa 15:1238993fd75f 342 }
masahikofukasawa 9:6fa3e7b17c27 343 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 344 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 345 {
masahikofukasawa 15:1238993fd75f 346 if(in->getArgNum() != 2){
masahikofukasawa 39:3821886c902e 347 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 348 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 349 return status;
masahikofukasawa 15:1238993fd75f 350 }
masahikofukasawa 15:1238993fd75f 351
masahikofukasawa 9:6fa3e7b17c27 352 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 353 const int len = (int)in->getArgument(1);
masahikofukasawa 9:6fa3e7b17c27 354 char data[len];
masahikofukasawa 9:6fa3e7b17c27 355 if( compass->read(address, data, len) != AkmECompass::SUCCESS) {
masahikofukasawa 9:6fa3e7b17c27 356 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 357 MSG("#Error: register read.\r\n");
masahikofukasawa 9:6fa3e7b17c27 358 }
masahikofukasawa 9:6fa3e7b17c27 359 for(int i=0; i<len; i++){
masahikofukasawa 9:6fa3e7b17c27 360 out->setArgument(i, data[i]);
masahikofukasawa 9:6fa3e7b17c27 361 }
masahikofukasawa 9:6fa3e7b17c27 362 break;
masahikofukasawa 9:6fa3e7b17c27 363 }
masahikofukasawa 9:6fa3e7b17c27 364 default:
masahikofukasawa 9:6fa3e7b17c27 365 {
masahikofukasawa 39:3821886c902e 366 MSG("#Error: no command.\r\n");
masahikofukasawa 9:6fa3e7b17c27 367 status = AkmSensor::ERROR;
masahikofukasawa 9:6fa3e7b17c27 368 break;
masahikofukasawa 9:6fa3e7b17c27 369 }
masahikofukasawa 9:6fa3e7b17c27 370 }
masahikofukasawa 15:1238993fd75f 371 return status;
masahikofukasawa 0:7a00359e701e 372 }