Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmsensormanager.cpp@28:dc4eb14e4d7e, 2017-03-11 (annotated)

Committer:
masahikofukasawa
Date:
Sat Mar 11 02:08:32 2017 +0000
Revision:
28:dc4eb14e4d7e
Parent:
27:41aa9fb23a2f
Child:
29:b488d2c89fba
modified for multi sensor demo. cont....

Who changed what in which revision?

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