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