Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2011 The Android Open Source Project |
| 3 | * |
| 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | * you may not use this file except in compliance with the License. |
| 6 | * You may obtain a copy of the License at |
| 7 | * |
| 8 | * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | * |
| 10 | * Unless required by applicable law or agreed to in writing, software |
| 11 | * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | * See the License for the specific language governing permissions and |
| 14 | * limitations under the License. |
| 15 | */ |
| 16 | |
| 17 | #include "SensorDevice.h" |
| 18 | #include "SensorFusion.h" |
| 19 | #include "SensorService.h" |
| 20 | |
| 21 | namespace android { |
| 22 | // --------------------------------------------------------------------------- |
| 23 | |
| 24 | ANDROID_SINGLETON_STATIC_INSTANCE(SensorFusion) |
| 25 | |
| 26 | SensorFusion::SensorFusion() |
| 27 | : mSensorDevice(SensorDevice::getInstance()), |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 28 | mEnabled(false), mGyroTime(0) |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 29 | { |
| 30 | sensor_t const* list; |
Mathias Agopian | 7b2b32f | 2011-07-14 16:39:46 -0700 | [diff] [blame] | 31 | ssize_t count = mSensorDevice.getSensorList(&list); |
| 32 | if (count > 0) { |
| 33 | for (size_t i=0 ; i<size_t(count) ; i++) { |
| 34 | if (list[i].type == SENSOR_TYPE_ACCELEROMETER) { |
| 35 | mAcc = Sensor(list + i); |
| 36 | } |
| 37 | if (list[i].type == SENSOR_TYPE_MAGNETIC_FIELD) { |
| 38 | mMag = Sensor(list + i); |
| 39 | } |
| 40 | if (list[i].type == SENSOR_TYPE_GYROSCOPE) { |
| 41 | mGyro = Sensor(list + i); |
| 42 | // 200 Hz for gyro events is a good compromise between precision |
| 43 | // and power/cpu usage. |
| 44 | mGyroRate = 200; |
| 45 | mTargetDelayNs = 1000000000LL/mGyroRate; |
| 46 | } |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 47 | } |
Mathias Agopian | 7b2b32f | 2011-07-14 16:39:46 -0700 | [diff] [blame] | 48 | mFusion.init(); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 49 | } |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 50 | } |
| 51 | |
| 52 | void SensorFusion::process(const sensors_event_t& event) { |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 53 | if (event.type == SENSOR_TYPE_GYROSCOPE) { |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 54 | if (mGyroTime != 0) { |
| 55 | const float dT = (event.timestamp - mGyroTime) / 1000000000.0f; |
| 56 | const float freq = 1 / dT; |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 57 | if (freq >= 100 && freq<1000) { // filter values obviously wrong |
| 58 | const float alpha = 1 / (1 + dT); // 1s time-constant |
| 59 | mGyroRate = freq + (mGyroRate - freq)*alpha; |
| 60 | } |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 61 | } |
| 62 | mGyroTime = event.timestamp; |
| 63 | mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate); |
| 64 | } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) { |
| 65 | const vec3_t mag(event.data); |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 66 | mFusion.handleMag(mag); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 67 | } else if (event.type == SENSOR_TYPE_ACCELEROMETER) { |
| 68 | const vec3_t acc(event.data); |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 69 | mFusion.handleAcc(acc); |
| 70 | mAttitude = mFusion.getAttitude(); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 71 | } |
| 72 | } |
| 73 | |
| 74 | template <typename T> inline T min(T a, T b) { return a<b ? a : b; } |
| 75 | template <typename T> inline T max(T a, T b) { return a>b ? a : b; } |
| 76 | |
| 77 | status_t SensorFusion::activate(void* ident, bool enabled) { |
| 78 | |
| 79 | LOGD_IF(DEBUG_CONNECTIONS, |
| 80 | "SensorFusion::activate(ident=%p, enabled=%d)", |
| 81 | ident, enabled); |
| 82 | |
| 83 | const ssize_t idx = mClients.indexOf(ident); |
| 84 | if (enabled) { |
| 85 | if (idx < 0) { |
| 86 | mClients.add(ident); |
| 87 | } |
| 88 | } else { |
| 89 | if (idx >= 0) { |
| 90 | mClients.removeItemsAt(idx); |
| 91 | } |
| 92 | } |
| 93 | |
| 94 | mSensorDevice.activate(ident, mAcc.getHandle(), enabled); |
| 95 | mSensorDevice.activate(ident, mMag.getHandle(), enabled); |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 96 | mSensorDevice.activate(ident, mGyro.getHandle(), enabled); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 97 | |
| 98 | const bool newState = mClients.size() != 0; |
| 99 | if (newState != mEnabled) { |
| 100 | mEnabled = newState; |
| 101 | if (newState) { |
| 102 | mFusion.init(); |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 103 | mGyroTime = 0; |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 104 | } |
| 105 | } |
| 106 | return NO_ERROR; |
| 107 | } |
| 108 | |
| 109 | status_t SensorFusion::setDelay(void* ident, int64_t ns) { |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 110 | mSensorDevice.setDelay(ident, mAcc.getHandle(), ns); |
| 111 | mSensorDevice.setDelay(ident, mMag.getHandle(), ms2ns(20)); |
| 112 | mSensorDevice.setDelay(ident, mGyro.getHandle(), mTargetDelayNs); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 113 | return NO_ERROR; |
| 114 | } |
| 115 | |
| 116 | |
| 117 | float SensorFusion::getPowerUsage() const { |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 118 | float power = mAcc.getPowerUsage() + |
| 119 | mMag.getPowerUsage() + |
| 120 | mGyro.getPowerUsage(); |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 121 | return power; |
| 122 | } |
| 123 | |
| 124 | int32_t SensorFusion::getMinDelay() const { |
| 125 | return mAcc.getMinDelay(); |
| 126 | } |
| 127 | |
| 128 | void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) { |
| 129 | const Fusion& fusion(mFusion); |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 130 | snprintf(buffer, SIZE, "9-axis fusion %s (%d clients), gyro-rate=%7.2fHz, " |
| 131 | "q=< %g, %g, %g, %g > (%g), " |
| 132 | "b=< %g, %g, %g >\n", |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 133 | mEnabled ? "enabled" : "disabled", |
| 134 | mClients.size(), |
| 135 | mGyroRate, |
| 136 | fusion.getAttitude().x, |
| 137 | fusion.getAttitude().y, |
| 138 | fusion.getAttitude().z, |
Mathias Agopian | 3301542 | 2011-05-27 18:18:13 -0700 | [diff] [blame] | 139 | fusion.getAttitude().w, |
| 140 | length(fusion.getAttitude()), |
Mathias Agopian | 984826c | 2011-05-17 22:54:42 -0700 | [diff] [blame] | 141 | fusion.getBias().x, |
| 142 | fusion.getBias().y, |
| 143 | fusion.getBias().z); |
| 144 | result.append(buffer); |
| 145 | } |
| 146 | |
| 147 | // --------------------------------------------------------------------------- |
| 148 | }; // namespace android |