diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e0292b1 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.o +*.a diff --git a/algorithm/osp/LICENSE b/algorithm/osp/LICENSE new file mode 100644 index 0000000..e06d208 --- /dev/null +++ b/algorithm/osp/LICENSE @@ -0,0 +1,202 @@ +Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright {yyyy} {name of copyright owner} + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + diff --git a/algorithm/osp/Makefile b/algorithm/osp/Makefile new file mode 100644 index 0000000..8895093 --- /dev/null +++ b/algorithm/osp/Makefile @@ -0,0 +1,12 @@ +CC=gcc +CFLAGS=-Wall -g -Iinclude -I../../include -DFEAT_STEP + +OSP_OBJS=SecondOrderLPF.o ecompass.o fp_atan2.o fp_sqrt.o fp_trig.o fpsup.o gravity_lin.o osp.o rotvec.o step.o tilt.o sigmot.o + +all: libOSP.a + +libOSP.a: $(OSP_OBJS) + ar r libOSP.a $(OSP_OBJS) + +clean: + rm -f *.o libOSP.a diff --git a/algorithm/osp/OSP_interface.c b/algorithm/osp/OSP_interface.c new file mode 100644 index 0000000..b7e79ae --- /dev/null +++ b/algorithm/osp/OSP_interface.c @@ -0,0 +1,59 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Interface layer for OSP. Implements a subset of the public + * functions as defined by the OSP API. + */ + +#include + +#include "osp-api.h" + +/* Dummy routines for OSP */ + +OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t* pSystemDesc) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_DoForegroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_DoBackgroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_SetInputData(InputSensorHandle_t h, TriAxisSensorRawData_t *d) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *pSensorDescriptor, InputSensorHandle_t *pReturnedHandle) +{ + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_SubscribeSensorResult(ResultDescriptor_t *pResultDescriptor, ResultHandle_t *pResultHandle) +{ + return OSP_STATUS_OK; +} + + +static const OSP_Library_Version_t Version = { + .VersionNumber = 0x99889988, + .VersionString = "OSP Dummy stub library", + .buildTime = __DATE__"@"__TIME__, +}; + +OSP_STATUS_t OSP_GetVersion(const OSP_Library_Version_t **ppVersionStruct) +{ + *ppVersionStruct = &Version; + return OSP_STATUS_OK; +} + diff --git a/algorithm/osp/SecondOrderLPF.c b/algorithm/osp/SecondOrderLPF.c new file mode 100644 index 0000000..813a344 --- /dev/null +++ b/algorithm/osp/SecondOrderLPF.c @@ -0,0 +1,127 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Second order IIR BiCubicFilter. based on + * Implementation from Android AOSP. + */ + +#include +#include "string.h" +#include "fpsup.h" +#include "lpf.h" +#if 0 +struct LPF { + Q15_t iQ; + Q15_t fc; + Q15_t K; + Q15_t iD; + Q15_t a0; + Q15_t a1; + Q15_t b1; + Q15_t b2; + + Q15_t x1; + Q15_t x2; + Q15_t y1; + Q15_t y2; +}; + +struct LPF_CBQF { + struct LPF mA; + struct LPF mB; +}; +#endif + +void LPF_init(struct LPF *lpf, Q15_t Q, Q15_t fc) +{ + lpf->iQ = RECIP_Q15(Q); + lpf->fc = fc; +} + +void LPF_setSamplingPeriod(struct LPF *lpf, Q15_t dT) +{ + + Q15_t tmp, k2; +#if 1 + tmp = MUL_Q15(q15_pi, lpf->fc); + tmp = MUL_Q15(tmp, dT); +#else + tmp = MUL_Q15(q15_pi, q15_c1); +#endif + lpf->K = tan_q15(tmp); + k2 = MUL_Q15(lpf->K, lpf->K); + + lpf->iD = RECIP_Q15(k2 + MUL_Q15(lpf->K, lpf->iQ) + q15_c1); + lpf->a0 = MUL_Q15(k2, lpf->iD); + lpf->a1 = MUL_Q15(q15_c2, lpf->a0); + lpf->b1 = MUL_Q15(q15_c2, lpf->iD); + lpf->b1 = MUL_Q15(k2 - q15_c1, lpf->b1); + lpf->b2 = MUL_Q15(lpf->iD, (k2 - MUL_Q15(lpf->K, lpf->iQ) + q15_c1)); +} + +Q15_t LPF_BQF_init(struct LPF *lpf, Q15_t x) +{ + lpf->x1 = x; + lpf->x2 = x; + + lpf->y1 = x; + lpf->y2 = x; + + return x; +} + +Q15_t LPF_BQF_data(struct LPF *lpf, Q15_t x) +{ + Q15_t y; + + y = MUL_Q15((x+lpf->x2), lpf->a0); + y += MUL_Q15(lpf->x1, lpf->a1); + y -= MUL_Q15(lpf->y1, lpf->b1); + y -= MUL_Q15(lpf->y2, lpf->b2); + + lpf->x2 = lpf->x1; + lpf->y2 = lpf->y1; + lpf->x1 = x; + lpf->y1 = y; + + return y; +} + + +void LPF_CBQF_init(struct LPF_CBQF *clpf, struct LPF *lpf, Q15_t x) +{ + memcpy(&clpf->mA, lpf, sizeof(struct LPF)); + memcpy(&clpf->mB, lpf, sizeof(struct LPF)); + + LPF_BQF_init(&clpf->mA, x); + LPF_BQF_init(&clpf->mB, x); +} + + +Q15_t LPF_CBQF_data(struct LPF_CBQF *clpf, Q15_t x) +{ + return LPF_BQF_data(&clpf->mB, LPF_BQF_data(&clpf->mA, x)); +} + +#if 0 +int main(int argc, char **argv) +{ + struct LPF lpf; + struct LPF_CBQF clpf; + Q15_t d; + + int i; + + LPF_init(&lpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_setSamplingPeriod(&lpf, FP_to_Q15(0.020f)); + LPF_CBQF_init(&clpf, &lpf, FP_to_Q15(9.8f)); + + for (i = 0; i < 200; i++) { + d = LPF_CBQF_data(&clpf, FP_to_Q15(9.8f)); + printf("Data: %f\n", Q15_to_FP(d)); + } +} +#endif diff --git a/algorithm/osp/ecompass.c b/algorithm/osp/ecompass.c new file mode 100644 index 0000000..ec23817 --- /dev/null +++ b/algorithm/osp/ecompass.c @@ -0,0 +1,150 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Project magnetometer data onto the X/Y plane using the + * accel. Angles are computed from that using trig functions. + */ + +#include +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "ecompass.h" + +struct Heading { + float roll; + float pitch; + float yaw; +}; + +static struct ThreeAxis HardIron; +static int calstate; +static int samplecount = 0; +static struct ThreeAxis minMag, maxMag; + +static Q15_t deg_atan2(Q15_t y, Q15_t x) +{ + return MUL_Q15(atan2_q15(y, x), FP_to_Q15(180.0/M_PI)); +} + + +static void adj_mag(struct ThreeAxis *mag) +{ + mag->x -= HardIron.x; + mag->y -= HardIron.y; + mag->z -= HardIron.z; +} + + +static void adj_acc(struct ThreeAxis *acc) +{ +} + +void OSP_ecompass_init(void) +{ + calstate = 0; + HardIron.x = 0; HardIron.y = 0; HardIron.z = 0; +} + +void OSP_ecompass_process(struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Euler *result) +{ + Q15_t pMx, pMy; + Q15_t hyp; + + samplecount++; + + if (calstate) { + /* Assume 30Hz, this is about 30 seconds */ + if (samplecount > 900) { + calstate = 0; + do { + if (maxMag.x == MIN_Q15 || + maxMag.y == MIN_Q15 || + maxMag.z == MIN_Q15) + break; + if (minMag.x == MAX_Q15 || + minMag.y == MAX_Q15 || + minMag.z == MAX_Q15) + break; + + HardIron.x = MUL_Q15((maxMag.x - minMag.x), q15_half); + HardIron.x = HardIron.x - maxMag.x; + + + HardIron.y = MUL_Q15((maxMag.y - minMag.y), q15_half); + HardIron.y = HardIron.y - maxMag.y; + + HardIron.z = MUL_Q15((maxMag.z - minMag.z), q15_half); + HardIron.z = HardIron.z - maxMag.z; + + } while (0); + printf("Cal completed - Offsets: %f %f %f\n", + Q15_to_FP(HardIron.x), + Q15_to_FP(HardIron.y), + Q15_to_FP(HardIron.z)); + } else { + if (mag->x > maxMag.x) maxMag.x = mag->x; + if (mag->y > maxMag.y) maxMag.y = mag->y; + if (mag->z > maxMag.z) maxMag.z = mag->z; + + if (mag->x < minMag.x) minMag.x = mag->x; + if (mag->y < minMag.y) minMag.y = mag->y; + if (mag->z < minMag.z) minMag.z = mag->z; + } + } + + adj_mag(mag); + adj_acc(acc); +#if 0 + acc->y = -acc->y; + acc->x = -acc->x; + acc->z = -acc->z; +#endif + result->roll = deg_atan2(acc->x, acc->z); +#if 0 + result->roll = result->roll+FP_to_Q15(180.0f); + if (result->roll > q15_c180) result->roll -= q15_c360; +#endif + + result->pitch = deg_atan2(acc->y, acc->z); +#if 0 + result->pitch = result->roll + q15_c180; + if (result->pitch > q15_c180) result->pitch -= q15_c360; +#endif + result->pitch = -result->pitch; + hyp = sqrt_q15(MUL_Q15(acc->x, acc->x) + MUL_Q15(acc->z, acc->z)); + if (hyp == 0) return; + + pMx = (MUL_Q15(mag->x, DIV_Q15(acc->z,hyp)) + MUL_Q15(mag->z, DIV_Q15(acc->x,hyp))); + + hyp = sqrt_q15(MUL_Q15(acc->y, acc->y) + MUL_Q15(acc->z, acc->z)); + if (hyp == 0) return; + + pMy = (MUL_Q15(mag->y, DIV_Q15(acc->z,hyp)) + MUL_Q15(mag->z, DIV_Q15(acc->y,hyp))); + + result->yaw = -1 * deg_atan2(pMx, pMy); + result->yaw += q15_c180; +#if 0 + printf("%s roll %f pitch %f yaw %f\n", __func__, Q15_to_FP(result->roll), Q15_to_FP(result->pitch), Q15_to_FP(result->yaw)); +#endif +} + +void OSP_ecompass_cal(void) +{ + calstate = 1; + samplecount = 0; + minMag.x = MAX_Q15; + minMag.y = MAX_Q15; + minMag.z = MAX_Q15; + + maxMag.x = MIN_Q15; + maxMag.y = MIN_Q15; + maxMag.z = MIN_Q15; + + printf("Cal requested\n"); +} diff --git a/algorithm/osp/ecompass.h b/algorithm/osp/ecompass.h new file mode 100644 index 0000000..1a09f31 --- /dev/null +++ b/algorithm/osp/ecompass.h @@ -0,0 +1,12 @@ +#ifndef _ECOMPAAS_H_ +#define _ECOMPASS_H_ 1 +#include "fp_sensor.h" + +/* Start a cal sequence */ +void OSP_ecompass_cal(void); +void OSP_ecompass_process(struct ThreeAxis *, + struct ThreeAxis *, + struct Euler *result); +void OSP_ecompass_init(void); + +#endif diff --git a/algorithm/osp/fp_atan2.c b/algorithm/osp/fp_atan2.c new file mode 100644 index 0000000..6d678aa --- /dev/null +++ b/algorithm/osp/fp_atan2.c @@ -0,0 +1,82 @@ +/* + * Approximate atan2 + */ + +#include +#include +#include +#include "fpsup.h" + +/* + * arctan of y/x + */ + +static const Q15_t q15_pi_4 = FP_to_Q15(M_PI/4.0); + +Q15_t atan2_q15(Q15_t y, Q15_t x) +{ + const Q15_t q15_3_4_PI = FP_to_Q15(3.0*M_PI/4.0f); + Q15_t abs_y, r, rt, angle; + + if (y == 0 && x < 0) return q15_pi; + if (y == 0 && x > 0) return 0; + if (y < 0 && x == 0) return -(q15_pi >> 1); /* -PI/2 */ + if (y > 0 && x == 0) return (q15_pi >> 1); /* PI/2 */ + if (y == 0 && x == 0) return q15_pi; + + abs_y = abs_q15(y); + + if (x >= 0) { + rt = RECIP_Q15(x+abs_y); + r = MUL_Q15((x-abs_y), rt); + + angle = q15_pi_4 - MUL_Q15(q15_pi_4, r); + } else { + rt = RECIP_Q15(abs_y-x); + r = MUL_Q15((x+abs_y), rt); + angle = q15_3_4_PI - MUL_Q15(q15_pi_4, r); + } + + if (y < 0) + return -angle; + else + return angle; +} +#ifdef TEST +int main(int argc, char **argv) +{ + float ang; + float ang_rad; + Q15_t qy, qx, qat; + float fy, fx, fat; + float err = 0.0; + + if (argc > 1) { + err = atof(argv[1]); + if (err > 2.0) + err = 0.0; + } + + fprintf(stderr, "Printing errors greater then %f\n", err); + + for (ang = 0; ang < 360.0f; ang+= 0.5) { + ang_rad = ang * M_PI/180.0f; + qx = q15_c1; + fx = 1.0; + fy = tan(ang_rad); + qy = FP_to_Q15(fy); + + fat = atan2(fy, fx); + qat = atan2_q15(qy, qx); + if ((Q15_to_FP(qat)-fat) > err) { + + fprintf(stderr, + "ANG: %f - tan = %f (%f), atan() = %f (%f) err = %f\n", + ang, fy, Q15_to_FP(qy), + fat, Q15_to_FP(qat), + Q15_to_FP(qat)-fat); + } + } +} + +#endif diff --git a/algorithm/osp/fp_sensor.h b/algorithm/osp/fp_sensor.h new file mode 100644 index 0000000..1b11eaf --- /dev/null +++ b/algorithm/osp/fp_sensor.h @@ -0,0 +1,65 @@ +#ifndef FP_SENSOR_H +#define FP_SENSOR_H 1 + +#include "fpsup.h" +#include "osp-sensors.h" + +/* NTEXTENDED is int32_t Q12 */ +/* NTPRECISE is int32_t Q24 */ +#define Q15_to_NTEXTENDED(x) (x >> (Q15_SHIFT-12)) +#define Q15_to_NTPRECISE(x) (x << (24-Q15_SHIFT)) +#define NTEXTENDED_to_Q15(x) (x << (Q15_SHIFT-12)) +#define NTPRECISE_to_Q15(x) (x >> (24-Q15_SHIFT)) + +#define FLAG(x) (1 << x) + +struct ThreeAxis { + Q15_t x; + Q15_t y; + Q15_t z; +}; + +struct Euler { + Q15_t roll; + Q15_t pitch; + Q15_t yaw; +}; + +struct Quat { + Q15_t x; + Q15_t y; + Q15_t z; + Q15_t w; +}; + +#ifdef FEAT_ROTVEC_PRECISE +struct Quat_precise { + Q24_t x; + Q24_t y; + Q24_t z; + Q24_t w; +}; +#endif + +struct StepInfo { + int32_t count; + int32_t detect; +}; + +struct Results { + union { + struct ThreeAxis result; + struct Euler euler; +#ifdef FEAT_ROTVEC_PRECISE + struct Quat_precise quat; +#else + struct Quat quat; +#endif +#ifdef FEAT_STEP + struct StepInfo step; +#endif + } ResType; + uint32_t time; +}; + +#endif diff --git a/algorithm/osp/fp_sqrt.c b/algorithm/osp/fp_sqrt.c new file mode 100644 index 0000000..86473ed --- /dev/null +++ b/algorithm/osp/fp_sqrt.c @@ -0,0 +1,98 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Math support routines for sqrt. + */ +/* Apache Licensed code from PebbleFit */ +#include +#include "fpsup.h" + +#ifdef ENABLE_Q12 +Q12_t sqrt_q12(Q12_t num) +{ + Q12_t a, p, e = FP_to_Q12(0.001f), b; + Q12_t t; + int nb = 0; + + a = num; + + p = MUL_Q12(a, a);; + while ((p - num >= e) && (nb++ < 40)) { + t = RECIP_Q12(a); + b = a + MUL_Q12(num, t); + a = b >> 1; + p = MUL_Q12(a, a); + } + + return a; +} +#endif +Q15_t sqrt_q15(Q15_t num) +{ + Q15_t ans; + LQ15_t a, b; + LQ15_t p, e = FP_to_Q15(0.0001f); + LQ15_t t, numl; + int nb = 0; + + numl = num; + a = numl; + + p = MUL_LQ15(a, a); + while ((abs_lq15(p - num) >= e) && (nb++ < 64)) { + t = RECIP_LQ15(a); + b = a + MUL_LQ15(numl, t); + a = b >> 1; + p = MUL_LQ15(a, a); + nb++; + } + ans = a; +#if 0 + printf("SQRT: %f = %f nb = %i\n", LQ15_to_FP(numl), LQ15_to_FP(a), nb); +#endif + return ans; +} + +#ifdef ENABLE_Q24 +Q24_t sqrt_q24(Q24_t num) +{ + Q24_t a, p, e = FP_to_Q24(0.001f), b; + Q24_t t; + int nb = 0; + + a = num; + + p = MUL_Q24(a, a);; + while ((abs_q24(p - num) >= e) && (nb++ < 40)) { + t = RECIP_Q24(a); + b = a + MUL_Q24(num, t); + a = b >> 1; + p = MUL_Q24(a, a); + nb++; + } + return a; +} +#endif + +#ifdef TEST_SQRT +#include +int main(int argc, char **argv) +{ + float testv[] = { + 37483.949219f, + -1.0f + }; + int i; + + for (i = 0; testv[i] > 0; i++) { + printf("SQRT(%f) = (Q15: %f, float: %f)\n", + testv[i], Q15_to_FP(sqrt_q15(FP_to_Q15(testv[i]))), + sqrt(testv[i])); + } + return 0; +} + +#endif diff --git a/algorithm/osp/fp_trig.c b/algorithm/osp/fp_trig.c new file mode 100644 index 0000000..6c36dd9 --- /dev/null +++ b/algorithm/osp/fp_trig.c @@ -0,0 +1,77 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Table look up tan/sin/cos + */ +#include "fpsup.h" +#include "trig_sin.c" + + +Q15_t sin_q15(Q15_t ang) +{ + int adj = 1; + int i; + + do { + /* Sine is odd */ + if (ang >= q15_pi/2 && ang < q15_pi) { + ang -= q15_pi/2; + break; + } else if (ang >= q15_pi) { + ang -= q15_pi; + adj = -1 * adj; + } else if (ang < 0) { + ang += q15_pi; + adj = -1 * adj; + } else { + /* Between 0 - pi/2 */ + break; + } + } while(1); + + for (i = 0; i < sizeof(trig_sin)/sizeof(struct TRIG_SIN); i++) { + if (ang < trig_sin[i].ang) { + return trig_sin[i].sin * adj; + } + } + return 0; +} + +Q15_t cos_q15(Q15_t ang) +{ + return sin_q15(ang + q15_pi/2); +} + +Q15_t tan_q15(Q15_t ang) +{ + return DIV_Q15(sin_q15(ang),cos_q15(ang)); +} + +Q15_t arcsin_q15(Q15_t v) +{ + Q15_t l, ang; + int i, adj; + + if (v < 0) adj = -1; else adj = 1; + l = v * adj; + + for (i = 0; i < sizeof(trig_sin)/sizeof(struct TRIG_SIN); i++) { + if (l < trig_sin[i].sin) { + ang = trig_sin[i].ang; + break; + } + } + return ang*adj; + +} + +Q15_t arccos_q15(Q15_t v) +{ + Q15_t ang; + + ang = arcsin_q15(v); + return (q15_pi/2 - ang); +} diff --git a/algorithm/osp/fpsup.c b/algorithm/osp/fpsup.c new file mode 100644 index 0000000..2d31663 --- /dev/null +++ b/algorithm/osp/fpsup.c @@ -0,0 +1,319 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + +/* + * Fixed point math support library. + * Q15 - value is scaled by 1<<15 and stored in an int32_t + * Q24 - value is scaled by 1<<24 and stored in an int32_t + */ + +#include +#include +#include +#include "fpsup.h" + +const Q15_t q15_pi = FP_to_Q15(M_PI); +const Q15_t q15_c360 = FP_to_Q15(360.0f); +const Q15_t q15_c180 = FP_to_Q15(180.0f); +const Q15_t q15_c90 = FP_to_Q15(90.0f); +const Q15_t q15_c1 = FP_to_Q15(1.0f); +const Q15_t q15_c2 = FP_to_Q15(2.0f); +const Q15_t q15_half = FP_to_Q15(0.5f); +const Q15_t q15_quarter = FP_to_Q15(0.25f); + +#ifdef ENABLE_Q12 +const Q15_t q12_pi = FP_to_Q12(M_PI); +const Q15_t q12_c360 = FP_to_Q12(360.0f); +const Q15_t q12_c180 = FP_to_Q12(180.0f); +const Q15_t q12_c90 = FP_to_Q12(90.0f); +const Q15_t q12_c1 = FP_to_Q12(1.0f); +const Q15_t q12_c2 = FP_to_Q12(2.0f); +const Q15_t q12_half = FP_to_Q12(0.5f); +const Q15_t q12_quarter = FP_to_Q12(0.25f); +#endif + +#ifdef ENABLE_Q24 +const Q24_t q24_pi = FP_to_Q24(M_PI); +const Q24_t q24_c90 = FP_to_Q24(90.0f); +const Q24_t q24_c1 = FP_to_Q24(1.0f); +const Q24_t q24_c2 = FP_to_Q24(2.0f); +const Q24_t q24_half = FP_to_Q24(0.5f); +const Q24_t q24_quarter = FP_to_Q24(0.25f); +#endif + +/* MUL_LQ15 */ +/* + * LQ15 - 64 bit storage: + * 1 signed bit + * 63 bits of value divided into: + * 48bits of integer + * 15bits of fractional. + * Multiplication of 2 LQ15 produces 126bits of value. + * 15bits needs to be thrown away to convert it to 15 bits of + * fractional. This leaves 111 bits of numbers. + * + * LQ15 is to support intermediates used for sqrt() Q15 guesses. So + * the largest value needed is: + * 1 sign bit + * 2*16 integer bits + * 15 fractional bits + * ------ + * 48bits total. Thus the upper 63 bits is always zero. (except for + * sign extension). + */ +LQ15_t MUL_LQ15(LQ15_t a, LQ15_t b) +{ + /* Long multiplication principle: + * (From: http://www.edaboard.com/thread253439.html) + * + * x,y is the 2 multiplier (signed 64 bit) + * after sign valuation and (if need) sign inversion + * it is split into xh xl yh yl (unsigned 32 bit) + * the operations performed are: + * + * xh xl * + * yh yl = + * ------- + * xl_yl + + * xh_yl + + * xl_yh + + * xh_yh = + * ------------- + * (sign) z3 z1 z2 z0 + * + */ + + int8_t sign = 1; + + /* Intermediates */ + uint64_t al_bl, ah_bl, al_bh, ah_bh; + + uint32_t al, ah; + uint32_t bl, bh; + + uint64_t res0, res1; + + if (a<0) { + a = -a; + sign = -sign; + } + + if (b<0) { + b = -b; + sign = -sign; + } + + al = a & 0xffffffff; ah = a>>32; + bl = b & 0xffffffff; bh = b>>32; + if (ah != 0 || bh != 0) { + fprintf(stderr, "HY-DBG: OVERFLOW in %s\n", __func__); + return 0; + } + + al_bl = (uint64_t)al*(uint64_t)bl; + ah_bl = (uint64_t)ah*(uint64_t)bl; + al_bh = (uint64_t)al*(uint64_t)bh; + ah_bh = (uint64_t)ah*(uint64_t)bh; + + res0 = al_bl & 0xffffffff; + + res1 = (al_bl>>32)+(ah_bl & 0xffffffff)+(al_bh & 0xffffffff); + res0 |= (res1&0xffffffff) << 32; + + res1 >>= 32; // this is 'carry out' of the previous sum + res1 += (ah_bl>>32) + (al_bh>>32) +ah_bh; + + res0 >>= Q15_SHIFT; + + res0 &= ~(((uint64_t)(1<<15)-1) << (64-Q15_SHIFT)); + res0 |= (res1 & ((1<<15)-1)) << (64-Q15_SHIFT); + + return res0; +} + +/* MUL_Q15 */ + +Q15_t MUL_Q15(Q15_t a, Q15_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q15_SHIFT; +} + +#ifdef ENABLE_Q12 +Q12_t MUL_Q12(Q12_t a, Q12_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q12_SHIFT; +} +#endif + +#ifdef ENABLE_Q24 +/* MUL_Q24 */ +Q24_t MUL_Q24(Q24_t a, Q24_t b) +{ + int64_t tmp; + + tmp = (int64_t)a * (int64_t)b; + return tmp >> Q24_SHIFT; +} +#endif +LQ15_t DIV_LQ15(LQ15_t a, LQ15_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q15_SHIFT; + + return tmp/b; +} +/* DIV_Q15 */ +Q15_t DIV_Q15(Q15_t a, Q15_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q15_SHIFT; + + return tmp/b; +} +#ifdef ENABLE_Q12 +/* DIV_Q12 */ +Q12_t DIV_Q12(Q12_t a, Q12_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q12_SHIFT; + + return tmp/b; +} +#endif + +#ifdef ENABLE_Q24 +/* DIV_Q24 */ +Q24_t DIV_Q24(Q24_t a, Q24_t b) +{ + int64_t tmp; + tmp = (int64_t)a << Q24_SHIFT; + + return tmp/b; +} +#endif +/* RECIP_Q15 */ + +LQ15_t RECIP_LQ15(LQ15_t a) +{ + return DIV_LQ15(FP_to_Q15(1.0f), a); +} + + +Q15_t RECIP_Q15(Q15_t a) +{ + return DIV_Q15(q15_c1, a); +} + +#ifdef ENABLE_Q12 +Q12_t RECIP_Q12(Q12_t a) +{ + return DIV_Q12(q12_c1, a); +} +#endif + +#ifdef ENABLE_Q24 +/* RECIP_Q24 */ +Q24_t RECIP_Q24(Q24_t a) +{ + return DIV_Q24(q24_c1, a); +} +#endif + +double Q15_to_FP(Q15_t v) +{ + return ((double)v)/(double)(1< + +#ifndef M_PI +#define M_PI 3.141592f +#endif +typedef int32_t Q15_t; +#define Q15_SHIFT 15 +#define FP_to_Q15(v) ((v)*(1<> (Q15_SHIFT-Q12_SHIFT)) +#define Q12_to_Q15(x) ((x) << (Q15_SHIFT-Q12_SHIFT)) +Q12_t RECIP_Q12(Q12_t); +Q12_t DIV_Q12(Q12_t, Q12_t); +#endif + +#define MAX_Q15 0x7fffffff +#define MIN_Q15 0x80000000 + +Q15_t DIV_Q15(Q15_t, Q15_t); +LQ15_t DIV_LQ15(LQ15_t, LQ15_t); +Q15_t MUL_Q15(Q15_t, Q15_t); +LQ15_t MUL_LQ15(LQ15_t, LQ15_t); +Q15_t RECIP_Q15(Q15_t); +LQ15_t RECIP_LQ15(LQ15_t); + +extern const Q15_t q15_pi; +extern const Q15_t q15_c360; +extern const Q15_t q15_c180; +extern const Q15_t q15_c90; +extern const Q15_t q15_c1; +extern const Q15_t q15_c2; +extern const Q15_t q15_half; +extern const Q15_t q15_quarter; + +#ifdef ENABLE_Q12 +const Q12_t q12_pi; +const Q12_t q12_c360; +const Q12_t q12_c180; +const Q12_t q12_c90; +const Q12_t q12_c1; +const Q12_t q12_c2; +const Q12_t q12_half; +const Q12_t q12_quarter; +#endif + +#ifdef ENABLE_Q24 +Q24_t DIV_Q24(Q24_t, Q24_t); +Q24_t MUL_Q24(Q24_t, Q24_t); +Q24_t RECIP_Q24(Q24_t); + +const Q24_t q24_pi; +const Q24_t q24_c360; +const Q24_t q24_c180; +const Q24_t q24_c90; +const Q24_t q24_c1; +const Q24_t q24_c2; +const Q24_t q24_half; +const Q24_t q24_quarter; + +double Q24_to_FP(Q24_t); +#endif +double Q15_to_FP(Q15_t); +double LQ15_to_FP(LQ15_t); + +#ifdef ENABLE_Q12 +double Q12_to_FP(Q12_t); +#endif + +Q15_t atan2_q15(Q15_t, Q15_t); +Q15_t tan_q15(Q15_t); +Q15_t sqrt_q15(Q15_t); +Q15_t arccos_q15(Q15_t); +Q15_t arcsin_q15(Q15_t); +#ifdef ENABLE_Q24 +Q15_t sqrt_q24(Q15_t); +#endif + +Q15_t pow_q15(Q15_t, Q15_t); +Q15_t abs_q15(Q15_t); +LQ15_t abs_lq15(LQ15_t); +#endif diff --git a/algorithm/osp/gravity_lin.c b/algorithm/osp/gravity_lin.c new file mode 100644 index 0000000..1e20607 --- /dev/null +++ b/algorithm/osp/gravity_lin.c @@ -0,0 +1,54 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Gravity and linear acceleration computation from AOSP. + */ +#include "fpsup.h" +#include "lpf.h" +#include "fp_sensor.h" +#include "gravity_lin.h" + +static struct LPF xlpf, ylpf, zlpf; +static struct LPF_CBQF xclpf, yclpf, zclpf; + +static const Q15_t SAMPLE_PERIOD = FP_to_Q15(0.020f); + +void OSP_gravity_init(void) +{ + LPF_init(&xlpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_init(&ylpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_init(&zlpf,FP_to_Q15(0.707107f), FP_to_Q15(1.5f)); + LPF_setSamplingPeriod(&xlpf, SAMPLE_PERIOD); + LPF_setSamplingPeriod(&ylpf, SAMPLE_PERIOD); + LPF_setSamplingPeriod(&zlpf, SAMPLE_PERIOD); + LPF_CBQF_init(&xclpf, &xlpf, 0); + LPF_CBQF_init(&yclpf, &ylpf, 0); + LPF_CBQF_init(&zclpf, &zlpf, FP_to_Q15(9.8f)); +} + +void OSP_linear_acc_init(void) +{ +} + +void OSP_gravity_process(struct ThreeAxis *acc, struct ThreeAxis *res) +{ + res->x = LPF_CBQF_data(&xclpf, acc->x); + res->y = LPF_CBQF_data(&yclpf, acc->y); + res->z = LPF_CBQF_data(&zclpf, acc->z); +}; + +void OSP_linear_acc_process(struct ThreeAxis *acc, + struct ThreeAxis *gravity, + struct ThreeAxis *res) +{ + res->x = acc->x - (gravity->x); + res->y = acc->y - (gravity->y); + res->z = acc->z - (gravity->z); +} diff --git a/algorithm/osp/gravity_lin.h b/algorithm/osp/gravity_lin.h new file mode 100644 index 0000000..6718acc --- /dev/null +++ b/algorithm/osp/gravity_lin.h @@ -0,0 +1,13 @@ +#ifndef _GRAVITY_LIN_H_ +#define _GRAVITY_LIN_H_ 1 +#include "fp_sensor.h" + +void OSP_gravity_init(void); +void OSP_gravity_process(struct ThreeAxis *acc, struct ThreeAxis *res); + +void OSP_linear_acc_init(void); +void OSP_linear_acc_process(struct ThreeAxis *acc, + struct ThreeAxis *gravity, + struct ThreeAxis *res); + +#endif diff --git a/algorithm/osp/include/osp-alg-types.h b/algorithm/osp/include/osp-alg-types.h new file mode 100755 index 0000000..aabbeff --- /dev/null +++ b/algorithm/osp/include/osp-alg-types.h @@ -0,0 +1,80 @@ +/* Open Sensor Platform Project + * https://github.com/sensorplatforms/open-sensor-platform + * + * Copyright (C) 2013 Sensor Platforms Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#if !defined (OSP_ALG_TYPES) +#define OSP_ALG_TYPES + +/*-------------------------------------------------------------------------------------------------*\ + | I N C L U D E F I L E S +\*-------------------------------------------------------------------------------------------------*/ +#include +#include "osp-types.h" +#include "osp-fixedpoint-types.h" + +/*-------------------------------------------------------------------------------------------------*\ + | C O N S T A N T S & M A C R O S +\*-------------------------------------------------------------------------------------------------*/ +#define NUM_ACCEL_AXES (3) + +/*-------------------------------------------------------------------------------------------------*\ + | T Y P E D E F I N I T I O N S +\*-------------------------------------------------------------------------------------------------*/ +//! Enumeration typedef for segment type +typedef enum EStepSegmentType { + firstStep, + midStep, + lastStep +} EStepSegmentType; + +//! Struct definition for a step segment +typedef struct { + //store start/stop times of the last step + NTTIME startTime; + NTTIME stopTime; + EStepSegmentType type; +} StepSegment_t; + +//struct for defining a step +typedef struct StepDataOSP_t{ + NTTIME startTime; + NTTIME stopTime; + osp_float_t stepFrequency; + uint32_t numStepsTotal; + uint32_t numStepsSinceWalking; +} StepDataOSP_t; + +typedef void (*OSP_StepSegmentResultCallback_t)(StepSegment_t * segment); +typedef void (*OSP_StepResultCallback_t)(StepDataOSP_t* stepData); +typedef void (*OSP_EventResultCallback_t)(NTTIME * eventTime); + +/*-------------------------------------------------------------------------------------------------*\ + | E X T E R N A L V A R I A B L E S & F U N C T I O N S +\*-------------------------------------------------------------------------------------------------*/ + +/*-------------------------------------------------------------------------------------------------*\ + | P U B L I C V A R I A B L E S D E F I N I T I O N S +\*-------------------------------------------------------------------------------------------------*/ + +/*-------------------------------------------------------------------------------------------------*\ + | P U B L I C F U N C T I O N D E C L A R A T I O N S +\*-------------------------------------------------------------------------------------------------*/ + + +#endif /* OSP_ALG_TYPES */ +/*-------------------------------------------------------------------------------------------------*\ + | E N D O F F I L E +\*-------------------------------------------------------------------------------------------------*/ diff --git a/algorithm/osp/lpf.h b/algorithm/osp/lpf.h new file mode 100644 index 0000000..7e05da0 --- /dev/null +++ b/algorithm/osp/lpf.h @@ -0,0 +1,35 @@ +#ifndef LPF_H +#define LPF_H 1 + +struct LPF { + Q15_t iQ; + Q15_t fc; + Q15_t K; + Q15_t iD; + Q15_t a0; + Q15_t a1; + Q15_t b1; + Q15_t b2; + + Q15_t x1; + Q15_t x2; + Q15_t y1; + Q15_t y2; +}; + +struct LPF_CBQF { + struct LPF mA; + struct LPF mB; +}; + +void LPF_init(struct LPF *lpf, Q15_t Q, Q15_t fc); +void LPF_setSamplingPeriod(struct LPF *lpf, Q15_t dT); +Q15_t LPF_BQF_init(struct LPF *lpf, Q15_t x); +Q15_t LPF_BQF_data(struct LPF *lpf, Q15_t x); +void LPF_CBQF_init(struct LPF_CBQF *clpf, struct LPF *lpf, Q15_t x); +Q15_t LPF_CBQF_data(struct LPF_CBQF *clpf, Q15_t x); + + + + +#endif diff --git a/algorithm/osp/osp.c b/algorithm/osp/osp.c new file mode 100644 index 0000000..763f0e7 --- /dev/null +++ b/algorithm/osp/osp.c @@ -0,0 +1,753 @@ +/* + * (C) Copyright 2015 HY Research LLC + * + * Author: hy-git@hy-research.com + * + * + * Apache License. + * + * Interface layer for OSP. Implements a subset of the public + * functions as defined by the OSP API. Translates between the + * Fix point names used by OSP to a more generic Q24/Q12/Q15/etc + * naming scheme. + */ + + + + +#include +#include +#include "fp_sensor.h" +#include "fpsup.h" +#include "ecompass.h" +#include "gravity_lin.h" +#include "rotvec.h" +#include "sigmot.h" +#include "osp-sensors.h" +#include "osp-fixedpoint-types.h" +#include "osp-version.h" +#include "osp-api.h" +#include "osp-alg-types.h" +#if 0 +#include "signalgenerator.h" +#include "significantmotiondetector.h" +#include "stepdetector.h" +#endif +#include "step.h" +#include "tilt.h" + +#define CLEAN 0 +#define DIRTY_IN (1<<0) +#define DIRTY_OUT (1<<1) + +#define SEN_ENABLE 1 + +static struct Results RESULTS[NUM_ANDROID_SENSOR_TYPE]; +static unsigned char dirty[NUM_ANDROID_SENSOR_TYPE]; +static SystemDescriptor_t const *sys; +static SensorDescriptor_t const *InputSensors[NUM_ANDROID_SENSOR_TYPE]; + +static const OSP_Library_Version_t libVersion = { + .VersionNumber = (OSP_VERSION_MAJOR << 16) | (OSP_VERSION_MINOR << 8) | (OSP_VERSION_PATCH), + .VersionString = OSP_VERSION_STRING, + .buildTime = __DATE__" "__TIME__ +}; + +static const uint64_t depend[NUM_ANDROID_SENSOR_TYPE] = +{ + [SENSOR_ACCELEROMETER] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_MAGNETIC_FIELD] = FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_ORIENTATION] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_ROTATION_VECTOR] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = FLAG(SENSOR_GRAVITY)|FLAG(SENSOR_MAGNETIC_FIELD), + [SENSOR_GRAVITY] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_LINEAR_ACCELERATION] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_GYROSCOPE] = FLAG(SENSOR_GYROSCOPE), + [SENSOR_PRESSURE] = FLAG(SENSOR_PRESSURE), + [SENSOR_STEP_DETECTOR] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_STEP_COUNTER] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_SIGNIFICANT_MOTION] = FLAG(SENSOR_ACCELEROMETER), + [SENSOR_TILT_DETECTOR] = FLAG(SENSOR_ACCELEROMETER), +}; + + +static ResultDescriptor_t * resHandles[NUM_ANDROID_SENSOR_TYPE]; + +static uint8_t sensor_state[NUM_ANDROID_SENSOR_TYPE]; +static void (*readyCB[NUM_ANDROID_SENSOR_TYPE])(struct Results *, int); + +static void OSP_SetDataMag(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.x = x; + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.y = y; + RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result.z = z; + RESULTS[SENSOR_MAGNETIC_FIELD].time = time; + dirty[SENSOR_MAGNETIC_FIELD] = DIRTY_IN | DIRTY_OUT; +} + +static void OSP_SetDataAcc(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + osp_float_t measurementFiltered[NUM_ACCEL_AXES]; + osp_float_t measurementFloat[NUM_ACCEL_AXES]; + NTTIME filterTime = time; + + RESULTS[SENSOR_ACCELEROMETER].ResType.result.x = x; + RESULTS[SENSOR_ACCELEROMETER].ResType.result.y = y; + RESULTS[SENSOR_ACCELEROMETER].ResType.result.z = z; + RESULTS[SENSOR_ACCELEROMETER].time = time; + dirty[SENSOR_ACCELEROMETER] = DIRTY_IN | DIRTY_OUT; +#if 0 + measurementFloat[0] = Q15_to_FP(x); + measurementFloat[1] = Q15_to_FP(y); + measurementFloat[2] = Q15_to_FP(z); + + if (SignalGenerator_SetAccelerometerData(measurementFloat, + measurementFiltered)){ + filterTime -= SIGNAL_GENERATOR_DELAY; + + //update significant motion alg + SignificantMotDetector_SetFilteredAccelerometerMeasurement( + filterTime, + measurementFiltered); + + StepDetector_SetFilteredAccelerometerMeasurement(filterTime, + measurementFiltered); + } +#endif +} + +static void OSP_SetDataGyr(Q15_t x, Q15_t y, Q15_t z, NTTIME time) +{ + RESULTS[SENSOR_GYROSCOPE].ResType.result.x = x; + RESULTS[SENSOR_GYROSCOPE].ResType.result.y = y; + RESULTS[SENSOR_GYROSCOPE].ResType.result.z = z; + RESULTS[SENSOR_GYROSCOPE].time = time; + dirty[SENSOR_GYROSCOPE] = DIRTY_IN | DIRTY_OUT; +} + +void OSPalg_SetDataBaro(Q15_t p, Q15_t t, NTTIME time) +{ + RESULTS[SENSOR_PRESSURE].ResType.result.x = p; + RESULTS[SENSOR_PRESSURE].ResType.result.z = t; + RESULTS[SENSOR_PRESSURE].time = time; + dirty[SENSOR_PRESSURE] = DIRTY_IN | DIRTY_OUT; +} + +static void OSPalg_EnableSensor(unsigned int sensor) +{ + int i; + if (sensor_state[sensor] == SEN_ENABLE) + return; + + if (depend[sensor] != FLAG(sensor)) { + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (i == sensor) + continue; + if (depend[sensor] & FLAG(i)) { + if (sensor_state[i] == SEN_ENABLE) + continue; + OSPalg_EnableSensor(i); + } + } + } + sensor_state[sensor] = SEN_ENABLE; + if (sys->SensorsControl) { + SensorControl_t ctrl; + + ctrl.Data = 0; + ctrl.Command = SENSOR_CONTROL_SENSOR_ON; + ctrl.Handle = &InputSensors[sensor]; + sys->SensorsControl(&ctrl); + } +} + +/* Dispatch data in the different esoteric structure */ +static void ResultReadyCB(struct Results *res, int sensor) +{ + union { + Android_TriAxisPreciseData_t calresult; + Android_TriAxisExtendedData_t calmag; + Android_UncalibratedTriAxisPreciseData_t uncalresult; + Android_UncalibratedTriAxisExtendedData_t uncalmag; + Android_BooleanResultData_t sigmot; + Android_OrientationResultData_t orient; + Android_RotationVectorResultData_t rotvec; + } r; + + if (!resHandles[sensor]) + return; + switch(sensor) { + case SENSOR_ACCELEROMETER: + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + r.uncalmag.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.uncalmag.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.uncalmag.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.uncalmag.TimeStamp = res->time; + break; + case SENSOR_MAGNETIC_FIELD: + r.calmag.X = Q15_to_NTEXTENDED(res->ResType.result.x); + r.calmag.Y = Q15_to_NTEXTENDED(res->ResType.result.y); + r.calmag.Z = Q15_to_NTEXTENDED(res->ResType.result.z); + r.calmag.TimeStamp = res->time; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + r.uncalresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.uncalresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.uncalresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.uncalresult.TimeStamp = res->time; + break; + case SENSOR_GYROSCOPE: + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; + break; + case SENSOR_ORIENTATION: + r.orient.Pitch = Q15_to_NTEXTENDED(res->ResType.euler.pitch); + r.orient.Roll = Q15_to_NTEXTENDED(res->ResType.euler.roll); + r.orient.Yaw = Q15_to_NTEXTENDED(res->ResType.euler.yaw); + r.orient.TimeStamp = res->time; + break; + case SENSOR_PRESSURE: + return; + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + r.calresult.X = Q15_to_NTPRECISE(res->ResType.result.x); + r.calresult.Y = Q15_to_NTPRECISE(res->ResType.result.y); + r.calresult.Z = Q15_to_NTPRECISE(res->ResType.result.z); + r.calresult.TimeStamp = res->time; + break; + + case SENSOR_ROTATION_VECTOR: + r.rotvec.X = Q15_to_NTPRECISE(res->ResType.quat.x); + r.rotvec.Y = Q15_to_NTPRECISE(res->ResType.quat.y); + r.rotvec.Z = Q15_to_NTPRECISE(res->ResType.quat.z); + r.rotvec.W = Q15_to_NTPRECISE(res->ResType.quat.w); + r.rotvec.TimeStamp = res->time; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_TILT_DETECTOR: + r.sigmot.data = true; + r.sigmot.TimeStamp = res->time; + break; + default: + return; + } + if (resHandles[sensor]->pResultReadyCallback) { + printf("Calling: %i\n", sensor); + resHandles[sensor]->pResultReadyCallback(resHandles[sensor], + &r); + } +} +#if 0 +static void OnStepResultsReady( StepDataOSP_t* stepData ) +{ + if (resHandles[SENSOR_STEP_COUNTER]) { + Android_StepCounterResultData_t callbackData; + + callbackData.StepCount = stepData->numStepsTotal; + callbackData.TimeStamp = stepData->startTime; //!TODO - Double check if start time or stop time + + resHandles[SENSOR_STEP_COUNTER]->pResultReadyCallback( + resHandles[SENSOR_STEP_COUNTER], + &callbackData); + } +} + +static void OnSignificantMotionResult(NTTIME * eventTime) +{ + if (resHandles[SENSOR_SIGNIFICANT_MOTION]) { + Android_BooleanResultData_t callbackData; + + callbackData.data = true; + callbackData.TimeStamp = *eventTime; + + resHandles[SENSOR_SIGNIFICANT_MOTION]->pResultReadyCallback( + resHandles[SENSOR_SIGNIFICANT_MOTION], + &callbackData); + } +} +#endif +static void OSPalg_EnableSensorCB(unsigned int sensor, + void (*ready)(struct Results *, int)) +{ + readyCB[sensor] = ready; + OSPalg_EnableSensor(sensor); +} + +static void OSPalg_DisableSensor(unsigned int sensor) +{ + int i; + int busy = 0; + + if (sensor_state[sensor] != SEN_ENABLE) + return; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (i == sensor) continue; + + if ((sensor_state[i] == SEN_ENABLE) && + (depend[i] & FLAG(sensor))) { + busy++; + } + } + if (busy == 0) { + sensor_state[sensor] = 0; + if (sys->SensorsControl) { + SensorControl_t ctrl; + + ctrl.Data = 0; + ctrl.Command = SENSOR_CONTROL_SENSOR_OFF; + ctrl.Handle = &InputSensors[sensor]; + sys->SensorsControl(&ctrl); + } + } +} + +void OSPalg_cal(void) +{ + OSP_ecompass_cal(); +} + +OSP_STATUS_t OSP_DoBackgroundProcessing(void) +{ + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_DoForegroundProcessing(void) +{ + int i; + if (sensor_state[SENSOR_GRAVITY] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN) { + OSP_gravity_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result); + dirty[SENSOR_GRAVITY] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_GRAVITY].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_LINEAR_ACCELERATION] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_linear_acc_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_LINEAR_ACCELERATION].ResType.result); + dirty[SENSOR_LINEAR_ACCELERATION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_LINEAR_ACCELERATION].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_SIGNIFICANT_MOTION] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_SIGNIFICANT_MOTION] & DIRTY_IN) { + OSP_sigmot_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_SIGNIFICANT_MOTION].ResType.result); + dirty[SENSOR_SIGNIFICANT_MOTION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_SIGNIFICANT_MOTION].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + + if (sensor_state[SENSOR_ORIENTATION] == SEN_ENABLE) + if (dirty[SENSOR_MAGNETIC_FIELD] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_ecompass_process(&RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_ORIENTATION].ResType.euler); + dirty[SENSOR_ORIENTATION] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_ORIENTATION].time = RESULTS[SENSOR_GRAVITY].time; + } + + if (sensor_state[SENSOR_ROTATION_VECTOR] == SEN_ENABLE) + if (dirty[SENSOR_MAGNETIC_FIELD] & DIRTY_IN || + dirty[SENSOR_GRAVITY] & DIRTY_IN) { + OSP_rotvec_process(&RESULTS[SENSOR_MAGNETIC_FIELD].ResType.result, + &RESULTS[SENSOR_GRAVITY].ResType.result, + &RESULTS[SENSOR_ROTATION_VECTOR].ResType.quat); + dirty[SENSOR_ROTATION_VECTOR] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_ROTATION_VECTOR].time = RESULTS[SENSOR_GRAVITY].time; + } + if (sensor_state[SENSOR_TILT_DETECTOR] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN || + dirty[SENSOR_TILT_DETECTOR] & DIRTY_IN) { + OSP_tilt_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_TILT_DETECTOR].ResType.result); + dirty[SENSOR_TILT_DETECTOR] = DIRTY_IN|DIRTY_OUT; + RESULTS[SENSOR_TILT_DETECTOR].time = RESULTS[SENSOR_ACCELEROMETER].time; + } + +#ifdef FEAT_STEP + if (sensor_state[SENSOR_STEP_COUNTER] == SEN_ENABLE || + sensor_state[SENSOR_STEP_DETECTOR] == SEN_ENABLE) + if (dirty[SENSOR_ACCELEROMETER] & DIRTY_IN) { + OSP_step_process(&RESULTS[SENSOR_ACCELEROMETER].ResType.result, + &RESULTS[SENSOR_STEP_COUNTER].ResType.step); + if (RESULTS[SENSOR_STEP_DETECTOR].ResType.step.detect) { + dirty[SENSOR_STEP_COUNTER] = DIRTY_IN|DIRTY_OUT; + dirty[SENSOR_STEP_DETECTOR] = DIRTY_IN|DIRTY_OUT; + } + RESULTS[SENSOR_STEP_DETECTOR].time = RESULTS[SENSOR_ACCELEROMETER].time; + } +#endif + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (dirty[i]) { + if (readyCB[i] && sensor_state[i] == SEN_ENABLE) { + (readyCB[i])(&RESULTS[i], i); + } + dirty[i] = CLEAN; + } + } + + return OSP_STATUS_IDLE; +} + +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *SenDesc, + InputSensorHandle_t *rHandle) +{ + if (!SenDesc) return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + switch(SenDesc->SensorType) { + case SENSOR_ACCELEROMETER: + case SENSOR_MAGNETIC_FIELD: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_GYROSCOPE: + case SENSOR_GYROSCOPE_UNCALIBRATED: + if (InputSensors[SenDesc->SensorType]) + return OSP_STATUS_SENSOR_ALREADY_REGISTERED; + InputSensors[SenDesc->SensorType] = SenDesc; + *rHandle = &InputSensors[SenDesc->SensorType]; + return OSP_STATUS_OK; + default: + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + } + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; +} + +static void conv2OSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long ts, int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + d->q24data.X = val[0]; + d->q24data.Y = val[1]; + d->q24data.Z = val[2]; + d->q24data.TimeStamp = ts; + break; + + case SENSOR_GEOMAGNETIC_FIELD: + d->q12data.X = val[0]; + d->q12data.Y = val[1]; + d->q12data.Z = val[2]; + d->q12data.TimeStamp = ts; + break; + + case SENSOR_ORIENTATION: + d->orientdata.Yaw = val[0]; + d->orientdata.Pitch = val[1]; + d->orientdata.Roll = val[2]; + d->orientdata.TimeStamp = ts; + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + d->rotvec.X = val[0]; + d->rotvec.Y = val[1]; + d->rotvec.Z = val[2]; + d->rotvec.W = val[3]; + d->rotvec.TimeStamp = ts; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + d->uncal_q24data.X = val[0]; + d->uncal_q24data.Y = val[1]; + d->uncal_q24data.Z = val[2]; + d->uncal_q24data.TimeStamp = ts; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + d->uncal_q12data.X = val[0]; + d->uncal_q12data.Y = val[1]; + d->uncal_q12data.Z = val[2]; + d->uncal_q12data.TimeStamp = ts; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + d->booldata.data = val[0]; + d->booldata.TimeStamp = ts; + break; + case SENSOR_STEP_COUNTER: + d->stepcount.StepCount = val[0]; + d->stepcount.TimeStamp = ts; + break; + default: + break; + } +} +static inline int extractOSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long *ts, int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + val[0] = d->q24data.X; + val[1] = d->q24data.Y; + val[2] = d->q24data.Z; + *ts = d->q24data.TimeStamp; + return 3; + + case SENSOR_GEOMAGNETIC_FIELD: + val[0] = d->q12data.X; + val[1] = d->q12data.Y; + val[2] = d->q12data.Z; + *ts = d->q12data.TimeStamp; + return 3; + + case SENSOR_ORIENTATION: + val[0] = d->orientdata.Yaw; + val[1] = d->orientdata.Pitch; + val[2] = d->orientdata.Roll; + *ts = d->orientdata.TimeStamp; + return 3; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + val[0] = d->rotvec.X; + val[1] = d->rotvec.Y; + val[2] = d->rotvec.Z; + val[3] = d->rotvec.W; + *ts = d->rotvec.TimeStamp; + return 4; + case SENSOR_GYROSCOPE_UNCALIBRATED: + val[0] = d->uncal_q24data.X; + val[1] = d->uncal_q24data.Y; + val[2] = d->uncal_q24data.Z; + *ts = d->uncal_q24data.TimeStamp; + return 3; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + val[0] = d->uncal_q12data.X; + val[1] = d->uncal_q12data.Y; + val[2] = d->uncal_q12data.Z; + *ts = d->uncal_q12data.TimeStamp; + return 3; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + val[0] = d->booldata.data; + *ts = d->booldata.TimeStamp; + return 1; + case SENSOR_STEP_COUNTER: + val[0] = d->stepcount.StepCount; + *ts = d->stepcount.TimeStamp; + return 1; + default: + *ts = 0; + return 0; + } + return 0; +} + +/* BUG: Mag needs to be handled differently. */ +OSP_STATUS_t OSP_SetInputData(InputSensorHandle_t handle, + OSP_InputSensorData_t *data) +{ + SensorDescriptor_t *s, **v; + Q15_t x, y, z; + int32_t rawX, rawY, rawZ; + int32_t xidx = 0, yidx = 1, zidx = 2; + int32_t xs = 1, ys = 1, zs = 1; + int i; + int val[5], vallen; + unsigned long long ts; + + if (!handle) return OSP_STATUS_INVALID_HANDLE; + v = handle; + s = *v; + vallen = extractOSP(s->SensorType, data, &ts, val); + + for (i = 0; i < 3; i++) { + switch (s->AxisMapping[i]) { + case AXIS_MAP_POSITIVE_X: + xidx = i; + xs = 1; + break; + case AXIS_MAP_NEGATIVE_X: + xidx = i; + xs = -1; + break; + case AXIS_MAP_POSITIVE_Y: + yidx = i; + ys = 1; + break; + case AXIS_MAP_NEGATIVE_Y: + yidx = i; + ys = -1; + break; + case AXIS_MAP_POSITIVE_Z: + zidx = i; + zs = 1; + break; + case AXIS_MAP_NEGATIVE_Z: + zidx = i; + zs = -1; + break; + default: + break; + } + } + rawX = xs*val[xidx] - s->ConversionOffset[xidx]; + rawY = ys*val[yidx] - s->ConversionOffset[yidx]; + rawZ = zs*val[zidx] - s->ConversionOffset[zidx]; + x = MUL_Q15(INT_to_Q15(rawX), NTPRECISE_to_Q15(s->ConversionScale[xidx])); + y = MUL_Q15(INT_to_Q15(rawY), NTPRECISE_to_Q15(s->ConversionScale[yidx])); + z = MUL_Q15(INT_to_Q15(rawZ), NTPRECISE_to_Q15(s->ConversionScale[zidx])); + + switch(s->SensorType) { + case SENSOR_ACCELEROMETER: + OSP_SetDataAcc(x, y, z, ts); + break; + case SENSOR_MAGNETIC_FIELD: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + OSP_SetDataMag(x, y, z, ts); + break; + case SENSOR_GYROSCOPE: + case SENSOR_GYROSCOPE_UNCALIBRATED: + OSP_SetDataGyr(x, y, z, ts); + break; + case SENSOR_PRESSURE: + /* Ignore for now */ + break; + default: + break; + } + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_SubscribeSensorResult(ResultDescriptor_t *ResDesc, + ResultHandle_t *ResHandle) +{ + if (!ResDesc) return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + switch (ResDesc->SensorType) { + case SENSOR_ACCELEROMETER: + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_MAGNETIC_FIELD: + case SENSOR_GYROSCOPE_UNCALIBRATED: + case SENSOR_GYROSCOPE: + case SENSOR_ORIENTATION: + case SENSOR_PRESSURE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_ROTATION_VECTOR: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; + OSPalg_EnableSensorCB(ResDesc->SensorType, + ResultReadyCB); + *ResHandle = resHandles[ResDesc->SensorType]; + break; + case SENSOR_STEP_DETECTOR: + case SENSOR_STEP_COUNTER: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + StepDetector_Init(OnStepResultsReady, NULL); +#endif + break; + case SENSOR_SIGNIFICANT_MOTION: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + SignificantMotDetector_Init(OnSignificantMotionResult); +#endif + OSP_sigmot_init(); + break; + case SENSOR_TILT_DETECTOR: + if (resHandles[ResDesc->SensorType] != NULL) + return OSP_STATUS_RESULT_IN_USE; + resHandles[ResDesc->SensorType] = ResDesc; +#if 0 + SignificantMotDetector_Init(OnSignificantMotionResult); +#endif + OSP_tilt_init(); + break; + default: + return OSP_STATUS_SENSOR_INVALID_TYPE; + } + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_UnsubscribeSensorResult(ResultHandle_t ResHandle) +{ + ResultDescriptor_t **rd; + + rd = ResHandle; + + if (!rd) return OSP_STATUS_INVALID_HANDLE; + + if (*rd == NULL) + return OSP_STATUS_NOT_SUBSCRIBED; + if ((*rd)->SensorType == SENSOR_SIGNIFICANT_MOTION) + OSP_sigmot_init(); + + if ((*rd)->SensorType == SENSOR_TILT_DETECTOR) + OSP_tilt_init(); +#if 0 + if ((*rd)->SensorType == SENSOR_SIGNIFICANT_MOTION) + SignificantMotDetector_Init(NULL); + if ((*rd)->SensorType == SENSOR_STEP_COUNTER) + StepDetector_Init(NULL, NULL); +#endif + /* Do magic with disabling callbacks */ + OSPalg_DisableSensor((*rd)->SensorType); + *(rd) = NULL; + + return OSP_STATUS_OK; +} + +OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t *sysdesc) +{ + int i; + + sys = sysdesc; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + InputSensors[i] = NULL; + dirty[i] = CLEAN; + sensor_state[i] = 0; + readyCB[i] = NULL; + resHandles[i] = NULL; + memset(&RESULTS[i], 0, sizeof(struct Results)); + } + OSP_gravity_init(); + OSP_ecompass_init(); + OSP_rotvec_init(); + OSP_linear_acc_init(); + OSP_sigmot_init(); +#ifdef FEAT_STEP + OSP_step_init(); +#endif + OSP_tilt_init(); + //Initialize signal generator +#if 0 + SignalGenerator_Init(); + + //Initialize algs + SignificantMotDetector_Init(NULL); + StepDetector_Init(NULL, NULL); +#endif + + + return OSP_STATUS_OK; +} + + +OSP_STATUS_t OSP_GetLibraryVersion(const OSP_Library_Version_t **v) +{ + *v = &libVersion; + return OSP_STATUS_OK; +} diff --git a/algorithm/osp/osp.h b/algorithm/osp/osp.h new file mode 100644 index 0000000..050da9b --- /dev/null +++ b/algorithm/osp/osp.h @@ -0,0 +1,16 @@ +#ifndef OSP_H +#define OSP_H 1 + +#include "osp-fixedpoint-types.h" +#include "fpsup.h" +#if 0 +void OSP_init(void); +void OSPalg_Process(void); +void OSPalg_SetDataAcc(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_SetDataMag(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_SetDataGyr(Q15_t, Q15_t, Q15_t, NTTIME); +void OSPalg_cal(void); +void OSPalg_EnableSensorCB(unsigned int, void (*ready)(struct Results *, int)); +void OSPalg_DisableSensor(unsigned int); +#endif +#endif \ No newline at end of file diff --git a/algorithm/osp/rotvec.c b/algorithm/osp/rotvec.c new file mode 100644 index 0000000..215116f --- /dev/null +++ b/algorithm/osp/rotvec.c @@ -0,0 +1,154 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Compute rotation vector from accel/mag. + * Based on Android implementation. Converted to fixed point. + */ + +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "rotvec.h" + +#include + +static Q15_t copysign_q15(Q15_t v, Q15_t s) +{ + if (s < 0) return -v; + else return v; +} + + +static Q15_t clamp_q15(Q15_t v) +{ + if (v < 0) return 0; + return v; +} + +void OSP_rotvec_init() +{ +} + +void OSP_rotvec_process( + struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Quat *rot) +{ + Q15_t normH; + Q15_t accMag2; + + Q15_t magMag2; + Q15_t Hx, Hy, Hz; + Q15_t Ex, Ey, Ez; + + Q15_t invA; + Q15_t invH; + + Q15_t Ax, Ay, Az; + Q15_t Mx, My, Mz; + + Q15_t qw, qx, qy, qz; + + Ax = acc->x; + Ay = acc->y; + Az = acc->z; + + if (Ax == 0 && Ay == 0 && Az == 0) return; + + Ex = mag->x; + Ey = mag->y; + Ez = mag->z; + +#if 0 + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez)); +#endif + Hx = MUL_Q15(Ey, Az) - MUL_Q15(Ez, Ay); + Hy = MUL_Q15(Ez, Ax) - MUL_Q15(Ex, Az); + Hz = MUL_Q15(Ex, Ay) - MUL_Q15(Ey, Ax); + + + magMag2 = MUL_Q15(Hx,Hx); + magMag2 += MUL_Q15(Hy, Hy); + magMag2 += MUL_Q15(Hz, Hz); + + normH = sqrt_q15(magMag2); + + if (normH < FP_to_Q15(0.1f)) { + printf("Bad mag? %f (from %f)\n", Q15_to_FP(normH), Q15_to_FP(magMag2)); + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f Hx: %f Hy: %f Hz: %f (%f, %f, %f)\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez), + Q15_to_FP(Hx), Q15_to_FP(Hy), Q15_to_FP(Hz), + Q15_to_FP(MUL_Q15(Hx,Hx)), + Q15_to_FP(MUL_Q15(Hy,Hy)), + Q15_to_FP(MUL_Q15(Hz,Hz)) + ); + return; + } + invH = RECIP_Q15(normH); + if (invH <= 0) { + printf("OF/UF?\n"); + } + + accMag2 = MUL_Q15(Ax, Ax); + accMag2 += MUL_Q15(Ay, Ay); + accMag2 += MUL_Q15(Az, Az); + + if (accMag2 < 0) { + printf("OF? - 2\n"); + } + + invA = RECIP_Q15(sqrt_q15(accMag2)); + + Hx = DIV_Q15(Hx, normH); + Hy = DIV_Q15(Hy, normH); + Hz = DIV_Q15(Hz, normH); + + Ax = MUL_Q15(Ax, invA); + Ay = MUL_Q15(Ay, invA); + Az = MUL_Q15(Az, invA); + + Mx = MUL_Q15(Ay, Hz) - MUL_Q15(Az, Hy); + My = MUL_Q15(Az, Hx) - MUL_Q15(Ax, Hz); + Mz = MUL_Q15(Ax, Hy) - MUL_Q15(Ay, Hx); +#if 0 + qw = sqrt_q12(MUL_Q12(clamp_q12( Hx+My+Az+q12_c1), q12_quarter)); + qx = sqrt_q12(MUL_Q12(clamp_q12( Hx-My-Az+q12_c1), q12_quarter)); + qy = sqrt_q12(MUL_Q12(clamp_q12(-Hx+My-Az+q12_c1), q12_quarter)); + qz = sqrt_q12(MUL_Q12(clamp_q12(-Hx-My+Az+q12_c1), q12_quarter)); +#else + { + Q15_t t; + + t = MUL_Q15(clamp_q15( Hx+My+Az+q15_c1), q15_quarter); + qw = sqrt_q15(t); + + t = MUL_Q15(clamp_q15( Hx-My-Az+q15_c1), q15_quarter); + qx = sqrt_q15(t); + + t = MUL_Q15(clamp_q15(-Hx+My-Az+q15_c1), q15_quarter); + qy = sqrt_q15(t); + + t = MUL_Q15(clamp_q15(-Hx-My+Az+q15_c1), q15_quarter); + qz = sqrt_q15(t); + } +#endif + + qx = copysign_q15(qx, Ay - Mz); + qy = copysign_q15(qy, Hz - Ax); + qz = copysign_q15(qz, Mx - Hy); + + rot->x = qx; + rot->y = qy; + rot->z = qz; + rot->w = qw; +} diff --git a/algorithm/osp/rotvec.h b/algorithm/osp/rotvec.h new file mode 100644 index 0000000..ff114de --- /dev/null +++ b/algorithm/osp/rotvec.h @@ -0,0 +1,13 @@ +#ifndef _ROTVEC_H_ +#define _ROTVEC_H_ + +#include "fp_sensor.h" + +void OSP_rotvec_init(void); +#ifdef FEAT_ROTVEC_PRECISE +void OSP_rotvec_process(struct ThreeAxis *, struct ThreeAxis *, struct Quat_precise *); +#else +void OSP_rotvec_process(struct ThreeAxis *, struct ThreeAxis *, struct Quat *); +#endif + +#endif diff --git a/algorithm/osp/rotvec_precise.c b/algorithm/osp/rotvec_precise.c new file mode 100644 index 0000000..a6b8445 --- /dev/null +++ b/algorithm/osp/rotvec_precise.c @@ -0,0 +1,110 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + + +/* + * Precise version of rotation vector using Q24's. + * Based on Android implementation. Converted to fixed point. + */ +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "rotvec.h" + +static Q24_t copysign_q24(Q24_t v, Q24_t s) +{ + if (s < 0) return -v; + else return v; +} + + +static Q24_t clamp_q24(Q24_t v) +{ + if (v < 0) return 0; + return v; +} + +void OSP_rotvec_init() +{ +} + +void OSP_rotvec_process( + struct ThreeAxis *mag, + struct ThreeAxis *acc, + struct Quat_precise *rot) +{ + Q24_t normH; + Q24_t magMag2, accMag2; + Q24_t invH, invA; + Q24_t Hx, Hy, Hz; + Q24_t Ax, Ay, Az; + Q24_t Mx, My, Mz; + Q24_t Ex, Ey, Ez; + + Q15_t qw, qx, qy, qz; + + Ax = q15_to_q24(acc->x); + Ay = q15_to_q24(acc->y); + Az = q15_to_q24(acc->z); + + Ex = q15_to_q24(mag->x); + Ey = q15_to_q24(mag->y); + Ez = q15_to_q24(mag->z); +#if 0 + printf("RV using: Ax: %f Ay: %f: Az: %f, Mx: %f, My: %f, Mz: %f\n", + Q15_to_FP(Ax), Q15_to_FP(Ay), Q15_to_FP(Az), + Q15_to_FP(Ex), Q15_to_FP(Ey), Q15_to_FP(Ez)); +#endif + Hx = MUL_Q24(Ey, Az) - MUL_Q24(Ez, Ay); + Hy = MUL_Q24(Ez, Ax) - MUL_Q24(Ex, Az); + Hz = MUL_Q24(Ex, Ay) - MUL_Q24(Ey, Ax); + + magMag2 = MUL_Q24(Hx,Hx); + magMag2 += MUL_Q24(Hy, Hy); + magMag2 += MUL_Q24(Hz, Hz); + + normH = sqrt_q24(magMag2); + + if (normH < FP_to_Q24(0.1f)) { + printf("Bad mag? %f\n", Q24_to_FP(normH)); + return; + } + invH = RECIP_Q24(normH); + + accMag2 = MUL_Q24(Ax, Ax); + accMag2 += MUL_Q24(Ay, Ay); + accMag2 += MUL_Q24(Az, Az); + + invA = RECIP_Q24(sqrt_q24(accMag2)); + + Hx = MUL_Q24(Hx, invH); + Hy = MUL_Q24(Hy, invH); + Hz = MUL_Q24(Hz, invH); + + Ax = MUL_Q24(Ax, invA); + Ay = MUL_Q24(Ay, invA); + Az = MUL_Q24(Az, invA); + + Mx = MUL_Q24(Ay, Hz) - MUL_Q15(Az, Hy); + My = MUL_Q24(Az, Hx) - MUL_Q15(Ax, Hz); + Mz = MUL_Q24(Ax, Hy) - MUL_Q15(Ay, Hx); + + qw = sqrt_q24(MUL_Q24(clamp_q24( Hx+My+Az+q24_c1), q24_quarter)); + qx = sqrt_q24(MUL_Q24(clamp_q24( Hx-My-Az+q24_c1), q24_quarter)); + qy = sqrt_q24(MUL_Q24(clamp_q24(-Hx+My-Az+q24_c1), q24_quarter)); + qz = sqrt_q24(MUL_Q24(clamp_q24(-Hx-My+Az+q24_c1), q24_quarter)); + + qx = copysign_q24(qx, Ay - Mz); + qy = copysign_q24(qy, Hz - Ax); + qz = copysign_q24(qz, Mx - Hy); + + rot->x = qx; + rot->y = qy; + rot->z = qz; + rot->w = qw; +} diff --git a/algorithm/osp/sigmot.c b/algorithm/osp/sigmot.c new file mode 100644 index 0000000..8103618 --- /dev/null +++ b/algorithm/osp/sigmot.c @@ -0,0 +1,28 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Initial attempt at the significant motion sensor. + */ + + +#include "sigmot.h" + +enum { + SIGMOT_RESET, + SIGMOT_WAIT, +}; + +int state; + +void OSP_sigmot_init(void) +{ + state = SIGMOT_WAIT; +} + +void OSP_sigmot_process(struct ThreeAxis *acc, struct ThreeAxis *res) +{ + res->x = 0; +} diff --git a/algorithm/osp/sigmot.h b/algorithm/osp/sigmot.h new file mode 100644 index 0000000..b372e57 --- /dev/null +++ b/algorithm/osp/sigmot.h @@ -0,0 +1,8 @@ +#ifndef _SIGMOT_H_ +#define _SIGMOT_H_ +#include "fp_sensor.h" + +void OSP_sigmot_init(void); +void OSP_sigmot_process(struct ThreeAxis *, struct ThreeAxis *); + +#endif diff --git a/algorithm/osp/step.c b/algorithm/osp/step.c new file mode 100644 index 0000000..a0d7a01 --- /dev/null +++ b/algorithm/osp/step.c @@ -0,0 +1,99 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Adopted from alg from PebbleFit, Apache Licensed . + */ + +#include +#include "fpsup.h" +#include "fp_sensor.h" +#include "step.h" + +static const Q15_t q15_0_20c = FP_to_Q15(0.20f); +static Q15_t sens_Const = FP_to_Q15(200.0f); +static Q15_t threshold = 0; + +void OSP_step_init(void) +{ +} + +/* + * Det is number of steps detected. + * Count is count accumated count. On return: + * *count += det; + * Assumes a call back at a period of 0.020sec + */ +void OSP_step_process(struct ThreeAxis *acc, struct StepInfo *step) +{ + static int sampcnt = 0; /* Used for avaeraging */ + static Q15_t x = 0, y = 0, z = 0; + static Q15_t dAcc, sampleOld = 0, sampleNew = 0; + Q15_t min = FP_to_Q15(10000.0f), max = FP_to_Q15(-10000.0f); + static int lastStepSamp = 0; + static int sampcnt2 = 0; + static int regulation = 0; + + + /* Process groups of 5 samples */ + x += acc->x; y += acc->y; z+= acc->z; + sampcnt++; + sampcnt2++; + + step->detect = 0; + + if (sampcnt > 5) { + sampcnt = 0; + x = MUL_Q15(x, q15_0_20c); + y = MUL_Q15(y, q15_0_20c); + z = MUL_Q15(z, q15_0_20c); + dAcc = MUL_Q15(x,x) + MUL_Q15(y,y) + MUL_Q15(z,z); + dAcc = sqrt_q15(dAcc); + sampleOld = sampleNew; + if (abs_q15(dAcc - sampleNew) > sens_Const) + sampleNew = dAcc; + if ((sampleNew < threshold && + sampleOld > threshold) && + sampleOld != 0) { + /* Cadence sanity check: + * 0.2sec - 2sec between steps. + */ + if (lastStepSamp && + (sampcnt2 - lastStepSamp) > 9 && + (sampcnt2 - lastStepSamp) < 100) { + /* Require 5 consecutive steps for validity */ + regulation++; + + if (regulation == 5) { + step->detect = 1; + step->count += 5; + } else if (regulation > 5) { + step->detect = 1; + step->count += 1; + } + } else { + regulation = 0; + } + lastStepSamp = sampcnt2; + } + if (dAcc < min) min = dAcc; + if (dAcc > max) max = dAcc; + + threshold = (max+min) >> 1; + } + + /* Avoid wrap */ + if (sampcnt2 > (1 << 30)) { + if (lastStepSamp == 0) { + sampcnt2 -= (1 << 30); + } else if (lastStepSamp > 0 && lastStepSamp > (1<<30)) { + sampcnt2 -= (1 << 30); + lastStepSamp -= (1 << 30); + } else if ((sampcnt2 - lastStepSamp) > 1000) { + sampcnt2 -= (1 << 30); + lastStepSamp = 0; + } + } +} diff --git a/algorithm/osp/step.h b/algorithm/osp/step.h new file mode 100644 index 0000000..13d0558 --- /dev/null +++ b/algorithm/osp/step.h @@ -0,0 +1,6 @@ +#ifndef _STEP_H_ +#define _STEP_H_ + +void OSP_step_process(struct ThreeAxis *, struct StepInfo *); +void OSP_step_init(void); +#endif diff --git a/algorithm/osp/tilt.c b/algorithm/osp/tilt.c new file mode 100644 index 0000000..53a9335 --- /dev/null +++ b/algorithm/osp/tilt.c @@ -0,0 +1,112 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + */ + +/* Compute a tilt sensor as defined by Android */ +#include "fpsup.h" +#include "tilt.h" + +#define NUM_SAMPLE2 50 +#define NUM_SAMPLE1 25 + +static struct ThreeAxis accHist[NUM_SAMPLE2]; +static struct ThreeAxis prevMean; +int sampcount; +int samp_head, samp_tail; + +#define ABS(x) ((x > 0)?x:-x) + +void OSP_tilt_init(void) +{ + sampcount = 0; + samp_head = 49, samp_tail = 0; +} + +static void computeMean(struct ThreeAxis *mean, struct ThreeAxis *buf, int beg, int end) +{ + int i, count; + + count = 0; + mean->x = 0; mean->y = 0; mean->z = 0; + for (i = beg; i != end; i++, i %= NUM_SAMPLE2) { + mean->x += buf[i].x; + mean->y += buf[i].y; + mean->z += buf[i].z; + count++; + } + mean->x = DIV_Q15(mean->x, INT_to_Q15(count)); + mean->y = DIV_Q15(mean->y, INT_to_Q15(count)); + mean->z = DIV_Q15(mean->z, INT_to_Q15(count)); +} + +static Q15_t dotProduct(struct ThreeAxis *v1, struct ThreeAxis *v2) +{ + Q15_t res; + + res = MUL_Q15(v1->x, v2->x); + res += MUL_Q15(v1->y, v2->y); + res += MUL_Q15(v1->z, v2->z); + + return res; +} + +static Q15_t norm(struct ThreeAxis *v1) +{ + Q15_t sum; + + sum = MUL_Q15(v1->x, v1->x); + sum += MUL_Q15(v1->y, v1->y); + sum += MUL_Q15(v1->z, v1->z); + return sqrt_q15(sum); +} + +static Q15_t computeAngle(struct ThreeAxis *v1, struct ThreeAxis *v2) +{ + Q15_t v, n; + + v = dotProduct(v1, v2); + n = MUL_Q15(norm(v1), norm(v2)); + v = DIV_Q15(v, n); + + return arccos_q15(v); +} + +/* Assumes the sensor runs at 50Hz */ +void OSP_tilt_process(struct ThreeAxis *acc, struct ThreeAxis *tilt) +{ + struct ThreeAxis cur; + int ang = 0; + + if (sampcount > 10000) { + tilt->x = 0; + return; + } + + + accHist[samp_head] = *acc; + + if (sampcount < (NUM_SAMPLE2-1)) { + if (sampcount == (NUM_SAMPLE1-1)) { + computeMean(&prevMean, accHist, 0, NUM_SAMPLE1); + } + sampcount++; + samp_head++; + samp_head %= NUM_SAMPLE2; + tilt->x = 0; + return; + } + + computeMean(&cur, accHist, samp_tail, samp_head); + samp_head++; samp_head %= NUM_SAMPLE2; + samp_tail++; samp_tail %= NUM_SAMPLE2; + ang = computeAngle(&cur, &prevMean); + if (ABS(ang) > INT_to_Q15(35)) { + tilt->x = 1; + sampcount = 99999; + } + return; +} diff --git a/algorithm/osp/tilt.h b/algorithm/osp/tilt.h new file mode 100644 index 0000000..36496a6 --- /dev/null +++ b/algorithm/osp/tilt.h @@ -0,0 +1,9 @@ +#ifndef _TILT_H_ +#define _TILT_H_ + +#include "fp_sensor.h" +void OSP_tilt_init(void); + +void OSP_tilt_process(struct ThreeAxis *acc, struct ThreeAxis *tilt); + +#endif diff --git a/algorithm/osp/trig_sin.c b/algorithm/osp/trig_sin.c new file mode 100644 index 0000000..b81a597 --- /dev/null +++ b/algorithm/osp/trig_sin.c @@ -0,0 +1,331 @@ +/* + * (C) Copyright 2015 HY Research LLC + * Author: hy-git@hy-research.com + * + * Apache License. + * + * Trig look up table. + */ + + + +static struct TRIG_SIN { + Q15_t sin; + Q15_t ang; +} trig_sin[] = { +{FP_to_Q15(0.00000), FP_to_Q15(0.00000)}, +{FP_to_Q15(0.00500), FP_to_Q15(0.00500)}, +{FP_to_Q15(0.01000), FP_to_Q15(0.01000)}, +{FP_to_Q15(0.01500), FP_to_Q15(0.01500)}, +{FP_to_Q15(0.02000), FP_to_Q15(0.02000)}, +{FP_to_Q15(0.02500), FP_to_Q15(0.02500)}, +{FP_to_Q15(0.03000), FP_to_Q15(0.03000)}, +{FP_to_Q15(0.03499), FP_to_Q15(0.03500)}, +{FP_to_Q15(0.03999), FP_to_Q15(0.04000)}, +{FP_to_Q15(0.04498), FP_to_Q15(0.04500)}, +{FP_to_Q15(0.04998), FP_to_Q15(0.05000)}, +{FP_to_Q15(0.05497), FP_to_Q15(0.05500)}, +{FP_to_Q15(0.05996), FP_to_Q15(0.06000)}, +{FP_to_Q15(0.06495), FP_to_Q15(0.06500)}, +{FP_to_Q15(0.06994), FP_to_Q15(0.07000)}, +{FP_to_Q15(0.07493), FP_to_Q15(0.07500)}, +{FP_to_Q15(0.07991), FP_to_Q15(0.08000)}, +{FP_to_Q15(0.08490), FP_to_Q15(0.08500)}, +{FP_to_Q15(0.08988), FP_to_Q15(0.09000)}, +{FP_to_Q15(0.09486), FP_to_Q15(0.09500)}, +{FP_to_Q15(0.09983), FP_to_Q15(0.10000)}, +{FP_to_Q15(0.10481), FP_to_Q15(0.10500)}, +{FP_to_Q15(0.10978), FP_to_Q15(0.11000)}, +{FP_to_Q15(0.11475), FP_to_Q15(0.11500)}, +{FP_to_Q15(0.11971), FP_to_Q15(0.12000)}, +{FP_to_Q15(0.12467), FP_to_Q15(0.12500)}, +{FP_to_Q15(0.12963), FP_to_Q15(0.13000)}, +{FP_to_Q15(0.13459), FP_to_Q15(0.13500)}, +{FP_to_Q15(0.13954), FP_to_Q15(0.14000)}, +{FP_to_Q15(0.14449), FP_to_Q15(0.14500)}, +{FP_to_Q15(0.14944), FP_to_Q15(0.15000)}, +{FP_to_Q15(0.15438), FP_to_Q15(0.15500)}, +{FP_to_Q15(0.15932), FP_to_Q15(0.16000)}, +{FP_to_Q15(0.16425), FP_to_Q15(0.16500)}, +{FP_to_Q15(0.16918), FP_to_Q15(0.17000)}, +{FP_to_Q15(0.17411), FP_to_Q15(0.17500)}, +{FP_to_Q15(0.17903), FP_to_Q15(0.18000)}, +{FP_to_Q15(0.18395), FP_to_Q15(0.18500)}, +{FP_to_Q15(0.18886), FP_to_Q15(0.19000)}, +{FP_to_Q15(0.19377), FP_to_Q15(0.19500)}, +{FP_to_Q15(0.19867), FP_to_Q15(0.20000)}, +{FP_to_Q15(0.20357), FP_to_Q15(0.20500)}, +{FP_to_Q15(0.20846), FP_to_Q15(0.21000)}, +{FP_to_Q15(0.21335), FP_to_Q15(0.21500)}, +{FP_to_Q15(0.21823), FP_to_Q15(0.22000)}, +{FP_to_Q15(0.22311), FP_to_Q15(0.22500)}, +{FP_to_Q15(0.22798), FP_to_Q15(0.23000)}, +{FP_to_Q15(0.23284), FP_to_Q15(0.23500)}, +{FP_to_Q15(0.23770), FP_to_Q15(0.24000)}, +{FP_to_Q15(0.24256), FP_to_Q15(0.24500)}, +{FP_to_Q15(0.24740), FP_to_Q15(0.25000)}, +{FP_to_Q15(0.25225), FP_to_Q15(0.25500)}, +{FP_to_Q15(0.25708), FP_to_Q15(0.26000)}, +{FP_to_Q15(0.26191), FP_to_Q15(0.26500)}, +{FP_to_Q15(0.26673), FP_to_Q15(0.27000)}, +{FP_to_Q15(0.27155), FP_to_Q15(0.27500)}, +{FP_to_Q15(0.27636), FP_to_Q15(0.28000)}, +{FP_to_Q15(0.28116), FP_to_Q15(0.28500)}, +{FP_to_Q15(0.28595), FP_to_Q15(0.29000)}, +{FP_to_Q15(0.29074), FP_to_Q15(0.29500)}, +{FP_to_Q15(0.29552), FP_to_Q15(0.30000)}, +{FP_to_Q15(0.30029), FP_to_Q15(0.30500)}, +{FP_to_Q15(0.30506), FP_to_Q15(0.31000)}, +{FP_to_Q15(0.30982), FP_to_Q15(0.31500)}, +{FP_to_Q15(0.31457), FP_to_Q15(0.32000)}, +{FP_to_Q15(0.31931), FP_to_Q15(0.32500)}, +{FP_to_Q15(0.32404), FP_to_Q15(0.33000)}, +{FP_to_Q15(0.32877), FP_to_Q15(0.33500)}, +{FP_to_Q15(0.33349), FP_to_Q15(0.34000)}, +{FP_to_Q15(0.33820), FP_to_Q15(0.34500)}, +{FP_to_Q15(0.34290), FP_to_Q15(0.35000)}, +{FP_to_Q15(0.34759), FP_to_Q15(0.35500)}, +{FP_to_Q15(0.35227), FP_to_Q15(0.36000)}, +{FP_to_Q15(0.35695), FP_to_Q15(0.36500)}, +{FP_to_Q15(0.36162), FP_to_Q15(0.37000)}, +{FP_to_Q15(0.36627), FP_to_Q15(0.37500)}, +{FP_to_Q15(0.37092), FP_to_Q15(0.38000)}, +{FP_to_Q15(0.37556), FP_to_Q15(0.38500)}, +{FP_to_Q15(0.38019), FP_to_Q15(0.39000)}, +{FP_to_Q15(0.38481), FP_to_Q15(0.39500)}, +{FP_to_Q15(0.38942), FP_to_Q15(0.40000)}, +{FP_to_Q15(0.39402), FP_to_Q15(0.40500)}, +{FP_to_Q15(0.39861), FP_to_Q15(0.41000)}, +{FP_to_Q15(0.40319), FP_to_Q15(0.41500)}, +{FP_to_Q15(0.40776), FP_to_Q15(0.42000)}, +{FP_to_Q15(0.41232), FP_to_Q15(0.42500)}, +{FP_to_Q15(0.41687), FP_to_Q15(0.43000)}, +{FP_to_Q15(0.42141), FP_to_Q15(0.43500)}, +{FP_to_Q15(0.42594), FP_to_Q15(0.44000)}, +{FP_to_Q15(0.43046), FP_to_Q15(0.44500)}, +{FP_to_Q15(0.43497), FP_to_Q15(0.45000)}, +{FP_to_Q15(0.43946), FP_to_Q15(0.45500)}, +{FP_to_Q15(0.44395), FP_to_Q15(0.46000)}, +{FP_to_Q15(0.44842), FP_to_Q15(0.46500)}, +{FP_to_Q15(0.45289), FP_to_Q15(0.47000)}, +{FP_to_Q15(0.45734), FP_to_Q15(0.47500)}, +{FP_to_Q15(0.46178), FP_to_Q15(0.48000)}, +{FP_to_Q15(0.46621), FP_to_Q15(0.48500)}, +{FP_to_Q15(0.47063), FP_to_Q15(0.49000)}, +{FP_to_Q15(0.47503), FP_to_Q15(0.49500)}, +{FP_to_Q15(0.47943), FP_to_Q15(0.50000)}, +{FP_to_Q15(0.48381), FP_to_Q15(0.50500)}, +{FP_to_Q15(0.48818), FP_to_Q15(0.51000)}, +{FP_to_Q15(0.49253), FP_to_Q15(0.51500)}, +{FP_to_Q15(0.49688), FP_to_Q15(0.52000)}, +{FP_to_Q15(0.50121), FP_to_Q15(0.52500)}, +{FP_to_Q15(0.50553), FP_to_Q15(0.53000)}, +{FP_to_Q15(0.50984), FP_to_Q15(0.53500)}, +{FP_to_Q15(0.51414), FP_to_Q15(0.54000)}, +{FP_to_Q15(0.51842), FP_to_Q15(0.54500)}, +{FP_to_Q15(0.52269), FP_to_Q15(0.55000)}, +{FP_to_Q15(0.52694), FP_to_Q15(0.55500)}, +{FP_to_Q15(0.53119), FP_to_Q15(0.56000)}, +{FP_to_Q15(0.53542), FP_to_Q15(0.56500)}, +{FP_to_Q15(0.53963), FP_to_Q15(0.57000)}, +{FP_to_Q15(0.54383), FP_to_Q15(0.57500)}, +{FP_to_Q15(0.54802), FP_to_Q15(0.58000)}, +{FP_to_Q15(0.55220), FP_to_Q15(0.58500)}, +{FP_to_Q15(0.55636), FP_to_Q15(0.59000)}, +{FP_to_Q15(0.56051), FP_to_Q15(0.59500)}, +{FP_to_Q15(0.56464), FP_to_Q15(0.60000)}, +{FP_to_Q15(0.56876), FP_to_Q15(0.60500)}, +{FP_to_Q15(0.57287), FP_to_Q15(0.61000)}, +{FP_to_Q15(0.57696), FP_to_Q15(0.61500)}, +{FP_to_Q15(0.58104), FP_to_Q15(0.62000)}, +{FP_to_Q15(0.58510), FP_to_Q15(0.62500)}, +{FP_to_Q15(0.58914), FP_to_Q15(0.63000)}, +{FP_to_Q15(0.59318), FP_to_Q15(0.63500)}, +{FP_to_Q15(0.59720), FP_to_Q15(0.64000)}, +{FP_to_Q15(0.60120), FP_to_Q15(0.64500)}, +{FP_to_Q15(0.60519), FP_to_Q15(0.65000)}, +{FP_to_Q15(0.60916), FP_to_Q15(0.65500)}, +{FP_to_Q15(0.61312), FP_to_Q15(0.66000)}, +{FP_to_Q15(0.61706), FP_to_Q15(0.66500)}, +{FP_to_Q15(0.62099), FP_to_Q15(0.67000)}, +{FP_to_Q15(0.62490), FP_to_Q15(0.67500)}, +{FP_to_Q15(0.62879), FP_to_Q15(0.68000)}, +{FP_to_Q15(0.63267), FP_to_Q15(0.68500)}, +{FP_to_Q15(0.63654), FP_to_Q15(0.69000)}, +{FP_to_Q15(0.64039), FP_to_Q15(0.69500)}, +{FP_to_Q15(0.64422), FP_to_Q15(0.70000)}, +{FP_to_Q15(0.64803), FP_to_Q15(0.70500)}, +{FP_to_Q15(0.65183), FP_to_Q15(0.71000)}, +{FP_to_Q15(0.65562), FP_to_Q15(0.71500)}, +{FP_to_Q15(0.65938), FP_to_Q15(0.72000)}, +{FP_to_Q15(0.66314), FP_to_Q15(0.72500)}, +{FP_to_Q15(0.66687), FP_to_Q15(0.73000)}, +{FP_to_Q15(0.67059), FP_to_Q15(0.73500)}, +{FP_to_Q15(0.67429), FP_to_Q15(0.74000)}, +{FP_to_Q15(0.67797), FP_to_Q15(0.74500)}, +{FP_to_Q15(0.68164), FP_to_Q15(0.75000)}, +{FP_to_Q15(0.68529), FP_to_Q15(0.75500)}, +{FP_to_Q15(0.68892), FP_to_Q15(0.76000)}, +{FP_to_Q15(0.69254), FP_to_Q15(0.76500)}, +{FP_to_Q15(0.69614), FP_to_Q15(0.77000)}, +{FP_to_Q15(0.69972), FP_to_Q15(0.77500)}, +{FP_to_Q15(0.70328), FP_to_Q15(0.78000)}, +{FP_to_Q15(0.70683), FP_to_Q15(0.78500)}, +{FP_to_Q15(0.71035), FP_to_Q15(0.79000)}, +{FP_to_Q15(0.71386), FP_to_Q15(0.79500)}, +{FP_to_Q15(0.71736), FP_to_Q15(0.80000)}, +{FP_to_Q15(0.72083), FP_to_Q15(0.80500)}, +{FP_to_Q15(0.72429), FP_to_Q15(0.81000)}, +{FP_to_Q15(0.72773), FP_to_Q15(0.81500)}, +{FP_to_Q15(0.73115), FP_to_Q15(0.82000)}, +{FP_to_Q15(0.73455), FP_to_Q15(0.82500)}, +{FP_to_Q15(0.73793), FP_to_Q15(0.83000)}, +{FP_to_Q15(0.74130), FP_to_Q15(0.83500)}, +{FP_to_Q15(0.74464), FP_to_Q15(0.84000)}, +{FP_to_Q15(0.74797), FP_to_Q15(0.84500)}, +{FP_to_Q15(0.75128), FP_to_Q15(0.85000)}, +{FP_to_Q15(0.75457), FP_to_Q15(0.85500)}, +{FP_to_Q15(0.75784), FP_to_Q15(0.86000)}, +{FP_to_Q15(0.76110), FP_to_Q15(0.86500)}, +{FP_to_Q15(0.76433), FP_to_Q15(0.87000)}, +{FP_to_Q15(0.76754), FP_to_Q15(0.87500)}, +{FP_to_Q15(0.77074), FP_to_Q15(0.88000)}, +{FP_to_Q15(0.77391), FP_to_Q15(0.88500)}, +{FP_to_Q15(0.77707), FP_to_Q15(0.89000)}, +{FP_to_Q15(0.78021), FP_to_Q15(0.89500)}, +{FP_to_Q15(0.78333), FP_to_Q15(0.90000)}, +{FP_to_Q15(0.78643), FP_to_Q15(0.90500)}, +{FP_to_Q15(0.78950), FP_to_Q15(0.91000)}, +{FP_to_Q15(0.79256), FP_to_Q15(0.91500)}, +{FP_to_Q15(0.79560), FP_to_Q15(0.92000)}, +{FP_to_Q15(0.79862), FP_to_Q15(0.92500)}, +{FP_to_Q15(0.80162), FP_to_Q15(0.93000)}, +{FP_to_Q15(0.80460), FP_to_Q15(0.93500)}, +{FP_to_Q15(0.80756), FP_to_Q15(0.94000)}, +{FP_to_Q15(0.81050), FP_to_Q15(0.94500)}, +{FP_to_Q15(0.81342), FP_to_Q15(0.95000)}, +{FP_to_Q15(0.81631), FP_to_Q15(0.95500)}, +{FP_to_Q15(0.81919), FP_to_Q15(0.96000)}, +{FP_to_Q15(0.82205), FP_to_Q15(0.96500)}, +{FP_to_Q15(0.82489), FP_to_Q15(0.97000)}, +{FP_to_Q15(0.82770), FP_to_Q15(0.97500)}, +{FP_to_Q15(0.83050), FP_to_Q15(0.98000)}, +{FP_to_Q15(0.83327), FP_to_Q15(0.98500)}, +{FP_to_Q15(0.83603), FP_to_Q15(0.99000)}, +{FP_to_Q15(0.83876), FP_to_Q15(0.99500)}, +{FP_to_Q15(0.84147), FP_to_Q15(1.00000)}, +{FP_to_Q15(0.84416), FP_to_Q15(1.00500)}, +{FP_to_Q15(0.84683), FP_to_Q15(1.01000)}, +{FP_to_Q15(0.84948), FP_to_Q15(1.01500)}, +{FP_to_Q15(0.85211), FP_to_Q15(1.02000)}, +{FP_to_Q15(0.85471), FP_to_Q15(1.02500)}, +{FP_to_Q15(0.85730), FP_to_Q15(1.03000)}, +{FP_to_Q15(0.85986), FP_to_Q15(1.03500)}, +{FP_to_Q15(0.86240), FP_to_Q15(1.04000)}, +{FP_to_Q15(0.86492), FP_to_Q15(1.04500)}, +{FP_to_Q15(0.86742), FP_to_Q15(1.05000)}, +{FP_to_Q15(0.86990), FP_to_Q15(1.05500)}, +{FP_to_Q15(0.87236), FP_to_Q15(1.06000)}, +{FP_to_Q15(0.87479), FP_to_Q15(1.06500)}, +{FP_to_Q15(0.87720), FP_to_Q15(1.07000)}, +{FP_to_Q15(0.87959), FP_to_Q15(1.07500)}, +{FP_to_Q15(0.88196), FP_to_Q15(1.08000)}, +{FP_to_Q15(0.88430), FP_to_Q15(1.08500)}, +{FP_to_Q15(0.88663), FP_to_Q15(1.09000)}, +{FP_to_Q15(0.88893), FP_to_Q15(1.09500)}, +{FP_to_Q15(0.89121), FP_to_Q15(1.10000)}, +{FP_to_Q15(0.89346), FP_to_Q15(1.10500)}, +{FP_to_Q15(0.89570), FP_to_Q15(1.11000)}, +{FP_to_Q15(0.89791), FP_to_Q15(1.11500)}, +{FP_to_Q15(0.90010), FP_to_Q15(1.12000)}, +{FP_to_Q15(0.90227), FP_to_Q15(1.12500)}, +{FP_to_Q15(0.90441), FP_to_Q15(1.13000)}, +{FP_to_Q15(0.90653), FP_to_Q15(1.13500)}, +{FP_to_Q15(0.90863), FP_to_Q15(1.14000)}, +{FP_to_Q15(0.91071), FP_to_Q15(1.14500)}, +{FP_to_Q15(0.91276), FP_to_Q15(1.15000)}, +{FP_to_Q15(0.91479), FP_to_Q15(1.15500)}, +{FP_to_Q15(0.91680), FP_to_Q15(1.16000)}, +{FP_to_Q15(0.91879), FP_to_Q15(1.16500)}, +{FP_to_Q15(0.92075), FP_to_Q15(1.17000)}, +{FP_to_Q15(0.92269), FP_to_Q15(1.17500)}, +{FP_to_Q15(0.92461), FP_to_Q15(1.18000)}, +{FP_to_Q15(0.92650), FP_to_Q15(1.18500)}, +{FP_to_Q15(0.92837), FP_to_Q15(1.19000)}, +{FP_to_Q15(0.93022), FP_to_Q15(1.19500)}, +{FP_to_Q15(0.93204), FP_to_Q15(1.20000)}, +{FP_to_Q15(0.93384), FP_to_Q15(1.20500)}, +{FP_to_Q15(0.93562), FP_to_Q15(1.21000)}, +{FP_to_Q15(0.93737), FP_to_Q15(1.21500)}, +{FP_to_Q15(0.93910), FP_to_Q15(1.22000)}, +{FP_to_Q15(0.94081), FP_to_Q15(1.22500)}, +{FP_to_Q15(0.94249), FP_to_Q15(1.23000)}, +{FP_to_Q15(0.94415), FP_to_Q15(1.23500)}, +{FP_to_Q15(0.94578), FP_to_Q15(1.24000)}, +{FP_to_Q15(0.94740), FP_to_Q15(1.24500)}, +{FP_to_Q15(0.94898), FP_to_Q15(1.25000)}, +{FP_to_Q15(0.95055), FP_to_Q15(1.25500)}, +{FP_to_Q15(0.95209), FP_to_Q15(1.26000)}, +{FP_to_Q15(0.95361), FP_to_Q15(1.26500)}, +{FP_to_Q15(0.95510), FP_to_Q15(1.27000)}, +{FP_to_Q15(0.95657), FP_to_Q15(1.27500)}, +{FP_to_Q15(0.95802), FP_to_Q15(1.28000)}, +{FP_to_Q15(0.95944), FP_to_Q15(1.28500)}, +{FP_to_Q15(0.96084), FP_to_Q15(1.29000)}, +{FP_to_Q15(0.96221), FP_to_Q15(1.29500)}, +{FP_to_Q15(0.96356), FP_to_Q15(1.30000)}, +{FP_to_Q15(0.96488), FP_to_Q15(1.30500)}, +{FP_to_Q15(0.96618), FP_to_Q15(1.31000)}, +{FP_to_Q15(0.96746), FP_to_Q15(1.31500)}, +{FP_to_Q15(0.96872), FP_to_Q15(1.32000)}, +{FP_to_Q15(0.96994), FP_to_Q15(1.32500)}, +{FP_to_Q15(0.97115), FP_to_Q15(1.33000)}, +{FP_to_Q15(0.97233), FP_to_Q15(1.33500)}, +{FP_to_Q15(0.97348), FP_to_Q15(1.34000)}, +{FP_to_Q15(0.97462), FP_to_Q15(1.34500)}, +{FP_to_Q15(0.97572), FP_to_Q15(1.35000)}, +{FP_to_Q15(0.97681), FP_to_Q15(1.35500)}, +{FP_to_Q15(0.97786), FP_to_Q15(1.36000)}, +{FP_to_Q15(0.97890), FP_to_Q15(1.36500)}, +{FP_to_Q15(0.97991), FP_to_Q15(1.37000)}, +{FP_to_Q15(0.98089), FP_to_Q15(1.37500)}, +{FP_to_Q15(0.98185), FP_to_Q15(1.38000)}, +{FP_to_Q15(0.98279), FP_to_Q15(1.38500)}, +{FP_to_Q15(0.98370), FP_to_Q15(1.39000)}, +{FP_to_Q15(0.98459), FP_to_Q15(1.39500)}, +{FP_to_Q15(0.98545), FP_to_Q15(1.40000)}, +{FP_to_Q15(0.98629), FP_to_Q15(1.40500)}, +{FP_to_Q15(0.98710), FP_to_Q15(1.41000)}, +{FP_to_Q15(0.98789), FP_to_Q15(1.41500)}, +{FP_to_Q15(0.98865), FP_to_Q15(1.42000)}, +{FP_to_Q15(0.98939), FP_to_Q15(1.42500)}, +{FP_to_Q15(0.99010), FP_to_Q15(1.43000)}, +{FP_to_Q15(0.99079), FP_to_Q15(1.43500)}, +{FP_to_Q15(0.99146), FP_to_Q15(1.44000)}, +{FP_to_Q15(0.99210), FP_to_Q15(1.44500)}, +{FP_to_Q15(0.99271), FP_to_Q15(1.45000)}, +{FP_to_Q15(0.99330), FP_to_Q15(1.45500)}, +{FP_to_Q15(0.99387), FP_to_Q15(1.46000)}, +{FP_to_Q15(0.99441), FP_to_Q15(1.46500)}, +{FP_to_Q15(0.99492), FP_to_Q15(1.47000)}, +{FP_to_Q15(0.99542), FP_to_Q15(1.47500)}, +{FP_to_Q15(0.99588), FP_to_Q15(1.48000)}, +{FP_to_Q15(0.99632), FP_to_Q15(1.48500)}, +{FP_to_Q15(0.99674), FP_to_Q15(1.49000)}, +{FP_to_Q15(0.99713), FP_to_Q15(1.49500)}, +{FP_to_Q15(0.99749), FP_to_Q15(1.50000)}, +{FP_to_Q15(0.99784), FP_to_Q15(1.50500)}, +{FP_to_Q15(0.99815), FP_to_Q15(1.51000)}, +{FP_to_Q15(0.99844), FP_to_Q15(1.51500)}, +{FP_to_Q15(0.99871), FP_to_Q15(1.52000)}, +{FP_to_Q15(0.99895), FP_to_Q15(1.52500)}, +{FP_to_Q15(0.99917), FP_to_Q15(1.53000)}, +{FP_to_Q15(0.99936), FP_to_Q15(1.53500)}, +{FP_to_Q15(0.99953), FP_to_Q15(1.54000)}, +{FP_to_Q15(0.99967), FP_to_Q15(1.54500)}, +{FP_to_Q15(0.99978), FP_to_Q15(1.55000)}, +{FP_to_Q15(0.99988), FP_to_Q15(1.55500)}, +{FP_to_Q15(0.99994), FP_to_Q15(1.56000)}, +{FP_to_Q15(0.99998), FP_to_Q15(1.56500)}, +{FP_to_Q15(1.00000), FP_to_Q15(1.57000)}, +}; diff --git a/docs/OSP-daemon.html b/docs/OSP-daemon.html new file mode 100755 index 0000000..2b30feb --- /dev/null +++ b/docs/OSP-daemon.html @@ -0,0 +1,132 @@ +

OSPDaemon config file structure

+

Comments

+
    +
  • Lines beginning with the following is consider a comment: + Space, tab, #, ; +
  • Blank lines are ignored. +
+

Line format

+ KEYn.tag=value +

+ Where KEY is: +

+
system +
output +
This configures a sensor with data created or passed out by the daemon. It is for outbound data. +
input +
This configures a sensor that feeds data into the daemon. The data +can be consumed and processed by algs and/or it can be merely copied out to an outbound sensor. +
+ n is an integer from 0 to 99. Each KEY has its own number space.
+ This number is used to identify all the attributes related + to that entity. It is also used to reference the entity. + +

Valid lines

+
+
output0.name and input0.name +
This defines the name of the sensor. Exact usage depends on the +driver. +
    +
  • For input event drivers, this is the name of the input +device. For the outbound case, this is the name created. For the inbound case, +this is the name to look for. +
  • For iio drivers, this is the name of the iio device to +look for. +
  • For file oriented drivers, this is the name of the file to +read from or create/append. +
+ +
output0.type and input0.type +
This defines the type of sensor for output and input purposes. It +should be a string like SENSOR_ACCELEROMETER. The name is defined by the +enumerated type in OSP. Outbound sensors will come from either another +inbound sensor or from the algs. + +
output0.driver and input0.driver +
This defines the driver associated with the entity. + +
input0.copy +
Defines an output sensor to send a copy of the data to. Value +is the name of the output entity. For example:

+input0.copy=output1

+This will send a copy of data received on the input0 entity to output1 without +any transformation. + +

input0.swap +
This defines how the 3 axises from the input sensor will be +handled when it is process by the alg. This is specified by providing 3 +comma seperated numbers: 0, 1, and 2 representing X, Y, and Z respectively. +The order is X, Y, Z. Examples:

+

    +
  • No change: 0,1,2 +
  • Swap X and Y: 1,0,2 +
+
input0.convert +
Define a conversion factor for each axis. This number is multiplied +by the data prior being used. The number is a floating point value. The value can be provided in 2 formats. +
    +
  1. A single value. The same value is applied to all 3 axis. +
  2. 3 common seperated values. Each axis can have a unique value. +
+This value is typically used to scale and if needed invert an axis. +
input0.noise +
Define a noise value for each axis. The number is a floating point value. The value can be provided in 2 formats. +
    +
  1. A single value. The same value is applied to all 3 axis. +
  2. 3 common seperated values. Each axis can have a unique value. +
+This value is used by the algs. +
output0.noprocess +
Value is ignored. This attribute prevents the alg version of a +sensor from being used. For example, if there is an inbound data source for +SENSOR_ACCELEROMETER and the alg also provides a SENSOR_ACCELEROMETER (prehaps +as a calibrated or otherwise modified), this attribute will for the usage of +the version directly from the inbound data source. Unless there is a corresponding copy from an inbound sensor, this output entity will preoduce no data. +
input0.noprocess +
Value is ignored. This attribute prevents the inbound data from +being submitted to the algs. Unless there is a copy attibute, the data from +this sensor is ignored. +
output0.format +
Modifies the value to fit a particular output format. This is +used with the noprocess option. Valid options are: +
+
integer +
Data is not modified. +
float +
Data is converted to a float first then the binary contents +of the float is sent. +
shift24 +
Shift data by 24 bits. +
shift16 +
Shift data by 16 bits. +
shift15 +
Shift data by 15 bits. +
shift12 +
Shift data by 12 bits. +
+
input0.option, output0.option +
This defines configuration option for the driver. Parameter is +a value. Values beginning with 0x are considered to be hex. Meaning of the +value depends on the driver. Currently, only the input event output driver +uses this: +
    +
  • bit 0: Enable embedded time stamps. 64 bit time stamps +as passed as ABS_THROTTLE (upper 32 bits)/ABS_RUDDER (lower 32bits) +
  • bit 1: Enable sending the 4th element of a quaternion. +This value is passed as ABS_WHEEL. +
  • bit 2: Enable dittering of the output to defeat the input +subsystem's duplicate value elimination. It adds a value from 0-2 to each +data. +
+
system0.calfile +
This defines the name of the file to write cal data to. The + last one specified will be used. Relative path are relative to the +process working directory. To avoid surprises, use absolute paths. +
system0.pmdir +
Define a directory for enabling/disabling sensors. A path in a +tmpfs is highly desireable to minimize flash wear. +
+
+Updated +Fri Sep 11 18:35:37 PDT 2015 + diff --git a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c index 6d1ee8c..c68e642 100644 --- a/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c +++ b/embedded/projects/osp-lpc54102/sources/app/i2c_slavecomm_t.c @@ -69,66 +69,113 @@ int FastData_add(volatile struct FastData *FD, Buffer_t *pHIFPkt); \*-------------------------------------------------------------------------*/ #define SH_WHO_AM_I 0x54 #define SH_VERSION0 0x01 -#define SH_VERSION1 0x23 +#define SH_VERSION1 0x24 static uint32_t SensorState[2]; +static uint32_t PSensorState[2]; // TODO: Need to call algorithm module to subscribe the enable sensor. -static void SensorEnable(ASensorType_t sen) +static void SensorEnable(ASensorType_t sen, int space) { int v = (int)sen; int set; - - if (v < 32) { - set = (1<= 0x20 && rx_buf[0] < 0x50) { /* ENABLE */ - SensorEnable((ASensorType_t)(rx_buf[0]-0x20)); + SensorEnable((ASensorType_t)(rx_buf[0]-0x20), space); D0_printf("Enable %i\r\n", rx_buf[0] - 0x20); } else if (rx_buf[0] >= 0x50 && rx_buf[0] < 0x80) { /* DISABLE */ - SensorDisable((ASensorType_t)(rx_buf[0]-0x50)); + SensorDisable((ASensorType_t)(rx_buf[0]-0x50), space); D0_printf("Disable %i\r\n", rx_buf[0] - 0x50); } break; diff --git a/include/linux/iio/events.h b/include/linux/iio/events.h new file mode 100644 index 0000000..c25f0e3 --- /dev/null +++ b/include/linux/iio/events.h @@ -0,0 +1,105 @@ +/* The industrial I/O - event passing to userspace + * + * Copyright (c) 2008-2011 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ +#ifndef _IIO_EVENTS_H_ +#define _IIO_EVENTS_H_ + +#include +#include +#include "types.h" + +/** + * struct iio_event_data - The actual event being pushed to userspace + * @id: event identifier + * @timestamp: best estimate of time of event occurrence (often from + * the interrupt handler) + */ +struct iio_event_data { + __u64 id; + __s64 timestamp; +}; + +#define IIO_GET_EVENT_FD_IOCTL _IOR('i', 0x90, int) + +enum iio_event_type { + IIO_EV_TYPE_THRESH, + IIO_EV_TYPE_MAG, + IIO_EV_TYPE_ROC, + IIO_EV_TYPE_THRESH_ADAPTIVE, + IIO_EV_TYPE_MAG_ADAPTIVE, +}; + +enum iio_event_direction { + IIO_EV_DIR_EITHER, + IIO_EV_DIR_RISING, + IIO_EV_DIR_FALLING, +}; + +/** + * IIO_EVENT_CODE() - create event identifier + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @diff: Whether the event is for an differential channel or not. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @direction: Direction of the event. One of enum iio_event_direction. + * @type: Type of the event. Should be one enum iio_event_type. + * @chan: Channel number for non-differential channels. + * @chan1: First channel number for differential channels. + * @chan2: Second channel number for differential channels. + */ + +#define IIO_EVENT_CODE(chan_type, diff, modifier, direction, \ + type, chan, chan1, chan2) \ + (((u64)type << 56) | ((u64)diff << 55) | \ + ((u64)direction << 48) | ((u64)modifier << 40) | \ + ((u64)chan_type << 32) | (((u16)chan2) << 16) | ((u16)chan1) | \ + ((u16)chan)) + + +#define IIO_EV_DIR_MAX 4 +#define IIO_EV_BIT(type, direction) \ + (1 << (type*IIO_EV_DIR_MAX + direction)) + +/** + * IIO_MOD_EVENT_CODE() - create event identifier for modified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @modifier: Modifier for the channel. Should be one of enum iio_modifier. + * @type: Type of the event. Should be one enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_MOD_EVENT_CODE(chan_type, number, modifier, \ + type, direction) \ + IIO_EVENT_CODE(chan_type, 0, modifier, direction, type, number, 0, 0) + +/** + * IIO_UNMOD_EVENT_CODE() - create event identifier for unmodified channels + * @chan_type: Type of the channel. Should be one of enum iio_chan_type. + * @number: Channel number. + * @type: Type of the event. Should be one enum iio_event_type. + * @direction: Direction of the event. One of enum iio_event_direction. + */ + +#define IIO_UNMOD_EVENT_CODE(chan_type, number, type, direction) \ + IIO_EVENT_CODE(chan_type, 0, 0, direction, type, number, 0, 0) + +#define IIO_EVENT_CODE_EXTRACT_TYPE(mask) ((mask >> 56) & 0xFF) + +#define IIO_EVENT_CODE_EXTRACT_DIR(mask) ((mask >> 48) & 0xCF) + +#define IIO_EVENT_CODE_EXTRACT_CHAN_TYPE(mask) ((mask >> 32) & 0xFF) + +/* Event code number extraction depends on which type of event we have. + * Perhaps review this function in the future*/ +#define IIO_EVENT_CODE_EXTRACT_CHAN(mask) ((__s16)(mask & 0xFFFF)) +#define IIO_EVENT_CODE_EXTRACT_CHAN2(mask) ((__s16)(((mask) >> 16) & 0xFFFF)) + +#define IIO_EVENT_CODE_EXTRACT_MODIFIER(mask) ((mask >> 40) & 0xFF) +#define IIO_EVENT_CODE_EXTRACT_DIFF(mask) (((mask) >> 55) & 0x1) + +#endif diff --git a/include/linux/iio/types.h b/include/linux/iio/types.h new file mode 100644 index 0000000..7873357 --- /dev/null +++ b/include/linux/iio/types.h @@ -0,0 +1,56 @@ +/* industrial I/O data types needed both in and out of kernel + * + * Copyright (c) 2008 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#ifndef _IIO_TYPES_H_ +#define _IIO_TYPES_H_ + +enum iio_chan_type { + /* real channel types */ + IIO_VOLTAGE, + IIO_CURRENT, + IIO_POWER, + IIO_ACCEL, + IIO_ANGL_VEL, + IIO_MAGN, + IIO_LIGHT, + IIO_INTENSITY, + IIO_PROXIMITY, + IIO_TEMP, + IIO_INCLI, + IIO_ROT, + IIO_ANGL, + IIO_TIMESTAMP, + IIO_CAPACITANCE, + IIO_PRESSURE, + IIO_QUATERNION, +}; + +enum iio_modifier { + IIO_NO_MOD, + IIO_MOD_X, + IIO_MOD_Y, + IIO_MOD_Z, + IIO_MOD_X_AND_Y, + IIO_MOD_X_AND_Z, + IIO_MOD_Y_AND_Z, + IIO_MOD_X_AND_Y_AND_Z, + IIO_MOD_X_OR_Y, + IIO_MOD_X_OR_Z, + IIO_MOD_Y_OR_Z, + IIO_MOD_X_OR_Y_OR_Z, + IIO_MOD_LIGHT_BOTH, + IIO_MOD_LIGHT_IR, + IIO_MOD_R, +}; + +#define IIO_VAL_INT 1 +#define IIO_VAL_INT_PLUS_MICRO 2 +#define IIO_VAL_INT_PLUS_NANO 3 + +#endif /* _IIO_TYPES_H_ */ diff --git a/include/linux/input.h b/include/linux/input.h new file mode 100644 index 0000000..d723b63 --- /dev/null +++ b/include/linux/input.h @@ -0,0 +1,1120 @@ +#ifndef _INPUT_H +#define _INPUT_H + +/* + * Copyright (c) 1999-2002 Vojtech Pavlik + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + */ + +#include +#include +#include +#include + +/* + * The event structure itself + */ + +struct input_event { + struct timeval time; + __u16 type; + __u16 code; + __s32 value; +}; + +/* + * Protocol version. + */ + +#define EV_VERSION 0x010001 + +/* + * IOCTLs (0x00 - 0x7f) + */ + +struct input_id { + __u16 bustype; + __u16 vendor; + __u16 product; + __u16 version; +}; + +/** + * struct input_absinfo - used by EVIOCGABS/EVIOCSABS ioctls + * @value: latest reported value for the axis. + * @minimum: specifies minimum value for the axis. + * @maximum: specifies maximum value for the axis. + * @fuzz: specifies fuzz value that is used to filter noise from + * the event stream. + * @flat: values that are within this value will be discarded by + * joydev interface and reported as 0 instead. + * @resolution: specifies resolution for the values reported for + * the axis. + * + * Note that input core does not clamp reported values to the + * [minimum, maximum] limits, such task is left to userspace. + * + * Resolution for main axes (ABS_X, ABS_Y, ABS_Z) is reported in + * units per millimeter (units/mm), resolution for rotational axes + * (ABS_RX, ABS_RY, ABS_RZ) is reported in units per radian. + */ +struct input_absinfo { + __s32 value; + __s32 minimum; + __s32 maximum; + __s32 fuzz; + __s32 flat; + __s32 resolution; +}; + +/** + * struct input_keymap_entry - used by EVIOCGKEYCODE/EVIOCSKEYCODE ioctls + * @scancode: scancode represented in machine-endian form. + * @len: length of the scancode that resides in @scancode buffer. + * @index: index in the keymap, may be used instead of scancode + * @flags: allows to specify how kernel should handle the request. For + * example, setting INPUT_KEYMAP_BY_INDEX flag indicates that kernel + * should perform lookup in keymap by @index instead of @scancode + * @keycode: key code assigned to this scancode + * + * The structure is used to retrieve and modify keymap data. Users have + * option of performing lookup either by @scancode itself or by @index + * in keymap entry. EVIOCGKEYCODE will also return scancode or index + * (depending on which element was used to perform lookup). + */ +struct input_keymap_entry { +#define INPUT_KEYMAP_BY_INDEX (1 << 0) + __u8 flags; + __u8 len; + __u16 index; + __u32 keycode; + __u8 scancode[32]; +}; + +#define EVIOCGVERSION _IOR('E', 0x01, int) /* get driver version */ +#define EVIOCGID _IOR('E', 0x02, struct input_id) /* get device ID */ +#define EVIOCGREP _IOR('E', 0x03, unsigned int[2]) /* get repeat settings */ +#define EVIOCSREP _IOW('E', 0x03, unsigned int[2]) /* set repeat settings */ + +#define EVIOCGKEYCODE _IOR('E', 0x04, unsigned int[2]) /* get keycode */ +#define EVIOCGKEYCODE_V2 _IOR('E', 0x04, struct input_keymap_entry) +#define EVIOCSKEYCODE _IOW('E', 0x04, unsigned int[2]) /* set keycode */ +#define EVIOCSKEYCODE_V2 _IOW('E', 0x04, struct input_keymap_entry) + +#define EVIOCGNAME(len) _IOC(_IOC_READ, 'E', 0x06, len) /* get device name */ +#define EVIOCGPHYS(len) _IOC(_IOC_READ, 'E', 0x07, len) /* get physical location */ +#define EVIOCGUNIQ(len) _IOC(_IOC_READ, 'E', 0x08, len) /* get unique identifier */ +#define EVIOCGPROP(len) _IOC(_IOC_READ, 'E', 0x09, len) /* get device properties */ + +#define EVIOCGKEY(len) _IOC(_IOC_READ, 'E', 0x18, len) /* get global key state */ +#define EVIOCGLED(len) _IOC(_IOC_READ, 'E', 0x19, len) /* get all LEDs */ +#define EVIOCGSND(len) _IOC(_IOC_READ, 'E', 0x1a, len) /* get all sounds status */ +#define EVIOCGSW(len) _IOC(_IOC_READ, 'E', 0x1b, len) /* get all switch states */ + +#define EVIOCGBIT(ev,len) _IOC(_IOC_READ, 'E', 0x20 + (ev), len) /* get event bits */ +#define EVIOCGABS(abs) _IOR('E', 0x40 + (abs), struct input_absinfo) /* get abs value/limits */ +#define EVIOCSABS(abs) _IOW('E', 0xc0 + (abs), struct input_absinfo) /* set abs value/limits */ + +#define EVIOCSFF _IOC(_IOC_WRITE, 'E', 0x80, sizeof(struct ff_effect)) /* send a force effect to a force feedback device */ +#define EVIOCRMFF _IOW('E', 0x81, int) /* Erase a force effect */ +#define EVIOCGEFFECTS _IOR('E', 0x84, int) /* Report number of effects playable at the same time */ + +#define EVIOCGRAB _IOW('E', 0x90, int) /* Grab/Release device */ + +/* + * Device properties and quirks + */ + +#define INPUT_PROP_POINTER 0x00 /* needs a pointer */ +#define INPUT_PROP_DIRECT 0x01 /* direct input devices */ +#define INPUT_PROP_BUTTONPAD 0x02 /* has button(s) under pad */ +#define INPUT_PROP_SEMI_MT 0x03 /* touch rectangle only */ + +#define INPUT_PROP_MAX 0x1f +#define INPUT_PROP_CNT (INPUT_PROP_MAX + 1) + +/* + * Event types + */ + +#define EV_SYN 0x00 +#define EV_KEY 0x01 +#define EV_REL 0x02 +#define EV_ABS 0x03 +#define EV_MSC 0x04 +#define EV_SW 0x05 +#define EV_LED 0x11 +#define EV_SND 0x12 +#define EV_REP 0x14 +#define EV_FF 0x15 +#define EV_PWR 0x16 +#define EV_FF_STATUS 0x17 +#define EV_MAX 0x1f +#define EV_CNT (EV_MAX+1) + +/* + * Synchronization events. + */ + +#define SYN_REPORT 0 +#define SYN_CONFIG 1 +#define SYN_MT_REPORT 2 +#define SYN_DROPPED 3 + +/* + * Keys and buttons + * + * Most of the keys/buttons are modeled after USB HUT 1.12 + * (see http://www.usb.org/developers/hidpage). + * Abbreviations in the comments: + * AC - Application Control + * AL - Application Launch Button + * SC - System Control + */ + +#define KEY_RESERVED 0 +#define KEY_ESC 1 +#define KEY_1 2 +#define KEY_2 3 +#define KEY_3 4 +#define KEY_4 5 +#define KEY_5 6 +#define KEY_6 7 +#define KEY_7 8 +#define KEY_8 9 +#define KEY_9 10 +#define KEY_0 11 +#define KEY_MINUS 12 +#define KEY_EQUAL 13 +#define KEY_BACKSPACE 14 +#define KEY_TAB 15 +#define KEY_Q 16 +#define KEY_W 17 +#define KEY_E 18 +#define KEY_R 19 +#define KEY_T 20 +#define KEY_Y 21 +#define KEY_U 22 +#define KEY_I 23 +#define KEY_O 24 +#define KEY_P 25 +#define KEY_LEFTBRACE 26 +#define KEY_RIGHTBRACE 27 +#define KEY_ENTER 28 +#define KEY_LEFTCTRL 29 +#define KEY_A 30 +#define KEY_S 31 +#define KEY_D 32 +#define KEY_F 33 +#define KEY_G 34 +#define KEY_H 35 +#define KEY_J 36 +#define KEY_K 37 +#define KEY_L 38 +#define KEY_SEMICOLON 39 +#define KEY_APOSTROPHE 40 +#define KEY_GRAVE 41 +#define KEY_LEFTSHIFT 42 +#define KEY_BACKSLASH 43 +#define KEY_Z 44 +#define KEY_X 45 +#define KEY_C 46 +#define KEY_V 47 +#define KEY_B 48 +#define KEY_N 49 +#define KEY_M 50 +#define KEY_COMMA 51 +#define KEY_DOT 52 +#define KEY_SLASH 53 +#define KEY_RIGHTSHIFT 54 +#define KEY_KPASTERISK 55 +#define KEY_LEFTALT 56 +#define KEY_SPACE 57 +#define KEY_CAPSLOCK 58 +#define KEY_F1 59 +#define KEY_F2 60 +#define KEY_F3 61 +#define KEY_F4 62 +#define KEY_F5 63 +#define KEY_F6 64 +#define KEY_F7 65 +#define KEY_F8 66 +#define KEY_F9 67 +#define KEY_F10 68 +#define KEY_NUMLOCK 69 +#define KEY_SCROLLLOCK 70 +#define KEY_KP7 71 +#define KEY_KP8 72 +#define KEY_KP9 73 +#define KEY_KPMINUS 74 +#define KEY_KP4 75 +#define KEY_KP5 76 +#define KEY_KP6 77 +#define KEY_KPPLUS 78 +#define KEY_KP1 79 +#define KEY_KP2 80 +#define KEY_KP3 81 +#define KEY_KP0 82 +#define KEY_KPDOT 83 + +#define KEY_ZENKAKUHANKAKU 85 +#define KEY_102ND 86 +#define KEY_F11 87 +#define KEY_F12 88 +#define KEY_RO 89 +#define KEY_KATAKANA 90 +#define KEY_HIRAGANA 91 +#define KEY_HENKAN 92 +#define KEY_KATAKANAHIRAGANA 93 +#define KEY_MUHENKAN 94 +#define KEY_KPJPCOMMA 95 +#define KEY_KPENTER 96 +#define KEY_RIGHTCTRL 97 +#define KEY_KPSLASH 98 +#define KEY_SYSRQ 99 +#define KEY_RIGHTALT 100 +#define KEY_LINEFEED 101 +#define KEY_HOME 102 +#define KEY_UP 103 +#define KEY_PAGEUP 104 +#define KEY_LEFT 105 +#define KEY_RIGHT 106 +#define KEY_END 107 +#define KEY_DOWN 108 +#define KEY_PAGEDOWN 109 +#define KEY_INSERT 110 +#define KEY_DELETE 111 +#define KEY_MACRO 112 +#define KEY_MUTE 113 +#define KEY_VOLUMEDOWN 114 +#define KEY_VOLUMEUP 115 +#define KEY_POWER 116 /* SC System Power Down */ +#define KEY_KPEQUAL 117 +#define KEY_KPPLUSMINUS 118 +#define KEY_PAUSE 119 +#define KEY_SCALE 120 /* AL Compiz Scale (Expose) */ + +#define KEY_KPCOMMA 121 +#define KEY_HANGEUL 122 +#define KEY_HANGUEL KEY_HANGEUL +#define KEY_HANJA 123 +#define KEY_YEN 124 +#define KEY_LEFTMETA 125 +#define KEY_RIGHTMETA 126 +#define KEY_COMPOSE 127 + +#define KEY_STOP 128 /* AC Stop */ +#define KEY_AGAIN 129 +#define KEY_PROPS 130 /* AC Properties */ +#define KEY_UNDO 131 /* AC Undo */ +#define KEY_FRONT 132 +#define KEY_COPY 133 /* AC Copy */ +#define KEY_OPEN 134 /* AC Open */ +#define KEY_PASTE 135 /* AC Paste */ +#define KEY_FIND 136 /* AC Search */ +#define KEY_CUT 137 /* AC Cut */ +#define KEY_HELP 138 /* AL Integrated Help Center */ +#define KEY_MENU 139 /* Menu (show menu) */ +#define KEY_CALC 140 /* AL Calculator */ +#define KEY_SETUP 141 +#define KEY_SLEEP 142 /* SC System Sleep */ +#define KEY_WAKEUP 143 /* System Wake Up */ +#define KEY_FILE 144 /* AL Local Machine Browser */ +#define KEY_SENDFILE 145 +#define KEY_DELETEFILE 146 +#define KEY_XFER 147 +#define KEY_PROG1 148 +#define KEY_PROG2 149 +#define KEY_WWW 150 /* AL Internet Browser */ +#define KEY_MSDOS 151 +#define KEY_COFFEE 152 /* AL Terminal Lock/Screensaver */ +#define KEY_SCREENLOCK KEY_COFFEE +#define KEY_DIRECTION 153 +#define KEY_CYCLEWINDOWS 154 +#define KEY_MAIL 155 +#define KEY_BOOKMARKS 156 /* AC Bookmarks */ +#define KEY_COMPUTER 157 +#define KEY_BACK 158 /* AC Back */ +#define KEY_FORWARD 159 /* AC Forward */ +#define KEY_CLOSECD 160 +#define KEY_EJECTCD 161 +#define KEY_EJECTCLOSECD 162 +#define KEY_NEXTSONG 163 +#define KEY_PLAYPAUSE 164 +#define KEY_PREVIOUSSONG 165 +#define KEY_STOPCD 166 +#define KEY_RECORD 167 +#define KEY_REWIND 168 +#define KEY_PHONE 169 /* Media Select Telephone */ +#define KEY_ISO 170 +#define KEY_CONFIG 171 /* AL Consumer Control Configuration */ +#define KEY_HOMEPAGE 172 /* AC Home */ +#define KEY_REFRESH 173 /* AC Refresh */ +#define KEY_EXIT 174 /* AC Exit */ +#define KEY_MOVE 175 +#define KEY_EDIT 176 +#define KEY_SCROLLUP 177 +#define KEY_SCROLLDOWN 178 +#define KEY_KPLEFTPAREN 179 +#define KEY_KPRIGHTPAREN 180 +#define KEY_NEW 181 /* AC New */ +#define KEY_REDO 182 /* AC Redo/Repeat */ + +#define KEY_F13 183 +#define KEY_F14 184 +#define KEY_F15 185 +#define KEY_F16 186 +#define KEY_F17 187 +#define KEY_F18 188 +#define KEY_F19 189 +#define KEY_F20 190 +#define KEY_F21 191 +#define KEY_F22 192 +#define KEY_F23 193 +#define KEY_F24 194 + +#define KEY_PLAYCD 200 +#define KEY_PAUSECD 201 +#define KEY_PROG3 202 +#define KEY_PROG4 203 +#define KEY_DASHBOARD 204 /* AL Dashboard */ +#define KEY_SUSPEND 205 +#define KEY_CLOSE 206 /* AC Close */ +#define KEY_PLAY 207 +#define KEY_FASTFORWARD 208 +#define KEY_BASSBOOST 209 +#define KEY_PRINT 210 /* AC Print */ +#define KEY_HP 211 +#define KEY_CAMERA 212 +#define KEY_SOUND 213 +#define KEY_QUESTION 214 +#define KEY_EMAIL 215 +#define KEY_CHAT 216 +#define KEY_SEARCH 217 +#define KEY_CONNECT 218 +#define KEY_FINANCE 219 /* AL Checkbook/Finance */ +#define KEY_SPORT 220 +#define KEY_SHOP 221 +#define KEY_ALTERASE 222 +#define KEY_CANCEL 223 /* AC Cancel */ +#define KEY_BRIGHTNESSDOWN 224 +#define KEY_BRIGHTNESSUP 225 +#define KEY_MEDIA 226 + +#define KEY_SWITCHVIDEOMODE 227 /* Cycle between available video + outputs (Monitor/LCD/TV-out/etc) */ +#define KEY_KBDILLUMTOGGLE 228 +#define KEY_KBDILLUMDOWN 229 +#define KEY_KBDILLUMUP 230 + +#define KEY_SEND 231 /* AC Send */ +#define KEY_REPLY 232 /* AC Reply */ +#define KEY_FORWARDMAIL 233 /* AC Forward Msg */ +#define KEY_SAVE 234 /* AC Save */ +#define KEY_DOCUMENTS 235 + +#define KEY_BATTERY 236 + +#define KEY_BLUETOOTH 237 +#define KEY_WLAN 238 +#define KEY_UWB 239 + +#define KEY_UNKNOWN 240 + +#define KEY_VIDEO_NEXT 241 /* drive next video source */ +#define KEY_VIDEO_PREV 242 /* drive previous video source */ +#define KEY_BRIGHTNESS_CYCLE 243 /* brightness up, after max is min */ +#define KEY_BRIGHTNESS_ZERO 244 /* brightness off, use ambient */ +#define KEY_DISPLAY_OFF 245 /* display device to off state */ + +#define KEY_WIMAX 246 +#define KEY_RFKILL 247 /* Key that controls all radios */ + +#define KEY_MICMUTE 248 /* Mute / unmute the microphone */ + +/* Code 255 is reserved for special needs of AT keyboard driver */ + +#define BTN_MISC 0x100 +#define BTN_0 0x100 +#define BTN_1 0x101 +#define BTN_2 0x102 +#define BTN_3 0x103 +#define BTN_4 0x104 +#define BTN_5 0x105 +#define BTN_6 0x106 +#define BTN_7 0x107 +#define BTN_8 0x108 +#define BTN_9 0x109 + +#define BTN_MOUSE 0x110 +#define BTN_LEFT 0x110 +#define BTN_RIGHT 0x111 +#define BTN_MIDDLE 0x112 +#define BTN_SIDE 0x113 +#define BTN_EXTRA 0x114 +#define BTN_FORWARD 0x115 +#define BTN_BACK 0x116 +#define BTN_TASK 0x117 + +#define BTN_JOYSTICK 0x120 +#define BTN_TRIGGER 0x120 +#define BTN_THUMB 0x121 +#define BTN_THUMB2 0x122 +#define BTN_TOP 0x123 +#define BTN_TOP2 0x124 +#define BTN_PINKIE 0x125 +#define BTN_BASE 0x126 +#define BTN_BASE2 0x127 +#define BTN_BASE3 0x128 +#define BTN_BASE4 0x129 +#define BTN_BASE5 0x12a +#define BTN_BASE6 0x12b +#define BTN_DEAD 0x12f + +#define BTN_GAMEPAD 0x130 +#define BTN_A 0x130 +#define BTN_B 0x131 +#define BTN_C 0x132 +#define BTN_X 0x133 +#define BTN_Y 0x134 +#define BTN_Z 0x135 +#define BTN_TL 0x136 +#define BTN_TR 0x137 +#define BTN_TL2 0x138 +#define BTN_TR2 0x139 +#define BTN_SELECT 0x13a +#define BTN_START 0x13b +#define BTN_MODE 0x13c +#define BTN_THUMBL 0x13d +#define BTN_THUMBR 0x13e + +#define BTN_DIGI 0x140 +#define BTN_TOOL_PEN 0x140 +#define BTN_TOOL_RUBBER 0x141 +#define BTN_TOOL_BRUSH 0x142 +#define BTN_TOOL_PENCIL 0x143 +#define BTN_TOOL_AIRBRUSH 0x144 +#define BTN_TOOL_FINGER 0x145 +#define BTN_TOOL_MOUSE 0x146 +#define BTN_TOOL_LENS 0x147 +#define BTN_TOOL_QUINTTAP 0x148 /* Five fingers on trackpad */ +#define BTN_TOUCH 0x14a +#define BTN_STYLUS 0x14b +#define BTN_STYLUS2 0x14c +#define BTN_TOOL_DOUBLETAP 0x14d +#define BTN_TOOL_TRIPLETAP 0x14e +#define BTN_TOOL_QUADTAP 0x14f /* Four fingers on trackpad */ + +#define BTN_WHEEL 0x150 +#define BTN_GEAR_DOWN 0x150 +#define BTN_GEAR_UP 0x151 + +#define KEY_OK 0x160 +#define KEY_SELECT 0x161 +#define KEY_GOTO 0x162 +#define KEY_CLEAR 0x163 +#define KEY_POWER2 0x164 +#define KEY_OPTION 0x165 +#define KEY_INFO 0x166 /* AL OEM Features/Tips/Tutorial */ +#define KEY_TIME 0x167 +#define KEY_VENDOR 0x168 +#define KEY_ARCHIVE 0x169 +#define KEY_PROGRAM 0x16a /* Media Select Program Guide */ +#define KEY_CHANNEL 0x16b +#define KEY_FAVORITES 0x16c +#define KEY_EPG 0x16d +#define KEY_PVR 0x16e /* Media Select Home */ +#define KEY_MHP 0x16f +#define KEY_LANGUAGE 0x170 +#define KEY_TITLE 0x171 +#define KEY_SUBTITLE 0x172 +#define KEY_ANGLE 0x173 +#define KEY_ZOOM 0x174 +#define KEY_MODE 0x175 +#define KEY_KEYBOARD 0x176 +#define KEY_SCREEN 0x177 +#define KEY_PC 0x178 /* Media Select Computer */ +#define KEY_TV 0x179 /* Media Select TV */ +#define KEY_TV2 0x17a /* Media Select Cable */ +#define KEY_VCR 0x17b /* Media Select VCR */ +#define KEY_VCR2 0x17c /* VCR Plus */ +#define KEY_SAT 0x17d /* Media Select Satellite */ +#define KEY_SAT2 0x17e +#define KEY_CD 0x17f /* Media Select CD */ +#define KEY_TAPE 0x180 /* Media Select Tape */ +#define KEY_RADIO 0x181 +#define KEY_TUNER 0x182 /* Media Select Tuner */ +#define KEY_PLAYER 0x183 +#define KEY_TEXT 0x184 +#define KEY_DVD 0x185 /* Media Select DVD */ +#define KEY_AUX 0x186 +#define KEY_MP3 0x187 +#define KEY_AUDIO 0x188 /* AL Audio Browser */ +#define KEY_VIDEO 0x189 /* AL Movie Browser */ +#define KEY_DIRECTORY 0x18a +#define KEY_LIST 0x18b +#define KEY_MEMO 0x18c /* Media Select Messages */ +#define KEY_CALENDAR 0x18d +#define KEY_RED 0x18e +#define KEY_GREEN 0x18f +#define KEY_YELLOW 0x190 +#define KEY_BLUE 0x191 +#define KEY_CHANNELUP 0x192 /* Channel Increment */ +#define KEY_CHANNELDOWN 0x193 /* Channel Decrement */ +#define KEY_FIRST 0x194 +#define KEY_LAST 0x195 /* Recall Last */ +#define KEY_AB 0x196 +#define KEY_NEXT 0x197 +#define KEY_RESTART 0x198 +#define KEY_SLOW 0x199 +#define KEY_SHUFFLE 0x19a +#define KEY_BREAK 0x19b +#define KEY_PREVIOUS 0x19c +#define KEY_DIGITS 0x19d +#define KEY_TEEN 0x19e +#define KEY_TWEN 0x19f +#define KEY_VIDEOPHONE 0x1a0 /* Media Select Video Phone */ +#define KEY_GAMES 0x1a1 /* Media Select Games */ +#define KEY_ZOOMIN 0x1a2 /* AC Zoom In */ +#define KEY_ZOOMOUT 0x1a3 /* AC Zoom Out */ +#define KEY_ZOOMRESET 0x1a4 /* AC Zoom */ +#define KEY_WORDPROCESSOR 0x1a5 /* AL Word Processor */ +#define KEY_EDITOR 0x1a6 /* AL Text Editor */ +#define KEY_SPREADSHEET 0x1a7 /* AL Spreadsheet */ +#define KEY_GRAPHICSEDITOR 0x1a8 /* AL Graphics Editor */ +#define KEY_PRESENTATION 0x1a9 /* AL Presentation App */ +#define KEY_DATABASE 0x1aa /* AL Database App */ +#define KEY_NEWS 0x1ab /* AL Newsreader */ +#define KEY_VOICEMAIL 0x1ac /* AL Voicemail */ +#define KEY_ADDRESSBOOK 0x1ad /* AL Contacts/Address Book */ +#define KEY_MESSENGER 0x1ae /* AL Instant Messaging */ +#define KEY_DISPLAYTOGGLE 0x1af /* Turn display (LCD) on and off */ +#define KEY_SPELLCHECK 0x1b0 /* AL Spell Check */ +#define KEY_LOGOFF 0x1b1 /* AL Logoff */ + +#define KEY_DOLLAR 0x1b2 +#define KEY_EURO 0x1b3 + +#define KEY_FRAMEBACK 0x1b4 /* Consumer - transport controls */ +#define KEY_FRAMEFORWARD 0x1b5 +#define KEY_CONTEXT_MENU 0x1b6 /* GenDesc - system context menu */ +#define KEY_MEDIA_REPEAT 0x1b7 /* Consumer - transport control */ +#define KEY_10CHANNELSUP 0x1b8 /* 10 channels up (10+) */ +#define KEY_10CHANNELSDOWN 0x1b9 /* 10 channels down (10-) */ +#define KEY_IMAGES 0x1ba /* AL Image Browser */ + +#define KEY_DEL_EOL 0x1c0 +#define KEY_DEL_EOS 0x1c1 +#define KEY_INS_LINE 0x1c2 +#define KEY_DEL_LINE 0x1c3 + +#define KEY_FN 0x1d0 +#define KEY_FN_ESC 0x1d1 +#define KEY_FN_F1 0x1d2 +#define KEY_FN_F2 0x1d3 +#define KEY_FN_F3 0x1d4 +#define KEY_FN_F4 0x1d5 +#define KEY_FN_F5 0x1d6 +#define KEY_FN_F6 0x1d7 +#define KEY_FN_F7 0x1d8 +#define KEY_FN_F8 0x1d9 +#define KEY_FN_F9 0x1da +#define KEY_FN_F10 0x1db +#define KEY_FN_F11 0x1dc +#define KEY_FN_F12 0x1dd +#define KEY_FN_1 0x1de +#define KEY_FN_2 0x1df +#define KEY_FN_D 0x1e0 +#define KEY_FN_E 0x1e1 +#define KEY_FN_F 0x1e2 +#define KEY_FN_S 0x1e3 +#define KEY_FN_B 0x1e4 + +#define KEY_BRL_DOT1 0x1f1 +#define KEY_BRL_DOT2 0x1f2 +#define KEY_BRL_DOT3 0x1f3 +#define KEY_BRL_DOT4 0x1f4 +#define KEY_BRL_DOT5 0x1f5 +#define KEY_BRL_DOT6 0x1f6 +#define KEY_BRL_DOT7 0x1f7 +#define KEY_BRL_DOT8 0x1f8 +#define KEY_BRL_DOT9 0x1f9 +#define KEY_BRL_DOT10 0x1fa + +#define KEY_NUMERIC_0 0x200 /* used by phones, remote controls, */ +#define KEY_NUMERIC_1 0x201 /* and other keypads */ +#define KEY_NUMERIC_2 0x202 +#define KEY_NUMERIC_3 0x203 +#define KEY_NUMERIC_4 0x204 +#define KEY_NUMERIC_5 0x205 +#define KEY_NUMERIC_6 0x206 +#define KEY_NUMERIC_7 0x207 +#define KEY_NUMERIC_8 0x208 +#define KEY_NUMERIC_9 0x209 +#define KEY_NUMERIC_STAR 0x20a +#define KEY_NUMERIC_POUND 0x20b + +#define KEY_CAMERA_FOCUS 0x210 +#define KEY_WPS_BUTTON 0x211 /* WiFi Protected Setup key */ + +#define KEY_TOUCHPAD_TOGGLE 0x212 /* Request switch touchpad on or off */ +#define KEY_TOUCHPAD_ON 0x213 +#define KEY_TOUCHPAD_OFF 0x214 + +#define KEY_CAMERA_ZOOMIN 0x215 +#define KEY_CAMERA_ZOOMOUT 0x216 +#define KEY_CAMERA_UP 0x217 +#define KEY_CAMERA_DOWN 0x218 +#define KEY_CAMERA_LEFT 0x219 +#define KEY_CAMERA_RIGHT 0x21a + +#define BTN_TRIGGER_HAPPY 0x2c0 +#define BTN_TRIGGER_HAPPY1 0x2c0 +#define BTN_TRIGGER_HAPPY2 0x2c1 +#define BTN_TRIGGER_HAPPY3 0x2c2 +#define BTN_TRIGGER_HAPPY4 0x2c3 +#define BTN_TRIGGER_HAPPY5 0x2c4 +#define BTN_TRIGGER_HAPPY6 0x2c5 +#define BTN_TRIGGER_HAPPY7 0x2c6 +#define BTN_TRIGGER_HAPPY8 0x2c7 +#define BTN_TRIGGER_HAPPY9 0x2c8 +#define BTN_TRIGGER_HAPPY10 0x2c9 +#define BTN_TRIGGER_HAPPY11 0x2ca +#define BTN_TRIGGER_HAPPY12 0x2cb +#define BTN_TRIGGER_HAPPY13 0x2cc +#define BTN_TRIGGER_HAPPY14 0x2cd +#define BTN_TRIGGER_HAPPY15 0x2ce +#define BTN_TRIGGER_HAPPY16 0x2cf +#define BTN_TRIGGER_HAPPY17 0x2d0 +#define BTN_TRIGGER_HAPPY18 0x2d1 +#define BTN_TRIGGER_HAPPY19 0x2d2 +#define BTN_TRIGGER_HAPPY20 0x2d3 +#define BTN_TRIGGER_HAPPY21 0x2d4 +#define BTN_TRIGGER_HAPPY22 0x2d5 +#define BTN_TRIGGER_HAPPY23 0x2d6 +#define BTN_TRIGGER_HAPPY24 0x2d7 +#define BTN_TRIGGER_HAPPY25 0x2d8 +#define BTN_TRIGGER_HAPPY26 0x2d9 +#define BTN_TRIGGER_HAPPY27 0x2da +#define BTN_TRIGGER_HAPPY28 0x2db +#define BTN_TRIGGER_HAPPY29 0x2dc +#define BTN_TRIGGER_HAPPY30 0x2dd +#define BTN_TRIGGER_HAPPY31 0x2de +#define BTN_TRIGGER_HAPPY32 0x2df +#define BTN_TRIGGER_HAPPY33 0x2e0 +#define BTN_TRIGGER_HAPPY34 0x2e1 +#define BTN_TRIGGER_HAPPY35 0x2e2 +#define BTN_TRIGGER_HAPPY36 0x2e3 +#define BTN_TRIGGER_HAPPY37 0x2e4 +#define BTN_TRIGGER_HAPPY38 0x2e5 +#define BTN_TRIGGER_HAPPY39 0x2e6 +#define BTN_TRIGGER_HAPPY40 0x2e7 + +/* We avoid low common keys in module aliases so they don't get huge. */ +#define KEY_MIN_INTERESTING KEY_MUTE +#define KEY_MAX 0x2ff +#define KEY_CNT (KEY_MAX+1) + +/* + * Relative axes + */ + +#define REL_X 0x00 +#define REL_Y 0x01 +#define REL_Z 0x02 +#define REL_RX 0x03 +#define REL_RY 0x04 +#define REL_RZ 0x05 +#define REL_HWHEEL 0x06 +#define REL_DIAL 0x07 +#define REL_WHEEL 0x08 +#define REL_MISC 0x09 +#define REL_MAX 0x0f +#define REL_CNT (REL_MAX+1) + +/* + * Absolute axes + */ + +#define ABS_X 0x00 +#define ABS_Y 0x01 +#define ABS_Z 0x02 +#define ABS_RX 0x03 +#define ABS_RY 0x04 +#define ABS_RZ 0x05 +#define ABS_THROTTLE 0x06 +#define ABS_RUDDER 0x07 +#define ABS_WHEEL 0x08 +#define ABS_GAS 0x09 +#define ABS_BRAKE 0x0a +#define ABS_HAT0X 0x10 +#define ABS_HAT0Y 0x11 +#define ABS_HAT1X 0x12 +#define ABS_HAT1Y 0x13 +#define ABS_HAT2X 0x14 +#define ABS_HAT2Y 0x15 +#define ABS_HAT3X 0x16 +#define ABS_HAT3Y 0x17 +#define ABS_PRESSURE 0x18 +#define ABS_DISTANCE 0x19 +#define ABS_TILT_X 0x1a +#define ABS_TILT_Y 0x1b +#define ABS_TOOL_WIDTH 0x1c + +#define ABS_VOLUME 0x20 + +#define ABS_MISC 0x28 + +#define ABS_MT_SLOT 0x2f /* MT slot being modified */ +#define ABS_MT_TOUCH_MAJOR 0x30 /* Major axis of touching ellipse */ +#define ABS_MT_TOUCH_MINOR 0x31 /* Minor axis (omit if circular) */ +#define ABS_MT_WIDTH_MAJOR 0x32 /* Major axis of approaching ellipse */ +#define ABS_MT_WIDTH_MINOR 0x33 /* Minor axis (omit if circular) */ +#define ABS_MT_ORIENTATION 0x34 /* Ellipse orientation */ +#define ABS_MT_POSITION_X 0x35 /* Center X ellipse position */ +#define ABS_MT_POSITION_Y 0x36 /* Center Y ellipse position */ +#define ABS_MT_TOOL_TYPE 0x37 /* Type of touching device */ +#define ABS_MT_BLOB_ID 0x38 /* Group a set of packets as a blob */ +#define ABS_MT_TRACKING_ID 0x39 /* Unique ID of initiated contact */ +#define ABS_MT_PRESSURE 0x3a /* Pressure on contact area */ +#define ABS_MT_DISTANCE 0x3b /* Contact hover distance */ + + +#define ABS_MAX 0x3f +#define ABS_CNT (ABS_MAX+1) + +/* + * Switch events + */ + +#define SW_LID 0x00 /* set = lid shut */ +#define SW_TABLET_MODE 0x01 /* set = tablet mode */ +#define SW_HEADPHONE_INSERT 0x02 /* set = inserted */ +#define SW_RFKILL_ALL 0x03 /* rfkill master switch, type "any" + set = radio enabled */ +#define SW_RADIO SW_RFKILL_ALL /* deprecated */ +#define SW_MICROPHONE_INSERT 0x04 /* set = inserted */ +#define SW_DOCK 0x05 /* set = plugged into dock */ +#define SW_LINEOUT_INSERT 0x06 /* set = inserted */ +#define SW_JACK_PHYSICAL_INSERT 0x07 /* set = mechanical switch set */ +#define SW_VIDEOOUT_INSERT 0x08 /* set = inserted */ +#define SW_CAMERA_LENS_COVER 0x09 /* set = lens covered */ +#define SW_KEYPAD_SLIDE 0x0a /* set = keypad slide out */ +#define SW_FRONT_PROXIMITY 0x0b /* set = front proximity sensor active */ +#define SW_ROTATE_LOCK 0x0c /* set = rotate locked/disabled */ +#define SW_LINEIN_INSERT 0x0d /* set = inserted */ +#define SW_MAX 0x0f +#define SW_CNT (SW_MAX+1) + +/* + * Misc events + */ + +#define MSC_SERIAL 0x00 +#define MSC_PULSELED 0x01 +#define MSC_GESTURE 0x02 +#define MSC_RAW 0x03 +#define MSC_SCAN 0x04 +#define MSC_MAX 0x07 +#define MSC_CNT (MSC_MAX+1) + +/* + * LEDs + */ + +#define LED_NUML 0x00 +#define LED_CAPSL 0x01 +#define LED_SCROLLL 0x02 +#define LED_COMPOSE 0x03 +#define LED_KANA 0x04 +#define LED_SLEEP 0x05 +#define LED_SUSPEND 0x06 +#define LED_MUTE 0x07 +#define LED_MISC 0x08 +#define LED_MAIL 0x09 +#define LED_CHARGING 0x0a +#define LED_MAX 0x0f +#define LED_CNT (LED_MAX+1) + +/* + * Autorepeat values + */ + +#define REP_DELAY 0x00 +#define REP_PERIOD 0x01 +#define REP_MAX 0x01 +#define REP_CNT (REP_MAX+1) + +/* + * Sounds + */ + +#define SND_CLICK 0x00 +#define SND_BELL 0x01 +#define SND_TONE 0x02 +#define SND_MAX 0x07 +#define SND_CNT (SND_MAX+1) + +/* + * IDs. + */ + +#define ID_BUS 0 +#define ID_VENDOR 1 +#define ID_PRODUCT 2 +#define ID_VERSION 3 + +#define BUS_PCI 0x01 +#define BUS_ISAPNP 0x02 +#define BUS_USB 0x03 +#define BUS_HIL 0x04 +#define BUS_BLUETOOTH 0x05 +#define BUS_VIRTUAL 0x06 + +#define BUS_ISA 0x10 +#define BUS_I8042 0x11 +#define BUS_XTKBD 0x12 +#define BUS_RS232 0x13 +#define BUS_GAMEPORT 0x14 +#define BUS_PARPORT 0x15 +#define BUS_AMIGA 0x16 +#define BUS_ADB 0x17 +#define BUS_I2C 0x18 +#define BUS_HOST 0x19 +#define BUS_GSC 0x1A +#define BUS_ATARI 0x1B +#define BUS_SPI 0x1C + +/* + * MT_TOOL types + */ +#define MT_TOOL_FINGER 0 +#define MT_TOOL_PEN 1 +#define MT_TOOL_MAX 1 + +/* + * Values describing the status of a force-feedback effect + */ +#define FF_STATUS_STOPPED 0x00 +#define FF_STATUS_PLAYING 0x01 +#define FF_STATUS_MAX 0x01 + +/* + * Structures used in ioctls to upload effects to a device + * They are pieces of a bigger structure (called ff_effect) + */ + +/* + * All duration values are expressed in ms. Values above 32767 ms (0x7fff) + * should not be used and have unspecified results. + */ + +/** + * struct ff_replay - defines scheduling of the force-feedback effect + * @length: duration of the effect + * @delay: delay before effect should start playing + */ +struct ff_replay { + __u16 length; + __u16 delay; +}; + +/** + * struct ff_trigger - defines what triggers the force-feedback effect + * @button: number of the button triggering the effect + * @interval: controls how soon the effect can be re-triggered + */ +struct ff_trigger { + __u16 button; + __u16 interval; +}; + +/** + * struct ff_envelope - generic force-feedback effect envelope + * @attack_length: duration of the attack (ms) + * @attack_level: level at the beginning of the attack + * @fade_length: duration of fade (ms) + * @fade_level: level at the end of fade + * + * The @attack_level and @fade_level are absolute values; when applying + * envelope force-feedback core will convert to positive/negative + * value based on polarity of the default level of the effect. + * Valid range for the attack and fade levels is 0x0000 - 0x7fff + */ +struct ff_envelope { + __u16 attack_length; + __u16 attack_level; + __u16 fade_length; + __u16 fade_level; +}; + +/** + * struct ff_constant_effect - defines parameters of a constant force-feedback effect + * @level: strength of the effect; may be negative + * @envelope: envelope data + */ +struct ff_constant_effect { + __s16 level; + struct ff_envelope envelope; +}; + +/** + * struct ff_ramp_effect - defines parameters of a ramp force-feedback effect + * @start_level: beginning strength of the effect; may be negative + * @end_level: final strength of the effect; may be negative + * @envelope: envelope data + */ +struct ff_ramp_effect { + __s16 start_level; + __s16 end_level; + struct ff_envelope envelope; +}; + +/** + * struct ff_condition_effect - defines a spring or friction force-feedback effect + * @right_saturation: maximum level when joystick moved all way to the right + * @left_saturation: same for the left side + * @right_coeff: controls how fast the force grows when the joystick moves + * to the right + * @left_coeff: same for the left side + * @deadband: size of the dead zone, where no force is produced + * @center: position of the dead zone + */ +struct ff_condition_effect { + __u16 right_saturation; + __u16 left_saturation; + + __s16 right_coeff; + __s16 left_coeff; + + __u16 deadband; + __s16 center; +}; + +/** + * struct ff_periodic_effect - defines parameters of a periodic force-feedback effect + * @waveform: kind of the effect (wave) + * @period: period of the wave (ms) + * @magnitude: peak value + * @offset: mean value of the wave (roughly) + * @phase: 'horizontal' shift + * @envelope: envelope data + * @custom_len: number of samples (FF_CUSTOM only) + * @custom_data: buffer of samples (FF_CUSTOM only) + * + * Known waveforms - FF_SQUARE, FF_TRIANGLE, FF_SINE, FF_SAW_UP, + * FF_SAW_DOWN, FF_CUSTOM. The exact syntax FF_CUSTOM is undefined + * for the time being as no driver supports it yet. + * + * Note: the data pointed by custom_data is copied by the driver. + * You can therefore dispose of the memory after the upload/update. + */ +struct ff_periodic_effect { + __u16 waveform; + __u16 period; + __s16 magnitude; + __s16 offset; + __u16 phase; + + struct ff_envelope envelope; + + __u32 custom_len; + __s16 *custom_data; +}; + +/** + * struct ff_rumble_effect - defines parameters of a periodic force-feedback effect + * @strong_magnitude: magnitude of the heavy motor + * @weak_magnitude: magnitude of the light one + * + * Some rumble pads have two motors of different weight. Strong_magnitude + * represents the magnitude of the vibration generated by the heavy one. + */ +struct ff_rumble_effect { + __u16 strong_magnitude; + __u16 weak_magnitude; +}; + +/** + * struct ff_effect - defines force feedback effect + * @type: type of the effect (FF_CONSTANT, FF_PERIODIC, FF_RAMP, FF_SPRING, + * FF_FRICTION, FF_DAMPER, FF_RUMBLE, FF_INERTIA, or FF_CUSTOM) + * @id: an unique id assigned to an effect + * @direction: direction of the effect + * @trigger: trigger conditions (struct ff_trigger) + * @replay: scheduling of the effect (struct ff_replay) + * @u: effect-specific structure (one of ff_constant_effect, ff_ramp_effect, + * ff_periodic_effect, ff_condition_effect, ff_rumble_effect) further + * defining effect parameters + * + * This structure is sent through ioctl from the application to the driver. + * To create a new effect application should set its @id to -1; the kernel + * will return assigned @id which can later be used to update or delete + * this effect. + * + * Direction of the effect is encoded as follows: + * 0 deg -> 0x0000 (down) + * 90 deg -> 0x4000 (left) + * 180 deg -> 0x8000 (up) + * 270 deg -> 0xC000 (right) + */ +struct ff_effect { + __u16 type; + __s16 id; + __u16 direction; + struct ff_trigger trigger; + struct ff_replay replay; + + union { + struct ff_constant_effect constant; + struct ff_ramp_effect ramp; + struct ff_periodic_effect periodic; + struct ff_condition_effect condition[2]; /* One for each axis */ + struct ff_rumble_effect rumble; + } u; +}; + +/* + * Force feedback effect types + */ + +#define FF_RUMBLE 0x50 +#define FF_PERIODIC 0x51 +#define FF_CONSTANT 0x52 +#define FF_SPRING 0x53 +#define FF_FRICTION 0x54 +#define FF_DAMPER 0x55 +#define FF_INERTIA 0x56 +#define FF_RAMP 0x57 + +#define FF_EFFECT_MIN FF_RUMBLE +#define FF_EFFECT_MAX FF_RAMP + +/* + * Force feedback periodic effect types + */ + +#define FF_SQUARE 0x58 +#define FF_TRIANGLE 0x59 +#define FF_SINE 0x5a +#define FF_SAW_UP 0x5b +#define FF_SAW_DOWN 0x5c +#define FF_CUSTOM 0x5d + +#define FF_WAVEFORM_MIN FF_SQUARE +#define FF_WAVEFORM_MAX FF_CUSTOM + +/* + * Set ff device properties + */ + +#define FF_GAIN 0x60 +#define FF_AUTOCENTER 0x61 + +#define FF_MAX 0x7f +#define FF_CNT (FF_MAX+1) + +#endif diff --git a/include/linux/uinput.h b/include/linux/uinput.h new file mode 100644 index 0000000..4466595 --- /dev/null +++ b/include/linux/uinput.h @@ -0,0 +1,137 @@ +#ifndef __UINPUT_H_ +#define __UINPUT_H_ +/* + * User level driver support for input subsystem + * + * Heavily based on evdev.c by Vojtech Pavlik + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Author: Aristeu Sergio Rozanski Filho + * + * Changes/Revisions: + * 0.3 24/05/2006 (Anssi Hannula ) + * - update ff support for the changes in kernel interface + * - add UINPUT_VERSION + * 0.2 16/10/2004 (Micah Dowty ) + * - added force feedback support + * - added UI_SET_PHYS + * 0.1 20/06/2002 + * - first public version + */ + +#include + +#define UINPUT_VERSION 3 + + +struct uinput_ff_upload { + int request_id; + int retval; + struct ff_effect effect; + struct ff_effect old; +}; + +struct uinput_ff_erase { + int request_id; + int retval; + int effect_id; +}; + +/* ioctl */ +#define UINPUT_IOCTL_BASE 'U' +#define UI_DEV_CREATE _IO(UINPUT_IOCTL_BASE, 1) +#define UI_DEV_DESTROY _IO(UINPUT_IOCTL_BASE, 2) + +#define UI_SET_EVBIT _IOW(UINPUT_IOCTL_BASE, 100, int) +#define UI_SET_KEYBIT _IOW(UINPUT_IOCTL_BASE, 101, int) +#define UI_SET_RELBIT _IOW(UINPUT_IOCTL_BASE, 102, int) +#define UI_SET_ABSBIT _IOW(UINPUT_IOCTL_BASE, 103, int) +#define UI_SET_MSCBIT _IOW(UINPUT_IOCTL_BASE, 104, int) +#define UI_SET_LEDBIT _IOW(UINPUT_IOCTL_BASE, 105, int) +#define UI_SET_SNDBIT _IOW(UINPUT_IOCTL_BASE, 106, int) +#define UI_SET_FFBIT _IOW(UINPUT_IOCTL_BASE, 107, int) +#define UI_SET_PHYS _IOW(UINPUT_IOCTL_BASE, 108, char*) +#define UI_SET_SWBIT _IOW(UINPUT_IOCTL_BASE, 109, int) +#define UI_SET_PROPBIT _IOW(UINPUT_IOCTL_BASE, 110, int) + +#define UI_BEGIN_FF_UPLOAD _IOWR(UINPUT_IOCTL_BASE, 200, struct uinput_ff_upload) +#define UI_END_FF_UPLOAD _IOW(UINPUT_IOCTL_BASE, 201, struct uinput_ff_upload) +#define UI_BEGIN_FF_ERASE _IOWR(UINPUT_IOCTL_BASE, 202, struct uinput_ff_erase) +#define UI_END_FF_ERASE _IOW(UINPUT_IOCTL_BASE, 203, struct uinput_ff_erase) + +/* + * To write a force-feedback-capable driver, the upload_effect + * and erase_effect callbacks in input_dev must be implemented. + * The uinput driver will generate a fake input event when one of + * these callbacks are invoked. The userspace code then uses + * ioctls to retrieve additional parameters and send the return code. + * The callback blocks until this return code is sent. + * + * The described callback mechanism is only used if ff_effects_max + * is set. + * + * To implement upload_effect(): + * 1. Wait for an event with type == EV_UINPUT and code == UI_FF_UPLOAD. + * A request ID will be given in 'value'. + * 2. Allocate a uinput_ff_upload struct, fill in request_id with + * the 'value' from the EV_UINPUT event. + * 3. Issue a UI_BEGIN_FF_UPLOAD ioctl, giving it the + * uinput_ff_upload struct. It will be filled in with the + * ff_effects passed to upload_effect(). + * 4. Perform the effect upload, and place a return code back into + the uinput_ff_upload struct. + * 5. Issue a UI_END_FF_UPLOAD ioctl, also giving it the + * uinput_ff_upload_effect struct. This will complete execution + * of our upload_effect() handler. + * + * To implement erase_effect(): + * 1. Wait for an event with type == EV_UINPUT and code == UI_FF_ERASE. + * A request ID will be given in 'value'. + * 2. Allocate a uinput_ff_erase struct, fill in request_id with + * the 'value' from the EV_UINPUT event. + * 3. Issue a UI_BEGIN_FF_ERASE ioctl, giving it the + * uinput_ff_erase struct. It will be filled in with the + * effect ID passed to erase_effect(). + * 4. Perform the effect erasure, and place a return code back + * into the uinput_ff_erase struct. + * 5. Issue a UI_END_FF_ERASE ioctl, also giving it the + * uinput_ff_erase_effect struct. This will complete execution + * of our erase_effect() handler. + */ + +/* + * This is the new event type, used only by uinput. + * 'code' is UI_FF_UPLOAD or UI_FF_ERASE, and 'value' + * is the unique request ID. This number was picked + * arbitrarily, above EV_MAX (since the input system + * never sees it) but in the range of a 16-bit int. + */ +#define EV_UINPUT 0x0101 +#define UI_FF_UPLOAD 1 +#define UI_FF_ERASE 2 + +#define UINPUT_MAX_NAME_SIZE 80 +struct uinput_user_dev { + char name[UINPUT_MAX_NAME_SIZE]; + struct input_id id; + int ff_effects_max; + int absmax[ABS_CNT]; + int absmin[ABS_CNT]; + int absfuzz[ABS_CNT]; + int absflat[ABS_CNT]; +}; +#endif /* __UINPUT_H_ */ + diff --git a/include/osp-api.h b/include/osp-api.h index 228b7f5..fa2ebe7 100644 --- a/include/osp-api.h +++ b/include/osp-api.h @@ -199,8 +199,15 @@ typedef struct { //! Union structure to encapsulate various data types to library algorithm. typedef union { - Android_TriAxisRawSensorData_t data; - // Add any future input data type here as needed. + Android_TriAxisRawSensorData_t rawdata; + Android_TriAxisPreciseData_t q24data; + Android_TriAxisExtendedData_t q12data; + Android_UncalibratedTriAxisPreciseData_t uncal_q24data; + Android_UncalibratedTriAxisExtendedData_t uncal_q12data; + Android_OrientationResultData_t orientdata; + Android_BooleanResultData_t booldata; + Android_StepCounterResultData_t stepcount; + Android_RotationVectorResultData_t rotvec; }OSP_InputSensorData_t; //! callback type used when the library needs to do an atomic operation @@ -293,8 +300,8 @@ typedef void (* OSP_ResultReadyCallback_t)(ResultHandle_t ResultHandle, void* p */ typedef struct { - InputSensor_t SensorType; //!< physical sensor accel, gyro, etc input to fusion algorithm - char* SensorName; //!< short human readable description, Null terminated + ASensorType_t SensorType; //!< sensor type from either the ASensorType_t or PSensorType_t list + char const * SensorName; //!< short human readable description, Null terminated uint32_t DataWidthMask; //!< how much of the data word that is sent significant AxisMapType_t AxisMapping[3]; //!< e.g. use to switch from left handed sensor to right handed system or swap X and Y axes of a two axis sensor int32_t ConversionOffset[3]; //!< an offset for X,Y & Z to compensate for sensor origin (0,0,0) @@ -402,7 +409,7 @@ OSP_STATUS_t OSP_Initialize(const SystemDescriptor_t* pSystemDesc); * * \return status as specified in OSP_Types.h */ -OSP_STATUS_t OSP_RegisterInputSensor(SensorDescriptor_t *pSensorDescriptor, +OSP_STATUS_t OSP_RegisterInputSensor(const SensorDescriptor_t *pSensorDescriptor, InputSensorHandle_t *pReturnedHandle); //! use to change a sensors operating mode (output rate, position, etc...) diff --git a/include/osp-sensors.h b/include/osp-sensors.h index d9ea94c..f8e5111 100644 --- a/include/osp-sensors.h +++ b/include/osp-sensors.h @@ -146,23 +146,6 @@ typedef enum _SensorParamId { } SensorParamId_t; -/* Enumeration for the input sensors. */ -typedef enum _InputSensor -{ - ACCEL_INPUT_SENSOR, - MAG_INPUT_SENSOR, - GYRO_INPUT_SENSOR, - LIGHT_INPUT_SENSOR, - PRESSURE_INPUT_SENSOR, - TEMPERATURE_INPUT_SENSOR, - PROXIMITY_INPUT_SENSOR, - RELATIVE_HUMIDITY_INPUT_SENSOR, - AMBIENT_TEMPERATURE_INPUT_SENSOR, - - NUM_INPUT_SENSORS, - UNKNOWN_INPUT_SENSOR = 0xFF -} InputSensor_t; - // TODO: Need more description how these are use. //! Segment detector subtypes typedef enum _SegmentSubType{ diff --git a/linux/daemon/Makefile b/linux/daemon/Makefile new file mode 100644 index 0000000..6a77ede --- /dev/null +++ b/linux/daemon/Makefile @@ -0,0 +1,17 @@ +ARMCC=arm-none-linux-gnueabi-gcc +CC=gcc +CFLAGS=-Wall -g -I../../include +LDFLAGS=-Wall -g +LIBS=-L../../algorithm/osp -lOSP + + +OBJS=OSPDaemon_queue.o OSPDaemon.o OSPDaemon_iio.o OSPDaemon_config.o \ + OSPDaemon_input.o OSPDaemon_inputreader.o \ + OSPDaemon_filecsv.o \ + OSPDaemon_pm.o OSPDaemon_driver.o + +OSPDaemon: $(OBJS) + $(CC) $(LDFLAGS) $(OBJS) -o OSPDaemon $(LIBS) + +clean: + rm -f *.o OSPDaemon diff --git a/linux/daemon/OSPDaemon.c b/linux/daemon/OSPDaemon.c new file mode 100644 index 0000000..1ecde30 --- /dev/null +++ b/linux/daemon/OSPDaemon.c @@ -0,0 +1,597 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_pm.h" +#include "OSPDaemon_driver.h" + +#include "osp-api.h" + + +unsigned int debug_level = 0; +unsigned int mainstatus = 0; +unsigned int disablepm = 0; + +static struct OSPDaemon_SensorDescription *sd; +static struct pollfd pfd[NUM_ANDROID_SENSOR_TYPE+NUM_PRIVATE_SENSOR_TYPE]; +static int nfd = NUM_ANDROID_SENSOR_TYPE+NUM_PRIVATE_SENSOR_TYPE; +static int ignorecal = 0; + +static OSP_STATUS_t OSPDaemon_SensorControl_cb(SensorControl_t *cmd); + +static const SystemDescriptor_t SystemDesc = { + .TstampConversionToSeconds = 1, + .EnterCritical = NULL, + .ExitCritical = NULL, + .SensorsControl = OSPDaemon_SensorControl_cb, +}; + + +static OSP_STATUS_t OSPDaemon_SensorControl_cb(SensorControl_t *cmd) +{ + int i; + + if (cmd == NULL) return OSP_STATUS_UNSPECIFIED_ERROR; + for (i = 0; i sensor_count; i++) { + if (sd->sensor[i].handle == cmd->Handle) + break; + } + + if (i == sd->sensor_count) + return OSP_STATUS_SENSOR_INVALID_DESCRIPTOR; + + switch (cmd->Command) { + case SENSOR_CONTROL_SENSOR_OFF: + OSPDaemon_driver_disable_in(&sd->sensor[i]); + return OSP_STATUS_OK; + case SENSOR_CONTROL_SENSOR_ON: + OSPDaemon_driver_enable_in(&sd->sensor[i]); + return OSP_STATUS_OK; + default: + return OSP_STATUS_UNSPECIFIED_ERROR; + } +} + +static unsigned char caldata[MAX_CALSIZE]; + +static void ReadCalData(char *name) +{ + int fd, ret; + int i; + struct OSPDaemon_CalRecord rec; + + if (ignorecal) return; + + fd = open(name, O_RDONLY); + if (fd < 0) return; + + do { + ret = read(fd, &rec, sizeof(rec)); + if (ret < sizeof(rec)) { + close(fd); + return; + } + if (rec.len > MAX_CALSIZE) + continue; + if (rec.len > 1) { + ret = read(fd, caldata+1, rec.len-1); + if (ret != rec.len-1) { + close(fd); + return; + } + caldata[0] = rec.data[0]; + } + /* Dispatch cal data - store in pCalibrationData */ + for (i = 0; i < sd->sensor_count; i++) { + if (sd->sensor[i].sensor.SensorType == rec.sensor) + break; + } + if (i != sd->sensor_count) { + memcpy(sd->sensor[i].caldata, caldata, rec.len); + sd->sensor[i].sensor.pCalibrationData = sd->sensor[i].caldata; + } + } while(1); +} + +static void updateCalFile(char *calfile, ASensorType_t s, + void *data, uint32_t sz) +{ + char tmpfile[PATH_MAX+1]; + int oldfd, newfd; + int ret; + struct OSPDaemon_CalRecord rec; + unsigned char *cdata; + + snprintf(tmpfile, PATH_MAX, "%s.update", calfile); + tmpfile[PATH_MAX] = '\0'; + unlink(tmpfile); + + oldfd = open(calfile, O_RDONLY); + if (oldfd < 0) return; + + newfd = open(tmpfile, O_WRONLY | O_CREAT, 0777); + if (newfd < 0) { + close(oldfd); + return; + } + + do { + ret = read(oldfd, &rec, sizeof(rec)); + if (ret == 0) { /* EOF */ + break; + } + if (ret < sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + if (rec.len > MAX_CALSIZE) + break; + if (rec.len > 1) { + ret = read(oldfd, caldata+1, rec.len-1); + if (ret != rec.len-1) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + caldata[0] = rec.data[0]; + } + if (rec.sensor == s) { + cdata = data; + rec.len = sz; + rec.data[0] = cdata[0]; + } else { + cdata = caldata; + } + ret = write(newfd, &rec, sizeof(rec)); + if (ret != sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + if (rec.len > 1) { + ret = write(newfd, cdata + 1, rec.len - 1); + if (ret != sizeof(rec)) { + close(oldfd); + close(newfd); + unlink(tmpfile); + return; + } + } + } while(1); + + close(newfd); + close(oldfd); + unlink(calfile); + rename(tmpfile, calfile); +} + +static void OSPDaemon_write_cal_cb(InputSensorHandle_t handle, + void *data, uint32_t sz, NTTIME ts) +{ + int i; + + for (i = 0; i < sd->sensor_count; i++) { + if (handle == sd->sensor[i].handle) { + break; + } + } + if (i == sd->sensor_count) { + fprintf(stderr, "Write call back with unknown handle\n"); + return; + } + if (sz > MAX_CALSIZE) return; + if (sd->CalFile) + updateCalFile(sd->CalFile, sd->sensor[i].sensor.SensorType, + data, sz); +} + +/* + * Generic alg data output call back. + * Looks up the queue assocaited with the sensor and + * places data on the queue. + */ +static void OSPDaemon_callback(ResultHandle_t handle, void *pData) +{ + int i; + struct OSPDaemon_output *out; + + for (i = 0; i < sd->output_count; i++) { + if (handle == sd->output[i].handle) + break; + } + if (i == sd->output_count) return; + out = &sd->output[i]; + + OSPDaemon_queue_put(&out->q, pData); + for (i = 0; i < nfd; i++) { + if (pfd[i].fd == out->fd) { + pfd[i].events = POLLOUT; + break; + } + } +} + +/* Apply modifications to the sensor data. Used with + * config file options to modify copied sensor data. + * Return: 0 - no mod, 1 - mod */ +static int modifyOutput(struct OSPDaemon_output *out, + OSP_InputSensorData_t *orig, + OSP_InputSensorData_t *mod) +{ + int val[5]; + int count; + int i; + unsigned long long ts; + union { + int i; + float f; + } conv; + + if (out->format == OUT_FORMAT_INTEGER) + return 0; + count = extractOSP(out->ResultDesc.SensorType, orig, &ts, val); + switch (out->format) { + case OUT_FORMAT_FLOAT: + for (i = 0; i < count; i++) { + conv.f = val[i]; + val[i] = conv.i; + } + break; + case OUT_FORMAT_Q24: + for (i = 0; i < count; i++) { + val[i] = val[i] << 24; + } + break; + case OUT_FORMAT_Q16: + for (i = 0; i < count; i++) { + val[i] = val[i] << 16; + } + break; + case OUT_FORMAT_Q15: + for (i = 0; i < count; i++) { + val[i] = val[i] << 15; + } + break; + case OUT_FORMAT_Q12: + for (i = 0; i < count; i++) { + val[i] = val[i] << 12; + } + break; + default: + break; + + } + conv2OSP(out->ResultDesc.SensorType, mod, ts, val); + return 1; +} +/* + * Processes received data: + * If the sensor is configured to be tee'd, queue it in the output sensor. + * Send data to the alg. + */ +static int OSPDaemon_senddata(struct OSPDaemon_SensorDetail *s) +{ + OSP_InputSensorData_t *od; + OSP_InputSensorData_t mod; + OSP_STATUS_t stat; + int ret = 0; + + if (!s->noprocess && s->pending) { + od = &s->pdata; + stat = OSP_SetInputData(s->handle, od); + if (stat != OSP_STATUS_OK) return 1; + s->pending = 0; + } + + do { + od = OSPDaemon_queue_get(&s->q); + if (od == NULL) break; + + if (s->output) { + if (disablepm || s->output->enable) { + if (modifyOutput(s->output, od, &mod) == 1) { + OSPDaemon_queue_put(&s->output->q, &mod); + } else { + OSPDaemon_queue_put(&s->output->q, od); + } + } + } + if (!s->noprocess) { + stat = OSP_SetInputData(s->handle, od); + if (stat != OSP_STATUS_OK) { + s->pending = 1; + memcpy(&s->pdata, od, sizeof(s->pdata)); + ret = 1; + break; + } + } + } while(1); + + return ret; +} + +static int OSPDaemon_process_inbound(struct pollfd *pfd, struct pollfd *list, int nfd, int dirty) +{ + int ret; + int j, k; + + /* Input events */ + for (j = 0; j < sd->sensor_count; j++) + if (sd->sensor[j].fd == pfd->fd) + break; + if (j < sd->sensor_count) { + OSPDaemon_driver_read(&sd->sensor[j]); + dirty++; + ret = OSPDaemon_senddata(&sd->sensor[j]); + if (sd->sensor[j].output) { + for (k = 0; k < nfd; k++) { + if (list[k].fd == sd->sensor[j].output->fd) { + list[k].events = POLLOUT; + break; + } + } + } + if (ret > 0) { + ret++; + } + } + return dirty; +} + +static int OSPDaemon_process_outbound(struct pollfd *pfd, int nfd) +{ + int j; + + for (j = 0; j < sd->output_count; j++) + if (sd->output[j].fd == pfd->fd) + break; + if (j < sd->output_count) { + if (OSPDaemon_driver_send(&sd->output[j]) == 0) { + pfd->events = 0; + } + } + + return 0; +} + +static int OSPDaemon_flush_out(struct OSPDaemon_SensorDescription *d) +{ + int j; + + for (j = 0; j < d->output_count; j++) { + if (!OSPDaemon_queue_isempty(&d->output[j].q)) { + OSPDaemon_driver_send(&d->output[j]); + } + } + + return 0; +} + +static int OSPDaemon_init_outbound(struct pollfd *pfd, int nfd, int start) +{ + int i, nstart; + OSP_STATUS_t oret; + + nstart = start; + for (i = 0; i < sd->output_count; i++) { + nstart++; + pfd[i+start].fd = sd->output[i].fd; + pfd[i+start].events = 0; + OSPDaemon_queue_init(&sd->output[i].q); + + if (sd->output[i].noprocess) continue; + + sd->output[i].ResultDesc.pResultReadyCallback = OSPDaemon_callback; + if (sd->output[i].type != -1) { + oret = OSP_SubscribeSensorResult(&sd->output[i].ResultDesc, &sd->output[i].handle); + if (oret != OSP_STATUS_OK) { + DBG(DEBUG_INIT, "Sensor subscribe failed\n"); + } + } + } + return nstart; +} + +/* + * start - index to start populating the pfd array. + * + * Returns start + number of entries used. + */ +static int OSPDaemon_init_inbound(struct pollfd *pfd, int nfd, int start) +{ + int i; + OSP_STATUS_t oret; + int nstart = start; + + if (sd->CalFile) + ReadCalData(sd->CalFile); + + for (i = 0; i < sd->sensor_count; i++) { + nstart++; + if (sd->sensor[i].fd < 0) + continue; + + pfd[i+start].fd = sd->sensor[i].fd; + pfd[i+start].events = POLLIN; + OSPDaemon_queue_init(&sd->sensor[i].q); + DBG(DEBUG_INIT, "Input sensor noprocess = %i\n", + sd->sensor[i].noprocess); + if (sd->sensor[i].noprocess) continue; + sd->sensor[i].sensor.pOptionalWriteDataCallback = + OSPDaemon_write_cal_cb; + + oret = OSP_RegisterInputSensor(&sd->sensor[i].sensor, &sd->sensor[i].handle); + if (oret != OSP_STATUS_OK) { + DBG(DEBUG_INIT, "Sensor register failed (%i)\n", oret); + } + } + return nstart; +} + +static void OSPDaemon(char *confname) +{ + int i, r = 0; + int dirty; + + if ((sd = OSPDaemon_config(confname)) == NULL) { + fprintf(stderr, "Bad/invalid config.\n"); + return; + } + + OSP_Initialize(&SystemDesc); + + for (i = 0; i < nfd; i++) { + pfd[i].fd = 0; + pfd[i].events = 0; + } + + OSPDaemon_driver_setup_in(sd->sensor, sd->sensor_count); + r = OSPDaemon_init_inbound(pfd, nfd, r); + OSPDaemon_driver_setup_out(sd->output, sd->output_count); + nfd = OSPDaemon_init_outbound(pfd, nfd, r); + sd->powerfd = OSPDaemon_power_init(sd); + if (sd->powerfd > 0) { + pfd[nfd].fd = sd->powerfd; + pfd[nfd].events |= POLLIN; + nfd++; + } + + for (i = 0; i < nfd; i++) { + DBG(DEBUG_INIT, "%i: fd %i events = %i\n", i, pfd[i].fd, pfd[i].events); + } + + dirty = 0; + while (!mainstatus) { + if (poll(pfd, nfd, -1) <= 0) + continue; + + for (i = 0; i < nfd; i++) { + if (pfd[i].revents & POLLIN) { + DBG(DEBUG_LOOP, "IN event\n"); + if (pfd[i].fd == sd->powerfd) { + OSPDaemon_power_process(sd, + sd->powerfd); + } else { + dirty = OSPDaemon_process_inbound( + &pfd[i], pfd, + nfd, dirty); + } + } + if (pfd[i].revents & POLLOUT) { + DBG(DEBUG_LOOP, "%s:%i OUT event\n", __func__, __LINE__); + OSPDaemon_process_outbound(&pfd[i], nfd); + } + } + + OSPDaemon_flush_out(sd); + + if (dirty) { + DBG(DEBUG_LOOP, "%s:%i PROCESS\n", __func__, __LINE__); + if (OSP_DoForegroundProcessing() == OSP_STATUS_IDLE) + dirty = 0; + OSP_DoBackgroundProcessing(); + } + } +} + +static void OSPDaemon_version(void) +{ + const OSP_Library_Version_t *ver; + OSP_GetLibraryVersion(&ver); + + printf("OSPDaemon compiled "__DATE__"@"__TIME__"\n"); + printf("Library version: %s\n", ver->VersionString); + printf("Library build: %s\n", ver->buildTime); +} + +static void OSPDaemon_help(const char *name) +{ + fprintf(stderr, "Usage %s [-c configfile|-d level|-v|-h|-p]\n", name); + fprintf(stderr, "-c configfile: Use configfile\n"); + fprintf(stderr, "-d level: Sets the debug level\n"); + fprintf(stderr, "-p: Disables power management.\n"); + fprintf(stderr, " All sensors default on\n"); + fprintf(stderr, "-v: Dumps version info and exits.\n"); + fprintf(stderr, "-h: Prints this message and exits.\n"); +} + +int main(int argc, char **argv) +{ + char *confname = NULL; + int ar = 1; + int quiet = 0; + + if (argc > 1) { + do { + if (argv[ar][0] == '-') { + switch (argv[ar][1]) { + case 'c': /* Config file */ + if (ar+1 < argc) { + confname = argv[ar+1]; + ar++; + } + break; + case 'd': /* Debug level */ + if (ar+1 < argc) { + debug_level = strtol(argv[ar+1], NULL, 0); + ar++; + } + break; + case 'h': /* Help */ + OSPDaemon_help(argv[0]); + exit(0); + break; + case 'p': /* Disable PM */ + disablepm = 1; + break; + case 'n': /* Ignore cal file */ + ignorecal = 1; + break; + case 'q': + quiet = 1; + break; + case 'v': /* Version */ + OSPDaemon_version(); + exit(0); + break; + } + ar++; + } + } while (ar < argc); + } + if (confname == NULL) + confname = "OSPDaemon.conf"; + if (!quiet) printf("Reading config from %s\n", confname); + if (!quiet) printf("Debug level 0x%08x\n", debug_level); + if (disablepm) { + if (!quiet) printf("Disabling Power Management\n"); + } + /* Catch SIGPIPE? */ + /* Driver inits. Replace with linker magic? */ + OSPDaemon_iio_init(); + OSPDaemon_inputreader_init(); + OSPDaemon_input_init(); + OSPDaemon_filecsv_init(); + /* End driver inits */ + + OSPDaemon(confname); + return 0; +} diff --git a/linux/daemon/OSPDaemon.h b/linux/daemon/OSPDaemon.h new file mode 100644 index 0000000..6cba63b --- /dev/null +++ b/linux/daemon/OSPDaemon.h @@ -0,0 +1,249 @@ +#ifndef _OSPDAEMON_H_ +#define _OSPDAEMON_H_ 1 +#include "osp-api.h" +#include "OSPDaemon_queue.h" +#include "osp-api.h" + +/* Per sensor cal data size. Data larger then this is ignored */ +#define MAX_CALSIZE 2048 + +struct OSPDaemon_SensorDetail; + +struct OSPDaemon_output { + ResultDescriptor_t ResultDesc; + ResultHandle_t handle; + struct OSPDaemon_SensorDetail *source; + int type; + char *name; + int fd; + int driver; + int noprocess; + int enable; + int usage; + int format; + struct queue q; + int option; +}; + +enum { + OUT_FORMAT_INTEGER, + OUT_FORMAT_FLOAT, + OUT_FORMAT_Q24, + OUT_FORMAT_Q16, + OUT_FORMAT_Q15, + OUT_FORMAT_Q12, +}; + +enum { + IN_FORMAT_INTEGER, + IN_FORMAT_FLOAT, +}; + +enum { + DRIVER_INVALID, + DRIVER_INPUT, + DRIVER_IIO, + DRIVER_IIOEVENT, + DRIVER_FILECSV, + DRIVER_UNKNOWN +}; + +enum { + DRIVER_TYPE_INPUT, + DRIVER_TYPE_OUTPUT, +}; + +/* Output embedded time stamps */ +#define INPUT_OPTION_EMBEDTS (1<<0) +/* Send all 4 elements of a quaternion */ +#define INPUT_OPTION_QUAT4 (1<<1) +/* Dither output to defeat input event dup rejection */ +#define INPUT_OPTION_DITHER (1<<2) + +static inline void conv2OSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long ts, + int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + d->q24data.X = val[0]; + d->q24data.Y = val[1]; + d->q24data.Z = val[2]; + d->q24data.TimeStamp = ts; + break; + + case SENSOR_GEOMAGNETIC_FIELD: + d->q12data.X = val[0]; + d->q12data.Y = val[1]; + d->q12data.Z = val[2]; + d->q12data.TimeStamp = ts; + break; + + case SENSOR_ORIENTATION: + d->orientdata.Yaw = val[0]; + d->orientdata.Pitch = val[1]; + d->orientdata.Roll = val[2]; + d->orientdata.TimeStamp = ts; + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + d->rotvec.X = val[0]; + d->rotvec.Y = val[1]; + d->rotvec.Z = val[2]; + d->rotvec.W = val[3]; + d->rotvec.TimeStamp = ts; + break; + case SENSOR_GYROSCOPE_UNCALIBRATED: + d->uncal_q24data.X = val[0]; + d->uncal_q24data.Y = val[1]; + d->uncal_q24data.Z = val[2]; + d->uncal_q24data.TimeStamp = ts; + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + d->uncal_q12data.X = val[0]; + d->uncal_q12data.Y = val[1]; + d->uncal_q12data.Z = val[2]; + d->uncal_q12data.TimeStamp = ts; + break; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + d->booldata.data = val[0]; + d->booldata.TimeStamp = ts; + break; + case SENSOR_STEP_COUNTER: + d->stepcount.StepCount = val[0]; + d->stepcount.TimeStamp = ts; + break; + default: + break; + } +} +static inline int extractOSP(ASensorType_t sensor, + OSP_InputSensorData_t *d, unsigned long long *ts, + int val[]) +{ + switch (sensor) { + case SENSOR_ACCELEROMETER: + case SENSOR_GYROSCOPE: + case SENSOR_GRAVITY: + case SENSOR_LINEAR_ACCELERATION: + val[0] = d->q24data.X; + val[1] = d->q24data.Y; + val[2] = d->q24data.Z; + *ts = d->q24data.TimeStamp; + return 3; + + case SENSOR_GEOMAGNETIC_FIELD: + val[0] = d->q12data.X; + val[1] = d->q12data.Y; + val[2] = d->q12data.Z; + *ts = d->q12data.TimeStamp; + return 3; + + case SENSOR_ORIENTATION: + val[0] = d->orientdata.Yaw; + val[1] = d->orientdata.Pitch; + val[2] = d->orientdata.Roll; + *ts = d->orientdata.TimeStamp; + return 3; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + val[0] = d->rotvec.X; + val[1] = d->rotvec.Y; + val[2] = d->rotvec.Z; + val[3] = d->rotvec.W; + *ts = d->rotvec.TimeStamp; + return 4; + case SENSOR_GYROSCOPE_UNCALIBRATED: + val[0] = d->uncal_q24data.X; + val[1] = d->uncal_q24data.Y; + val[2] = d->uncal_q24data.Z; + *ts = d->uncal_q24data.TimeStamp; + return 3; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + val[0] = d->uncal_q12data.X; + val[1] = d->uncal_q12data.Y; + val[2] = d->uncal_q12data.Z; + *ts = d->uncal_q12data.TimeStamp; + return 3; + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_DETECTOR: + val[0] = d->booldata.data; + *ts = d->booldata.TimeStamp; + return 1; + case SENSOR_STEP_COUNTER: + val[0] = d->stepcount.StepCount; + *ts = d->stepcount.TimeStamp; + return 1; + default: + *ts = 0; + return 0; + } + return 0; +} + + +struct OSPDaemon_SensorDetail { + SensorDescriptor_t sensor; + InputSensorHandle_t handle; + char caldata[MAX_CALSIZE]; + char const *name; + int fd; + int driver; + void *private; + struct OSPDaemon_output *output; + struct queue q; + int pending; + int noprocess; + int format; + OSP_InputSensorData_t pdata; + int option; +}; + +struct OSPDaemon_SensorDescription { + struct OSPDaemon_SensorDetail *sensor; + int sensor_count; + struct OSPDaemon_output *output; + int output_count; + char *CalFile; + char *powerPath; + int powerfd; +}; + + +/* Cal file format is an array of struct OSPDaemon_CalRecord */ + +struct OSPDaemon_CalRecord { + ASensorType_t sensor; + uint32_t len; + unsigned char data[1]; /* Array length is determined by len */ +}; + +#define DBG(x, ...) if (debug_level & x) {printf(__VA_ARGS__);} + +#define DEBUG_INIT (1<<0) +#define DEBUG_CONF (1<<1) +#define DEBUG_INDRIVER (1<<2) +#define DEBUG_OUTDRIVER (1<<3) +#define DEBUG_LOOP (1<<4) +#define DEBUG_DATA (1<<5) +#define DEBUG_PM (1<<6) +#define DEBUG_DRIVER (1<<7) + +extern unsigned int debug_level; +extern unsigned int mainstatus; +extern unsigned int disablepm; + +struct OSPDaemon_SensorDescription * OSPDaemon_config(char *); + +void OSPDaemon_iio_init(void); +void OSPDaemon_input_init(void); +void OSPDaemon_inputreader_init(void); +void OSPDaemon_filecsv_init(void); + +#endif diff --git a/linux/daemon/OSPDaemon_config.c b/linux/daemon/OSPDaemon_config.c new file mode 100644 index 0000000..12a9210 --- /dev/null +++ b/linux/daemon/OSPDaemon_config.c @@ -0,0 +1,686 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ +/* + * Config file parser. + * + * Does not explicitly use malloc. + */ + +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "osp-api.h" + +#define MAX_TAG 50 +#define NAME_LEN 80 + +#define DBGOUT(x...) DBG(DEBUG_CONF, "CONFIG: "x) +/* Pre-allocate storate to avoid malloc.*/ +static struct OSPDaemon_SensorDetail Sensor[MAX_TAG]; +static char SensorName[MAX_TAG][NAME_LEN]; +static int sensor_count = 0; +static struct OSPDaemon_output Output[MAX_TAG]; +static int output_count = 0; +static char CalFile[PATH_MAX]; +static char powerPath[PATH_MAX]; +char output_name[MAX_TAG][NAME_LEN]; +static struct OSPDaemon_SensorDescription sd; +/* + * File format: + * Lines beginning with '#' are comments. + * + * For source sensors (sensors to read from) + * sensor0.type = + * sensor0.driver = + * sensor0. + * .... + * For output sensor (sensors to output) + * output0.type = + * output0.name = + * output0.driver = + */ + +#define SEN_MAP(x) [x] = #x + +static int proc_sensor_type(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_noise(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_convert(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_swap(int linetype, int tag, char *val, int *taglist); +static int proc_name(int linetype, int tag, char *val, int *taglist); +static int proc_driver(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_copy(int linetype, int tag, char *val, int *taglist); +static int proc_output_noprocess(int linetype, int tag, char *val, int *taglist); +static int proc_output_format(int linetype, int tag, char *val, int *taglist); +static int proc_sensor_format(int linetype, int tag, char *val, int *taglist); +static int proc_system_calfile(int linetype, int tag, char *val, int *taglist); +static int proc_system_pmdir(int linetype, int tag, char *val, int *taglist); +static int proc_option(int linetype, int tag, char *val, int *taglist); + +enum { + LINE_UNKNOWN, + LINE_SENSOR, + LINE_OUTPUT, + LINE_SYSTEM, +}; +static const char *LineName[] = { + [LINE_UNKNOWN] = "Unkown", + [LINE_SENSOR] = "Input", + [LINE_OUTPUT] = "Output", + [LINE_SYSTEM] = "System", +}; + +static const char * SensorTypeMap[] = { + SEN_MAP(SENSOR_ACCELEROMETER), + SEN_MAP(SENSOR_GYROSCOPE), + SEN_MAP(SENSOR_MAGNETIC_FIELD), + SEN_MAP(SENSOR_ORIENTATION), + SEN_MAP(SENSOR_LIGHT), + SEN_MAP(SENSOR_PRESSURE), + SEN_MAP(SENSOR_TEMPERATURE), + SEN_MAP(SENSOR_PROXIMITY), + SEN_MAP(SENSOR_GRAVITY), + SEN_MAP(SENSOR_LINEAR_ACCELERATION), + SEN_MAP(SENSOR_ROTATION_VECTOR), + SEN_MAP(SENSOR_RELATIVE_HUMIDITY), + SEN_MAP(SENSOR_AMBIENT_TEMPERATURE), + SEN_MAP(SENSOR_MAGNETIC_FIELD_UNCALIBRATED), + SEN_MAP(SENSOR_GAME_ROTATION_VECTOR), + SEN_MAP(SENSOR_GYROSCOPE_UNCALIBRATED), + SEN_MAP(SENSOR_SIGNIFICANT_MOTION), + SEN_MAP(SENSOR_STEP_DETECTOR), + SEN_MAP(SENSOR_STEP_COUNTER), + SEN_MAP(SENSOR_GEOMAGNETIC_ROTATION_VECTOR), + [NUM_ANDROID_SENSOR_TYPE] = NULL +}; +static const char * SensorTypeMapP[] = { + SEN_MAP(PSENSOR_ACCELEROMETER_RAW), + SEN_MAP(PSENSOR_MAGNETIC_FIELD_RAW), + SEN_MAP(PSENSOR_GYROSCOPE_RAW), + SEN_MAP(PSENSOR_ACCELEROMETER_UNCALIBRATED), + [NUM_PRIVATE_SENSOR_TYPE] = NULL +}; + +static const char *DriverTypeMap[] = { + [DRIVER_INVALID] = "invalid", + [DRIVER_INPUT] = "input", + [DRIVER_IIO] = "iio", + [DRIVER_IIOEVENT] = "iioevent", + [DRIVER_FILECSV] = "file_csv", +}; + +static const struct _keymap { + char *key; + int (*proc)(int linetype, int tag, char *val, int *taglist); +} KeyMap[] = { + {"type", proc_sensor_type}, + {"swap", proc_sensor_swap}, + {"convert", proc_sensor_convert}, + {"noise", proc_sensor_noise}, + {"name", proc_name}, + {"driver", proc_driver}, + {"copy", proc_sensor_copy}, + {"noprocess", proc_output_noprocess}, + {"calfile", proc_system_calfile}, + {"pmdir", proc_system_pmdir}, + {"format", proc_output_format}, + {"iformat", proc_sensor_format}, + {"option", proc_option}, +}; + +/* Parse a string containing 3 comma seperated values */ +static int parse_3_double(char *val, double out[3]) +{ + char *ptr; + + out[0] = strtod(val, &ptr); + if (ptr && *ptr == ',') { + out[1] = strtod(ptr+1, &ptr); + if (ptr && *ptr == ',') + out[2] = strtod(ptr+1, NULL); + } else { + out[1] = out[0]; + out[2] = out[0]; + } + return 0; +} + +static int allocate_sensor(void) +{ + int ret; + + ret = sensor_count; + sensor_count++; + /* Initialize descriptor to sane values */ + Sensor[ret].sensor.DataWidthMask = 0xffffffff; + Sensor[ret].sensor.AxisMapping[0] = AXIS_MAP_POSITIVE_X; + Sensor[ret].sensor.AxisMapping[1] = AXIS_MAP_POSITIVE_Y; + Sensor[ret].sensor.AxisMapping[2] = AXIS_MAP_POSITIVE_Z; + Sensor[ret].sensor.MaxValue = 0xffffffff; + Sensor[ret].sensor.MinValue = 0xffffffff; + Sensor[ret].sensor.ConversionScale[0] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.ConversionScale[1] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.ConversionScale[2] = TOFIX_PRECISE(1.0f); + + Sensor[ret].sensor.NominalSamplePeriodInSeconds = TOFIX_PRECISE(0.02f); + Sensor[ret].sensor.factoryskr[0][0] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.factoryskr[0][1] = 0; + Sensor[ret].sensor.factoryskr[0][2] = 0; + + Sensor[ret].sensor.factoryskr[1][0] = 0; + Sensor[ret].sensor.factoryskr[1][1] = TOFIX_PRECISE(1.0f); + Sensor[ret].sensor.factoryskr[1][2] = 0; + + Sensor[ret].sensor.factoryskr[2][0]= 0; + Sensor[ret].sensor.factoryskr[2][1]= 0; + Sensor[ret].sensor.factoryskr[2][2]= TOFIX_PRECISE(1.0f); + + return ret; +} + +static int allocate_output(void) +{ + int ret; + + ret = output_count; + output_count++; + return ret; +} + +static int proc_system_calfile(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return -1; + + strncpy(CalFile, val, PATH_MAX-1); + CalFile[PATH_MAX-1] = '\0'; + DBGOUT("Cal file: %s\n", val); + + return 0; +} +static int proc_system_pmdir(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return -1; + + strncpy(powerPath, val, PATH_MAX-1); + powerPath[PATH_MAX-1] = '\0'; + DBGOUT("power path: %s\n", val); + + return 0; +} + +/* + * sensor0.copy=output0 + */ +static int proc_sensor_copy(int linetype, int tag, char *val, int *taglist) +{ + int out, ret = -1; + + if (linetype != LINE_SENSOR) return -1; + DBGOUT("Line %s", LineName[linetype]); + + if (taglist[tag] < 0) { + taglist[tag] = allocate_sensor(); + } + + if (strncmp("output", val, 6) == 0) { + out = atoi(val+6); + DBGOUT("Linking to out tag %i\n", out); + if (out >= 0 && out < MAX_TAG) { + Sensor[taglist[tag]].output = &Output[out]; + Output[out].source = &Sensor[taglist[tag]]; + ret = 0; + } + } + + return ret; +} +static int proc_output_noprocess(int linetype, int tag, char *val, int *taglist) +{ + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s", LineName[linetype]); + if (linetype == LINE_SENSOR) + Sensor[taglist[tag]].noprocess = 1; + else if (linetype == LINE_OUTPUT) + Output[taglist[tag]].noprocess = 1; + + return 0; +} +static int proc_option(int linetype, int tag, char *val, int *taglist) +{ + unsigned int opt; + + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s", LineName[linetype]); + + opt = strtol(val, NULL, 0); + if (linetype == LINE_OUTPUT) { + Output[taglist[tag]].option = opt; + } else if (linetype == LINE_SENSOR) { + Sensor[taglist[tag]].option = opt; + } else { + return -1; + } +} + +static int proc_output_format(int linetype, int tag, char *val, int *taglist) +{ + int shift; + + if (linetype != LINE_OUTPUT) return 0; + + if (taglist[tag] < 0) { + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s scale %s\n", LineName[linetype], val); + + /* scaleNN -> shift by NN */ + if (strncasecmp("shift", val, 5) == 0) { + shift = atoi(val+5); + DBGOUT("Line %s scale %s shift %i\n", LineName[linetype], val, shift); + switch(shift) { + case 24: + Output[taglist[tag]].format = OUT_FORMAT_Q24; + break; + case 16: + Output[taglist[tag]].format = OUT_FORMAT_Q16; + break; + case 12: + Output[taglist[tag]].format = OUT_FORMAT_Q12; + break; + case 15: + Output[taglist[tag]].format = OUT_FORMAT_Q15; + break; + default: + break; + } + + } else if (strcasecmp("integer", val) == 0) { + Output[taglist[tag]].format = OUT_FORMAT_INTEGER; + + } else if (strcasecmp("float", val) == 0) { + Output[taglist[tag]].format = OUT_FORMAT_FLOAT; + } + + return 0; +} + +/* Internal code (algs, etc) assume data is Qnn. If the input is + * not in that format, this config option converts to that. + */ +static int proc_sensor_format(int linetype, int tag, char *val, int *taglist) +{ + if (linetype != LINE_SYSTEM) return 0; + + if (taglist[tag] < 0) { + taglist[tag] = allocate_output(); + } + DBGOUT("Line %s scale %s\n", LineName[linetype], val); + + /* scaleNN -> shift by NN */ + if (strcasecmp("integer", val) == 0) { + Sensor[taglist[tag]].format = IN_FORMAT_INTEGER; + + } else if (strcasecmp("float", val) == 0) { + Sensor[taglist[tag]].format = IN_FORMAT_FLOAT; + } + + return 0; +} +static int proc_driver(int linetype, int tag, char *val, int *taglist) +{ + int i; + + for (i = 0; i < DRIVER_UNKNOWN; i++) { + DBGOUT("Comparing %s to %s\n", DriverTypeMap[i], val); + if (strcmp(DriverTypeMap[i], val) == 0) { + break; + } + } + DBGOUT("Line %s val = %s mapping to %i\n", LineName[linetype], val, i); + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + + if (linetype == LINE_OUTPUT) { + Output[taglist[tag]].driver = i; + } else { + Sensor[taglist[tag]].driver = i; + } + return 0; +} + +static int proc_name(int linetype, int tag, char *val, int *taglist) +{ + if (taglist[tag] < 0) { + if (linetype == LINE_SENSOR) + taglist[tag] = allocate_sensor(); + else if (linetype == LINE_OUTPUT) + taglist[tag] = allocate_output(); + } + + if (linetype == LINE_OUTPUT) { + strncpy(output_name[taglist[tag]], val, NAME_LEN-1); + output_name[taglist[tag]][NAME_LEN-1] = '\0'; + Output[taglist[tag]].name = output_name[taglist[tag]]; + DBGOUT("OUTPUT: tag %i - %s\n", tag, val); + } else if (linetype == LINE_SENSOR) { + strncpy(SensorName[taglist[tag]], val, NAME_LEN-1); + SensorName[taglist[tag]][NAME_LEN-1] = '\0'; + Sensor[taglist[tag]].name = SensorName[taglist[tag]]; + DBGOUT("SENSOR: tag %i - %s\n", tag, val); + } else { + return -1; + } + + return 0; +} + +/* Mapping to values used by the descriptor */ +static const int OSPaxisMap[3] = { + [0] = AXIS_MAP_POSITIVE_X, + [1] = AXIS_MAP_POSITIVE_Y, + [2] = AXIS_MAP_POSITIVE_Z, +}; + +/* sensor0.swap=0,1,2 */ +static int proc_sensor_swap(int linetype, int tag, char *val, int *taglist) +{ + int map[3]; + int ret = -1; + int idx; + + if (linetype != LINE_SENSOR) return -1; + + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + idx = 0; + + map[0] = val[idx]-'0'; + if (map[0] < 0 || map[0] > 2) goto error; + + idx++; + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + if (val[idx] != ',') goto error; + idx++; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + map[1] = val[idx]-'0'; + idx++; + if (map[1] < 0 || map[1] > 2) goto error; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + if (val[3] != ',') ret = -1; + idx++; + + while (val[idx] != '\0' && val[idx] == ' ') + idx++; + if (val[idx] == '\0') goto error; + + map[2] = val[idx]-'0'; + if (map[1] < 0 || map[1] > 2) goto error; + + if (map[0] == map[1] || map[0] == map[2] || + map[1] == map[2]) { + ret = -2; + goto error; + } + ret = 0; + + Sensor[taglist[tag]].sensor.AxisMapping[0] = OSPaxisMap[map[0]]; + Sensor[taglist[tag]].sensor.AxisMapping[1] = OSPaxisMap[map[1]]; + Sensor[taglist[tag]].sensor.AxisMapping[2] = OSPaxisMap[map[2]]; + DBGOUT("MAP: tag %i - %i %i %i\n", tag, map[0], map[1], map[2]); + +error: + if (ret < 0) + printf("INVALID MAP\n"); + return ret; +} + +/* sensor0.convert=-0.12345,-0.12345,-0.12345 */ +/* sensor0.convert=-0.12345 */ +static int proc_sensor_convert(int linetype, int tag, char *val, int *taglist) +{ + double convert[3]; + + if (linetype != LINE_SENSOR) return -1; + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + parse_3_double(val, convert); + Sensor[taglist[tag]].sensor.ConversionScale[0] = convert[0]; + Sensor[taglist[tag]].sensor.ConversionScale[1] = convert[1]; + Sensor[taglist[tag]].sensor.ConversionScale[2] = convert[2]; + + DBGOUT("CONVERT: tag %i %f %f %f\n", tag, convert[0], convert[1], convert[2]); + return 0; + +} +/* sensor0.noise=-0.12345,-0.12345,-0.12345 */ +/* sensor0.noise=-0.12345 */ +static int proc_sensor_noise(int linetype, int tag, char *val, int *taglist) +{ + double noise[3]; + + if (linetype != LINE_SENSOR) return -1; + + if (taglist[tag] < 0) + taglist[tag] = allocate_sensor(); + + parse_3_double(val, noise); + Sensor[taglist[tag]].sensor.Noise[0] = noise[0]; + Sensor[taglist[tag]].sensor.Noise[1] = noise[1]; + Sensor[taglist[tag]].sensor.Noise[2] = noise[2]; + + DBGOUT("NOISE: tag %i %f %f %f\n", tag, noise[0], noise[1], noise[2]); + + return 0; +} + +/* sensor0.type=SENSOR_ACCELEROMETER */ +static int proc_sensor_type(int linetype, int tag, char *val, int *taglist) +{ + int i; + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (SensorTypeMap[i] == NULL) continue; + if (strcmp(SensorTypeMap[i], val) == 0) { + DBGOUT("FOUND SENSOR: %i tag %i : %s\n", linetype, tag, SensorTypeMap[i]); + if (linetype == LINE_SENSOR) { + taglist[tag] = sensor_count; + sensor_count++; + Sensor[taglist[tag]].sensor.SensorType = i; + Sensor[taglist[tag]].sensor.SensorName = SensorTypeMap[i]; + } else if (linetype == LINE_OUTPUT) { + taglist[tag] = output_count; + output_count++; + Output[taglist[tag]].type = i; + Output[taglist[tag]].ResultDesc.SensorType = i; + Output[taglist[tag]].ResultDesc.OutputRateInSeconds = 0; + Output[taglist[tag]].ResultDesc.OptionData = &Output[taglist[tag]]; + } + break; + } + } + if (i != NUM_ANDROID_SENSOR_TYPE) { + return 0; + } + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + if (SensorTypeMapP[i] == NULL) continue; + if (strcmp(SensorTypeMapP[i], val) == 0) { + DBGOUT("FOUND SENSOR: %i tag %i : %s\n", linetype, tag, SensorTypeMapP[i]); + if (linetype == LINE_SENSOR) { + taglist[tag] = sensor_count; + sensor_count++; + Sensor[taglist[tag]].sensor.SensorType = M_PSensorToAndroidBase(i); + Sensor[taglist[tag]].sensor.SensorName = SensorTypeMapP[i]; + } else if (linetype == LINE_OUTPUT) { + taglist[tag] = output_count; + output_count++; + Output[taglist[tag]].type = M_PSensorToAndroidBase(i); + Output[taglist[tag]].ResultDesc.SensorType = M_PSensorToAndroidBase(i); + Output[taglist[tag]].ResultDesc.OutputRateInSeconds = 0; + Output[taglist[tag]].ResultDesc.OptionData = &Output[taglist[tag]]; + } + break; + } + } + if (i == NUM_PRIVATE_SENSOR_TYPE) { + fprintf(stderr, "Bad/invalid sensor: %s\n", val); + } + return 0; +} + +static void remove_trail_whitespace(char *p, int crlf) +{ + char *ptr; + ptr = p; + while (*ptr != '\0') { + if (*ptr == '\t') { + *ptr = '\0'; + continue; + } + if (*ptr == ' ') { + *ptr = '\0'; + continue; + } + if (crlf) { + if (*ptr == '\r') { + *ptr = '\0'; + continue; + } + if (*ptr == '\n') { + *ptr = '\0'; + continue; + } + } + ptr++; + } +} +static char * remove_lead_whitespace(char *p) +{ + char *ptr; + ptr = p; + while (*ptr == ' ' || *ptr == '\t') ptr++; + + return ptr; +} + +static void parse_line(int lineno, char *l, int *taglist1, int *taglist2) +{ + int i; + char *eq, *ptr, *key; + int tag; + int linetype = LINE_UNKNOWN; + + if (l[0] == '#' || + l[0] == '\r' || + l[0] == '\n' || + l[0] == ' ' || + l[0] == '\t' || + l[0] == ';') return; /* Comments */ + eq = strchr(l, '='); + if (!eq) return; + if (strncmp(l, "input", 5) == 0) + linetype = LINE_SENSOR; + else if (strncmp(l, "output", 6) == 0) + linetype = LINE_OUTPUT; + else if (strncmp(l, "system", 6) == 0) + linetype = LINE_SYSTEM; + + DBGOUT("Line %i: %s\n", lineno, LineName[linetype]); + if (linetype > 0) { + tag = strtol(l+6, &ptr, 0); + if (tag >= MAX_TAG) return; + + if (ptr > eq) return; + if (*ptr != '.') return; + key=ptr+1; + *eq = '\0'; + + remove_trail_whitespace(key, 0); + ptr = remove_lead_whitespace(eq+1); + remove_trail_whitespace(ptr, 1); + + if (ptr[0] == '\0') return; + + for (i = 0; i < sizeof(KeyMap)/sizeof(struct _keymap); i++) { + if (strcmp(key, KeyMap[i].key) == 0) { + if (KeyMap[i].proc == NULL) + break; + KeyMap[i].proc(linetype, tag, ptr, linetype == LINE_SENSOR?taglist1:(linetype == LINE_OUTPUT?taglist2:NULL)); + } + } + } +} + +/* A nonzero return will abort the daemon */ +struct OSPDaemon_SensorDescription *OSPDaemon_config(char *confname) +{ + FILE *f; + char line[2048], *r; + int i; + int tagmap1[MAX_TAG]; + int tagmap2[MAX_TAG]; + int lineno = 1; + + f = fopen(confname, "r"); + if (!f) return NULL; + for (i = 0; i < MAX_TAG; i++) { + tagmap1[i] = -1; + tagmap2[i] = -1; + memset(&Sensor[i], 0, sizeof(Sensor[i])); + Sensor[i].fd = -1; + memset(&Output[i], 0, sizeof(Sensor[i])); + Output[i].fd = -1; + } + sd.powerPath = NULL; + powerPath[0] = '\0'; + DBGOUT("%s:%i\n", __func__, __LINE__); + do { + r = fgets(line, 2047, f); + if (r) { + line[2047] = '\0'; + parse_line(lineno, line, tagmap1, tagmap2); + } + lineno++; + } while (r != NULL); + DBGOUT("Processed %i lines\n", lineno); + fclose(f); + sd.sensor = Sensor; + sd.sensor_count = sensor_count; + sd.output = Output; + sd.output_count = output_count; + sd.CalFile = CalFile; + if (powerPath[0] != '\0') + sd.powerPath = powerPath; + return &sd; +} diff --git a/linux/daemon/OSPDaemon_driver.c b/linux/daemon/OSPDaemon_driver.c new file mode 100644 index 0000000..5325b65 --- /dev/null +++ b/linux/daemon/OSPDaemon_driver.c @@ -0,0 +1,128 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +/* Driver framework */ + +#include +#include +#include "OSPDaemon.h" +#include "OSPDaemon_driver.h" + + +static struct OSPDaemon_driver *inmap[] = { + [DRIVER_INPUT] = NULL, + [DRIVER_IIO] = NULL, + [DRIVER_IIOEVENT] = NULL, + [DRIVER_FILECSV] = NULL, +}, *outmap[] = { + [DRIVER_INPUT] = NULL, + [DRIVER_IIO] = NULL, + [DRIVER_IIOEVENT] = NULL, + [DRIVER_FILECSV] = NULL, +}; + +int OSPDaemon_driver_register(struct OSPDaemon_driver *ops) +{ + if (ops == NULL) return -EINVAL; + + if (ops->drvtype == DRIVER_TYPE_INPUT) { + switch (ops->driver) { + case DRIVER_INPUT: + case DRIVER_IIO: + case DRIVER_IIOEVENT: + case DRIVER_FILECSV: + break; + default: + return -EINVAL; + } + inmap[ops->driver] = ops; + } else if (ops->drvtype == DRIVER_TYPE_OUTPUT) { + switch (ops->driver) { + case DRIVER_INPUT: + case DRIVER_FILECSV: + break; + default: + return -EINVAL; + } + outmap[ops->driver] = ops; + } else { + return -EINVAL; + } + return 0; +} + +int OSPDaemon_driver_read(struct OSPDaemon_SensorDetail *s) +{ + if (s == NULL || inmap[s->driver] == NULL) + return -EINVAL; + + return inmap[s->driver]->read(s); +} + +int OSPDaemon_driver_send(struct OSPDaemon_output *s) +{ + if (s == NULL || outmap[s->driver] == NULL) + return -EINVAL; + DBG(DEBUG_DRIVER, "Sending to driver %i\n", s->driver); + + return outmap[s->driver]->send(s); +} + +int OSPDaemon_driver_setup_in(struct OSPDaemon_SensorDetail *s, int count) +{ + if (inmap[DRIVER_IIO]) + inmap[DRIVER_IIO]->setup_in(s, count); + if (inmap[DRIVER_INPUT]) + inmap[DRIVER_INPUT]->setup_in(s, count); + if (inmap[DRIVER_FILECSV]) + inmap[DRIVER_FILECSV]->setup_in(s, count); + + return 0; +} + +int OSPDaemon_driver_setup_out(struct OSPDaemon_output *s, int count) +{ + DBG(DEBUG_DRIVER, "Calling INPUT outbound setup\n"); + if (outmap[DRIVER_INPUT]) + outmap[DRIVER_INPUT]->setup_out(s, count); + DBG(DEBUG_DRIVER, "Calling FileCSV outbound setup\n"); + if (outmap[DRIVER_FILECSV]) + outmap[DRIVER_FILECSV]->setup_out(s, count); + DBG(DEBUG_DRIVER, "Done out setup\n"); + + return 0; +} + +int OSPDaemon_driver_enable_out(struct OSPDaemon_output *s) +{ + s->enable = 1; + s->usage = 1; + + return 0; +} + +int OSPDaemon_driver_disable_out(struct OSPDaemon_output *s) +{ + s->enable = 0; + s->usage = 0; + + return 0; +} + +int OSPDaemon_driver_enable_in(struct OSPDaemon_SensorDetail *s) +{ + if (!s) return -1; + if (inmap[s->driver]->enable_in) + inmap[s->driver]->enable_in(s); + return 0; +} +int OSPDaemon_driver_disable_in(struct OSPDaemon_SensorDetail *s) +{ + if (!s) return -1; + if (inmap[s->driver]->disable_in) + inmap[s->driver]->disable_in(s); + return 0; +} diff --git a/linux/daemon/OSPDaemon_driver.h b/linux/daemon/OSPDaemon_driver.h new file mode 100644 index 0000000..0db87ec --- /dev/null +++ b/linux/daemon/OSPDaemon_driver.h @@ -0,0 +1,26 @@ +#ifndef _OSPDAEMON_DRIVER_H_ +#define _OSPDAEMON_DRIVER_H_ + +struct OSPDaemon_driver { + int drvtype; + int driver; + int (*setup_in)(struct OSPDaemon_SensorDetail *, int count); + int (*setup_out)(struct OSPDaemon_output *, int count); + int (*send)(struct OSPDaemon_output *); + int (*read)(struct OSPDaemon_SensorDetail *s); + int (*enable_out)(struct OSPDaemon_output *); + int (*enable_in)(struct OSPDaemon_SensorDetail *); + int (*disable_out)(struct OSPDaemon_output *); + int (*disable_in)(struct OSPDaemon_SensorDetail *); +}; + +int OSPDaemon_driver_setup_out(struct OSPDaemon_output *s, int count); +int OSPDaemon_driver_setup_in(struct OSPDaemon_SensorDetail *s, int count); +int OSPDaemon_driver_send(struct OSPDaemon_output *); +int OSPDaemon_driver_read(struct OSPDaemon_SensorDetail *); +int OSPDaemon_driver_register(struct OSPDaemon_driver *); +int OSPDaemon_driver_enable_out(struct OSPDaemon_output *s); +int OSPDaemon_driver_disable_out(struct OSPDaemon_output *s); +int OSPDaemon_driver_enable_in(struct OSPDaemon_SensorDetail *s); +int OSPDaemon_driver_disable_in(struct OSPDaemon_SensorDetail *s); +#endif diff --git a/linux/daemon/OSPDaemon_filecsv.c b/linux/daemon/OSPDaemon_filecsv.c new file mode 100644 index 0000000..da489a6 --- /dev/null +++ b/linux/daemon/OSPDaemon_filecsv.c @@ -0,0 +1,249 @@ +/* + * (C) 2015 HY Research LLC + * This file is under the Apache Public License. + */ + +/* + * Driver to read and write input from a file: + * Since a normal file is always "ready", we need to do a few + * special things like pausing on errors. + * This requires the rest of the system to be muxed with poll + * to insure fairness. Otherwise, this code will monopolize the + * system. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_filecsv.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_driver.h" + +#define BUF_SZ 512 +#define NUM_CSV 10 +#define LINE_LEN 80 + +static int filecsv_count = 0; +static struct FileCSV_Sensor { + int axis_count; + char buf[BUF_SZ]; + int remain; + int start; +} FileCSV[NUM_CSV]; + +static int OSPDaemon_filecsv_create(const char *name, struct FileCSV_Sensor *fs) +{ + int fd; + + fd = open(name, O_RDONLY); + if (fd < 0) return -1; + fs->remain = 0; + fs->start = 0; + + return fd; +} + +static int OSPDaemon_filecsv_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i; + int f_count = 0, f_success = 0; + + for (i = 0; i < count; i++) { + if (s->driver == DRIVER_FILECSV) { + f_count++; + if (filecsv_count >= NUM_CSV) continue; + s->fd = OSPDaemon_filecsv_create(s->name, &FileCSV[filecsv_count]); + if (s->fd > 0) { + f_success++; + s->private = &FileCSV[filecsv_count]; + switch (s->sensor.SensorType) { + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + FileCSV[filecsv_count].axis_count = 4; + break; + default: + FileCSV[filecsv_count].axis_count = 3; + } + filecsv_count++; + } + } + s++; + } + DBG(DEBUG_INDRIVER, "FileCSV: req %i success %i\n", f_count, f_success); + return 0; +} + +static void pause20(void) +{ + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 20000; + select(0, NULL, NULL, NULL, &tv); +} + +static int OSPDaemon_filecsv_read(struct OSPDaemon_SensorDetail *s) +{ + char line[LINE_LEN]; + char *next; + struct FileCSV_Sensor *fs = s->private; + OSP_InputSensorData_t od; + int val[5]; + int i, ret; + int lineloc = 0; + int gotline = 0; + int count; + + do { + if (fs->remain == 0) { + ret = read(s->fd, fs->buf, BUF_SZ); + if (ret <= 0) { + pause20(); + mainstatus = 1; + return 0; + } + fs->remain = ret; + fs->start = 0; + } + count = fs->remain; + for (i = 0; i < count && gotline == 0; i++, fs->start++, fs->remain--) { + if (lineloc > LINE_LEN-2) { + gotline = 1; + line[LINE_LEN-1] = '\0'; + break; + } + switch(fs->buf[fs->start]) { + case '\0': + case '\r': + case '\n': + gotline = 1; + line[lineloc] = '\0'; + break; + default: + line[lineloc] = fs->buf[fs->start]; + lineloc++; + break; + } + } + } while(!gotline); + + /* Assume X,Y,Z in integers */ + switch (line[0]) { + /* Comments */ + case '#': + case ';': + case '/': + case '\t': + case ' ': + return 0; + } + + DBG(DEBUG_INDRIVER, "Data line: %s\n", line); + + val[0] = strtol(line, &next, 10); + if (next == NULL) return 0; + + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[1] = strtol(next, &next, 10); + + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[2] = strtol(next, &next, 10); + + if (fs->axis_count == 4) { + while(!isdigit(*next) && *next != '\0' && *next != '-') next++; + if (*next == '\0') return 0; + val[3] = strtol(next, &next, 10); + } + conv2OSP(s->sensor.SensorType, &od, 0, val); + DBG(DEBUG_INDRIVER, "Data: %i, %i, %i\n", val[0], val[1], val[2]); + OSPDaemon_queue_put(&s->q, &od); + pause20(); + + return 1; +} + +static void OSPDaemon_writecsv_create(struct OSPDaemon_output *out) +{ + int fd; + DBG(DEBUG_OUTDRIVER, "Init for %s\n", out->name); + fd = open(out->name, O_WRONLY|O_CREAT|O_APPEND, S_IRWXU|S_IRGRP|S_IROTH); + if (fd < 0) return; + out->fd = fd; +} + +static int OSPDaemon_writecsv_setup(struct OSPDaemon_output *out, int count) +{ + int i; + int f_count = 0, f_success = 0; + for (i = 0; i < count; i++) { + if (out[i].driver == DRIVER_FILECSV) { + f_count++; + OSPDaemon_writecsv_create(&out[i]); + if (out[i].fd >= 0) + f_success++; + } + } + DBG(DEBUG_OUTDRIVER, "FileCSV: req %i suc %i\n", f_count, f_success); + return 0; +} + +static int OSPDaemon_writecsv_send(struct OSPDaemon_output *out) +{ + OSP_InputSensorData_t *od; + char line[LINE_LEN]; + int val[5], vallen; + unsigned long long ts; + int c; + int count = 0; + + while ((od = OSPDaemon_queue_get(&out->q)) != NULL) { + count++; + vallen = extractOSP(out->type, od, &ts, val); + DBG(DEBUG_OUTDRIVER,"Sending (FILE): %i %i %i fd = %i\n", + val[0], val[1], val[2], out->fd); + switch(out->ResultDesc.SensorType) { + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + c = snprintf(line, 79, "%i, %i, %i, %i, %lli\n", + val[0], val[1], val[2], val[3], ts); + break; + default: + c = snprintf(line, 79, "%i, %i, %i, %lli\n", + val[0], val[1], val[2], ts); + break; + } + if (out->fd >= 0) + write(out->fd, line, c); + if (count > 5) break; /* Limit writes */ + } + + if (od == NULL) return 0; else return 1; +} + +static struct OSPDaemon_driver ReadCSV = { + .driver = DRIVER_FILECSV, + .drvtype = DRIVER_TYPE_INPUT, + .setup_in= OSPDaemon_filecsv_setup, + .read = OSPDaemon_filecsv_read, +}, WriteCSV = { + .driver = DRIVER_FILECSV, + .drvtype = DRIVER_TYPE_OUTPUT, + .setup_out = OSPDaemon_writecsv_setup, + .send = OSPDaemon_writecsv_send, +}; + +void OSPDaemon_filecsv_init(void) +{ + OSPDaemon_driver_register(&ReadCSV); + OSPDaemon_driver_register(&WriteCSV); +} diff --git a/linux/daemon/OSPDaemon_filecsv.h b/linux/daemon/OSPDaemon_filecsv.h new file mode 100644 index 0000000..eae9e25 --- /dev/null +++ b/linux/daemon/OSPDaemon_filecsv.h @@ -0,0 +1,5 @@ +#ifndef _OSPDAEMON_FILECSV_H_ +#define _OSPDAEMON_FILECSV_H_ +#include "OSPDaemon.h" + +#endif diff --git a/linux/daemon/OSPDaemon_iio.c b/linux/daemon/OSPDaemon_iio.c new file mode 100644 index 0000000..b0ebfbb --- /dev/null +++ b/linux/daemon/OSPDaemon_iio.c @@ -0,0 +1,528 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * + * Licensed under Apache Public License. + */ + +/* + * Driver to read data from an IIO device. + * For now, it assumes the IIO device done in the form of the + * OSP IIO drivers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_iio.h" +#include "OSPDaemon_driver.h" + +#define IIO_DEVICE_DIR "/sys/bus/iio/devices" + +#define DBGOUT(x...) DBG(DEBUG_INDRIVER, "IIO: "x) + +static struct IIO_Sensor IIOSen[MAX_SENSOR]; + +/* sysfs support */ +static void sysfs_write_val(const char *path, const int val) +{ + FILE *f; + + f = fopen(path, "w"); + if (f) { + fprintf(f, "%i", val); + fclose(f); + } +} +static void sysfs_write_str(const char *path, const char *str) +{ + FILE *f; + + f = fopen(path, "w"); + if (f) { + fprintf(f, "%s", str); + fclose(f); + } +} +/* -------------- */ +/* Parse a IIO type file */ +static void parse_type(const char *path, struct DataDesc *dd) +{ + FILE *f; + char desc[512]; + char *ptr = NULL, *ptr2; + + DBGOUT("Parsing %s\n", path); + f = fopen(path, "r"); + if (f) { + ptr = fgets(desc, 512, f); + fclose(f); + } + if (ptr) { + /* le:s64/64>>0 */ + /* Ignore endianness for now */ + if (desc[3] == 's') { + dd->sign = 1; + } else { + dd->sign = 0; + } + + dd->size = strtol(desc+4, &ptr, 0); + dd->store = strtol(ptr+1, &ptr2, 0); + if (*ptr2 == '>') { + dd->shift = strtol(ptr2+2, NULL, 0); + } else { + dd->shift = 0; + } + } +} +/* Expects: + * in_NAME_AXIS_misc + */ +static const char *tsname = "timestamp"; + +static int parse_axis(const char *name) +{ + int axis = axis_invalid; + int i, j; + + if (name[0] == 'i' && name[1] == 'n' && name[2] == '_') { + for (i = 3, j = 0; name[i] != '\0' && i < 1024; i++, j++) { + if (name[i] == '_') break; + if (j < 2048) { + if (name[i] != tsname[j]) + j = 2048; + } + } + if (j < 2048) + axis = axis_timestamp; + else if (name[i] == '_') { + i++; + switch (name[i]) { + case 'x': + axis = axis_x; + break; + case 'y': + axis = axis_y; + break; + case 'z': + axis = axis_z; + break; + case 'r': + axis = axis_r; + break; + } + } + } + + return axis; +} + +/* + * Parse the index file. + */ +static int parse_index(const char *fname) +{ + FILE *f; + char desc[1024]; + char *ptr = NULL; + int idx = -1; + + f = fopen(fname, "r"); + if (f) { + ptr = fgets(desc, 1024, f); + desc[1023] = '\0'; + fclose(f); + } + if (ptr) { + idx = atoi(desc); + } + return idx; +} + +static OSP_InputSensorData_t * parse_iiodata(struct IIO_Sensor *is, char *buf, int len) +{ + int i, ax; + union { + uint8_t b[8]; + uint16_t s[4]; + uint32_t w[2]; + uint64_t ll[1]; + int8_t sb[8]; + int16_t ss[4]; + int32_t sw[2]; + int64_t sll[1]; + } conf; + int val[6]; + unsigned long long ts = 0; + double outval; + + for (i = 0; i < MAX_AXIS; i++) { + ax = is->index2axis[i]; + if (ax < 0) continue; + conf.w[0] = 0; conf.w[1] = 0; + + switch (is->IIOAxis[ax].dd.size) { + case 8: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sb[0]; + else + outval = conf.b[0]; + break; + case 16: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.ss[0]; + else + outval = conf.s[0]; + break; + case 32: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + conf.b[2] = buf[is->IIOAxis[ax].offset+2]; + conf.b[3] = buf[is->IIOAxis[ax].offset+3]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sw[0]; + else + outval = conf.w[0]; + break; + case 64: + conf.b[0] = buf[is->IIOAxis[ax].offset]; + conf.b[1] = buf[is->IIOAxis[ax].offset+1]; + conf.b[2] = buf[is->IIOAxis[ax].offset+2]; + conf.b[3] = buf[is->IIOAxis[ax].offset+3]; + conf.b[4] = buf[is->IIOAxis[ax].offset+4]; + conf.b[5] = buf[is->IIOAxis[ax].offset+5]; + conf.b[6] = buf[is->IIOAxis[ax].offset+6]; + conf.b[7] = buf[is->IIOAxis[ax].offset+7]; + if (is->IIOAxis[ax].dd.sign) + outval = conf.sll[0]; + else + outval = conf.ll[0]; + break; + default: + outval = 0.0; + } +#if 0 + if (is->IIOAxis[ax].dd.shift) + outval /= (double)(1<IIOAxis[ax].dd.shift); +#endif +#if 0 + if (ax == axis_timestamp) + printf("%s = 0x%llx | ", axis_name[ax], conf.ll[0]); + else + printf("%s = %08f | ", axis_name[ax], outval); +#else + /* DANGEROUS CODE. ASSUMES LAYOUT DETAILS */ + switch (ax) { + case axis_x: + val[0] = outval; + break; + case axis_y: + val[1] = outval; + break; + case axis_z: + val[2] = outval; + break; + case axis_r: + val[3] = outval; + break; + case axis_timestamp: + ts = conf.ll[0]; + break; + } +#endif + } + conv2OSP(is->type, &is->data, ts, val); + return &is->data; +} + +/* + * Parse contents of scan elements directory. + * Enable all elements. + */ +static void setupiio(struct IIO_Sensor *is) +{ + char dname[PATH_MAX]; + char fname[PATH_MAX]; + DIR *d; + struct dirent *dent; + int len, ax; + + snprintf(dname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements", is->iionum); + + d = opendir(dname); + if (d == NULL) return; + + while((dent = readdir(d)) != NULL) { + if (dent->d_name[0] == '.') continue; + len = strlen(dent->d_name); + if (len < 6) continue; + + ax = parse_axis(dent->d_name); + if (ax < 0) continue; + + if (strcmp(dent->d_name + len - 5, "_type") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + + parse_type(fname, &is->IIOAxis[ax].dd); + DBGOUT("%s - %i, %i\n", fname, + is->IIOAxis[ax].dd.store, ax); + } else if (strcmp(dent->d_name + len - 3, "_en") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + sysfs_write_val(fname, 1); + } else if (strcmp(dent->d_name + len - 6, "_index") == 0) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/scan_elements/%s", is->iionum, dent->d_name); + is->IIOAxis[ax].index = parse_index(fname); + is->index2axis[is->IIOAxis[ax].index] = ax; + } + } + + closedir(d); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/trigger/current_trigger", is->iionum); + DBGOUT("setting trigger name %s to %s\n", is->name, fname); + sysfs_write_str(fname, is->name); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/length", is->iionum); + sysfs_write_val(fname, 100); + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/buffer/enable", is->iionum); + sysfs_write_val(fname, 1); +} +/* + * Find the IIO device number associate with the named sensor + */ +static int getiionum(const char *sname) +{ + int i, j; + FILE *f; + char fname[PATH_MAX]; + char name[1024]; + + for (i = 0; i < MAX_IIO; i++) { + snprintf(fname, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/name", i); + fname[PATH_MAX-1] = '\0'; + + f = fopen(fname, "r"); + if (f == NULL) continue; + if (fgets(name, 1023, f) != NULL) { + name[1023] = '\0'; + for (j = 0; j < 1024 && name[j] != '\0'; j++) { + if (name[j] == '\n' || + name[j] == '\r') { + name[j] = '\0'; + break; + } + } + } + fclose(f); + if (strcmp(name, sname) == 0) + return i; + } + return -1; +} + +static void OSPDaemon_iio_SetState(struct IIO_Sensor *s, int state) +{ + char name[PATH_MAX+1]; + + if (state != 0) state = 1; + name[PATH_MAX] = '\0'; + snprintf(name, PATH_MAX, IIO_DEVICE_DIR"/iio:device%i/enable", + s->iionum); + + sysfs_write_val(name, state); +} + +static int OSPDaemon_iio_enable(struct OSPDaemon_SensorDetail *s) +{ + struct IIO_Sensor *is; + + is = s->private; + OSPDaemon_iio_SetState(is, 1); + + return 0; +} + +static int OSPDaemon_iio_disable(struct OSPDaemon_SensorDetail *s) +{ + struct IIO_Sensor *is; + + is = s->private; + OSPDaemon_iio_SetState(is, 0); + + return 0; +} + +static int OSPDaemon_iio_create(const char *name, struct IIO_Sensor *s) +{ + int rec_sz; + int i; + int fd; + char dname[PATH_MAX]; + + s->iionum = getiionum(name); + if (s->iionum < 0) return -1; + DBGOUT("HY-DBG: %s:%i - IIOnum = %i\n", __func__, __LINE__, s->iionum); + s->name = name; + setupiio(s); + + rec_sz = 0; + for (i = 0; i < MAX_AXIS; i++) { + if (s->IIOAxis[i].index >= 0) { + if (s->IIOAxis[i].dd.store == 0) continue; + + /* Make sure each group is aligned */ + DBGOUT("1: rec_sz = %i, s->IIOAxis[i].dd.store = %i, %i,\n", rec_sz, s->IIOAxis[i].dd.store, i); + if (rec_sz % (s->IIOAxis[i].dd.store/8) != 0) { + rec_sz += ((s->IIOAxis[i].dd.store/8)-(rec_sz % (s->IIOAxis[i].dd.store/8))); + } + s->IIOAxis[i].offset = rec_sz; + rec_sz += s->IIOAxis[i].dd.store/8; + DBGOUT("2: rec_sz = %i\n", rec_sz); + } + } + s->rec_sz = rec_sz; + + snprintf(dname, PATH_MAX, "/dev/iio:device%i", s->iionum); + + fd = open(dname, O_RDONLY); + if (!disablepm) { + OSPDaemon_iio_SetState(s, 0); + } + return fd; +} + +static int OSPDaemon_iioevent_create(const char *name, struct IIO_Sensor *s) +{ + int fd, evfd; + char dname[PATH_MAX]; + int ret; + + s->iionum = getiionum(name); + if (s->iionum < 0) return -1; + DBGOUT("HY-DBG: %s:%i - IIOnum = %i\n", __func__, __LINE__, s->iionum); + s->name = name; + + snprintf(dname, PATH_MAX, "/dev/iio:device%i", s->iionum); + + fd = open(dname, O_RDONLY); + /* Can the main fd be closed? */ + ret = ioctl(fd, IIO_GET_EVENT_FD_IOCTL, &evfd); + return evfd; +} + +static int OSPDaemon_iio_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i, j; + int iiocount = 0; + for (i = 0; i < count; i++) { + DBGOUT("Driver: %i, count = %i\n", s->driver, count); + if (s->driver == DRIVER_IIO) { + DBGOUT("IIO Driver: %i\n", s->driver); + for (j = 0; j < MAX_AXIS; j++) { + IIOSen[i].IIOAxis[j].index = -1; + IIOSen[i].index2axis[j] = -1; + } + DBGOUT("%s:%i Setting up: %s\n", __func__, __LINE__, s->name); + s->fd = OSPDaemon_iio_create(s->name, &IIOSen[i]); + s->private = &IIOSen[i]; + IIOSen[i].type = s->sensor.SensorType; + if (s->fd < 0) { + fprintf(stderr, "IIO: Failed on %s\n", s->name); + } + iiocount++; + } else if (s->driver == DRIVER_IIOEVENT) { + DBGOUT("IIO Event Driver: %i\n", s->driver); + s->fd = OSPDaemon_iioevent_create(s->name, &IIOSen[i]); + s->private = &IIOSen[i]; + iiocount++; + } + s++; + } + DBGOUT("IIO: Found %i iio requests\n", iiocount); + + return 0; +} + +static int OSPDaemon_iio_read(struct OSPDaemon_SensorDetail *s) +{ + char readbuf[2048]; + struct IIO_Sensor *is; + int ret, used; + OSP_InputSensorData_t *od; + + is = s->private; + + ret = read(s->fd, readbuf, 2048); + + if (ret > 0) { + used = 0; + do { + od = parse_iiodata(is, readbuf+used, ret-used); + if (od != NULL) { + DBGOUT("Got data for %s\n", is->name); + OSPDaemon_queue_put(&s->q, od); + DBGOUT("Queued data for %s\n", is->name); + } + ret -= is->rec_sz; + used += is->rec_sz; + } while (ret > 0); + } else { + DBGOUT("Read error for fd %i (errno = %i)\n", s->fd, errno); + } + + DBGOUT("Finish reading data for %s\n", is->name); + return 0; +} + +static int OSPDaemon_iioevent_read(struct OSPDaemon_SensorDetail *s) +{ + int ret; + struct iio_event_data event; + struct IIO_Sensor *is; + + is = s->private; + + ret = read(s->fd, &event, sizeof(event)); + if (ret < 0) { + DBGOUT("Error on event channel read %i\n", errno); + } else { + is->data.booldata.TimeStamp = event.timestamp; + is->data.booldata.data = 1; + OSPDaemon_queue_put(&s->q, &is->data); + } + + return 0; +} +static struct OSPDaemon_driver IIODriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_IIO, + .setup_in = OSPDaemon_iio_setup, + .read = OSPDaemon_iio_read, + .enable_in = OSPDaemon_iio_enable, + .disable_in = OSPDaemon_iio_disable, +}, IIOEventDriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_IIOEVENT, + .setup_in = NULL, + .read = OSPDaemon_iioevent_read, +}; + + +void OSPDaemon_iio_init(void) +{ + OSPDaemon_driver_register(&IIODriver); + OSPDaemon_driver_register(&IIOEventDriver); +} diff --git a/linux/daemon/OSPDaemon_iio.h b/linux/daemon/OSPDaemon_iio.h new file mode 100644 index 0000000..c9b4208 --- /dev/null +++ b/linux/daemon/OSPDaemon_iio.h @@ -0,0 +1,43 @@ +#ifndef _OSPDAEMON_IIO_H_ +#define _OSPDAEMON_IIO_H_ +#include "OSPDaemon.h" + +#define MAX_AXIS 5 +#define MAX_IIO 99 +#define MAX_SENSOR 10 + +#define IIO_DEVICE_DIR "/sys/bus/iio/devices" + +enum { + axis_x, + axis_y, + axis_z, + axis_r, + axis_timestamp, + axis_invalid +}; + +struct DataDesc { + int sign; + int size; + int store; + int shift; +}; + +struct IIO_SensorAxis { + struct DataDesc dd; + int index; + int offset; +}; + +struct IIO_Sensor { + struct IIO_SensorAxis IIOAxis[MAX_AXIS]; + OSP_InputSensorData_t data; + int index2axis[MAX_AXIS]; + int iionum; + int rec_sz; + char const *name; + int type; +}; + +#endif diff --git a/linux/daemon/OSPDaemon_input.c b/linux/daemon/OSPDaemon_input.c new file mode 100644 index 0000000..6156c3a --- /dev/null +++ b/linux/daemon/OSPDaemon_input.c @@ -0,0 +1,211 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License + */ + +/* + * Driver for input events OUTPUT. + */ + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "OSPDaemon_driver.h" + +#define DBGOUT(x...) DBG(DEBUG_INDRIVER, "INPUT: "x) + +static int OSPDaemon_input_create(struct OSPDaemon_output *out) +{ + struct uinput_user_dev uindev; + int fd; + int ret; + + DBGOUT("%s:%i\n", __func__, __LINE__); + + fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (fd < 0) { + ret = fd; + goto error; + } + memset(&uindev, 0, sizeof(uindev)); + + strncpy(uindev.name, out->name, UINPUT_MAX_NAME_SIZE); + uindev.id.bustype = BUS_HOST; + uindev.id.vendor = 0; + uindev.id.product = 0; + uindev.id.version = 1; + uindev.absmin[ABS_X] = 0; + uindev.absmax[ABS_X] = 1023; + uindev.absmin[ABS_Y] = 0; + uindev.absmax[ABS_Y] = 1023; + uindev.absmin[ABS_Z] = 0; + uindev.absmax[ABS_Z] = 1023; + uindev.absmin[ABS_WHEEL] = 0; + uindev.absmax[ABS_WHEEL] = 1023; + + ret = write(fd, &uindev, sizeof(uindev)); + if (ret < 0) { + perror("uindev write"); + goto error; + } + + ret = ioctl(fd, UI_SET_EVBIT, EV_ABS); + if (ret < 0) { + perror("ioctl EV_ABS EVBIT"); + goto error; + } + ret = ioctl(fd, UI_SET_EVBIT, EV_SYN); + if (ret < 0) { + perror("ioctl EV_SYN EVBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_X); + if (ret < 0) { + perror("ioctl ABS_X ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_Y); + if (ret < 0) { + perror("ioctl ABS_Y ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_Z); + if (ret < 0) { + perror("ioctl ABS_Z ABSBIT"); + goto error; + } + if (out->option & INPUT_OPTION_EMBEDTS) { + ret = ioctl(fd, UI_SET_ABSBIT, ABS_THROTTLE); + if (ret < 0) { + perror("ioctl ABS_THROTTLE ABSBIT"); + goto error; + } + + ret = ioctl(fd, UI_SET_ABSBIT, ABS_RUDDER); + if (ret < 0) { + perror("ioctl ABS_RUDDER ABSBIT"); + goto error; + } + } + if (out->option & INPUT_OPTION_QUAT4) { + ret = ioctl(fd, UI_SET_ABSBIT, ABS_WHEEL); + if (ret < 0) { + perror("ioctl ABS_WHEEL ABSBIT"); + goto error; + } + } + + ret = ioctl(fd, UI_DEV_CREATE); + if (ret < 0) { + perror("ioctl CREATE"); + goto error; + } + out->fd = fd; + +error: + if (fd < 0) { + fprintf(stderr, "INPUT: Failed on %s\n", out->name); + } + return ret; +} + +static int OSPDaemon_input_setup(struct OSPDaemon_output *out, int count) +{ + int i; + + for (i = 0; i < count; i++) { + if (out[i].driver == DRIVER_INPUT) + OSPDaemon_input_create(&out[i]); + } + + return 0; +} + +static int OSPDaemon_input_send(struct OSPDaemon_output *out) +{ + OSP_InputSensorData_t *od; + struct input_event ev; + int ret; + static int noise=0; + int val[5], vallen; + unsigned long long ts; + + while ((od = OSPDaemon_queue_get(&out->q)) != NULL) { + vallen = extractOSP(out->type, od, &ts, val); + DBGOUT("Sending (INPUT) %i %i %i\n", val[0], val[1], val[2]); + if (out->option & INPUT_OPTION_DITHER) { + noise++; + noise %= 2; + } + ev.type = EV_ABS; + ev.code = ABS_X; + ev.value = val[0]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + + ev.type = EV_ABS; + ev.code = ABS_Y; + ev.value = val[1]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + + ev.type = EV_ABS; + ev.code = ABS_Z; + ev.value = val[2]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if (ret <= 0) return 1; + if (vallen > 3 && out->option & INPUT_OPTION_QUAT4) { + ev.type = EV_ABS; + ev.code = ABS_WHEEL; /* ??? */ + ev.value = val[3]+noise; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } + if (out->option & INPUT_OPTION_EMBEDTS) { + /* Upper 32 bit */ + ev.type = EV_ABS; + ev.code = ABS_RUDDER; /* ??? */ + ev.value = (ts >> 32); + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + /* Lower 32 bit */ + ev.type = EV_ABS; + ev.code = ABS_THROTTLE; + ev.value = (ts & ((((unsigned long long)1)<<32)-1)); + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } + + ev.type = EV_SYN; + ev.code = 0; + ev.value = 0; + ret = write(out->fd, &ev, sizeof(ev)); + if ( ret <= 0) return 1; + } + if (od == NULL) return 0; else return 1; + return 0; +} + +static struct OSPDaemon_driver InputDriver = { + .drvtype = DRIVER_TYPE_OUTPUT, + .driver = DRIVER_INPUT, + .setup_out = OSPDaemon_input_setup, + .send = OSPDaemon_input_send, +}; + +void OSPDaemon_input_init(void) +{ + OSPDaemon_driver_register(&InputDriver); +} diff --git a/linux/daemon/OSPDaemon_inputreader.c b/linux/daemon/OSPDaemon_inputreader.c new file mode 100644 index 0000000..f5e71c9 --- /dev/null +++ b/linux/daemon/OSPDaemon_inputreader.c @@ -0,0 +1,132 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License. + */ + +/* + * Driver for reading data from an input event device. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_inputreader.h" +#include "OSPDaemon_driver.h" + +#define INPUT_EVENT_DIR "/dev/input" + +struct RawBuf { + int val[5]; + unsigned long long ts; +} databuf[MAX_SENSOR]; + +static int OSPDaemon_inputreader_create(char const *name) +{ + int i; + int fd = -1; + char dname[PATH_MAX]; + char evname[80]; + int ret; + + for (i = 0; i < 99; i++) { + snprintf(dname, PATH_MAX, INPUT_EVENT_DIR"/event%i", i); + fd = open(dname, O_RDONLY); + if (fd < 0) continue; + ret = ioctl(fd, EVIOCGNAME(sizeof(evname)-1), &evname); + if (ret < 0) { + close(fd); + continue; + } + close(fd); + if (strncmp(evname, name, 80) == 0) { + break; + } + fd = -1; + } + + return fd; +} + +static int OSPDaemon_inputreader_setup(struct OSPDaemon_SensorDetail *s, int count) +{ + int i; + int ircount = 0, irsuccess = 0; + DBG(DEBUG_INDRIVER, "InputReader %s:%i\n", __func__, __LINE__); + + for (i = 0; i < count; i++) { + if (s->driver == DRIVER_INPUT) { + s->fd = OSPDaemon_inputreader_create(s->name); + ircount++; + if (s->fd > 0) { + irsuccess++; + s->private = &databuf[i]; + } + } + s++; + } + DBG(DEBUG_INDRIVER, "InputReader: %i request, %i success\n", + ircount, irsuccess); + + return 0; +} + +#define NUM_EVENT 10 + +static int OSPDaemon_inputreader_read(struct OSPDaemon_SensorDetail *s) +{ + int ret; + struct input_event iev[NUM_EVENT]; + int i; + OSP_InputSensorData_t od; + struct RawBuf *d; + int dirty = 0; + + ret = read(s->fd, &iev, sizeof(struct input_event)*NUM_EVENT); + if (ret < 0) return -1; + for (i = 0; i < NUM_EVENT; i++) { + if (sizeof(struct input_event)*i > ret) + break; + if (iev[i].type == EV_REL || iev[i].type == EV_ABS) { + d = s->private; + switch (iev[i].code) { + case ABS_X: + d->val[0] = iev[i].value; + break; + case ABS_Y: + d->val[1] = iev[i].value; + break; + case ABS_Z: + d->val[2] = iev[i].value; + break; + /* Missing Quat R case */ + } + } else if (iev[i].type == EV_SYN) { + d = s->private; + d->ts = iev[i].time.tv_usec; + conv2OSP(s->sensor.SensorType, &od, d->ts, d->val); + OSPDaemon_queue_put(&s->q, &od); + dirty++; + } + } + return dirty; +} +static struct OSPDaemon_driver InputReaderDriver = { + .drvtype = DRIVER_TYPE_INPUT, + .driver = DRIVER_INPUT, + .setup_in = OSPDaemon_inputreader_setup, + .read = OSPDaemon_inputreader_read, +}; + +void OSPDaemon_inputreader_init(void) +{ + OSPDaemon_driver_register(&InputReaderDriver); +} diff --git a/linux/daemon/OSPDaemon_inputreader.h b/linux/daemon/OSPDaemon_inputreader.h new file mode 100644 index 0000000..edfab19 --- /dev/null +++ b/linux/daemon/OSPDaemon_inputreader.h @@ -0,0 +1,8 @@ +#ifndef _OSPDAEMON_INPUTREADER_H_ +#define _OSPDAEMON_INPUTREADER_H_ +#include "OSPDaemon.h" + +#define MAX_AXIS 5 +#define MAX_SENSOR 10 + +#endif diff --git a/linux/daemon/OSPDaemon_pm.c b/linux/daemon/OSPDaemon_pm.c new file mode 100644 index 0000000..cfde971 --- /dev/null +++ b/linux/daemon/OSPDaemon_pm.c @@ -0,0 +1,112 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience, Inc. + * + * Apache Licensed. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_driver.h" +#include "OSPDaemon_pm.h" + +#define DBGOUT(x...) DBG(DEBUG_PM, "PM: "x); + +int OSPDaemon_power_init(struct OSPDaemon_SensorDescription *sd) +{ + int fd; + int ret; + int i; + char fname[PATH_MAX+1]; + + if (sd->powerPath == NULL) { + DBGOUT("No power path defined. Disable PM.\n"); + disablepm = 1; + return -1; + } + + /* Clear out the directory */ + for (i = 0; i < sd->output_count; i++) { + snprintf(fname, PATH_MAX, "%s/%s", + sd->powerPath, sd->output[i].name); + unlink(fname); + } + + fd = inotify_init(); + if (fd < 0) { + DBGOUT("inotify_init failed\n"); + return fd; + } + ret = inotify_add_watch(fd, sd->powerPath, IN_CREATE | IN_DELETE); + if (ret < 0) { + mkdir(sd->powerPath, 0777); + ret = inotify_add_watch(fd, sd->powerPath, + IN_CREATE | IN_DELETE); + if (ret < 0) { + DBGOUT("inotify_add_watch failed\n"); + close(fd); + fd = -1; + } + } + DBGOUT("Successfully added inotify watch for PM using %s\n", sd->powerPath); + + return fd; +} + +static void pm_process_output(struct OSPDaemon_output *out, int mask) +{ + if (mask & IN_CREATE) { + /* Enable sensor */ + out->enable = 1; + OSPDaemon_driver_enable_out(out); + if (!out->noprocess) { + OSP_SubscribeSensorResult(&out->ResultDesc, + &out->handle); + DBGOUT("Subscribe"); + } else + OSPDaemon_driver_enable_in(out->source); + DBGOUT("Enabling %s\n", out->name); + } else if (mask & IN_DELETE) { + /* Disable sensor */ + out->enable = 0; + OSPDaemon_driver_disable_out(out); + if (!out->noprocess) { + OSP_UnsubscribeSensorResult(out->handle); + DBGOUT("UnSubscribe"); + } else + OSPDaemon_driver_disable_in(out->source); + DBGOUT("Disabling %s\n", out->name); + } +} + +int OSPDaemon_power_process(struct OSPDaemon_SensorDescription *sd, int fd) +{ + int i; + int ret; + char buf[2048], *ptr; + struct inotify_event *inptr; + + ret = read(fd, buf, 2048); + if (ret <= 0) + return 0; + ptr = buf; + do { + inptr = (struct inotify_event *)ptr; + for (i = 0; i < sd->output_count; i++) { + if (strcmp(sd->output[i].name, inptr->name) == 0) { + printf("Power event found 0x%x\n", inptr->mask); + pm_process_output(&sd->output[i], inptr->mask); + break; + } + } + ptr += (sizeof(struct inotify_event)+inptr->len); + } while(ptr < (buf+ret)); + + return 0; +} diff --git a/linux/daemon/OSPDaemon_pm.h b/linux/daemon/OSPDaemon_pm.h new file mode 100644 index 0000000..5fa73c5 --- /dev/null +++ b/linux/daemon/OSPDaemon_pm.h @@ -0,0 +1,6 @@ +#ifndef _OSPDAEMON_PM_H_ +#define _OSPDAEMON_PM_H_ + +int OSPDaemon_power_init(struct OSPDaemon_SensorDescription *); +int OSPDaemon_power_process(struct OSPDaemon_SensorDescription *, int); +#endif diff --git a/linux/daemon/OSPDaemon_queue.c b/linux/daemon/OSPDaemon_queue.c new file mode 100644 index 0000000..44f825e --- /dev/null +++ b/linux/daemon/OSPDaemon_queue.c @@ -0,0 +1,58 @@ +/* + * (C) Copyright 2015 HY Research LLC for Audience. + * Licensed under Apache Public License + */ + +/* + * Queue management routes. + */ + +#include +#include + +#include "OSPDaemon.h" +#include "OSPDaemon_queue.h" +#include "osp-api.h" +int OSPDaemon_queue_init(struct queue *q) +{ + if (q == NULL) + return -1; + + q->write = 0; q->read = 0; + + return 0; +} + +/* Data valid til next enqueue op */ +OSP_InputSensorData_t *OSPDaemon_queue_get(struct queue *q) +{ + OSP_InputSensorData_t *d; + + if (q == NULL) return NULL; + + if (q->write == q->read) return NULL; + d = &(q->data[q->read]); + q->read++; + q->read %= QUEUE_LEN; + return d; +} +/* d can be destory after this call */ +int OSPDaemon_queue_put(struct queue *q, OSP_InputSensorData_t *d) +{ + if ((q->write+1)%QUEUE_LEN == q->read) return -1; + memcpy(&(q->data[q->write]), d, sizeof(OSP_InputSensorData_t)); + q->write++; + q->write %= QUEUE_LEN; + + return 0; +} + + +/* Checks a queue for emptyness. + * 1 - empty + * 0 - there is data. + */ +int OSPDaemon_queue_isempty(struct queue *q) +{ + return (q->write == q->read)?1:0; +} diff --git a/linux/daemon/OSPDaemon_queue.h b/linux/daemon/OSPDaemon_queue.h new file mode 100644 index 0000000..79cae45 --- /dev/null +++ b/linux/daemon/OSPDaemon_queue.h @@ -0,0 +1,20 @@ +#ifndef _OSPDAEMON_QUEUE_H_ +#define _OSPDAEMON_QUEUE_H_ 1 + +#define QUEUE_LEN 25 +#include "osp-api.h" +struct queue { + int write; + int read; + OSP_InputSensorData_t data[QUEUE_LEN]; +}; + +int OSPDaemon_queue_init(struct queue *q); + +/* Pointer to data is valid til the next queue op */ +OSP_InputSensorData_t *OSPDaemon_queue_get(struct queue *q); +/* Pointer to data only needs to be valid during this call. */ +int OSPDaemon_queue_put(struct queue *q, OSP_InputSensorData_t *d); +int OSPDaemon_queue_isempty(struct queue *q); + +#endif diff --git a/linux/driver/MQ_sensors.h b/linux/driver/MQ_sensors.h deleted file mode 100644 index c789428..0000000 --- a/linux/driver/MQ_sensors.h +++ /dev/null @@ -1,232 +0,0 @@ -/**************************************************************************************************** - * @file MQ_sensors.h - * - * Sensors related enumerations - * - * < Detailed description of subsystem > - * - * @seealso < Links to relevant files, references, etc. > - * - * @info - * $Author: rverma $ - * $DateTime: 2014/10/16 15:32:24 $ - * $Revision: #4 $ - * $Id: //AudEngr/Hardware/DeltaPlus/Software/BoskoApp_D300_SensorHub/include/MQ_sensors.h#4 $ - * - * Copyright 2012 Audience, Incorporated. All rights reserved. - * - * This software is the confidential and proprietary information of - * Audience Inc. ("Confidential Information"). You shall not disclose - * such Confidential Information and shall use it only in accordance - * with the Terms of Sale of Audience products and the terms of any - * license agreement you entered into with Audience for such products. - * - * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY - * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, - * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE - * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR - * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE - * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF - * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. - * - ***************************************************************************************************/ -#if !defined (MQ_SENSORS_H) -#define MQ_SENSORS_H - -/*-------------------------------------------------------------------------------------------------*\ - | I N C L U D E F I L E S -\*-------------------------------------------------------------------------------------------------*/ -/* This file is meant to provide a common definition of sensor related enumerations/defines and - * generally should not depend on any other includes - */ - -/*-------------------------------------------------------------------------------------------------*\ - | C O N S T A N T S & M A C R O S -\*-------------------------------------------------------------------------------------------------*/ -#define SENSOR_SUBTYPE_UNUSED 0 //!< Subtype is not used for the sensor type -#define SENSOR_SUBTYPE_START 1 //!< Subtype enumeration starts with 1 -#define SENSOR_DEVICE_PRIVATE_BASE 0x10000 //!< Android defined private sensor type base - -#define M_PSensorToAndroidBase(type) ((type) | SENSOR_DEVICE_PRIVATE_BASE) - - -/*-------------------------------------------------------------------------------------------------*\ - | T Y P E D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ -//! use to specify the kind of sensor input or output -/*! - * \sa OSP_RegisterInputSensor - * \sa OSP_SubscribeOutputSensor - * - * Final units of input/outputs are defined by the sensor data convention field of the sensor descriptor. - * Flags in the descriptor specify if sensor is calibrated/uncalibrated and/or used as input - * If a sensor type not is supported by the library implementation, an error will be returned on its usage - */ -/* Android sensor types - this should match the defines/enumeration in Android's sensors.h */ -/* Note that these are defined as SENSOR_ instead of SENSOR_TYPE_ to avoid clash with Android defines in - * situations where this header in included for packet processing in Sensor HAL - */ -typedef enum _ASensorType { - SENSOR_META_DATA = 0, - SENSOR_ACCELEROMETER = 1, - SENSOR_GEOMAGNETIC_FIELD = 2, - SENSOR_MAGNETIC_FIELD = SENSOR_GEOMAGNETIC_FIELD, - SENSOR_ORIENTATION = 3, - SENSOR_GYROSCOPE = 4, - SENSOR_LIGHT = 5, - SENSOR_PRESSURE = 6, - SENSOR_TEMPERATURE = 7, - SENSOR_PROXIMITY = 8, - SENSOR_GRAVITY = 9, - SENSOR_LINEAR_ACCELERATION = 10, - SENSOR_ROTATION_VECTOR = 11, - SENSOR_RELATIVE_HUMIDITY = 12, - SENSOR_AMBIENT_TEMPERATURE = 13, - SENSOR_MAGNETIC_FIELD_UNCALIBRATED = 14, - SENSOR_GAME_ROTATION_VECTOR = 15, - SENSOR_GYROSCOPE_UNCALIBRATED = 16, - SENSOR_SIGNIFICANT_MOTION = 17, - SENSOR_STEP_DETECTOR = 18, - SENSOR_STEP_COUNTER = 19, - SENSOR_GEOMAGNETIC_ROTATION_VECTOR = 20, - - NUM_ANDROID_SENSOR_TYPE //!< Total number of Android sensor type -} ASensorType_t; - -/* Private Sensor types (translates to Android SENSOR_TYPE_DEVICE_PRIVATE_BASE start) */ -typedef enum _PSensorType { - PSENSOR_ENUM_FIRST_SENSOR = 0, - - PSENSOR_DEBUG_TUNNEL = PSENSOR_ENUM_FIRST_SENSOR, //!< Debug message pipe to host - PSENSOR_ACCELEROMETER_RAW = 1, //!< raw accelerometer data (direct from sensor) - PSENSOR_MAGNETIC_FIELD_RAW = 2, //!< magnetometer data (direct from sensor) - PSENSOR_GYROSCOPE_RAW = 3, //!< calibrated gyroscope data (direct from sensor) - PSENSOR_LIGHT_UV = 4, //!< UV light sensor data (Android Units) - PSENSOR_LIGHT_RGB = 5, //!< RGB light sensor data (Android Units) - PSENSOR_STEP = 6, //!< step data - PSENSOR_ACCELEROMETER_UNCALIBRATED = 7, //!< uncalibrated accelerometer data (Android Units) - PSENSOR_ORIENTATION = 8, //!< yaw, pitch, roll (also use this for Win8 Inclinometer) - PSENSOR_CONTEXT_DEVICE_MOTION = 9, //!< context of device relative to world frame - PSENSOR_CONTEXT_CARRY = 10, //!< context of device relative to user - PSENSOR_CONTEXT_POSTURE = 11, //!< context of user relative to world frame - PSENSOR_CONTEXT_TRANSPORT = 12, //!< context of environment relative to world frame - PSENSOR_GESTURE_EVENT = 13, //!< gesture event such as a double-tap or shake - PSENSOR_HEART_RATE = 15, //!< heart-rate data - SYSTEM_REAL_TIME_CLOCK = 16, //!< Real time clock used for time stamp - - NUM_PRIVATE_SENSOR_TYPE //!< Total number of Private sensor type -} PSensorType_t; - -/* Sensor Parameter Identifiers */ -typedef enum _SensorParamId { - SENSOR_PARAM_ERROR_CODE = 0, //!< Used to convey error code instead of parameter value - SENSOR_PARAM_OFFSET = 1, //!< Offset or bias of a sensor - SENSOR_PARAM_DATA_RATE = 2, //!< Datarate for the sensor - SENSOR_PARAM_BAND_WIDTH = 3, //!< Bandwidth setting for the sensor - SENSOR_PARAM_HP_FILTER = 4, //!< High Pass filter setting for the sensor - SENSOR_PARAM_LP_FILTER = 5, //!< Low Pass filter setting for the sensor - SENSOR_PARAM_ENABLE = 6, //!< Sensor Enable control - - NUM_SENSOR_PARAM -} SensorParamId_t; - - -//! Use these values as a sub-type for STEP result -typedef enum _StepSubType { - CONTEXT_STEP = SENSOR_SUBTYPE_START, //!< only one kind of step now - STEP_SEGMENT_DETECTOR, //!< low compute trigger for analyzing if step may have occured - - NUM_PSENSOR_STEP_SUBTYPE -} StepSubType_t; - -//! Use these values as a sub-type for CONTEXT_DEVICE_MOTION result -typedef enum _ContextDeviceMotionSubType { - CONTEXT_DEVICE_MOTION_STILL = SENSOR_SUBTYPE_START, - CONTEXT_DEVICE_MOTION_ACCELERATING, - CONTEXT_DEVICE_MOTION_ROTATING, - CONTEXT_DEIVCE_MOTION_TRANSLATING, - CONTEXT_DEVICE_MOTION_FREE_FALLING, - CONTEXT_DEVICE_MOTION_SIGNIFICANT_MOTION, //!< significant motion (as specified by Android HAL 1.0) - CONTEXT_DEVICE_MOTION_SIGNIFICANT_STILLNESS, //!< complement to significant motion - CONTEXT_DEVICE_MOTION_CHANGE_DETECTOR, //!< low compute trigger for seeing if context may have changed - - NUM_PSENSOR_CONTEXT_DEVICE_MOTION_SUBTYPE -} ContextDeviceMotionSubType_t; - -//! Use these values as a sub-type for CONTEXT_CARRY result -typedef enum _ContextCarrySubType { - CONTEXT_CARRY_IN_POCKET = SENSOR_SUBTYPE_START, - CONTEXT_CARRY_IN_HAND, - CONTEXT_CARRY_NOT_ON_PERSON, - CONTEXT_CARRY_IN_HAND_FRONT, - CONTEXT_CARRY_IN_HAND_SIDE, - - NUM_PSENSOR_CONTEXT_CARRY_SUBTYPE -} ContextCarrySubType_t; - -//! Use these values as a sub-type for CONTEXT_POSTURE result -typedef enum _ContextPostureSubType { - CONTEXT_POSTURE_WALKING = SENSOR_SUBTYPE_START, - CONTEXT_POSTURE_STANDING, - CONTEXT_POSTURE_SITTING, - CONTEXT_POSTURE_JOGGING, - CONTEXT_POSTURE_RUNNING, - - NUM_PSENSOR_CONTEXT_POSTURE_SUBTYPE -} ContextPostureSubType_t; - - -//! Use these values as a sub-type for CONTEXT_TRANSPORT result -typedef enum _ContextTransportSubType { - CONTEXT_TRANSPORT_VEHICLE = SENSOR_SUBTYPE_START, - CONTEXT_TRANSPORT_CAR, - CONTEXT_TRANSPORT_TRAIN, - CONTEXT_TRANSPORT_UP_STAIRS, - CONTEXT_TRANSPORT_DOWN_STAIRS, - CONTEXT_TRANSPORT_UP_ELEVATOR, - CONTEXT_TRANSPORT_DOWN_ELEVATOR, - CONTEXT_TRANSPORT_ON_BIKE, - - NUM_PSENSOR_CONTEXT_TRANSPORT_SUBTYPE -} ContextTransportSubType_t; - -//! Use these values as a sub-type for GESTURE_EVENT result -typedef enum _GestureSubType { - SENSOR_GESTURE_TAP = SENSOR_SUBTYPE_START, - SENSOR_GESTURE_DOUBLE_TAP, - SENSOR_GESTURE_SHAKE, - - NUM_PSENSOR_GESTURE_SUBTYPE -} GestureSubType_t; - -//! Time Stamp definition for capturing raw time counts -typedef union _TimeStamp { - uint64_t TS64; - uint32_t TS32[2]; - uint8_t TS8[8]; -} TimeStamp_t; - -//! Generic structure definition for a 3-axis sensor raw values in 2's complement format -typedef struct TriAxisRawData_t -{ - TimeStamp_t TStamp; - int32_t Axis[3]; -} TriAxisRawData_t; - -/*-------------------------------------------------------------------------------------------------*\ - | E X T E R N A L V A R I A B L E S & F U N C T I O N S -\*-------------------------------------------------------------------------------------------------*/ - -/*-------------------------------------------------------------------------------------------------*\ - | P U B L I C V A R I A B L E S D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ - -/*-------------------------------------------------------------------------------------------------*\ - | P U B L I C F U N C T I O N D E C L A R A T I O N S -\*-------------------------------------------------------------------------------------------------*/ - - -#endif /* MQ_SENSORS_H */ -/*-------------------------------------------------------------------------------------------------*\ - | E N D O F F I L E -\*-------------------------------------------------------------------------------------------------*/ diff --git a/linux/driver/README b/linux/driver/README new file mode 100644 index 0000000..bead904 --- /dev/null +++ b/linux/driver/README @@ -0,0 +1,63 @@ +Linux kernel driver for OSP sensor hub +-------------------------------------- +The driver is composed of 2 parts. A core OSP packet parsing driver +and a driver responsible for outputing to userland in the desire fashion. +The 2 parts communicate with a defined API. The userland driver attaches +as a child driver of the main OSP driver. + +The API between the 2 parts is defined by linux/osp-sh.h. The API +consist of a callback registration to claim output for a particular +sensor type along with an enable/disable call. Once registered, the call +back will be called wit the sensor data. + +Provided is 2 userland interface drivers. One provides data using the +input event system. The other driver provides it using the IIO subsystem. + +Installation +------------ +Copy the contents of the linux directory into include/linux on your +kernel tree. + +Place osp-sh.c in an appropriate place and modify the Makefile to build it. +It needs to be able to access the following header files. +MQ_sensors.h +SensorPackets.h +osp_i2c_map.h + +Connect the OSP I2C device with the osp-sh driver. The process will +depend on the version of the kernel. Currently, there is no specific +hooks for device tree. For pre-device tree kernels, the board file +will need to be modified or userland will need to manually instantiate +the device. + +For the IIO option: (only 1 can be used at a time) +Place osp-iio.c in the appropriate place for IIO drivers in your tree. This +is often under drivers/iio or drivers/staging/iio. Modify the Makefile to +pickup the driver. + +For the input event ouption: +Place osp-input.c in the appropriate place. For example, drivers/input/misc. +Modify the Makefile to pickup the driver. + +Notes: + +Input driver needs to be manually enabled using the enable_new attribute +associated with the device. + +IIO driver will automatically enable/disable with the triggers. However, +an additional control is available with the enable attribute. This allows +the HAL to enable/disable the driver without changing the poll descriptor. +[Is there a better way using triggers?] + +The IIO driver provides data in 2 ways. One is a general IIO channel. This +is used for sensors that report actual numeric data. For example, an +accelerometer or a step counter. The second way is using IIO events. +This is used for one shot sensors. The step detector and significant +motion are examples of events. A single IIO device can generate channel +data AND events if they are logically connected. For example, the step +counter driver outputs channel data for the count. It also reports step +detector data using events. Most IIO sensors can also report data +using sysfs attributes. This is for debugging and non buffered output. + +Each sensor is presented as a seperate device. They are identified by +name. diff --git a/linux/driver/SensorPackets.h b/linux/driver/SensorPackets.h index 7a309d6..9789963 100644 --- a/linux/driver/SensorPackets.h +++ b/linux/driver/SensorPackets.h @@ -1,7 +1,7 @@ -/**************************************************************************************************** +/***************************************************************************** * @file SensorPackets.h * - * Definitions and macros for host inteface packet format + * Definitions and macros for host interface packet format * * < Detailed description of subsystem > * @@ -9,104 +9,107 @@ * * @info * $Author: rverma $ - * $DateTime: 2014/11/13 17:24:40 $ - * $Revision: #8 $ - * $Id: //AudEngr/Hardware/DeltaPlus/Software/BoskoApp_D300_SensorHub/overlay/SensorHub/HostInterface/SensorPackets.h#8 $ + * $DateTime: 2015/06/26 11:03:37 $ + * $Revision: #4 $ * * Copyright 2012 Audience, Incorporated. All rights reserved. * * This software is the confidential and proprietary information of - * Audience Inc. ("Confidential Information"). You shall not disclose - * such Confidential Information and shall use it only in accordance - * with the Terms of Sale of Audience products and the terms of any + * Audience Inc. ("Confidential Information"). You shall not disclose + * such Confidential Information and shall use it only in accordance + * with the Terms of Sale of Audience products and the terms of any * license agreement you entered into with Audience for such products. - * - * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY - * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, - * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE - * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR - * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE - * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF - * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. * - ***************************************************************************************************/ + * AUDIENCE SOURCE CODE STRICTLY "AS IS" WITHOUT ANY WARRANTY + * WHATSOEVER, AND AUDIENCE EXPRESSLY DISCLAIMS ALL WARRANTIES, + * EXPRESS, IMPLIED OR STATUTORY WITH REGARD THERETO, INCLUDING THE + * IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR + * PURPOSE, TITLE OR NON-NFRINGEMENT OF THIRD PARTY RIGHTS. AUDIENCE + * SHALL NOT BE LIABLE FOR ANY DAMAGES SUFFERED BY YOU AS A RESULT OF + * USING, MODIFYING OR DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. + * + ****************************************************************************/ #if !defined (SENSOR_PACKETS_H) #define SENSOR_PACKETS_H -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | I N C L U D E F I L E S - \*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ #if 0 #include #include #endif -#include "MQ_sensors.h" +#include "osp-sensors.h" +#if 0 +#include "osp-types.h" +#endif -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | C O N S T A N T S & M A C R O S - \*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ /********************************************************/ /* COMMON PACKET DEFINITION */ /********************************************************/ /* Packet Identifier for version 0 packets */ #define PKID_SENSOR_DATA 0x00 -#define PKID_CONTROL_REQ 0x10 -#define PKID_CONTROL_RESP 0x20 -#define PKID_TEST_DATA 0x30 +#define PKID_CONTROL_REQ_RD 0x10 +#define PKID_CONTROL_REQ_WR 0x20 +#define PKID_CONTROL_RESP 0x30 +#define PKID_TEST_DATA 0x40 #define PKID_MASK_VER0 0xF0 /** =============== CONTROL ID BYTE =============== */ -#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) -#define SENSOR_TYPE_ANDROID 0x0 -#define SENSOR_TYPE_PRIVATE 0x1 +#define M_GetSensorType(i) ((uint8_t)((i >> 16 ) & 0x01)) +#define SENSOR_TYPE_ANDROID 0x0 +#define SENSOR_TYPE_PRIVATE 0x1 /** =============== SENSOR ID BYTE =============== */ -#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) +#define M_SensorMetaData(i) ((uint8_t)((i << 6) & 0xC0)) #define M_ParseSensorMetaData(i) ((uint8_t)((i >> 6) & 0x03)) -#define M_SensorType(s) ((uint8_t)(s & 0x3F)) +#define M_SensorType(s) ((uint8_t)(s & 0x3F)) /** =============== ATTRIBUTE BYTE =============== */ -#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) +#define M_SensorSubType(st) ((uint8_t)((st << 4) & 0xF0)) #define M_ParseSensorSubType(st) ((uint8_t)((st >> 4) & 0x0F)) /* Checksum option (check sum is always 16-bit CRC if enabled) */ -#define CHECK_SUM_PRESENT 0x01 +#define CHECK_SUM_PRESENT 0x01 /********************************************************/ /* SENSOR DATA PACKET */ /********************************************************/ /** =============== CONTROL BYTE =============== */ /* CRC option */ -#define CRC_ENABLE 0x08 +#define CRC_ENABLE 0x08 /*Enumeration type of sensor*/ #define SENSOR_ANDROID_TYPE_MASK 0x01 /* Format for data values */ -#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor values */ -#define DATA_FORMAT_FIXPOINT 0x04 /* Fixed point format in application defined units */ -#define DATA_FORMAT_MASK 0x04 +#define DATA_FORMAT_RAW 0x00 /* Raw counts (no units) from sensor */ +#define DATA_FORMAT_FIXPOINT 0x04 /* FixPoint format in app defined units */ +#define DATA_FORMAT_MASK 0x04 /* Time Format option */ -#define TIME_FORMAT_RAW 0x00 /* RAW count values for time stamp base */ -#define TIME_FORMAT_FIXPOINT 0x02 /* Fixed point format specified in seconds */ -#define TIME_FORMAT_MASK 0x02 +#define TIME_FORMAT_RAW 0x00 /* RAW count values for timestamp base */ +#define TIME_FORMAT_FIXPOINT 0x02 /* FixPoint format in seconds */ +#define TIME_FORMAT_MASK 0x02 /** ============ SENSOR IDENTIFIER BYTE ========== */ -#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ -#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ +#define META_DATA_UNUSED 0x00 /* Meta Data Identifier no used*/ +#define META_DATA_OFFSET_CHANGE 0x01 /* Meta Data Identifier */ /** =============== ATTRIBUTE BYTE =============== */ /* Data size option */ -#define DATA_SIZE_8_BIT 0x00 -#define DATA_SIZE_16_BIT 0x02 -#define DATA_SIZE_32_BIT 0x04 -#define DATA_SIZE_64_BIT 0x06 -#define DATA_SIZE_MASK 0x06 +#define DATA_SIZE_8_BIT 0x00 +#define DATA_SIZE_16_BIT 0x02 +#define DATA_SIZE_32_BIT 0x04 +#define DATA_SIZE_64_BIT 0x06 +#define DATA_SIZE_MASK 0x06 /* Time Stamp Size */ -#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ -#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ -#define TIME_STAMP_SIZE_MASK 0x01 +#define TIME_STAMP_32_BIT 0x00 /* Uncompressed 32-bit time stamp */ +#define TIME_STAMP_64_BIT 0x01 /* Uncompressed 64-bit time stamp */ +#define TIME_STAMP_SIZE_MASK 0x01 /********************************************************/ /* SENSOR CONTROL REQ/RESP PACKET */ @@ -114,77 +117,164 @@ /** =============== CONTROL BYTE =============== */ /* First 5 MS bits are same as Sensor Data Packet */ /* Data Format */ -#define CTRL_PKT_DF_INTEGER 0x00 -#define CTRL_PKT_DF_FIXPOINT 0x02 -#define CTRL_PKT_DF_FLOAT 0x04 -#define CTRL_PKT_DF_DOUBLE 0x06 +#define CTRL_PKT_DF_INTEGER 0x00 +#define CTRL_PKT_DF_FIXPOINT 0x02 +#define CTRL_PKT_DF_FLOAT 0x04 +#define CTRL_PKT_DF_DOUBLE 0x06 /* A/P Definition same as Sensor Data Packet */ /** =============== ATTRIBUTE BYTE 1 =============== */ /* Sequence Number for request/response */ -#define M_SequenceNum(sNum) ((sNum) & 0x0F) +#define M_SequenceNum(sNum) ((sNum) & 0x0F) /** =============== ATTRIBUTE BYTE 2 =============== */ /* Parameter ID */ -#define M_GetParamId(AttrByte2) ((AttrByte2) >> 3) -#define M_SetParamId(id) (((id) & 0x1F) << 3) - -/* Parameter Size */ -#define M_GetParamSize(AttrByte2) ((AttrByte2) & 0x07) - -/* Param data size */ -#define PARAM_DATA_SZ_8_BIT 0x00 -#define PARAM_DATA_SZ_16_BIT 0x01 -#define PARAM_DATA_SZ_32_BIT 0x02 -#define PARAM_DATA_SZ_64_BIT 0x03 -#define PARAM_DATA_SZ_BOOL_FALSE 0x04 -#define PARAM_DATA_SZ_BOOL_TRUE 0x05 -#define PARAM_DATA_SZ_UNKNOWN 0x07 +#define M_GetParamId(AttrByte2) (AttrByte2) +#define M_SetParamId(id) (id) + +/* Parameter Identifier*/ +#define PARAM_ID_ENABLE 0x01 +#define PARAM_ID_BATCH 0x02 +#define PARAM_ID_FLUSH 0x03 +#define PARAM_ID_RANGE_RESOLUTION 0x04 +#define PARAM_ID_POWER 0x05 +#define PARAM_ID_MINMAX_DELAY 0x06 +#define PARAM_ID_FIFO_EVT_CNT 0x07 +#define PARAM_ID_AXIS_MAPPING 0x08 +#define PARAM_ID_CONVERSION_OFFSET 0x09 +#define PARAM_ID_CONVERSION_SCALE 0x0A +#define PARAM_ID_SENSOR_NOISE 0x0B +#define PARAM_ID_TIMESTAMP_OFFSET 0x0C +#define PARAM_ID_ONTIME_WAKETIME 0x0D +#define PARAM_ID_HPF_LPF_CUTOFF 0x0E +#define PARAM_ID_SENSOR_NAME 0x0F +#define PARAM_ID_XYZ_OFFSET 0x10 +#define PARAM_ID_F_SKOR_MATRIX 0x11 +#define PARAM_ID_F_CAL_OFFSET 0x12 +#define PARAM_ID_F_NONLINEAR_EFFECTS 0x13 +#define PARAM_ID_BIAS_STABILITY 0x14 +#define PARAM_ID_REPEATABILITY 0x15 +#define PARAM_ID_TEMP_COEFF 0x16 +#define PARAM_ID_SHAKE_SUSCEPTIBILIY 0x17 +#define PARAM_ID_EXPECTED_NORM 0x18 +#define PARAM_ID_VERISON 0x19 +#define PARAM_ID_DYNAMIC_CAL_SCALE 0x1A +#define PARAM_ID_DYNAMIC_CAL_SKEW 0x1B +#define PARAM_ID_DYNAMIC_CAL_OFFSET 0x1C +#define PARAM_ID_DYNAMIC_CAL_ROTATION 0x1D +#define PARAM_ID_DYNAMIC_CAL_QUALITY 0x1E +#define PARAM_ID_DYNAMIC_CAL_SOURCE 0x1F +#define PARAM_ID_CONFIG_DONE 0x20 /* Byte extraction macros */ -#define BYTE3(x) ((uint8_t)((x >> 24) & 0xFF)) -#define BYTE2(x) ((uint8_t)((x >> 16) & 0xFF)) -#define BYTE1(x) ((uint8_t)((x >> 8) & 0xFF)) -#define BYTE0(x) ((uint8_t)(x & 0xFF)) - -/* 16-bit & 32-bit data combine macros */ +#define BYTE7(x) ((uint8_t)((x >> 56) & 0xFF)) +#define BYTE6(x) ((uint8_t)((x >> 48) & 0xFF)) +#define BYTE5(x) ((uint8_t)((x >> 40) & 0xFF)) +#define BYTE4(x) ((uint8_t)((x >> 32) & 0xFF)) +#define BYTE3(x) ((uint8_t)((x >> 24) & 0xFF)) +#define BYTE2(x) ((uint8_t)((x >> 16) & 0xFF)) +#define BYTE1(x) ((uint8_t)((x >> 8) & 0xFF)) +#define BYTE0(x) ((uint8_t)(x & 0xFF)) + +/* 16-bit, 32-bit & 64-bit data macros */ #define BYTES_TO_SHORT(b1,b0) ((int16_t)(((int16_t)b1 << 8) | b0)) + #define BYTES_TO_LONG_ARR(arr,ind) BYTES_TO_LONG(arr[ind],arr[ind+1],arr[ind+2],arr[ind+3]) + #define BYTES_TO_LONG(b3,b2,b1,b0) \ - ((int32_t)(((int32_t)b3 << 24) | ((uint32_t)b2 << 16) | ((uint32_t)b1 << 8) | b0)) + ((int32_t)(((int32_t)b3 << 24) | ((uint32_t)b2 << 16) | \ + ((uint32_t)b1 << 8) | b0)) -/*-------------------------------------------------------------------------------------------------*\ +#define BYTES_TO_LONGLONG(b7,b6,b5,b4,b3,b2,b1,b0) \ + ((uint64_t)(((uint64_t)b7 << 56) | ((uint64_t)b6 << 48) | \ + ((uint64_t)b5 << 40) | ((uint64_t)b4 << 32) | ((uint64_t)b3 << 24) | \ + ((uint64_t)b2 << 16) | ((uint64_t)b1 << 8) | b0)) + +/*--------------------------------------------------------------------------*\ | T Y P E D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ + +/* To be removed */ +typedef union _TimeStamp { + uint64_t TS64; + uint32_t TS32[2]; + uint8_t TS8[8]; +} TimeStamp_t; +/* End to be removed */ + /* Definition for quaternion data packets for internal usage */ typedef struct _QuaternionFixP { - TimeStamp_t TimeStamp; - int32_t Quat[4]; //W,X,Y,Z order + TimeStamp_t TimeStamp; + int32_t Quat[4]; //W,X,Y,Z order +#if 0 + // Add these two fields to comply with Android spec. + int32_t HeadingError; + int32_t TiltError; +#endif } QuaternionFixP_t; +typedef struct _OrientationFixP { + TimeStamp_t TimeStamp; + int32_t Pitch; + int32_t Roll; + int32_t Yaw; +} OrientationFixP_t; + +typedef struct _ThreeAxisFixP { + TimeStamp_t TimeStamp; + int32_t Axis[3]; + uint8_t Accuracy; +} ThreeAxisFixP_t; + +typedef struct _SignificantMotion { + TimeStamp_t TimeStamp; + uint8_t MotionDetected; +} SignificantMotion_t; + +typedef struct _StepCounter { + TimeStamp_t TimeStamp; + uint64_t NumStepsTotal; +} StepCounter_t; + +typedef struct _StepDetector { + TimeStamp_t TimeStamp; + uint8_t StepDetected; +} StepDetector_t; + typedef struct _UncalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z order - int32_t Offset[3]; //XOFF,YOFF,ZOFF order + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z order + int32_t Offset[3]; //XOFF,YOFF,ZOFF order } UncalibratedFixP_t; typedef struct _CalibratedFixP { - TimeStamp_t TimeStamp; - int32_t Axis[3]; //X,Y,Z, + TimeStamp_t TimeStamp; + int32_t Axis[3]; //X,Y,Z, } CalibratedFixP_t; +// Possible combine this and the CalibratedFixP_t into a common data type. +typedef struct _TriAxisRawData { + TimeStamp_t TStamp; + int32_t Axis[3]; //X,Y,Z, +} TriAxisRawData_t; + /* Union of the structures that can be parsed out of Sensor Data packet */ typedef struct _SensorPacketTypes { - union { - TriAxisRawData_t RawSensor; - UncalibratedFixP_t UncalFixP; - CalibratedFixP_t CalFixP; - QuaternionFixP_t QuatFixP; - } P; - ASensorType_t SType; - uint8_t SubType; + union { + TriAxisRawData_t RawSensor; + UncalibratedFixP_t UncalFixP; + CalibratedFixP_t CalFixP; + QuaternionFixP_t QuatFixP; + OrientationFixP_t OrientFixP; + ThreeAxisFixP_t ThreeAxisFixP; + SignificantMotion_t SigMotion; + StepCounter_t StepCount; + StepDetector_t StepDetector; + } P; + ASensorType_t SType; + uint8_t SubType; } SensorPacketTypes_t; /* ========== Host Inteface Packet definitions ========== */ @@ -195,84 +285,169 @@ typedef struct _SensorPacketTypes { /* Sensor Packet Type qualifier */ typedef struct _HifSensorPktQualifier { - uint8_t ControlByte; - uint8_t SensorIdByte; - uint8_t AttributeByte; + uint8_t ControlByte; + uint8_t SensorIdByte; + uint8_t AttributeByte; } HifSnsrPktQualifier_t; /* Basic packet: Raw 16-bit data, 32-bit integer time stamp; No checksum */ typedef struct _HifSensorDataRaw { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[4]; //32-bit Time Stamp - uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[4]; //32-bit Time Stamp + uint8_t DataRaw[6]; //3-Axis Raw 16-bit sensor data } HifSensorDataRaw_t; -#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) +#define SENSOR_RAW_DATA_PKT_SZ sizeof(HifSensorDataRaw_t) -/* Uncalibrated data packet: 32-bit Fixedpoint uncalibrated; 64-bit Fixedpoint time stamp; No Checksum */ +/* Uncalibrated data packet: + * 32-bit Fixedpoint uncalibrated; + * 64-bit Fixedpoint time stamp; + * No Checksum */ typedef struct _HifUncalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data - uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit uncalibrated data + uint8_t Offset[12]; //3-Axis Fixed point 32-bit Offset } HifUncalibratedFixPoint_t; -#define UNCALIB_FIXP_DATA_PKT_SZ (offsetof(HifUncalibratedFixPoint_t, Offset)) +#define UNCALIB_FIXP_DATA_PKT_SZ \ + (offsetof(HifUncalibratedFixPoint_t, Offset)) #define UNCALIB_FIXP_DATA_OFFSET_PKT_SZ sizeof(HifUncalibratedFixPoint_t) -/* Calibrated data packet: 32-bit Fixedpoint Calibrated; 64-bit Fixedpoint time stamp; No Checksum */ +/* Calibrated data packet: 32-bit Fixedpoint Calibrated; 64-bit Fixedpoint + time stamp; No Checksum */ typedef struct _HifCalibratedFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data } HifCalibratedFixPoint_t; #define CALIBRATED_FIXP_DATA_PKT_SZ sizeof(HifCalibratedFixPoint_t) -/* Quaternion data packet: 32-bit Fixedpoint quaternion; 64-bit Fixedpoint time stamp; No Checksum */ +/* Three Axis Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifThreeAxisPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[12]; //3-Axis Fixed point 32-bit calibrated data +} HifThreeAxisPktFixPoint_t; + +#define THREEAXIS_FIXP_DATA_PKT_SZ sizeof(HifThreeAxisPktFixPoint_t) + +/* SignificantMotion Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifSignificantMotionPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t significantMotionDetected; +} HifSignificantMotionPktFixPoint_t; + +#define SIGNIFICANTMOTION_FIXP_DATA_PKT_SZ \ + sizeof(HifSignificantMotionPktFixPoint_t) + +/* StepCounter data packet: 64-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifStepCounter { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t NumStepsTotal[8]; +} HifStepCounter_t; + +#define STEPCOUNTER_DATA_PKT_SZ sizeof(HifStepCounter_t) + +/* StepDetector data packet: 8-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifStepDetector { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t stepDetected; +} HifStepDetector_t; + +#define STEPDETECTOR_DATA_PKT_SZ sizeof(HifStepDetector_t) + +/* Orientation Fixed Point data packet: 32-bit Fixedpoint Calibrated; + 64-bit Fixedpoint time stamp; No Checksum */ +typedef struct _HifOrientationPktFixPoint { + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //!< Time in seconds + uint8_t Pitch[4]; //!< pitch in degrees + uint8_t Roll[4]; //!< roll in degrees + uint8_t Yaw[4]; //!< yaw in degrees +} HifOrientationPktFixPoint_t; + +#define ORIENTATION_FIXP_DATA_PKT_SZ sizeof(HifOrientationPktFixPoint_t) + +/* Quaternion data packet: 32-bit Fixedpoint quaternion; + 64-bit Fixedpoint time stamp; No Checksum */ typedef struct _HifQuaternionFixPoint { - HifSnsrPktQualifier_t Q; - uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format - uint8_t Data[16]; //4-Axis Fixed point 32-bit quaternion data + HifSnsrPktQualifier_t Q; + uint8_t TimeStamp[8]; //64-bit Time Stamp in fixed point format + uint8_t Data[16]; //4-Axis Fixed point 32-bit quaternion data } HifQuaternionFixPoint_t; -#define QUATERNION_FIXP_DATA_PKT_SZ sizeof(HifQuaternionFixPoint_t) +#define QUATERNION_FIXP_DATA_PKT_SZ sizeof(HifQuaternionFixPoint_t) -/* Sensor control packet for enable/disable sensor */ -typedef struct _HifSensorEnable { - HifSnsrPktQualifier_t Q; - uint8_t AttrByte2; -} HifSensorEnable_t; +/* Sensor control packet for Boolean parameters */ +typedef struct _HifSensorParamBool { + HifSnsrPktQualifier_t Q; + uint8_t AttrByte2; + uint8_t Data; +} HifSensorParamBool_t; -#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorEnable_t) +#define SENSOR_ENABLE_REQ_PKT_SZ sizeof(HifSensorParamBool_t) /* Define union for all the host interface packet types */ typedef union _HostIFPackets { - HifSensorDataRaw_t SensPktRaw; - HifUncalibratedFixPoint_t UncalPktFixP; - HifCalibratedFixPoint_t CalPktFixP; - HifQuaternionFixPoint_t QuatPktFixP; - HifSensorEnable_t Enable; + HifSensorDataRaw_t SensPktRaw; + HifUncalibratedFixPoint_t UncalPktFixP; + HifCalibratedFixPoint_t CalPktFixP; + HifQuaternionFixPoint_t QuatPktFixP; + HifSensorParamBool_t Enable; + HifOrientationPktFixPoint_t OrientationFixP; + HifThreeAxisPktFixPoint_t ThreeAxisFixp; + HifSignificantMotionPktFixPoint_t SignificantMotion; + HifStepCounter_t StepCounter; + HifStepDetector_t StepDetector; } HostIFPackets_t; -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | E X T E R N A L V A R I A B L E S & F U N C T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | P U B L I C V A R I A B L E S D E F I N I T I O N S -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | P U B L I C F U N C T I O N D E C L A R A T I O N S -\*-------------------------------------------------------------------------------------------------*/ -int16_t FormatSensorDataPktRaw( uint8_t *pDest, const TriAxisRawData_t *pSensData, uint8_t metaData, ASensorType_t sType, uint8_t subType ); -int16_t FormatQuaternionPktFixP( uint8_t *pDest, const QuaternionFixP_t *pQuatData, ASensorType_t sType ); -int16_t FormatUncalibratedPktFixP( uint8_t *pDest, const UncalibratedFixP_t *pUncalData, uint8_t metaData, ASensorType_t sType ); -int16_t FormatCalibratedPktFixP( uint8_t *pDest, const CalibratedFixP_t *pCalData, ASensorType_t sType ); -int16_t ParseHostIntefacePkt( SensorPacketTypes_t *pOut, uint8_t *pPacket, uint16_t pktSize ); +\*--------------------------------------------------------------------------*/ +int16_t FormatSensorDataPktRaw( uint8_t *pDest, + const TriAxisRawData_t *pSensData, uint8_t metaData, + ASensorType_t sType, uint8_t subType ); +int16_t FormatQuaternionPktFixP( uint8_t *pDest, + const QuaternionFixP_t *pQuatData, ASensorType_t sType ); +int16_t FormatUncalibratedPktFixP( uint8_t *pDest, + const UncalibratedFixP_t *pUncalData, uint8_t metaData, + ASensorType_t sType ); +int16_t FormatCalibratedPktFixP( uint8_t *pDest, + const CalibratedFixP_t *pCalData, ASensorType_t sType ); +int16_t ParseHostInterfacePkt( SensorPacketTypes_t *pOut, uint8_t *pPacket, + uint16_t pktSize ); +int16_t FormatSensorEnableReq( uint8_t *pDest, uint8_t enable, + ASensorType_t sType, uint8_t subType, uint8_t seqNum ); +int16_t FormatStepDetectorPkt( uint8_t *pDest, + const StepDetector_t *pStepDetectorData, ASensorType_t sType ); +int16_t FormatStepCounterPkt( uint8_t *pDest, + const StepCounter_t *pStepCounterData, ASensorType_t sType ); +int16_t FormatThreeAxisPktFixP( uint8_t *pDest, + const ThreeAxisFixP_t *p3AxisData, ASensorType_t sType ); +int16_t FormatSignificantMotionPktFixP( uint8_t *pDest, + const SignificantMotion_t *pSignificantMotionData, + ASensorType_t sType ); +int16_t FormatOrientationFixP( uint8_t *pDest, + const OrientationFixP_t *pOrientationData, ASensorType_t sType ); #endif /* SENSOR_PACKETS_H */ -/*-------------------------------------------------------------------------------------------------*\ +/*--------------------------------------------------------------------------*\ | E N D O F F I L E -\*-------------------------------------------------------------------------------------------------*/ +\*--------------------------------------------------------------------------*/ diff --git a/linux/driver/linux/osp-sh.h b/linux/driver/linux/osp-sh.h new file mode 100644 index 0000000..44c8bc1 --- /dev/null +++ b/linux/driver/linux/osp-sh.h @@ -0,0 +1,33 @@ +#ifndef _OSP_SH_H_ +#define _OSP_SH_H_ + +union OSP_SensorData { + struct { + u32 x; + u32 y; + u32 z; + u64 ts; + } xyz; + struct { + u32 x; + u32 y; + u32 z; + u32 r; + u64 ts; + } quat; +}; + +/* Register for sensor data. Only one call back per sensor.*/ +int OSP_Sensor_Register(int sensor, /* Sensor name and space */ + int private, + void (*dataready)(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata), + void *prv /* Private data for callback use */ + ); + +int OSP_Sensor_UnRegister(int sensor, int private); + +int OSP_Sensor_State(int sensor, int private, int state); + +#endif diff --git a/linux/driver/osp-iio.c b/linux/driver/osp-iio.c new file mode 100644 index 0000000..c2a447a --- /dev/null +++ b/linux/driver/osp-iio.c @@ -0,0 +1,1188 @@ +/** + * Copyright (c) 2015 Hunyue Yau for Audience + * + * Based on iio_simple_dummy by: + * Copyright (c) 2011 Jonathan Cameron + * + * This program is free software; you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 2 as published by + * the Free Software Foundation. + * + * A reference industrial I/O driver to illustrate the functionality available. + * + * There are numerous real drivers to illustrate the finer points. + * The purpose of this driver is to provide a driver with far more comments + * and explanatory notes than any 'real' driver would have. + * Anyone starting out writing an IIO driver should first make sure they + * understand all of this driver except those bits specifically marked + * as being present to allow us to 'fake' the presence of hardware. + + * Simple OSP IIO output layer. + */ + +#include +#include +#include +#include +#include +#include + +#include "iio.h" +#include "sysfs.h" +#include "events.h" +#include "buffer.h" +#include "kfifo_buf.h" +#include "trigger.h" +#include "trigger_consumer.h" +#include + +#include "osp-sensors.h" + +/* Simulate periodic data */ +static struct timer_list osp_timer; + +enum { + axis_x, + axis_y, + axis_z, + axis_r, +}; + +static ssize_t osp_iio_renable(struct device *dev, + struct device_attribute *attr, char *buf); +static ssize_t osp_iio_wenable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len); + +static IIO_DEVICE_ATTR(enable, S_IWUSR|S_IRUSR|S_IRGRP|S_IWGRP, + &osp_iio_renable, &osp_iio_wenable, 0); + +static struct attribute *osp_iio_attrs[] = { + &iio_dev_attr_enable.dev_attr.attr, + NULL, +}; + +static const struct attribute_group osp_iio_group = { + .attrs = osp_iio_attrs, +}; +/* + * Description of available channels + * + * This array of structures tells the IIO core about what the device + * actually provides for a given channel. + */ +static struct iio_chan_spec step_channels[] = { + { + .type = IIO_ACCEL, /* Nothing better defined */ + .modified = 1, + .channel = 0, + .address = 0, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec accel_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 0, + .address = 0, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 1, + .address = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + .channel = 2, + .address = 2, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static struct iio_chan_spec gyro_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ANGL_VEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; + +static struct iio_chan_spec mag_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .scan_type = IIO_ST('s', 32, 32, 12), + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + }, + { + .type = IIO_MAGN, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec linacc_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_ACCEL, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec orient_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + { + .type = IIO_INCLI, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 12), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(3), +}; +static struct iio_chan_spec rotvec_channels[] = { + /* + * 'modified' (i.e. axis specified) acceleration channel + * in_accel_z_raw + */ + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Y, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_y, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_Z, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_z, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + { + .type = IIO_QUATERNION, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_R, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_r, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + /* + * Convenience macro for timestamps. 4 is the index in + * the buffer. + */ + IIO_CHAN_SOFT_TIMESTAMP(4), +}; + +static struct iio_chan_spec press_channels[] = { + { + .type = IIO_PRESSURE, + .modified = 1, + /* Channel 2 is use for modifiers */ + .channel2 = IIO_MOD_X, + .info_mask = + /* + * Internal bias correction value. Applied + * by the hardware or driver prior to userspace + * seeing the readings. Typically part of hardware + * calibration. + */ + 0, + .scan_index = axis_x, + .scan_type = IIO_ST('s', 32, 32, 24), + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; +struct osp_iio_sensor { + struct iio_dev *indio_dev; + struct iio_trigger *trigger; + struct mutex lock; + int sensor; + int private; + int state; + long long ts; + union OSP_SensorData data; +}; + +struct osp_iio_sensor *osp_sensors_and[NUM_ANDROID_SENSOR_TYPE]; +struct osp_iio_sensor *osp_sensors_prv[NUM_PRIVATE_SENSOR_TYPE]; + +static int osp_sensor_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask); +static int osp_sensor_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, int val2, long mask); +/* + * Device type specific information. + */ +static const struct iio_info osp_sensor_info = { + .driver_module = THIS_MODULE, + .read_raw = &osp_sensor_read_raw, + .write_raw = &osp_sensor_write_raw, + .attrs = &osp_iio_group, +}; + +static const struct OSP_SensorDesc { + char *name; + char *trigname; + struct iio_chan_spec *channels; + int num_channels; + struct iio_info const *info; + int usebuffer; + int useevent; +} and_sensor[NUM_ANDROID_SENSOR_TYPE] = { + [SENSOR_GYROSCOPE] = { + .name = "osp_gyro", + .trigname = "osp_gyro", + .channels = gyro_channels, + .num_channels = ARRAY_SIZE(gyro_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GYROSCOPE_UNCALIBRATED] = { + .name = "osp_uncal_gyro", + .trigname = "osp_uncal_gyro", + .channels = gyro_channels, + .num_channels = ARRAY_SIZE(gyro_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ACCELEROMETER] = { + .name = "osp_accel", + .trigname = "osp_accel", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GRAVITY] = { + .name = "osp_gravity", + .trigname = "osp_gravity", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_MAGNETIC_FIELD] = { + .name = "osp_mag", + .trigname = "osp_mag", + .channels = mag_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = { + .name = "osp_uncal_mag", + .trigname = "osp_uncal_mag", + .channels = mag_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ORIENTATION] = { + .name = "osp_compass_orientation", + .trigname = "osp_compass_orientation", + .channels = orient_channels, + .num_channels = ARRAY_SIZE(mag_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_LINEAR_ACCELERATION] = { + .name = "osp_linear_acceleration", + .trigname = "osp_linear_acceleration", + .channels = linacc_channels, + .num_channels = ARRAY_SIZE(linacc_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_ROTATION_VECTOR] = { + .name = "osp_rotation_vector", + .trigname = "osp_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GAME_ROTATION_VECTOR] = { + .name = "osp_game_rotation_vector", + .trigname = "osp_game_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = { + .name = "osp_geo_rotation_vector", + .trigname = "osp_geo_rotation_vector", + .channels = rotvec_channels, + .num_channels = ARRAY_SIZE(rotvec_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [SENSOR_SIGNIFICANT_MOTION] = { + .name = "osp_sigmot", + .trigname = "osp_sigmot", + .channels = NULL, + .num_channels = 0, + .info = &osp_sensor_info, + .usebuffer = 0, + .useevent = 1, + }, + [SENSOR_STEP_COUNTER] = { + .name = "osp_step", + .trigname = "osp_step", + .channels = step_channels, + .num_channels = ARRAY_SIZE(step_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 1, + }, + [SENSOR_PRESSURE] = { + .name = "osp_pressure", + .trigname = "osp_pressure", + .channels = NULL, + .num_channels = 0, + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, +}, prv_sensor[NUM_PRIVATE_SENSOR_TYPE] = { + [PSENSOR_ACCELEROMETER_RAW] = { + .name = "osp_accel_raw", + .trigname = "osp_accel_raw", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [PSENSOR_ACCELEROMETER_UNCALIBRATED] = { + .name = "osp_uncal_accel", + .trigname = "osp_uncal_accel", + .channels = accel_channels, + .num_channels = ARRAY_SIZE(accel_channels), + .info = &osp_sensor_info, + .usebuffer = 1, + .useevent = 0, + }, + [PSENSOR_CONTEXT_DEVICE_MOTION] = { + .name = "osp_context", + .trigname = "osp_context", + .usebuffer = 0, + }, +}; + +/* ------- sysfs ------- */ + +static ssize_t osp_iio_renable(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + ssize_t len = 0; + + len += sprintf(buf, "%i\n", osp_sensor->state); + + return len; +} + +static ssize_t osp_iio_wenable(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t len) +{ + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + int en; + + if (buf[0] == '1') + en = 1; + else + en = 0; + + OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, en); + osp_sensor->state = en; + + return len; +} + +/* --------------------- */ + +/** + * osp_sensor_read_raw() - data read function. + * @indio_dev: the struct iio_dev associated with this device instance + * @chan: the channel whose data is to be read + * @val: first element of returned value (typically INT) + * @val2: second element of returned value (typically MICRO) + * @mask: what we actually want to read. 0 is the channel, everything else + * is as per the info_mask in iio_chan_spec. + */ +static int osp_sensor_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, + int *val2, + long mask) +{ + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + int ret = -EINVAL; + + printk("HY-DBG: %s:%i called\n", __func__, __LINE__); + + mutex_lock(&osp_sensor->lock); + switch (mask) { + case 0: /* magic value - channel value read */ + switch (chan->type) { + case IIO_ACCEL: + case IIO_ANGL_VEL: + case IIO_MAGN: + switch (chan->scan_index) { + case axis_x: + *val = osp_sensor->data.xyz.x; + break; + case axis_y: + *val = osp_sensor->data.xyz.y; + break; + case axis_z: + *val = osp_sensor->data.xyz.z; + break; + default: + break; + } + ret = IIO_VAL_INT; + break; + + default: + break; + } + break; + default: + break; + } + mutex_unlock(&osp_sensor->lock); + return ret; +} + +/** + * osp_sensor_write_raw() - data write function. + * @indio_dev: the struct iio_dev associated with this device instance + * @chan: the channel whose data is to be read + * @val: first element of returned value (typically INT) + * @val2: second element of returned value (typically MICRO) + * @mask: what we actually want to read. 0 is the channel, everything else + * is as per the info_mask in iio_chan_spec. + * + * Note that all raw writes are assumed IIO_VAL_INT and info mask elements + * are assumed to be IIO_INT_PLUS_MICRO unless the callback write_raw_get_fmt + * in struct iio_info is provided by the driver. + */ +static int osp_sensor_write_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int val, + int val2, + long mask) +{ +#if 0 + int i; + int ret = 0; + struct iio_dummy_state *st = iio_priv(indio_dev); + + printk("HY-DBG: %s:%i called\n", __func__, __LINE__); + + switch (mask) { + case 0: + if (chan->output == 0) + return -EINVAL; + + /* Locking not required as writing single value */ + mutex_lock(&st->lock); + st->dac_val = val; + mutex_unlock(&st->lock); + return 0; + case IIO_CHAN_INFO_CALIBBIAS: + mutex_lock(&st->lock); + /* Compare against table - hard matching here */ + for (i = 0; i < ARRAY_SIZE(dummy_scales); i++) + if (val == dummy_scales[i].val && + val2 == dummy_scales[i].val2) + break; + if (i == ARRAY_SIZE(dummy_scales)) + ret = -EINVAL; + else + st->accel_calibscale = &dummy_scales[i]; + mutex_unlock(&st->lock); + return ret; + default: + return -EINVAL; + } +#else + return 0; +#endif +} + +/** + * osp_iio_sensor_init() - Sets up sensor specific details. + * @indio_dev: the iio device structure + * + * Most drivers have one of these to set up default values, + * reset the device to known state etc. + * + * Init data to known values for debugging. + */ +static int osp_iio_sensor_init(struct iio_dev *indio_dev, int sensor, int type) +{ + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + osp_sensor->indio_dev = indio_dev; + memset(&osp_sensor->data, 0, sizeof(osp_sensor->data)); + osp_sensor->sensor = sensor; + + if (type == 0) { + switch (sensor) { + case SENSOR_ACCELEROMETER: + osp_sensor->data.xyz.x = 0; + osp_sensor->data.xyz.y = 1; + osp_sensor->data.xyz.z = 2; + break; + case SENSOR_GYROSCOPE: + osp_sensor->data.xyz.x = 2002; + osp_sensor->data.xyz.y = 2003; + osp_sensor->data.xyz.z = 2004; + break; + case SENSOR_MAGNETIC_FIELD: + osp_sensor->data.xyz.x = 4002; + osp_sensor->data.xyz.y = 4003; + osp_sensor->data.xyz.z = 4004; + break; + case SENSOR_ROTATION_VECTOR: + osp_sensor->data.quat.x = 6001; + osp_sensor->data.quat.y = 6002; + osp_sensor->data.quat.z = 6003; + osp_sensor->data.quat.r = 1004; + break; + } + } + + return 0; +} + +static int osp_iio_trigger(struct iio_trigger *trig, bool state) +{ + /* HY-DBG 0 */ + struct iio_dev *indio_dev = trig->private_data; + struct osp_iio_sensor *osp_sensor; + osp_sensor = iio_priv(indio_dev); + + printk("HY-DBG: %s:%i for %s (%i)\n", __func__, __LINE__, + (osp_sensor->private)? + prv_sensor[osp_sensor->sensor].name: + and_sensor[osp_sensor->sensor].name, state); + OSP_Sensor_State(osp_sensor->sensor, osp_sensor->private, state); + osp_sensor->state = state; + + return 0; +} + +static irqreturn_t osp_iio_trigger_handler(int irq, void *p) +{ + struct iio_poll_func *pf = p; + struct iio_dev *indio_dev = pf->indio_dev; + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + iio_push_to_buffer(indio_dev->buffer, (u8 *)&osp_sensor->data, + osp_sensor->ts); + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static const struct iio_trigger_ops osp_iio_trigger_ops = { + .owner = THIS_MODULE, + .set_trigger_state = &osp_iio_trigger, +}; + +static const struct iio_buffer_setup_ops osp_iio_buffer_setup_ops = { + .preenable = &iio_sw_buffer_preenable, + .postenable = &iio_triggered_buffer_postenable, + .predisable = &iio_triggered_buffer_predisable, +}; + +/* + * Call back from core OSP code for data ready. + * Store data and trigger if buffers used. + */ +static void dataready(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata) +{ + struct iio_dev *indio_dev = private; + struct osp_iio_sensor *osp_sensor = iio_priv(indio_dev); + + osp_sensor->ts = ts; + osp_sensor->data = *sensordata; + if (osp_sensor->private == 0 && and_sensor[sensor].useevent) { + switch (sensor) { + case SENSOR_SIGNIFICANT_MOTION: + case SENSOR_STEP_COUNTER: + iio_push_event(indio_dev, + IIO_MOD_EVENT_CODE(IIO_INTENSITY, 0, + IIO_MOD_X, + IIO_EV_TYPE_THRESH, + IIO_EV_DIR_EITHER), + ts); + break; + } + } + if ((osp_sensor->private == 0 && and_sensor[sensor].usebuffer) || + (osp_sensor->private == 1 && prv_sensor[sensor].usebuffer)) + iio_trigger_poll(osp_sensor->trigger, osp_sensor->ts); +} + +static int osp_iio_destroy(int sensor, int space) +{ + struct iio_dev *indio_dev; + int ret = 0; + struct osp_iio_sensor **osp_sensors; + + if (space == 0) + osp_sensors = osp_sensors_and; + else + osp_sensors = osp_sensors_prv; + + indio_dev = osp_sensors[sensor]->indio_dev; + + /* Disable the sensor */ + OSP_Sensor_State(sensor, space, 0); + osp_sensors[sensor]->state = 0; + + /* Unregister the device */ + iio_device_unregister(indio_dev); + + /* Device specific code to power down etc */ + + /* Buffered capture related cleanup */ + iio_buffer_unregister(indio_dev); +#if 0 + iio_simple_dummy_unconfigure_buffer(indio_dev); +#endif + /* Free all structures */ + iio_free_device(indio_dev); + +error_ret: + return ret; +} + +/** + * osp_iio_create() - Create a IIO sensor instance. + * index - sensor id. enum space is determined by value of space. + * space - enum space for sensors. + * dev - caller used pointer. + */ +static int osp_iio_create(int index, int space, void *dev) +{ + int ret; + struct iio_dev *indio_dev; + struct osp_iio_sensor *st; + struct OSP_SensorDesc *desc; + struct osp_iio_sensor **osp_sensor; + + if (space == 1) { /* Private sensors */ + if (index >= NUM_PRIVATE_SENSOR_TYPE || index < 0) + return -EINVAL; + desc = prv_sensor; + osp_sensor = osp_sensors_prv; + } else if (space == 0) { /* Android sensors */ + if (index >= NUM_ANDROID_SENSOR_TYPE || index < 0) + return -EINVAL; + desc = and_sensor; + osp_sensor = osp_sensors_and; + } else { + return -EINVAL; + } + /* + * Allocate an IIO device. + * + * This structure contains all generic state + * information about the device instance. + * It also has a region (accessed by iio_priv() + * for chip specific state information. + */ + indio_dev = iio_allocate_device(sizeof(*st)); + if (indio_dev == NULL) { + ret = -ENOMEM; + goto error_ret; + } + + st = iio_priv(indio_dev); + st->private = space; + st->state = 0; + st->data.xyz.x = 0; + st->data.xyz.y = 0; + st->data.xyz.z = 0; + mutex_init(&st->lock); + osp_iio_sensor_init(indio_dev, index, 0); + + indio_dev->dev.parent = dev; + + osp_sensor[index] = st; + + /* + * Set the device name. + * + * This is typically a part number and obtained from the module + * id table. + * e.g. for i2c and spi: + * indio_dev->name = id->name; + * indio_dev->name = spi_get_device_id(spi)->name; + */ + indio_dev->name = desc[index].name; + + /* Provide description of available channels */ + indio_dev->channels = desc[index].channels; + indio_dev->num_channels = desc[index].num_channels; + + /* + * Provide device type specific interface functions and + * constant data. + */ + indio_dev->info = desc[index].info; + + /* Specify that device provides sysfs type interfaces */ + indio_dev->modes = INDIO_DIRECT_MODE | + INDIO_BUFFER_TRIGGERED; + st->trigger = iio_allocate_trigger(desc[index].trigname); + + if (st->trigger) { + st->trigger->ops = &osp_iio_trigger_ops; + iio_trigger_register(st->trigger); + st->trigger->private_data = indio_dev; + } + + if (desc[index].usebuffer) { +#if 1 + indio_dev->pollfunc = iio_alloc_pollfunc( + &iio_pollfunc_store_time, + &osp_iio_trigger_handler, + IRQF_ONESHOT, indio_dev, + desc[index].trigname); + + indio_dev->buffer = iio_kfifo_allocate(indio_dev); + indio_dev->setup_ops = &osp_iio_buffer_setup_ops; + + ret = iio_buffer_register(indio_dev, + desc[index].channels, + desc[index].num_channels); + + indio_dev->buffer->scan_timestamp = 1; + +#else + ret = iio_triggered_buffer_setup(indio_dev, + &iio_pollfunc_store_time, + &osp_iio_trigger_handler, NULL); + if (ret < 0) + goto error_free_device; +#endif + } + OSP_Sensor_Register(index, space, dataready, indio_dev); + ret = iio_device_register(indio_dev); + if (ret < 0) + goto error_unregister_buffer; + + return 0; +error_unregister_buffer: + iio_buffer_unregister(indio_dev); +#if 0 +error_unconfigure_buffer: + iio_simple_dummy_unconfigure_buffer(indio_dev); +error_unregister_events: + iio_simple_dummy_events_unregister(indio_dev); +#endif +error_free_device: + /* Note free device should only be called, before registration + * has succeeded. */ + iio_free_device(indio_dev); +error_ret: + return ret; +} + +static int osp_iio_probe(struct platform_device *pdev) +{ + int ret, i; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + osp_sensors_and[i] = NULL; + } + + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + osp_sensors_prv[i] = NULL; + } +#if 0 + osp_iio_create(SENSOR_GYROSCOPE, 0, NULL); + osp_iio_create(SENSOR_GYROSCOPE_UNCALIBRATED, 0, NULL); + osp_iio_create(SENSOR_ACCELEROMETER, 0, NULL); + osp_iio_create(SENSOR_GRAVITY, 0, NULL); + osp_iio_create(SENSOR_MAGNETIC_FIELD, 0, NULL); + osp_iio_create(SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 0, NULL); + osp_iio_create(SENSOR_ORIENTATION, 0, NULL); + osp_iio_create(SENSOR_LINEAR_ACCELERATION, 0, NULL); + osp_iio_create(SENSOR_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_GAME_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_GEOMAGNETIC_ROTATION_VECTOR, 0, NULL); + osp_iio_create(SENSOR_SIGNIFICANT_MOTION, 0, NULL); + osp_iio_create(SENSOR_STEP_COUNTER, 0, NULL); +#endif +#if 1 + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (and_sensor[i].name) { + ret = osp_iio_create(i, 0, NULL); + if (ret < 0) { + printk("Cannot create %s\n", and_sensor[i].name); + } + } + } +#if 0 + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + if (prv_sensor[i].name) { + ret = osp_iio_create(i, 1, NULL); + if (ret < 0) { + printk("Cannot create %s\n", and_sensor[i].name); + } + } + } +#endif +#endif +#if 0 + ret = osp_iio_create(SENSOR_ACCELEROMETER, 0, NULL); + ret = osp_iio_create(SENSOR_GYROSCOPE, 0, NULL); + ret = osp_iio_create(SENSOR_MAGNETIC_FIELD, 0, NULL); + ret = osp_iio_create(SENSOR_ROTATION_VECTOR, 0, NULL); + ret = osp_iio_create(SENSOR_ORIENTATION, 0, NULL); + ret = osp_iio_create(SENSOR_LINEAR_ACCELERATION, 0, NULL); + ret = osp_iio_create(SENSOR_GRAVITY, 0, NULL); + ret = osp_iio_create(SENSOR_MAGNETIC_FIELD_UNCALIBRATED, 0, NULL); +#endif + return 0; +} + +static int osp_iio_remove(struct platform_device *pdev) +{ + return 0; +} +static struct platform_driver osp_iio_driver = { + .probe = osp_iio_probe, + .remove = osp_iio_remove, + .driver = { + .name = "osp-output", + .owner = THIS_MODULE, + }, +}; +static __init int osp_iio_init(void) +{ + return platform_driver_register(&osp_iio_driver); +} +module_init(osp_iio_init); + +/** + * osp_iio_exit() - device driver removal + * + * Varies depending on bus type of the device. + * As there is no device here, call remove directly. + */ +static __exit void osp_iio_exit(void) +{ + int i; + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) + if (osp_sensors_and[i]) { + osp_iio_destroy(i, osp_sensors_and[i]->private); + } + + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) + if (osp_sensors_prv[i]) { + osp_iio_destroy(i, osp_sensors_and[i]->private); + } +} +module_exit(osp_iio_exit); + +MODULE_AUTHOR("Hunyue Yau "); +MODULE_DESCRIPTION("OSP IIO driver"); +MODULE_LICENSE("GPL v2"); diff --git a/linux/driver/osp-input.c b/linux/driver/osp-input.c new file mode 100644 index 0000000..ec80d8a --- /dev/null +++ b/linux/driver/osp-input.c @@ -0,0 +1,306 @@ +/* + * Copyright (C) 2041 Audience, Inc. + * Written by Hunyue Yau + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA + * 02111-1307, USA + */ + +#include +#include +#include +#include +#include +#include "osp-sensors.h" +#include + +/* INPUT_ABS CONSTANTS */ +#define FUZZ 3 +#define FLAT 3 +#define G_MAX 8000 +#if 0 +static char *in_pname[NUM_PRIVATE_SENSOR_TYPE] = { + [PSENSOR_ACCELEROMETER_RAW] = "acc_raw", + [PSENSOR_ACCELEROMETER_UNCALIBRATED] = "acc_uncal", +}; +#endif + +struct osp_input_sensor { + struct input_dev *input_dev; + char *name; +} and_sensor[NUM_ANDROID_SENSOR_TYPE] = { + [SENSOR_ACCELEROMETER] = { + .name = "fm-accelerometer", + }, + [SENSOR_MAGNETIC_FIELD] = { + .name = "fm-magnetometer", + }, + [SENSOR_GYROSCOPE] = { + .name = "fm-gyroscope", + }, + [SENSOR_ROTATION_VECTOR] = { + .name = "fm-rotation-vector", + }, + [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = { + .name = "fm-uncalibrated-magnetometer", + }, + [SENSOR_GYROSCOPE_UNCALIBRATED] = { + .name = "fm-uncalibrated-gyroscope", + }, + [SENSOR_ORIENTATION] = { + .name = "fm-compass-orientation", + }, + [SENSOR_LINEAR_ACCELERATION] = { + .name = "fm-linear-acceleration", + }, + [SENSOR_GRAVITY] = { + .name = "fm-gravity", + }, + [SENSOR_STEP_COUNTER] = { + .name = "fm-step-counter", + }, + [SENSOR_STEP_DETECTOR] = { + .name = "fm-step-detector", + }, + [SENSOR_SIGNIFICANT_MOTION] = { + .name = "fm-significant-motion", + }, + [SENSOR_PRESSURE] = { + .name = "pressure", + }, + [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = { + .name = "fm-geomagnetic-rotation-vector", + }, + [SENSOR_GAME_ROTATION_VECTOR] = { + .name = "fm-game-rotation-vector", + }, +}; + +static ssize_t osp_get_enable(struct device *dev, struct device_attribute *attr, char *buf) +{ + return 0; +} + +static ssize_t osp_set_enable(struct device *dev, struct device_attribute *attr, + const char *buf, size_t count) +{ + struct input_dev *input = to_input_dev(dev); + struct osp_input_sensor *osp_input = input_get_drvdata(input); + int i; + unsigned long v; + + if (strict_strtoul(buf, 0, &v)) + return -EINVAL; + + for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { + if (and_sensor[i].input_dev == input) { + break; + } + } + + if (i == NUM_ANDROID_SENSOR_TYPE) + return -EINVAL; + OSP_Sensor_State(i, 0, v); + + return count; +} +#if 0 +static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR, osp_get_enable, osp_set_enable); +#else +static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR|S_IWUGO, osp_get_enable, osp_set_enable); +#endif + +static struct attribute *osp_attributes[] = { + &dev_attr_enable_new.attr, + NULL +}; + +static struct attribute_group osp_attribute_group = { + .attrs = osp_attributes +}; + +static int osp_input_open(struct input_dev *input) +{ + return 0; +} + +static void osp_input_close(struct input_dev *dev) +{ +} + +static void dataready(int sensor, int prv, + void *private, long long ts, + union OSP_SensorData *sensordata) +{ + struct osp_input_sensor *sen = private; + struct input_dev *input_dev = sen->input_dev; + static int counter = 0; + int PSensor; + + if (prv != 0) return; + + switch (sensor) { + case SENSOR_STEP_DETECTOR: + case SENSOR_SIGNIFICANT_MOTION: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, 1); + input_report_abs(input_dev, ABS_GAS, counter); + counter++; + input_sync(input_dev); + break; + case SENSOR_STEP_COUNTER: + case SENSOR_PRESSURE: + break; + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: + case SENSOR_GYROSCOPE_UNCALIBRATED: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case SENSOR_ACCELEROMETER: + printk("HY-DBG: %s:%i ACCEL\n", __func__, __LINE__); + case SENSOR_MAGNETIC_FIELD: + case SENSOR_GYROSCOPE: + case SENSOR_ORIENTATION: + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: + if (!input_dev) break; + input_report_abs(input_dev, ABS_GAS, sensordata->quat.r); + input_report_abs(input_dev, ABS_X, sensordata->quat.x); + input_report_abs(input_dev, ABS_Y, sensordata->quat.y); + input_report_abs(input_dev, ABS_Z, sensordata->quat.z); + input_sync(input_dev); + break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED|SENSOR_DEVICE_PRIVATE_BASE: + PSensor = sensor & ~(SENSOR_DEVICE_PRIVATE_BASE); + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + case PSENSOR_ACCELEROMETER_RAW|SENSOR_DEVICE_PRIVATE_BASE: + PSensor = sensor & ~(SENSOR_DEVICE_PRIVATE_BASE); + if (!input_dev) break; + input_report_abs(input_dev, ABS_X, sensordata->xyz.x); + input_report_abs(input_dev, ABS_Y, sensordata->xyz.y); + input_report_abs(input_dev, ABS_Z, sensordata->xyz.z); + input_sync(input_dev); + break; + default: + break; + } +} + + +static int osp_input_create(struct device *dev, int sensor, int private) +{ + struct input_dev *input_dev; + struct osp_input_sensor *sen; + int err; + + if (private == 0) { + sen = and_sensor; + } else { + return -EINVAL; + } + + input_dev = input_allocate_device(); + if (!input_dev) { + dev_err(dev, "input device allocate failed\n"); + return -ENOMEM; + } + + sen[sensor].input_dev = input_dev; + + input_dev->open = osp_input_open; + input_dev->close = osp_input_close; + input_set_drvdata(input_dev, &sen[sensor]); + + __set_bit(EV_ABS, input_dev->evbit); + input_set_abs_params(input_dev, ABS_X, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_Y, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_Z, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_THROTTLE, -G_MAX, G_MAX, FUZZ, FLAT); + input_set_abs_params(input_dev, ABS_GAS, -G_MAX, G_MAX, FUZZ, FLAT); + + input_dev->name = sen[sensor].name; + input_dev->id.bustype = BUS_I2C; + input_dev->dev.parent = dev; + input_dev->relbit[0] |= BIT_MASK(REL_X); + + err = input_register_device(input_dev); + if (err) { + dev_err(dev, + "unable to register input polled device %s: %d\n", + input_dev->name, err); + input_free_device(input_dev); + return err; + } + err = sysfs_create_group(&input_dev->dev.kobj, &osp_attribute_group); + if (err) { + printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); + } + + OSP_Sensor_Register(sensor, private, dataready, &sen[sensor]); + + return 0; +} + +static int osp_input_probe(struct platform_device *pdev) +{ + osp_input_create(&pdev->dev, SENSOR_ACCELEROMETER, 0); + osp_input_create(&pdev->dev, SENSOR_MAGNETIC_FIELD, 0); + osp_input_create(&pdev->dev, SENSOR_GYROSCOPE, 0); + osp_input_create(&pdev->dev, SENSOR_ROTATION_VECTOR, 0); + osp_input_create(&pdev->dev, SENSOR_ORIENTATION, 0); + osp_input_create(&pdev->dev, SENSOR_LINEAR_ACCELERATION, 0); + osp_input_create(&pdev->dev, SENSOR_GRAVITY, 0); + + return 0; +} +static int osp_input_remove(struct platform_device *pdev) +{ + return 0; +} + +static struct platform_driver osp_input_driver = { + .probe = osp_input_probe, + .remove = osp_input_remove, + .driver = { + .name = "osp-output", + .owner = THIS_MODULE, + }, +}; + +static int __init osp_input_init(void) +{ + return platform_driver_register(&osp_input_driver); +} + +module_init(osp_input_init); + + +MODULE_DESCRIPTION("OSP Input driver"); +MODULE_AUTHOR("Hunyue Yau "); +MODULE_LICENSE("GPL"); diff --git a/linux/driver/osp-sh.c b/linux/driver/osp-sh.c index c1d0433..5623171 100644 --- a/linux/driver/osp-sh.c +++ b/linux/driver/osp-sh.c @@ -1,6 +1,6 @@ /* * Copyright (C) 2041 Audience, Inc. - * Written by Hunyue Yau + * Written by Hunyue Yau * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as @@ -19,44 +19,44 @@ #include #include -#include #include #include #include -#include #include -#include "MQ_sensors.h" +#include +#include "osp-sensors.h" #include "SensorPackets.h" #include "osp_i2c_map.h" +#include #define NAME "ospsh" -/* INPUT_ABS CONSTANTS */ -#define FUZZ 3 -#define FLAT 3 -#define G_MAX 8000 - -static char *in_name[NUM_ANDROID_SENSOR_TYPE] = { - [SENSOR_ACCELEROMETER] = "fm-accelerometer", - [SENSOR_MAGNETIC_FIELD] = "fm-magnetometer", - [SENSOR_GYROSCOPE] = "fm-gyroscope", - [SENSOR_ROTATION_VECTOR] = "fm-rotation-vector", - [SENSOR_MAGNETIC_FIELD_UNCALIBRATED] = "fm-uncalibrated-magnetometer", - [SENSOR_GYROSCOPE_UNCALIBRATED] = "fm-uncalibrated-gyroscope", - [SENSOR_ORIENTATION] = "fm-compass-orientation", - [SENSOR_LINEAR_ACCELERATION] = "fm-linear-acceleration", - [SENSOR_GRAVITY] = "fm-gravity", - [SENSOR_STEP_COUNTER] = "fm-step-counter", - [SENSOR_STEP_DETECTOR] = "fm-step-detector", - [SENSOR_SIGNIFICANT_MOTION] = "fm-significant-motion", - [SENSOR_PRESSURE] = "pressure", - [SENSOR_GEOMAGNETIC_ROTATION_VECTOR] = "fm-geomagnetic-rotation-vector", - [SENSOR_GAME_ROTATION_VECTOR] = "fm-game-rotation-vector", -}; -static char *in_pname[NUM_PRIVATE_SENSOR_TYPE] = { - [PSENSOR_ACCELEROMETER_RAW] = "acc_raw", - [PSENSOR_ACCELEROMETER_UNCALIBRATED] = "acc_uncal", +/* Private sensors */ +#define FEAT_PRIVATE (1<<0) + +static struct OSP_Sensor { + void (*dataready)(int sensor, int prv, + void *private, long long ts, union OSP_SensorData *sensordata); + + void *private; +} and_sensor[NUM_ANDROID_SENSOR_TYPE], prv_sensor[NUM_PRIVATE_SENSOR_TYPE]; + +struct osp_data { + struct i2c_client *client; + struct timer_list osp_timer; + struct work_struct osp_work; + struct mutex lock; + unsigned int features; }; + +static struct osp_data *gOSP; + +static struct work_queue *osp_workq; + +/* Number of packets to parse */ +#define NUM_PACK 2 +static unsigned char *osp_pack[NUM_PACK]; + /* ------------ OSP Packet parsing code -------------------- */ /*********************************************************************** * @fn ParseSensorDataPkt @@ -77,9 +77,9 @@ static int OSP_ParseSensorDataPkt_Private( uint8_t dSize, uint8_t dFormat, uint8_t timeFormat, - uint8_t tSize, + uint8_t tSize, uint8_t hasMetaData -) + ) { int errCode = -EPROTONOSUPPORT; int lengthParsed; @@ -90,47 +90,55 @@ static int OSP_ParseSensorDataPkt_Private( case PSENSOR_MAGNETIC_FIELD_RAW: case PSENSOR_GYROSCOPE_RAW: if ((sensSubType == SENSOR_SUBTYPE_UNUSED) && - (dSize == DATA_SIZE_16_BIT) && - (dFormat == DATA_FORMAT_RAW) && - (timeFormat == TIME_FORMAT_RAW)) { + (dSize == DATA_SIZE_16_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_RAW)) { /* Extract Raw sensor data from packet */ - pOut->SType = (ASensorType_t)M_PSensorToAndroidBase(sensType); + pOut->SType = (ASensorType_t)M_PSensorToAndroidBase( + sensType); pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.RawSensor.Axis[0] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[0], - pHif->SensPktRaw.DataRaw[1]); + pHif->SensPktRaw.DataRaw[0], + pHif->SensPktRaw.DataRaw[1]); pOut->P.RawSensor.Axis[1] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[2], - pHif->SensPktRaw.DataRaw[3]); + pHif->SensPktRaw.DataRaw[2], + pHif->SensPktRaw.DataRaw[3]); pOut->P.RawSensor.Axis[2] = BYTES_TO_SHORT( - pHif->SensPktRaw.DataRaw[4], - pHif->SensPktRaw.DataRaw[5]); - + pHif->SensPktRaw.DataRaw[4], + pHif->SensPktRaw.DataRaw[5]); + /* Extract time stamp */ - pOut->P.RawSensor.TStamp.TS64 = 0; //helps clear higher 32-bit - pOut->P.RawSensor.TStamp.TS8[3] = pHif->SensPktRaw.TimeStamp[0]; //MSB - pOut->P.RawSensor.TStamp.TS8[2] = pHif->SensPktRaw.TimeStamp[1]; - pOut->P.RawSensor.TStamp.TS8[1] = pHif->SensPktRaw.TimeStamp[2]; - pOut->P.RawSensor.TStamp.TS8[0] = pHif->SensPktRaw.TimeStamp[3]; //LSB + pOut->P.RawSensor.TStamp.TS64 = 0; //helps clear higher + // 32-bit + pOut->P.RawSensor.TStamp.TS8[3] = + pHif->SensPktRaw.TimeStamp[0]; //MSB + pOut->P.RawSensor.TStamp.TS8[2] = + pHif->SensPktRaw.TimeStamp[1]; + pOut->P.RawSensor.TStamp.TS8[1] = + pHif->SensPktRaw.TimeStamp[2]; + pOut->P.RawSensor.TStamp.TS8[0] = + pHif->SensPktRaw.TimeStamp[3]; //LSB //TODO: 64-bit time stamp extension?? errCode = 0; lengthParsed = SENSOR_RAW_DATA_PKT_SZ; } break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract uncalibrated sensor data from packet */ - pOut->SType = (ASensorType_t)M_PSensorToAndroidBase(sensType); + pOut->SType = (ASensorType_t)M_PSensorToAndroidBase( + sensType); pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.UncalFixP.Axis[0] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data,0); + pHif->UncalPktFixP.Data,0); pOut->P.UncalFixP.Axis[1] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 4); + pHif->UncalPktFixP.Data, 4); pOut->P.UncalFixP.Axis[2] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 8); + pHif->UncalPktFixP.Data, 8); /* Check if META_DATA is set to 0x01 then read offset */ if (hasMetaData) { @@ -148,16 +156,18 @@ static int OSP_ParseSensorDataPkt_Private( for (i = 0; i < sizeof(uint64_t); i++) { /* Copy LSB to MSB data - remember * that HIF packets are Big-Endian formatted */ - pOut->P.UncalFixP.TimeStamp.TS8[i] = pHif->UncalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pOut->P.UncalFixP.TimeStamp.TS8[i] = + pHif->UncalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; } break; + case SYSTEM_REAL_TIME_CLOCK: - printk("HY-DBG: %s:%i type %i\n", __func__, __LINE__, sensType); break; + default: - printk("HY-DBG: %s:%i type %i\n", __func__, __LINE__, sensType); break; } if (errCode == 0) @@ -174,9 +184,9 @@ static int OSP_ParseSensorDataPkt_Android( uint8_t dSize, uint8_t dFormat, uint8_t timeFormat, - uint8_t tSize, + uint8_t tSize, uint8_t hasMetaData -) + ) { int errCode = -EPROTONOSUPPORT; int lengthParsed; @@ -186,22 +196,19 @@ static int OSP_ParseSensorDataPkt_Android( case SENSOR_ACCELEROMETER: case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: - case SENSOR_ORIENTATION: - case SENSOR_LINEAR_ACCELERATION: - case SENSOR_GRAVITY: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract sensor data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.CalFixP.Axis[0] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 0); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 0); pOut->P.CalFixP.Axis[1] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 4); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 4); pOut->P.CalFixP.Axis[2] = - BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 8); + BYTES_TO_LONG_ARR(pHif->CalPktFixP.Data, 8); /* Extract fixed point time stamp */ for (i = 0; i < sizeof(uint64_t); i++) { @@ -210,62 +217,62 @@ static int OSP_ParseSensorDataPkt_Android( * Big-Endian formatted */ pOut->P.CalFixP.TimeStamp.TS8[i] = - pHif->CalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pHif->CalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; lengthParsed = CALIBRATED_FIXP_DATA_PKT_SZ; } break; - case SENSOR_GAME_ROTATION_VECTOR: - case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: case SENSOR_ROTATION_VECTOR: + case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: + case SENSOR_GAME_ROTATION_VECTOR: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract Quaternion data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.QuatFixP.Quat[0] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 0); + pHif->QuatPktFixP.Data, 0); pOut->P.QuatFixP.Quat[1] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 4); + pHif->QuatPktFixP.Data, 4); pOut->P.QuatFixP.Quat[2] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 8); + pHif->QuatPktFixP.Data, 8); pOut->P.QuatFixP.Quat[3] = BYTES_TO_LONG_ARR( - pHif->QuatPktFixP.Data, 12); + pHif->QuatPktFixP.Data, 12); /* Extract fixed point time stamp */ for (i = 0; i < sizeof(uint64_t); i++) { /* Copy LSB to MSB data - remember * that HIF packets are Big-Endian formatted */ - pOut->P.QuatFixP.TimeStamp.TS8[i] = pHif->QuatPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pOut->P.QuatFixP.TimeStamp.TS8[i] = + pHif->QuatPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; lengthParsed = QUATERNION_FIXP_DATA_PKT_SZ; } break; - case SENSOR_SIGNIFICANT_MOTION: - printk("HY-DBG: %s:%i\n", __func__, __LINE__); + case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_GYROSCOPE_UNCALIBRATED: case SENSOR_PRESSURE: - case SENSOR_STEP_DETECTOR: - case SENSOR_STEP_COUNTER: if ((dSize == DATA_SIZE_32_BIT) && - (dFormat == DATA_FORMAT_FIXPOINT) && - (timeFormat == TIME_FORMAT_FIXPOINT) && - (tSize == TIME_STAMP_64_BIT)) { + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { /* Extract Quaternion data from packet */ pOut->SType = (ASensorType_t)sensType; pOut->SubType = SENSOR_SUBTYPE_UNUSED; pOut->P.UncalFixP.Axis[0] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 0); + pHif->UncalPktFixP.Data, 0); pOut->P.UncalFixP.Axis[1] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 4); + pHif->UncalPktFixP.Data, 4); pOut->P.UncalFixP.Axis[2] = BYTES_TO_LONG_ARR( - pHif->UncalPktFixP.Data, 8); + pHif->UncalPktFixP.Data, 8); /* Check if META_DATA is set to 0x01 then read offset */ if (hasMetaData) { @@ -284,39 +291,176 @@ static int OSP_ParseSensorDataPkt_Android( /* Copy LSB to MSB data - remember that * HIF packets are Big-Endian formatted */ pOut->P.UncalFixP.TimeStamp.TS8[i] = - pHif->UncalPktFixP.TimeStamp[sizeof(uint64_t)-i-1]; + pHif->UncalPktFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; } errCode = 0; } break; - default: - { - int i; - unsigned char *p; - printk("HY-DBG: %s:%i type %i: ", __func__, __LINE__, sensType); - p = (unsigned char *)pHif; - for (i = 0; i < 8; i++) { - printk("%02x ", p[i]); + case SENSOR_SIGNIFICANT_MOTION: + if ((dSize == DATA_SIZE_8_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract SignificantMotion data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.SigMotion.MotionDetected = + pHif->SignificantMotion. + significantMotionDetected; + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.SigMotion.TimeStamp.TS8[i] = + pHif->SignificantMotion.TimeStamp[ + sizeof(uint64_t) - i-1]; } - printk("\n"); - + errCode = 0; + lengthParsed = SIGNIFICANTMOTION_FIXP_DATA_PKT_SZ; + } + break; + + case SENSOR_STEP_DETECTOR: + if ((dSize == DATA_SIZE_8_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract StepDetector data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.StepDetector.StepDetected = + pHif->StepDetector.stepDetected; + + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.StepDetector.TimeStamp.TS8[i] = + pHif->StepDetector.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = STEPDETECTOR_DATA_PKT_SZ; + } + break; + + case SENSOR_STEP_COUNTER: + if ((dSize == DATA_SIZE_64_BIT) && + (dFormat == DATA_FORMAT_RAW) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract StepCounter data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.StepCount.NumStepsTotal = BYTES_TO_LONGLONG( + pHif->StepCounter.NumStepsTotal[0], + pHif->StepCounter.NumStepsTotal[1], + pHif->StepCounter.NumStepsTotal[2], + pHif->StepCounter.NumStepsTotal[3], + pHif->StepCounter.NumStepsTotal[4], + pHif->StepCounter.NumStepsTotal[5], + pHif->StepCounter.NumStepsTotal[6], + pHif->StepCounter.NumStepsTotal[7]); + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.StepCount.TimeStamp.TS8[i] = + pHif->StepCounter.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = STEPCOUNTER_DATA_PKT_SZ; + } + break; + + case SENSOR_ORIENTATION: + if ((dSize == DATA_SIZE_32_BIT) && + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract OrientationFixP data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.OrientFixP.Pitch = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Pitch, 0); + pOut->P.OrientFixP.Roll = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Roll, 0); + pOut->P.OrientFixP.Yaw = BYTES_TO_LONG_ARR( + pHif->OrientationFixP.Yaw, 0); + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.OrientFixP.TimeStamp.TS8[i] = + pHif->OrientationFixP.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = ORIENTATION_FIXP_DATA_PKT_SZ; + } + break; + + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_GRAVITY: + if ((dSize == DATA_SIZE_32_BIT) && + (dFormat == DATA_FORMAT_FIXPOINT) && + (timeFormat == TIME_FORMAT_FIXPOINT) && + (tSize == TIME_STAMP_64_BIT)) { + /* Extract ThreeAxisFixp data from packet */ + pOut->SType = (ASensorType_t)sensType; + pOut->SubType = SENSOR_SUBTYPE_UNUSED; + pOut->P.ThreeAxisFixP.Axis[0] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 0); + + pOut->P.ThreeAxisFixP.Axis[1] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 4); + + pOut->P.ThreeAxisFixP.Axis[2] = BYTES_TO_LONG_ARR( + pHif->ThreeAxisFixp.Data, 8); + //TODO: How to handle the Accuracy here ? Leave it in + // the Metadeta ? + /* Extract fixed point time stamp */ + for (i = 0; i < sizeof(uint64_t); i++) { + /* Copy LSB to MSB data - remember that HIF + packets are Big-Endian formatted */ + pOut->P.ThreeAxisFixP.TimeStamp.TS8[i] = + pHif->ThreeAxisFixp.TimeStamp[ + sizeof(uint64_t) - i-1]; + } + errCode = 0; + lengthParsed = THREEAXIS_FIXP_DATA_PKT_SZ; } break; + + default: + { + int i; + unsigned char *p; + p = (unsigned char *)pHif; + for (i = 0; i < 8; i++) { + printk("%02x ", p[i]); + } + printk("\n"); + } + break; } if (errCode == 0) return lengthParsed; else return errCode; } + static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, - uint8_t *pPacket, uint16_t pktSize) + uint8_t *pPacket, uint16_t pktSize) { HostIFPackets_t *pHif = (HostIFPackets_t*)pPacket; int errCode = -EPROTONOSUPPORT; uint8_t sensType; uint8_t sensSubType, dSize, dFormat, timeFormat, - tSize, isPrivateType, hasMetaData; + tSize, isPrivateType, hasMetaData; /* Sanity... */ if ((pOut == NULL) || (pPacket == NULL)) @@ -326,104 +470,57 @@ static int16_t OSP_ParseSensorDataPkt(SensorPacketTypes_t *pOut, sensType = M_SensorType(pHif->SensPktRaw.Q.SensorIdByte); sensSubType = M_ParseSensorSubType(pHif->SensPktRaw.Q.AttributeByte); isPrivateType = pHif->SensPktRaw.Q.ControlByte & - SENSOR_ANDROID_TYPE_MASK; + SENSOR_ANDROID_TYPE_MASK; hasMetaData = M_ParseSensorMetaData (pHif->SensPktRaw.Q.SensorIdByte); dSize = pHif->SensPktRaw.Q.AttributeByte & DATA_SIZE_MASK; dFormat = pHif->SensPktRaw.Q.ControlByte & DATA_FORMAT_MASK; timeFormat = pHif->SensPktRaw.Q.ControlByte & TIME_FORMAT_MASK; tSize = pHif->SensPktRaw.Q.AttributeByte & TIME_STAMP_SIZE_MASK; - /* Check Sensor enumeration type Android or Private */ + /* Check Sensor enumeration type Android or Private */ if (!isPrivateType) { /*Sensor Enumeration type is Android*/ errCode = OSP_ParseSensorDataPkt_Android(pOut, pHif, - (ASensorType_t)sensType, - sensSubType, dSize, - dFormat, timeFormat, tSize, - hasMetaData); + (ASensorType_t)sensType, + sensSubType, dSize, + dFormat, timeFormat, tSize, + hasMetaData); } else { /*Sensor Enumeration type is Private*/ errCode = OSP_ParseSensorDataPkt_Private(pOut, pHif, - (ASensorType_t)sensType, - sensSubType, dSize, - dFormat, timeFormat, tSize, - hasMetaData); - - + (ASensorType_t)sensType, + sensSubType, dSize, + dFormat, timeFormat, tSize, + hasMetaData); } return errCode; } + /* ------------ END OSP Packet parsing code -------------------- */ -struct osp_data { - struct i2c_client *client; - struct input_dev *input_dev; - struct input_polled_dev *poll_dev; - struct input_dev *in_dev[NUM_ANDROID_SENSOR_TYPE]; - struct input_dev *in_pdev[NUM_PRIVATE_SENSOR_TYPE]; - struct timer_list osp_timer; - struct work_struct osp_work; -}; +static int OSP_Sensor_enable(struct osp_data *osp, int sensor, int private) +{ + int retval = -EINVAL; -static struct osp_data *gOSP; + if (private != 0 && !(osp->features & FEAT_PRIVATE)) return -EINVAL; -static struct work_queue *osp_workq; + if (sensor < 0x30) + retval = i2c_smbus_read_byte_data(osp->client, 0x20+sensor); -static ssize_t osp_get_enable(struct device *dev, struct device_attribute *attr, char *buf) -{ - return 0; + return retval; } -static ssize_t osp_set_enable(struct device *dev, struct device_attribute *attr, - const char *buf, size_t count) +static int OSP_Sensor_disable(struct osp_data *osp, int sensor, int private) { - struct input_dev *input = to_input_dev(dev); - struct osp_data *osp = input_get_drvdata(input); - int i; - int retval; - unsigned long v; - - if (strict_strtoul(buf, 0, &v)) - return -EINVAL; - - for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == input) { - break; - } - } + int retval = -EINVAL; - if (i == NUM_ANDROID_SENSOR_TYPE) - return -EINVAL; + if (private != 0 && !(osp->features & FEAT_PRIVATE)) return -EINVAL; - printk("HY-DBG: %s:%i - osp %p\n", __func__, __LINE__, osp); - if (v) { - printk("HY-DBG: %s:%i - enabling %i\n", __func__, __LINE__, i); - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x20+i); - } - } else { - printk("HY-DBG: %s:%i - disabling %i\n", __func__, __LINE__, i); - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x50+i); - } - } + if (sensor < 0x30) + retval = i2c_smbus_read_byte_data(osp->client, 0x50+sensor); - return count; + return retval; } -#if 0 -static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR, osp_get_enable, osp_set_enable); -#else -static DEVICE_ATTR(enable_new, S_IRUGO|S_IWUSR|S_IWUGO, osp_get_enable, osp_set_enable); -#endif - -static struct attribute *osp_attributes[] = { - &dev_attr_enable_new.attr, - NULL -}; - -static struct attribute_group osp_attribute_group = { - .attrs = osp_attributes -}; static int osp_i2c_read(struct osp_data *osp, u8 addr, u8 *data, int len) { @@ -474,394 +571,273 @@ static int osp_i2c_read(struct osp_data *osp, u8 addr, u8 *data, int len) #endif } -#if 0 -static irqreturn_t osp_isr(int irq, void *dev) -{ - struct osp_data *osp = dev; - int err; - - /* data ready is the only possible interrupt type */ - osp_report_acceleration_data(tj9); - err = i2c_smbus_read_byte_data(tj9->client, INT_REL); - if (err < 0) - dev_err(&osp->client->dev, - "error clearing interrupt status: %d\n", err); - - return IRQ_HANDLED; -} -#endif -static int osp_enable(struct osp_data *osp) -{ - return 0; -} static void osp_disable(struct osp_data *osp) { } -static int osp_input_open(struct input_dev *input) +/* ------------ Call back management code -------------- */ +static void OSP_CB_init(void) { - struct osp_data *osp = input_get_drvdata(input); -#if 0 int i; - int retval; for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == input) { - break; - } + and_sensor[i].dataready = NULL; } - printk("HY-DBG: %s:%i - closing %i\n", __func__, __LINE__, i); - - if (i == NUM_ANDROID_SENSOR_TYPE) - return 0; - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x20+i); + for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { + prv_sensor[i].dataready = NULL; } -#endif - return osp_enable(osp); } -static void osp_input_close(struct input_dev *dev) +/* API Call: Register a callback for a sensor */ +int OSP_Sensor_Register(int sensor, int private, + void (*dataready)(int sensor, int prv, + void *private, long long ts, union OSP_SensorData *sensordata), + void *prv) { - struct osp_data *osp = input_get_drvdata(dev); -#if 0 - int i; - int retval; + struct OSP_Sensor *sen; - for (i = 0; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (osp->in_dev[i] == dev) { - break; - } - } + if (private == 1) + sen = prv_sensor; + else if (private == 0) + sen = and_sensor; + else + return -EINVAL; - printk("HY-DBG: %s:%i - closing %i\n", __func__, __LINE__, i); + if (sen[sensor].dataready != NULL) + return -EBUSY; - if (i == NUM_ANDROID_SENSOR_TYPE) - return; - if (i < 0x30) { - retval = i2c_smbus_read_byte_data(osp->client, 0x50+i); - } -#endif - osp_disable(osp); -} + sen[sensor].dataready = dataready; + sen[sensor].private = prv; -static void osp_init_input_device(struct osp_data *osp, - struct input_dev *input_dev, char *name) -{ - __set_bit(EV_ABS, input_dev->evbit); - input_set_abs_params(input_dev, ABS_X, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_Y, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_Z, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_THROTTLE, -G_MAX, G_MAX, FUZZ, FLAT); - input_set_abs_params(input_dev, ABS_GAS, -G_MAX, G_MAX, FUZZ, FLAT); - - input_dev->name = name; - input_dev->id.bustype = BUS_I2C; - input_dev->dev.parent = &osp->client->dev; - input_dev->relbit[0] |= BIT_MASK(REL_X); + return 0; } -static int osp_setup_other_input(struct osp_data *osp) +EXPORT_SYMBOL_GPL(OSP_Sensor_Register); + +/* API Call: Release a sensor */ +int OSP_Sensor_UnRegister(int sensor, int private) { - int i = 0, j = 0; - struct input_dev *input_dev; - int err; + struct OSP_Sensor *sen; - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - osp->in_dev[i] = NULL; - } - for (i = 0; i < NUM_PRIVATE_SENSOR_TYPE; i++) { - osp->in_pdev[i] = NULL; - } + if (private == 1) + sen = prv_sensor; + else if (private == 0) + sen = and_sensor; + else + return -EINVAL; - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - err = -ENOMEM; - goto err_input_alloc; - } else { - osp->in_dev[i] = input_dev; - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - osp_init_input_device(osp, input_dev, in_name[i]); - } - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_pname[j] == NULL) - continue; - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - err = -ENOMEM; - goto err_input_alloc; - } else { - osp->in_pdev[j] = input_dev; - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - osp_init_input_device(osp, input_dev, in_pname[j]); - } - } - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - err = input_register_device(osp->in_dev[i]); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->in_dev[i]->name, err); - goto err_input_reg; - } - err = sysfs_create_group(&osp->in_dev[i]->dev.kobj, &osp_attribute_group); - if (err) { - printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); - } - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_pname[j] == NULL) - continue; - err = input_register_device(osp->in_pdev[j]); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->in_pdev[j]->name, err); - goto err_input_reg; - } - } + sen->dataready = NULL; return 0; -err_input_reg: - i--; - for (; i > 1; i--) { - if (in_name[i] == NULL) - continue; - input_unregister_device(osp->in_dev[i]); - } - if (j != 0) { - j--; - for (; j > 1; j--) { - if (in_pname[j] == NULL) - continue; - input_unregister_device(osp->in_dev[j]); - } - } - -err_input_alloc: - for (i = 2; i < NUM_ANDROID_SENSOR_TYPE; i++) { - if (in_name[i] == NULL) - continue; - if (osp->in_dev) - input_free_device(osp->in_dev[i]); - } - for (j = 0; j < NUM_PRIVATE_SENSOR_TYPE; j++) { - if (in_name[j] == NULL) - continue; - if (osp->in_dev) - input_free_device(osp->in_pdev[j]); - } - - return err; } -static int osp_setup_input_device(struct osp_data *osp) -{ - struct input_dev *input_dev; - int err; - input_dev = input_allocate_device(); - if (!input_dev) { - dev_err(&osp->client->dev, "input device allocate failed\n"); - return -ENOMEM; - } - - osp->input_dev = input_dev; - osp->in_dev[1] = input_dev; - - input_dev->open = osp_input_open; - input_dev->close = osp_input_close; - input_set_drvdata(input_dev, osp); - - osp_init_input_device(osp, input_dev, in_name[1]); +EXPORT_SYMBOL_GPL(OSP_Sensor_UnRegister); +/* API Call: Activates/suspends a sensor */ +int OSP_Sensor_State(int sensor, int private, int state) +{ + if (private != 1 && private != 0) + return -EINVAL; - err = input_register_device(osp->input_dev); - if (err) { - dev_err(&osp->client->dev, - "unable to register input polled device %s: %d\n", - osp->input_dev->name, err); - input_free_device(osp->input_dev); - return err; + if (state == 1) { + OSP_Sensor_enable(gOSP, sensor, private); + } else { + OSP_Sensor_disable(gOSP, sensor, private); } - err = sysfs_create_group(&osp->in_dev[1]->dev.kobj, &osp_attribute_group); - if (err) { - printk("HY-DBG: %s:%i - err = %i\n", __func__, __LINE__, err); - } - osp_setup_other_input(osp); return 0; } -static void OSP_ReportSensor(struct osp_data *osp, - SensorPacketTypes_t *spack) +EXPORT_SYMBOL_GPL(OSP_Sensor_State); + +static void OSP_ReportSensor(struct osp_data *osp, + SensorPacketTypes_t *spack) { int PSensor; - static int counter = 0; + static int sig_counter = 0, step_counter = 0; + union OSP_SensorData data; - /* printk("HY-DBG:%s:%i(%i)\n", __func__,__LINE__, spack->SType); */ switch(spack->SType) { case SENSOR_STEP_DETECTOR: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.StepDetector.StepDetected; + data.xyz.y = step_counter; + step_counter++; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.StepDetector.TimeStamp.TS64, &data); + break; + case SENSOR_SIGNIFICANT_MOTION: - printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.SigMotion.MotionDetected; + data.xyz.y = sig_counter; + sig_counter++; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.SigMotion.TimeStamp.TS64, &data); + break; - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, 1); - input_report_abs(osp->in_dev[spack->SType], ABS_GAS, counter); - counter++; - input_sync(osp->in_dev[spack->SType]); + case SENSOR_PRESSURE: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.CalFixP.Axis[0]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.CalFixP.TimeStamp.TS64, &data); break; + case SENSOR_STEP_COUNTER: - case SENSOR_PRESSURE: - printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = (uint32_t)spack->P.StepCount.NumStepsTotal; + data.xyz.y = (uint32_t)(spack->P.StepCount.NumStepsTotal >> 32); + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.StepCount.TimeStamp.TS64, &data); + break; case SENSOR_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_GYROSCOPE_UNCALIBRATED: - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.UncalFixP.Axis[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.UncalFixP.Axis[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.UncalFixP.Axis[2]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.UncalFixP.Axis[0]; + data.xyz.y = spack->P.UncalFixP.Axis[1]; + data.xyz.z = spack->P.UncalFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.UncalFixP.TimeStamp.TS64, &data); break; + case SENSOR_ACCELEROMETER: case SENSOR_MAGNETIC_FIELD: case SENSOR_GYROSCOPE: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.CalFixP.Axis[0]; + data.xyz.y = spack->P.CalFixP.Axis[1]; + data.xyz.z = spack->P.CalFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.CalFixP.TimeStamp.TS64, &data); + break; + case SENSOR_LINEAR_ACCELERATION: + case SENSOR_GRAVITY: + if (!and_sensor[spack->SType].dataready) break; + data.xyz.x = spack->P.ThreeAxisFixP.Axis[0]; + data.xyz.y = spack->P.ThreeAxisFixP.Axis[1]; + data.xyz.z = spack->P.ThreeAxisFixP.Axis[2]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.ThreeAxisFixP.TimeStamp.TS64, &data); + break; + case SENSOR_ORIENTATION: - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType], spack->P.CalFixP.Axis[0], spack->P.CalFixP.Axis[1], spack->P.CalFixP.Axis[2]); */ - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.CalFixP.Axis[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.CalFixP.Axis[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.CalFixP.Axis[2]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.xyz.y = spack->P.OrientFixP.Pitch; + data.xyz.z = spack->P.OrientFixP.Roll; + data.xyz.x = spack->P.OrientFixP.Yaw; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.OrientFixP.TimeStamp.TS64, &data); break; + case SENSOR_ROTATION_VECTOR: case SENSOR_GEOMAGNETIC_ROTATION_VECTOR: case SENSOR_GAME_ROTATION_VECTOR: - /* printk("HY-DBG:%s:%i(%i) %p\n", __func__,__LINE__, spack->SType, osp->in_dev[spack->SType]); */ - if (!osp->in_dev[spack->SType]) break; - input_report_abs(osp->in_dev[spack->SType], ABS_GAS, spack->P.QuatFixP.Quat[0]); - input_report_abs(osp->in_dev[spack->SType], ABS_X, spack->P.QuatFixP.Quat[1]); - input_report_abs(osp->in_dev[spack->SType], ABS_Y, spack->P.QuatFixP.Quat[2]); - input_report_abs(osp->in_dev[spack->SType], ABS_Z, spack->P.QuatFixP.Quat[3]); - input_sync(osp->in_dev[spack->SType]); + if (!and_sensor[spack->SType].dataready) break; + data.quat.r = spack->P.QuatFixP.Quat[0]; + data.quat.x = spack->P.QuatFixP.Quat[1]; + data.quat.y = spack->P.QuatFixP.Quat[2]; + data.quat.z = spack->P.QuatFixP.Quat[3]; + and_sensor[spack->SType].dataready(spack->SType, 0, + and_sensor[spack->SType].private, + spack->P.QuatFixP.TimeStamp.TS64, &data); break; + case PSENSOR_ACCELEROMETER_UNCALIBRATED|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_pdev[PSensor], spack->P.UncalFixP.Axis[0], spack->P.UncalFixP.Axis[1], spack->P.UncalFixP.Axis[2]); */ - if (!osp->in_pdev[PSensor]) break; - input_report_abs(osp->in_pdev[PSensor], ABS_X, spack->P.UncalFixP.Axis[0]); - input_report_abs(osp->in_pdev[PSensor], ABS_Y, spack->P.UncalFixP.Axis[1]); - input_report_abs(osp->in_pdev[PSensor], ABS_Z, spack->P.UncalFixP.Axis[2]); - input_sync(osp->in_pdev[PSensor]); + if (!prv_sensor[PSensor].dataready) break; + + data.xyz.x = spack->P.UncalFixP.Axis[0]; + data.xyz.y = spack->P.UncalFixP.Axis[1]; + data.xyz.z = spack->P.UncalFixP.Axis[2]; + prv_sensor[PSensor].dataready(PSensor, 1, + prv_sensor[PSensor].private, + spack->P.UncalFixP.TimeStamp.TS64, &data); break; + case PSENSOR_ACCELEROMETER_RAW|SENSOR_DEVICE_PRIVATE_BASE: PSensor = spack->SType & ~(SENSOR_DEVICE_PRIVATE_BASE); - /* printk("HY-DBG:%s:%i(%i) %p %i %i %i\n", __func__,__LINE__, spack->SType, osp->in_pdev[PSensor], spack->P.RawSensor.Axis[0], spack->P.RawSensor.Axis[1], spack->P.RawSensor.Axis[2]); */ - if (!osp->in_pdev[PSensor]) break; - input_report_abs(osp->in_pdev[PSensor], ABS_X, spack->P.RawSensor.Axis[0]); - input_report_abs(osp->in_pdev[PSensor], ABS_Y, spack->P.RawSensor.Axis[1]); - input_report_abs(osp->in_pdev[PSensor], ABS_Z, spack->P.RawSensor.Axis[2]); - input_sync(osp->in_pdev[PSensor]); + if (!prv_sensor[PSensor].dataready) break; + data.xyz.x = spack->P.RawSensor.Axis[0]; + data.xyz.y = spack->P.RawSensor.Axis[1]; + data.xyz.z = spack->P.RawSensor.Axis[2]; + prv_sensor[PSensor].dataready(PSensor, 1, + prv_sensor[PSensor].private, + spack->P.RawSensor.TStamp.TS64, &data); break; + default: break; } - /* printk("HY-DBG:%s:%i(%i)\n", __func__,__LINE__, spack->SType); */ } -static unsigned char *osp_pack; -static unsigned char *osp_pack2; - +/* Attempt to grab 2 packets in a row to maximize through put. + * I2C is a slow bus (can sleep during xfers) and may take longer then + * the time it takes to create the data on the hub if we wait too long. + */ static void osp_work_q(struct work_struct *work) { struct osp_data *osp = container_of(work, struct osp_data, osp_work); - uint16_t plen = 0, plen_old = 0; - uint16_t plen2 = 0, plen2_old = 0; + SensorPacketTypes_t spack; unsigned char *pack_ptr; int pack_count; - SensorPacketTypes_t spack; - int err; int ret; uint32_t intlen; - int gotpack1 = 0; - int gotpack2 = 0; - - ret = osp_i2c_read(osp, OSP_INT_LEN, (unsigned char *)&intlen, 4); - if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { - plen = (intlen >> 4); - plen_old = plen; - if (plen>0 && plen < 8192) { - ret = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack, plen); - if (ret < 0) return; - gotpack1 = 1; + int i; + int gotpack[NUM_PACK] = {0,0}; + uint16_t plen[NUM_PACK] = {0,0}; + + /* Grab buffers as quickly as possible */ + for (i = 0; i < NUM_PACK; i++) { + ret = osp_i2c_read(osp, OSP_INT_LEN, + (unsigned char *)&intlen, 4); + if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { + plen[i] = (intlen >> 4); + if (plen[i] > 0 && plen[i] < 8192) { + ret = osp_i2c_read(osp, OSP_DATA_OUT, + osp_pack[i], plen[i]); + if (ret < 0) return; + gotpack[i] = 1; + } } } - - ret = osp_i2c_read(osp, OSP_INT_LEN, (unsigned char *)&intlen, 4); - if ((ret >= 0) && (intlen&OSP_INT_DRDY)) { - plen2 = (intlen >> 4); - plen2_old = plen2; - if (plen2>0 && plen2 < 8192) { - ret = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack2, plen2); - if (ret >= 0) - gotpack2 = 1; + for (i = 0; i < NUM_PACK; i++) { + if (gotpack[i]) { + pack_count = 0; + pack_ptr = osp_pack[i]; + do { + ret = OSP_ParseSensorDataPkt(&spack, pack_ptr, + plen[i]); + if (ret>0) { + OSP_ReportSensor(osp, &spack); + } else { + dev_err( + &osp->client->dev, + "OSP packet parsing error = %i, pack_count = %i plen = %i\n", + ret, pack_count, plen[i]); + break; + } + plen[i] -= ret; + pack_ptr += ret; + pack_count++; + } while (plen[i] > 0); } } - - if (gotpack1) { - pack_count = 0; - pack_ptr = osp_pack; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen); - if (err>0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("OSP packet parsing error = %i, pack_count = %i plen = %i, plen_old = %i\n", err, pack_count, plen, plen_old); - break; - } - plen -= err; - pack_ptr += err; - pack_count++; - } while (plen > 0 && plen2 < 8192); - } - if (gotpack2) { - pack_count = 0; - pack_ptr = osp_pack2; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen2); - if (err>0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("OSP packet parsing error = %i, pack_count = %i plen2 = %i, plen2_old = %i\n", err, pack_count, plen2, plen2_old); - return; - } - plen2 -= err; - pack_ptr += err; - pack_count++; - } while (plen2 > 0 && plen2 < 8192); - } - /* printk("HY-DBG: pack_count = %i\n", pack_count); */ } + static void osp_poll_timer(unsigned long _osp) { struct osp_data *osp = (void *)_osp; queue_work(osp_workq, &osp->osp_work); -#if 0 - mod_timer(&osp->osp_timer, jiffies+msecs_to_jiffies(20)); -#else mod_timer(&osp->osp_timer, jiffies+1); -#endif } static irqreturn_t osp_irq_thread(int irq, void *dev) @@ -869,152 +845,56 @@ static irqreturn_t osp_irq_thread(int irq, void *dev) struct osp_data *osp = dev; queue_work(osp_workq, &osp->osp_work); - return IRQ_HANDLED; } -static void osp_poll(struct input_polled_dev *dev) -{ -#if 0 - struct osp_data *osp = dev->private; - int retval; - int maxcount = 0x1ff; - uint16_t plen, plen_old; - unsigned char *pack_ptr; - int pack_count; - SensorPacketTypes_t spack; - int err; - - do { - retval = i2c_smbus_read_byte_data(osp->client, OSP_INT_REASON); - if (retval&OSP_INT_DRDY) { - osp_i2c_read(osp, OSP_DATA_LEN_L, (unsigned char *)&plen, 2); - pack_count = 0; - if (plen>0 && plen < 8192) { - pack_ptr = osp_pack; - retval = osp_i2c_read(osp, OSP_DATA_OUT, osp_pack, plen); - plen_old = plen; - do { - err = OSP_ParseSensorDataPkt(&spack, pack_ptr, plen); - if (err>=0) { - OSP_ReportSensor(osp, &spack); - } else { - printk("%s:%i: OSP packet parsing error = %i, pack_count = %i plen = %i, plen_old = %i\n", __func__, __LINE__, err, pack_count, plen, plen_old); - break; - } - plen -= err; - pack_ptr += err; - pack_count++; - } while (plen > 0); - } - /* printk("HY-DBG: pack_count = %i\n", pack_count); */ - } else - break; - maxcount--; - } while (maxcount); - if (maxcount < 0x3f) { - printk("WARNING - Maxcount = %i\n", maxcount); - if (!maxcount) - printk("Too busy in poll, maxcount = %i\n", maxcount); - } -#endif -} -static void osp_polled_input_open(struct input_polled_dev *dev) -{ - struct osp_data *osp = dev->private; - - osp_enable(osp); -} - -static void osp_polled_input_close(struct input_polled_dev *dev) -{ - struct osp_data *osp = dev->private; - - osp_disable(osp); -} - - -static int osp_setup_polled_device(struct osp_data *osp) -{ - int err; - struct input_polled_dev *poll_dev; - poll_dev = input_allocate_polled_device(); - - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - if (!poll_dev) { - dev_err(&osp->client->dev, - "Failed to allocate polled device\n"); - return -ENOMEM; - } - - osp->poll_dev = poll_dev; - osp->input_dev = poll_dev->input; - - poll_dev->private = osp; - poll_dev->poll = osp_poll; - poll_dev->open = osp_polled_input_open; - poll_dev->close = osp_polled_input_close; -#if 1 - poll_dev->poll_interval = 20; /* 50Hz */ -#else - poll_dev->poll_interval = 0; /* 50Hz */ -#endif - - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - osp_init_input_device(osp, poll_dev->input, in_name[1]); - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - - osp->in_dev[1] = poll_dev->input; - err = input_register_polled_device(poll_dev); - if (err) { - dev_err(&osp->client->dev, - "Unable to register polled device, err=%d\n", err); - input_free_polled_device(poll_dev); - return err; - } - - osp_setup_other_input(osp); - printk("HY-DBG: %s:%i\n", __func__, __LINE__); - - return 0; -} - -static void osp_teardown_polled_device(struct osp_data *osp) -{ - /* Need to add other input devices */ - input_unregister_polled_device(osp->poll_dev); - input_free_polled_device(osp->poll_dev); -} - +/* HY-DBG: Verify return code with latest firmwrae. */ static int osp_verify(struct osp_data *osp) { int retval; retval = i2c_smbus_read_byte_data(osp->client, OSP_WHOAMI); if (retval < 0) { - dev_err(&osp->client->dev, "read err int source, 0x%x\n", retval); + dev_err(&osp->client->dev, + "read err int source, 0x%x\n", retval); goto out; } - retval = (retval != 0x54) ? -EIO : 0; - retval = i2c_smbus_read_byte_data(osp->client, OSP_CONFIG); + retval = (retval != 0x54) ? -EIO : 0; + retval = 0; out: return retval; } + +static int OSP_add_child(struct osp_data *osp) +{ + struct platform_device *pdev; + + pdev = platform_device_alloc("osp-output", 0); + if (!pdev) { + printk("Cannot allocate dev\n"); + return -ENOMEM; + } + pdev->dev.parent = &osp->client->dev; + + return platform_device_add(pdev); +} + static int osp_probe(struct i2c_client *client, - const struct i2c_device_id *id) + const struct i2c_device_id *id) { struct osp_data *osp; + int err; + int i; if (!i2c_check_functionality(client->adapter, - I2C_FUNC_I2C | I2C_FUNC_SMBUS_BYTE_DATA)) { + I2C_FUNC_I2C | I2C_FUNC_SMBUS_BYTE_DATA)) { dev_err(&client->dev, "client is not i2c capable\n"); return -ENXIO; } - osp = kzalloc(sizeof(*osp), GFP_KERNEL); if (!osp) { dev_err(&client->dev, @@ -1022,17 +902,18 @@ static int osp_probe(struct i2c_client *client, return -ENOMEM; } gOSP = osp; - osp_pack = kzalloc(8192, GFP_KERNEL); - if (!osp_pack) { - dev_err(&client->dev, - "failed to allocate memory for packet data\n"); - return -ENOMEM; - } - osp_pack2 = kzalloc(8192, GFP_KERNEL); - if (!osp_pack2) { - dev_err(&client->dev, - "failed to allocate memory for packet data\n"); - return -ENOMEM; + mutex_init(&osp->lock); + for (i = 0; i < NUM_PACK; i++) { + osp_pack[i] = kzalloc(8192, GFP_KERNEL); + if (!osp_pack[i]) { + dev_err(&client->dev, + "failed to allocate memory for packet data\n"); + i--; + for (; i >= 0; i--) { + kfree(osp_pack[i]); + } + return -ENOMEM; + } } osp->client = client; @@ -1050,43 +931,31 @@ static int osp_probe(struct i2c_client *client, INIT_WORK(&osp->osp_work, osp_work_q); } -#if 0 - osp_setup_polled_device(osp); -#else - osp_setup_input_device(osp); -#endif - - client->irq = -1; /* Force it for now */ + client->irq = -1; /* Force it for now */ if (client->irq > 0) { - err = request_threaded_irq(client->irq, NULL, osp_irq_thread, IRQF_TRIGGER_LOW, "osp-sh", osp); + err = + request_threaded_irq(client->irq, NULL, osp_irq_thread, + IRQF_TRIGGER_LOW, + "osp-sh", + osp); if (err < 0) { dev_err(&client->dev, "irq request failed %d, error %d\n", client->irq, err); } - - } else { - err = -EINVAL; } - setup_timer(&osp->osp_timer, osp_poll_timer, (unsigned long)osp); - if (err < 0) { + if (client->irq <= 0 || err < 0) { + setup_timer(&osp->osp_timer, osp_poll_timer, (unsigned long)osp); mod_timer(&osp->osp_timer, jiffies + msecs_to_jiffies(10)); - } + } + + OSP_CB_init(); + /* Create child device */ + OSP_add_child(osp); return 0; -#if 0 -err_free_irq: - free_irq(client->irq, osp); -err_destroy_input: - input_unregister_device(osp->input_dev); -#endif -#if 0 -err_pdata_exit: - if (tj9->pdata.exit) - tj9->pdata.exit(); -#endif err_free_mem: kfree(osp); return err; @@ -1097,13 +966,7 @@ static int osp_remove(struct i2c_client *client) struct osp_data *osp = i2c_get_clientdata(client); if (client->irq) { -#if 0 - sysfs_remove_group(&client->dev.kobj, &kxtj9_attribute_group); -#endif free_irq(client->irq, osp); - input_unregister_device(osp->input_dev); - } else { - osp_teardown_polled_device(osp); } kfree(osp); @@ -1116,32 +979,18 @@ static int osp_suspend(struct device *dev) { struct i2c_client *client = to_i2c_client(dev); struct osp_data *osp = i2c_get_clientdata(client); - struct input_dev *input_dev = osp->input_dev; - - mutex_lock(&input_dev->mutex); - if (input_dev->users) - osp_disable(osp); - - mutex_unlock(&input_dev->mutex); + mutex_lock(&osp->lock); + osp_disable(osp); + mutex_unlock(&osp->lock); return 0; } static int osp_resume(struct device *dev) { - struct i2c_client *client = to_i2c_client(dev); - struct osp_data *osp = i2c_get_clientdata(client); - struct input_dev *input_dev = osp->input_dev; - int retval = 0; - - mutex_lock(&input_dev->mutex); - - if (input_dev->users) - osp_enable(osp); - - mutex_unlock(&input_dev->mutex); - return retval; + return 0; } + #endif static SIMPLE_DEV_PM_OPS(osp_pm_ops, osp_suspend, osp_resume); @@ -1170,11 +1019,11 @@ static int __init osp_init(void) { return i2c_add_driver(&osp_driver); } -module_init(osp_init); +module_init(osp_init); #endif MODULE_DESCRIPTION("OSP driver"); -MODULE_AUTHOR("Hunyue Yau "); +MODULE_AUTHOR("Hunyue Yau "); MODULE_LICENSE("GPL");