Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: akmsensormanager.cpp
- Revision:
- 0:7a00359e701e
- Child:
- 1:b46b8653331f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/akmsensormanager.cpp Thu Apr 28 21:12:04 2016 +0000 @@ -0,0 +1,366 @@ +#include "ble/services/UARTService.h" +#include "akmsensormanager.h" +#include "akmanglesensor.h" +#include "akmhallswitch.h" +#include "akmlinearsensor.h" +#include "akmakd.h" +#include "debug.h" +#include "Message.h" + +#define FIRMWARE_VERSION 0x02 +#define MAGNETOMETER_ID 0x0A + +#define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) + + +static char* AKM_PRIMARY_ID_STR[] = { + "AKD Daughter Cards(SPI)", // Temporary + "Switch, Unipolar", + "Switch, Onmipolar", + "Latch, Bipolar", + "Switch, Dual Output", + "Onechip Encoder", + "TBD1", + "TBD2", + "Linear Sensor Legacy", + "Current Sensor", + "MISC(Analog)", + "Linear Sensor", + "TBD3", + "TBD4", + "Angle Sensor(SPI)", // Temporary + "AKD Daughter Cards(I2C)", + }; + +AkmSensorManager::AkmSensorManager(SerialNano* com, UARTService* service) +{ + serial = com; + uartService = service; + isEnabledBle = true; + isEnabledUsb = true; + eventCommandReceived = false; + eventConnected = false; + eventDisconnected = false; +} + +AkmSensorManager::Status AkmSensorManager::init() +{ + sensor = AkmSensorManager::getAkmSensor(); + if(sensor == NULL) return AkmSensorManager::ERROR; + return AkmSensorManager::SUCCESS; +} + +void AkmSensorManager::setEventConnected() +{ + eventConnected = true; +} + +void AkmSensorManager::setEventDisconnected() +{ + eventDisconnected = true; +} + +AkmSensor* AkmSensorManager::getAkmSensor() +{ + AkmSensor* sensor = NULL; + + // primary id check + id = AkmSensorManager::getId(ANALOG_SENSOR_ID,4); + + // secondary id check + switch(id){ + case AkmSensor::AKM_PRIMARY_ID_AKD_I2C: + subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,5); // 5bit sub id + AkmAkd* akd; + akd = new AkmAkd(); + sensor = akd; + break; + + case AkmSensor::AKM_PRIMARY_ID_AKD_SPI: // Temporary + case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR: + subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id + AkmAngleSensor* angleSensor; + angleSensor = new AkmAngleSensor(); + sensor = angleSensor; + break; + + case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR: + case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR: + case AkmSensor::AKM_PRIMARY_ID_LATCH: + case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT: + case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER: + subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id + AkmHallSwitch* hallswitch; + hallswitch = new AkmHallSwitch(); + sensor = hallswitch; + break; + + case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY: + subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id + AkmLinearSensor* linearsensor; + linearsensor = new AkmLinearSensor(); + sensor = linearsensor; + break; + + default: + subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id + MSG("#Can't find ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); + return NULL; // couldn't find + } + + if(sensor->init(id, subId) != AkmSensor::SUCCESS){ + MSG("#Can't find ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); + return NULL; // couldn't find + } + + MSG("#ID=%d SubID=%d %s\r\n", id, subId, AKM_PRIMARY_ID_STR[id]); + return sensor; +} + +AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){ + // Construct message + Status status = SUCCESS; + + Message::Status st; + if ((st=Message::parse(&msg, buf)) != Message::SUCCESS) { + MSG("#Failed to parse message. status = %02x. %s\n", st, buf); + status = ERROR; + eventCommandReceived = false; + } + eventCommandReceived = true; + return status; +} + + +uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits) +{ + wait(0.05); + AnalogIn id(pin); + double s = id + 1.0/(double)(pow(2.0,bits+1)); + uint8_t value = (uint8_t)(s*pow(2.0,bits)); + wait(0.05); + return value; +} + +bool AkmSensorManager::isEvent() +{ + return (sensor->isEvent() || + eventCommandReceived || + eventConnected || + eventDisconnected); +} + + +void AkmSensorManager::processCommand() +{ + AkmSensorManager::Status status = AkmSensorManager::SUCCESS; +// MSG("#processCommand.\n"); + + // Gets command in the message + Message::Command cmd = msg.getCommand(); + + // Creates an message object to return + Message resMsg; + + // Return message has the same command as input + resMsg.setCommand(cmd); + + switch(cmd) + { + case Message::CMD_GET_FW_VERSION: + resMsg.setArgument(0, FIRMWARE_VERSION); + throwMessage(&resMsg); + MSG("#FW version is reported.\n"); + break; + + case Message::CMD_GET_MAG_PART: + resMsg.setArgument(0, MAGNETOMETER_ID); + throwMessage(&resMsg); + MSG("#Mag ID is reported.\n"); + break; + + case Message::CMD_SET_SERIAL_TARGET: + isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false; + isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false; + resMsg.setArgument(0, 0); + throwMessage(&resMsg); + MSG("#Serial out is set.\n"); + break; + + case Message::CMD_GET_ID: // return Primary ID and Sub ID + if(id == 0) id = 0xE; // Temporary for Angle Sensor + resMsg.setArgument(0, id); + resMsg.setArgument(1, subId); + throwMessage(&resMsg); + wait(0.4); // wait for App initialization + MSG("#Mag ID is reported.\n"); + break; + + case Message::CMD_STOP_MEASUREMENT: + if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + resMsg.setArgument(0, status==AkmSensorManager::SUCCESS ? 0 : 1); + throwMessage(&resMsg); + MSG("#Stop measurement.\n"); + break; + + case Message::CMD_START_MEASUREMENT: + MSG("#Start measurement.\n"); + sensor->startSensor(); + // get initial sensor state for switch type sensors + if( id == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR || + id == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR || + id == AkmSensor::AKM_PRIMARY_ID_LATCH || + id == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT || + id == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){ + Message temp; + sensor->readSensorData(&temp); + throwMessage(&temp); + } + break; + + case Message::CMD_ANGLE_ZERO_RESET: + sensor->requestCommand(&msg,&resMsg); + throwMessage(&resMsg); + break; +/* + case Message::CMD_IR_GET_THRESHOLD: + AK9750::Threshold th; + if (Hardware::getInstance()->ak9750.getThreshold(&th) != AK9750::SUCCESS) { + MSG("Failed to get threshold of AK9750.\n"); + return; + } + resMsg.setArgument(0, (char)(th.eth13h >> 8)); + resMsg.setArgument(1, (char)(th.eth13h & 0x00FF)); + resMsg.setArgument(2, (char)(th.eth13l >> 8)); + resMsg.setArgument(3, (char)(th.eth13l & 0x00FF)); + resMsg.setArgument(4, (char)(th.eth24h >> 8)); + resMsg.setArgument(5, (char)(th.eth24h & 0x00FF)); + resMsg.setArgument(6, (char)(th.eth24l >> 8)); + resMsg.setArgument(7, (char)(th.eth24l & 0x00FF)); + break; + + case Message::CMD_IR_SET_THRESHOLD: + if (msg->getArgNum() < 8) { + MSG("Too few arguments.\n"); + resMsg.setArgument(0, 0x01); // error + } else { + ak9750Threshold.eth13h = CONV16I(msg->getArgument(0), msg->getArgument(1)); + ak9750Threshold.eth13l = CONV16I(msg->getArgument(2), msg->getArgument(3)); + ak9750Threshold.eth24h = CONV16I(msg->getArgument(4), msg->getArgument(5)); + ak9750Threshold.eth24l = CONV16I(msg->getArgument(6), msg->getArgument(7)); + if (Hardware::getInstance()->ak9750.setThreshold(&ak9750Threshold) != AK9750::SUCCESS) { + resMsg.setArgument(0, 0x01); // error + } else { + resMsg.setArgument(0, 0x00); // success + isIrThresholdSet = true; + } + } + break; + + case Message::CMD_IR_GET_HYSTERESIS: + AK9750::Hysteresis hys; + if (Hardware::getInstance()->ak9750.getHysteresis(&hys) != AK9750::SUCCESS) { + MSG("Failed to get hysteresis setting from AK9750.\n"); + return; + } + resMsg.setArgument(0, hys.ehys13); + resMsg.setArgument(1, hys.ehys24); + break; + + case Message::CMD_IR_SET_HYSTERESIS: + if (msg->getArgNum() < 2) { // sanity check + MSG("Too few argument.\n"); + resMsg.setArgument(0, 0x01); // error + } else { + AK9750::Hysteresis hys; + hys.ehys13 = msg->getArgument(0); + hys.ehys24 = msg->getArgument(1); + if (Hardware::getInstance()->ak9750.setHysteresis(&hys) != AK9750::SUCCESS) { + MSG("Failed to set hysteresis to AK9750.\n"); + resMsg.setArgument(0, 0x01); // error + } else { + resMsg.setArgument(0, 0x00); // success + } + } + break; +*/ + default: + MSG("#Can't find command:%s\n", (char)cmd); + break; + } +} + +AkmSensorManager::Status AkmSensorManager::processEvent() +{ + AkmSensorManager::Status status = AkmSensorManager::SUCCESS; + + // command received from the host + if(eventCommandReceived) + { + processCommand(); + eventCommandReceived = false; + } + if(sensor->isEvent()) // sensor read data event + { + Message msg; + if( sensor->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + throwMessage(&msg); + } + + if(eventConnected) // BLE connected. Start sensor. + { +// if( sensor->startSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + eventConnected = false; + } + if(eventDisconnected) // BLE dis-connected. Stop sensor. + { + if( sensor->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + eventDisconnected = false; + } + return status; +} + +AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) { + int len = Message::getMaxMessageLength(); + char buf[len]; + + buf[0] = '$'; + + // Processes command + char cmd = (char)msg->getCommand(); + Message::charToAscii(&buf[1], &cmd); + + // Processes arguments + for (int i=0; i < msg->getArgNum(); i++) { + char arg = msg->getArgument(i); + Message::charToAscii(&buf[3+2*i], &arg); + } + + // Add termination characters, 0x0D(\r), \n and \0, to the end of string + int tIdx = 3 + 2 * (msg->getArgNum()); + int bufSize = sizeof(buf)/sizeof(buf[0]); + if ((tIdx + 3) > (bufSize - 1)) { + MSG("#Error: Message data exceeds the buffer.\n"); + return ERROR; + } + buf[tIdx++] = 0x0D; // '\r' + buf[tIdx++] = '\n'; + buf[tIdx] = '\0'; + + // Send to the BLE buffer +// Hardware *hw = Hardware::getInstance(); +// hw->uartService->writeString(buf); + if(isEnabledBle) uartService->writeString(buf); + if(isEnabledUsb) serial->printf(buf); + +// MSG("#A message thrown: \"%s\"\n", buf); + + return SUCCESS; +} + +/* +void AkmSensorManager::dataOut(char* str){ + if(isBleMode) uartService->writeString(str); + if(isSerialMode) serial->printf(str); +} +*/ \ No newline at end of file