Ironcup Mar 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
SensorsLibrary/FXAS21002.cpp@22:b7cca3089dfe, 2020-09-21 (annotated)
- Committer:
- starling
- Date:
- Mon Sep 21 21:45:08 2020 +0000
- Revision:
- 22:b7cca3089dfe
- Parent:
- 14:e8cd237c8639
01 mar 2020
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
drelliak | 0:88faaa1afb83 | 1 | /* Copyright (c) 2015 NXP Semiconductors. MIT License |
drelliak | 0:88faaa1afb83 | 2 | * |
drelliak | 0:88faaa1afb83 | 3 | * Permission is hereby granted, free of charge, to any person obtaining a copy of this software |
drelliak | 0:88faaa1afb83 | 4 | * and associated documentation files (the "Software"), to deal in the Software without |
drelliak | 0:88faaa1afb83 | 5 | * restriction, including without limitation the rights to use, copy, modify, merge, publish, |
drelliak | 0:88faaa1afb83 | 6 | * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the |
drelliak | 0:88faaa1afb83 | 7 | * Software is furnished to do so, subject to the following conditions: |
drelliak | 0:88faaa1afb83 | 8 | * |
drelliak | 0:88faaa1afb83 | 9 | * The above copyright notice and this permission notice shall be included in all copies or |
drelliak | 0:88faaa1afb83 | 10 | * substantial portions of the Software. |
drelliak | 0:88faaa1afb83 | 11 | * |
drelliak | 0:88faaa1afb83 | 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING |
drelliak | 0:88faaa1afb83 | 13 | * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
drelliak | 0:88faaa1afb83 | 14 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
drelliak | 0:88faaa1afb83 | 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
drelliak | 0:88faaa1afb83 | 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
drelliak | 0:88faaa1afb83 | 17 | */ |
drelliak | 0:88faaa1afb83 | 18 | |
drelliak | 0:88faaa1afb83 | 19 | #include "FXAS21002.h" |
Alexandre Seidy |
9:bd0fb9d17803 | 20 | #include "mbed.h" |
drelliak | 0:88faaa1afb83 | 21 | FXAS21002::FXAS21002(PinName sda, PinName scl) : gyroi2c(sda,scl) |
Alexandre Seidy |
9:bd0fb9d17803 | 22 | { |
drelliak | 0:88faaa1afb83 | 23 | |
Alexandre Seidy |
9:bd0fb9d17803 | 24 | } |
Alexandre Seidy |
9:bd0fb9d17803 | 25 | |
drelliak | 12:273752f540be | 26 | void FXAS21002::set_gyro(gyro_mode mode) // Protected method |
drelliak | 12:273752f540be | 27 | { |
Alexandre Seidy |
9:bd0fb9d17803 | 28 | char d[2]; |
Alexandre Seidy |
9:bd0fb9d17803 | 29 | d[0] = FXAS21002_CTRL_REG1; //Puts device in standby mode |
Alexandre Seidy |
9:bd0fb9d17803 | 30 | d[1] = 0x08; |
Alexandre Seidy |
9:bd0fb9d17803 | 31 | gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2); |
Alexandre Seidy |
9:bd0fb9d17803 | 32 | |
Alexandre Seidy |
9:bd0fb9d17803 | 33 | d[0] = FXAS21002_CTRL_REG0; //Sets FSR and Sensitivity |
drelliak | 14:e8cd237c8639 | 34 | d[1] = mode + 0x80; |
Alexandre Seidy |
9:bd0fb9d17803 | 35 | gyroi2c.write(FXAS21002_I2C_ADDRESS, d, 2); |
Alexandre Seidy |
9:bd0fb9d17803 | 36 | |
Alexandre Seidy |
9:bd0fb9d17803 | 37 | d[0] = FXAS21002_CTRL_REG1; //Puts device in active mode |
Alexandre Seidy |
9:bd0fb9d17803 | 38 | d[1] = 0x0A; |
Alexandre Seidy |
9:bd0fb9d17803 | 39 | gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2); |
Alexandre Seidy |
9:bd0fb9d17803 | 40 | } |
Alexandre Seidy |
9:bd0fb9d17803 | 41 | |
drelliak | 12:273752f540be | 42 | |
drelliak | 12:273752f540be | 43 | void FXAS21002::stop_measure(void) |
drelliak | 12:273752f540be | 44 | { |
drelliak | 14:e8cd237c8639 | 45 | interrupt.detach(); |
drelliak | 14:e8cd237c8639 | 46 | angle = 0; |
drelliak | 12:273752f540be | 47 | } |
drelliak | 12:273752f540be | 48 | |
drelliak | 12:273752f540be | 49 | float FXAS21002::get_angle(void) |
drelliak | 12:273752f540be | 50 | { |
drelliak | 12:273752f540be | 51 | return angle; |
drelliak | 12:273752f540be | 52 | } |
drelliak | 12:273752f540be | 53 | |
drelliak | 12:273752f540be | 54 | void FXAS21002::integrate_gyro_angle(void) |
drelliak | 12:273752f540be | 55 | { |
drelliak | 12:273752f540be | 56 | float gyro_data[3]; |
drelliak | 12:273752f540be | 57 | acquire_gyro_data_dps(gyro_data); |
starling | 22:b7cca3089dfe | 58 | |
drelliak | 12:273752f540be | 59 | angle = angle + (gyro_data[2]-GYRO_OFFSET)*(period/1000000); |
drelliak | 12:273752f540be | 60 | if(angle > 180) |
drelliak | 12:273752f540be | 61 | angle = angle - 360; |
drelliak | 12:273752f540be | 62 | if(angle < -180) |
drelliak | 12:273752f540be | 63 | angle = angle + 360; |
drelliak | 12:273752f540be | 64 | } |
drelliak | 12:273752f540be | 65 | |
drelliak | 12:273752f540be | 66 | void FXAS21002::start_measure(float period_us) |
drelliak | 12:273752f540be | 67 | { |
drelliak | 12:273752f540be | 68 | period = period_us; |
drelliak | 12:273752f540be | 69 | interrupt.attach_us(this,&FXAS21002::integrate_gyro_angle,period); |
drelliak | 12:273752f540be | 70 | angle = 0; |
drelliak | 12:273752f540be | 71 | } |
drelliak | 12:273752f540be | 72 | |
Alexandre Seidy |
9:bd0fb9d17803 | 73 | void FXAS21002::gyro_config(void) |
Alexandre Seidy |
9:bd0fb9d17803 | 74 | { |
Alexandre Seidy |
11:7f569811a5f1 | 75 | set_gyro(MODE_2); //Default implementation |
Alexandre Seidy |
9:bd0fb9d17803 | 76 | sensitivity = 0.03125; |
Alexandre Seidy |
9:bd0fb9d17803 | 77 | } |
drelliak | 0:88faaa1afb83 | 78 | |
drelliak | 12:273752f540be | 79 | void FXAS21002::gyro_config(gyro_mode mode) |
drelliak | 12:273752f540be | 80 | { |
Alexandre Seidy |
9:bd0fb9d17803 | 81 | set_gyro(mode); |
Alexandre Seidy |
9:bd0fb9d17803 | 82 | switch(mode) |
Alexandre Seidy |
9:bd0fb9d17803 | 83 | { |
Alexandre Seidy |
9:bd0fb9d17803 | 84 | case MODE_1: sensitivity = 0.0625; break; |
Alexandre Seidy |
9:bd0fb9d17803 | 85 | case MODE_2: sensitivity = 0.03125; break; |
Alexandre Seidy |
9:bd0fb9d17803 | 86 | case MODE_3: sensitivity = 0.015625; break; |
Alexandre Seidy |
11:7f569811a5f1 | 87 | case MODE_4: sensitivity = 0.0078125; break; |
Alexandre Seidy |
9:bd0fb9d17803 | 88 | } |
Alexandre Seidy |
9:bd0fb9d17803 | 89 | } |
Alexandre Seidy |
9:bd0fb9d17803 | 90 | |
Alexandre Seidy |
9:bd0fb9d17803 | 91 | void FXAS21002::acquire_gyro_data_dps(float * g_data) |
Alexandre Seidy |
9:bd0fb9d17803 | 92 | { |
drelliak | 0:88faaa1afb83 | 93 | |
drelliak | 0:88faaa1afb83 | 94 | char data_bytes[7]; |
Alexandre Seidy |
9:bd0fb9d17803 | 95 | gyroi2c.write(FXAS21002_I2C_ADDRESS,FXAS21002_STATUS,1,true); // Read the 6 data bytes - LSB and MSB for X, Y and Z Axes. |
Alexandre Seidy |
9:bd0fb9d17803 | 96 | gyroi2c.read(FXAS21002_I2C_ADDRESS,data_bytes,7); |
Alexandre Seidy |
9:bd0fb9d17803 | 97 | |
Alexandre Seidy |
9:bd0fb9d17803 | 98 | g_data[0] = (float)((int16_t)((data_bytes[1]*256) + (data_bytes[2]))) * sensitivity; |
Alexandre Seidy |
9:bd0fb9d17803 | 99 | g_data[1] = (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * sensitivity; |
Alexandre Seidy |
9:bd0fb9d17803 | 100 | g_data[2] = (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * sensitivity; |
drelliak | 0:88faaa1afb83 | 101 | |
Alexandre Seidy |
9:bd0fb9d17803 | 102 | } |
drelliak | 0:88faaa1afb83 | 103 | |
drelliak | 0:88faaa1afb83 | 104 | |
drelliak | 0:88faaa1afb83 | 105 | |
drelliak | 0:88faaa1afb83 | 106 |