Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmsensormanager.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:
12:c06cd8b76358
RevD7_004

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 "akmhallswitch.h"
masahikofukasawa 11:cef8dc1cf010 4 #include "akmanalogsensor.h"
masahikofukasawa 10:5c69b067d88a 5 #include "ak09970ctrl.h"
masahikofukasawa 10:5c69b067d88a 6 #include "ak9750ctrl.h"
masahikofukasawa 10:5c69b067d88a 7 #include "ak9752ctrl.h"
masahikofukasawa 11:cef8dc1cf010 8 #include "ak7451ctrl.h"
masahikofukasawa 10:5c69b067d88a 9 #include "ak7401ctrl.h"
masahikofukasawa 0:7a00359e701e 10 #include "akmakd.h"
masahikofukasawa 0:7a00359e701e 11 #include "debug.h"
masahikofukasawa 0:7a00359e701e 12 #include "Message.h"
coisme 2:11fe67783c4c 13 #ifdef REV_D
coisme 2:11fe67783c4c 14 #include "mcp342x.h"
masahikofukasawa 6:c4401549d68f 15 #include "I2CNano.h"
masahikofukasawa 7:e269411568c9 16 #include "tca9554a.h"
coisme 2:11fe67783c4c 17 #endif
masahikofukasawa 0:7a00359e701e 18
masahikofukasawa 11:cef8dc1cf010 19 #define CR '\r'
masahikofukasawa 11:cef8dc1cf010 20 #define LF '\n'
masahikofukasawa 0:7a00359e701e 21 #define FIRMWARE_VERSION 0x02
masahikofukasawa 0:7a00359e701e 22 #define MAGNETOMETER_ID 0x0A
masahikofukasawa 0:7a00359e701e 23
masahikofukasawa 0:7a00359e701e 24 #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low)))
masahikofukasawa 0:7a00359e701e 25
masahikofukasawa 0:7a00359e701e 26
masahikofukasawa 10:5c69b067d88a 27 const char* AKM_PRIMARY_ID_STR[] = {
masahikofukasawa 7:e269411568c9 28 "AKD Daughter Cards(SPI)",
masahikofukasawa 0:7a00359e701e 29 "Switch, Unipolar",
masahikofukasawa 0:7a00359e701e 30 "Switch, Onmipolar",
masahikofukasawa 0:7a00359e701e 31 "Latch, Bipolar",
masahikofukasawa 0:7a00359e701e 32 "Switch, Dual Output",
masahikofukasawa 0:7a00359e701e 33 "Onechip Encoder",
masahikofukasawa 0:7a00359e701e 34 "TBD1",
masahikofukasawa 0:7a00359e701e 35 "TBD2",
masahikofukasawa 0:7a00359e701e 36 "Linear Sensor Legacy",
masahikofukasawa 0:7a00359e701e 37 "Current Sensor",
masahikofukasawa 0:7a00359e701e 38 "MISC(Analog)",
masahikofukasawa 0:7a00359e701e 39 "Linear Sensor",
masahikofukasawa 4:af13b985c689 40 "TBD3",
masahikofukasawa 1:b46b8653331f 41 "IR Sensor",
masahikofukasawa 7:e269411568c9 42 "Angle Sensor(SPI)",
masahikofukasawa 10:5c69b067d88a 43 "AKD Daughter Cards(I2C)"
masahikofukasawa 0:7a00359e701e 44 };
masahikofukasawa 0:7a00359e701e 45
masahikofukasawa 0:7a00359e701e 46 AkmSensorManager::AkmSensorManager(SerialNano* com, UARTService* service)
masahikofukasawa 0:7a00359e701e 47 {
masahikofukasawa 0:7a00359e701e 48 serial = com;
masahikofukasawa 0:7a00359e701e 49 uartService = service;
masahikofukasawa 0:7a00359e701e 50 isEnabledBle = true;
masahikofukasawa 0:7a00359e701e 51 isEnabledUsb = true;
masahikofukasawa 0:7a00359e701e 52 eventCommandReceived = false;
masahikofukasawa 0:7a00359e701e 53 eventConnected = false;
masahikofukasawa 0:7a00359e701e 54 eventDisconnected = false;
masahikofukasawa 0:7a00359e701e 55 }
masahikofukasawa 0:7a00359e701e 56
masahikofukasawa 10:5c69b067d88a 57 AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid)
masahikofukasawa 0:7a00359e701e 58 {
masahikofukasawa 10:5c69b067d88a 59 primaryId = id;
masahikofukasawa 10:5c69b067d88a 60 subId = subid;
masahikofukasawa 0:7a00359e701e 61 sensor = AkmSensorManager::getAkmSensor();
masahikofukasawa 0:7a00359e701e 62 if(sensor == NULL) return AkmSensorManager::ERROR;
masahikofukasawa 0:7a00359e701e 63 return AkmSensorManager::SUCCESS;
masahikofukasawa 0:7a00359e701e 64 }
masahikofukasawa 0:7a00359e701e 65
masahikofukasawa 0:7a00359e701e 66 void AkmSensorManager::setEventConnected()
masahikofukasawa 0:7a00359e701e 67 {
masahikofukasawa 0:7a00359e701e 68 eventConnected = true;
masahikofukasawa 0:7a00359e701e 69 }
masahikofukasawa 0:7a00359e701e 70
masahikofukasawa 0:7a00359e701e 71 void AkmSensorManager::setEventDisconnected()
masahikofukasawa 0:7a00359e701e 72 {
masahikofukasawa 0:7a00359e701e 73 eventDisconnected = true;
masahikofukasawa 0:7a00359e701e 74 }
masahikofukasawa 0:7a00359e701e 75
masahikofukasawa 0:7a00359e701e 76 AkmSensor* AkmSensorManager::getAkmSensor()
masahikofukasawa 0:7a00359e701e 77 {
masahikofukasawa 0:7a00359e701e 78 AkmSensor* sensor = NULL;
masahikofukasawa 0:7a00359e701e 79
masahikofukasawa 10:5c69b067d88a 80 switch(primaryId){
masahikofukasawa 7:e269411568c9 81 case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:
masahikofukasawa 0:7a00359e701e 82 case AkmSensor::AKM_PRIMARY_ID_AKD_I2C:
masahikofukasawa 10:5c69b067d88a 83 {
masahikofukasawa 10:5c69b067d88a 84 if(subId != Ak09970Ctrl::SUB_ID_AK09970){
masahikofukasawa 10:5c69b067d88a 85 AkmAkd* akd = new AkmAkd();
masahikofukasawa 10:5c69b067d88a 86 sensor = akd;
masahikofukasawa 10:5c69b067d88a 87 }
masahikofukasawa 10:5c69b067d88a 88 else{
masahikofukasawa 10:5c69b067d88a 89 Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
masahikofukasawa 10:5c69b067d88a 90 sensor = ak09970;
masahikofukasawa 10:5c69b067d88a 91 }
masahikofukasawa 0:7a00359e701e 92 break;
masahikofukasawa 10:5c69b067d88a 93 }
masahikofukasawa 0:7a00359e701e 94 case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR:
masahikofukasawa 10:5c69b067d88a 95 {
masahikofukasawa 11:cef8dc1cf010 96 if(subId == Ak7451Ctrl::SUB_ID_AK7451){
masahikofukasawa 11:cef8dc1cf010 97 Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl();
masahikofukasawa 11:cef8dc1cf010 98 sensor = ak7451ctrl;
masahikofukasawa 10:5c69b067d88a 99 break;
masahikofukasawa 10:5c69b067d88a 100 }
masahikofukasawa 10:5c69b067d88a 101 else if(subId == Ak7401Ctrl::SUB_ID_AK7401){
masahikofukasawa 10:5c69b067d88a 102 Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl();
masahikofukasawa 10:5c69b067d88a 103 sensor = ak7401ctrl;
masahikofukasawa 10:5c69b067d88a 104 break;
masahikofukasawa 10:5c69b067d88a 105 }
masahikofukasawa 0:7a00359e701e 106 break;
masahikofukasawa 10:5c69b067d88a 107 }
masahikofukasawa 0:7a00359e701e 108 case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR:
masahikofukasawa 0:7a00359e701e 109 case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR:
masahikofukasawa 0:7a00359e701e 110 case AkmSensor::AKM_PRIMARY_ID_LATCH:
masahikofukasawa 0:7a00359e701e 111 case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT:
masahikofukasawa 0:7a00359e701e 112 case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER:
masahikofukasawa 10:5c69b067d88a 113 {
masahikofukasawa 10:5c69b067d88a 114 AkmHallSwitch* hallswitch = new AkmHallSwitch();
masahikofukasawa 0:7a00359e701e 115 sensor = hallswitch;
masahikofukasawa 0:7a00359e701e 116 break;
masahikofukasawa 10:5c69b067d88a 117 }
masahikofukasawa 0:7a00359e701e 118 case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY:
masahikofukasawa 4:af13b985c689 119 case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR:
masahikofukasawa 10:5c69b067d88a 120 case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR:
masahikofukasawa 10:5c69b067d88a 121 case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG:
masahikofukasawa 10:5c69b067d88a 122 {
masahikofukasawa 11:cef8dc1cf010 123 AkmAnalogSensor* analogsensor = new AkmAnalogSensor();
masahikofukasawa 11:cef8dc1cf010 124 sensor = analogsensor;
masahikofukasawa 0:7a00359e701e 125 break;
masahikofukasawa 10:5c69b067d88a 126 }
masahikofukasawa 1:b46b8653331f 127 case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR:
masahikofukasawa 10:5c69b067d88a 128 {
masahikofukasawa 10:5c69b067d88a 129 if(subId == Ak9750Ctrl::SUB_ID_AK9750){
masahikofukasawa 10:5c69b067d88a 130 Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl();
masahikofukasawa 10:5c69b067d88a 131 sensor = ak9750ctrl;
masahikofukasawa 10:5c69b067d88a 132 }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){
masahikofukasawa 10:5c69b067d88a 133 Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
masahikofukasawa 10:5c69b067d88a 134 sensor = ak9752ctrl;
masahikofukasawa 10:5c69b067d88a 135 }else{
masahikofukasawa 10:5c69b067d88a 136 return NULL; // couldn't find
masahikofukasawa 10:5c69b067d88a 137 }
masahikofukasawa 10:5c69b067d88a 138
masahikofukasawa 1:b46b8653331f 139 break;
masahikofukasawa 10:5c69b067d88a 140 }
masahikofukasawa 0:7a00359e701e 141 default:
masahikofukasawa 10:5c69b067d88a 142 {
masahikofukasawa 10:5c69b067d88a 143 MSG("#Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 144 return NULL; // couldn't find
masahikofukasawa 10:5c69b067d88a 145 }
masahikofukasawa 0:7a00359e701e 146 }
masahikofukasawa 11:cef8dc1cf010 147
masahikofukasawa 10:5c69b067d88a 148 if(sensor->init(primaryId, subId) != AkmSensor::SUCCESS){
masahikofukasawa 10:5c69b067d88a 149 MSG("#sensor->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 150 return NULL; // couldn't find
masahikofukasawa 0:7a00359e701e 151 }
masahikofukasawa 0:7a00359e701e 152
masahikofukasawa 10:5c69b067d88a 153 MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 154 return sensor;
masahikofukasawa 0:7a00359e701e 155 }
masahikofukasawa 0:7a00359e701e 156
masahikofukasawa 0:7a00359e701e 157 AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){
masahikofukasawa 0:7a00359e701e 158 // Construct message
masahikofukasawa 0:7a00359e701e 159 Status status = SUCCESS;
masahikofukasawa 0:7a00359e701e 160
masahikofukasawa 10:5c69b067d88a 161 if ((Message::parse(&msg, buf)) != Message::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 162 MSG("#Failed to parse message. %s\r\n", buf);
masahikofukasawa 0:7a00359e701e 163 status = ERROR;
masahikofukasawa 0:7a00359e701e 164 eventCommandReceived = false;
masahikofukasawa 0:7a00359e701e 165 }
masahikofukasawa 0:7a00359e701e 166 eventCommandReceived = true;
masahikofukasawa 0:7a00359e701e 167 return status;
masahikofukasawa 0:7a00359e701e 168 }
masahikofukasawa 0:7a00359e701e 169
coisme 2:11fe67783c4c 170 #ifdef REV_D
coisme 2:11fe67783c4c 171 int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) {
coisme 2:11fe67783c4c 172 const int WAIT_ADC_MS = 1;
coisme 2:11fe67783c4c 173
coisme 2:11fe67783c4c 174 // Configure channel and trigger.
coisme 2:11fe67783c4c 175 mcp3428->setChannel(ch);
coisme 2:11fe67783c4c 176 mcp3428->setSampleSetting(s);
coisme 2:11fe67783c4c 177 mcp3428->trigger();
coisme 2:11fe67783c4c 178
coisme 2:11fe67783c4c 179 // polling data (!blocking)
coisme 2:11fe67783c4c 180 MCP342X::Data data;
coisme 2:11fe67783c4c 181 do {
coisme 2:11fe67783c4c 182 wait_ms(WAIT_ADC_MS);
coisme 2:11fe67783c4c 183 mcp3428->getData(&data);
coisme 2:11fe67783c4c 184 } while(data.st == MCP342X::DATA_NOT_UPDATED);
coisme 2:11fe67783c4c 185
coisme 2:11fe67783c4c 186 return data.value;
coisme 2:11fe67783c4c 187 }
coisme 2:11fe67783c4c 188 #endif
masahikofukasawa 0:7a00359e701e 189
masahikofukasawa 0:7a00359e701e 190 uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
masahikofukasawa 0:7a00359e701e 191 {
coisme 2:11fe67783c4c 192 #ifndef REV_D
coisme 2:11fe67783c4c 193 /* Rev.C */
masahikofukasawa 0:7a00359e701e 194 AnalogIn id(pin);
masahikofukasawa 11:cef8dc1cf010 195 MSG("#Voltage=%5.2f[V]\r\n",id*3.0);
masahikofukasawa 0:7a00359e701e 196 double s = id + 1.0/(double)(pow(2.0,bits+1));
masahikofukasawa 0:7a00359e701e 197 uint8_t value = (uint8_t)(s*pow(2.0,bits));
coisme 2:11fe67783c4c 198 #else
coisme 2:11fe67783c4c 199 /* Rev.D */
masahikofukasawa 11:cef8dc1cf010 200 MSG("#GetID\r\n");
masahikofukasawa 6:c4401549d68f 201
masahikofukasawa 6:c4401549d68f 202 I2C i2c(I2C_SDA, I2C_SCL);
coisme 2:11fe67783c4c 203 // ADC
coisme 2:11fe67783c4c 204 MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH);
coisme 2:11fe67783c4c 205 mcp342x.setConversionMode(MCP342X::ONE_SHOT);
coisme 2:11fe67783c4c 206 MCP342X::AdcChannel ch;
coisme 2:11fe67783c4c 207 if (pin == ANALOG_SENSOR_ID) {
coisme 2:11fe67783c4c 208 ch = MCP342X::ADC_CH1;
coisme 2:11fe67783c4c 209 } else { // pin == ANALOG_SENSOR_ID_SUB
coisme 2:11fe67783c4c 210 ch = MCP342X::ADC_CH2;
coisme 2:11fe67783c4c 211 }
coisme 2:11fe67783c4c 212 int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT);
masahikofukasawa 11:cef8dc1cf010 213 MSG("#12bit ADC Val = %d.\r\n", val);
coisme 2:11fe67783c4c 214
coisme 2:11fe67783c4c 215 const int16_t VAL_MAX = 3000-2048; // Corresponds to 3V
coisme 2:11fe67783c4c 216 const int16_t VAL_MIN = -2048; // Corresponds to 0V
coisme 2:11fe67783c4c 217
masahikofukasawa 6:c4401549d68f 218 uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5);
masahikofukasawa 11:cef8dc1cf010 219 MSG("#ID = %d.\r\n", value);
masahikofukasawa 6:c4401549d68f 220
coisme 2:11fe67783c4c 221 #endif
masahikofukasawa 0:7a00359e701e 222 return value;
masahikofukasawa 0:7a00359e701e 223 }
masahikofukasawa 0:7a00359e701e 224
masahikofukasawa 0:7a00359e701e 225 bool AkmSensorManager::isEvent()
masahikofukasawa 0:7a00359e701e 226 {
masahikofukasawa 0:7a00359e701e 227 return (sensor->isEvent() ||
masahikofukasawa 0:7a00359e701e 228 eventCommandReceived ||
masahikofukasawa 0:7a00359e701e 229 eventConnected ||
masahikofukasawa 0:7a00359e701e 230 eventDisconnected);
masahikofukasawa 0:7a00359e701e 231 }
masahikofukasawa 0:7a00359e701e 232
masahikofukasawa 0:7a00359e701e 233
masahikofukasawa 0:7a00359e701e 234 void AkmSensorManager::processCommand()
masahikofukasawa 0:7a00359e701e 235 {
masahikofukasawa 0:7a00359e701e 236 AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
masahikofukasawa 0:7a00359e701e 237
masahikofukasawa 0:7a00359e701e 238 // Gets command in the message
masahikofukasawa 0:7a00359e701e 239 Message::Command cmd = msg.getCommand();
masahikofukasawa 0:7a00359e701e 240
masahikofukasawa 0:7a00359e701e 241 // Creates an message object to return
masahikofukasawa 0:7a00359e701e 242 Message resMsg;
masahikofukasawa 0:7a00359e701e 243
masahikofukasawa 0:7a00359e701e 244 // Return message has the same command as input
masahikofukasawa 0:7a00359e701e 245 resMsg.setCommand(cmd);
masahikofukasawa 0:7a00359e701e 246
masahikofukasawa 0:7a00359e701e 247 switch(cmd)
masahikofukasawa 0:7a00359e701e 248 {
masahikofukasawa 0:7a00359e701e 249 case Message::CMD_GET_FW_VERSION:
masahikofukasawa 11:cef8dc1cf010 250 {
masahikofukasawa 0:7a00359e701e 251 resMsg.setArgument(0, FIRMWARE_VERSION);
masahikofukasawa 0:7a00359e701e 252 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 253 MSG("#FW version is reported.\r\n");
masahikofukasawa 0:7a00359e701e 254 break;
masahikofukasawa 11:cef8dc1cf010 255 }
masahikofukasawa 0:7a00359e701e 256 case Message::CMD_GET_MAG_PART:
masahikofukasawa 11:cef8dc1cf010 257 {
masahikofukasawa 0:7a00359e701e 258 resMsg.setArgument(0, MAGNETOMETER_ID);
masahikofukasawa 0:7a00359e701e 259 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 260 MSG("#Mag ID is reported.\r\n");
masahikofukasawa 0:7a00359e701e 261 break;
masahikofukasawa 11:cef8dc1cf010 262 }
masahikofukasawa 0:7a00359e701e 263 case Message::CMD_SET_SERIAL_TARGET:
masahikofukasawa 11:cef8dc1cf010 264 {
masahikofukasawa 0:7a00359e701e 265 isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false;
masahikofukasawa 0:7a00359e701e 266 isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false;
masahikofukasawa 0:7a00359e701e 267 break;
masahikofukasawa 11:cef8dc1cf010 268 }
masahikofukasawa 0:7a00359e701e 269 case Message::CMD_GET_ID: // return Primary ID and Sub ID
masahikofukasawa 11:cef8dc1cf010 270 {
masahikofukasawa 10:5c69b067d88a 271 resMsg.setArgument(0, primaryId);
masahikofukasawa 0:7a00359e701e 272 resMsg.setArgument(1, subId);
masahikofukasawa 0:7a00359e701e 273 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 274 MSG("#Mag ID is reported.\r\n");
masahikofukasawa 0:7a00359e701e 275 break;
masahikofukasawa 11:cef8dc1cf010 276 }
masahikofukasawa 0:7a00359e701e 277 case Message::CMD_STOP_MEASUREMENT:
masahikofukasawa 11:cef8dc1cf010 278 {
masahikofukasawa 0:7a00359e701e 279 if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
masahikofukasawa 0:7a00359e701e 280 resMsg.setArgument(0, status==AkmSensorManager::SUCCESS ? 0 : 1);
masahikofukasawa 0:7a00359e701e 281 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 282 MSG("#Stop measurement.\r\n");
masahikofukasawa 0:7a00359e701e 283 break;
masahikofukasawa 11:cef8dc1cf010 284 }
masahikofukasawa 0:7a00359e701e 285 case Message::CMD_START_MEASUREMENT:
masahikofukasawa 11:cef8dc1cf010 286 {
masahikofukasawa 11:cef8dc1cf010 287 int error_code = AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 288 if(msg.getArgNum() == 0){
masahikofukasawa 11:cef8dc1cf010 289 error_code = sensor->startSensor();
masahikofukasawa 11:cef8dc1cf010 290 if( error_code != AkmSensor::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 291 MSG("#StartSensor Error. Code=%d\r\n",error_code);
masahikofukasawa 11:cef8dc1cf010 292 }
masahikofukasawa 11:cef8dc1cf010 293 else{
masahikofukasawa 11:cef8dc1cf010 294 MSG("#Start measurement.\r\n");
masahikofukasawa 11:cef8dc1cf010 295 }
masahikofukasawa 11:cef8dc1cf010 296 }else if(msg.getArgNum() == 1){
masahikofukasawa 11:cef8dc1cf010 297 float interval = (float)(1.0 / (float)msg.getArgument(0));
masahikofukasawa 11:cef8dc1cf010 298 error_code = sensor->startSensor(interval);
masahikofukasawa 11:cef8dc1cf010 299 if( error_code != AkmSensor::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 300 MSG("#StartSensor Error. Code=%d\r\n",error_code);
masahikofukasawa 11:cef8dc1cf010 301 }
masahikofukasawa 11:cef8dc1cf010 302 else{
masahikofukasawa 11:cef8dc1cf010 303 MSG("#Start measurement.\r\n");
masahikofukasawa 11:cef8dc1cf010 304 }
masahikofukasawa 11:cef8dc1cf010 305 }else{
masahikofukasawa 11:cef8dc1cf010 306 MSG("#StartSensor Error. Wrong Argument num.\r\n");
masahikofukasawa 11:cef8dc1cf010 307 }
masahikofukasawa 11:cef8dc1cf010 308 if(error_code == AkmSensor::SUCCESS){
masahikofukasawa 11:cef8dc1cf010 309 // get initial sensor state for switch type sensors
masahikofukasawa 11:cef8dc1cf010 310 if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR ||
masahikofukasawa 11:cef8dc1cf010 311 primaryId == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR ||
masahikofukasawa 11:cef8dc1cf010 312 primaryId == AkmSensor::AKM_PRIMARY_ID_LATCH ||
masahikofukasawa 11:cef8dc1cf010 313 primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT ||
masahikofukasawa 11:cef8dc1cf010 314 primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){
masahikofukasawa 11:cef8dc1cf010 315 Message temp;
masahikofukasawa 11:cef8dc1cf010 316 sensor->readSensorData(&temp);
masahikofukasawa 11:cef8dc1cf010 317 throwMessage(&temp);
masahikofukasawa 11:cef8dc1cf010 318 }
masahikofukasawa 0:7a00359e701e 319 }
masahikofukasawa 0:7a00359e701e 320 break;
masahikofukasawa 11:cef8dc1cf010 321 }
masahikofukasawa 9:6fa3e7b17c27 322 case Message::CMD_PROGSW_GET_THRESHOLD:
masahikofukasawa 9:6fa3e7b17c27 323 case Message::CMD_PROGSW_SET_THRESHOLD:
masahikofukasawa 9:6fa3e7b17c27 324 case Message::CMD_PROGSW_GET_READ_COFIGURATION:
masahikofukasawa 9:6fa3e7b17c27 325 case Message::CMD_PROGSW_SET_READ_COFIGURATION:
masahikofukasawa 9:6fa3e7b17c27 326 case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION:
masahikofukasawa 9:6fa3e7b17c27 327 case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION:
masahikofukasawa 9:6fa3e7b17c27 328 case Message::CMD_PROGSW_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 329 case Message::CMD_PROGSW_SET_OPERATION_MODE:
masahikofukasawa 1:b46b8653331f 330 case Message::CMD_IR_GET_THRESHOLD:
masahikofukasawa 1:b46b8653331f 331 case Message::CMD_IR_SET_THRESHOLD:
masahikofukasawa 1:b46b8653331f 332 case Message::CMD_IR_GET_HYSTERESIS:
masahikofukasawa 1:b46b8653331f 333 case Message::CMD_IR_SET_HYSTERESIS:
masahikofukasawa 1:b46b8653331f 334 case Message::CMD_IR_GET_INTERRUPT:
masahikofukasawa 1:b46b8653331f 335 case Message::CMD_IR_SET_INTERRUPT:
masahikofukasawa 1:b46b8653331f 336 case Message::CMD_IR_GET_OPERATION_MODE:
masahikofukasawa 1:b46b8653331f 337 case Message::CMD_IR_SET_OPERATION_MODE:
masahikofukasawa 1:b46b8653331f 338 case Message::CMD_IR_GET_THRESHOLD_EEPROM:
masahikofukasawa 1:b46b8653331f 339 case Message::CMD_IR_SET_THRESHOLD_EEPROM:
masahikofukasawa 1:b46b8653331f 340 case Message::CMD_IR_GET_HYSTERESIS_EEPROM:
masahikofukasawa 1:b46b8653331f 341 case Message::CMD_IR_SET_HYSTERESIS_EEPROM:
masahikofukasawa 1:b46b8653331f 342 case Message::CMD_IR_GET_INTERRUPT_EEPROM:
masahikofukasawa 1:b46b8653331f 343 case Message::CMD_IR_SET_INTERRUPT_EEPROM:
masahikofukasawa 1:b46b8653331f 344 case Message::CMD_IR_GET_OPERATION_MODE_EEPROM:
masahikofukasawa 1:b46b8653331f 345 case Message::CMD_IR_SET_OPERATION_MODE_EEPROM:
masahikofukasawa 9:6fa3e7b17c27 346 case Message::CMD_ANGLE_ZERO_RESET:
masahikofukasawa 9:6fa3e7b17c27 347 case Message::CMD_REG_WRITE:
masahikofukasawa 9:6fa3e7b17c27 348 case Message::CMD_REG_WRITEN:
masahikofukasawa 9:6fa3e7b17c27 349 case Message::CMD_REG_READ:
masahikofukasawa 9:6fa3e7b17c27 350 case Message::CMD_REG_READN:
masahikofukasawa 9:6fa3e7b17c27 351 case Message::CMD_COMPASS_GET_OPERATION_MODE:
masahikofukasawa 9:6fa3e7b17c27 352 case Message::CMD_COMPASS_SET_OPERATION_MODE:
masahikofukasawa 11:cef8dc1cf010 353 {
masahikofukasawa 0:7a00359e701e 354 sensor->requestCommand(&msg,&resMsg);
masahikofukasawa 0:7a00359e701e 355 throwMessage(&resMsg);
masahikofukasawa 0:7a00359e701e 356 break;
masahikofukasawa 11:cef8dc1cf010 357 }
masahikofukasawa 0:7a00359e701e 358 default:
masahikofukasawa 11:cef8dc1cf010 359 {
masahikofukasawa 11:cef8dc1cf010 360 MSG("#Can't find command:%s\r\n", (char)cmd);
masahikofukasawa 0:7a00359e701e 361 break;
masahikofukasawa 11:cef8dc1cf010 362 }
masahikofukasawa 0:7a00359e701e 363 }
masahikofukasawa 0:7a00359e701e 364 }
masahikofukasawa 0:7a00359e701e 365
masahikofukasawa 0:7a00359e701e 366 AkmSensorManager::Status AkmSensorManager::processEvent()
masahikofukasawa 0:7a00359e701e 367 {
masahikofukasawa 0:7a00359e701e 368 AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
masahikofukasawa 0:7a00359e701e 369
masahikofukasawa 0:7a00359e701e 370 // command received from the host
masahikofukasawa 0:7a00359e701e 371 if(eventCommandReceived)
masahikofukasawa 0:7a00359e701e 372 {
masahikofukasawa 0:7a00359e701e 373 processCommand();
masahikofukasawa 0:7a00359e701e 374 eventCommandReceived = false;
masahikofukasawa 0:7a00359e701e 375 }
masahikofukasawa 0:7a00359e701e 376 if(sensor->isEvent()) // sensor read data event
masahikofukasawa 0:7a00359e701e 377 {
masahikofukasawa 0:7a00359e701e 378 Message msg;
masahikofukasawa 0:7a00359e701e 379 if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
masahikofukasawa 0:7a00359e701e 380 throwMessage(&msg);
masahikofukasawa 0:7a00359e701e 381 }
masahikofukasawa 0:7a00359e701e 382
masahikofukasawa 0:7a00359e701e 383 if(eventConnected) // BLE connected. Start sensor.
masahikofukasawa 0:7a00359e701e 384 {
masahikofukasawa 0:7a00359e701e 385 eventConnected = false;
masahikofukasawa 0:7a00359e701e 386 }
masahikofukasawa 0:7a00359e701e 387 if(eventDisconnected) // BLE dis-connected. Stop sensor.
masahikofukasawa 0:7a00359e701e 388 {
masahikofukasawa 0:7a00359e701e 389 if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
masahikofukasawa 0:7a00359e701e 390 eventDisconnected = false;
masahikofukasawa 0:7a00359e701e 391 }
masahikofukasawa 0:7a00359e701e 392 return status;
masahikofukasawa 0:7a00359e701e 393 }
masahikofukasawa 0:7a00359e701e 394
masahikofukasawa 0:7a00359e701e 395 AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) {
masahikofukasawa 0:7a00359e701e 396 int len = Message::getMaxMessageLength();
masahikofukasawa 0:7a00359e701e 397 char buf[len];
masahikofukasawa 0:7a00359e701e 398
masahikofukasawa 0:7a00359e701e 399 buf[0] = '$';
masahikofukasawa 0:7a00359e701e 400
masahikofukasawa 0:7a00359e701e 401 // Processes command
masahikofukasawa 0:7a00359e701e 402 char cmd = (char)msg->getCommand();
masahikofukasawa 0:7a00359e701e 403 Message::charToAscii(&buf[1], &cmd);
masahikofukasawa 0:7a00359e701e 404
masahikofukasawa 0:7a00359e701e 405 // Processes arguments
masahikofukasawa 0:7a00359e701e 406 for (int i=0; i < msg->getArgNum(); i++) {
masahikofukasawa 0:7a00359e701e 407 char arg = msg->getArgument(i);
masahikofukasawa 0:7a00359e701e 408 Message::charToAscii(&buf[3+2*i], &arg);
masahikofukasawa 0:7a00359e701e 409 }
masahikofukasawa 0:7a00359e701e 410
masahikofukasawa 0:7a00359e701e 411 // Add termination characters, 0x0D(\r), \n and \0, to the end of string
masahikofukasawa 0:7a00359e701e 412 int tIdx = 3 + 2 * (msg->getArgNum());
masahikofukasawa 0:7a00359e701e 413 int bufSize = sizeof(buf)/sizeof(buf[0]);
masahikofukasawa 0:7a00359e701e 414 if ((tIdx + 3) > (bufSize - 1)) {
masahikofukasawa 11:cef8dc1cf010 415 MSG("#Error: Message data exceeds the buffer.\r\n");
masahikofukasawa 0:7a00359e701e 416 return ERROR;
masahikofukasawa 0:7a00359e701e 417 }
masahikofukasawa 11:cef8dc1cf010 418 buf[tIdx++] = CR; // '\r'
masahikofukasawa 11:cef8dc1cf010 419 buf[tIdx++] = LF; // '\n'
masahikofukasawa 11:cef8dc1cf010 420 buf[tIdx] = '\0';
masahikofukasawa 0:7a00359e701e 421 if(isEnabledBle) uartService->writeString(buf);
masahikofukasawa 0:7a00359e701e 422 if(isEnabledUsb) serial->printf(buf);
masahikofukasawa 0:7a00359e701e 423
masahikofukasawa 0:7a00359e701e 424 return SUCCESS;
masahikofukasawa 11:cef8dc1cf010 425 }