Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak09970ctrl.cpp@29:b488d2c89fba, 2017-03-17 (annotated)

Committer:
masahikofukasawa
Date:
Fri Mar 17 23:29:20 2017 +0000
Revision:
29:b488d2c89fba
Parent:
27:41aa9fb23a2f
Child:
39:3821886c902e
Modified for multi sensor demo.

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