Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp
- Committer:
- masahikofukasawa
- Date:
- 2017-03-17
- Revision:
- 29:b488d2c89fba
- Parent:
- 28:dc4eb14e4d7e
- Child:
- 30:5a241d9b3262
File content as of revision 29:b488d2c89fba:
#include "ble/services/UARTService.h" #include "akdphwinfo.h" #include "akmsensormanager.h" #include "akmhallswitch.h" #include "akmanalogsensor.h" #include "ak09970ctrl.h" #include "ak9750ctrl.h" #include "ak9752ctrl.h" #include "ak7451ctrl.h" #include "ak7401ctrl.h" #include "akmakd.h" #include "debug.h" #include "Message.h" #include "mcp342x.h" #include "I2CNano.h" #define MAGNETOMETER_ID 0x0A #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) const char* AKM_PRIMARY_ID_STR[] = { "AKD Daughter Cards(SPI)", "Switch, Unipolar", "Switch, Onmipolar", "Latch, Bipolar", "Switch, Dual Output", "Onechip Encoder", "TBD1", "TBD2", "DEMO", "Current Sensor", "MISC(Analog)", "Linear Sensor", "Motor Drivers", "IR Sensor", "Angle Sensor(SPI)", "AKD Daughter Cards(I2C)" }; AkmSensorManager::AkmSensorManager(SerialNano* com) { serial = com; isEnabledUsb = true; eventCommandReceived = false; eventConnected = false; eventDisconnected = false; interrupt = NULL; sensorIndex = 0; sensorNum = 0; drdyType = AkmAkd::INTERRUPT_DISABLED; } AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid) { primaryId = id; subId = subid; sensorIndex = 0; sensorNum = 0; if(AkmSensorManager::checkAkmSensor()!= true) return AkmSensorManager::ERROR; return AkmSensorManager::SUCCESS; } void AkmSensorManager::setBleUartService(UARTService* service) { uartService = service; isEnabledBle = true; } void AkmSensorManager::setEventConnected() { eventConnected = true; } void AkmSensorManager::setEventDisconnected() { eventDisconnected = true; } bool AkmSensorManager::checkAkmSensor() { sensorNum = 0; // int i=0; // while(sensor[i] != NULL){ // delete(sensor[i]); // i++; // } if(interrupt) delete interrupt; drdyType = AkmAkd::INTERRUPT_DISABLED; switch(primaryId){ case AkmSensor::AKM_PRIMARY_ID_AKD_SPI: { MSG("#SPI Interface.\r\n"); } case AkmSensor::AKM_PRIMARY_ID_AKD_I2C: { if(subId == Ak09970Ctrl::SUB_ID_AK09970){ drdyType = AkmAkd::INTERRUPT_ENABLED_PP; // Only support push-pull Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); sensor[0] = ak09970; sensorNum = 1; MSG("#AK09970.\r\n"); } else{ AkmAkd* akd = new AkmAkd(); drdyType = akd->getInterrupt(primaryId, subId); sensor[0] = akd; sensorNum = 1; MSG("#e-Compass.\r\n"); } break; } case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR: { if(subId == Ak7451Ctrl::SUB_ID_AK7451){ Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl(); sensor[0] = ak7451ctrl; sensorNum = 1; } else if(subId == Ak7401Ctrl::SUB_ID_AK7401){ Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl(); sensor[0] = ak7401ctrl; sensorNum = 1; } 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: { AkmHallSwitch* hallswitch = new AkmHallSwitch(); sensor[0] = hallswitch; sensorNum = 1; break; } case AkmSensor::AKM_PRIMARY_ID_DEMO: { if(subId == 0x08){ drdyType = AkmAkd::INTERRUPT_ENABLED_OD; // Only support open drain type DRDY Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); sensor[0] = ak09970; if(sensor[0]->init(AkmSensor::AKM_PRIMARY_ID_AKD_I2C, Ak09970Ctrl::SUB_ID_AK09970) != AkmSensor::SUCCESS){ MSG("#sensor[0]->init failed.\r\n"); return false; // couldn't find } Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl(); sensor[1] = ak9752ctrl; if(sensor[1]->init(AkmSensor::AKM_PRIMARY_ID_IR_SENSOR, Ak9752Ctrl::SUB_ID_AK9752) != AkmSensor::SUCCESS){ MSG("#sensor[1]->init failed.\r\n"); return false; // couldn't find } sensorNum = 2; } break; } case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR: case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR: case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG: { AkmAnalogSensor* analogsensor = new AkmAnalogSensor(); sensor[0] = analogsensor; sensorNum = 1; break; } case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR: { drdyType = AkmAkd::INTERRUPT_ENABLED_OD; if(subId == Ak9750Ctrl::SUB_ID_AK9750){ Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl(); sensor[0] = ak9750ctrl; sensorNum = 1; }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){ Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl(); sensor[0] = ak9753ctrl; sensorNum = 1; }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){ Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl(); sensor[0] = ak9752ctrl; sensorNum = 1; }else{ return false; // couldn't find } break; } default: { MSG("#Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); return false; // couldn't find } } if(primaryId != AkmSensor::AKM_PRIMARY_ID_DEMO){ for(int i=0; i<sensorNum; i++){ if(sensor[i]->init(primaryId, subId) != AkmSensor::SUCCESS){ MSG("#sensor[i]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); return false; // couldn't find } } MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); } return true; } void AkmSensorManager::detectDRDY(){ // MSG("#detect DRDY.\r\n"); for(int i=0; i<sensorNum; i++){ sensor[i]->setEvent(); } } void AkmSensorManager::dummyCallbackForCommandReceived(){} AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){ // Construct message Status status = SUCCESS; if ((Message::parse(&msg, buf)) != Message::SUCCESS) { MSG("#Failed to parse message. %s\r\n", buf); status = ERROR; eventCommandReceived = false; }else{ eventCommandReceived = true; // MSG("#Parsed message. %s\r\n", buf); } t.attach(callback(this, &AkmSensorManager::dummyCallbackForCommandReceived),0); // wake-up from ble.waitForEvent return status; } 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; } uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits) { MSG("#GetID\r\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.\r\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.\r\n", value); return value; } bool AkmSensorManager::isEvent() { // check sensor related event for(int i=0; i<sensorNum; i++){ if(sensor[i]->isEvent()){ return true; } } // other events return ( eventCommandReceived || eventConnected || eventDisconnected); } void AkmSensorManager::processCommand() { // 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.\r\n"); break; } case Message::CMD_GET_MAG_PART: { resMsg.setArgument(0, MAGNETOMETER_ID); throwMessage(&resMsg); MSG("#Mag ID is reported.\r\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; break; } case Message::CMD_GET_ID: // return Primary ID and Sub ID { resMsg.setArgument(0, primaryId); resMsg.setArgument(1, subId); throwMessage(&resMsg); MSG("#ID is reported.\r\n"); break; } case Message::CMD_GET_SENSOR_INDEX: { resMsg.setArgument(0, sensorIndex); throwMessage(&resMsg); MSG("#Get Sensor Index=%d.\r\n", sensorIndex); break; } case Message::CMD_SET_SENSOR_INDEX: { uint8_t index = (uint8_t)msg.getArgument(0); if( index >= sensorNum ){ resMsg.setArgument(0, 1); MSG("#Error: Set Sensor Index=%d.\r\n", sensorIndex); }else{ resMsg.setArgument(0, 0); sensorIndex = index; MSG("#Set Sensor Index=%d.\r\n", sensorIndex); } throwMessage(&resMsg); break; } case Message::CMD_GET_TOTAL_SENSOR_NUM: { resMsg.setArgument(0, sensorNum); throwMessage(&resMsg); MSG("#Get Sensor Total Num=%d.\r\n", sensorNum); break; } case Message::CMD_STOP_MEASUREMENT: { if( sensor[sensorIndex]->stopSensor() != AkmSensor::SUCCESS){ resMsg.setArgument(0, 1); }else{ resMsg.setArgument(0, 0); } throwMessage(&resMsg); MSG("#Stop measurement:%s.\r\n",sensor[sensorIndex]->getSensorName()); break; } case Message::CMD_START_MEASUREMENT: { int error_code = AkmSensor::SUCCESS; if(msg.getArgNum() == 0){ error_code = sensor[sensorIndex]->startSensor(); if( error_code != AkmSensor::SUCCESS ){ MSG("#StartSensor Error. Code=%d\r\n",error_code); } else{ switch(drdyType){ case AkmAkd::INTERRUPT_ENABLED_PP: { if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY); else interrupt = new InterruptIn(I2C_DRDY); interrupt->rise(callback(this, &AkmSensorManager::detectDRDY)); break; } case AkmAkd::INTERRUPT_ENABLED_OD: { if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY); else interrupt = new InterruptIn(I2C_DRDY); interrupt->fall(callback(this, &AkmSensorManager::detectDRDY)); break; } default: { // nothing. } } } }else if(msg.getArgNum() == 1){ float interval = (float)(1.0 / (float)msg.getArgument(0)); error_code = sensor[sensorIndex]->startSensor(interval); if( error_code != AkmSensor::SUCCESS ){ MSG("#StartSensor Error. Code=%d\r\n",error_code); } else{ if(interrupt) delete interrupt; } }else{ MSG("#StartSensor Error. Wrong Argument num.\r\n"); } if(error_code == AkmSensor::SUCCESS){ // get initial sensor state for switch type sensors if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR || primaryId == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR || primaryId == AkmSensor::AKM_PRIMARY_ID_LATCH || primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT || primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){ Message temp; sensor[sensorIndex]->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: { AkmSensor::Status st = sensor[sensorIndex]->requestCommand(&msg,&resMsg); if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) ) { MSG("#Command failed.\r\n"); }else{ throwMessage(&resMsg); } break; } default: { MSG("#Can't find command.\r\n"); break; } } } AkmSensorManager::Status AkmSensorManager::processEvent() { AkmSensorManager::Status status = AkmSensorManager::SUCCESS; // command received from the host if(eventCommandReceived) { // MSG("#Command received.\r\n"); processCommand(); eventCommandReceived = false; } // check sensor event for(int i=0; i<sensorNum; i++){ if( sensor[i]->isEvent() ){ Message msg; if( sensor[i]->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; throwMessage(&msg); } } if(eventConnected) // BLE connected. Start sensor. { eventConnected = false; MSG("#BLE connected.\r\n"); } if(eventDisconnected) // BLE dis-connected. Stop sensor. { MSG("#BLE dis-connected.\r\n"); for(int i=0; i<sensorNum; i++){ if( sensor[i]->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.\r\n"); return ERROR; } buf[tIdx++] = CR; // '\r' buf[tIdx++] = LF; // '\n' buf[tIdx] = '\0'; if(isEnabledBle) uartService->writeString(buf); if(isEnabledUsb) serial->printf(buf); return SUCCESS; } char* AkmSensorManager::my_strcat(char* str1, char* str2) { int num1; char* str; num1=strlen(str1) + strlen(str2); str = (char *)malloc(num1 + 1); sprintf(str,"%s%s",str1,str2); return str; } char* AkmSensorManager::getSensorName(){ char* name = ""; for(int i=0; i<sensorNum; i++){ name = my_strcat(name, sensor[i]->getSensorName()); if( sensorNum > (i+1) )name = my_strcat(name, "+"); } MSG("#Sensor Name='%s'.\r\n",name); return name; }