Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak09970ctrl.cpp@16:d85be9bafb80, 2017-01-04 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Wed Jan 04 00:52:30 2017 +0000
- Revision:
- 16:d85be9bafb80
- Parent:
- 15:1238993fd75f
- Child:
- 27:41aa9fb23a2f
Rev012. Modified for using the latest BLE_API, nRF51822 and mbed libs.
Who changed what in which revision?
User | Revision | Line number | New 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 | 16:d85be9bafb80 | 138 | sw->rise(callback(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 | } |