Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmakd.cpp@41:a3ea80c594ec, 2017-08-17 (annotated)

Committer:
masahikofukasawa
Date:
Thu Aug 17 19:59:10 2017 +0000
Revision:
41:a3ea80c594ec
Parent:
40:42e48427e4b7
Child:
42:b48b3ab8690e
Modified for AK09940

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