Modified for compatibility with Rev.E. hardware
Fork of AkmSensor by
akmsensormanager.cpp@39:3821886c902e, 2017-07-18 (annotated)
- Committer:
- masahikofukasawa
- Date:
- Tue Jul 18 23:20:26 2017 +0000
- Revision:
- 39:3821886c902e
- Parent:
- 37:c76d2edf3426
- Child:
- 41:a3ea80c594ec
For AK09970 Small demo board. Takezawa-san's request.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
masahikofukasawa | 0:7a00359e701e | 1 | #include "ble/services/UARTService.h" |
masahikofukasawa | 15:1238993fd75f | 2 | #include "akdphwinfo.h" |
masahikofukasawa | 0:7a00359e701e | 3 | #include "akmsensormanager.h" |
masahikofukasawa | 0:7a00359e701e | 4 | #include "akmhallswitch.h" |
masahikofukasawa | 11:cef8dc1cf010 | 5 | #include "akmanalogsensor.h" |
masahikofukasawa | 10:5c69b067d88a | 6 | #include "ak09970ctrl.h" |
masahikofukasawa | 10:5c69b067d88a | 7 | #include "ak9750ctrl.h" |
masahikofukasawa | 10:5c69b067d88a | 8 | #include "ak9752ctrl.h" |
masahikofukasawa | 11:cef8dc1cf010 | 9 | #include "ak7451ctrl.h" |
masahikofukasawa | 10:5c69b067d88a | 10 | #include "ak7401ctrl.h" |
tkstreet | 33:d3e1e9eb2ef9 | 11 | #include "ap1017ctrl.h" |
masahikofukasawa | 0:7a00359e701e | 12 | #include "akmakd.h" |
masahikofukasawa | 0:7a00359e701e | 13 | #include "debug.h" |
masahikofukasawa | 0:7a00359e701e | 14 | #include "Message.h" |
coisme | 2:11fe67783c4c | 15 | #include "mcp342x.h" |
masahikofukasawa | 6:c4401549d68f | 16 | #include "I2CNano.h" |
masahikofukasawa | 0:7a00359e701e | 17 | |
masahikofukasawa | 0:7a00359e701e | 18 | #define MAGNETOMETER_ID 0x0A |
masahikofukasawa | 0:7a00359e701e | 19 | |
masahikofukasawa | 0:7a00359e701e | 20 | #define CONV16I(high,low) ((int16_t)(((high) << 8) | (low))) |
masahikofukasawa | 0:7a00359e701e | 21 | |
masahikofukasawa | 0:7a00359e701e | 22 | |
masahikofukasawa | 10:5c69b067d88a | 23 | const char* AKM_PRIMARY_ID_STR[] = { |
masahikofukasawa | 7:e269411568c9 | 24 | "AKD Daughter Cards(SPI)", |
masahikofukasawa | 0:7a00359e701e | 25 | "Switch, Unipolar", |
masahikofukasawa | 0:7a00359e701e | 26 | "Switch, Onmipolar", |
masahikofukasawa | 0:7a00359e701e | 27 | "Latch, Bipolar", |
masahikofukasawa | 0:7a00359e701e | 28 | "Switch, Dual Output", |
masahikofukasawa | 0:7a00359e701e | 29 | "Onechip Encoder", |
masahikofukasawa | 0:7a00359e701e | 30 | "TBD1", |
masahikofukasawa | 37:c76d2edf3426 | 31 | "Current Sensor 3V", |
masahikofukasawa | 37:c76d2edf3426 | 32 | "Test", |
masahikofukasawa | 37:c76d2edf3426 | 33 | "Current Sensor 5V", |
masahikofukasawa | 0:7a00359e701e | 34 | "MISC(Analog)", |
masahikofukasawa | 0:7a00359e701e | 35 | "Linear Sensor", |
masahikofukasawa | 18:b7182d5ad8d5 | 36 | "Motor Drivers", |
masahikofukasawa | 1:b46b8653331f | 37 | "IR Sensor", |
masahikofukasawa | 7:e269411568c9 | 38 | "Angle Sensor(SPI)", |
masahikofukasawa | 10:5c69b067d88a | 39 | "AKD Daughter Cards(I2C)" |
masahikofukasawa | 0:7a00359e701e | 40 | }; |
masahikofukasawa | 0:7a00359e701e | 41 | |
masahikofukasawa | 13:d008249f0359 | 42 | AkmSensorManager::AkmSensorManager(SerialNano* com) |
masahikofukasawa | 0:7a00359e701e | 43 | { |
masahikofukasawa | 0:7a00359e701e | 44 | serial = com; |
masahikofukasawa | 0:7a00359e701e | 45 | isEnabledUsb = true; |
masahikofukasawa | 0:7a00359e701e | 46 | eventCommandReceived = false; |
masahikofukasawa | 0:7a00359e701e | 47 | eventConnected = false; |
masahikofukasawa | 0:7a00359e701e | 48 | eventDisconnected = false; |
masahikofukasawa | 29:b488d2c89fba | 49 | interrupt = NULL; |
masahikofukasawa | 29:b488d2c89fba | 50 | sensorIndex = 0; |
masahikofukasawa | 29:b488d2c89fba | 51 | sensorNum = 0; |
masahikofukasawa | 29:b488d2c89fba | 52 | drdyType = AkmAkd::INTERRUPT_DISABLED; |
masahikofukasawa | 0:7a00359e701e | 53 | } |
masahikofukasawa | 0:7a00359e701e | 54 | |
masahikofukasawa | 10:5c69b067d88a | 55 | AkmSensorManager::Status AkmSensorManager::init(uint8_t id, uint8_t subid) |
masahikofukasawa | 0:7a00359e701e | 56 | { |
masahikofukasawa | 10:5c69b067d88a | 57 | primaryId = id; |
masahikofukasawa | 10:5c69b067d88a | 58 | subId = subid; |
masahikofukasawa | 29:b488d2c89fba | 59 | sensorIndex = 0; |
masahikofukasawa | 29:b488d2c89fba | 60 | sensorNum = 0; |
masahikofukasawa | 27:41aa9fb23a2f | 61 | if(AkmSensorManager::checkAkmSensor()!= true) return AkmSensorManager::ERROR; |
masahikofukasawa | 0:7a00359e701e | 62 | return AkmSensorManager::SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 63 | } |
masahikofukasawa | 0:7a00359e701e | 64 | |
masahikofukasawa | 13:d008249f0359 | 65 | |
masahikofukasawa | 13:d008249f0359 | 66 | void AkmSensorManager::setBleUartService(UARTService* service) |
masahikofukasawa | 13:d008249f0359 | 67 | { |
masahikofukasawa | 13:d008249f0359 | 68 | uartService = service; |
masahikofukasawa | 13:d008249f0359 | 69 | isEnabledBle = true; |
masahikofukasawa | 13:d008249f0359 | 70 | } |
masahikofukasawa | 13:d008249f0359 | 71 | |
masahikofukasawa | 0:7a00359e701e | 72 | void AkmSensorManager::setEventConnected() |
masahikofukasawa | 0:7a00359e701e | 73 | { |
masahikofukasawa | 0:7a00359e701e | 74 | eventConnected = true; |
masahikofukasawa | 0:7a00359e701e | 75 | } |
masahikofukasawa | 0:7a00359e701e | 76 | |
masahikofukasawa | 0:7a00359e701e | 77 | void AkmSensorManager::setEventDisconnected() |
masahikofukasawa | 0:7a00359e701e | 78 | { |
masahikofukasawa | 0:7a00359e701e | 79 | eventDisconnected = true; |
masahikofukasawa | 0:7a00359e701e | 80 | } |
masahikofukasawa | 0:7a00359e701e | 81 | |
masahikofukasawa | 27:41aa9fb23a2f | 82 | bool AkmSensorManager::checkAkmSensor() |
masahikofukasawa | 29:b488d2c89fba | 83 | { |
masahikofukasawa | 29:b488d2c89fba | 84 | sensorNum = 0; |
masahikofukasawa | 29:b488d2c89fba | 85 | |
masahikofukasawa | 29:b488d2c89fba | 86 | // int i=0; |
masahikofukasawa | 29:b488d2c89fba | 87 | // while(sensor[i] != NULL){ |
masahikofukasawa | 29:b488d2c89fba | 88 | // delete(sensor[i]); |
masahikofukasawa | 29:b488d2c89fba | 89 | // i++; |
masahikofukasawa | 29:b488d2c89fba | 90 | // } |
masahikofukasawa | 39:3821886c902e | 91 | detachInterrupt(); |
masahikofukasawa | 29:b488d2c89fba | 92 | drdyType = AkmAkd::INTERRUPT_DISABLED; |
masahikofukasawa | 0:7a00359e701e | 93 | |
masahikofukasawa | 10:5c69b067d88a | 94 | switch(primaryId){ |
masahikofukasawa | 17:9abb7c28709c | 95 | |
masahikofukasawa | 7:e269411568c9 | 96 | case AkmSensor::AKM_PRIMARY_ID_AKD_SPI: |
masahikofukasawa | 29:b488d2c89fba | 97 | { |
masahikofukasawa | 29:b488d2c89fba | 98 | MSG("#SPI Interface.\r\n"); |
masahikofukasawa | 29:b488d2c89fba | 99 | } |
masahikofukasawa | 0:7a00359e701e | 100 | case AkmSensor::AKM_PRIMARY_ID_AKD_I2C: |
masahikofukasawa | 10:5c69b067d88a | 101 | { |
masahikofukasawa | 29:b488d2c89fba | 102 | if(subId == Ak09970Ctrl::SUB_ID_AK09970){ |
masahikofukasawa | 29:b488d2c89fba | 103 | drdyType = AkmAkd::INTERRUPT_ENABLED_PP; // Only support push-pull |
masahikofukasawa | 29:b488d2c89fba | 104 | Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); |
masahikofukasawa | 29:b488d2c89fba | 105 | sensor[0] = ak09970; |
masahikofukasawa | 29:b488d2c89fba | 106 | sensorNum = 1; |
masahikofukasawa | 29:b488d2c89fba | 107 | MSG("#AK09970.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 108 | } |
masahikofukasawa | 10:5c69b067d88a | 109 | else{ |
masahikofukasawa | 29:b488d2c89fba | 110 | AkmAkd* akd = new AkmAkd(); |
masahikofukasawa | 29:b488d2c89fba | 111 | drdyType = akd->getInterrupt(primaryId, subId); |
masahikofukasawa | 29:b488d2c89fba | 112 | sensor[0] = akd; |
masahikofukasawa | 29:b488d2c89fba | 113 | sensorNum = 1; |
masahikofukasawa | 29:b488d2c89fba | 114 | MSG("#e-Compass.\r\n"); |
masahikofukasawa | 10:5c69b067d88a | 115 | } |
masahikofukasawa | 0:7a00359e701e | 116 | break; |
masahikofukasawa | 29:b488d2c89fba | 117 | } |
masahikofukasawa | 0:7a00359e701e | 118 | case AkmSensor::AKM_PRIMARY_ID_ANGLE_SENSOR: |
masahikofukasawa | 10:5c69b067d88a | 119 | { |
masahikofukasawa | 11:cef8dc1cf010 | 120 | if(subId == Ak7451Ctrl::SUB_ID_AK7451){ |
masahikofukasawa | 11:cef8dc1cf010 | 121 | Ak7451Ctrl* ak7451ctrl = new Ak7451Ctrl(); |
masahikofukasawa | 27:41aa9fb23a2f | 122 | sensor[0] = ak7451ctrl; |
masahikofukasawa | 29:b488d2c89fba | 123 | sensorNum = 1; |
masahikofukasawa | 10:5c69b067d88a | 124 | } |
masahikofukasawa | 10:5c69b067d88a | 125 | else if(subId == Ak7401Ctrl::SUB_ID_AK7401){ |
masahikofukasawa | 10:5c69b067d88a | 126 | Ak7401Ctrl* ak7401ctrl = new Ak7401Ctrl(); |
masahikofukasawa | 27:41aa9fb23a2f | 127 | sensor[0] = ak7401ctrl; |
masahikofukasawa | 29:b488d2c89fba | 128 | sensorNum = 1; |
masahikofukasawa | 10:5c69b067d88a | 129 | } |
masahikofukasawa | 0:7a00359e701e | 130 | break; |
masahikofukasawa | 29:b488d2c89fba | 131 | } |
masahikofukasawa | 17:9abb7c28709c | 132 | |
masahikofukasawa | 0:7a00359e701e | 133 | case AkmSensor::AKM_PRIMARY_ID_UNIPOLAR: |
masahikofukasawa | 0:7a00359e701e | 134 | case AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR: |
masahikofukasawa | 0:7a00359e701e | 135 | case AkmSensor::AKM_PRIMARY_ID_LATCH: |
masahikofukasawa | 0:7a00359e701e | 136 | case AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT: |
masahikofukasawa | 0:7a00359e701e | 137 | case AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER: |
masahikofukasawa | 10:5c69b067d88a | 138 | { |
masahikofukasawa | 10:5c69b067d88a | 139 | AkmHallSwitch* hallswitch = new AkmHallSwitch(); |
masahikofukasawa | 27:41aa9fb23a2f | 140 | sensor[0] = hallswitch; |
masahikofukasawa | 29:b488d2c89fba | 141 | sensorNum = 1; |
masahikofukasawa | 0:7a00359e701e | 142 | break; |
masahikofukasawa | 10:5c69b067d88a | 143 | } |
masahikofukasawa | 27:41aa9fb23a2f | 144 | case AkmSensor::AKM_PRIMARY_ID_DEMO: |
masahikofukasawa | 27:41aa9fb23a2f | 145 | { |
masahikofukasawa | 29:b488d2c89fba | 146 | if(subId == 0x08){ |
masahikofukasawa | 29:b488d2c89fba | 147 | drdyType = AkmAkd::INTERRUPT_ENABLED_OD; // Only support open drain type DRDY |
masahikofukasawa | 29:b488d2c89fba | 148 | |
masahikofukasawa | 29:b488d2c89fba | 149 | Ak09970Ctrl* ak09970 = new Ak09970Ctrl(); |
masahikofukasawa | 29:b488d2c89fba | 150 | sensor[0] = ak09970; |
masahikofukasawa | 29:b488d2c89fba | 151 | if(sensor[0]->init(AkmSensor::AKM_PRIMARY_ID_AKD_I2C, Ak09970Ctrl::SUB_ID_AK09970) != AkmSensor::SUCCESS){ |
masahikofukasawa | 39:3821886c902e | 152 | MSG("#Error: sensor[0]->init failed.\r\n"); |
masahikofukasawa | 27:41aa9fb23a2f | 153 | return false; // couldn't find |
masahikofukasawa | 29:b488d2c89fba | 154 | } |
masahikofukasawa | 29:b488d2c89fba | 155 | |
masahikofukasawa | 29:b488d2c89fba | 156 | Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl(); |
masahikofukasawa | 29:b488d2c89fba | 157 | sensor[1] = ak9752ctrl; |
masahikofukasawa | 29:b488d2c89fba | 158 | if(sensor[1]->init(AkmSensor::AKM_PRIMARY_ID_IR_SENSOR, Ak9752Ctrl::SUB_ID_AK9752) != AkmSensor::SUCCESS){ |
masahikofukasawa | 39:3821886c902e | 159 | MSG("#Error: sensor[1]->init failed.\r\n"); |
masahikofukasawa | 27:41aa9fb23a2f | 160 | return false; // couldn't find |
masahikofukasawa | 29:b488d2c89fba | 161 | } |
masahikofukasawa | 29:b488d2c89fba | 162 | sensorNum = 2; |
masahikofukasawa | 27:41aa9fb23a2f | 163 | } |
masahikofukasawa | 29:b488d2c89fba | 164 | break; |
masahikofukasawa | 27:41aa9fb23a2f | 165 | } |
masahikofukasawa | 4:af13b985c689 | 166 | case AkmSensor::AKM_PRIMARY_ID_LINEAR_SENSOR: |
masahikofukasawa | 37:c76d2edf3426 | 167 | case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR_3V: |
masahikofukasawa | 37:c76d2edf3426 | 168 | case AkmSensor::AKM_PRIMARY_ID_CURRENT_SENSOR_5V: |
masahikofukasawa | 10:5c69b067d88a | 169 | case AkmSensor::AKM_PRIMARY_ID_MISC_ANALOG: |
masahikofukasawa | 10:5c69b067d88a | 170 | { |
masahikofukasawa | 11:cef8dc1cf010 | 171 | AkmAnalogSensor* analogsensor = new AkmAnalogSensor(); |
masahikofukasawa | 27:41aa9fb23a2f | 172 | sensor[0] = analogsensor; |
masahikofukasawa | 29:b488d2c89fba | 173 | sensorNum = 1; |
masahikofukasawa | 0:7a00359e701e | 174 | break; |
masahikofukasawa | 10:5c69b067d88a | 175 | } |
masahikofukasawa | 17:9abb7c28709c | 176 | |
masahikofukasawa | 1:b46b8653331f | 177 | case AkmSensor::AKM_PRIMARY_ID_IR_SENSOR: |
masahikofukasawa | 10:5c69b067d88a | 178 | { |
masahikofukasawa | 29:b488d2c89fba | 179 | drdyType = AkmAkd::INTERRUPT_ENABLED_OD; |
masahikofukasawa | 29:b488d2c89fba | 180 | |
masahikofukasawa | 10:5c69b067d88a | 181 | if(subId == Ak9750Ctrl::SUB_ID_AK9750){ |
masahikofukasawa | 10:5c69b067d88a | 182 | Ak9750Ctrl* ak9750ctrl = new Ak9750Ctrl(); |
masahikofukasawa | 27:41aa9fb23a2f | 183 | sensor[0] = ak9750ctrl; |
masahikofukasawa | 29:b488d2c89fba | 184 | sensorNum = 1; |
masahikofukasawa | 15:1238993fd75f | 185 | }else if(subId == Ak9750Ctrl::SUB_ID_AK9753){ |
masahikofukasawa | 15:1238993fd75f | 186 | Ak9750Ctrl* ak9753ctrl = new Ak9750Ctrl(); |
masahikofukasawa | 27:41aa9fb23a2f | 187 | sensor[0] = ak9753ctrl; |
masahikofukasawa | 29:b488d2c89fba | 188 | sensorNum = 1; |
masahikofukasawa | 10:5c69b067d88a | 189 | }else if(subId == Ak9752Ctrl::SUB_ID_AK9752){ |
masahikofukasawa | 10:5c69b067d88a | 190 | Ak9752Ctrl* ak9752ctrl = new Ak9752Ctrl(); |
masahikofukasawa | 27:41aa9fb23a2f | 191 | sensor[0] = ak9752ctrl; |
masahikofukasawa | 29:b488d2c89fba | 192 | sensorNum = 1; |
masahikofukasawa | 10:5c69b067d88a | 193 | }else{ |
masahikofukasawa | 27:41aa9fb23a2f | 194 | return false; // couldn't find |
masahikofukasawa | 10:5c69b067d88a | 195 | } |
masahikofukasawa | 1:b46b8653331f | 196 | break; |
masahikofukasawa | 10:5c69b067d88a | 197 | } |
tkstreet | 33:d3e1e9eb2ef9 | 198 | case AkmSensor::AKM_PRIMARY_ID_MOTOR_DRIVER: |
tkstreet | 33:d3e1e9eb2ef9 | 199 | { // TODO: Other motor driver cases |
tkstreet | 33:d3e1e9eb2ef9 | 200 | if(subId == Ap1017Ctrl::SUB_ID_AP1017){ |
tkstreet | 33:d3e1e9eb2ef9 | 201 | Ap1017Ctrl* ap1017ctrl = new Ap1017Ctrl(); |
tkstreet | 34:1ea3357c8d9a | 202 | sensor[0] = ap1017ctrl; |
tkstreet | 34:1ea3357c8d9a | 203 | sensorNum = 1; |
tkstreet | 33:d3e1e9eb2ef9 | 204 | }else{ |
tkstreet | 33:d3e1e9eb2ef9 | 205 | return NULL; |
tkstreet | 33:d3e1e9eb2ef9 | 206 | } |
tkstreet | 33:d3e1e9eb2ef9 | 207 | break; |
tkstreet | 33:d3e1e9eb2ef9 | 208 | } |
masahikofukasawa | 0:7a00359e701e | 209 | default: |
masahikofukasawa | 10:5c69b067d88a | 210 | { |
masahikofukasawa | 39:3821886c902e | 211 | MSG("#Error: Can't find ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); |
masahikofukasawa | 27:41aa9fb23a2f | 212 | return false; // couldn't find |
masahikofukasawa | 10:5c69b067d88a | 213 | } |
masahikofukasawa | 0:7a00359e701e | 214 | } |
masahikofukasawa | 29:b488d2c89fba | 215 | |
masahikofukasawa | 29:b488d2c89fba | 216 | if(primaryId != AkmSensor::AKM_PRIMARY_ID_DEMO){ |
masahikofukasawa | 29:b488d2c89fba | 217 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 218 | if(sensor[i]->init(primaryId, subId) != AkmSensor::SUCCESS){ |
masahikofukasawa | 39:3821886c902e | 219 | MSG("#Error: sensor[i]->init failed. ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); |
masahikofukasawa | 29:b488d2c89fba | 220 | return false; // couldn't find |
masahikofukasawa | 29:b488d2c89fba | 221 | } |
masahikofukasawa | 29:b488d2c89fba | 222 | } |
masahikofukasawa | 27:41aa9fb23a2f | 223 | MSG("#ID=%d SubID=%d %s\r\n", primaryId, subId, AKM_PRIMARY_ID_STR[primaryId]); |
masahikofukasawa | 0:7a00359e701e | 224 | } |
masahikofukasawa | 29:b488d2c89fba | 225 | |
masahikofukasawa | 39:3821886c902e | 226 | attachInterrupt(); |
masahikofukasawa | 39:3821886c902e | 227 | |
masahikofukasawa | 27:41aa9fb23a2f | 228 | return true; |
masahikofukasawa | 0:7a00359e701e | 229 | } |
masahikofukasawa | 0:7a00359e701e | 230 | |
masahikofukasawa | 29:b488d2c89fba | 231 | void AkmSensorManager::detectDRDY(){ |
masahikofukasawa | 29:b488d2c89fba | 232 | // MSG("#detect DRDY.\r\n"); |
masahikofukasawa | 29:b488d2c89fba | 233 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 234 | sensor[i]->setEvent(); |
masahikofukasawa | 29:b488d2c89fba | 235 | } |
masahikofukasawa | 29:b488d2c89fba | 236 | } |
masahikofukasawa | 29:b488d2c89fba | 237 | |
masahikofukasawa | 16:d85be9bafb80 | 238 | void AkmSensorManager::dummyCallbackForCommandReceived(){} |
masahikofukasawa | 16:d85be9bafb80 | 239 | |
tkstreet | 33:d3e1e9eb2ef9 | 240 | |
tkstreet | 33:d3e1e9eb2ef9 | 241 | // For commands received via BLE |
masahikofukasawa | 0:7a00359e701e | 242 | AkmSensorManager::Status AkmSensorManager::commandReceived(char* buf){ |
masahikofukasawa | 0:7a00359e701e | 243 | // Construct message |
masahikofukasawa | 0:7a00359e701e | 244 | Status status = SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 245 | |
masahikofukasawa | 10:5c69b067d88a | 246 | if ((Message::parse(&msg, buf)) != Message::SUCCESS) { |
masahikofukasawa | 39:3821886c902e | 247 | MSG("#Error: Failed to parse message. %s\r\n", buf); |
masahikofukasawa | 0:7a00359e701e | 248 | status = ERROR; |
tkstreet | 33:d3e1e9eb2ef9 | 249 | eventCommandReceived = false; // Reset flag |
masahikofukasawa | 15:1238993fd75f | 250 | }else{ |
tkstreet | 33:d3e1e9eb2ef9 | 251 | eventCommandReceived = true; // Set flag |
masahikofukasawa | 16:d85be9bafb80 | 252 | // MSG("#Parsed message. %s\r\n", buf); |
masahikofukasawa | 0:7a00359e701e | 253 | } |
masahikofukasawa | 16:d85be9bafb80 | 254 | t.attach(callback(this, &AkmSensorManager::dummyCallbackForCommandReceived),0); // wake-up from ble.waitForEvent |
masahikofukasawa | 0:7a00359e701e | 255 | return status; |
masahikofukasawa | 0:7a00359e701e | 256 | } |
masahikofukasawa | 0:7a00359e701e | 257 | |
tkstreet | 33:d3e1e9eb2ef9 | 258 | |
coisme | 2:11fe67783c4c | 259 | int16_t AkmSensorManager::getAdcData(MCP342X *mcp3428, MCP342X::AdcChannel ch, MCP342X::SampleSetting s) { |
coisme | 2:11fe67783c4c | 260 | const int WAIT_ADC_MS = 1; |
coisme | 2:11fe67783c4c | 261 | |
coisme | 2:11fe67783c4c | 262 | // Configure channel and trigger. |
coisme | 2:11fe67783c4c | 263 | mcp3428->setChannel(ch); |
coisme | 2:11fe67783c4c | 264 | mcp3428->setSampleSetting(s); |
coisme | 2:11fe67783c4c | 265 | mcp3428->trigger(); |
coisme | 2:11fe67783c4c | 266 | |
coisme | 2:11fe67783c4c | 267 | // polling data (!blocking) |
coisme | 2:11fe67783c4c | 268 | MCP342X::Data data; |
coisme | 2:11fe67783c4c | 269 | do { |
coisme | 2:11fe67783c4c | 270 | wait_ms(WAIT_ADC_MS); |
coisme | 2:11fe67783c4c | 271 | mcp3428->getData(&data); |
coisme | 2:11fe67783c4c | 272 | } while(data.st == MCP342X::DATA_NOT_UPDATED); |
coisme | 2:11fe67783c4c | 273 | |
coisme | 2:11fe67783c4c | 274 | return data.value; |
coisme | 2:11fe67783c4c | 275 | } |
masahikofukasawa | 0:7a00359e701e | 276 | |
tkstreet | 33:d3e1e9eb2ef9 | 277 | |
masahikofukasawa | 0:7a00359e701e | 278 | uint8_t AkmSensorManager::getId(PinName pin, uint8_t bits) |
masahikofukasawa | 0:7a00359e701e | 279 | { |
masahikofukasawa | 11:cef8dc1cf010 | 280 | MSG("#GetID\r\n"); |
masahikofukasawa | 6:c4401549d68f | 281 | |
tkstreet | 21:966724730ce6 | 282 | I2C i2c(I2C_SDA, I2C_SCL); // establish I2C to read ID |
tkstreet | 21:966724730ce6 | 283 | |
coisme | 2:11fe67783c4c | 284 | // ADC |
tkstreet | 21:966724730ce6 | 285 | MCP342X mcp342x(&i2c, MCP342X::SLAVE_ADDRESS_6EH); // ADC to convert voltage |
tkstreet | 21:966724730ce6 | 286 | mcp342x.setConversionMode(MCP342X::ONE_SHOT); // Set to single sample |
coisme | 2:11fe67783c4c | 287 | MCP342X::AdcChannel ch; |
tkstreet | 21:966724730ce6 | 288 | |
tkstreet | 21:966724730ce6 | 289 | if (pin == ANALOG_SENSOR_ID) { // Primary ID |
coisme | 2:11fe67783c4c | 290 | ch = MCP342X::ADC_CH1; |
tkstreet | 21:966724730ce6 | 291 | } else { // pin == ANALOG_SENSOR_ID_SUB |
tkstreet | 21:966724730ce6 | 292 | ch = MCP342X::ADC_CH2; // Secondary ID |
coisme | 2:11fe67783c4c | 293 | } |
tkstreet | 21:966724730ce6 | 294 | |
coisme | 2:11fe67783c4c | 295 | int16_t val = getAdcData(&mcp342x, ch, MCP342X::SAMPLE_240HZ_12BIT); |
masahikofukasawa | 11:cef8dc1cf010 | 296 | MSG("#12bit ADC Val = %d.\r\n", val); |
coisme | 2:11fe67783c4c | 297 | |
tkstreet | 21:966724730ce6 | 298 | // Voltage boundaries for ID voltage divider system |
coisme | 2:11fe67783c4c | 299 | const int16_t VAL_MAX = 3000-2048; // Corresponds to 3V |
coisme | 2:11fe67783c4c | 300 | const int16_t VAL_MIN = -2048; // Corresponds to 0V |
coisme | 2:11fe67783c4c | 301 | |
tkstreet | 21:966724730ce6 | 302 | // Convert voltage to ID value |
masahikofukasawa | 6:c4401549d68f | 303 | uint8_t value = (uint8_t)((val - VAL_MIN)/(float)(VAL_MAX - VAL_MIN) * (1 << bits) + 0.5); |
masahikofukasawa | 11:cef8dc1cf010 | 304 | MSG("#ID = %d.\r\n", value); |
masahikofukasawa | 6:c4401549d68f | 305 | |
masahikofukasawa | 0:7a00359e701e | 306 | return value; |
masahikofukasawa | 0:7a00359e701e | 307 | } |
masahikofukasawa | 0:7a00359e701e | 308 | |
tkstreet | 33:d3e1e9eb2ef9 | 309 | |
masahikofukasawa | 0:7a00359e701e | 310 | bool AkmSensorManager::isEvent() |
masahikofukasawa | 0:7a00359e701e | 311 | { |
masahikofukasawa | 29:b488d2c89fba | 312 | // check sensor related event |
masahikofukasawa | 29:b488d2c89fba | 313 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 314 | if(sensor[i]->isEvent()){ |
masahikofukasawa | 29:b488d2c89fba | 315 | return true; |
masahikofukasawa | 29:b488d2c89fba | 316 | } |
masahikofukasawa | 29:b488d2c89fba | 317 | } |
masahikofukasawa | 29:b488d2c89fba | 318 | |
masahikofukasawa | 29:b488d2c89fba | 319 | // other events |
masahikofukasawa | 29:b488d2c89fba | 320 | return ( |
masahikofukasawa | 0:7a00359e701e | 321 | eventCommandReceived || |
masahikofukasawa | 0:7a00359e701e | 322 | eventConnected || |
masahikofukasawa | 0:7a00359e701e | 323 | eventDisconnected); |
masahikofukasawa | 0:7a00359e701e | 324 | } |
masahikofukasawa | 0:7a00359e701e | 325 | |
masahikofukasawa | 0:7a00359e701e | 326 | void AkmSensorManager::processCommand() |
masahikofukasawa | 0:7a00359e701e | 327 | { |
tkstreet | 23:50c98b286e41 | 328 | // Extracts command contained in the message |
masahikofukasawa | 0:7a00359e701e | 329 | Message::Command cmd = msg.getCommand(); |
masahikofukasawa | 0:7a00359e701e | 330 | |
tkstreet | 21:966724730ce6 | 331 | // Creates a message object to return |
masahikofukasawa | 0:7a00359e701e | 332 | Message resMsg; |
masahikofukasawa | 0:7a00359e701e | 333 | |
tkstreet | 23:50c98b286e41 | 334 | // Return message contains the extracted command |
masahikofukasawa | 0:7a00359e701e | 335 | resMsg.setCommand(cmd); |
masahikofukasawa | 0:7a00359e701e | 336 | |
masahikofukasawa | 0:7a00359e701e | 337 | switch(cmd) |
masahikofukasawa | 0:7a00359e701e | 338 | { |
masahikofukasawa | 0:7a00359e701e | 339 | case Message::CMD_GET_FW_VERSION: |
masahikofukasawa | 11:cef8dc1cf010 | 340 | { |
masahikofukasawa | 0:7a00359e701e | 341 | resMsg.setArgument(0, FIRMWARE_VERSION); |
masahikofukasawa | 0:7a00359e701e | 342 | throwMessage(&resMsg); |
masahikofukasawa | 11:cef8dc1cf010 | 343 | MSG("#FW version is reported.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 344 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 345 | } |
masahikofukasawa | 0:7a00359e701e | 346 | case Message::CMD_GET_MAG_PART: |
masahikofukasawa | 11:cef8dc1cf010 | 347 | { |
masahikofukasawa | 0:7a00359e701e | 348 | resMsg.setArgument(0, MAGNETOMETER_ID); |
masahikofukasawa | 0:7a00359e701e | 349 | throwMessage(&resMsg); |
masahikofukasawa | 11:cef8dc1cf010 | 350 | MSG("#Mag ID is reported.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 351 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 352 | } |
masahikofukasawa | 0:7a00359e701e | 353 | case Message::CMD_SET_SERIAL_TARGET: |
masahikofukasawa | 11:cef8dc1cf010 | 354 | { |
masahikofukasawa | 0:7a00359e701e | 355 | isEnabledBle = msg.getArgument(0)==Message::SW_ON ? true : false; |
masahikofukasawa | 0:7a00359e701e | 356 | isEnabledUsb = msg.getArgument(1)==Message::SW_ON ? true : false; |
masahikofukasawa | 0:7a00359e701e | 357 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 358 | } |
masahikofukasawa | 0:7a00359e701e | 359 | case Message::CMD_GET_ID: // return Primary ID and Sub ID |
masahikofukasawa | 11:cef8dc1cf010 | 360 | { |
masahikofukasawa | 10:5c69b067d88a | 361 | resMsg.setArgument(0, primaryId); |
masahikofukasawa | 0:7a00359e701e | 362 | resMsg.setArgument(1, subId); |
masahikofukasawa | 0:7a00359e701e | 363 | throwMessage(&resMsg); |
masahikofukasawa | 29:b488d2c89fba | 364 | MSG("#ID is reported.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 365 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 366 | } |
masahikofukasawa | 29:b488d2c89fba | 367 | case Message::CMD_GET_SENSOR_INDEX: |
masahikofukasawa | 27:41aa9fb23a2f | 368 | { |
masahikofukasawa | 29:b488d2c89fba | 369 | resMsg.setArgument(0, sensorIndex); |
masahikofukasawa | 27:41aa9fb23a2f | 370 | throwMessage(&resMsg); |
masahikofukasawa | 29:b488d2c89fba | 371 | MSG("#Get Sensor Index=%d.\r\n", sensorIndex); |
masahikofukasawa | 27:41aa9fb23a2f | 372 | break; |
masahikofukasawa | 27:41aa9fb23a2f | 373 | } |
masahikofukasawa | 29:b488d2c89fba | 374 | case Message::CMD_SET_SENSOR_INDEX: |
masahikofukasawa | 27:41aa9fb23a2f | 375 | { |
masahikofukasawa | 29:b488d2c89fba | 376 | uint8_t index = (uint8_t)msg.getArgument(0); |
masahikofukasawa | 30:5a241d9b3262 | 377 | if(msg.getArgNum() == 1 && index<sensorNum){ |
masahikofukasawa | 28:dc4eb14e4d7e | 378 | resMsg.setArgument(0, 0); |
masahikofukasawa | 29:b488d2c89fba | 379 | sensorIndex = index; |
masahikofukasawa | 29:b488d2c89fba | 380 | MSG("#Set Sensor Index=%d.\r\n", sensorIndex); |
masahikofukasawa | 30:5a241d9b3262 | 381 | }else{ |
masahikofukasawa | 30:5a241d9b3262 | 382 | resMsg.setArgument(0, 1); |
masahikofukasawa | 30:5a241d9b3262 | 383 | MSG("#Error: Set Sensor Index=%d.\r\n", sensorIndex); |
masahikofukasawa | 29:b488d2c89fba | 384 | } |
masahikofukasawa | 28:dc4eb14e4d7e | 385 | throwMessage(&resMsg); |
masahikofukasawa | 27:41aa9fb23a2f | 386 | break; |
masahikofukasawa | 27:41aa9fb23a2f | 387 | } |
masahikofukasawa | 29:b488d2c89fba | 388 | case Message::CMD_GET_TOTAL_SENSOR_NUM: |
masahikofukasawa | 27:41aa9fb23a2f | 389 | { |
masahikofukasawa | 29:b488d2c89fba | 390 | resMsg.setArgument(0, sensorNum); |
masahikofukasawa | 27:41aa9fb23a2f | 391 | throwMessage(&resMsg); |
masahikofukasawa | 29:b488d2c89fba | 392 | MSG("#Get Sensor Total Num=%d.\r\n", sensorNum); |
masahikofukasawa | 0:7a00359e701e | 393 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 394 | } |
masahikofukasawa | 0:7a00359e701e | 395 | case Message::CMD_STOP_MEASUREMENT: |
masahikofukasawa | 11:cef8dc1cf010 | 396 | { |
masahikofukasawa | 29:b488d2c89fba | 397 | if( sensor[sensorIndex]->stopSensor() != AkmSensor::SUCCESS){ |
masahikofukasawa | 15:1238993fd75f | 398 | resMsg.setArgument(0, 1); |
masahikofukasawa | 15:1238993fd75f | 399 | }else{ |
masahikofukasawa | 27:41aa9fb23a2f | 400 | resMsg.setArgument(0, 0); |
masahikofukasawa | 15:1238993fd75f | 401 | } |
masahikofukasawa | 0:7a00359e701e | 402 | throwMessage(&resMsg); |
masahikofukasawa | 39:3821886c902e | 403 | |
masahikofukasawa | 39:3821886c902e | 404 | detachInterrupt(); |
masahikofukasawa | 39:3821886c902e | 405 | |
masahikofukasawa | 29:b488d2c89fba | 406 | MSG("#Stop measurement:%s.\r\n",sensor[sensorIndex]->getSensorName()); |
masahikofukasawa | 0:7a00359e701e | 407 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 408 | } |
masahikofukasawa | 0:7a00359e701e | 409 | case Message::CMD_START_MEASUREMENT: |
masahikofukasawa | 11:cef8dc1cf010 | 410 | { |
masahikofukasawa | 11:cef8dc1cf010 | 411 | int error_code = AkmSensor::SUCCESS; |
masahikofukasawa | 11:cef8dc1cf010 | 412 | if(msg.getArgNum() == 0){ |
masahikofukasawa | 29:b488d2c89fba | 413 | error_code = sensor[sensorIndex]->startSensor(); |
masahikofukasawa | 11:cef8dc1cf010 | 414 | if( error_code != AkmSensor::SUCCESS ){ |
masahikofukasawa | 39:3821886c902e | 415 | MSG("#Error: StartSensor Error. Code=%d\r\n",error_code); |
masahikofukasawa | 39:3821886c902e | 416 | }else{ |
masahikofukasawa | 29:b488d2c89fba | 417 | switch(drdyType){ |
masahikofukasawa | 29:b488d2c89fba | 418 | case AkmAkd::INTERRUPT_ENABLED_PP: |
masahikofukasawa | 29:b488d2c89fba | 419 | { |
masahikofukasawa | 29:b488d2c89fba | 420 | interrupt->rise(callback(this, &AkmSensorManager::detectDRDY)); |
masahikofukasawa | 29:b488d2c89fba | 421 | break; |
masahikofukasawa | 29:b488d2c89fba | 422 | } |
masahikofukasawa | 29:b488d2c89fba | 423 | case AkmAkd::INTERRUPT_ENABLED_OD: |
masahikofukasawa | 29:b488d2c89fba | 424 | { |
masahikofukasawa | 29:b488d2c89fba | 425 | interrupt->fall(callback(this, &AkmSensorManager::detectDRDY)); |
masahikofukasawa | 29:b488d2c89fba | 426 | break; |
masahikofukasawa | 29:b488d2c89fba | 427 | } |
masahikofukasawa | 29:b488d2c89fba | 428 | default: |
masahikofukasawa | 29:b488d2c89fba | 429 | { |
masahikofukasawa | 29:b488d2c89fba | 430 | // nothing. |
masahikofukasawa | 29:b488d2c89fba | 431 | } |
masahikofukasawa | 29:b488d2c89fba | 432 | } |
masahikofukasawa | 11:cef8dc1cf010 | 433 | } |
masahikofukasawa | 11:cef8dc1cf010 | 434 | }else if(msg.getArgNum() == 1){ |
masahikofukasawa | 11:cef8dc1cf010 | 435 | float interval = (float)(1.0 / (float)msg.getArgument(0)); |
masahikofukasawa | 29:b488d2c89fba | 436 | error_code = sensor[sensorIndex]->startSensor(interval); |
masahikofukasawa | 11:cef8dc1cf010 | 437 | if( error_code != AkmSensor::SUCCESS ){ |
masahikofukasawa | 39:3821886c902e | 438 | MSG("#Error: StartSensor Error. Code=%d\r\n",error_code); |
masahikofukasawa | 11:cef8dc1cf010 | 439 | } |
masahikofukasawa | 11:cef8dc1cf010 | 440 | else{ |
masahikofukasawa | 39:3821886c902e | 441 | detachInterrupt(); |
masahikofukasawa | 11:cef8dc1cf010 | 442 | } |
masahikofukasawa | 11:cef8dc1cf010 | 443 | }else{ |
masahikofukasawa | 39:3821886c902e | 444 | MSG("#Error: StartSensor Error. Wrong Argument num.\r\n"); |
masahikofukasawa | 11:cef8dc1cf010 | 445 | } |
masahikofukasawa | 39:3821886c902e | 446 | |
masahikofukasawa | 11:cef8dc1cf010 | 447 | if(error_code == AkmSensor::SUCCESS){ |
masahikofukasawa | 11:cef8dc1cf010 | 448 | // get initial sensor state for switch type sensors |
masahikofukasawa | 11:cef8dc1cf010 | 449 | if( primaryId == AkmSensor::AKM_PRIMARY_ID_UNIPOLAR || |
masahikofukasawa | 11:cef8dc1cf010 | 450 | primaryId == AkmSensor::AKM_PRIMARY_ID_OMNIPOLAR || |
masahikofukasawa | 11:cef8dc1cf010 | 451 | primaryId == AkmSensor::AKM_PRIMARY_ID_LATCH || |
masahikofukasawa | 11:cef8dc1cf010 | 452 | primaryId == AkmSensor::AKM_PRIMARY_ID_DUAL_OUTPUT || |
masahikofukasawa | 11:cef8dc1cf010 | 453 | primaryId == AkmSensor::AKM_PRIMARY_ID_ONECHIP_ENCODER ){ |
masahikofukasawa | 11:cef8dc1cf010 | 454 | Message temp; |
masahikofukasawa | 29:b488d2c89fba | 455 | sensor[sensorIndex]->readSensorData(&temp); |
masahikofukasawa | 11:cef8dc1cf010 | 456 | throwMessage(&temp); |
masahikofukasawa | 11:cef8dc1cf010 | 457 | } |
masahikofukasawa | 0:7a00359e701e | 458 | } |
masahikofukasawa | 39:3821886c902e | 459 | |
masahikofukasawa | 39:3821886c902e | 460 | MSG("#Start measurement:%s.\r\n",sensor[sensorIndex]->getSensorName()); |
masahikofukasawa | 0:7a00359e701e | 461 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 462 | } |
tkstreet | 33:d3e1e9eb2ef9 | 463 | case Message::CMD_MOTOR_START_MOTOR: |
tkstreet | 33:d3e1e9eb2ef9 | 464 | case Message::CMD_MOTOR_STOP_MOTOR: |
tkstreet | 33:d3e1e9eb2ef9 | 465 | case Message::CMD_MOTOR_SET_DIRECTION: |
tkstreet | 33:d3e1e9eb2ef9 | 466 | case Message::CMD_MOTOR_SET_DUTY_CYCLE: |
masahikofukasawa | 9:6fa3e7b17c27 | 467 | case Message::CMD_PROGSW_GET_THRESHOLD: |
masahikofukasawa | 9:6fa3e7b17c27 | 468 | case Message::CMD_PROGSW_SET_THRESHOLD: |
masahikofukasawa | 9:6fa3e7b17c27 | 469 | case Message::CMD_PROGSW_GET_READ_COFIGURATION: |
masahikofukasawa | 9:6fa3e7b17c27 | 470 | case Message::CMD_PROGSW_SET_READ_COFIGURATION: |
masahikofukasawa | 9:6fa3e7b17c27 | 471 | case Message::CMD_PROGSW_GET_SWITCH_COFIGURATION: |
masahikofukasawa | 9:6fa3e7b17c27 | 472 | case Message::CMD_PROGSW_SET_SWITCH_COFIGURATION: |
masahikofukasawa | 9:6fa3e7b17c27 | 473 | case Message::CMD_PROGSW_GET_OPERATION_MODE: |
masahikofukasawa | 9:6fa3e7b17c27 | 474 | case Message::CMD_PROGSW_SET_OPERATION_MODE: |
masahikofukasawa | 1:b46b8653331f | 475 | case Message::CMD_IR_GET_THRESHOLD: |
masahikofukasawa | 1:b46b8653331f | 476 | case Message::CMD_IR_SET_THRESHOLD: |
masahikofukasawa | 1:b46b8653331f | 477 | case Message::CMD_IR_GET_HYSTERESIS: |
masahikofukasawa | 1:b46b8653331f | 478 | case Message::CMD_IR_SET_HYSTERESIS: |
masahikofukasawa | 1:b46b8653331f | 479 | case Message::CMD_IR_GET_INTERRUPT: |
masahikofukasawa | 1:b46b8653331f | 480 | case Message::CMD_IR_SET_INTERRUPT: |
masahikofukasawa | 1:b46b8653331f | 481 | case Message::CMD_IR_GET_OPERATION_MODE: |
masahikofukasawa | 1:b46b8653331f | 482 | case Message::CMD_IR_SET_OPERATION_MODE: |
masahikofukasawa | 1:b46b8653331f | 483 | case Message::CMD_IR_GET_THRESHOLD_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 484 | case Message::CMD_IR_SET_THRESHOLD_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 485 | case Message::CMD_IR_GET_HYSTERESIS_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 486 | case Message::CMD_IR_SET_HYSTERESIS_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 487 | case Message::CMD_IR_GET_INTERRUPT_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 488 | case Message::CMD_IR_SET_INTERRUPT_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 489 | case Message::CMD_IR_GET_OPERATION_MODE_EEPROM: |
masahikofukasawa | 1:b46b8653331f | 490 | case Message::CMD_IR_SET_OPERATION_MODE_EEPROM: |
masahikofukasawa | 9:6fa3e7b17c27 | 491 | case Message::CMD_ANGLE_ZERO_RESET: |
masahikofukasawa | 30:5a241d9b3262 | 492 | case Message::CMD_ANGLE_READ: |
masahikofukasawa | 9:6fa3e7b17c27 | 493 | case Message::CMD_REG_WRITE: |
masahikofukasawa | 9:6fa3e7b17c27 | 494 | case Message::CMD_REG_WRITEN: |
masahikofukasawa | 9:6fa3e7b17c27 | 495 | case Message::CMD_REG_READ: |
masahikofukasawa | 9:6fa3e7b17c27 | 496 | case Message::CMD_REG_READN: |
masahikofukasawa | 9:6fa3e7b17c27 | 497 | case Message::CMD_COMPASS_GET_OPERATION_MODE: |
masahikofukasawa | 9:6fa3e7b17c27 | 498 | case Message::CMD_COMPASS_SET_OPERATION_MODE: |
masahikofukasawa | 11:cef8dc1cf010 | 499 | { |
masahikofukasawa | 29:b488d2c89fba | 500 | AkmSensor::Status st = sensor[sensorIndex]->requestCommand(&msg,&resMsg); |
masahikofukasawa | 15:1238993fd75f | 501 | if( (resMsg.getArgNum() == 0) && (st != AkmSensor::SUCCESS) ) |
masahikofukasawa | 15:1238993fd75f | 502 | { |
masahikofukasawa | 39:3821886c902e | 503 | MSG("#Error: Command failed.\r\n"); |
masahikofukasawa | 15:1238993fd75f | 504 | }else{ |
masahikofukasawa | 15:1238993fd75f | 505 | throwMessage(&resMsg); |
masahikofukasawa | 15:1238993fd75f | 506 | } |
masahikofukasawa | 0:7a00359e701e | 507 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 508 | } |
masahikofukasawa | 0:7a00359e701e | 509 | default: |
masahikofukasawa | 11:cef8dc1cf010 | 510 | { |
masahikofukasawa | 39:3821886c902e | 511 | MSG("#Error: Can't find command.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 512 | break; |
masahikofukasawa | 11:cef8dc1cf010 | 513 | } |
masahikofukasawa | 0:7a00359e701e | 514 | } |
masahikofukasawa | 0:7a00359e701e | 515 | } |
masahikofukasawa | 0:7a00359e701e | 516 | |
masahikofukasawa | 0:7a00359e701e | 517 | AkmSensorManager::Status AkmSensorManager::processEvent() |
masahikofukasawa | 0:7a00359e701e | 518 | { |
masahikofukasawa | 0:7a00359e701e | 519 | AkmSensorManager::Status status = AkmSensorManager::SUCCESS; |
masahikofukasawa | 0:7a00359e701e | 520 | |
tkstreet | 33:d3e1e9eb2ef9 | 521 | // If event is a command received from BLE |
masahikofukasawa | 0:7a00359e701e | 522 | if(eventCommandReceived) |
masahikofukasawa | 0:7a00359e701e | 523 | { |
masahikofukasawa | 16:d85be9bafb80 | 524 | // MSG("#Command received.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 525 | processCommand(); |
tkstreet | 21:966724730ce6 | 526 | eventCommandReceived = false; // clear the flag |
masahikofukasawa | 0:7a00359e701e | 527 | } |
masahikofukasawa | 29:b488d2c89fba | 528 | |
masahikofukasawa | 29:b488d2c89fba | 529 | // check sensor event |
masahikofukasawa | 29:b488d2c89fba | 530 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 531 | if( sensor[i]->isEvent() ){ |
masahikofukasawa | 29:b488d2c89fba | 532 | Message msg; |
masahikofukasawa | 29:b488d2c89fba | 533 | if( sensor[i]->readSensorData(&msg) != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
tkstreet | 34:1ea3357c8d9a | 534 | throwMessage(&msg); // Process and output message to USB/BLE |
masahikofukasawa | 29:b488d2c89fba | 535 | } |
masahikofukasawa | 0:7a00359e701e | 536 | } |
masahikofukasawa | 0:7a00359e701e | 537 | |
tkstreet | 34:1ea3357c8d9a | 538 | if(eventConnected) // If BLE connected, clear flag and start sensor. |
masahikofukasawa | 0:7a00359e701e | 539 | { |
tkstreet | 21:966724730ce6 | 540 | eventConnected = false; // clear the flag |
masahikofukasawa | 12:c06cd8b76358 | 541 | MSG("#BLE connected.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 542 | } |
tkstreet | 23:50c98b286e41 | 543 | |
tkstreet | 23:50c98b286e41 | 544 | // If event is the BLE being disconnected, stop the sensor |
tkstreet | 23:50c98b286e41 | 545 | if(eventDisconnected) |
masahikofukasawa | 0:7a00359e701e | 546 | { |
masahikofukasawa | 12:c06cd8b76358 | 547 | MSG("#BLE dis-connected.\r\n"); |
masahikofukasawa | 29:b488d2c89fba | 548 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 549 | if( sensor[i]->stopSensor() != AkmSensor::SUCCESS) status = AkmSensorManager::ERROR; |
masahikofukasawa | 29:b488d2c89fba | 550 | } |
tkstreet | 34:1ea3357c8d9a | 551 | eventDisconnected = false; // clear the flag |
masahikofukasawa | 0:7a00359e701e | 552 | } |
masahikofukasawa | 0:7a00359e701e | 553 | return status; |
masahikofukasawa | 0:7a00359e701e | 554 | } |
masahikofukasawa | 0:7a00359e701e | 555 | |
masahikofukasawa | 0:7a00359e701e | 556 | AkmSensorManager::Status AkmSensorManager::throwMessage(const Message *msg) { |
masahikofukasawa | 0:7a00359e701e | 557 | int len = Message::getMaxMessageLength(); |
masahikofukasawa | 0:7a00359e701e | 558 | char buf[len]; |
masahikofukasawa | 0:7a00359e701e | 559 | |
tkstreet | 23:50c98b286e41 | 560 | buf[0] = '$'; // Output message prefix |
masahikofukasawa | 0:7a00359e701e | 561 | |
tkstreet | 23:50c98b286e41 | 562 | // Processes command in msg to ASCII |
masahikofukasawa | 0:7a00359e701e | 563 | char cmd = (char)msg->getCommand(); |
masahikofukasawa | 0:7a00359e701e | 564 | Message::charToAscii(&buf[1], &cmd); |
masahikofukasawa | 0:7a00359e701e | 565 | |
tkstreet | 23:50c98b286e41 | 566 | // Processes arguments in msg to ASCII |
masahikofukasawa | 0:7a00359e701e | 567 | for (int i=0; i < msg->getArgNum(); i++) { |
masahikofukasawa | 0:7a00359e701e | 568 | char arg = msg->getArgument(i); |
masahikofukasawa | 0:7a00359e701e | 569 | Message::charToAscii(&buf[3+2*i], &arg); |
masahikofukasawa | 0:7a00359e701e | 570 | } |
masahikofukasawa | 0:7a00359e701e | 571 | |
masahikofukasawa | 0:7a00359e701e | 572 | // Add termination characters, 0x0D(\r), \n and \0, to the end of string |
masahikofukasawa | 0:7a00359e701e | 573 | int tIdx = 3 + 2 * (msg->getArgNum()); |
masahikofukasawa | 0:7a00359e701e | 574 | int bufSize = sizeof(buf)/sizeof(buf[0]); |
masahikofukasawa | 0:7a00359e701e | 575 | if ((tIdx + 3) > (bufSize - 1)) { |
masahikofukasawa | 11:cef8dc1cf010 | 576 | MSG("#Error: Message data exceeds the buffer.\r\n"); |
masahikofukasawa | 0:7a00359e701e | 577 | return ERROR; |
masahikofukasawa | 0:7a00359e701e | 578 | } |
masahikofukasawa | 11:cef8dc1cf010 | 579 | buf[tIdx++] = CR; // '\r' |
masahikofukasawa | 11:cef8dc1cf010 | 580 | buf[tIdx++] = LF; // '\n' |
masahikofukasawa | 11:cef8dc1cf010 | 581 | buf[tIdx] = '\0'; |
tkstreet | 23:50c98b286e41 | 582 | |
tkstreet | 33:d3e1e9eb2ef9 | 583 | // If BLE is enabled, send to AKDP app |
tkstreet | 23:50c98b286e41 | 584 | if(isEnabledBle) uartService->writeString(buf); |
tkstreet | 23:50c98b286e41 | 585 | |
tkstreet | 33:d3e1e9eb2ef9 | 586 | // If USB is enabled, send to serial terminal |
masahikofukasawa | 0:7a00359e701e | 587 | if(isEnabledUsb) serial->printf(buf); |
masahikofukasawa | 0:7a00359e701e | 588 | |
masahikofukasawa | 0:7a00359e701e | 589 | return SUCCESS; |
masahikofukasawa | 12:c06cd8b76358 | 590 | } |
masahikofukasawa | 13:d008249f0359 | 591 | |
masahikofukasawa | 27:41aa9fb23a2f | 592 | char* AkmSensorManager::my_strcat(char* str1, char* str2) |
masahikofukasawa | 27:41aa9fb23a2f | 593 | { |
masahikofukasawa | 27:41aa9fb23a2f | 594 | int num1; |
masahikofukasawa | 27:41aa9fb23a2f | 595 | char* str; |
masahikofukasawa | 27:41aa9fb23a2f | 596 | |
masahikofukasawa | 27:41aa9fb23a2f | 597 | num1=strlen(str1) + strlen(str2); |
masahikofukasawa | 27:41aa9fb23a2f | 598 | str = (char *)malloc(num1 + 1); |
masahikofukasawa | 27:41aa9fb23a2f | 599 | sprintf(str,"%s%s",str1,str2); |
masahikofukasawa | 27:41aa9fb23a2f | 600 | return str; |
masahikofukasawa | 27:41aa9fb23a2f | 601 | } |
masahikofukasawa | 27:41aa9fb23a2f | 602 | |
masahikofukasawa | 13:d008249f0359 | 603 | char* AkmSensorManager::getSensorName(){ |
masahikofukasawa | 29:b488d2c89fba | 604 | char* name = ""; |
masahikofukasawa | 29:b488d2c89fba | 605 | for(int i=0; i<sensorNum; i++){ |
masahikofukasawa | 29:b488d2c89fba | 606 | name = my_strcat(name, sensor[i]->getSensorName()); |
masahikofukasawa | 29:b488d2c89fba | 607 | if( sensorNum > (i+1) )name = my_strcat(name, "+"); |
masahikofukasawa | 27:41aa9fb23a2f | 608 | } |
masahikofukasawa | 27:41aa9fb23a2f | 609 | MSG("#Sensor Name='%s'.\r\n",name); |
masahikofukasawa | 27:41aa9fb23a2f | 610 | return name; |
masahikofukasawa | 39:3821886c902e | 611 | } |
masahikofukasawa | 39:3821886c902e | 612 | |
masahikofukasawa | 39:3821886c902e | 613 | void AkmSensorManager::attachInterrupt(){ |
masahikofukasawa | 39:3821886c902e | 614 | switch(drdyType){ |
masahikofukasawa | 39:3821886c902e | 615 | case AkmAkd::INTERRUPT_ENABLED_PP: |
masahikofukasawa | 39:3821886c902e | 616 | { |
masahikofukasawa | 39:3821886c902e | 617 | if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY); |
masahikofukasawa | 39:3821886c902e | 618 | else interrupt = new InterruptIn(I2C_DRDY); |
masahikofukasawa | 39:3821886c902e | 619 | break; |
masahikofukasawa | 39:3821886c902e | 620 | } |
masahikofukasawa | 39:3821886c902e | 621 | case AkmAkd::INTERRUPT_ENABLED_OD: |
masahikofukasawa | 39:3821886c902e | 622 | { |
masahikofukasawa | 39:3821886c902e | 623 | if(primaryId == AkmSensor::AKM_PRIMARY_ID_AKD_SPI) interrupt = new InterruptIn(SPI_DRDY); |
masahikofukasawa | 39:3821886c902e | 624 | else interrupt = new InterruptIn(I2C_DRDY); |
masahikofukasawa | 39:3821886c902e | 625 | break; |
masahikofukasawa | 39:3821886c902e | 626 | } |
masahikofukasawa | 39:3821886c902e | 627 | default: |
masahikofukasawa | 39:3821886c902e | 628 | { |
masahikofukasawa | 39:3821886c902e | 629 | // nothing. |
masahikofukasawa | 39:3821886c902e | 630 | } |
masahikofukasawa | 39:3821886c902e | 631 | } |
masahikofukasawa | 39:3821886c902e | 632 | } |
masahikofukasawa | 39:3821886c902e | 633 | |
masahikofukasawa | 39:3821886c902e | 634 | void AkmSensorManager::detachInterrupt(){ |
masahikofukasawa | 39:3821886c902e | 635 | switch(drdyType){ |
masahikofukasawa | 39:3821886c902e | 636 | case AkmAkd::INTERRUPT_ENABLED_PP: |
masahikofukasawa | 39:3821886c902e | 637 | { |
masahikofukasawa | 39:3821886c902e | 638 | interrupt->rise(NULL); |
masahikofukasawa | 39:3821886c902e | 639 | break; |
masahikofukasawa | 39:3821886c902e | 640 | } |
masahikofukasawa | 39:3821886c902e | 641 | case AkmAkd::INTERRUPT_ENABLED_OD: |
masahikofukasawa | 39:3821886c902e | 642 | { |
masahikofukasawa | 39:3821886c902e | 643 | interrupt->fall(NULL); |
masahikofukasawa | 39:3821886c902e | 644 | break; |
masahikofukasawa | 39:3821886c902e | 645 | } |
masahikofukasawa | 39:3821886c902e | 646 | default: |
masahikofukasawa | 39:3821886c902e | 647 | { |
masahikofukasawa | 39:3821886c902e | 648 | // nothing. |
masahikofukasawa | 39:3821886c902e | 649 | } |
masahikofukasawa | 39:3821886c902e | 650 | } |
masahikofukasawa | 13:d008249f0359 | 651 | } |