Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp
- Committer:
- masahikofukasawa
- Date:
- 2016-05-09
- Revision:
- 4:af13b985c689
- Parent:
- 2:11fe67783c4c
- Child:
- 6:c4401549d68f
File content as of revision 4:af13b985c689:
#include "ble/services/UARTService.h" #include "akmsensormanager.h" #include "akmanglesensor.h" #include "akmhallswitch.h" #include "akmlinearsensor.h" #include "akmirsensor.h" #include "akmakd.h" #include "debug.h" #include "Message.h" #ifdef REV_D #include "mcp342x.h" #endif #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", "IR Sensor", "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); 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: case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR: subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id AkmLinearSensor* linearsensor; linearsensor = new AkmLinearSensor(); sensor = linearsensor; break; case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR: subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id AkmIrSensor* irsensor; irsensor = new AkmIrSensor(); sensor = irsensor; 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("#sensor->init failed. 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; } #ifdef REV_D int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) { const int WAIT_ADC_MS = 1; // Configure channel and trigger. mcp3428->setChannel(ch); mcp3428->setSampleSetting(s); mcp3428->trigger(); // polling data (!blocking) MCP342X::Data data; do { wait_ms(WAIT_ADC_MS); mcp3428->getData(&data); } while(data.st == MCP342X::DATA_NOT_UPDATED); return data.value; } #endif uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits) { #ifndef REV_D /* Rev.C */ AnalogIn id(pin); MSG("#Voltage=%5.2f[V]\n",id*3.0); double s = id + 1.0/(double)(pow(2.0,bits+1)); uint8_t value = (uint8_t)(s*pow(2.0,bits)); #else /* Rev.D */ MSG("#GetID\n"); I2C i2c(I2C_SDA0, I2C_SCL0); // ADC MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH); mcp342x.setConversionMode(MCP342X::ONE_SHOT); MCP342X::AdcChannel ch; if (pin == ANALOG_SENSOR_ID) { ch = MCP342X::ADC_CH1; } else { // pin == ANALOG_SENSOR_ID_SUB ch = MCP342X::ADC_CH2; } int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT); MSG("#12bit ADC Val = %d.\n", val); const int16_t VAL_MAX = 3000-2048; // Corresponds to 3V const int16_t VAL_MIN = -2048; // Corresponds to 0V uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits)); MSG("#ID = %d.\n", value); #endif 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: case Message::CMD_IR_GET_THRESHOLD: case Message::CMD_IR_SET_THRESHOLD: case Message::CMD_IR_GET_HYSTERESIS: case Message::CMD_IR_SET_HYSTERESIS: case Message::CMD_IR_GET_INTERRUPT: case Message::CMD_IR_SET_INTERRUPT: case Message::CMD_IR_GET_OPERATION_MODE: case Message::CMD_IR_SET_OPERATION_MODE: case Message::CMD_IR_GET_THRESHOLD_EEPROM: case Message::CMD_IR_SET_THRESHOLD_EEPROM: case Message::CMD_IR_GET_HYSTERESIS_EEPROM: case Message::CMD_IR_SET_HYSTERESIS_EEPROM: case Message::CMD_IR_GET_INTERRUPT_EEPROM: case Message::CMD_IR_SET_INTERRUPT_EEPROM: case Message::CMD_IR_GET_OPERATION_MODE_EEPROM: case Message::CMD_IR_SET_OPERATION_MODE_EEPROM: sensor->requestCommand(&msg,&resMsg); throwMessage(&resMsg); 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); } */