Taylor Street
Modified for compatibility with Rev.E. hardware
Fork of
AkmSensor
by 
AKM Development Platform
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;
