Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp
- Committer:
- masahikofukasawa
- Date:
- 2016-04-28
- Revision:
- 0:7a00359e701e
- Child:
- 1:b46b8653331f
File content as of revision 0:7a00359e701e:
#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); } */