Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
ak9750ctrl.cpp@13:d008249f0359, 2016-08-12 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Fri Aug 12 00:04:48 2016 +0000
- Revision:
- 13:d008249f0359
- Parent:
- 11:cef8dc1cf010
- Child:
- 15:1238993fd75f
Ver008. Modified for faster BLE connection, and sensor name in the BLE device name.
Who changed what in which revision?
User | Revision | Line number | New 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 | 10:5c69b067d88a | 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 | 10:5c69b067d88a | 78 | // Gets threshold from EEPROM |
masahikofukasawa | 10:5c69b067d88a | 79 | AK9750::Threshold th; |
masahikofukasawa | 10:5c69b067d88a | 80 | if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 81 | MSG("#Failed to get threshold from EEPROM AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 82 | } |
masahikofukasawa | 10:5c69b067d88a | 83 | // Gets hysteresis from EEPROM |
masahikofukasawa | 10:5c69b067d88a | 84 | AK9750::Hysteresis hys; |
masahikofukasawa | 10:5c69b067d88a | 85 | if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 86 | MSG("#Failed to get hysteresis from EEPROM AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 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 | 11:cef8dc1cf010 | 91 | MSG("#Failed to get interrupts of AK9750 from EEPROM.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 92 | } |
masahikofukasawa | 10:5c69b067d88a | 93 | |
masahikofukasawa | 10:5c69b067d88a | 94 | // Gets operation mode from EEPROM |
masahikofukasawa | 10:5c69b067d88a | 95 | if ((ak9750->getOperationModeFromEEPROM(&mode,&filter)) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 96 | MSG("#Failed to get operation mode of AK9750 from EEPROM.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 97 | } |
masahikofukasawa | 11:cef8dc1cf010 | 98 | MSG("#Operation Mode:(0x%02X,0x%02X)\r\n",mode, filter); |
masahikofukasawa | 10:5c69b067d88a | 99 | |
masahikofukasawa | 10:5c69b067d88a | 100 | // Back to Stand-by Mode for register access |
masahikofukasawa | 10:5c69b067d88a | 101 | if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 102 | MSG("#Error setOperationMode to stand-by mode of AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 103 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 104 | } |
masahikofukasawa | 10:5c69b067d88a | 105 | |
masahikofukasawa | 10:5c69b067d88a | 106 | // Sets threshold from the red EEPROM values |
masahikofukasawa | 10:5c69b067d88a | 107 | if (ak9750->setThreshold(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 108 | MSG("#Failed to set threshold to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 109 | } |
masahikofukasawa | 11:cef8dc1cf010 | 110 | MSG("#Threshold:(0x%02X,0x%02X,0x%02X,0x%02X)\r\n",th.eth13h,th.eth13l,th.eth24h,th.eth24l); |
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 | 11:cef8dc1cf010 | 114 | MSG("#Failed to set hysteresis to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 115 | } |
masahikofukasawa | 11:cef8dc1cf010 | 116 | MSG("#Hysteresis:(0x%02X,0x%02X)\r\n",hys.ehys13,hys.ehys24); |
masahikofukasawa | 10:5c69b067d88a | 117 | |
masahikofukasawa | 10:5c69b067d88a | 118 | // Sets interruput status from the red EEPROM values |
masahikofukasawa | 10:5c69b067d88a | 119 | AK9750::Status status = ak9750->setInterruptEnable(&intStatus); |
masahikofukasawa | 10:5c69b067d88a | 120 | if (status != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 121 | MSG("#Failed to set interrupts of AK9750. Status = 0x%02X\r\n", status); |
masahikofukasawa | 10:5c69b067d88a | 122 | } |
masahikofukasawa | 11:cef8dc1cf010 | 123 | 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 | 124 | |
masahikofukasawa | 11:cef8dc1cf010 | 125 | MSG("#Init success AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 126 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 127 | } |
masahikofukasawa | 10:5c69b067d88a | 128 | |
masahikofukasawa | 10:5c69b067d88a | 129 | void Ak9750Ctrl::detectINT(){ |
masahikofukasawa | 10:5c69b067d88a | 130 | event = true; |
masahikofukasawa | 10:5c69b067d88a | 131 | } |
masahikofukasawa | 10:5c69b067d88a | 132 | |
masahikofukasawa | 10:5c69b067d88a | 133 | bool Ak9750Ctrl::isEvent(){ |
masahikofukasawa | 10:5c69b067d88a | 134 | return event; |
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 | // enable interrupt |
masahikofukasawa | 11:cef8dc1cf010 | 143 | interrupt->fall(this, &Ak9750Ctrl::detectINT); |
masahikofukasawa | 11:cef8dc1cf010 | 144 | |
masahikofukasawa | 11:cef8dc1cf010 | 145 | // set operation mode |
masahikofukasawa | 10:5c69b067d88a | 146 | if(ak9750->setOperationMode(mode,filter) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 147 | MSG("#Start sensor failed.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 148 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 149 | } |
masahikofukasawa | 11:cef8dc1cf010 | 150 | |
masahikofukasawa | 11:cef8dc1cf010 | 151 | MSG("#Start sensor succceeded.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 152 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 153 | } |
masahikofukasawa | 10:5c69b067d88a | 154 | |
masahikofukasawa | 10:5c69b067d88a | 155 | AkmSensor::Status Ak9750Ctrl::startSensor(const float sec){ |
masahikofukasawa | 10:5c69b067d88a | 156 | interrupt->fall(this, &Ak9750Ctrl::detectINT); |
masahikofukasawa | 10:5c69b067d88a | 157 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 158 | } |
masahikofukasawa | 10:5c69b067d88a | 159 | |
masahikofukasawa | 10:5c69b067d88a | 160 | AkmSensor::Status Ak9750Ctrl::stopSensor(){ |
masahikofukasawa | 10:5c69b067d88a | 161 | event = false; |
masahikofukasawa | 11:cef8dc1cf010 | 162 | interrupt->fall(NULL); |
masahikofukasawa | 10:5c69b067d88a | 163 | if(ak9750->setOperationMode(AK9750::MODE_STANDBY, filter) != AK9750::SUCCESS) { |
masahikofukasawa | 11:cef8dc1cf010 | 164 | MSG("#Error setOperationMode. AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 165 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 166 | } |
masahikofukasawa | 10:5c69b067d88a | 167 | |
masahikofukasawa | 10:5c69b067d88a | 168 | // read one data to clear INT pin |
masahikofukasawa | 11:cef8dc1cf010 | 169 | AK9750::SensorData data; |
masahikofukasawa | 11:cef8dc1cf010 | 170 | ak9750->getSensorData(&data); |
masahikofukasawa | 10:5c69b067d88a | 171 | |
masahikofukasawa | 10:5c69b067d88a | 172 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 173 | } |
masahikofukasawa | 10:5c69b067d88a | 174 | |
masahikofukasawa | 10:5c69b067d88a | 175 | AkmSensor::Status Ak9750Ctrl::readSensorData(Message* msg){ |
masahikofukasawa | 10:5c69b067d88a | 176 | event = false; |
masahikofukasawa | 10:5c69b067d88a | 177 | |
masahikofukasawa | 10:5c69b067d88a | 178 | AK9750::SensorData data; |
masahikofukasawa | 10:5c69b067d88a | 179 | AK9750::Status status = ak9750->getSensorData(&data); |
masahikofukasawa | 10:5c69b067d88a | 180 | if( status != AK9750::SUCCESS){ |
masahikofukasawa | 11:cef8dc1cf010 | 181 | MSG("#Error getSensorData. AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 182 | return AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 183 | } |
masahikofukasawa | 10:5c69b067d88a | 184 | msg->setCommand(Message::CMD_START_MEASUREMENT); |
masahikofukasawa | 10:5c69b067d88a | 185 | msg->setArgument( 0, data.intStatus.ir13h); |
masahikofukasawa | 10:5c69b067d88a | 186 | msg->setArgument( 1, data.intStatus.ir13l); |
masahikofukasawa | 10:5c69b067d88a | 187 | msg->setArgument( 2, data.intStatus.ir24h); |
masahikofukasawa | 10:5c69b067d88a | 188 | msg->setArgument( 3, data.intStatus.ir24l); |
masahikofukasawa | 10:5c69b067d88a | 189 | msg->setArgument( 4, data.intStatus.drdy); |
masahikofukasawa | 10:5c69b067d88a | 190 | msg->setArgument( 5,(char)((int32_t)(data.ir1) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 191 | msg->setArgument( 6, (char)((int32_t)(data.ir1) & 0x00FF) ); |
masahikofukasawa | 10:5c69b067d88a | 192 | msg->setArgument( 7,(char)((int32_t)(data.ir2) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 193 | msg->setArgument( 8, (char)((int32_t)(data.ir2) & 0x00FF) ); |
masahikofukasawa | 10:5c69b067d88a | 194 | msg->setArgument( 9,(char)((int32_t)(data.ir3) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 195 | msg->setArgument( 10, (char)((int32_t)(data.ir3) & 0x00FF) ); |
masahikofukasawa | 10:5c69b067d88a | 196 | msg->setArgument( 11,(char)((int32_t)(data.ir4) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 197 | msg->setArgument( 12, (char)((int32_t)(data.ir4) & 0x00FF) ); |
masahikofukasawa | 10:5c69b067d88a | 198 | msg->setArgument( 13,(char)((int32_t)(data.temperature) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 199 | msg->setArgument( 14, (char)((int32_t)(data.temperature) & 0x00FF) ); |
masahikofukasawa | 10:5c69b067d88a | 200 | msg->setArgument( 15, data.dor); |
masahikofukasawa | 10:5c69b067d88a | 201 | |
masahikofukasawa | 10:5c69b067d88a | 202 | return AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 203 | } |
masahikofukasawa | 10:5c69b067d88a | 204 | |
masahikofukasawa | 10:5c69b067d88a | 205 | AkmSensor::Status Ak9750Ctrl::requestCommand(Message* in, Message* out){ |
masahikofukasawa | 10:5c69b067d88a | 206 | AkmSensor::Status status = AkmSensor::SUCCESS; |
masahikofukasawa | 10:5c69b067d88a | 207 | Message::Command cmd = in->getCommand(); |
masahikofukasawa | 10:5c69b067d88a | 208 | AK9750::Threshold th; |
masahikofukasawa | 10:5c69b067d88a | 209 | AK9750::Hysteresis hys; |
masahikofukasawa | 10:5c69b067d88a | 210 | AK9750::InterruptStatus interrupt; |
masahikofukasawa | 10:5c69b067d88a | 211 | |
masahikofukasawa | 10:5c69b067d88a | 212 | out->setCommand(cmd); |
masahikofukasawa | 10:5c69b067d88a | 213 | |
masahikofukasawa | 10:5c69b067d88a | 214 | switch(cmd){ |
masahikofukasawa | 10:5c69b067d88a | 215 | case Message::CMD_IR_GET_THRESHOLD: |
masahikofukasawa | 10:5c69b067d88a | 216 | { |
masahikofukasawa | 10:5c69b067d88a | 217 | if (ak9750->getThreshold(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 218 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 219 | MSG("#Failed to set threshold to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 220 | } |
masahikofukasawa | 10:5c69b067d88a | 221 | out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 222 | out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 223 | out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 224 | out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 225 | out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 226 | out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 227 | out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 228 | out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 229 | break; |
masahikofukasawa | 10:5c69b067d88a | 230 | } |
masahikofukasawa | 10:5c69b067d88a | 231 | case Message::CMD_IR_SET_THRESHOLD: |
masahikofukasawa | 10:5c69b067d88a | 232 | { |
masahikofukasawa | 10:5c69b067d88a | 233 | th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1)); |
masahikofukasawa | 10:5c69b067d88a | 234 | th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3)); |
masahikofukasawa | 10:5c69b067d88a | 235 | th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5)); |
masahikofukasawa | 10:5c69b067d88a | 236 | th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7)); |
masahikofukasawa | 10:5c69b067d88a | 237 | if (ak9750->setThreshold(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 238 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 239 | MSG("#Failed to set threshold to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 240 | } |
masahikofukasawa | 10:5c69b067d88a | 241 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 242 | break; |
masahikofukasawa | 10:5c69b067d88a | 243 | } |
masahikofukasawa | 10:5c69b067d88a | 244 | case Message::CMD_IR_GET_HYSTERESIS: |
masahikofukasawa | 10:5c69b067d88a | 245 | { |
masahikofukasawa | 10:5c69b067d88a | 246 | if (ak9750->getHysteresis(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 247 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 248 | MSG("#Failed to set hysteresis to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 249 | } |
masahikofukasawa | 10:5c69b067d88a | 250 | out->setArgument(0,hys.ehys13); |
masahikofukasawa | 10:5c69b067d88a | 251 | out->setArgument(1,hys.ehys24); |
masahikofukasawa | 10:5c69b067d88a | 252 | break; |
masahikofukasawa | 10:5c69b067d88a | 253 | } |
masahikofukasawa | 10:5c69b067d88a | 254 | case Message::CMD_IR_SET_HYSTERESIS: |
masahikofukasawa | 10:5c69b067d88a | 255 | { |
masahikofukasawa | 10:5c69b067d88a | 256 | hys.ehys13 = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 257 | hys.ehys24 = in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 258 | if (ak9750->setHysteresis(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 259 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 260 | MSG("#Failed to set hysteresis to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 261 | } |
masahikofukasawa | 10:5c69b067d88a | 262 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 263 | break; |
masahikofukasawa | 10:5c69b067d88a | 264 | } |
masahikofukasawa | 10:5c69b067d88a | 265 | case Message::CMD_IR_GET_INTERRUPT: |
masahikofukasawa | 10:5c69b067d88a | 266 | { |
masahikofukasawa | 10:5c69b067d88a | 267 | if (ak9750->getInterruptEnable(&interrupt) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 268 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 269 | MSG("#Failed to set hysteresis to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 270 | } |
masahikofukasawa | 10:5c69b067d88a | 271 | out->setArgument(0, interrupt.ir13h); |
masahikofukasawa | 10:5c69b067d88a | 272 | out->setArgument(1, interrupt.ir13l); |
masahikofukasawa | 10:5c69b067d88a | 273 | out->setArgument(2, interrupt.ir24h); |
masahikofukasawa | 10:5c69b067d88a | 274 | out->setArgument(3, interrupt.ir24l); |
masahikofukasawa | 10:5c69b067d88a | 275 | out->setArgument(4, interrupt.drdy); |
masahikofukasawa | 10:5c69b067d88a | 276 | break; |
masahikofukasawa | 10:5c69b067d88a | 277 | } |
masahikofukasawa | 10:5c69b067d88a | 278 | case Message::CMD_IR_SET_INTERRUPT: |
masahikofukasawa | 10:5c69b067d88a | 279 | { |
masahikofukasawa | 10:5c69b067d88a | 280 | interrupt.ir13h = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 281 | interrupt.ir13l = in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 282 | interrupt.ir24h = in->getArgument(2); |
masahikofukasawa | 10:5c69b067d88a | 283 | interrupt.ir24l = in->getArgument(3); |
masahikofukasawa | 10:5c69b067d88a | 284 | interrupt.drdy = in->getArgument(4); |
masahikofukasawa | 10:5c69b067d88a | 285 | if (ak9750->setInterruptEnable(&interrupt) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 286 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 287 | MSG("#Failed to set hysteresis to AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 288 | } |
masahikofukasawa | 10:5c69b067d88a | 289 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 290 | break; |
masahikofukasawa | 10:5c69b067d88a | 291 | } |
masahikofukasawa | 10:5c69b067d88a | 292 | case Message::CMD_IR_GET_OPERATION_MODE: |
masahikofukasawa | 10:5c69b067d88a | 293 | { |
masahikofukasawa | 10:5c69b067d88a | 294 | if(ak9750->getOperationMode(&mode, &filter) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 295 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 296 | MSG("#Error setOperationMode. AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 297 | } |
masahikofukasawa | 10:5c69b067d88a | 298 | out->setArgument(0,(char)mode); |
masahikofukasawa | 10:5c69b067d88a | 299 | out->setArgument(1,(char)filter); |
masahikofukasawa | 10:5c69b067d88a | 300 | break; |
masahikofukasawa | 10:5c69b067d88a | 301 | } |
masahikofukasawa | 10:5c69b067d88a | 302 | case Message::CMD_IR_SET_OPERATION_MODE: |
masahikofukasawa | 10:5c69b067d88a | 303 | { |
masahikofukasawa | 10:5c69b067d88a | 304 | mode = (AK9750::OperationMode)in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 305 | filter = (AK9750::DigitalFilter)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 306 | if(ak9750->setOperationMode(mode, filter) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 307 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 308 | MSG("#Error setOperationMode. AK9750.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 309 | } |
masahikofukasawa | 10:5c69b067d88a | 310 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 311 | break; |
masahikofukasawa | 10:5c69b067d88a | 312 | } |
masahikofukasawa | 10:5c69b067d88a | 313 | case Message::CMD_IR_GET_THRESHOLD_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 314 | { |
masahikofukasawa | 10:5c69b067d88a | 315 | if (ak9750->getThresholdFromEEPROM(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 316 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 317 | MSG("#Failed to set threshold to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 318 | } |
masahikofukasawa | 10:5c69b067d88a | 319 | out->setArgument(0,(char)((int32_t)(th.eth13h) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 320 | out->setArgument(1,(char)((int32_t)(th.eth13h) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 321 | out->setArgument(2,(char)((int32_t)(th.eth13l) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 322 | out->setArgument(3,(char)((int32_t)(th.eth13l) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 323 | out->setArgument(4,(char)((int32_t)(th.eth24h) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 324 | out->setArgument(5,(char)((int32_t)(th.eth24h) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 325 | out->setArgument(6,(char)((int32_t)(th.eth24l) >> 8)); |
masahikofukasawa | 10:5c69b067d88a | 326 | out->setArgument(7,(char)((int32_t)(th.eth24l) & 0x00FF)); |
masahikofukasawa | 10:5c69b067d88a | 327 | break; |
masahikofukasawa | 10:5c69b067d88a | 328 | } |
masahikofukasawa | 10:5c69b067d88a | 329 | case Message::CMD_IR_SET_THRESHOLD_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 330 | { |
masahikofukasawa | 10:5c69b067d88a | 331 | th.eth13h = CONV16I(in->getArgument(0), in->getArgument(1)); |
masahikofukasawa | 10:5c69b067d88a | 332 | th.eth13l = CONV16I(in->getArgument(2), in->getArgument(3)); |
masahikofukasawa | 10:5c69b067d88a | 333 | th.eth24h = CONV16I(in->getArgument(4), in->getArgument(5)); |
masahikofukasawa | 10:5c69b067d88a | 334 | th.eth24l = CONV16I(in->getArgument(6), in->getArgument(7)); |
masahikofukasawa | 10:5c69b067d88a | 335 | if (ak9750->setThresholdToEEPROM(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 336 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 337 | MSG("#Failed to set threshold to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 338 | } |
masahikofukasawa | 10:5c69b067d88a | 339 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 340 | break; |
masahikofukasawa | 10:5c69b067d88a | 341 | } |
masahikofukasawa | 10:5c69b067d88a | 342 | case Message::CMD_IR_GET_HYSTERESIS_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 343 | { |
masahikofukasawa | 10:5c69b067d88a | 344 | if (ak9750->getHysteresisFromEEPROM(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 345 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 346 | MSG("#Failed to set hysteresis to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 347 | } |
masahikofukasawa | 10:5c69b067d88a | 348 | out->setArgument(0,hys.ehys13); |
masahikofukasawa | 10:5c69b067d88a | 349 | out->setArgument(1,hys.ehys24); |
masahikofukasawa | 10:5c69b067d88a | 350 | break; |
masahikofukasawa | 10:5c69b067d88a | 351 | } |
masahikofukasawa | 10:5c69b067d88a | 352 | case Message::CMD_IR_SET_HYSTERESIS_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 353 | { |
masahikofukasawa | 10:5c69b067d88a | 354 | hys.ehys13 = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 355 | hys.ehys24 = in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 356 | if (ak9750->setHysteresisToEEPROM(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 357 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 358 | MSG("#Failed to set hysteresis to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 359 | } |
masahikofukasawa | 10:5c69b067d88a | 360 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 361 | break; |
masahikofukasawa | 10:5c69b067d88a | 362 | } |
masahikofukasawa | 10:5c69b067d88a | 363 | case Message::CMD_IR_GET_INTERRUPT_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 364 | { |
masahikofukasawa | 10:5c69b067d88a | 365 | if (ak9750->getInterruptEnableFromEEPROM(&interrupt) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 366 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 367 | MSG("#Failed to set interrupt to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 368 | } |
masahikofukasawa | 10:5c69b067d88a | 369 | out->setArgument(0, interrupt.ir13h); |
masahikofukasawa | 10:5c69b067d88a | 370 | out->setArgument(1, interrupt.ir13l); |
masahikofukasawa | 10:5c69b067d88a | 371 | out->setArgument(2, interrupt.ir24h); |
masahikofukasawa | 10:5c69b067d88a | 372 | out->setArgument(3, interrupt.ir24l); |
masahikofukasawa | 10:5c69b067d88a | 373 | out->setArgument(4, interrupt.drdy); |
masahikofukasawa | 10:5c69b067d88a | 374 | break; |
masahikofukasawa | 10:5c69b067d88a | 375 | } |
masahikofukasawa | 10:5c69b067d88a | 376 | case Message::CMD_IR_SET_INTERRUPT_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 377 | { |
masahikofukasawa | 10:5c69b067d88a | 378 | interrupt.ir13h = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 379 | interrupt.ir13l = in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 380 | interrupt.ir24h = in->getArgument(2); |
masahikofukasawa | 10:5c69b067d88a | 381 | interrupt.ir24l = in->getArgument(2); |
masahikofukasawa | 10:5c69b067d88a | 382 | interrupt.drdy = in->getArgument(4); |
masahikofukasawa | 10:5c69b067d88a | 383 | if (ak9750->setInterruptEnableToEEPROM(&interrupt) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 384 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 385 | MSG("#Failed to set interrupt to AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 386 | } |
masahikofukasawa | 10:5c69b067d88a | 387 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 388 | break; |
masahikofukasawa | 10:5c69b067d88a | 389 | } |
masahikofukasawa | 10:5c69b067d88a | 390 | case Message::CMD_IR_GET_OPERATION_MODE_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 391 | { |
masahikofukasawa | 10:5c69b067d88a | 392 | if(ak9750->getOperationModeFromEEPROM(&mode, &filter) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 393 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 394 | MSG("#Error getOperationMode. AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 395 | } |
masahikofukasawa | 10:5c69b067d88a | 396 | out->setArgument(0,(char)mode); |
masahikofukasawa | 10:5c69b067d88a | 397 | out->setArgument(1,(char)filter); |
masahikofukasawa | 10:5c69b067d88a | 398 | break; |
masahikofukasawa | 10:5c69b067d88a | 399 | } |
masahikofukasawa | 10:5c69b067d88a | 400 | case Message::CMD_IR_SET_OPERATION_MODE_EEPROM: |
masahikofukasawa | 10:5c69b067d88a | 401 | { |
masahikofukasawa | 10:5c69b067d88a | 402 | mode = (AK9750::OperationMode)in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 403 | filter = (AK9750::DigitalFilter)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 404 | if(ak9750->setOperationModeToEEPROM(mode, filter) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 405 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 406 | MSG("#Error setOperationMode. AK9750(EEPROM).\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 407 | } |
masahikofukasawa | 10:5c69b067d88a | 408 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 409 | break; |
masahikofukasawa | 10:5c69b067d88a | 410 | } |
masahikofukasawa | 10:5c69b067d88a | 411 | case Message::CMD_REG_WRITE: |
masahikofukasawa | 10:5c69b067d88a | 412 | { |
masahikofukasawa | 10:5c69b067d88a | 413 | char address = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 414 | int len = (int)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 415 | const char data = in->getArgument(2); |
masahikofukasawa | 10:5c69b067d88a | 416 | if( ak9750->write(address, &data, len) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 417 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 418 | MSG("#Error register write.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 419 | } |
masahikofukasawa | 10:5c69b067d88a | 420 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 421 | break; |
masahikofukasawa | 10:5c69b067d88a | 422 | } |
masahikofukasawa | 10:5c69b067d88a | 423 | case Message::CMD_REG_WRITEN: |
masahikofukasawa | 10:5c69b067d88a | 424 | { |
masahikofukasawa | 10:5c69b067d88a | 425 | char address = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 426 | int len = (int)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 427 | char data[len]; |
masahikofukasawa | 10:5c69b067d88a | 428 | for(int i=0; i<len; i++){ |
masahikofukasawa | 10:5c69b067d88a | 429 | data[i] = in->getArgument(i); |
masahikofukasawa | 10:5c69b067d88a | 430 | } |
masahikofukasawa | 10:5c69b067d88a | 431 | if( ak9750->write(address, data, len) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 432 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 433 | MSG("#Error register write.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 434 | } |
masahikofukasawa | 10:5c69b067d88a | 435 | out->setArgument(0,(char)status); |
masahikofukasawa | 10:5c69b067d88a | 436 | break; |
masahikofukasawa | 10:5c69b067d88a | 437 | } |
masahikofukasawa | 10:5c69b067d88a | 438 | case Message::CMD_REG_READ: |
masahikofukasawa | 10:5c69b067d88a | 439 | { |
masahikofukasawa | 10:5c69b067d88a | 440 | char address = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 441 | int len = (int)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 442 | char data; |
masahikofukasawa | 10:5c69b067d88a | 443 | if( ak9750->read(address, &data, len) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 444 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 445 | MSG("#Error register read.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 446 | } |
masahikofukasawa | 10:5c69b067d88a | 447 | out->setArgument(0,data); |
masahikofukasawa | 10:5c69b067d88a | 448 | break; |
masahikofukasawa | 10:5c69b067d88a | 449 | } |
masahikofukasawa | 10:5c69b067d88a | 450 | case Message::CMD_REG_READN: |
masahikofukasawa | 10:5c69b067d88a | 451 | { |
masahikofukasawa | 10:5c69b067d88a | 452 | char address = in->getArgument(0); |
masahikofukasawa | 10:5c69b067d88a | 453 | int len = (int)in->getArgument(1); |
masahikofukasawa | 10:5c69b067d88a | 454 | char data[len]; |
masahikofukasawa | 10:5c69b067d88a | 455 | if( ak9750->read(address, data, len) != AK9750::SUCCESS) { |
masahikofukasawa | 10:5c69b067d88a | 456 | status = AkmSensor::ERROR; |
masahikofukasawa | 11:cef8dc1cf010 | 457 | MSG("#Error register read.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 458 | } |
masahikofukasawa | 10:5c69b067d88a | 459 | for(int i=0; i<len; i++){ |
masahikofukasawa | 10:5c69b067d88a | 460 | out->setArgument(i, data[i]); |
masahikofukasawa | 10:5c69b067d88a | 461 | } |
masahikofukasawa | 10:5c69b067d88a | 462 | break; |
masahikofukasawa | 10:5c69b067d88a | 463 | } |
masahikofukasawa | 10:5c69b067d88a | 464 | default: |
masahikofukasawa | 10:5c69b067d88a | 465 | { |
masahikofukasawa | 11:cef8dc1cf010 | 466 | MSG("#Error no command.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 467 | status = AkmSensor::ERROR; |
masahikofukasawa | 10:5c69b067d88a | 468 | break; |
masahikofukasawa | 10:5c69b067d88a | 469 | } |
masahikofukasawa | 10:5c69b067d88a | 470 | } |
masahikofukasawa | 10:5c69b067d88a | 471 | return status; |
masahikofukasawa | 10:5c69b067d88a | 472 | } |
masahikofukasawa | 13:d008249f0359 | 473 | char* Ak9750Ctrl::getSensorName(){ |
masahikofukasawa | 13:d008249f0359 | 474 | return sensorName; |
masahikofukasawa | 13:d008249f0359 | 475 | } |