Bruno Allaire-Lemay / Mbed 2 deprecated APP1test

df

Dependencies:   mbed

Fork of APP1 by Team APP

Accelerometer.cpp@5:f59b51ac4b40, 2017-01-15 (annotated)

Committer:
dupm2216
Date:
Sun Jan 15 01:34:25 2017 +0000
Revision:
5:f59b51ac4b40
Parent:
4:303fb83498fd
Child:
6:3facf0329142
Get angle from horizontal; ; Works on the prototype!

Who changed what in which revision?

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