Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak09970ctrl.cpp@15:1238993fd75f, 2016-10-28 (annotated)

Committer:
masahikofukasawa
Date:
Fri Oct 28 21:27:33 2016 +0000
Revision:
15:1238993fd75f
Parent:
11:cef8dc1cf010
Child:
16:d85be9bafb80
debugged AK9750 EEPROM access and others.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 10:5c69b067d88a 1 #include "ak09970ctrl.h"
masahikofukasawa 10:5c69b067d88a 2 #include "debug.h"
masahikofukasawa 10:5c69b067d88a 3
masahikofukasawa 10:5c69b067d88a 4 #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low)))
masahikofukasawa 10:5c69b067d88a 5 /**
masahikofukasawa 10:5c69b067d88a 6 * Constructor.
masahikofukasawa 10:5c69b067d88a 7 *
masahikofukasawa 10:5c69b067d88a 8 */
masahikofukasawa 10:5c69b067d88a 9 Ak09970Ctrl::Ak09970Ctrl(){
masahikofukasawa 10:5c69b067d88a 10 event = false;
masahikofukasawa 10:5c69b067d88a 11 ak09970 = NULL;
masahikofukasawa 10:5c69b067d88a 12 sw = NULL;
masahikofukasawa 10:5c69b067d88a 13 sensorName = "";
masahikofukasawa 10:5c69b067d88a 14 }
masahikofukasawa 10:5c69b067d88a 15
masahikofukasawa 10:5c69b067d88a 16 /**
masahikofukasawa 10:5c69b067d88a 17 * Destructor.
masahikofukasawa 10:5c69b067d88a 18 *
masahikofukasawa 10:5c69b067d88a 19 */
masahikofukasawa 10:5c69b067d88a 20 Ak09970Ctrl::~Ak09970Ctrl(){
masahikofukasawa 10:5c69b067d88a 21 sw->rise(0);
masahikofukasawa 10:5c69b067d88a 22 if (ak09970) delete ak09970;
masahikofukasawa 10:5c69b067d88a 23 if (sw) delete sw;
masahikofukasawa 10:5c69b067d88a 24 }
masahikofukasawa 10:5c69b067d88a 25
masahikofukasawa 10:5c69b067d88a 26 AkmSensor::Status Ak09970Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 27 primaryId = id;
masahikofukasawa 10:5c69b067d88a 28 subId = subid;
masahikofukasawa 10:5c69b067d88a 29
masahikofukasawa 10:5c69b067d88a 30 // initialize readConfig
masahikofukasawa 10:5c69b067d88a 31 readConfig.enabledReadX = false;
masahikofukasawa 10:5c69b067d88a 32 readConfig.enabledReadY = false;
masahikofukasawa 10:5c69b067d88a 33 readConfig.enabledReadZ = false;
masahikofukasawa 10:5c69b067d88a 34 readConfig.enabledUpperOnly = false;
masahikofukasawa 10:5c69b067d88a 35
masahikofukasawa 10:5c69b067d88a 36 if(primaryId == AKM_PRIMARY_ID_AKD_I2C){
masahikofukasawa 10:5c69b067d88a 37
masahikofukasawa 10:5c69b067d88a 38 sw = new InterruptIn(I2C_DRDY);
masahikofukasawa 10:5c69b067d88a 39 sw->rise(0);
masahikofukasawa 10:5c69b067d88a 40
masahikofukasawa 10:5c69b067d88a 41 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 10:5c69b067d88a 42 i2c->frequency(I2C_SPEED_100KHZ);
masahikofukasawa 10:5c69b067d88a 43
masahikofukasawa 10:5c69b067d88a 44 switch(subid){
masahikofukasawa 10:5c69b067d88a 45 case Ak09970Ctrl::SUB_ID_AK09970:
masahikofukasawa 10:5c69b067d88a 46 ak09970 = new AK09970();
masahikofukasawa 10:5c69b067d88a 47 Ak09970Ctrl::sensorName = "AK09970";
masahikofukasawa 10:5c69b067d88a 48 break;
masahikofukasawa 10:5c69b067d88a 49 default:
masahikofukasawa 10:5c69b067d88a 50 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 51 }
masahikofukasawa 10:5c69b067d88a 52
masahikofukasawa 10:5c69b067d88a 53 bool foundSensor = false;
masahikofukasawa 10:5c69b067d88a 54
masahikofukasawa 10:5c69b067d88a 55 AK09970::SlaveAddress slaveAddr[]
masahikofukasawa 10:5c69b067d88a 56 = { AK09970::SLAVE_ADDR_1,
masahikofukasawa 10:5c69b067d88a 57 AK09970::SLAVE_ADDR_2};
masahikofukasawa 10:5c69b067d88a 58
masahikofukasawa 10:5c69b067d88a 59 for(int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 10:5c69b067d88a 60 {
masahikofukasawa 10:5c69b067d88a 61 ak09970->init(i2c, slaveAddr[i]);
masahikofukasawa 10:5c69b067d88a 62 // Checks connectivity
masahikofukasawa 10:5c69b067d88a 63 if(ak09970->checkConnection() == AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 64 // found
masahikofukasawa 10:5c69b067d88a 65 foundSensor = true;
masahikofukasawa 10:5c69b067d88a 66 break;
masahikofukasawa 10:5c69b067d88a 67 }
masahikofukasawa 10:5c69b067d88a 68 }
masahikofukasawa 10:5c69b067d88a 69 if(foundSensor != true){
masahikofukasawa 10:5c69b067d88a 70 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 71 }
masahikofukasawa 10:5c69b067d88a 72
masahikofukasawa 10:5c69b067d88a 73 // software reset
masahikofukasawa 10:5c69b067d88a 74 ak09970->reset();
masahikofukasawa 10:5c69b067d88a 75
masahikofukasawa 10:5c69b067d88a 76 }else if(primaryId == AKM_PRIMARY_ID_AKD_SPI){
masahikofukasawa 10:5c69b067d88a 77
masahikofukasawa 10:5c69b067d88a 78 sw = new InterruptIn(SPI_DRDY);
masahikofukasawa 10:5c69b067d88a 79 sw->rise(0);
masahikofukasawa 10:5c69b067d88a 80
masahikofukasawa 10:5c69b067d88a 81 SPI* spi = new SPI(SPI_MOSI, SPI_MISO, SPI_SCK);
masahikofukasawa 10:5c69b067d88a 82 spi->format(8,3); // 8bit, Mode=3
masahikofukasawa 10:5c69b067d88a 83 spi->frequency(1000000);
masahikofukasawa 10:5c69b067d88a 84
masahikofukasawa 10:5c69b067d88a 85 DigitalOut* cs = new DigitalOut(SPI_CS);
masahikofukasawa 10:5c69b067d88a 86
masahikofukasawa 10:5c69b067d88a 87 switch(subid){
masahikofukasawa 10:5c69b067d88a 88 case Ak09970Ctrl::SUB_ID_AK09970:
masahikofukasawa 10:5c69b067d88a 89 ak09970 = new AK09970();
masahikofukasawa 10:5c69b067d88a 90 Ak09970Ctrl::sensorName = "AK09970";
masahikofukasawa 10:5c69b067d88a 91 break;
masahikofukasawa 10:5c69b067d88a 92 default:
masahikofukasawa 10:5c69b067d88a 93 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 94 }
masahikofukasawa 10:5c69b067d88a 95
masahikofukasawa 10:5c69b067d88a 96 bool foundSensor = false;
masahikofukasawa 10:5c69b067d88a 97 ak09970->init(spi, cs);
masahikofukasawa 10:5c69b067d88a 98 if(ak09970->checkConnection() == AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 99 foundSensor = true;
masahikofukasawa 10:5c69b067d88a 100 }
masahikofukasawa 10:5c69b067d88a 101 if(foundSensor != true){
masahikofukasawa 11:cef8dc1cf010 102 MSG("#failed checkConnetion(SPI). %s\r\n",Ak09970Ctrl::sensorName);
masahikofukasawa 10:5c69b067d88a 103 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 104 }
masahikofukasawa 10:5c69b067d88a 105
masahikofukasawa 10:5c69b067d88a 106 // software reset
masahikofukasawa 10:5c69b067d88a 107 ak09970->reset();
masahikofukasawa 10:5c69b067d88a 108 }
masahikofukasawa 10:5c69b067d88a 109 else{
masahikofukasawa 10:5c69b067d88a 110 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 111 }
masahikofukasawa 10:5c69b067d88a 112
masahikofukasawa 11:cef8dc1cf010 113 MSG("#%s detected.\r\n", Ak09970Ctrl::sensorName);
masahikofukasawa 10:5c69b067d88a 114 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 115 }
masahikofukasawa 10:5c69b067d88a 116
masahikofukasawa 10:5c69b067d88a 117
masahikofukasawa 10:5c69b067d88a 118 void Ak09970Ctrl::checkINT(){
masahikofukasawa 10:5c69b067d88a 119 // AK09970::Status status = ak09970->isDataReady();
masahikofukasawa 10:5c69b067d88a 120 // if( status == AK09970::DATA_READY ||
masahikofukasawa 10:5c69b067d88a 121 // status == AK09970::DATA_OVER_RUN ) event = true;
masahikofukasawa 10:5c69b067d88a 122 }
masahikofukasawa 10:5c69b067d88a 123
masahikofukasawa 10:5c69b067d88a 124 void Ak09970Ctrl::detectINT(){
masahikofukasawa 10:5c69b067d88a 125 event = true;
masahikofukasawa 10:5c69b067d88a 126 }
masahikofukasawa 10:5c69b067d88a 127
masahikofukasawa 10:5c69b067d88a 128 bool Ak09970Ctrl::isEvent(){
masahikofukasawa 10:5c69b067d88a 129 return event;
masahikofukasawa 10:5c69b067d88a 130 }
masahikofukasawa 10:5c69b067d88a 131
masahikofukasawa 10:5c69b067d88a 132 AkmSensor::Status Ak09970Ctrl::startSensor(){
masahikofukasawa 11:cef8dc1cf010 133 // read one data to clear switch
masahikofukasawa 11:cef8dc1cf010 134 AK09970::SwitchStatus sw_status;
masahikofukasawa 11:cef8dc1cf010 135 ak09970->getSwitchStatus(&sw_status, readConfig);
masahikofukasawa 11:cef8dc1cf010 136
masahikofukasawa 11:cef8dc1cf010 137 // enable interrupt
masahikofukasawa 11:cef8dc1cf010 138 sw->rise(this, &Ak09970Ctrl::detectINT);
masahikofukasawa 11:cef8dc1cf010 139
masahikofukasawa 11:cef8dc1cf010 140 // set operation mode
masahikofukasawa 10:5c69b067d88a 141 if(ak09970->setOperationMode(mode,sensorDriveMode,sensorMeasurementRange) != AK09970::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 142 MSG("#Start sensor failed.\r\n");
masahikofukasawa 10:5c69b067d88a 143 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 144 }
masahikofukasawa 10:5c69b067d88a 145
masahikofukasawa 11:cef8dc1cf010 146 MSG("#Start sensor succceeded.\r\n");
masahikofukasawa 10:5c69b067d88a 147 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 148 }
masahikofukasawa 10:5c69b067d88a 149
masahikofukasawa 10:5c69b067d88a 150 AkmSensor::Status Ak09970Ctrl::startSensor(const float sec){
masahikofukasawa 11:cef8dc1cf010 151 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 152 }
masahikofukasawa 10:5c69b067d88a 153
masahikofukasawa 10:5c69b067d88a 154 AkmSensor::Status Ak09970Ctrl::stopSensor(){
masahikofukasawa 11:cef8dc1cf010 155 sw->rise(NULL);
masahikofukasawa 11:cef8dc1cf010 156 event = false;
masahikofukasawa 11:cef8dc1cf010 157
masahikofukasawa 10:5c69b067d88a 158 if(ak09970->setOperationMode(AK09970::MODE_POWER_DOWN, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 159 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 160 }
masahikofukasawa 11:cef8dc1cf010 161
masahikofukasawa 11:cef8dc1cf010 162 // read one data to clear switch
masahikofukasawa 11:cef8dc1cf010 163 AK09970::SwitchStatus sw_status;
masahikofukasawa 11:cef8dc1cf010 164 ak09970->getSwitchStatus(&sw_status, readConfig);
masahikofukasawa 11:cef8dc1cf010 165
masahikofukasawa 10:5c69b067d88a 166 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 167 }
masahikofukasawa 10:5c69b067d88a 168
masahikofukasawa 10:5c69b067d88a 169 AkmSensor::Status Ak09970Ctrl::readSensorData(Message* msg){
masahikofukasawa 10:5c69b067d88a 170 event = false;
masahikofukasawa 10:5c69b067d88a 171
masahikofukasawa 10:5c69b067d88a 172 AK09970::SwitchStatus sw_status;
masahikofukasawa 10:5c69b067d88a 173 if( ak09970->getSwitchStatus(&sw_status, readConfig) != AkmSensor::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 174 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 175 }
masahikofukasawa 10:5c69b067d88a 176
masahikofukasawa 10:5c69b067d88a 177 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 178 msg->setArgument(0, sensorMeasurementRange);
masahikofukasawa 10:5c69b067d88a 179 msg->setArgument(1, sw_status.st_hi);
masahikofukasawa 10:5c69b067d88a 180 msg->setArgument(2, sw_status.st_lo);
masahikofukasawa 10:5c69b067d88a 181 msg->setArgument(3, (char)(sw_status.mag.mx>>8));
masahikofukasawa 10:5c69b067d88a 182 msg->setArgument(4, (char)(sw_status.mag.mx & 0x00FF));
masahikofukasawa 10:5c69b067d88a 183 msg->setArgument(5, (char)(sw_status.mag.my>>8));
masahikofukasawa 10:5c69b067d88a 184 msg->setArgument(6, (char)(sw_status.mag.my & 0x00FF));
masahikofukasawa 10:5c69b067d88a 185 msg->setArgument(7, (char)(sw_status.mag.mz>>8));
masahikofukasawa 10:5c69b067d88a 186 msg->setArgument(8, (char)(sw_status.mag.mz & 0x00FF));
masahikofukasawa 10:5c69b067d88a 187
masahikofukasawa 10:5c69b067d88a 188 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 189 }
masahikofukasawa 10:5c69b067d88a 190
masahikofukasawa 10:5c69b067d88a 191 AkmSensor::Status Ak09970Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 192 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 193 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 194 out->setCommand(cmd);
masahikofukasawa 10:5c69b067d88a 195
masahikofukasawa 10:5c69b067d88a 196 switch(cmd){
masahikofukasawa 10:5c69b067d88a 197 case Message::CMD_PROGSW_GET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 198 {
masahikofukasawa 10:5c69b067d88a 199 if(in->getArgNum() != 1){
masahikofukasawa 11:cef8dc1cf010 200 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 201 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 202 }
masahikofukasawa 10:5c69b067d88a 203
masahikofukasawa 10:5c69b067d88a 204 AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 205 AK09970::Threshold th;
masahikofukasawa 10:5c69b067d88a 206 if( ak09970->getThreshold(axis, &th) != AkmSensor::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 207 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 208 }
masahikofukasawa 10:5c69b067d88a 209 out->setArgument(0, axis);
masahikofukasawa 10:5c69b067d88a 210 out->setArgument(1,(char)((int32_t)(th.BOP1) >> 8));
masahikofukasawa 10:5c69b067d88a 211 out->setArgument(2,(char)((int32_t)(th.BOP1) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 212 out->setArgument(3,(char)((int32_t)(th.BRP1) >> 8));
masahikofukasawa 10:5c69b067d88a 213 out->setArgument(4,(char)((int32_t)(th.BRP1) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 214 out->setArgument(5,(char)((int32_t)(th.BOP2) >> 8));
masahikofukasawa 10:5c69b067d88a 215 out->setArgument(6,(char)((int32_t)(th.BOP2) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 216 out->setArgument(7,(char)((int32_t)(th.BRP2) >> 8));
masahikofukasawa 10:5c69b067d88a 217 out->setArgument(8,(char)((int32_t)(th.BRP2) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 218 break;
masahikofukasawa 10:5c69b067d88a 219 }
masahikofukasawa 10:5c69b067d88a 220 case Message::CMD_PROGSW_SET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 221 {
masahikofukasawa 10:5c69b067d88a 222 if(in->getArgNum() != 9){
masahikofukasawa 11:cef8dc1cf010 223 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 224 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 225 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 226 return status;
masahikofukasawa 10:5c69b067d88a 227 }
masahikofukasawa 10:5c69b067d88a 228
masahikofukasawa 10:5c69b067d88a 229 AK09970::SensorAxis axis = (AK09970::SensorAxis)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 230 AK09970::Threshold th;
masahikofukasawa 10:5c69b067d88a 231
masahikofukasawa 10:5c69b067d88a 232 th.BOP1 = CONV16I(in->getArgument(1),in->getArgument(2));
masahikofukasawa 10:5c69b067d88a 233 th.BRP1 = CONV16I(in->getArgument(3),in->getArgument(4));
masahikofukasawa 10:5c69b067d88a 234 th.BOP2 = CONV16I(in->getArgument(5),in->getArgument(6));
masahikofukasawa 10:5c69b067d88a 235 th.BRP2 = CONV16I(in->getArgument(7),in->getArgument(8));
masahikofukasawa 10:5c69b067d88a 236 if( ak09970->setThreshold(axis, th) != AkmSensor::SUCCESS ){
masahikofukasawa 10:5c69b067d88a 237 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 238 }
masahikofukasawa 10:5c69b067d88a 239 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 240 break;
masahikofukasawa 10:5c69b067d88a 241 }
masahikofukasawa 10:5c69b067d88a 242 case Message::CMD_PROGSW_GET_READ_COFIGURATION:
masahikofukasawa 10:5c69b067d88a 243 {
masahikofukasawa 10:5c69b067d88a 244 if(in->getArgNum() != 0){
masahikofukasawa 11:cef8dc1cf010 245 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 246 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 247 }
masahikofukasawa 10:5c69b067d88a 248
masahikofukasawa 10:5c69b067d88a 249 out->setArgument(0, readConfig.enabledReadX);
masahikofukasawa 10:5c69b067d88a 250 out->setArgument(1, readConfig.enabledReadY);
masahikofukasawa 10:5c69b067d88a 251 out->setArgument(2, readConfig.enabledReadZ);
masahikofukasawa 10:5c69b067d88a 252 out->setArgument(3, readConfig.enabledUpperOnly);
masahikofukasawa 10:5c69b067d88a 253 break;
masahikofukasawa 10:5c69b067d88a 254 }
masahikofukasawa 10:5c69b067d88a 255 case Message::CMD_PROGSW_SET_READ_COFIGURATION:
masahikofukasawa 10:5c69b067d88a 256 {
masahikofukasawa 10:5c69b067d88a 257 if(in->getArgNum() != 4){
masahikofukasawa 11:cef8dc1cf010 258 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 259 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 260 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 261 return status;
masahikofukasawa 10:5c69b067d88a 262 }
masahikofukasawa 10:5c69b067d88a 263
masahikofukasawa 10:5c69b067d88a 264 readConfig.enabledReadX = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 265 readConfig.enabledReadY = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 266 readConfig.enabledReadZ = in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 267 readConfig.enabledUpperOnly = in->getArgument(3);
masahikofukasawa 10:5c69b067d88a 268 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 269 break;
masahikofukasawa 10:5c69b067d88a 270 }
masahikofukasawa 10:5c69b067d88a 271 case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION:
masahikofukasawa 10:5c69b067d88a 272 {
masahikofukasawa 10:5c69b067d88a 273 if(in->getArgNum() != 0){
masahikofukasawa 11:cef8dc1cf010 274 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 275 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 276 }
masahikofukasawa 10:5c69b067d88a 277
masahikofukasawa 10:5c69b067d88a 278 if(ak09970->getSwitchConfig(&switchConfig) != AK09970::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 279 MSG("#Error getSwitchConfig. AK09970.\r\n");
masahikofukasawa 15:1238993fd75f 280 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 281 }
masahikofukasawa 10:5c69b067d88a 282 out->setArgument(0,switchConfig.enabledODINTEN);
masahikofukasawa 10:5c69b067d88a 283 out->setArgument(1,switchConfig.enabledINTEN);
masahikofukasawa 10:5c69b067d88a 284 out->setArgument(2,switchConfig.enabledERRADCEN);
masahikofukasawa 10:5c69b067d88a 285 out->setArgument(3,switchConfig.enabledERRXYEN);
masahikofukasawa 10:5c69b067d88a 286 out->setArgument(4,switchConfig.enabledSWZ2EN);
masahikofukasawa 10:5c69b067d88a 287 out->setArgument(5,switchConfig.enabledSWZ1EN);
masahikofukasawa 10:5c69b067d88a 288 out->setArgument(6,switchConfig.enabledSWY2EN);
masahikofukasawa 10:5c69b067d88a 289 out->setArgument(7,switchConfig.enabledSWY1EN);
masahikofukasawa 10:5c69b067d88a 290 out->setArgument(8,switchConfig.enabledSWX2EN);
masahikofukasawa 10:5c69b067d88a 291 out->setArgument(9,switchConfig.enabledSWX1EN);
masahikofukasawa 10:5c69b067d88a 292 out->setArgument(10,switchConfig.enabledDRDYEN);
masahikofukasawa 10:5c69b067d88a 293 break;
masahikofukasawa 10:5c69b067d88a 294 }
masahikofukasawa 10:5c69b067d88a 295 case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION:
masahikofukasawa 10:5c69b067d88a 296 {
masahikofukasawa 10:5c69b067d88a 297 if(in->getArgNum() != 11){
masahikofukasawa 11:cef8dc1cf010 298 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 299 for(int i=0; i<in->getArgNum(); i++){
masahikofukasawa 11:cef8dc1cf010 300 MSG("#%d = %02X\r\n",i,in->getArgument(i));
masahikofukasawa 10:5c69b067d88a 301 }
masahikofukasawa 10:5c69b067d88a 302 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 303 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 304 return status;
masahikofukasawa 10:5c69b067d88a 305 }
masahikofukasawa 10:5c69b067d88a 306
masahikofukasawa 10:5c69b067d88a 307 switchConfig.enabledODINTEN = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 308 switchConfig.enabledINTEN = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 309 switchConfig.enabledERRADCEN = in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 310 switchConfig.enabledERRXYEN = in->getArgument(3);
masahikofukasawa 10:5c69b067d88a 311 switchConfig.enabledSWZ2EN = in->getArgument(4);
masahikofukasawa 10:5c69b067d88a 312 switchConfig.enabledSWZ1EN = in->getArgument(5);
masahikofukasawa 10:5c69b067d88a 313 switchConfig.enabledSWY2EN = in->getArgument(6);
masahikofukasawa 10:5c69b067d88a 314 switchConfig.enabledSWY1EN = in->getArgument(7);
masahikofukasawa 10:5c69b067d88a 315 switchConfig.enabledSWX2EN = in->getArgument(8);
masahikofukasawa 10:5c69b067d88a 316 switchConfig.enabledSWX1EN = in->getArgument(9);
masahikofukasawa 10:5c69b067d88a 317 switchConfig.enabledDRDYEN = in->getArgument(10);
masahikofukasawa 10:5c69b067d88a 318
masahikofukasawa 10:5c69b067d88a 319 if(ak09970->setSwitchConfig(switchConfig) != AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 320 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 321 MSG("#Error setSwitchConfig. AK09970.\r\n");
masahikofukasawa 10:5c69b067d88a 322 }
masahikofukasawa 10:5c69b067d88a 323 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 324 break;
masahikofukasawa 10:5c69b067d88a 325 }
masahikofukasawa 10:5c69b067d88a 326 case Message::CMD_PROGSW_GET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 327 {
masahikofukasawa 10:5c69b067d88a 328 if(in->getArgNum() != 0){
masahikofukasawa 11:cef8dc1cf010 329 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 330 return AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 331 }
masahikofukasawa 10:5c69b067d88a 332
masahikofukasawa 10:5c69b067d88a 333 if(ak09970->getOperationMode(&mode, &sensorDriveMode, &sensorMeasurementRange) != AK09970::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 334 MSG("#Error getOperationMode. AK09970.\r\n");
masahikofukasawa 15:1238993fd75f 335 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 336 }
masahikofukasawa 10:5c69b067d88a 337 out->setArgument(0,(char)sensorMeasurementRange);
masahikofukasawa 10:5c69b067d88a 338 out->setArgument(1,(char)sensorDriveMode);
masahikofukasawa 10:5c69b067d88a 339 out->setArgument(2,(char)mode);
masahikofukasawa 10:5c69b067d88a 340 break;
masahikofukasawa 10:5c69b067d88a 341 }
masahikofukasawa 10:5c69b067d88a 342 case Message::CMD_PROGSW_SET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 343 {
masahikofukasawa 10:5c69b067d88a 344 if(in->getArgNum() != 3){
masahikofukasawa 11:cef8dc1cf010 345 MSG("#Error argument num. AK09970. Args=%d\r\n",in->getArgNum());
masahikofukasawa 10:5c69b067d88a 346 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 347 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 348 return status;
masahikofukasawa 10:5c69b067d88a 349 }
masahikofukasawa 10:5c69b067d88a 350
masahikofukasawa 10:5c69b067d88a 351 sensorMeasurementRange = (AK09970::SensorMeasurementRange)(in->getArgument(0));
masahikofukasawa 10:5c69b067d88a 352 sensorDriveMode = (AK09970::SensorDriveMode)(in->getArgument(1));
masahikofukasawa 10:5c69b067d88a 353 mode = (AK09970::OperationMode)(in->getArgument(2));
masahikofukasawa 10:5c69b067d88a 354 if(ak09970->setOperationMode(mode, sensorDriveMode, sensorMeasurementRange) != AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 355 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 356 MSG("#Error setOperationMode. AK09970. %d,%d,%d\r\n",mode,sensorDriveMode,sensorMeasurementRange);
masahikofukasawa 10:5c69b067d88a 357 }
masahikofukasawa 10:5c69b067d88a 358 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 359 break;
masahikofukasawa 10:5c69b067d88a 360 }
masahikofukasawa 10:5c69b067d88a 361 case Message::CMD_REG_WRITE:
masahikofukasawa 10:5c69b067d88a 362 case Message::CMD_REG_WRITEN:
masahikofukasawa 10:5c69b067d88a 363 {
masahikofukasawa 10:5c69b067d88a 364 char address = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 365 int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 366 if(in->getArgNum() != len+2){
masahikofukasawa 15:1238993fd75f 367 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 368 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 369 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 370 return status;
masahikofukasawa 15:1238993fd75f 371 }
masahikofukasawa 15:1238993fd75f 372
masahikofukasawa 10:5c69b067d88a 373 char data[len];
masahikofukasawa 10:5c69b067d88a 374 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 375 data[i] = in->getArgument(i+2);
masahikofukasawa 10:5c69b067d88a 376 }
masahikofukasawa 10:5c69b067d88a 377 if( ak09970->write(address, data, len) != AK09970::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 378 status = AkmSensor::ERROR;
masahikofukasawa 11:cef8dc1cf010 379 MSG("#Error register write.\r\n");
masahikofukasawa 10:5c69b067d88a 380 }
masahikofukasawa 10:5c69b067d88a 381 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 382 break;
masahikofukasawa 10:5c69b067d88a 383 }
masahikofukasawa 10:5c69b067d88a 384 case Message::CMD_REG_READ:
masahikofukasawa 10:5c69b067d88a 385 case Message::CMD_REG_READN:
masahikofukasawa 10:5c69b067d88a 386 {
masahikofukasawa 10:5c69b067d88a 387 if(in->getArgNum() != 2){
masahikofukasawa 15:1238993fd75f 388 MSG("#Error argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 389 return AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 390 }
masahikofukasawa 10:5c69b067d88a 391
masahikofukasawa 10:5c69b067d88a 392 char address = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 393 int len = (int)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 394 char data[len];
masahikofukasawa 10:5c69b067d88a 395 if( ak09970->read(address, data, len) != AK09970::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 396 MSG("#Error register read.\r\n");
masahikofukasawa 15:1238993fd75f 397 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 398 }
masahikofukasawa 10:5c69b067d88a 399 for(int i=0; i<len; i++){
masahikofukasawa 10:5c69b067d88a 400 out->setArgument(i, data[i]);
masahikofukasawa 10:5c69b067d88a 401 }
masahikofukasawa 10:5c69b067d88a 402 break;
masahikofukasawa 10:5c69b067d88a 403 }
masahikofukasawa 10:5c69b067d88a 404 default:
masahikofukasawa 10:5c69b067d88a 405 {
masahikofukasawa 11:cef8dc1cf010 406 MSG("#Error no command.\r\n");
masahikofukasawa 10:5c69b067d88a 407 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 408 break;
masahikofukasawa 10:5c69b067d88a 409 }
masahikofukasawa 10:5c69b067d88a 410 }
masahikofukasawa 10:5c69b067d88a 411 return status;
masahikofukasawa 10:5c69b067d88a 412 }
masahikofukasawa 10:5c69b067d88a 413
masahikofukasawa 10:5c69b067d88a 414 char* Ak09970Ctrl::getSensorName(){
masahikofukasawa 10:5c69b067d88a 415 return sensorName;
masahikofukasawa 10:5c69b067d88a 416 }