Taylor Street
Modified for compatibility with Rev.E. hardware
Fork of
AkmSensor
by 
AKM Development Platform
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
