Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp
- Committer:
- masahikofukasawa
- Date:
- 2016-06-16
- Revision:
- 9:6fa3e7b17c27
- Parent:
- 8:8fa4b81db50b
- Child:
- 10:5c69b067d88a
File content as of revision 9:6fa3e7b17c27:
#include "ble/services/UARTService.h" #include "akmsensormanager.h" #include "akmanglesensor.h" #include "akmhallswitch.h" #include "akmlinearsensor.h" #include "akmcurrentsensor.h" #include "akmirsensor.h" #include "akmakd.h" #include "debug.h" #include "Message.h" #ifdef REV_D #include "mcp342x.h" #include "I2CNano.h" #include "tca9554a.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)", "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)", "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_SPI: 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; #ifdef REV_D { const int TIME_FOR_OE_MS = 100; const TCA9554A::Port PORT_OE_LVS2 = TCA9554A::PORT_6; I2C i2c(I2C_SDA, I2C_SCL); TCA9554A tca9554a(&i2c, TCA9554A::SLAVE_ADDRESS_38H); // Makes sure that the OE is low first. tca9554a.setPortLevel(PORT_OE_LVS2, TCA9554A::LOW); wait_ms(TIME_FOR_OE_MS); } #endif break; 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_CURRENT_SENSOR: subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id AkmCurrentSensor* currentsensor; currentsensor = new AkmCurrentSensor(); sensor = currentsensor; 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 } #ifdef REV_D // I2C turn off releaseTWI(); #endif 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_SDA, I2C_SCL); // 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) + 0.5); 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 target is set.\n"); break; case Message::CMD_GET_ID: // return Primary ID and Sub ID 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_PROGSW_GET_THRESHOLD: case Message::CMD_PROGSW_SET_THRESHOLD: case Message::CMD_PROGSW_GET_READ_COFIGURATION: case Message::CMD_PROGSW_SET_READ_COFIGURATION: case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION: case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION: case Message::CMD_PROGSW_GET_OPERATION_MODE: case Message::CMD_PROGSW_SET_OPERATION_MODE: 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: case Message::CMD_ANGLE_ZERO_RESET: case Message::CMD_REG_WRITE: case Message::CMD_REG_WRITEN: case Message::CMD_REG_READ: case Message::CMD_REG_READN: case Message::CMD_COMPASS_GET_OPERATION_MODE: case Message::CMD_COMPASS_SET_OPERATION_MODE: 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::releaseTWI(){ NRF_TWI0->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos; NRF_TWI0->POWER = 0; NRF_TWI1->ENABLE = TWI_ENABLE_ENABLE_Disabled << TWI_ENABLE_ENABLE_Pos; NRF_TWI1->POWER = 0; } /* void AkmSensorManager::dataOut(char* str){ if(isBleMode) uartService->writeString(str); if(isSerialMode) serial->printf(str); } */