Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

ak9750ctrl.cpp@40:42e48427e4b7, 2017-07-19 (annotated)

Committer:
masahikofukasawa
Date:
Wed Jul 19 23:30:22 2017 +0000
Revision:
40:42e48427e4b7
Parent:
39:3821886c902e
Child:
43:45225713cd58
Changed I2C speed:100kHz --> 400kHz, Serial baud-rate: 115200bps --> 460800bps for faster sampling.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 10:5c69b067d88a 1 #include "ak9750ctrl.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 /**
masahikofukasawa 10:5c69b067d88a 7 * Constructor.
masahikofukasawa 10:5c69b067d88a 8 *
masahikofukasawa 10:5c69b067d88a 9 */
masahikofukasawa 27:41aa9fb23a2f 10 Ak9750Ctrl::Ak9750Ctrl() : AkmSensor(){
masahikofukasawa 10:5c69b067d88a 11 ak9750 = NULL;
masahikofukasawa 10:5c69b067d88a 12 }
masahikofukasawa 10:5c69b067d88a 13
masahikofukasawa 10:5c69b067d88a 14 /**
masahikofukasawa 10:5c69b067d88a 15 * Destructor.
masahikofukasawa 10:5c69b067d88a 16 *
masahikofukasawa 10:5c69b067d88a 17 */
masahikofukasawa 10:5c69b067d88a 18 Ak9750Ctrl::~Ak9750Ctrl(){
masahikofukasawa 10:5c69b067d88a 19 if (ak9750) delete ak9750;
masahikofukasawa 10:5c69b067d88a 20 }
masahikofukasawa 10:5c69b067d88a 21
masahikofukasawa 10:5c69b067d88a 22 AkmSensor::Status Ak9750Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 23 primaryId = id;
masahikofukasawa 10:5c69b067d88a 24 subId = subid;
masahikofukasawa 10:5c69b067d88a 25
masahikofukasawa 10:5c69b067d88a 26 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 40:42e48427e4b7 27 i2c->frequency(I2C_SPEED);
masahikofukasawa 10:5c69b067d88a 28
masahikofukasawa 10:5c69b067d88a 29 if(subId == SUB_ID_AK9750){
masahikofukasawa 10:5c69b067d88a 30 ak9750 = new AK9750();
masahikofukasawa 13:d008249f0359 31 sensorName = "AK9750";
masahikofukasawa 13:d008249f0359 32 }
masahikofukasawa 13:d008249f0359 33 else if(subId == SUB_ID_AK9753){
masahikofukasawa 13:d008249f0359 34 ak9750 = new AK9750();
masahikofukasawa 13:d008249f0359 35 sensorName = "AK9753";
masahikofukasawa 10:5c69b067d88a 36 }
masahikofukasawa 10:5c69b067d88a 37 else{
masahikofukasawa 10:5c69b067d88a 38 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 39 }
masahikofukasawa 10:5c69b067d88a 40
masahikofukasawa 10:5c69b067d88a 41 bool foundSensor = false;
masahikofukasawa 10:5c69b067d88a 42
masahikofukasawa 10:5c69b067d88a 43 AK9750::SlaveAddress slaveAddr[]
masahikofukasawa 10:5c69b067d88a 44 = { AK9750::SLAVE_ADDR_1,
masahikofukasawa 10:5c69b067d88a 45 AK9750::SLAVE_ADDR_2,
masahikofukasawa 10:5c69b067d88a 46 AK9750::SLAVE_ADDR_3};
masahikofukasawa 10:5c69b067d88a 47
masahikofukasawa 10:5c69b067d88a 48 for(int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 10:5c69b067d88a 49 {
masahikofukasawa 10:5c69b067d88a 50 ak9750->init(i2c, slaveAddr[i]);
masahikofukasawa 10:5c69b067d88a 51 // Checks connectivity
masahikofukasawa 10:5c69b067d88a 52 if(ak9750->checkConnection() == Ak9750Ctrl::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 53 // found
masahikofukasawa 10:5c69b067d88a 54 foundSensor = true;
masahikofukasawa 10:5c69b067d88a 55 break;
masahikofukasawa 10:5c69b067d88a 56 }
masahikofukasawa 10:5c69b067d88a 57 }
masahikofukasawa 10:5c69b067d88a 58 if(foundSensor != true){
masahikofukasawa 39:3821886c902e 59 MSG("#Error: Failed to checkConnection AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 60 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 61 }
masahikofukasawa 10:5c69b067d88a 62
masahikofukasawa 10:5c69b067d88a 63 // reset
masahikofukasawa 10:5c69b067d88a 64 if (ak9750->reset() != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 65 MSG("#Error: Failed to reset AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 66 }
masahikofukasawa 15:1238993fd75f 67 /*
masahikofukasawa 10:5c69b067d88a 68 // Set to EEPROM mode to EEPROM access
masahikofukasawa 10:5c69b067d88a 69 if(ak9750->setOperationMode(AK9750::MODE_EEPROM_ACCESS, AK9750::DF_0P3HZ) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 70 MSG("#Error: setOperationMode to EEPROM mode. AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 71 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 72 }
masahikofukasawa 15:1238993fd75f 73 */
masahikofukasawa 10:5c69b067d88a 74 // Gets threshold from EEPROM
masahikofukasawa 10:5c69b067d88a 75 AK9750::Threshold th;
masahikofukasawa 10:5c69b067d88a 76 if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 77 MSG("#Error: Failed to get threshold from EEPROM AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 78 }
masahikofukasawa 15:1238993fd75f 79 MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\r\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l);
masahikofukasawa 15:1238993fd75f 80
masahikofukasawa 10:5c69b067d88a 81 // Gets hysteresis from EEPROM
masahikofukasawa 10:5c69b067d88a 82 AK9750::Hysteresis hys;
masahikofukasawa 10:5c69b067d88a 83 if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 84 MSG("#Error: Failed to get hysteresis from EEPROM AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 85 }
masahikofukasawa 15:1238993fd75f 86 MSG("#Hysteresis:(0x%02X,0x%02X)\r\n",hys.ehys13,hys.ehys24);
masahikofukasawa 15:1238993fd75f 87
masahikofukasawa 10:5c69b067d88a 88 // Gets interrupt status from EEPROM
masahikofukasawa 10:5c69b067d88a 89 AK9750::InterruptStatus intStatus;
masahikofukasawa 10:5c69b067d88a 90 if ((ak9750->getInterruptEnableFromEEPROM(&intStatus)) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 91 MSG("#Error: Failed to get interrupts of AK9750 from EEPROM.\r\n");
masahikofukasawa 10:5c69b067d88a 92 }
masahikofukasawa 15:1238993fd75f 93 MSG("#Interrupt:(0x%02X,0x%02X,0x%02X,0x%02X,0x%02X)\r\n",intStatus.ir13h,intStatus.ir13l,intStatus.ir24h,intStatus.ir24l,intStatus.drdy);
masahikofukasawa 10:5c69b067d88a 94
masahikofukasawa 10:5c69b067d88a 95 // Gets operation mode from EEPROM
masahikofukasawa 10:5c69b067d88a 96 if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 97 MSG("#Error: Failed to get operation mode of AK9750 from EEPROM.\r\n");
masahikofukasawa 10:5c69b067d88a 98 }
masahikofukasawa 11:cef8dc1cf010 99 MSG("#Operation Mode:(0x%02X,0x%02X)\r\n",mode, filter);
masahikofukasawa 15:1238993fd75f 100 /*
masahikofukasawa 10:5c69b067d88a 101 // Back to Stand-by Mode for register access
masahikofukasawa 10:5c69b067d88a 102 if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 103 MSG("#Error: setOperationMode to stand-by mode of AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 104 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 105 }
masahikofukasawa 15:1238993fd75f 106 */
masahikofukasawa 10:5c69b067d88a 107 // Sets threshold from the red EEPROM values
masahikofukasawa 10:5c69b067d88a 108 if (ak9750->setThreshold(&th) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 109 MSG("#Error: Failed to set threshold to AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 110 }
masahikofukasawa 10:5c69b067d88a 111
masahikofukasawa 10:5c69b067d88a 112 // Sets hysteresis from the red EEPROM values
masahikofukasawa 10:5c69b067d88a 113 if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 114 MSG("#Error: Failed to set hysteresis to AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 115 }
masahikofukasawa 10:5c69b067d88a 116
masahikofukasawa 10:5c69b067d88a 117 // Sets interruput status from the red EEPROM values
masahikofukasawa 10:5c69b067d88a 118 AK9750::Status status = ak9750->setInterruptEnable(&intStatus);
masahikofukasawa 10:5c69b067d88a 119 if (status != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 120 MSG("#Error: Failed to set interrupts of AK9750. Status = 0x%02X\r\n", status);
masahikofukasawa 10:5c69b067d88a 121 }
masahikofukasawa 10:5c69b067d88a 122
masahikofukasawa 15:1238993fd75f 123 if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 124 MSG("#Error: setOperationMode of AK9750.\r\n");
masahikofukasawa 15:1238993fd75f 125 return AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 126 }
masahikofukasawa 15:1238993fd75f 127
masahikofukasawa 11:cef8dc1cf010 128 MSG("#Init success AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 129 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 130 }
masahikofukasawa 10:5c69b067d88a 131
masahikofukasawa 29:b488d2c89fba 132 void Ak9750Ctrl::setEvent(){
masahikofukasawa 29:b488d2c89fba 133 AK9750::Status status = ak9750->isDataReady();
masahikofukasawa 29:b488d2c89fba 134 if( status == AK9750::DATA_READY ) base::setEvent();
masahikofukasawa 10:5c69b067d88a 135 }
masahikofukasawa 10:5c69b067d88a 136
masahikofukasawa 10:5c69b067d88a 137 AkmSensor::Status Ak9750Ctrl::startSensor(){
masahikofukasawa 11:cef8dc1cf010 138 // read one data to clear INT pin
masahikofukasawa 11:cef8dc1cf010 139 AK9750::SensorData data;
masahikofukasawa 11:cef8dc1cf010 140 ak9750->getSensorData(&data);
masahikofukasawa 11:cef8dc1cf010 141
masahikofukasawa 11:cef8dc1cf010 142 // set operation mode
masahikofukasawa 10:5c69b067d88a 143 if(ak9750->setOperationMode(mode,filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 144 MSG("#Error: Start sensor failed %s\r\n", sensorName);
masahikofukasawa 10:5c69b067d88a 145 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 146 }
masahikofukasawa 11:cef8dc1cf010 147
masahikofukasawa 29:b488d2c89fba 148 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 10:5c69b067d88a 149 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 150 }
masahikofukasawa 10:5c69b067d88a 151
masahikofukasawa 10:5c69b067d88a 152 AkmSensor::Status Ak9750Ctrl::startSensor(const float sec){
masahikofukasawa 10:5c69b067d88a 153 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 154 }
masahikofukasawa 10:5c69b067d88a 155
masahikofukasawa 10:5c69b067d88a 156 AkmSensor::Status Ak9750Ctrl::stopSensor(){
masahikofukasawa 29:b488d2c89fba 157 AkmSensor::clearEvent();
masahikofukasawa 29:b488d2c89fba 158
masahikofukasawa 10:5c69b067d88a 159 if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 160 MSG("#Error: setOperationMode. AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 161 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 162 }
masahikofukasawa 10:5c69b067d88a 163
masahikofukasawa 10:5c69b067d88a 164 // read one data to clear INT pin
masahikofukasawa 11:cef8dc1cf010 165 AK9750::SensorData data;
masahikofukasawa 11:cef8dc1cf010 166 ak9750->getSensorData(&data);
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 Ak9750Ctrl::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 172 AkmSensor::clearEvent();
masahikofukasawa 10:5c69b067d88a 173
masahikofukasawa 10:5c69b067d88a 174 AK9750::SensorData data;
masahikofukasawa 10:5c69b067d88a 175 AK9750::Status status = ak9750->getSensorData(&data);
masahikofukasawa 10:5c69b067d88a 176 if( status != AK9750::SUCCESS){
masahikofukasawa 39:3821886c902e 177 MSG("#Error: getSensorData. AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 178 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 179 }
masahikofukasawa 10:5c69b067d88a 180 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 181 msg->setArgument( 0, data.intStatus.ir13h);
masahikofukasawa 10:5c69b067d88a 182 msg->setArgument( 1, data.intStatus.ir13l);
masahikofukasawa 10:5c69b067d88a 183 msg->setArgument( 2, data.intStatus.ir24h);
masahikofukasawa 10:5c69b067d88a 184 msg->setArgument( 3, data.intStatus.ir24l);
masahikofukasawa 10:5c69b067d88a 185 msg->setArgument( 4, data.intStatus.drdy);
masahikofukasawa 10:5c69b067d88a 186 msg->setArgument( 5,(char)((int32_t)(data.ir1) >> 8));
masahikofukasawa 10:5c69b067d88a 187 msg->setArgument( 6, (char)((int32_t)(data.ir1) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 188 msg->setArgument( 7,(char)((int32_t)(data.ir2) >> 8));
masahikofukasawa 10:5c69b067d88a 189 msg->setArgument( 8, (char)((int32_t)(data.ir2) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 190 msg->setArgument( 9,(char)((int32_t)(data.ir3) >> 8));
masahikofukasawa 10:5c69b067d88a 191 msg->setArgument( 10, (char)((int32_t)(data.ir3) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 192 msg->setArgument( 11,(char)((int32_t)(data.ir4) >> 8));
masahikofukasawa 10:5c69b067d88a 193 msg->setArgument( 12, (char)((int32_t)(data.ir4) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 194 msg->setArgument( 13,(char)((int32_t)(data.temperature) >> 8));
masahikofukasawa 10:5c69b067d88a 195 msg->setArgument( 14, (char)((int32_t)(data.temperature) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 196 msg->setArgument( 15, data.dor);
masahikofukasawa 10:5c69b067d88a 197
masahikofukasawa 10:5c69b067d88a 198 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 199 }
masahikofukasawa 10:5c69b067d88a 200
masahikofukasawa 10:5c69b067d88a 201 AkmSensor::Status Ak9750Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 202 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 203 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 204 AK9750::Threshold th;
masahikofukasawa 10:5c69b067d88a 205 AK9750::Hysteresis hys;
masahikofukasawa 10:5c69b067d88a 206 AK9750::InterruptStatus interrupt;
masahikofukasawa 10:5c69b067d88a 207
masahikofukasawa 10:5c69b067d88a 208 out->setCommand(cmd);
masahikofukasawa 10:5c69b067d88a 209
masahikofukasawa 10:5c69b067d88a 210 switch(cmd){
masahikofukasawa 10:5c69b067d88a 211 case Message::CMD_IR_GET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 212 {
masahikofukasawa 10:5c69b067d88a 213 if (ak9750->getThreshold(&th) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 214 MSG("#Error: Failed to get threshold of AK9750.\r\n");
masahikofukasawa 15:1238993fd75f 215 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 216 }
masahikofukasawa 10:5c69b067d88a 217 out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
masahikofukasawa 10:5c69b067d88a 218 out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 219 out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8));
masahikofukasawa 10:5c69b067d88a 220 out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 221 out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8));
masahikofukasawa 10:5c69b067d88a 222 out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 223 out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8));
masahikofukasawa 10:5c69b067d88a 224 out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 225 break;
masahikofukasawa 10:5c69b067d88a 226 }
masahikofukasawa 10:5c69b067d88a 227 case Message::CMD_IR_SET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 228 {
masahikofukasawa 10:5c69b067d88a 229 th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1));
masahikofukasawa 10:5c69b067d88a 230 th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3));
masahikofukasawa 10:5c69b067d88a 231 th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5));
masahikofukasawa 10:5c69b067d88a 232 th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7));
masahikofukasawa 10:5c69b067d88a 233 if (ak9750->setThreshold(&th) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 234 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 235 MSG("#Error: Failed to set threshold to AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 236 }
masahikofukasawa 10:5c69b067d88a 237 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 238 break;
masahikofukasawa 10:5c69b067d88a 239 }
masahikofukasawa 10:5c69b067d88a 240 case Message::CMD_IR_GET_HYSTERESIS:
masahikofukasawa 10:5c69b067d88a 241 {
masahikofukasawa 10:5c69b067d88a 242 if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 243 MSG("#Error: Failed to get hysteresis of AK9750.\r\n");
masahikofukasawa 15:1238993fd75f 244 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 245 }
masahikofukasawa 10:5c69b067d88a 246 out->setArgument(0,hys.ehys13);
masahikofukasawa 10:5c69b067d88a 247 out->setArgument(1,hys.ehys24);
masahikofukasawa 10:5c69b067d88a 248 break;
masahikofukasawa 10:5c69b067d88a 249 }
masahikofukasawa 10:5c69b067d88a 250 case Message::CMD_IR_SET_HYSTERESIS:
masahikofukasawa 10:5c69b067d88a 251 {
masahikofukasawa 10:5c69b067d88a 252 hys.ehys13 = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 253 hys.ehys24 = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 254 if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 255 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 256 MSG("#Error: Failed to set hysteresis to AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 257 }
masahikofukasawa 10:5c69b067d88a 258 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 259 break;
masahikofukasawa 10:5c69b067d88a 260 }
masahikofukasawa 10:5c69b067d88a 261 case Message::CMD_IR_GET_INTERRUPT:
masahikofukasawa 10:5c69b067d88a 262 {
masahikofukasawa 10:5c69b067d88a 263 if (ak9750->getInterruptEnable(&interrupt) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 264 MSG("#Error: Failed to get interrupt of AK9750.\r\n");
masahikofukasawa 15:1238993fd75f 265 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 266 }
masahikofukasawa 10:5c69b067d88a 267 out->setArgument(0, interrupt.ir13h);
masahikofukasawa 10:5c69b067d88a 268 out->setArgument(1, interrupt.ir13l);
masahikofukasawa 10:5c69b067d88a 269 out->setArgument(2, interrupt.ir24h);
masahikofukasawa 10:5c69b067d88a 270 out->setArgument(3, interrupt.ir24l);
masahikofukasawa 10:5c69b067d88a 271 out->setArgument(4, interrupt.drdy);
masahikofukasawa 10:5c69b067d88a 272 break;
masahikofukasawa 10:5c69b067d88a 273 }
masahikofukasawa 10:5c69b067d88a 274 case Message::CMD_IR_SET_INTERRUPT:
masahikofukasawa 10:5c69b067d88a 275 {
masahikofukasawa 10:5c69b067d88a 276 interrupt.ir13h = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 277 interrupt.ir13l = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 278 interrupt.ir24h = in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 279 interrupt.ir24l = in->getArgument(3);
masahikofukasawa 10:5c69b067d88a 280 interrupt.drdy = in->getArgument(4);
masahikofukasawa 10:5c69b067d88a 281 if (ak9750->setInterruptEnable(&interrupt) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 282 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 283 MSG("#Error: Failed to set interrupt to AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 284 }
masahikofukasawa 10:5c69b067d88a 285 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 286 break;
masahikofukasawa 10:5c69b067d88a 287 }
masahikofukasawa 10:5c69b067d88a 288 case Message::CMD_IR_GET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 289 {
masahikofukasawa 10:5c69b067d88a 290 if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 291 MSG("#Error: getOperationMode. AK9750.\r\n");
masahikofukasawa 15:1238993fd75f 292 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 293 }
masahikofukasawa 10:5c69b067d88a 294 out->setArgument(0,(char)mode);
masahikofukasawa 10:5c69b067d88a 295 out->setArgument(1,(char)filter);
masahikofukasawa 10:5c69b067d88a 296 break;
masahikofukasawa 10:5c69b067d88a 297 }
masahikofukasawa 10:5c69b067d88a 298 case Message::CMD_IR_SET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 299 {
masahikofukasawa 10:5c69b067d88a 300 mode = (AK9750::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 301 filter = (AK9750::DigitalFilter)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 302 if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 303 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 304 MSG("#Error: setOperationMode. AK9750.\r\n");
masahikofukasawa 10:5c69b067d88a 305 }
masahikofukasawa 10:5c69b067d88a 306 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 307 break;
masahikofukasawa 10:5c69b067d88a 308 }
masahikofukasawa 10:5c69b067d88a 309 case Message::CMD_IR_GET_THRESHOLD_EEPROM:
masahikofukasawa 10:5c69b067d88a 310 {
masahikofukasawa 10:5c69b067d88a 311 if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 312 MSG("#Error: Failed to get threshold of AK9750(EEPROM).\r\n");
masahikofukasawa 15:1238993fd75f 313 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 314 }
masahikofukasawa 10:5c69b067d88a 315 out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8));
masahikofukasawa 10:5c69b067d88a 316 out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 317 out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8));
masahikofukasawa 10:5c69b067d88a 318 out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 319 out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8));
masahikofukasawa 10:5c69b067d88a 320 out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 321 out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8));
masahikofukasawa 10:5c69b067d88a 322 out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 323 break;
masahikofukasawa 10:5c69b067d88a 324 }
masahikofukasawa 10:5c69b067d88a 325 case Message::CMD_IR_SET_THRESHOLD_EEPROM:
masahikofukasawa 10:5c69b067d88a 326 {
masahikofukasawa 10:5c69b067d88a 327 th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1));
masahikofukasawa 10:5c69b067d88a 328 th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3));
masahikofukasawa 10:5c69b067d88a 329 th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5));
masahikofukasawa 10:5c69b067d88a 330 th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7));
masahikofukasawa 10:5c69b067d88a 331 if (ak9750->setThresholdToEEPROM(&th) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 332 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 333 MSG("#Error: Failed to set threshold to AK9750(EEPROM).\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_IR_GET_HYSTERESIS_EEPROM:
masahikofukasawa 10:5c69b067d88a 339 {
masahikofukasawa 10:5c69b067d88a 340 if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 341 MSG("#Error: Failed to get hysteresis of AK9750(EEPROM).\r\n");
masahikofukasawa 15:1238993fd75f 342 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 343 }
masahikofukasawa 10:5c69b067d88a 344 out->setArgument(0,hys.ehys13);
masahikofukasawa 10:5c69b067d88a 345 out->setArgument(1,hys.ehys24);
masahikofukasawa 10:5c69b067d88a 346 break;
masahikofukasawa 10:5c69b067d88a 347 }
masahikofukasawa 10:5c69b067d88a 348 case Message::CMD_IR_SET_HYSTERESIS_EEPROM:
masahikofukasawa 10:5c69b067d88a 349 {
masahikofukasawa 10:5c69b067d88a 350 hys.ehys13 = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 351 hys.ehys24 = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 352 if (ak9750->setHysteresisToEEPROM(&hys) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 353 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 354 MSG("#Error: Failed to set hysteresis to AK9750(EEPROM).\r\n");
masahikofukasawa 10:5c69b067d88a 355 }
masahikofukasawa 10:5c69b067d88a 356 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 357 break;
masahikofukasawa 10:5c69b067d88a 358 }
masahikofukasawa 10:5c69b067d88a 359 case Message::CMD_IR_GET_INTERRUPT_EEPROM:
masahikofukasawa 10:5c69b067d88a 360 {
masahikofukasawa 10:5c69b067d88a 361 if (ak9750->getInterruptEnableFromEEPROM(&interrupt) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 362 MSG("#Error: Failed to get interrupt of AK9750(EEPROM).\r\n");
masahikofukasawa 15:1238993fd75f 363 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 364 }
masahikofukasawa 10:5c69b067d88a 365 out->setArgument(0, interrupt.ir13h);
masahikofukasawa 10:5c69b067d88a 366 out->setArgument(1, interrupt.ir13l);
masahikofukasawa 10:5c69b067d88a 367 out->setArgument(2, interrupt.ir24h);
masahikofukasawa 10:5c69b067d88a 368 out->setArgument(3, interrupt.ir24l);
masahikofukasawa 10:5c69b067d88a 369 out->setArgument(4, interrupt.drdy);
masahikofukasawa 10:5c69b067d88a 370 break;
masahikofukasawa 10:5c69b067d88a 371 }
masahikofukasawa 10:5c69b067d88a 372 case Message::CMD_IR_SET_INTERRUPT_EEPROM:
masahikofukasawa 10:5c69b067d88a 373 {
masahikofukasawa 10:5c69b067d88a 374 interrupt.ir13h = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 375 interrupt.ir13l = in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 376 interrupt.ir24h = in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 377 interrupt.ir24l = in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 378 interrupt.drdy = in->getArgument(4);
masahikofukasawa 10:5c69b067d88a 379 if (ak9750->setInterruptEnableToEEPROM(&interrupt) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 380 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 381 MSG("#Error: Failed to set interrupt to AK9750(EEPROM).\r\n");
masahikofukasawa 10:5c69b067d88a 382 }
masahikofukasawa 10:5c69b067d88a 383 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 384 break;
masahikofukasawa 10:5c69b067d88a 385 }
masahikofukasawa 10:5c69b067d88a 386 case Message::CMD_IR_GET_OPERATION_MODE_EEPROM:
masahikofukasawa 10:5c69b067d88a 387 {
masahikofukasawa 10:5c69b067d88a 388 if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 389 MSG("#Error: getOperationMode. AK9750(EEPROM).\r\n");
masahikofukasawa 15:1238993fd75f 390 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 391 }
masahikofukasawa 10:5c69b067d88a 392 out->setArgument(0,(char)mode);
masahikofukasawa 10:5c69b067d88a 393 out->setArgument(1,(char)filter);
masahikofukasawa 10:5c69b067d88a 394 break;
masahikofukasawa 10:5c69b067d88a 395 }
masahikofukasawa 10:5c69b067d88a 396 case Message::CMD_IR_SET_OPERATION_MODE_EEPROM:
masahikofukasawa 10:5c69b067d88a 397 {
masahikofukasawa 10:5c69b067d88a 398 mode = (AK9750::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 399 filter = (AK9750::DigitalFilter)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 400 if(ak9750->setOperationModeToEEPROM(mode, filter) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 401 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 402 MSG("#Error: setOperationMode. AK9750(EEPROM).\r\n");
masahikofukasawa 10:5c69b067d88a 403 }
masahikofukasawa 10:5c69b067d88a 404 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 405 break;
masahikofukasawa 10:5c69b067d88a 406 }
masahikofukasawa 10:5c69b067d88a 407 case Message::CMD_REG_WRITE:
masahikofukasawa 10:5c69b067d88a 408 case Message::CMD_REG_WRITEN:
masahikofukasawa 10:5c69b067d88a 409 {
masahikofukasawa 10:5c69b067d88a 410 char address = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 411 int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 412 if(in->getArgNum() != len+2){
masahikofukasawa 39:3821886c902e 413 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 414 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 415 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 416 return status;
masahikofukasawa 15:1238993fd75f 417 }
masahikofukasawa 15:1238993fd75f 418
masahikofukasawa 10:5c69b067d88a 419 char data[len];
masahikofukasawa 10:5c69b067d88a 420 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 421 data[i] = in->getArgument(i+2);
masahikofukasawa 10:5c69b067d88a 422 }
masahikofukasawa 10:5c69b067d88a 423 if( ak9750->write(address, data, len) != AK9750::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 424 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 425 MSG("#Error: register write.\r\n");
masahikofukasawa 10:5c69b067d88a 426 }
masahikofukasawa 10:5c69b067d88a 427 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 428 break;
masahikofukasawa 10:5c69b067d88a 429 }
masahikofukasawa 10:5c69b067d88a 430 case Message::CMD_REG_READ:
masahikofukasawa 10:5c69b067d88a 431 case Message::CMD_REG_READN:
masahikofukasawa 10:5c69b067d88a 432 {
masahikofukasawa 15:1238993fd75f 433 if(in->getArgNum() != 2){
masahikofukasawa 39:3821886c902e 434 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 435 return AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 436 }
masahikofukasawa 15:1238993fd75f 437
masahikofukasawa 10:5c69b067d88a 438 char address = in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 439 int len = (int)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 440 char data[len];
masahikofukasawa 10:5c69b067d88a 441 if( ak9750->read(address, data, len) != AK9750::SUCCESS) {
masahikofukasawa 39:3821886c902e 442 MSG("#Error: register read.\r\n");
masahikofukasawa 15:1238993fd75f 443 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 444 }
masahikofukasawa 10:5c69b067d88a 445 for(int i=0; i<len; i++){
masahikofukasawa 10:5c69b067d88a 446 out->setArgument(i, data[i]);
masahikofukasawa 10:5c69b067d88a 447 }
masahikofukasawa 10:5c69b067d88a 448 break;
masahikofukasawa 10:5c69b067d88a 449 }
masahikofukasawa 10:5c69b067d88a 450 default:
masahikofukasawa 10:5c69b067d88a 451 {
masahikofukasawa 39:3821886c902e 452 MSG("#Error: no command.\r\n");
masahikofukasawa 10:5c69b067d88a 453 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 454 break;
masahikofukasawa 10:5c69b067d88a 455 }
masahikofukasawa 10:5c69b067d88a 456 }
masahikofukasawa 10:5c69b067d88a 457 return status;
masahikofukasawa 10:5c69b067d88a 458 }