Bruno Allaire-Lemay / Mbed 2 deprecated APP1test

df

Dependencies:   mbed

Fork of APP1 by Team APP

Accelerometer.cpp@23:2531e72d92b9, 2017-02-09 (annotated)

Committer:
GaiSensei
Date:
Thu Feb 09 15:55:18 2017 +0000
Revision:
23:2531e72d92b9
Parent:
21:a111be2582be 
gh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dupm2216 21:a111be2582be 1 /////////////////////////////////////////////////////////////
dupm2216 21:a111be2582be 2 // APP 1: Systèmes à microprocesseurs //
dupm2216 21:a111be2582be 3 // //
dupm2216 21:a111be2582be 4 // Université de Sherbrooke //
dupm2216 21:a111be2582be 5 // Génie informatique //
dupm2216 21:a111be2582be 6 // Session 5, Hiver 2017 //
dupm2216 21:a111be2582be 7 // //
dupm2216 21:a111be2582be 8 // Date: 17 janvier 2017 //
dupm2216 21:a111be2582be 9 // //
dupm2216 21:a111be2582be 10 // Auteurs: Maxime Dupuis, dupm2216 //
dupm2216 21:a111be2582be 11 // Bruno Allaire-Lemay, allb2701 //
dupm2216 21:a111be2582be 12 /////////////////////////////////////////////////////////////
dupm2216 21:a111be2582be 13
dupm2216 1:7becb0e903e3 14 #include "Accelerometer.hpp"
dupm2216 6:3facf0329142 15 #include "Utility.hpp"
dupm2216 1:7becb0e903e3 16
dupm2216 8:862e28c7f6f6 17 #include <cmath>
dupm2216 8:862e28c7f6f6 18
dupm2216 7:1e00dfecc92d 19 namespace accelerometer
dupm2216 5:f59b51ac4b40 20 {
dupm2216 7:1e00dfecc92d 21 //Compute inverse two's complement to obtain a signed value
dupm2216 7:1e00dfecc92d 22 //See page 21: https://www.gel.usherbrooke.ca/s5info/h17/doc/app1/file/MMA8452Q.pdf
dupm2216 7:1e00dfecc92d 23 //See: https://en.wikipedia.org/wiki/Two's_complement
dupm2216 7:1e00dfecc92d 24 //Turns out, the signed char does it for free
dupm2216 7:1e00dfecc92d 25 //We have to specify "signed char" because the "standard does not specify if plain char is signed or unsigned"
dupm2216 7:1e00dfecc92d 26 //http://stackoverflow.com/a/2054941/3212785
dupm2216 7:1e00dfecc92d 27 int raw_axis_data_to_int(signed char raw_axis_data)
dupm2216 7:1e00dfecc92d 28 {
dupm2216 7:1e00dfecc92d 29 return raw_axis_data;
dupm2216 7:1e00dfecc92d 30 }
dupm2216 7:1e00dfecc92d 31
dupm2216 7:1e00dfecc92d 32 char get_axis_register(Axis axis)
dupm2216 7:1e00dfecc92d 33 {
dupm2216 7:1e00dfecc92d 34 switch(axis)
dupm2216 7:1e00dfecc92d 35 {
dupm2216 7:1e00dfecc92d 36 case AXIS_X: return OUT_X_MSB_REGISTER;
dupm2216 7:1e00dfecc92d 37 case AXIS_Y: return OUT_Y_MSB_REGISTER;
dupm2216 7:1e00dfecc92d 38 case AXIS_Z: return OUT_Z_MSB_REGISTER;
dupm2216 7:1e00dfecc92d 39 default: return AXIS_INVALID;
dupm2216 7:1e00dfecc92d 40 }
dupm2216 7:1e00dfecc92d 41 }
dupm2216 8:862e28c7f6f6 42
dupm2216 7:1e00dfecc92d 43 double g_force_from_int_axis_data(const int axis_data)
dupm2216 1:7becb0e903e3 44 {
dupm2216 7:1e00dfecc92d 45 return (double)axis_data / 64.0;
dupm2216 7:1e00dfecc92d 46 }
dupm2216 7:1e00dfecc92d 47
dupm2216 7:1e00dfecc92d 48 //Z axis is perpendicular to the horizontal plane, towards the floor when the accelerometer is flat
dupm2216 7:1e00dfecc92d 49 //Therefore,
dupm2216 7:1e00dfecc92d 50 // - if the Z force is +1g, the accelerometer is flat
dupm2216 7:1e00dfecc92d 51 // - if the Z force is -1g, the accelerometer is upside down
dupm2216 7:1e00dfecc92d 52 // - if the Z force is 0g, the accelerometer 90 degree from the horizontal
dupm2216 8:862e28c7f6f6 53 //cos(theta) = Z force in g
dupm2216 7:1e00dfecc92d 54 double angle_from_int_axis_data(const int axis_data)
dupm2216 7:1e00dfecc92d 55 {
dupm2216 7:1e00dfecc92d 56 const double z_g_force = g_force_from_int_axis_data(axis_data);
dupm2216 8:862e28c7f6f6 57 const double absolute_z_g_force = std::fabs(z_g_force);
dupm2216 8:862e28c7f6f6 58 if(absolute_z_g_force > 1.0)
dupm2216 8:862e28c7f6f6 59 {
dupm2216 8:862e28c7f6f6 60 return 0.0;
dupm2216 8:862e28c7f6f6 61 }
dupm2216 8:862e28c7f6f6 62
dupm2216 8:862e28c7f6f6 63 const double angle_radian = std::acos(absolute_z_g_force);
dupm2216 7:1e00dfecc92d 64 return utility::degree_from_radian(angle_radian);
dupm2216 1:7becb0e903e3 65 }
dupm2216 7:1e00dfecc92d 66
dupm2216 7:1e00dfecc92d 67 Accelerometer::Accelerometer(
dupm2216 7:1e00dfecc92d 68 PinName sda_pin,
dupm2216 7:1e00dfecc92d 69 PinName scl_pin,
dupm2216 19:f5aa0ce5546b 70 const int filter_size,
dupm2216 7:1e00dfecc92d 71 const int slave_address
dupm2216 7:1e00dfecc92d 72 ) :
dupm2216 7:1e00dfecc92d 73 device(sda_pin, scl_pin),
dupm2216 19:f5aa0ce5546b 74 filter(filter_size),
dupm2216 7:1e00dfecc92d 75 slave_address(slave_address)
dupm2216 7:1e00dfecc92d 76 {
dupm2216 7:1e00dfecc92d 77 }
dupm2216 7:1e00dfecc92d 78
dupm2216 7:1e00dfecc92d 79 void Accelerometer::write_register(const char register_address, const char new_value)
dupm2216 7:1e00dfecc92d 80 {
dupm2216 7:1e00dfecc92d 81 const int left_shifted_slave_address = slave_address << 1;
dupm2216 7:1e00dfecc92d 82 char data[2];
dupm2216 7:1e00dfecc92d 83 data[0] = register_address;
dupm2216 7:1e00dfecc92d 84 data[1] = new_value;
dupm2216 7:1e00dfecc92d 85
dupm2216 7:1e00dfecc92d 86 const int write_return = device.write(left_shifted_slave_address, data, 2);
dupm2216 7:1e00dfecc92d 87 if(write_return < 0)
dupm2216 7:1e00dfecc92d 88 {
dupm2216 7:1e00dfecc92d 89 printf("Write error: I2C error");
dupm2216 7:1e00dfecc92d 90 }
dupm2216 7:1e00dfecc92d 91 }
dupm2216 7:1e00dfecc92d 92
dupm2216 7:1e00dfecc92d 93 char Accelerometer::read_register(const char register_address)
dupm2216 1:7becb0e903e3 94 {
dupm2216 7:1e00dfecc92d 95 char result;
dupm2216 7:1e00dfecc92d 96 const int left_shifted_slave_address = slave_address << 1;
dupm2216 7:1e00dfecc92d 97
dupm2216 7:1e00dfecc92d 98 const int write_return = device.write(left_shifted_slave_address, &register_address, 1, true);
dupm2216 7:1e00dfecc92d 99 if(write_return < 0)
dupm2216 7:1e00dfecc92d 100 {
dupm2216 7:1e00dfecc92d 101 printf("Write error: I2C error");
dupm2216 7:1e00dfecc92d 102 }
dupm2216 7:1e00dfecc92d 103
dupm2216 7:1e00dfecc92d 104 const int read_return = device.read(left_shifted_slave_address, &result, 1);
dupm2216 7:1e00dfecc92d 105 if(read_return != 0)
dupm2216 7:1e00dfecc92d 106 {
dupm2216 7:1e00dfecc92d 107 printf("Read error: I2C error (nack)");
dupm2216 7:1e00dfecc92d 108 }
dupm2216 7:1e00dfecc92d 109
dupm2216 7:1e00dfecc92d 110 return result;
dupm2216 1:7becb0e903e3 111 }
dupm2216 7:1e00dfecc92d 112
dupm2216 7:1e00dfecc92d 113 //axis_data must be an array of 6 bytes
dupm2216 7:1e00dfecc92d 114 void Accelerometer::read_all_axis(signed char* axis_data)
dupm2216 1:7becb0e903e3 115 {
dupm2216 7:1e00dfecc92d 116 for(int i = 0; i < NUMBER_OF_DATA_REGISTERS; ++i)
dupm2216 7:1e00dfecc92d 117 {
dupm2216 7:1e00dfecc92d 118 const char current_register = OUT_X_MSB_REGISTER + i;
dupm2216 7:1e00dfecc92d 119 axis_data[i] = read_register(current_register);
dupm2216 7:1e00dfecc92d 120 }
dupm2216 1:7becb0e903e3 121 }
dupm2216 7:1e00dfecc92d 122
dupm2216 7:1e00dfecc92d 123 void Accelerometer::print_all_axis_data()
dupm2216 1:7becb0e903e3 124 {
dupm2216 7:1e00dfecc92d 125 signed char axis_data[NUMBER_OF_DATA_REGISTERS];
dupm2216 7:1e00dfecc92d 126 for(int i=0; i<NUMBER_OF_DATA_REGISTERS; i++)
dupm2216 7:1e00dfecc92d 127 {
dupm2216 7:1e00dfecc92d 128 axis_data[i] = 0;
dupm2216 7:1e00dfecc92d 129 }
dupm2216 7:1e00dfecc92d 130
dupm2216 7:1e00dfecc92d 131 read_all_axis(axis_data);
dupm2216 7:1e00dfecc92d 132
dupm2216 7:1e00dfecc92d 133 printf("Register content: ");
dupm2216 7:1e00dfecc92d 134 for(int i=0; i<NUMBER_OF_DATA_REGISTERS; i++)
dupm2216 7:1e00dfecc92d 135 {
dupm2216 7:1e00dfecc92d 136 const int current_data = (int)(axis_data[i]);
dupm2216 7:1e00dfecc92d 137 printf("%d, ", current_data);
dupm2216 7:1e00dfecc92d 138 }
dupm2216 7:1e00dfecc92d 139 printf("\r\n");
dupm2216 7:1e00dfecc92d 140 }
dupm2216 7:1e00dfecc92d 141
dupm2216 7:1e00dfecc92d 142 void Accelerometer::set_standby()
dupm2216 7:1e00dfecc92d 143 {
dupm2216 7:1e00dfecc92d 144 const char previous_ctrl_reg1 = read_register(CTRL_REG1_REGISTER_ADDRESS);
dupm2216 7:1e00dfecc92d 145 const char new_ctrl_reg1_value = previous_ctrl_reg1 & ~(0x01);
dupm2216 7:1e00dfecc92d 146 write_register(CTRL_REG1_REGISTER_ADDRESS, new_ctrl_reg1_value);
dupm2216 1:7becb0e903e3 147 }
dupm2216 7:1e00dfecc92d 148
dupm2216 7:1e00dfecc92d 149 void Accelerometer::set_active()
dupm2216 7:1e00dfecc92d 150 {
dupm2216 7:1e00dfecc92d 151 const char previous_ctrl_reg1 = read_register(CTRL_REG1_REGISTER_ADDRESS);
dupm2216 7:1e00dfecc92d 152 const char new_ctrl_reg1_value = previous_ctrl_reg1 | 0x01;
dupm2216 7:1e00dfecc92d 153 write_register(CTRL_REG1_REGISTER_ADDRESS, new_ctrl_reg1_value);
dupm2216 7:1e00dfecc92d 154 }
dupm2216 7:1e00dfecc92d 155
dupm2216 7:1e00dfecc92d 156 void Accelerometer::init()
dupm2216 7:1e00dfecc92d 157 {
dupm2216 7:1e00dfecc92d 158 set_active();
dupm2216 7:1e00dfecc92d 159 }
dupm2216 7:1e00dfecc92d 160
dupm2216 7:1e00dfecc92d 161 int Accelerometer::read_axis_data_8_bits(Axis axis)
dupm2216 7:1e00dfecc92d 162 {
dupm2216 7:1e00dfecc92d 163 const char axis_register = get_axis_register(axis);
dupm2216 7:1e00dfecc92d 164 const char register_value = read_register(axis_register);
dupm2216 7:1e00dfecc92d 165 return raw_axis_data_to_int(register_value);
dupm2216 7:1e00dfecc92d 166 }
dupm2216 7:1e00dfecc92d 167
dupm2216 7:1e00dfecc92d 168 double Accelerometer::get_angle_from_horizontal()
dupm2216 7:1e00dfecc92d 169 {
dupm2216 19:f5aa0ce5546b 170 const int z_axis_data = read_axis_data_8_bits(AXIS_Z);
dupm2216 19:f5aa0ce5546b 171 const int filtered_z_axis_data = this->filter.calculate(z_axis_data);
dupm2216 19:f5aa0ce5546b 172 return angle_from_int_axis_data(filtered_z_axis_data);
dupm2216 7:1e00dfecc92d 173 }
dupm2216 9:12519f9dd3cd 174 }