Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
masahikofukasawa 10:5c69b067d88a 1 #include "ak9752ctrl.h"
masahikofukasawa 10:5c69b067d88a 2
masahikofukasawa 10:5c69b067d88a 3 #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low)))
masahikofukasawa 10:5c69b067d88a 4
masahikofukasawa 10:5c69b067d88a 5 /**
masahikofukasawa 10:5c69b067d88a 6 * Constructor.
masahikofukasawa 10:5c69b067d88a 7 *
masahikofukasawa 10:5c69b067d88a 8 */
masahikofukasawa 27:41aa9fb23a2f 9 Ak9752Ctrl::Ak9752Ctrl() : AkmSensor(){
masahikofukasawa 10:5c69b067d88a 10 ak9752 = NULL;
masahikofukasawa 10:5c69b067d88a 11 }
masahikofukasawa 10:5c69b067d88a 12
masahikofukasawa 10:5c69b067d88a 13 /**
masahikofukasawa 10:5c69b067d88a 14 * Destructor.
masahikofukasawa 10:5c69b067d88a 15 *
masahikofukasawa 10:5c69b067d88a 16 */
masahikofukasawa 10:5c69b067d88a 17 Ak9752Ctrl::~Ak9752Ctrl(){
masahikofukasawa 10:5c69b067d88a 18 if (ak9752) delete ak9752;
masahikofukasawa 10:5c69b067d88a 19 }
masahikofukasawa 10:5c69b067d88a 20
masahikofukasawa 10:5c69b067d88a 21 AkmSensor::Status Ak9752Ctrl::init(const uint8_t id, const uint8_t subid){
masahikofukasawa 10:5c69b067d88a 22 primaryId = id;
masahikofukasawa 10:5c69b067d88a 23 subId = subid;
masahikofukasawa 10:5c69b067d88a 24
masahikofukasawa 10:5c69b067d88a 25 I2C* i2c = new I2C(I2C_SDA,I2C_SCL);
masahikofukasawa 40:42e48427e4b7 26 i2c->frequency(I2C_SPEED);
masahikofukasawa 10:5c69b067d88a 27
masahikofukasawa 10:5c69b067d88a 28 if(subId == SUB_ID_AK9752){
masahikofukasawa 10:5c69b067d88a 29 ak9752 = new AK9752();
masahikofukasawa 13:d008249f0359 30 sensorName = "AK9752";
masahikofukasawa 10:5c69b067d88a 31 }
masahikofukasawa 10:5c69b067d88a 32 else{
masahikofukasawa 10:5c69b067d88a 33 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 34 }
masahikofukasawa 10:5c69b067d88a 35
masahikofukasawa 10:5c69b067d88a 36 bool foundSensor = false;
masahikofukasawa 10:5c69b067d88a 37
masahikofukasawa 10:5c69b067d88a 38 AK9752::SlaveAddress slaveAddr[]
masahikofukasawa 10:5c69b067d88a 39 = { AK9752::SLAVE_ADDR_1 };
masahikofukasawa 10:5c69b067d88a 40
masahikofukasawa 47:221ec4b404ec 41 for(unsigned int i=0; i<sizeof(slaveAddr); i++)
masahikofukasawa 10:5c69b067d88a 42 {
masahikofukasawa 10:5c69b067d88a 43 ak9752->init(i2c, slaveAddr[i]);
masahikofukasawa 10:5c69b067d88a 44 // Checks connectivity
masahikofukasawa 29:b488d2c89fba 45 if(ak9752->checkConnection() == AK9752::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 46 // found
masahikofukasawa 10:5c69b067d88a 47 foundSensor = true;
masahikofukasawa 10:5c69b067d88a 48 break;
masahikofukasawa 10:5c69b067d88a 49 }
masahikofukasawa 10:5c69b067d88a 50 }
masahikofukasawa 10:5c69b067d88a 51 if(foundSensor != true) return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 52
masahikofukasawa 10:5c69b067d88a 53 // reset
masahikofukasawa 10:5c69b067d88a 54 if (ak9752->reset() != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 55 MSG("#Error: Failed to reset AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 56 }
masahikofukasawa 10:5c69b067d88a 57
masahikofukasawa 11:cef8dc1cf010 58 MSG("#Init success AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 59 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 60 }
masahikofukasawa 10:5c69b067d88a 61
masahikofukasawa 29:b488d2c89fba 62 void Ak9752Ctrl::setEvent(){
masahikofukasawa 29:b488d2c89fba 63 // MSG("#setEvent() in %s.\r\n",sensorName);
masahikofukasawa 29:b488d2c89fba 64 AK9752::Status status = ak9752->isDataReady();
masahikofukasawa 29:b488d2c89fba 65 if( status == AK9752::DATA_READY ) base::setEvent();
masahikofukasawa 10:5c69b067d88a 66 }
masahikofukasawa 10:5c69b067d88a 67
masahikofukasawa 10:5c69b067d88a 68 AkmSensor::Status Ak9752Ctrl::startSensor(){
masahikofukasawa 11:cef8dc1cf010 69 // read one data to clear INT pin
masahikofukasawa 11:cef8dc1cf010 70 AK9752::SensorData data;
masahikofukasawa 11:cef8dc1cf010 71 ak9752->getSensorData(&data);
masahikofukasawa 11:cef8dc1cf010 72
masahikofukasawa 11:cef8dc1cf010 73 // set operation mode
masahikofukasawa 10:5c69b067d88a 74 if(ak9752->setOperationMode(mode,fc_tmp, fc_ir) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 75 MSG("#Error: Start sensor failed %s\r\n", sensorName);
masahikofukasawa 10:5c69b067d88a 76 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 77 }
masahikofukasawa 10:5c69b067d88a 78
masahikofukasawa 29:b488d2c89fba 79 MSG("#Start sensor %s.\r\n",sensorName);
masahikofukasawa 10:5c69b067d88a 80 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 81 }
masahikofukasawa 10:5c69b067d88a 82
masahikofukasawa 10:5c69b067d88a 83 AkmSensor::Status Ak9752Ctrl::startSensor(const float sec){
masahikofukasawa 10:5c69b067d88a 84 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 85 }
masahikofukasawa 10:5c69b067d88a 86
masahikofukasawa 10:5c69b067d88a 87 AkmSensor::Status Ak9752Ctrl::stopSensor(){
masahikofukasawa 29:b488d2c89fba 88 AkmSensor::clearEvent();
masahikofukasawa 29:b488d2c89fba 89
masahikofukasawa 10:5c69b067d88a 90 if(ak9752->setOperationMode(AK9752::MODE_STANDBY, fc_tmp, fc_ir) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 91 MSG("#Error: setOperationMode. AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 92 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 93 }
masahikofukasawa 10:5c69b067d88a 94
masahikofukasawa 10:5c69b067d88a 95 // read one data to clear INT pin
masahikofukasawa 11:cef8dc1cf010 96 AK9752::SensorData data;
masahikofukasawa 11:cef8dc1cf010 97 ak9752->getSensorData(&data);
masahikofukasawa 10:5c69b067d88a 98
masahikofukasawa 10:5c69b067d88a 99 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 100 }
masahikofukasawa 10:5c69b067d88a 101
masahikofukasawa 10:5c69b067d88a 102 AkmSensor::Status Ak9752Ctrl::readSensorData(Message* msg){
masahikofukasawa 29:b488d2c89fba 103 AkmSensor::clearEvent();
masahikofukasawa 10:5c69b067d88a 104
masahikofukasawa 10:5c69b067d88a 105 AK9752::SensorData data;
masahikofukasawa 10:5c69b067d88a 106 AK9752::Status status = ak9752->getSensorData(&data);
masahikofukasawa 10:5c69b067d88a 107 if( status != AK9752::SUCCESS){
masahikofukasawa 39:3821886c902e 108 MSG("#Error: getSensorData. AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 109 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 110 }
masahikofukasawa 10:5c69b067d88a 111 msg->setCommand(Message::CMD_START_MEASUREMENT);
masahikofukasawa 10:5c69b067d88a 112 msg->setArgument( 0, data.intStatus.irh);
masahikofukasawa 10:5c69b067d88a 113 msg->setArgument( 1, data.intStatus.irl);
masahikofukasawa 10:5c69b067d88a 114 msg->setArgument( 2, data.intStatus.tmph);
masahikofukasawa 10:5c69b067d88a 115 msg->setArgument( 3, data.intStatus.tmpl);
masahikofukasawa 10:5c69b067d88a 116 msg->setArgument( 4, data.intStatus.dr);
masahikofukasawa 10:5c69b067d88a 117 msg->setArgument( 5,(char)((int32_t)(data.ir) >> 8));
masahikofukasawa 10:5c69b067d88a 118 msg->setArgument( 6, (char)((int32_t)(data.ir) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 119 msg->setArgument( 7,(char)((int32_t)(data.temperature) >> 8));
masahikofukasawa 10:5c69b067d88a 120 msg->setArgument( 8, (char)((int32_t)(data.temperature) & 0x00FF) );
masahikofukasawa 10:5c69b067d88a 121 msg->setArgument( 9, data.dor);
masahikofukasawa 10:5c69b067d88a 122
masahikofukasawa 10:5c69b067d88a 123 return AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 124 }
masahikofukasawa 10:5c69b067d88a 125
masahikofukasawa 10:5c69b067d88a 126 AkmSensor::Status Ak9752Ctrl::requestCommand(Message* in, Message* out){
masahikofukasawa 10:5c69b067d88a 127 AkmSensor::Status status = AkmSensor::SUCCESS;
masahikofukasawa 10:5c69b067d88a 128 Message::Command cmd = in->getCommand();
masahikofukasawa 10:5c69b067d88a 129 AK9752::Threshold th;
masahikofukasawa 29:b488d2c89fba 130 AK9752::InterruptStatus interruptStatus;
masahikofukasawa 10:5c69b067d88a 131
masahikofukasawa 10:5c69b067d88a 132 out->setCommand(cmd);
masahikofukasawa 10:5c69b067d88a 133
masahikofukasawa 10:5c69b067d88a 134 switch(cmd){
masahikofukasawa 10:5c69b067d88a 135 case Message::CMD_IR_GET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 136 {
masahikofukasawa 10:5c69b067d88a 137 if (ak9752->getThreshold(&th) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 138 MSG("#Error: Failed to set threshold to AK9752.\r\n");
masahikofukasawa 15:1238993fd75f 139 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 140 }
masahikofukasawa 10:5c69b067d88a 141 out->setArgument(0,(char)((int32_t)(th.thirh) >> 8));
masahikofukasawa 10:5c69b067d88a 142 out->setArgument(1,(char)((int32_t)(th.thirh) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 143 out->setArgument(2,(char)((int32_t)(th.thirl) >> 8));
masahikofukasawa 10:5c69b067d88a 144 out->setArgument(3,(char)((int32_t)(th.thirl) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 145 out->setArgument(4,(char)((int32_t)(th.thtmph) >> 8));
masahikofukasawa 10:5c69b067d88a 146 out->setArgument(5,(char)((int32_t)(th.thtmph) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 147 out->setArgument(6,(char)((int32_t)(th.thtmpl) >> 8));
masahikofukasawa 10:5c69b067d88a 148 out->setArgument(7,(char)((int32_t)(th.thtmpl) & 0x00FF));
masahikofukasawa 10:5c69b067d88a 149 break;
masahikofukasawa 10:5c69b067d88a 150 }
masahikofukasawa 10:5c69b067d88a 151 case Message::CMD_IR_SET_THRESHOLD:
masahikofukasawa 10:5c69b067d88a 152 {
masahikofukasawa 10:5c69b067d88a 153 th.thirh = CONV16I(in->getArgument(0), in->getArgument(1));
masahikofukasawa 10:5c69b067d88a 154 th.thirl = CONV16I(in->getArgument(2), in->getArgument(3));
masahikofukasawa 10:5c69b067d88a 155 th.thtmph = CONV16I(in->getArgument(4), in->getArgument(5));
masahikofukasawa 10:5c69b067d88a 156 th.thtmpl = CONV16I(in->getArgument(6), in->getArgument(7));
masahikofukasawa 10:5c69b067d88a 157 if (ak9752->setThreshold(&th) != AK9752::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 158 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 159 MSG("#Error: Failed to set threshold to AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 160 }
masahikofukasawa 10:5c69b067d88a 161 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 162 break;
masahikofukasawa 10:5c69b067d88a 163 }
masahikofukasawa 10:5c69b067d88a 164 case Message::CMD_IR_GET_INTERRUPT:
masahikofukasawa 10:5c69b067d88a 165 {
masahikofukasawa 29:b488d2c89fba 166 if (ak9752->getInterruptEnable(&interruptStatus) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 167 MSG("#Error: Failed to set hysteresis to AK9752.\r\n");
masahikofukasawa 15:1238993fd75f 168 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 169 }
masahikofukasawa 29:b488d2c89fba 170 out->setArgument(0, interruptStatus.irh);
masahikofukasawa 29:b488d2c89fba 171 out->setArgument(1, interruptStatus.irl);
masahikofukasawa 29:b488d2c89fba 172 out->setArgument(2, interruptStatus.tmph);
masahikofukasawa 29:b488d2c89fba 173 out->setArgument(3, interruptStatus.tmpl);
masahikofukasawa 29:b488d2c89fba 174 out->setArgument(4, interruptStatus.dr);
masahikofukasawa 10:5c69b067d88a 175 break;
masahikofukasawa 10:5c69b067d88a 176 }
masahikofukasawa 10:5c69b067d88a 177 case Message::CMD_IR_SET_INTERRUPT:
masahikofukasawa 10:5c69b067d88a 178 {
masahikofukasawa 29:b488d2c89fba 179 interruptStatus.irh = in->getArgument(0);
masahikofukasawa 29:b488d2c89fba 180 interruptStatus.irl = in->getArgument(1);
masahikofukasawa 29:b488d2c89fba 181 interruptStatus.tmph = in->getArgument(2);
masahikofukasawa 29:b488d2c89fba 182 interruptStatus.tmpl = in->getArgument(3);
masahikofukasawa 29:b488d2c89fba 183 interruptStatus.dr = in->getArgument(4);
masahikofukasawa 29:b488d2c89fba 184 if (ak9752->setInterruptEnable(&interruptStatus) != AK9752::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 185 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 186 MSG("#Error: Failed to set hysteresis to AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 187 }
masahikofukasawa 10:5c69b067d88a 188 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 189 break;
masahikofukasawa 10:5c69b067d88a 190 }
masahikofukasawa 10:5c69b067d88a 191 case Message::CMD_IR_GET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 192 {
masahikofukasawa 10:5c69b067d88a 193 if(ak9752->getOperationMode(&mode, &fc_tmp, &fc_ir) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 194 MSG("#Error: getOperationMode. AK9752.\r\n");
masahikofukasawa 15:1238993fd75f 195 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 196 }
masahikofukasawa 10:5c69b067d88a 197 out->setArgument(0,(char)mode);
masahikofukasawa 10:5c69b067d88a 198 out->setArgument(1,(char)fc_tmp);
masahikofukasawa 10:5c69b067d88a 199 out->setArgument(2,(char)fc_ir);
masahikofukasawa 10:5c69b067d88a 200 break;
masahikofukasawa 10:5c69b067d88a 201 }
masahikofukasawa 10:5c69b067d88a 202 case Message::CMD_IR_SET_OPERATION_MODE:
masahikofukasawa 10:5c69b067d88a 203 {
masahikofukasawa 10:5c69b067d88a 204 mode = (AK9752::OperationMode)in->getArgument(0);
masahikofukasawa 10:5c69b067d88a 205 fc_tmp = (AK9752::FcTmp)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 206 fc_ir = (AK9752::FcIr)in->getArgument(2);
masahikofukasawa 10:5c69b067d88a 207 if(ak9752->setOperationMode(mode, fc_tmp, fc_ir) != AK9752::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 208 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 209 MSG("#Error: setOperationMode. AK9752.\r\n");
masahikofukasawa 10:5c69b067d88a 210 }
masahikofukasawa 10:5c69b067d88a 211 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 212 break;
masahikofukasawa 10:5c69b067d88a 213 }
masahikofukasawa 10:5c69b067d88a 214 case Message::CMD_REG_WRITE:
masahikofukasawa 10:5c69b067d88a 215 case Message::CMD_REG_WRITEN:
masahikofukasawa 10:5c69b067d88a 216 {
masahikofukasawa 10:5c69b067d88a 217 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 218 const int len = (int)in->getArgument(1);
masahikofukasawa 15:1238993fd75f 219 if(in->getArgNum() != len+2){
masahikofukasawa 39:3821886c902e 220 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 221 status = AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 222 out->setArgument(0,(char)status);
masahikofukasawa 15:1238993fd75f 223 return status;
masahikofukasawa 15:1238993fd75f 224 }
masahikofukasawa 15:1238993fd75f 225
masahikofukasawa 10:5c69b067d88a 226 char data[len];
masahikofukasawa 10:5c69b067d88a 227 for(int i=0; i<len; i++){
masahikofukasawa 15:1238993fd75f 228 data[i] = in->getArgument(i+2);
masahikofukasawa 10:5c69b067d88a 229 }
masahikofukasawa 10:5c69b067d88a 230 if( ak9752->write(address, data, len) != AK9752::SUCCESS) {
masahikofukasawa 10:5c69b067d88a 231 status = AkmSensor::ERROR;
masahikofukasawa 39:3821886c902e 232 MSG("#Error: register write.\r\n");
masahikofukasawa 10:5c69b067d88a 233 }
masahikofukasawa 10:5c69b067d88a 234 out->setArgument(0,(char)status);
masahikofukasawa 10:5c69b067d88a 235 break;
masahikofukasawa 10:5c69b067d88a 236 }
masahikofukasawa 10:5c69b067d88a 237 case Message::CMD_REG_READ:
masahikofukasawa 10:5c69b067d88a 238 case Message::CMD_REG_READN:
masahikofukasawa 10:5c69b067d88a 239 {
masahikofukasawa 15:1238993fd75f 240 if(in->getArgNum() != 2){
masahikofukasawa 39:3821886c902e 241 MSG("#Error: argument num. Args=%d\r\n",in->getArgNum());
masahikofukasawa 15:1238993fd75f 242 return AkmSensor::ERROR;
masahikofukasawa 15:1238993fd75f 243 }
masahikofukasawa 15:1238993fd75f 244
masahikofukasawa 10:5c69b067d88a 245 char address = in->getArgument(0);
masahikofukasawa 46:5938ad2039b0 246 const int len = (int)in->getArgument(1);
masahikofukasawa 10:5c69b067d88a 247 char data[len];
masahikofukasawa 10:5c69b067d88a 248 if( ak9752->read(address, data, len) != AK9752::SUCCESS) {
masahikofukasawa 39:3821886c902e 249 MSG("#Error: register read.\r\n");
masahikofukasawa 15:1238993fd75f 250 return AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 251 }
masahikofukasawa 10:5c69b067d88a 252 for(int i=0; i<len; i++){
masahikofukasawa 10:5c69b067d88a 253 out->setArgument(i, data[i]);
masahikofukasawa 10:5c69b067d88a 254 }
masahikofukasawa 10:5c69b067d88a 255 break;
masahikofukasawa 10:5c69b067d88a 256 }
masahikofukasawa 10:5c69b067d88a 257 default:
masahikofukasawa 10:5c69b067d88a 258 {
masahikofukasawa 39:3821886c902e 259 MSG("#Error: No command.\r\n");
masahikofukasawa 10:5c69b067d88a 260 status = AkmSensor::ERROR;
masahikofukasawa 10:5c69b067d88a 261 break;
masahikofukasawa 10:5c69b067d88a 262 }
masahikofukasawa 10:5c69b067d88a 263 }
masahikofukasawa 10:5c69b067d88a 264 return status;
masahikofukasawa 10:5c69b067d88a 265 }