Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
Diff: akmsensormanager.cpp
- Revision:
- 11:cef8dc1cf010
- Parent:
- 10:5c69b067d88a
- Child:
- 12:c06cd8b76358
diff -r 5c69b067d88a -r cef8dc1cf010 akmsensormanager.cpp --- a/akmsensormanager.cpp Fri Jul 08 22:26:26 2016 +0000 +++ b/akmsensormanager.cpp Fri Jul 22 22:54:11 2016 +0000 @@ -1,12 +1,11 @@ #include "ble/services/UARTService.h" #include "akmsensormanager.h" -#include "akmanglesensor.h" #include "akmhallswitch.h" -#include "akmlinearsensor.h" -#include "akmcurrentsensor.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" @@ -17,6 +16,8 @@ #include "tca9554a.h" #endif +#define CR '\r' +#define LF '\n' #define FIRMWARE_VERSION 0x02 #define MAGNETOMETER_ID 0x0A @@ -76,33 +77,10 @@ { AkmSensor* sensor = NULL; - // primary id check -// id = AkmSensorManager::getId(ANALOG_SENSOR_ID,4); -// subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,4); // 4bit sub id - switch(primaryId){ case AkmSensor::AKM_PRIMARY_ID_AKD_SPI: case AkmSensor::AKM_PRIMARY_ID_AKD_I2C: { -/* -#ifdef REV_D - { - // Power on reset - // RSV low to high for resetting(RSTN) AKD daughter card - const int TIME_FOR_OE_MS = 100; - const TCA9554A::Port PORT_RSV_RSTN = TCA9554A::PORT_0; - I2C i2c(I2C_SDA, I2C_SCL); - TCA9554A tca9554a(&i2c, TCA9554A::SLAVE_ADDRESS_38H); - tca9554a.configurePort(PORT_RSV_RSTN, TCA9554A::DIR_OUTPUT); - tca9554a.setPortLevel(PORT_RSV_RSTN, TCA9554A::LOW); - wait_ms(TIME_FOR_OE_MS); - tca9554a.setPortLevel(PORT_RSV_RSTN, TCA9554A::HIGH); - } -#endif -*/ - -// subId = AkmSensorManager::getId(ANALOG_SENSOR_ID_SUB,5); // Update subId for 5bit ID - if(subId != Ak09970Ctrl::SUB_ID_AK09970){ AkmAkd* akd = new AkmAkd(); sensor = akd; @@ -111,27 +89,13 @@ Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); sensor = ak09970; } -/* -#ifdef REV_D - { - // Disable the level shifter of ADC - 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: { - if(subId == AkmAngleSensor::SUB_ID_AK7451){ - AkmAngleSensor* angleSensor = new AkmAngleSensor(); - sensor = angleSensor; + if(subId == Ak7451Ctrl::SUB_ID_AK7451){ + Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl(); + sensor = ak7451ctrl; break; } else if(subId == Ak7401Ctrl::SUB_ID_AK7401){ @@ -153,21 +117,11 @@ } case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR_LEGACY: case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR: - { - AkmLinearSensor* linearsensor = new AkmLinearSensor(); - sensor = linearsensor; - break; - } case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR: - { - AkmCurrentSensor* currentsensor = new AkmCurrentSensor(); - sensor = currentsensor; - break; - } case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG: { - AkmLinearSensor* linearsensor = new AkmLinearSensor(); - sensor = linearsensor; + AkmAnalogSensor* analogsensor = new AkmAnalogSensor(); + sensor = analogsensor; break; } case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR: @@ -190,12 +144,7 @@ return NULL; // couldn't find } } -/* -#ifdef REV_D - // I2C turn off - releaseTWI(); -#endif -*/ + if(sensor->init(primaryId, subId) != AkmSensor::SUCCESS){ MSG("#sensor->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); return NULL; // couldn't find @@ -210,7 +159,7 @@ Status status = SUCCESS; if ((Message::parse(&msg, buf)) != Message::SUCCESS) { - MSG("#Failed to parse message. %s\n", buf); + MSG("#Failed to parse message. %s\r\n", buf); status = ERROR; eventCommandReceived = false; } @@ -243,12 +192,12 @@ #ifndef REV_D /* Rev.C */ AnalogIn id(pin); - MSG("#Voltage=%5.2f[V]\n",id*3.0); + MSG("#Voltage=%5.2f[V]\r\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"); + MSG("#GetID\r\n"); I2C i2c(I2C_SDA, I2C_SCL); // ADC @@ -261,13 +210,13 @@ ch = MCP342X::ADC_CH2; } int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT); - MSG("#12bit ADC Val = %d.\n", val); + 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.\n", value); + MSG("#ID = %d.\r\n", value); #endif return value; @@ -298,55 +247,78 @@ switch(cmd) { case Message::CMD_GET_FW_VERSION: + { resMsg.setArgument(0, FIRMWARE_VERSION); throwMessage(&resMsg); - MSG("#FW version is reported.\n"); + 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.\n"); + 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; - 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, primaryId); resMsg.setArgument(1, subId); throwMessage(&resMsg); - wait(0.4); // wait for App initialization - MSG("#Mag ID is reported.\n"); + MSG("#Mag ID is reported.\r\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"); + MSG("#Stop measurement.\r\n"); break; - + } case Message::CMD_START_MEASUREMENT: - MSG("#Start measurement.\n"); - sensor->startSensor(); - // 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->readSensorData(&temp); - throwMessage(&temp); + { + int error_code = AkmSensor::SUCCESS; + if(msg.getArgNum() == 0){ + error_code = sensor->startSensor(); + if( error_code != AkmSensor::SUCCESS ){ + MSG("#StartSensor Error. Code=%d\r\n",error_code); + } + else{ + MSG("#Start measurement.\r\n"); + } + }else if(msg.getArgNum() == 1){ + float interval = (float)(1.0 / (float)msg.getArgument(0)); + error_code = sensor->startSensor(interval); + if( error_code != AkmSensor::SUCCESS ){ + MSG("#StartSensor Error. Code=%d\r\n",error_code); + } + else{ + MSG("#Start measurement.\r\n"); + } + }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->readSensorData(&temp); + throwMessage(&temp); + } } break; - + } case Message::CMD_PROGSW_GET_THRESHOLD: case Message::CMD_PROGSW_SET_THRESHOLD: case Message::CMD_PROGSW_GET_READ_COFIGURATION: @@ -378,13 +350,16 @@ 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); + { + MSG("#Can't find command:%s\r\n", (char)cmd); break; + } } } @@ -437,24 +412,14 @@ 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"); + MSG("#Error: Message data exceeds the buffer.\r\n"); return ERROR; } - buf[tIdx++] = 0x0D; // '\r' - buf[tIdx++] = '\n'; - buf[tIdx] = '\0'; - - // Send to the BLE buffer + buf[tIdx++] = CR; // '\r' + buf[tIdx++] = LF; // '\n' + buf[tIdx] = '\0'; if(isEnabledBle) uartService->writeString(buf); if(isEnabledUsb) serial->printf(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; -} -*/ \ No newline at end of file +} \ No newline at end of file