Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

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?

UserRevisionLine numberNew 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 */