Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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

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

Who changed what in which revision?

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