Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: akmsensormanager.cpp
- Revision:
- 29:b488d2c89fba
- Parent:
- 28:dc4eb14e4d7e
- Child:
- 30:5a241d9b3262
--- a/akmsensormanager.cpp Sat Mar 11 02:08:32 2017 +0000 +++ b/akmsensormanager.cpp Fri Mar 17 23:29:20 2017 +0000 @@ -45,12 +45,18 @@ 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; } @@ -73,43 +79,55 @@ } bool AkmSensorManager::checkAkmSensor() -{ - currentSensorNumber = 0; - int i=0; - for(i=0; i<MAX_SENSOR_NUM; i++){ - sensor[i] = NULL; - } +{ + 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){ - AkmAkd* akd = new AkmAkd(); - sensor[0] = akd; + 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{ - Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); - sensor[0] = ak09970; + 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; - break; + sensorNum = 1; } else if(subId == Ak7401Ctrl::SUB_ID_AK7401){ Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl(); sensor[0] = ak7401ctrl; - break; + sensorNum = 1; } break; - } + } case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR: case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR: @@ -119,26 +137,30 @@ { AkmHallSwitch* hallswitch = new AkmHallSwitch(); sensor[0] = hallswitch; + sensorNum = 1; break; } case AkmSensor::AKM_PRIMARY_ID_DEMO: { - if(subId == 8){ - Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl(); - sensor[0] = ak9752ctrl; - if(sensor[currentSensorNumber]->init(AkmSensor::AKM_PRIMARY_ID_IR_SENSOR, Ak9752Ctrl::SUB_ID_AK9752) != AkmSensor::SUCCESS){ - MSG("#sensor[0]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); + 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 - } - - Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); - sensor[1] = ak09970; - if(sensor[1]->init(AkmSensor::AKM_PRIMARY_ID_AKD_I2C, Ak09970Ctrl::SUB_ID_AK09970) != AkmSensor::SUCCESS){ - MSG("#sensor[1]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); + } + + 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; + break; } case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR: case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR: @@ -146,20 +168,26 @@ { 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 } @@ -172,18 +200,27 @@ return false; // couldn't find } } - - if(primaryId != AkmSensor::AKM_PRIMARY_ID_DEMO){ - if(sensor[currentSensorNumber]->init(primaryId, subId) != AkmSensor::SUCCESS){ - MSG("#sensor[currentSensorNumber]->init failed. 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){ @@ -248,13 +285,20 @@ bool AkmSensorManager::isEvent() { - return (sensor[currentSensorNumber]->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 @@ -293,79 +337,86 @@ resMsg.setArgument(0, primaryId); resMsg.setArgument(1, subId); throwMessage(&resMsg); - MSG("#Mag ID is reported.\r\n"); + MSG("#ID is reported.\r\n"); break; } - case Message::CMD_GET_SENSOR_NUM: + case Message::CMD_GET_SENSOR_INDEX: { - resMsg.setArgument(0, currentSensorNumber+1); + resMsg.setArgument(0, sensorIndex); throwMessage(&resMsg); - MSG("#Current Sensor Number reported.\r\n"); + MSG("#Get Sensor Index=%d.\r\n", sensorIndex); break; } - case Message::CMD_SET_SENSOR_NUM: + case Message::CMD_SET_SENSOR_INDEX: { - uint8_t i=0; - while(sensor[i] != NULL){ - i++; - } - - uint8_t arg = (uint8_t)msg.getArgument(0); - - if( arg != 0 && msg.getArgNum() == 1 && arg < i+1 ){ - currentSensorNumber = (uint8_t)msg.getArgument(0)-1; - MSG("#Current Sensor Number set to %d.\r\n", currentSensorNumber+1); + 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); - } - else{ - currentSensorNumber = 0; - resMsg.setArgument(0, 1); - MSG("#Error: Current Sensor Number set to %d.\r\n", currentSensorNumber+1); - } + sensorIndex = index; + MSG("#Set Sensor Index=%d.\r\n", sensorIndex); + } throwMessage(&resMsg); break; } - case Message::CMD_GET_SENSOR_COUNT: + case Message::CMD_GET_TOTAL_SENSOR_NUM: { - int i=0; - while(sensor[i] != NULL){ - i++; - } - resMsg.setArgument(0, i); + resMsg.setArgument(0, sensorNum); throwMessage(&resMsg); - MSG("#Sensor Total Count reported.\r\n"); + MSG("#Get Sensor Total Num=%d.\r\n", sensorNum); break; } case Message::CMD_STOP_MEASUREMENT: { - if( sensor[currentSensorNumber]->stopSensor() != AkmSensor::SUCCESS){ + if( sensor[sensorIndex]->stopSensor() != AkmSensor::SUCCESS){ resMsg.setArgument(0, 1); }else{ resMsg.setArgument(0, 0); } throwMessage(&resMsg); - MSG("#Stop measurement.\r\n"); + 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[currentSensorNumber]->startSensor(); + error_code = sensor[sensorIndex]->startSensor(); if( error_code != AkmSensor::SUCCESS ){ MSG("#StartSensor Error. Code=%d\r\n",error_code); } else{ - MSG("#Start measurement.\r\n"); + 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[currentSensorNumber]->startSensor(interval); + error_code = sensor[sensorIndex]->startSensor(interval); if( error_code != AkmSensor::SUCCESS ){ MSG("#StartSensor Error. Code=%d\r\n",error_code); } else{ - MSG("#Start measurement.\r\n"); + if(interrupt) delete interrupt; } }else{ MSG("#StartSensor Error. Wrong Argument num.\r\n"); @@ -378,7 +429,7 @@ primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT || primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){ Message temp; - sensor[currentSensorNumber]->readSensorData(&temp); + sensor[sensorIndex]->readSensorData(&temp); throwMessage(&temp); } } @@ -416,7 +467,7 @@ case Message::CMD_COMPASS_GET_OPERATION_MODE: case Message::CMD_COMPASS_SET_OPERATION_MODE: { - AkmSensor::Status st = sensor[currentSensorNumber]->requestCommand(&msg,&resMsg); + AkmSensor::Status st = sensor[sensorIndex]->requestCommand(&msg,&resMsg); if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) ) { MSG("#Command failed.\r\n"); @@ -444,13 +495,16 @@ processCommand(); eventCommandReceived = false; } - if(sensor[currentSensorNumber]->isEvent()) // sensor read data event - { -// MSG("#Sensor event received.\r\n"); - Message msg; - if( sensor[currentSensorNumber]->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; - throwMessage(&msg); + + // 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; @@ -459,7 +513,9 @@ if(eventDisconnected) // BLE dis-connected. Stop sensor. { MSG("#BLE dis-connected.\r\n"); - if( sensor[currentSensorNumber]->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + for(int i=0; i<sensorNum; i++){ + if( sensor[i]->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; + } eventDisconnected = false; } return status; @@ -497,6 +553,8 @@ return SUCCESS; } + + char* AkmSensorManager::my_strcat(char* str1, char* str2) { int num1; @@ -509,14 +567,10 @@ } char* AkmSensorManager::getSensorName(){ - char* name=""; - int i=0; - while(sensor[i] != NULL){ - name = my_strcat(name,sensor[i]->getSensorName()); - if(sensor[i+1] != NULL){ - name = my_strcat(name,"_"); - } - i++; + 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;