Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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

Committer:
tkstreet
Date:
Tue May 01 21:31:15 2018 +0000
Revision:
49:c8f8946129b6
Parent:
47:221ec4b404ec 
Modified for Rev.E. compatibility.

Who changed what in which revision?

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