Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmsensormanager.cpp@10:5c69b067d88a, 2016-07-08 (annotated)

Committer:
masahikofukasawa
Date:
Fri Jul 08 22:26:26 2016 +0000
Revision:
10:5c69b067d88a
Parent:
9:6fa3e7b17c27
Child:
11:cef8dc1cf010
RevD with AK09970 Release to Japan.

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