Taylor Street / AkmSensor

Modified for compatibility with Rev.E. hardware

Fork of AkmSensor by AKM Development Platform

akmsensormanager.cpp@33:d3e1e9eb2ef9, 2017-04-21 (annotated)

Committer:
tkstreet
Date:
Fri Apr 21 20:23:24 2017 +0000
Revision:
33:d3e1e9eb2ef9
Parent:
24:1d37438f31a9
Child:
34:1ea3357c8d9a
Added Motor Driver case. Added AP1017 functions. Included debugging MSG commands at various points.  Updated some comments.

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"
tkstreet 33:d3e1e9eb2ef9 11 #include "ap1017ctrl.h"
masahikofukasawa 0:7a00359e701e 12 #include "akmakd.h"
masahikofukasawa 0:7a00359e701e 13 #include "debug.h"
masahikofukasawa 0:7a00359e701e 14 #include "Message.h"
coisme 2:11fe67783c4c 15 #include "mcp342x.h"
masahikofukasawa 6:c4401549d68f 16 #include "I2CNano.h"
masahikofukasawa 0:7a00359e701e 17
masahikofukasawa 0:7a00359e701e 18 #define MAGNETOMETER_ID 0x0A
masahikofukasawa 0:7a00359e701e 19
masahikofukasawa 0:7a00359e701e 20 #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low)))
masahikofukasawa 0:7a00359e701e 21
masahikofukasawa 0:7a00359e701e 22
masahikofukasawa 10:5c69b067d88a 23 const char* AKM_PRIMARY_ID_STR[] = {
masahikofukasawa 7:e269411568c9 24 "AKD Daughter Cards(SPI)",
masahikofukasawa 0:7a00359e701e 25 "Switch, Unipolar",
masahikofukasawa 0:7a00359e701e 26 "Switch, Onmipolar",
masahikofukasawa 0:7a00359e701e 27 "Latch, Bipolar",
masahikofukasawa 0:7a00359e701e 28 "Switch, Dual Output",
masahikofukasawa 0:7a00359e701e 29 "Onechip Encoder",
masahikofukasawa 0:7a00359e701e 30 "TBD1",
masahikofukasawa 0:7a00359e701e 31 "TBD2",
masahikofukasawa 0:7a00359e701e 32 "Linear Sensor Legacy",
masahikofukasawa 0:7a00359e701e 33 "Current Sensor",
masahikofukasawa 0:7a00359e701e 34 "MISC(Analog)",
masahikofukasawa 0:7a00359e701e 35 "Linear Sensor",
masahikofukasawa 18:b7182d5ad8d5 36 "Motor Drivers",
masahikofukasawa 1:b46b8653331f 37 "IR Sensor",
masahikofukasawa 7:e269411568c9 38 "Angle Sensor(SPI)",
masahikofukasawa 10:5c69b067d88a 39 "AKD Daughter Cards(I2C)"
masahikofukasawa 0:7a00359e701e 40 };
masahikofukasawa 0:7a00359e701e 41
masahikofukasawa 13:d008249f0359 42 AkmSensorManager::AkmSensorManager(SerialNano* com)
masahikofukasawa 0:7a00359e701e 43 {
masahikofukasawa 0:7a00359e701e 44 serial = com;
masahikofukasawa 0:7a00359e701e 45 isEnabledUsb = true;
masahikofukasawa 0:7a00359e701e 46 eventCommandReceived = false;
masahikofukasawa 0:7a00359e701e 47 eventConnected = false;
masahikofukasawa 0:7a00359e701e 48 eventDisconnected = false;
masahikofukasawa 0:7a00359e701e 49 }
masahikofukasawa 0:7a00359e701e 50
masahikofukasawa 10:5c69b067d88a 51 AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid)
masahikofukasawa 0:7a00359e701e 52 {
masahikofukasawa 10:5c69b067d88a 53 primaryId = id;
masahikofukasawa 10:5c69b067d88a 54 subId = subid;
masahikofukasawa 0:7a00359e701e 55 sensor = AkmSensorManager::getAkmSensor();
masahikofukasawa 0:7a00359e701e 56 if(sensor == NULL) return AkmSensorManager::ERROR;
masahikofukasawa 0:7a00359e701e 57 return AkmSensorManager::SUCCESS;
masahikofukasawa 0:7a00359e701e 58 }
masahikofukasawa 0:7a00359e701e 59
masahikofukasawa 13:d008249f0359 60
masahikofukasawa 13:d008249f0359 61 void AkmSensorManager::setBleUartService(UARTService* service)
masahikofukasawa 13:d008249f0359 62 {
masahikofukasawa 13:d008249f0359 63 uartService = service;
masahikofukasawa 13:d008249f0359 64 isEnabledBle = true;
masahikofukasawa 13:d008249f0359 65 }
masahikofukasawa 13:d008249f0359 66
masahikofukasawa 0:7a00359e701e 67 void AkmSensorManager::setEventConnected()
masahikofukasawa 0:7a00359e701e 68 {
masahikofukasawa 0:7a00359e701e 69 eventConnected = true;
masahikofukasawa 0:7a00359e701e 70 }
masahikofukasawa 0:7a00359e701e 71
masahikofukasawa 0:7a00359e701e 72 void AkmSensorManager::setEventDisconnected()
masahikofukasawa 0:7a00359e701e 73 {
masahikofukasawa 0:7a00359e701e 74 eventDisconnected = true;
masahikofukasawa 0:7a00359e701e 75 }
masahikofukasawa 0:7a00359e701e 76
masahikofukasawa 0:7a00359e701e 77 AkmSensor* AkmSensorManager::getAkmSensor()
masahikofukasawa 0:7a00359e701e 78 {
masahikofukasawa 0:7a00359e701e 79 AkmSensor* sensor = NULL;
masahikofukasawa 0:7a00359e701e 80
masahikofukasawa 10:5c69b067d88a 81 switch(primaryId){
masahikofukasawa 17:9abb7c28709c 82
masahikofukasawa 7:e269411568c9 83 case AkmSensor::AKM_PRIMARY_ID_AKD_SPI:
masahikofukasawa 0:7a00359e701e 84 case AkmSensor::AKM_PRIMARY_ID_AKD_I2C:
masahikofukasawa 10:5c69b067d88a 85 {
masahikofukasawa 10:5c69b067d88a 86 if(subId != Ak09970Ctrl::SUB_ID_AK09970){
masahikofukasawa 10:5c69b067d88a 87 AkmAkd* akd = new AkmAkd();
masahikofukasawa 10:5c69b067d88a 88 sensor = akd;
masahikofukasawa 10:5c69b067d88a 89 }
masahikofukasawa 10:5c69b067d88a 90 else{
masahikofukasawa 10:5c69b067d88a 91 Ak09970Ctrl* ak09970 = new Ak09970Ctrl();
masahikofukasawa 10:5c69b067d88a 92 sensor = ak09970;
masahikofukasawa 10:5c69b067d88a 93 }
masahikofukasawa 0:7a00359e701e 94 break;
masahikofukasawa 10:5c69b067d88a 95 }
masahikofukasawa 17:9abb7c28709c 96
masahikofukasawa 0:7a00359e701e 97 case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR:
masahikofukasawa 10:5c69b067d88a 98 {
masahikofukasawa 11:cef8dc1cf010 99 if(subId == Ak7451Ctrl::SUB_ID_AK7451){
masahikofukasawa 11:cef8dc1cf010 100 Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl();
masahikofukasawa 11:cef8dc1cf010 101 sensor = ak7451ctrl;
masahikofukasawa 10:5c69b067d88a 102 break;
masahikofukasawa 10:5c69b067d88a 103 }
masahikofukasawa 10:5c69b067d88a 104 else if(subId == Ak7401Ctrl::SUB_ID_AK7401){
masahikofukasawa 10:5c69b067d88a 105 Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl();
masahikofukasawa 10:5c69b067d88a 106 sensor = ak7401ctrl;
masahikofukasawa 10:5c69b067d88a 107 break;
masahikofukasawa 10:5c69b067d88a 108 }
masahikofukasawa 0:7a00359e701e 109 break;
masahikofukasawa 10:5c69b067d88a 110 }
masahikofukasawa 17:9abb7c28709c 111
masahikofukasawa 0:7a00359e701e 112 case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR:
masahikofukasawa 0:7a00359e701e 113 case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR:
masahikofukasawa 0:7a00359e701e 114 case AkmSensor::AKM_PRIMARY_ID_LATCH:
masahikofukasawa 0:7a00359e701e 115 case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT:
masahikofukasawa 0:7a00359e701e 116 case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER:
masahikofukasawa 10:5c69b067d88a 117 {
masahikofukasawa 10:5c69b067d88a 118 AkmHallSwitch* hallswitch = new AkmHallSwitch();
masahikofukasawa 0:7a00359e701e 119 sensor = hallswitch;
masahikofukasawa 0:7a00359e701e 120 break;
masahikofukasawa 10:5c69b067d88a 121 }
masahikofukasawa 17:9abb7c28709c 122 // case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY:
masahikofukasawa 4:af13b985c689 123 case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR:
masahikofukasawa 10:5c69b067d88a 124 case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR:
masahikofukasawa 10:5c69b067d88a 125 case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG:
masahikofukasawa 10:5c69b067d88a 126 {
masahikofukasawa 11:cef8dc1cf010 127 AkmAnalogSensor* analogsensor = new AkmAnalogSensor();
masahikofukasawa 11:cef8dc1cf010 128 sensor = analogsensor;
masahikofukasawa 0:7a00359e701e 129 break;
masahikofukasawa 10:5c69b067d88a 130 }
masahikofukasawa 17:9abb7c28709c 131
masahikofukasawa 1:b46b8653331f 132 case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR:
masahikofukasawa 10:5c69b067d88a 133 {
masahikofukasawa 10:5c69b067d88a 134 if(subId == Ak9750Ctrl::SUB_ID_AK9750){
masahikofukasawa 10:5c69b067d88a 135 Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl();
masahikofukasawa 10:5c69b067d88a 136 sensor = ak9750ctrl;
masahikofukasawa 15:1238993fd75f 137 }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){
masahikofukasawa 15:1238993fd75f 138 Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl();
masahikofukasawa 15:1238993fd75f 139 sensor = ak9753ctrl;
masahikofukasawa 10:5c69b067d88a 140 }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){
masahikofukasawa 10:5c69b067d88a 141 Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl();
masahikofukasawa 10:5c69b067d88a 142 sensor = ak9752ctrl;
masahikofukasawa 10:5c69b067d88a 143 }else{
masahikofukasawa 10:5c69b067d88a 144 return NULL; // couldn't find
masahikofukasawa 10:5c69b067d88a 145 }
masahikofukasawa 10:5c69b067d88a 146
masahikofukasawa 1:b46b8653331f 147 break;
masahikofukasawa 10:5c69b067d88a 148 }
tkstreet 33:d3e1e9eb2ef9 149 case AkmSensor::AKM_PRIMARY_ID_MOTOR_DRIVER:
tkstreet 33:d3e1e9eb2ef9 150 { // TODO: Other motor driver cases
tkstreet 33:d3e1e9eb2ef9 151 if(subId == Ap1017Ctrl::SUB_ID_AP1017){
tkstreet 33:d3e1e9eb2ef9 152 Ap1017Ctrl* ap1017ctrl = new Ap1017Ctrl();
tkstreet 33:d3e1e9eb2ef9 153 sensor = ap1017ctrl;
tkstreet 33:d3e1e9eb2ef9 154 }else{
tkstreet 33:d3e1e9eb2ef9 155 return NULL;
tkstreet 33:d3e1e9eb2ef9 156 }
tkstreet 33:d3e1e9eb2ef9 157 break;
tkstreet 33:d3e1e9eb2ef9 158 }
masahikofukasawa 0:7a00359e701e 159 default:
masahikofukasawa 10:5c69b067d88a 160 {
masahikofukasawa 17:9abb7c28709c 161 MSG("#Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 162 return NULL; // couldn't find
masahikofukasawa 10:5c69b067d88a 163 }
masahikofukasawa 0:7a00359e701e 164 }
masahikofukasawa 11:cef8dc1cf010 165
masahikofukasawa 10:5c69b067d88a 166 if(sensor->init(primaryId, subId) != AkmSensor::SUCCESS){
masahikofukasawa 10:5c69b067d88a 167 MSG("#sensor->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 168 return NULL; // couldn't find
masahikofukasawa 0:7a00359e701e 169 }
masahikofukasawa 0:7a00359e701e 170
masahikofukasawa 10:5c69b067d88a 171 MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]);
masahikofukasawa 0:7a00359e701e 172 return sensor;
masahikofukasawa 0:7a00359e701e 173 }
masahikofukasawa 0:7a00359e701e 174
masahikofukasawa 16:d85be9bafb80 175
masahikofukasawa 16:d85be9bafb80 176 void AkmSensorManager::dummyCallbackForCommandReceived(){}
masahikofukasawa 16:d85be9bafb80 177
tkstreet 33:d3e1e9eb2ef9 178
tkstreet 33:d3e1e9eb2ef9 179 // For commands received via BLE
masahikofukasawa 0:7a00359e701e 180 AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){
masahikofukasawa 0:7a00359e701e 181 // Construct message
masahikofukasawa 0:7a00359e701e 182 Status status = SUCCESS;
masahikofukasawa 0:7a00359e701e 183
masahikofukasawa 10:5c69b067d88a 184 if ((Message::parse(&msg, buf)) != Message::SUCCESS) {
masahikofukasawa 11:cef8dc1cf010 185 MSG("#Failed to parse message. %s\r\n", buf);
masahikofukasawa 0:7a00359e701e 186 status = ERROR;
tkstreet 33:d3e1e9eb2ef9 187 eventCommandReceived = false; // Reset flag
masahikofukasawa 15:1238993fd75f 188 }else{
tkstreet 33:d3e1e9eb2ef9 189 eventCommandReceived = true; // Set flag
masahikofukasawa 16:d85be9bafb80 190 // MSG("#Parsed message. %s\r\n", buf);
masahikofukasawa 0:7a00359e701e 191 }
masahikofukasawa 16:d85be9bafb80 192 t.attach(callback(this, &AkmSensorManager::dummyCallbackForCommandReceived),0); // wake-up from ble.waitForEvent
masahikofukasawa 0:7a00359e701e 193 return status;
masahikofukasawa 0:7a00359e701e 194 }
masahikofukasawa 0:7a00359e701e 195
tkstreet 33:d3e1e9eb2ef9 196
coisme 2:11fe67783c4c 197 int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) {
coisme 2:11fe67783c4c 198 const int WAIT_ADC_MS = 1;
coisme 2:11fe67783c4c 199
coisme 2:11fe67783c4c 200 // Configure channel and trigger.
coisme 2:11fe67783c4c 201 mcp3428->setChannel(ch);
coisme 2:11fe67783c4c 202 mcp3428->setSampleSetting(s);
coisme 2:11fe67783c4c 203 mcp3428->trigger();
coisme 2:11fe67783c4c 204
coisme 2:11fe67783c4c 205 // polling data (!blocking)
coisme 2:11fe67783c4c 206 MCP342X::Data data;
coisme 2:11fe67783c4c 207 do {
coisme 2:11fe67783c4c 208 wait_ms(WAIT_ADC_MS);
coisme 2:11fe67783c4c 209 mcp3428->getData(&data);
coisme 2:11fe67783c4c 210 } while(data.st == MCP342X::DATA_NOT_UPDATED);
coisme 2:11fe67783c4c 211
coisme 2:11fe67783c4c 212 return data.value;
coisme 2:11fe67783c4c 213 }
masahikofukasawa 0:7a00359e701e 214
tkstreet 33:d3e1e9eb2ef9 215
masahikofukasawa 0:7a00359e701e 216 uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits)
masahikofukasawa 0:7a00359e701e 217 {
masahikofukasawa 11:cef8dc1cf010 218 MSG("#GetID\r\n");
masahikofukasawa 6:c4401549d68f 219
tkstreet 21:966724730ce6 220 I2C i2c(I2C_SDA, I2C_SCL); // establish I2C to read ID
tkstreet 21:966724730ce6 221
coisme 2:11fe67783c4c 222 // ADC
tkstreet 21:966724730ce6 223 MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH); // ADC to convert voltage
tkstreet 21:966724730ce6 224 mcp342x.setConversionMode(MCP342X::ONE_SHOT); // Set to single sample
coisme 2:11fe67783c4c 225 MCP342X::AdcChannel ch;
tkstreet 21:966724730ce6 226
tkstreet 21:966724730ce6 227 if (pin == ANALOG_SENSOR_ID) { // Primary ID
coisme 2:11fe67783c4c 228 ch = MCP342X::ADC_CH1;
tkstreet 21:966724730ce6 229 } else { // pin == ANALOG_SENSOR_ID_SUB
tkstreet 21:966724730ce6 230 ch = MCP342X::ADC_CH2; // Secondary ID
coisme 2:11fe67783c4c 231 }
tkstreet 21:966724730ce6 232
coisme 2:11fe67783c4c 233 int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT);
masahikofukasawa 11:cef8dc1cf010 234 MSG("#12bit ADC Val = %d.\r\n", val);
coisme 2:11fe67783c4c 235
tkstreet 21:966724730ce6 236 // Voltage boundaries for ID voltage divider system
coisme 2:11fe67783c4c 237 const int16_t VAL_MAX = 3000-2048; // Corresponds to 3V
coisme 2:11fe67783c4c 238 const int16_t VAL_MIN = -2048; // Corresponds to 0V
coisme 2:11fe67783c4c 239
tkstreet 21:966724730ce6 240 // Convert voltage to ID value
masahikofukasawa 6:c4401549d68f 241 uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5);
masahikofukasawa 11:cef8dc1cf010 242 MSG("#ID = %d.\r\n", value);
masahikofukasawa 6:c4401549d68f 243
masahikofukasawa 0:7a00359e701e 244 return value;
masahikofukasawa 0:7a00359e701e 245 }
masahikofukasawa 0:7a00359e701e 246
tkstreet 33:d3e1e9eb2ef9 247
masahikofukasawa 0:7a00359e701e 248 bool AkmSensorManager::isEvent()
masahikofukasawa 0:7a00359e701e 249 {
masahikofukasawa 0:7a00359e701e 250 return (sensor->isEvent() ||
masahikofukasawa 0:7a00359e701e 251 eventCommandReceived ||
masahikofukasawa 0:7a00359e701e 252 eventConnected ||
masahikofukasawa 0:7a00359e701e 253 eventDisconnected);
masahikofukasawa 0:7a00359e701e 254 }
masahikofukasawa 0:7a00359e701e 255
tkstreet 33:d3e1e9eb2ef9 256
masahikofukasawa 0:7a00359e701e 257 void AkmSensorManager::processCommand()
masahikofukasawa 0:7a00359e701e 258 {
tkstreet 23:50c98b286e41 259 // Extracts command contained in the message
masahikofukasawa 0:7a00359e701e 260 Message::Command cmd = msg.getCommand();
masahikofukasawa 0:7a00359e701e 261
tkstreet 21:966724730ce6 262 // Creates a message object to return
masahikofukasawa 0:7a00359e701e 263 Message resMsg;
masahikofukasawa 0:7a00359e701e 264
tkstreet 23:50c98b286e41 265 // Return message contains the extracted command
masahikofukasawa 0:7a00359e701e 266 resMsg.setCommand(cmd);
masahikofukasawa 0:7a00359e701e 267
masahikofukasawa 0:7a00359e701e 268 switch(cmd)
masahikofukasawa 0:7a00359e701e 269 {
masahikofukasawa 0:7a00359e701e 270 case Message::CMD_GET_FW_VERSION:
masahikofukasawa 11:cef8dc1cf010 271 {
masahikofukasawa 0:7a00359e701e 272 resMsg.setArgument(0, FIRMWARE_VERSION);
masahikofukasawa 0:7a00359e701e 273 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 274 MSG("#FW version is reported.\r\n");
masahikofukasawa 0:7a00359e701e 275 break;
masahikofukasawa 11:cef8dc1cf010 276 }
masahikofukasawa 0:7a00359e701e 277 case Message::CMD_GET_MAG_PART:
masahikofukasawa 11:cef8dc1cf010 278 {
masahikofukasawa 0:7a00359e701e 279 resMsg.setArgument(0, MAGNETOMETER_ID);
masahikofukasawa 0:7a00359e701e 280 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 281 MSG("#Mag ID is reported.\r\n");
masahikofukasawa 0:7a00359e701e 282 break;
masahikofukasawa 11:cef8dc1cf010 283 }
masahikofukasawa 0:7a00359e701e 284 case Message::CMD_SET_SERIAL_TARGET:
masahikofukasawa 11:cef8dc1cf010 285 {
masahikofukasawa 0:7a00359e701e 286 isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false;
masahikofukasawa 0:7a00359e701e 287 isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false;
masahikofukasawa 0:7a00359e701e 288 break;
masahikofukasawa 11:cef8dc1cf010 289 }
masahikofukasawa 0:7a00359e701e 290 case Message::CMD_GET_ID: // return Primary ID and Sub ID
masahikofukasawa 11:cef8dc1cf010 291 {
masahikofukasawa 10:5c69b067d88a 292 resMsg.setArgument(0, primaryId);
masahikofukasawa 0:7a00359e701e 293 resMsg.setArgument(1, subId);
masahikofukasawa 0:7a00359e701e 294 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 295 MSG("#Mag ID is reported.\r\n");
masahikofukasawa 0:7a00359e701e 296 break;
masahikofukasawa 11:cef8dc1cf010 297 }
masahikofukasawa 0:7a00359e701e 298 case Message::CMD_STOP_MEASUREMENT:
masahikofukasawa 11:cef8dc1cf010 299 {
masahikofukasawa 15:1238993fd75f 300 if( sensor->stopSensor() != AkmSensor::SUCCESS){
masahikofukasawa 15:1238993fd75f 301 resMsg.setArgument(0, 1);
masahikofukasawa 15:1238993fd75f 302 }else{
masahikofukasawa 15:1238993fd75f 303 resMsg.setArgument(0, 0);
masahikofukasawa 15:1238993fd75f 304 }
masahikofukasawa 0:7a00359e701e 305 throwMessage(&resMsg);
masahikofukasawa 11:cef8dc1cf010 306 MSG("#Stop measurement.\r\n");
masahikofukasawa 0:7a00359e701e 307 break;
masahikofukasawa 11:cef8dc1cf010 308 }
masahikofukasawa 0:7a00359e701e 309 case Message::CMD_START_MEASUREMENT:
masahikofukasawa 11:cef8dc1cf010 310 {
masahikofukasawa 11:cef8dc1cf010 311 int error_code = AkmSensor::SUCCESS;
masahikofukasawa 11:cef8dc1cf010 312 if(msg.getArgNum() == 0){
masahikofukasawa 11:cef8dc1cf010 313 error_code = sensor->startSensor();
masahikofukasawa 11:cef8dc1cf010 314 if( error_code != AkmSensor::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 315 MSG("#StartSensor Error. Code=%d\r\n",error_code);
masahikofukasawa 11:cef8dc1cf010 316 }
masahikofukasawa 11:cef8dc1cf010 317 else{
masahikofukasawa 11:cef8dc1cf010 318 MSG("#Start measurement.\r\n");
masahikofukasawa 11:cef8dc1cf010 319 }
masahikofukasawa 11:cef8dc1cf010 320 }else if(msg.getArgNum() == 1){
masahikofukasawa 11:cef8dc1cf010 321 float interval = (float)(1.0 / (float)msg.getArgument(0));
masahikofukasawa 11:cef8dc1cf010 322 error_code = sensor->startSensor(interval);
masahikofukasawa 11:cef8dc1cf010 323 if( error_code != AkmSensor::SUCCESS ){
masahikofukasawa 11:cef8dc1cf010 324 MSG("#StartSensor Error. Code=%d\r\n",error_code);
masahikofukasawa 11:cef8dc1cf010 325 }
masahikofukasawa 11:cef8dc1cf010 326 else{
masahikofukasawa 11:cef8dc1cf010 327 MSG("#Start measurement.\r\n");
masahikofukasawa 11:cef8dc1cf010 328 }
masahikofukasawa 11:cef8dc1cf010 329 }else{
masahikofukasawa 11:cef8dc1cf010 330 MSG("#StartSensor Error. Wrong Argument num.\r\n");
masahikofukasawa 11:cef8dc1cf010 331 }
masahikofukasawa 11:cef8dc1cf010 332 if(error_code == AkmSensor::SUCCESS){
masahikofukasawa 11:cef8dc1cf010 333 // get initial sensor state for switch type sensors
masahikofukasawa 11:cef8dc1cf010 334 if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR ||
masahikofukasawa 11:cef8dc1cf010 335 primaryId == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR ||
masahikofukasawa 11:cef8dc1cf010 336 primaryId == AkmSensor::AKM_PRIMARY_ID_LATCH ||
masahikofukasawa 11:cef8dc1cf010 337 primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT ||
masahikofukasawa 11:cef8dc1cf010 338 primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){
masahikofukasawa 11:cef8dc1cf010 339 Message temp;
masahikofukasawa 11:cef8dc1cf010 340 sensor->readSensorData(&temp);
masahikofukasawa 11:cef8dc1cf010 341 throwMessage(&temp);
masahikofukasawa 11:cef8dc1cf010 342 }
masahikofukasawa 0:7a00359e701e 343 }
masahikofukasawa 0:7a00359e701e 344 break;
masahikofukasawa 11:cef8dc1cf010 345 }
tkstreet 33:d3e1e9eb2ef9 346 case Message::CMD_MOTOR_START_MOTOR:
tkstreet 33:d3e1e9eb2ef9 347 case Message::CMD_MOTOR_STOP_MOTOR:
tkstreet 33:d3e1e9eb2ef9 348 case Message::CMD_MOTOR_SET_DIRECTION:
tkstreet 33:d3e1e9eb2ef9 349 case Message::CMD_MOTOR_SET_DUTY_CYCLE:
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 11:cef8dc1cf010 381 {
masahikofukasawa 15:1238993fd75f 382 AkmSensor::Status st = sensor->requestCommand(&msg,&resMsg);
masahikofukasawa 15:1238993fd75f 383 if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) )
masahikofukasawa 15:1238993fd75f 384 {
masahikofukasawa 15:1238993fd75f 385 MSG("#Command failed.\r\n");
masahikofukasawa 15:1238993fd75f 386 }else{
masahikofukasawa 15:1238993fd75f 387 throwMessage(&resMsg);
masahikofukasawa 15:1238993fd75f 388 }
masahikofukasawa 0:7a00359e701e 389 break;
masahikofukasawa 11:cef8dc1cf010 390 }
masahikofukasawa 0:7a00359e701e 391 default:
masahikofukasawa 11:cef8dc1cf010 392 {
masahikofukasawa 15:1238993fd75f 393 MSG("#Can't find command.\r\n");
masahikofukasawa 0:7a00359e701e 394 break;
masahikofukasawa 11:cef8dc1cf010 395 }
masahikofukasawa 0:7a00359e701e 396 }
masahikofukasawa 0:7a00359e701e 397 }
masahikofukasawa 0:7a00359e701e 398
masahikofukasawa 0:7a00359e701e 399 AkmSensorManager::Status AkmSensorManager::processEvent()
masahikofukasawa 0:7a00359e701e 400 {
masahikofukasawa 0:7a00359e701e 401 AkmSensorManager::Status status = AkmSensorManager::SUCCESS;
masahikofukasawa 0:7a00359e701e 402
tkstreet 33:d3e1e9eb2ef9 403 // If event is a command received from BLE
masahikofukasawa 0:7a00359e701e 404 if(eventCommandReceived)
masahikofukasawa 0:7a00359e701e 405 {
masahikofukasawa 16:d85be9bafb80 406 // MSG("#Command received.\r\n");
masahikofukasawa 0:7a00359e701e 407 processCommand();
tkstreet 21:966724730ce6 408 eventCommandReceived = false; // clear the flag
masahikofukasawa 0:7a00359e701e 409 }
tkstreet 23:50c98b286e41 410
tkstreet 23:50c98b286e41 411 // If event is sensor read data event, read the message
tkstreet 23:50c98b286e41 412 if(sensor->isEvent())
masahikofukasawa 0:7a00359e701e 413 {
masahikofukasawa 16:d85be9bafb80 414 // MSG("#Sensor event received.\r\n");
masahikofukasawa 0:7a00359e701e 415 Message msg;
masahikofukasawa 0:7a00359e701e 416 if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
tkstreet 23:50c98b286e41 417 throwMessage(&msg); // Process and output message to USB/BLE
masahikofukasawa 0:7a00359e701e 418 }
masahikofukasawa 0:7a00359e701e 419
tkstreet 23:50c98b286e41 420 // If event is the BLE being connected, clear the flag and start sensor
tkstreet 23:50c98b286e41 421 if(eventConnected)
masahikofukasawa 0:7a00359e701e 422 {
tkstreet 21:966724730ce6 423 eventConnected = false; // clear the flag
masahikofukasawa 12:c06cd8b76358 424 MSG("#BLE connected.\r\n");
masahikofukasawa 0:7a00359e701e 425 }
tkstreet 23:50c98b286e41 426
tkstreet 23:50c98b286e41 427 // If event is the BLE being disconnected, stop the sensor
tkstreet 23:50c98b286e41 428 if(eventDisconnected)
masahikofukasawa 0:7a00359e701e 429 {
masahikofukasawa 12:c06cd8b76358 430 MSG("#BLE dis-connected.\r\n");
masahikofukasawa 0:7a00359e701e 431 if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR;
tkstreet 21:966724730ce6 432 eventDisconnected = false; // clear the flag
masahikofukasawa 0:7a00359e701e 433 }
masahikofukasawa 0:7a00359e701e 434 return status;
masahikofukasawa 0:7a00359e701e 435 }
masahikofukasawa 0:7a00359e701e 436
masahikofukasawa 0:7a00359e701e 437 AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) {
masahikofukasawa 0:7a00359e701e 438 int len = Message::getMaxMessageLength();
masahikofukasawa 0:7a00359e701e 439 char buf[len];
masahikofukasawa 0:7a00359e701e 440
tkstreet 23:50c98b286e41 441 buf[0] = '$'; // Output message prefix
masahikofukasawa 0:7a00359e701e 442
tkstreet 23:50c98b286e41 443 // Processes command in msg to ASCII
masahikofukasawa 0:7a00359e701e 444 char cmd = (char)msg->getCommand();
masahikofukasawa 0:7a00359e701e 445 Message::charToAscii(&buf[1], &cmd);
masahikofukasawa 0:7a00359e701e 446
tkstreet 23:50c98b286e41 447 // Processes arguments in msg to ASCII
masahikofukasawa 0:7a00359e701e 448 for (int i=0; i < msg->getArgNum(); i++) {
masahikofukasawa 0:7a00359e701e 449 char arg = msg->getArgument(i);
masahikofukasawa 0:7a00359e701e 450 Message::charToAscii(&buf[3+2*i], &arg);
masahikofukasawa 0:7a00359e701e 451 }
masahikofukasawa 0:7a00359e701e 452
masahikofukasawa 0:7a00359e701e 453 // Add termination characters, 0x0D(\r), \n and \0, to the end of string
masahikofukasawa 0:7a00359e701e 454 int tIdx = 3 + 2 * (msg->getArgNum());
masahikofukasawa 0:7a00359e701e 455 int bufSize = sizeof(buf)/sizeof(buf[0]);
masahikofukasawa 0:7a00359e701e 456 if ((tIdx + 3) > (bufSize - 1)) {
masahikofukasawa 11:cef8dc1cf010 457 MSG("#Error: Message data exceeds the buffer.\r\n");
masahikofukasawa 0:7a00359e701e 458 return ERROR;
masahikofukasawa 0:7a00359e701e 459 }
masahikofukasawa 11:cef8dc1cf010 460 buf[tIdx++] = CR; // '\r'
masahikofukasawa 11:cef8dc1cf010 461 buf[tIdx++] = LF; // '\n'
masahikofukasawa 11:cef8dc1cf010 462 buf[tIdx] = '\0';
tkstreet 23:50c98b286e41 463
tkstreet 33:d3e1e9eb2ef9 464 // If BLE is enabled, send to AKDP app
tkstreet 23:50c98b286e41 465 if(isEnabledBle) uartService->writeString(buf);
tkstreet 23:50c98b286e41 466
tkstreet 33:d3e1e9eb2ef9 467 // If USB is enabled, send to serial terminal
masahikofukasawa 0:7a00359e701e 468 if(isEnabledUsb) serial->printf(buf);
masahikofukasawa 0:7a00359e701e 469
masahikofukasawa 0:7a00359e701e 470 return SUCCESS;
masahikofukasawa 12:c06cd8b76358 471 }
masahikofukasawa 13:d008249f0359 472
masahikofukasawa 13:d008249f0359 473 char* AkmSensorManager::getSensorName(){
masahikofukasawa 13:d008249f0359 474 return sensor->getSensorName();
masahikofukasawa 13:d008249f0359 475 }