Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp@0:7a00359e701e, 2016-04-28 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Thu Apr 28 21:12:04 2016 +0000
- Revision:
- 0:7a00359e701e
- Child:
- 1:b46b8653331f
First commit;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
masahikofukasawa | 0:7a00359e701e | 1 | #include "ble/services/UARTService.h" |
masahikofukasawa | 0:7a00359e701e | 2 | #include "akmsensormanager.h" |
masahikofukasawa | 0:7a00359e701e | 3 | #include "akmanglesensor.h" |
masahikofukasawa | 0:7a00359e701e | 4 | #include "akmhallswitch.h" |
masahikofukasawa | 0:7a00359e701e | 5 | #include "akmlinearsensor.h" |
masahikofukasawa | 0:7a00359e701e | 6 | #include "akmakd.h" |
masahikofukasawa | 0:7a00359e701e | 7 | #include "debug.h" |
masahikofukasawa | 0:7a00359e701e | 8 | #include "Message.h" |
masahikofukasawa | 0:7a00359e701e | 9 | |
masahikofukasawa | 0:7a00359e701e | 10 | #define FIRMWARE_VERSION 0x02 |
masahikofukasawa | 0:7a00359e701e | 11 | #define MAGNETOMETER_ID 0x0A |
masahikofukasawa | 0:7a00359e701e | 12 | |
masahikofukasawa | 0:7a00359e701e | 13 | #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) |
masahikofukasawa | 0:7a00359e701e | 14 | |
masahikofukasawa | 0:7a00359e701e | 15 | |
masahikofukasawa | 0:7a00359e701e | 16 | static char* AKM_PRIMARY_ID_STR[] = { |
masahikofukasawa | 0:7a00359e701e | 17 | "AKD Daughter Cards(SPI)", // Temporary |
masahikofukasawa | 0:7a00359e701e | 18 | "Switch, Unipolar", |
masahikofukasawa | 0:7a00359e701e | 19 | "Switch, Onmipolar", |
masahikofukasawa | 0:7a00359e701e | 20 | "Latch, Bipolar", |
masahikofukasawa | 0:7a00359e701e | 21 | "Switch, Dual Output", |
masahikofukasawa | 0:7a00359e701e | 22 | "Onechip Encoder", |
masahikofukasawa | 0:7a00359e701e | 23 | "TBD1", |
masahikofukasawa | 0:7a00359e701e | 24 | "TBD2", |
masahikofukasawa | 0:7a00359e701e | 25 | "Linear Sensor Legacy", |
masahikofukasawa | 0:7a00359e701e | 26 | "Current Sensor", |
masahikofukasawa | 0:7a00359e701e | 27 | "MISC(Analog)", |
masahikofukasawa | 0:7a00359e701e | 28 | "Linear Sensor", |
masahikofukasawa | 0:7a00359e701e | 29 | "TBD3", |
masahikofukasawa | 0:7a00359e701e | 30 | "TBD4", |
masahikofukasawa | 0:7a00359e701e | 31 | "Angle Sensor(SPI)", // Temporary |
masahikofukasawa | 0:7a00359e701e | 32 | "AKD Daughter Cards(I2C)", |
masahikofukasawa | 0:7a00359e701e | 33 | }; |
masahikofukasawa | 0:7a00359e701e | 34 | |
masahikofukasawa | 0:7a00359e701e | 35 | AkmSensorManager::AkmSensorManager(SerialNano* com, UARTService* service) |
masahikofukasawa | 0:7a00359e701e | 36 | { |
masahikofukasawa | 0:7a00359e701e | 37 | serial = com; |
masahikofukasawa | 0:7a00359e701e | 38 | uartService = service; |
masahikofukasawa | 0:7a00359e701e | 39 | isEnabledBle = true; |
masahikofukasawa | 0:7a00359e701e | 40 | isEnabledUsb = true; |
masahikofukasawa | 0:7a00359e701e | 41 | eventCommandReceived = false; |
masahikofukasawa | 0:7a00359e701e | 42 | eventConnected = false; |
masahikofukasawa | 0:7a00359e701e | 43 | eventDisconnected = false; |
masahikofukasawa | 0:7a00359e701e | 44 | } |
masahikofukasawa | 0:7a00359e701e | 45 | |
masahikofukasawa | 0:7a00359e701e | 46 | AkmSensorManager::Status AkmSensorManager::init() |
masahikofukasawa | 0:7a00359e701e | 47 | { |
masahikofukasawa | 0:7a00359e701e | 48 | sensor = AkmSensorManager::getAkmSensor(); |
masahikofukasawa | 0:7a00359e701e | 49 | if(sensor == NULL) return AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 50 | return AkmSensorManager::SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 51 | } |
masahikofukasawa | 0:7a00359e701e | 52 | |
masahikofukasawa | 0:7a00359e701e | 53 | void AkmSensorManager::setEventConnected() |
masahikofukasawa | 0:7a00359e701e | 54 | { |
masahikofukasawa | 0:7a00359e701e | 55 | eventConnected = true; |
masahikofukasawa | 0:7a00359e701e | 56 | } |
masahikofukasawa | 0:7a00359e701e | 57 | |
masahikofukasawa | 0:7a00359e701e | 58 | void AkmSensorManager::setEventDisconnected() |
masahikofukasawa | 0:7a00359e701e | 59 | { |
masahikofukasawa | 0:7a00359e701e | 60 | eventDisconnected = true; |
masahikofukasawa | 0:7a00359e701e | 61 | } |
masahikofukasawa | 0:7a00359e701e | 62 | |
masahikofukasawa | 0:7a00359e701e | 63 | AkmSensor* AkmSensorManager::getAkmSensor() |
masahikofukasawa | 0:7a00359e701e | 64 | { |
masahikofukasawa | 0:7a00359e701e | 65 | AkmSensor* sensor = NULL; |
masahikofukasawa | 0:7a00359e701e | 66 | |
masahikofukasawa | 0:7a00359e701e | 67 | // primary id check |
masahikofukasawa | 0:7a00359e701e | 68 | id = AkmSensorManager::getId(ANALOG_SENSOR_ID,4); |
masahikofukasawa | 0:7a00359e701e | 69 | |
masahikofukasawa | 0:7a00359e701e | 70 | // secondary id check |
masahikofukasawa | 0:7a00359e701e | 71 | switch(id){ |
masahikofukasawa | 0:7a00359e701e | 72 | case AkmSensor::AKM_PRIMARY_ID_AKD_I2C: |
masahikofukasawa | 0:7a00359e701e | 73 | subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,5); // 5bit sub id |
masahikofukasawa | 0:7a00359e701e | 74 | AkmAkd* akd; |
masahikofukasawa | 0:7a00359e701e | 75 | akd = new AkmAkd(); |
masahikofukasawa | 0:7a00359e701e | 76 | sensor = akd; |
masahikofukasawa | 0:7a00359e701e | 77 | break; |
masahikofukasawa | 0:7a00359e701e | 78 | |
masahikofukasawa | 0:7a00359e701e | 79 | case AkmSensor::AKM_PRIMARY_ID_AKD_SPI: // Temporary |
masahikofukasawa | 0:7a00359e701e | 80 | case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR: |
masahikofukasawa | 0:7a00359e701e | 81 | subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id |
masahikofukasawa | 0:7a00359e701e | 82 | AkmAngleSensor* angleSensor; |
masahikofukasawa | 0:7a00359e701e | 83 | angleSensor = new AkmAngleSensor(); |
masahikofukasawa | 0:7a00359e701e | 84 | sensor = angleSensor; |
masahikofukasawa | 0:7a00359e701e | 85 | break; |
masahikofukasawa | 0:7a00359e701e | 86 | |
masahikofukasawa | 0:7a00359e701e | 87 | case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR: |
masahikofukasawa | 0:7a00359e701e | 88 | case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR: |
masahikofukasawa | 0:7a00359e701e | 89 | case AkmSensor::AKM_PRIMARY_ID_LATCH: |
masahikofukasawa | 0:7a00359e701e | 90 | case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT: |
masahikofukasawa | 0:7a00359e701e | 91 | case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER: |
masahikofukasawa | 0:7a00359e701e | 92 | subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id |
masahikofukasawa | 0:7a00359e701e | 93 | AkmHallSwitch* hallswitch; |
masahikofukasawa | 0:7a00359e701e | 94 | hallswitch = new AkmHallSwitch(); |
masahikofukasawa | 0:7a00359e701e | 95 | sensor = hallswitch; |
masahikofukasawa | 0:7a00359e701e | 96 | break; |
masahikofukasawa | 0:7a00359e701e | 97 | |
masahikofukasawa | 0:7a00359e701e | 98 | case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY: |
masahikofukasawa | 0:7a00359e701e | 99 | subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id |
masahikofukasawa | 0:7a00359e701e | 100 | AkmLinearSensor* linearsensor; |
masahikofukasawa | 0:7a00359e701e | 101 | linearsensor = new AkmLinearSensor(); |
masahikofukasawa | 0:7a00359e701e | 102 | sensor = linearsensor; |
masahikofukasawa | 0:7a00359e701e | 103 | break; |
masahikofukasawa | 0:7a00359e701e | 104 | |
masahikofukasawa | 0:7a00359e701e | 105 | default: |
masahikofukasawa | 0:7a00359e701e | 106 | subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id |
masahikofukasawa | 0:7a00359e701e | 107 | MSG("#Can't find ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); |
masahikofukasawa | 0:7a00359e701e | 108 | return NULL; // couldn't find |
masahikofukasawa | 0:7a00359e701e | 109 | } |
masahikofukasawa | 0:7a00359e701e | 110 | |
masahikofukasawa | 0:7a00359e701e | 111 | if(sensor->init(id, subId) != AkmSensor::SUCCESS){ |
masahikofukasawa | 0:7a00359e701e | 112 | MSG("#Can't find ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); |
masahikofukasawa | 0:7a00359e701e | 113 | return NULL; // couldn't find |
masahikofukasawa | 0:7a00359e701e | 114 | } |
masahikofukasawa | 0:7a00359e701e | 115 | |
masahikofukasawa | 0:7a00359e701e | 116 | MSG("#ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); |
masahikofukasawa | 0:7a00359e701e | 117 | return sensor; |
masahikofukasawa | 0:7a00359e701e | 118 | } |
masahikofukasawa | 0:7a00359e701e | 119 | |
masahikofukasawa | 0:7a00359e701e | 120 | AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){ |
masahikofukasawa | 0:7a00359e701e | 121 | // Construct message |
masahikofukasawa | 0:7a00359e701e | 122 | Status status = SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 123 | |
masahikofukasawa | 0:7a00359e701e | 124 | Message::Status st; |
masahikofukasawa | 0:7a00359e701e | 125 | if ((st=Message::parse(&msg, buf)) != Message::SUCCESS) { |
masahikofukasawa | 0:7a00359e701e | 126 | MSG("#Failed to parse message. status = %02x. %s\n", st, buf); |
masahikofukasawa | 0:7a00359e701e | 127 | status = ERROR; |
masahikofukasawa | 0:7a00359e701e | 128 | eventCommandReceived = false; |
masahikofukasawa | 0:7a00359e701e | 129 | } |
masahikofukasawa | 0:7a00359e701e | 130 | eventCommandReceived = true; |
masahikofukasawa | 0:7a00359e701e | 131 | return status; |
masahikofukasawa | 0:7a00359e701e | 132 | } |
masahikofukasawa | 0:7a00359e701e | 133 | |
masahikofukasawa | 0:7a00359e701e | 134 | |
masahikofukasawa | 0:7a00359e701e | 135 | uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits) |
masahikofukasawa | 0:7a00359e701e | 136 | { |
masahikofukasawa | 0:7a00359e701e | 137 | wait(0.05); |
masahikofukasawa | 0:7a00359e701e | 138 | AnalogIn id(pin); |
masahikofukasawa | 0:7a00359e701e | 139 | double s = id + 1.0/(double)(pow(2.0,bits+1)); |
masahikofukasawa | 0:7a00359e701e | 140 | uint8_t value = (uint8_t)(s*pow(2.0,bits)); |
masahikofukasawa | 0:7a00359e701e | 141 | wait(0.05); |
masahikofukasawa | 0:7a00359e701e | 142 | return value; |
masahikofukasawa | 0:7a00359e701e | 143 | } |
masahikofukasawa | 0:7a00359e701e | 144 | |
masahikofukasawa | 0:7a00359e701e | 145 | bool AkmSensorManager::isEvent() |
masahikofukasawa | 0:7a00359e701e | 146 | { |
masahikofukasawa | 0:7a00359e701e | 147 | return (sensor->isEvent() || |
masahikofukasawa | 0:7a00359e701e | 148 | eventCommandReceived || |
masahikofukasawa | 0:7a00359e701e | 149 | eventConnected || |
masahikofukasawa | 0:7a00359e701e | 150 | eventDisconnected); |
masahikofukasawa | 0:7a00359e701e | 151 | } |
masahikofukasawa | 0:7a00359e701e | 152 | |
masahikofukasawa | 0:7a00359e701e | 153 | |
masahikofukasawa | 0:7a00359e701e | 154 | void AkmSensorManager::processCommand() |
masahikofukasawa | 0:7a00359e701e | 155 | { |
masahikofukasawa | 0:7a00359e701e | 156 | AkmSensorManager::Status status = AkmSensorManager::SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 157 | // MSG("#processCommand.\n"); |
masahikofukasawa | 0:7a00359e701e | 158 | |
masahikofukasawa | 0:7a00359e701e | 159 | // Gets command in the message |
masahikofukasawa | 0:7a00359e701e | 160 | Message::Command cmd = msg.getCommand(); |
masahikofukasawa | 0:7a00359e701e | 161 | |
masahikofukasawa | 0:7a00359e701e | 162 | // Creates an message object to return |
masahikofukasawa | 0:7a00359e701e | 163 | Message resMsg; |
masahikofukasawa | 0:7a00359e701e | 164 | |
masahikofukasawa | 0:7a00359e701e | 165 | // Return message has the same command as input |
masahikofukasawa | 0:7a00359e701e | 166 | resMsg.setCommand(cmd); |
masahikofukasawa | 0:7a00359e701e | 167 | |
masahikofukasawa | 0:7a00359e701e | 168 | switch(cmd) |
masahikofukasawa | 0:7a00359e701e | 169 | { |
masahikofukasawa | 0:7a00359e701e | 170 | case Message::CMD_GET_FW_VERSION: |
masahikofukasawa | 0:7a00359e701e | 171 | resMsg.setArgument(0, FIRMWARE_VERSION); |
masahikofukasawa | 0:7a00359e701e | 172 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 173 | MSG("#FW version is reported.\n"); |
masahikofukasawa | 0:7a00359e701e | 174 | break; |
masahikofukasawa | 0:7a00359e701e | 175 | |
masahikofukasawa | 0:7a00359e701e | 176 | case Message::CMD_GET_MAG_PART: |
masahikofukasawa | 0:7a00359e701e | 177 | resMsg.setArgument(0, MAGNETOMETER_ID); |
masahikofukasawa | 0:7a00359e701e | 178 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 179 | MSG("#Mag ID is reported.\n"); |
masahikofukasawa | 0:7a00359e701e | 180 | break; |
masahikofukasawa | 0:7a00359e701e | 181 | |
masahikofukasawa | 0:7a00359e701e | 182 | case Message::CMD_SET_SERIAL_TARGET: |
masahikofukasawa | 0:7a00359e701e | 183 | isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false; |
masahikofukasawa | 0:7a00359e701e | 184 | isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false; |
masahikofukasawa | 0:7a00359e701e | 185 | resMsg.setArgument(0, 0); |
masahikofukasawa | 0:7a00359e701e | 186 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 187 | MSG("#Serial out is set.\n"); |
masahikofukasawa | 0:7a00359e701e | 188 | break; |
masahikofukasawa | 0:7a00359e701e | 189 | |
masahikofukasawa | 0:7a00359e701e | 190 | case Message::CMD_GET_ID: // return Primary ID and Sub ID |
masahikofukasawa | 0:7a00359e701e | 191 | if(id == 0) id = 0xE; // Temporary for Angle Sensor |
masahikofukasawa | 0:7a00359e701e | 192 | resMsg.setArgument(0, id); |
masahikofukasawa | 0:7a00359e701e | 193 | resMsg.setArgument(1, subId); |
masahikofukasawa | 0:7a00359e701e | 194 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 195 | wait(0.4); // wait for App initialization |
masahikofukasawa | 0:7a00359e701e | 196 | MSG("#Mag ID is reported.\n"); |
masahikofukasawa | 0:7a00359e701e | 197 | break; |
masahikofukasawa | 0:7a00359e701e | 198 | |
masahikofukasawa | 0:7a00359e701e | 199 | case Message::CMD_STOP_MEASUREMENT: |
masahikofukasawa | 0:7a00359e701e | 200 | if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 201 | resMsg.setArgument(0, status==AkmSensorManager::SUCCESS ? 0 : 1); |
masahikofukasawa | 0:7a00359e701e | 202 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 203 | MSG("#Stop measurement.\n"); |
masahikofukasawa | 0:7a00359e701e | 204 | break; |
masahikofukasawa | 0:7a00359e701e | 205 | |
masahikofukasawa | 0:7a00359e701e | 206 | case Message::CMD_START_MEASUREMENT: |
masahikofukasawa | 0:7a00359e701e | 207 | MSG("#Start measurement.\n"); |
masahikofukasawa | 0:7a00359e701e | 208 | sensor->startSensor(); |
masahikofukasawa | 0:7a00359e701e | 209 | // get initial sensor state for switch type sensors |
masahikofukasawa | 0:7a00359e701e | 210 | if( id == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR || |
masahikofukasawa | 0:7a00359e701e | 211 | id == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR || |
masahikofukasawa | 0:7a00359e701e | 212 | id == AkmSensor::AKM_PRIMARY_ID_LATCH || |
masahikofukasawa | 0:7a00359e701e | 213 | id == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT || |
masahikofukasawa | 0:7a00359e701e | 214 | id == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){ |
masahikofukasawa | 0:7a00359e701e | 215 | Message temp; |
masahikofukasawa | 0:7a00359e701e | 216 | sensor->readSensorData(&temp); |
masahikofukasawa | 0:7a00359e701e | 217 | throwMessage(&temp); |
masahikofukasawa | 0:7a00359e701e | 218 | } |
masahikofukasawa | 0:7a00359e701e | 219 | break; |
masahikofukasawa | 0:7a00359e701e | 220 | |
masahikofukasawa | 0:7a00359e701e | 221 | case Message::CMD_ANGLE_ZERO_RESET: |
masahikofukasawa | 0:7a00359e701e | 222 | sensor->requestCommand(&msg,&resMsg); |
masahikofukasawa | 0:7a00359e701e | 223 | throwMessage(&resMsg); |
masahikofukasawa | 0:7a00359e701e | 224 | break; |
masahikofukasawa | 0:7a00359e701e | 225 | /* |
masahikofukasawa | 0:7a00359e701e | 226 | case Message::CMD_IR_GET_THRESHOLD: |
masahikofukasawa | 0:7a00359e701e | 227 | AK9750::Threshold th; |
masahikofukasawa | 0:7a00359e701e | 228 | if (Hardware::getInstance()->ak9750.getThreshold(&th) != AK9750::SUCCESS) { |
masahikofukasawa | 0:7a00359e701e | 229 | MSG("Failed to get threshold of AK9750.\n"); |
masahikofukasawa | 0:7a00359e701e | 230 | return; |
masahikofukasawa | 0:7a00359e701e | 231 | } |
masahikofukasawa | 0:7a00359e701e | 232 | resMsg.setArgument(0, (char)(th.eth13h >> 8)); |
masahikofukasawa | 0:7a00359e701e | 233 | resMsg.setArgument(1, (char)(th.eth13h & 0x00FF)); |
masahikofukasawa | 0:7a00359e701e | 234 | resMsg.setArgument(2, (char)(th.eth13l >> 8)); |
masahikofukasawa | 0:7a00359e701e | 235 | resMsg.setArgument(3, (char)(th.eth13l & 0x00FF)); |
masahikofukasawa | 0:7a00359e701e | 236 | resMsg.setArgument(4, (char)(th.eth24h >> 8)); |
masahikofukasawa | 0:7a00359e701e | 237 | resMsg.setArgument(5, (char)(th.eth24h & 0x00FF)); |
masahikofukasawa | 0:7a00359e701e | 238 | resMsg.setArgument(6, (char)(th.eth24l >> 8)); |
masahikofukasawa | 0:7a00359e701e | 239 | resMsg.setArgument(7, (char)(th.eth24l & 0x00FF)); |
masahikofukasawa | 0:7a00359e701e | 240 | break; |
masahikofukasawa | 0:7a00359e701e | 241 | |
masahikofukasawa | 0:7a00359e701e | 242 | case Message::CMD_IR_SET_THRESHOLD: |
masahikofukasawa | 0:7a00359e701e | 243 | if (msg->getArgNum() < 8) { |
masahikofukasawa | 0:7a00359e701e | 244 | MSG("Too few arguments.\n"); |
masahikofukasawa | 0:7a00359e701e | 245 | resMsg.setArgument(0, 0x01); // error |
masahikofukasawa | 0:7a00359e701e | 246 | } else { |
masahikofukasawa | 0:7a00359e701e | 247 | ak9750Threshold.eth13h = CONV16I(msg->getArgument(0), msg->getArgument(1)); |
masahikofukasawa | 0:7a00359e701e | 248 | ak9750Threshold.eth13l = CONV16I(msg->getArgument(2), msg->getArgument(3)); |
masahikofukasawa | 0:7a00359e701e | 249 | ak9750Threshold.eth24h = CONV16I(msg->getArgument(4), msg->getArgument(5)); |
masahikofukasawa | 0:7a00359e701e | 250 | ak9750Threshold.eth24l = CONV16I(msg->getArgument(6), msg->getArgument(7)); |
masahikofukasawa | 0:7a00359e701e | 251 | if (Hardware::getInstance()->ak9750.setThreshold(&ak9750Threshold) != AK9750::SUCCESS) { |
masahikofukasawa | 0:7a00359e701e | 252 | resMsg.setArgument(0, 0x01); // error |
masahikofukasawa | 0:7a00359e701e | 253 | } else { |
masahikofukasawa | 0:7a00359e701e | 254 | resMsg.setArgument(0, 0x00); // success |
masahikofukasawa | 0:7a00359e701e | 255 | isIrThresholdSet = true; |
masahikofukasawa | 0:7a00359e701e | 256 | } |
masahikofukasawa | 0:7a00359e701e | 257 | } |
masahikofukasawa | 0:7a00359e701e | 258 | break; |
masahikofukasawa | 0:7a00359e701e | 259 | |
masahikofukasawa | 0:7a00359e701e | 260 | case Message::CMD_IR_GET_HYSTERESIS: |
masahikofukasawa | 0:7a00359e701e | 261 | AK9750::Hysteresis hys; |
masahikofukasawa | 0:7a00359e701e | 262 | if (Hardware::getInstance()->ak9750.getHysteresis(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 0:7a00359e701e | 263 | MSG("Failed to get hysteresis setting from AK9750.\n"); |
masahikofukasawa | 0:7a00359e701e | 264 | return; |
masahikofukasawa | 0:7a00359e701e | 265 | } |
masahikofukasawa | 0:7a00359e701e | 266 | resMsg.setArgument(0, hys.ehys13); |
masahikofukasawa | 0:7a00359e701e | 267 | resMsg.setArgument(1, hys.ehys24); |
masahikofukasawa | 0:7a00359e701e | 268 | break; |
masahikofukasawa | 0:7a00359e701e | 269 | |
masahikofukasawa | 0:7a00359e701e | 270 | case Message::CMD_IR_SET_HYSTERESIS: |
masahikofukasawa | 0:7a00359e701e | 271 | if (msg->getArgNum() < 2) { // sanity check |
masahikofukasawa | 0:7a00359e701e | 272 | MSG("Too few argument.\n"); |
masahikofukasawa | 0:7a00359e701e | 273 | resMsg.setArgument(0, 0x01); // error |
masahikofukasawa | 0:7a00359e701e | 274 | } else { |
masahikofukasawa | 0:7a00359e701e | 275 | AK9750::Hysteresis hys; |
masahikofukasawa | 0:7a00359e701e | 276 | hys.ehys13 = msg->getArgument(0); |
masahikofukasawa | 0:7a00359e701e | 277 | hys.ehys24 = msg->getArgument(1); |
masahikofukasawa | 0:7a00359e701e | 278 | if (Hardware::getInstance()->ak9750.setHysteresis(&hys) != AK9750::SUCCESS) { |
masahikofukasawa | 0:7a00359e701e | 279 | MSG("Failed to set hysteresis to AK9750.\n"); |
masahikofukasawa | 0:7a00359e701e | 280 | resMsg.setArgument(0, 0x01); // error |
masahikofukasawa | 0:7a00359e701e | 281 | } else { |
masahikofukasawa | 0:7a00359e701e | 282 | resMsg.setArgument(0, 0x00); // success |
masahikofukasawa | 0:7a00359e701e | 283 | } |
masahikofukasawa | 0:7a00359e701e | 284 | } |
masahikofukasawa | 0:7a00359e701e | 285 | break; |
masahikofukasawa | 0:7a00359e701e | 286 | */ |
masahikofukasawa | 0:7a00359e701e | 287 | default: |
masahikofukasawa | 0:7a00359e701e | 288 | MSG("#Can't find command:%s\n", (char)cmd); |
masahikofukasawa | 0:7a00359e701e | 289 | break; |
masahikofukasawa | 0:7a00359e701e | 290 | } |
masahikofukasawa | 0:7a00359e701e | 291 | } |
masahikofukasawa | 0:7a00359e701e | 292 | |
masahikofukasawa | 0:7a00359e701e | 293 | AkmSensorManager::Status AkmSensorManager::processEvent() |
masahikofukasawa | 0:7a00359e701e | 294 | { |
masahikofukasawa | 0:7a00359e701e | 295 | AkmSensorManager::Status status = AkmSensorManager::SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 296 | |
masahikofukasawa | 0:7a00359e701e | 297 | // command received from the host |
masahikofukasawa | 0:7a00359e701e | 298 | if(eventCommandReceived) |
masahikofukasawa | 0:7a00359e701e | 299 | { |
masahikofukasawa | 0:7a00359e701e | 300 | processCommand(); |
masahikofukasawa | 0:7a00359e701e | 301 | eventCommandReceived = false; |
masahikofukasawa | 0:7a00359e701e | 302 | } |
masahikofukasawa | 0:7a00359e701e | 303 | if(sensor->isEvent()) // sensor read data event |
masahikofukasawa | 0:7a00359e701e | 304 | { |
masahikofukasawa | 0:7a00359e701e | 305 | Message msg; |
masahikofukasawa | 0:7a00359e701e | 306 | if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 307 | throwMessage(&msg); |
masahikofukasawa | 0:7a00359e701e | 308 | } |
masahikofukasawa | 0:7a00359e701e | 309 | |
masahikofukasawa | 0:7a00359e701e | 310 | if(eventConnected) // BLE connected. Start sensor. |
masahikofukasawa | 0:7a00359e701e | 311 | { |
masahikofukasawa | 0:7a00359e701e | 312 | // if( sensor->startSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 313 | eventConnected = false; |
masahikofukasawa | 0:7a00359e701e | 314 | } |
masahikofukasawa | 0:7a00359e701e | 315 | if(eventDisconnected) // BLE dis-connected. Stop sensor. |
masahikofukasawa | 0:7a00359e701e | 316 | { |
masahikofukasawa | 0:7a00359e701e | 317 | if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 318 | eventDisconnected = false; |
masahikofukasawa | 0:7a00359e701e | 319 | } |
masahikofukasawa | 0:7a00359e701e | 320 | return status; |
masahikofukasawa | 0:7a00359e701e | 321 | } |
masahikofukasawa | 0:7a00359e701e | 322 | |
masahikofukasawa | 0:7a00359e701e | 323 | AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) { |
masahikofukasawa | 0:7a00359e701e | 324 | int len = Message::getMaxMessageLength(); |
masahikofukasawa | 0:7a00359e701e | 325 | char buf[len]; |
masahikofukasawa | 0:7a00359e701e | 326 | |
masahikofukasawa | 0:7a00359e701e | 327 | buf[0] = '$'; |
masahikofukasawa | 0:7a00359e701e | 328 | |
masahikofukasawa | 0:7a00359e701e | 329 | // Processes command |
masahikofukasawa | 0:7a00359e701e | 330 | char cmd = (char)msg->getCommand(); |
masahikofukasawa | 0:7a00359e701e | 331 | Message::charToAscii(&buf[1], &cmd); |
masahikofukasawa | 0:7a00359e701e | 332 | |
masahikofukasawa | 0:7a00359e701e | 333 | // Processes arguments |
masahikofukasawa | 0:7a00359e701e | 334 | for (int i=0; i < msg->getArgNum(); i++) { |
masahikofukasawa | 0:7a00359e701e | 335 | char arg = msg->getArgument(i); |
masahikofukasawa | 0:7a00359e701e | 336 | Message::charToAscii(&buf[3+2*i], &arg); |
masahikofukasawa | 0:7a00359e701e | 337 | } |
masahikofukasawa | 0:7a00359e701e | 338 | |
masahikofukasawa | 0:7a00359e701e | 339 | // Add termination characters, 0x0D(\r), \n and \0, to the end of string |
masahikofukasawa | 0:7a00359e701e | 340 | int tIdx = 3 + 2 * (msg->getArgNum()); |
masahikofukasawa | 0:7a00359e701e | 341 | int bufSize = sizeof(buf)/sizeof(buf[0]); |
masahikofukasawa | 0:7a00359e701e | 342 | if ((tIdx + 3) > (bufSize - 1)) { |
masahikofukasawa | 0:7a00359e701e | 343 | MSG("#Error: Message data exceeds the buffer.\n"); |
masahikofukasawa | 0:7a00359e701e | 344 | return ERROR; |
masahikofukasawa | 0:7a00359e701e | 345 | } |
masahikofukasawa | 0:7a00359e701e | 346 | buf[tIdx++] = 0x0D; // '\r' |
masahikofukasawa | 0:7a00359e701e | 347 | buf[tIdx++] = '\n'; |
masahikofukasawa | 0:7a00359e701e | 348 | buf[tIdx] = '\0'; |
masahikofukasawa | 0:7a00359e701e | 349 | |
masahikofukasawa | 0:7a00359e701e | 350 | // Send to the BLE buffer |
masahikofukasawa | 0:7a00359e701e | 351 | // Hardware *hw = Hardware::getInstance(); |
masahikofukasawa | 0:7a00359e701e | 352 | // hw->uartService->writeString(buf); |
masahikofukasawa | 0:7a00359e701e | 353 | if(isEnabledBle) uartService->writeString(buf); |
masahikofukasawa | 0:7a00359e701e | 354 | if(isEnabledUsb) serial->printf(buf); |
masahikofukasawa | 0:7a00359e701e | 355 | |
masahikofukasawa | 0:7a00359e701e | 356 | // MSG("#A message thrown: \"%s\"\n", buf); |
masahikofukasawa | 0:7a00359e701e | 357 | |
masahikofukasawa | 0:7a00359e701e | 358 | return SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 359 | } |
masahikofukasawa | 0:7a00359e701e | 360 | |
masahikofukasawa | 0:7a00359e701e | 361 | /* |
masahikofukasawa | 0:7a00359e701e | 362 | void AkmSensorManager::dataOut(char* str){ |
masahikofukasawa | 0:7a00359e701e | 363 | if(isBleMode) uartService->writeString(str); |
masahikofukasawa | 0:7a00359e701e | 364 | if(isSerialMode) serial->printf(str); |
masahikofukasawa | 0:7a00359e701e | 365 | } |
masahikofukasawa | 0:7a00359e701e | 366 | */ |