rakha asyrofi / Mbed 2 deprecated UART_ADXL345

this program using UART Loopback but can't read ADXL345 data over bluetooth terminal... please help to fix it

Dependencies:   ADXL345 BLE_API mbed nRF51822

Fork of BLE_LoopbackUART by Bluetooth Low Energy

main.cpp@14:033ae98fca47, 2017-10-11 (annotated)

Committer:
asyrofi
Date:
Wed Oct 11 00:02:55 2017 +0000
Revision:
14:033ae98fca47
Parent:
13:15764cc1f12c 
need revision, because can't read sensor data.. just UART

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:e910d9bb040f 1 /* mbed Microcontroller Library
yihui 0:e910d9bb040f 2 * Copyright (c) 2006-2013 ARM Limited
yihui 0:e910d9bb040f 3 *
yihui 0:e910d9bb040f 4 * Licensed under the Apache License, Version 2.0 (the "License");
yihui 0:e910d9bb040f 5 * you may not use this file except in compliance with the License.
yihui 0:e910d9bb040f 6 * You may obtain a copy of the License at
yihui 0:e910d9bb040f 7 *
yihui 0:e910d9bb040f 8 * http://www.apache.org/licenses/LICENSE-2.0
yihui 0:e910d9bb040f 9 *
yihui 0:e910d9bb040f 10 * Unless required by applicable law or agreed to in writing, software
yihui 0:e910d9bb040f 11 * distributed under the License is distributed on an "AS IS" BASIS,
yihui 0:e910d9bb040f 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
yihui 0:e910d9bb040f 13 * See the License for the specific language governing permissions and
yihui 0:e910d9bb040f 14 * limitations under the License.
yihui 0:e910d9bb040f 15 */
yihui 0:e910d9bb040f 16
yihui 0:e910d9bb040f 17 #include "mbed.h"
rgrover1 12:33153cf38631 18 #include "ble/BLE.h"
yihui 0:e910d9bb040f 19
asyrofi 14:033ae98fca47 20 #include "ADXL345.h"
rgrover1 12:33153cf38631 21 #include "ble/services/UARTService.h"
yihui 0:e910d9bb040f 22
rgrover1 6:e0fc9072e853 23 #define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
Rohit Grover 2:e060367b9024 24 * it will have an impact on code-size and power consumption. */
yihui 0:e910d9bb040f 25
Rohit Grover 2:e060367b9024 26 #if NEED_CONSOLE_OUTPUT
rgrover1 6:e0fc9072e853 27 #define DEBUG(...) { printf(__VA_ARGS__); }
yihui 0:e910d9bb040f 28 #else
Rohit Grover 2:e060367b9024 29 #define DEBUG(...) /* nothing */
Rohit Grover 2:e060367b9024 30 #endif /* #if NEED_CONSOLE_OUTPUT */
yihui 0:e910d9bb040f 31
Rohit Grover 2:e060367b9024 32 BLEDevice ble;
Rohit Grover 2:e060367b9024 33 DigitalOut led1(LED1);
yihui 0:e910d9bb040f 34
rgrover1 6:e0fc9072e853 35 UARTService *uartServicePtr;
asyrofi 14:033ae98fca47 36 ADXL345 accelerometer(p5, p6, p7, p8); // (SDA, SDO, SCL, CS);
asyrofi 14:033ae98fca47 37 Serial pc(USBTX, USBRX); //For raw data
asyrofi 14:033ae98fca47 38
asyrofi 14:033ae98fca47 39 int readings[3] = {0, 0, 0};
asyrofi 14:033ae98fca47 40 float mag_lsb;
asyrofi 14:033ae98fca47 41
asyrofi 14:033ae98fca47 42 float x,y,z,mag_g;
asyrofi 14:033ae98fca47 43 float acc_readings[6][3];
asyrofi 14:033ae98fca47 44 float calc_offsets[3];
asyrofi 14:033ae98fca47 45 const float g=9.80665;
asyrofi 14:033ae98fca47 46 const float g_mm=g*1000;
asyrofi 14:033ae98fca47 47 const float acc_res_mg=3.9063; //acc resolution mg/LSB
asyrofi 14:033ae98fca47 48
asyrofi 14:033ae98fca47 49 int8_t offset2_8bit_int[3]={0,0,0};
asyrofi 14:033ae98fca47 50 char offset2_8bit_char[3];
asyrofi 14:033ae98fca47 51
asyrofi 14:033ae98fca47 52 float mg_offsets[3]={0.0,0.0,0.0}; //fine offsets in mg's which cannot be corrected by adxl
asyrofi 14:033ae98fca47 53
asyrofi 14:033ae98fca47 54 void acc_read();
asyrofi 14:033ae98fca47 55 void display_acc();
asyrofi 14:033ae98fca47 56 void acc_store_offsets();
asyrofi 14:033ae98fca47 57 int sign(float input);
asyrofi 14:033ae98fca47 58 void display_fine_acc();
asyrofi 14:033ae98fca47 59
yihui 0:e910d9bb040f 60
rgrover1 13:15764cc1f12c 61 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
yihui 0:e910d9bb040f 62 {
Rohit Grover 2:e060367b9024 63 DEBUG("Disconnected!\n\r");
Rohit Grover 2:e060367b9024 64 DEBUG("Restarting the advertising process\n\r");
Rohit Grover 2:e060367b9024 65 ble.startAdvertising();
Rohit Grover 2:e060367b9024 66 }
yihui 0:e910d9bb040f 67
rgrover1 12:33153cf38631 68 void onDataWritten(const GattWriteCallbackParams *params)
yihui 0:e910d9bb040f 69 {
rgrover1 12:33153cf38631 70 if ((uartServicePtr != NULL) && (params->handle == uartServicePtr->getTXCharacteristicHandle())) {
rgrover1 5:4bc41267a03a 71 uint16_t bytesRead = params->len;
rgrover1 5:4bc41267a03a 72 DEBUG("received %u bytes\n\r", bytesRead);
rgrover1 6:e0fc9072e853 73 ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), params->data, bytesRead);
yihui 0:e910d9bb040f 74 }
Rohit Grover 2:e060367b9024 75 }
yihui 0:e910d9bb040f 76
Rohit Grover 2:e060367b9024 77 void periodicCallback(void)
Rohit Grover 2:e060367b9024 78 {
rgrover1 6:e0fc9072e853 79 led1 = !led1;
Rohit Grover 2:e060367b9024 80 }
yihui 0:e910d9bb040f 81
yihui 0:e910d9bb040f 82 int main(void)
yihui 0:e910d9bb040f 83 {
Rohit Grover 2:e060367b9024 84 led1 = 1;
Rohit Grover 2:e060367b9024 85 Ticker ticker;
Rohit Grover 2:e060367b9024 86 ticker.attach(periodicCallback, 1);
yihui 0:e910d9bb040f 87
Rohit Grover 2:e060367b9024 88 DEBUG("Initialising the nRF51822\n\r");
Rohit Grover 2:e060367b9024 89 ble.init();
Rohit Grover 2:e060367b9024 90 ble.onDisconnection(disconnectionCallback);
Rohit Grover 2:e060367b9024 91 ble.onDataWritten(onDataWritten);
yihui 0:e910d9bb040f 92
Rohit Grover 2:e060367b9024 93 /* setup advertising */
Rohit Grover 2:e060367b9024 94 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
Rohit Grover 2:e060367b9024 95 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
Rohit Grover 2:e060367b9024 96 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
rgrover1 6:e0fc9072e853 97 (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
Rohit Grover 2:e060367b9024 98 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
rgrover1 6:e0fc9072e853 99 (const uint8_t *)UARTServiceUUID_reversed, sizeof(UARTServiceUUID_reversed));
yihui 0:e910d9bb040f 100
rgrover1 12:33153cf38631 101 ble.setAdvertisingInterval(1000); /* 1000ms; in multiples of 0.625ms. */
Rohit Grover 2:e060367b9024 102 ble.startAdvertising();
yihui 0:e910d9bb040f 103
rgrover1 6:e0fc9072e853 104 UARTService uartService(ble);
rgrover1 6:e0fc9072e853 105 uartServicePtr = &uartService;
yihui 0:e910d9bb040f 106
Rohit Grover 2:e060367b9024 107 while (true) {
Rohit Grover 2:e060367b9024 108 ble.waitForEvent();
yihui 0:e910d9bb040f 109 }
asyrofi 14:033ae98fca47 110
asyrofi 14:033ae98fca47 111 pc.baud(9600); //set baud rate for USB serial to 921600bps
asyrofi 14:033ae98fca47 112
asyrofi 14:033ae98fca47 113 pc.printf("Starting ADXL345 test...\n");
asyrofi 14:033ae98fca47 114 pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
asyrofi 14:033ae98fca47 115
asyrofi 14:033ae98fca47 116
asyrofi 14:033ae98fca47 117 //Go into standby mode to configure the device.
asyrofi 14:033ae98fca47 118 accelerometer.setPowerControl(0x00);
asyrofi 14:033ae98fca47 119
asyrofi 14:033ae98fca47 120 //Full resolution, +/-4g, 3.9mg/LSB.
asyrofi 14:033ae98fca47 121 accelerometer.setDataFormatControl(0x0B);
asyrofi 14:033ae98fca47 122
asyrofi 14:033ae98fca47 123 //3.2kHz data rate.
asyrofi 14:033ae98fca47 124 accelerometer.setDataRate(ADXL345_3200HZ);
asyrofi 14:033ae98fca47 125
asyrofi 14:033ae98fca47 126 pc.printf("Axes offsets: ");
asyrofi 14:033ae98fca47 127 pc.printf("X: %d \t",accelerometer.getOffset(0x00));
asyrofi 14:033ae98fca47 128 pc.printf("Y: %d \t",accelerometer.getOffset(0x01));
asyrofi 14:033ae98fca47 129 pc.printf("Z: %d \n",accelerometer.getOffset(0x02));
asyrofi 14:033ae98fca47 130
asyrofi 14:033ae98fca47 131 //Measurement mode.
asyrofi 14:033ae98fca47 132 accelerometer.setPowerControl(0x08);
asyrofi 14:033ae98fca47 133
asyrofi 14:033ae98fca47 134 pc.printf("Current IMU reading: \n");
asyrofi 14:033ae98fca47 135 acc_read();
asyrofi 14:033ae98fca47 136 display_acc();
asyrofi 14:033ae98fca47 137
asyrofi 14:033ae98fca47 138 pc.printf("\nTo Calculate new offsets IMU needs to be re-oriented 12 times,\n Send 'R' when ready...\n\n");
asyrofi 14:033ae98fca47 139 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 140
asyrofi 14:033ae98fca47 141 pc.printf("Resetting current offsets... \n");
asyrofi 14:033ae98fca47 142 acc_store_offsets();
asyrofi 14:033ae98fca47 143
asyrofi 14:033ae98fca47 144 {
asyrofi 14:033ae98fca47 145 pc.printf("Okay, ready for first coarse reading set,\n Put IMU with +Z pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 146 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 147 wait(0.1);
asyrofi 14:033ae98fca47 148 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 149 {
asyrofi 14:033ae98fca47 150 wait(0.1);
asyrofi 14:033ae98fca47 151 acc_read();
asyrofi 14:033ae98fca47 152 acc_readings[4][0]+=x;
asyrofi 14:033ae98fca47 153 acc_readings[4][1]+=y;
asyrofi 14:033ae98fca47 154 acc_readings[4][2]+=z;
asyrofi 14:033ae98fca47 155 }
asyrofi 14:033ae98fca47 156 //display_acc();
asyrofi 14:033ae98fca47 157 pc.printf("\n");
asyrofi 14:033ae98fca47 158
asyrofi 14:033ae98fca47 159 pc.printf("Put IMU with -Z pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 160 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 161 wait(0.1);
asyrofi 14:033ae98fca47 162 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 163 {
asyrofi 14:033ae98fca47 164 wait(0.1);
asyrofi 14:033ae98fca47 165 acc_read();
asyrofi 14:033ae98fca47 166 acc_readings[5][0]+=x;
asyrofi 14:033ae98fca47 167 acc_readings[5][1]+=y;
asyrofi 14:033ae98fca47 168 acc_readings[5][2]+=z;
asyrofi 14:033ae98fca47 169 }
asyrofi 14:033ae98fca47 170 //display_acc();
asyrofi 14:033ae98fca47 171 pc.printf("\n");
asyrofi 14:033ae98fca47 172
asyrofi 14:033ae98fca47 173
asyrofi 14:033ae98fca47 174 pc.printf("Put IMU with +Y pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 175 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 176 wait(0.1);
asyrofi 14:033ae98fca47 177 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 178 {
asyrofi 14:033ae98fca47 179 wait(0.1);
asyrofi 14:033ae98fca47 180 acc_read();
asyrofi 14:033ae98fca47 181 acc_readings[2][0]+=x;
asyrofi 14:033ae98fca47 182 acc_readings[2][1]+=y;
asyrofi 14:033ae98fca47 183 acc_readings[2][2]+=z;
asyrofi 14:033ae98fca47 184 }
asyrofi 14:033ae98fca47 185 //display_acc();
asyrofi 14:033ae98fca47 186 pc.printf("\n");
asyrofi 14:033ae98fca47 187
asyrofi 14:033ae98fca47 188 pc.printf("Put IMU with -Y pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 189 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 190 wait(0.1);
asyrofi 14:033ae98fca47 191 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 192 {
asyrofi 14:033ae98fca47 193 wait(0.1);
asyrofi 14:033ae98fca47 194 acc_read();
asyrofi 14:033ae98fca47 195 acc_readings[3][0]+=x;
asyrofi 14:033ae98fca47 196 acc_readings[3][1]+=y;
asyrofi 14:033ae98fca47 197 acc_readings[3][2]+=z;
asyrofi 14:033ae98fca47 198 }
asyrofi 14:033ae98fca47 199 //display_acc();
asyrofi 14:033ae98fca47 200 pc.printf("\n");
asyrofi 14:033ae98fca47 201
asyrofi 14:033ae98fca47 202 pc.printf("Put IMU with +X pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 203 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 204 wait(0.1);
asyrofi 14:033ae98fca47 205 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 206 {
asyrofi 14:033ae98fca47 207 wait(0.1);
asyrofi 14:033ae98fca47 208 acc_read();
asyrofi 14:033ae98fca47 209 acc_readings[0][0]+=x;
asyrofi 14:033ae98fca47 210 acc_readings[0][1]+=y;
asyrofi 14:033ae98fca47 211 acc_readings[0][2]+=z;
asyrofi 14:033ae98fca47 212 }
asyrofi 14:033ae98fca47 213 //display_acc();
asyrofi 14:033ae98fca47 214 pc.printf("\n");
asyrofi 14:033ae98fca47 215
asyrofi 14:033ae98fca47 216 pc.printf("Put IMU with -X pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 217 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 218 wait(0.1);
asyrofi 14:033ae98fca47 219 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 220 {
asyrofi 14:033ae98fca47 221 wait(0.1);
asyrofi 14:033ae98fca47 222 acc_read();
asyrofi 14:033ae98fca47 223 acc_readings[1][0]+=x;
asyrofi 14:033ae98fca47 224 acc_readings[1][1]+=y;
asyrofi 14:033ae98fca47 225 acc_readings[1][2]+=z;
asyrofi 14:033ae98fca47 226 }
asyrofi 14:033ae98fca47 227 //display_acc();
asyrofi 14:033ae98fca47 228 pc.printf("\n");
asyrofi 14:033ae98fca47 229 }
asyrofi 14:033ae98fca47 230
asyrofi 14:033ae98fca47 231 calc_offsets[0] = (acc_readings[0][0] - acc_readings[1][0])/(10*2*acc_res_mg) - 256;
asyrofi 14:033ae98fca47 232 calc_offsets[1] = (acc_readings[2][1] - acc_readings[3][1])/(10*2*acc_res_mg) - 256;
asyrofi 14:033ae98fca47 233 calc_offsets[2] = (acc_readings[4][2] - acc_readings[5][2])/(10*2*acc_res_mg) - 256;
asyrofi 14:033ae98fca47 234
asyrofi 14:033ae98fca47 235 calc_offsets[0] = calc_offsets[0]/4; //convert to 8 bit scale 15.6 mg/LSB
asyrofi 14:033ae98fca47 236 calc_offsets[1] = calc_offsets[1]/4; //convert to 8 bit scale 15.6 mg/LSB
asyrofi 14:033ae98fca47 237 calc_offsets[2] = calc_offsets[2]/4; //convert to 8 bit scale 15.6 mg/LSB
asyrofi 14:033ae98fca47 238
asyrofi 14:033ae98fca47 239 pc.printf("Coarse calibration complete, offsets in 8bit scale (15.6mg/LSB) are: \t");
asyrofi 14:033ae98fca47 240 pc.printf("X: %.1f, Y: %.1f, Z: %.1f", calc_offsets[0], calc_offsets[1], calc_offsets[2]);
asyrofi 14:033ae98fca47 241
asyrofi 14:033ae98fca47 242
asyrofi 14:033ae98fca47 243 pc.printf("\n Apply offsets to ADXL345? Press Y to continue... \n");
asyrofi 14:033ae98fca47 244 while(pc.getc() != 'Y') {}
asyrofi 14:033ae98fca47 245 acc_store_offsets();
asyrofi 14:033ae98fca47 246 pc.printf("\n Done! ADXL 345 is now calibrated!!!\n\n New offsets are: \n");
asyrofi 14:033ae98fca47 247 pc.printf("X: %d \t",accelerometer.getOffset(0x00));
asyrofi 14:033ae98fca47 248 pc.printf("Y: %d \t",accelerometer.getOffset(0x01));
asyrofi 14:033ae98fca47 249 pc.printf("Z: %d \n",accelerometer.getOffset(0x02));
asyrofi 14:033ae98fca47 250 pc.printf("\n");
asyrofi 14:033ae98fca47 251 wait(1);
asyrofi 14:033ae98fca47 252
asyrofi 14:033ae98fca47 253
asyrofi 14:033ae98fca47 254 {
asyrofi 14:033ae98fca47 255 float mg_offsets_x[2]={0,0};
asyrofi 14:033ae98fca47 256 float mg_offsets_y[2]={0,0};
asyrofi 14:033ae98fca47 257 float mg_offsets_z[2]={0,0};
asyrofi 14:033ae98fca47 258
asyrofi 14:033ae98fca47 259 pc.printf("Ready for fine reading set,\n Put IMU with +Z pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 260 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 261 wait(0.1);
asyrofi 14:033ae98fca47 262 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 263 {
asyrofi 14:033ae98fca47 264 wait(0.1);
asyrofi 14:033ae98fca47 265 acc_read();
asyrofi 14:033ae98fca47 266 mg_offsets_z[0]+=z;
asyrofi 14:033ae98fca47 267 }
asyrofi 14:033ae98fca47 268 pc.printf("\n");
asyrofi 14:033ae98fca47 269
asyrofi 14:033ae98fca47 270 pc.printf("Put IMU with -Z pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 271 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 272 wait(0.1);
asyrofi 14:033ae98fca47 273 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 274 {
asyrofi 14:033ae98fca47 275 wait(0.1);
asyrofi 14:033ae98fca47 276 acc_read();
asyrofi 14:033ae98fca47 277 mg_offsets_z[1]+=z;
asyrofi 14:033ae98fca47 278 }
asyrofi 14:033ae98fca47 279 pc.printf("\n");
asyrofi 14:033ae98fca47 280
asyrofi 14:033ae98fca47 281 pc.printf("Put IMU with +Y pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 282 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 283 wait(0.1);
asyrofi 14:033ae98fca47 284 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 285 {
asyrofi 14:033ae98fca47 286 wait(0.1);
asyrofi 14:033ae98fca47 287 acc_read();
asyrofi 14:033ae98fca47 288 mg_offsets_y[0]+=y;
asyrofi 14:033ae98fca47 289 }
asyrofi 14:033ae98fca47 290 pc.printf("\n");
asyrofi 14:033ae98fca47 291
asyrofi 14:033ae98fca47 292 pc.printf("Put IMU with -Y pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 293 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 294 wait(0.1);
asyrofi 14:033ae98fca47 295 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 296 {
asyrofi 14:033ae98fca47 297 wait(0.1);
asyrofi 14:033ae98fca47 298 acc_read();
asyrofi 14:033ae98fca47 299 mg_offsets_y[1]+=y;
asyrofi 14:033ae98fca47 300 }
asyrofi 14:033ae98fca47 301 pc.printf("\n");
asyrofi 14:033ae98fca47 302
asyrofi 14:033ae98fca47 303 pc.printf("Put IMU with +X pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 304 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 305 wait(0.1);
asyrofi 14:033ae98fca47 306 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 307 {
asyrofi 14:033ae98fca47 308 wait(0.1);
asyrofi 14:033ae98fca47 309 acc_read();
asyrofi 14:033ae98fca47 310 mg_offsets_x[0]+=x;
asyrofi 14:033ae98fca47 311 }
asyrofi 14:033ae98fca47 312 pc.printf("\n");
asyrofi 14:033ae98fca47 313
asyrofi 14:033ae98fca47 314 pc.printf("Put IMU with -X pointing up, then press 'R' when ready \n");
asyrofi 14:033ae98fca47 315 while(pc.getc() != 'R') {}
asyrofi 14:033ae98fca47 316 wait(0.1);
asyrofi 14:033ae98fca47 317 for(int n=0; n<10; n++)
asyrofi 14:033ae98fca47 318 {
asyrofi 14:033ae98fca47 319 wait(0.1);
asyrofi 14:033ae98fca47 320 acc_read();
asyrofi 14:033ae98fca47 321 mg_offsets_x[1]+=x;
asyrofi 14:033ae98fca47 322 }
asyrofi 14:033ae98fca47 323 pc.printf("\n");
asyrofi 14:033ae98fca47 324
asyrofi 14:033ae98fca47 325 mg_offsets[0] = ((mg_offsets_x[0]/10 - 1000) + (1000 + mg_offsets_x[1]/10))/2;
asyrofi 14:033ae98fca47 326 mg_offsets[1] = ((mg_offsets_y[0]/10 - 1000) + (1000 + mg_offsets_y[1]/10))/2;
asyrofi 14:033ae98fca47 327 mg_offsets[2] = ((mg_offsets_z[0]/10 - 1000) + (1000 + mg_offsets_z[1]/10))/2;
asyrofi 14:033ae98fca47 328
asyrofi 14:033ae98fca47 329 pc.printf("Fine calibration complete, offsets in mg are: \t");
asyrofi 14:033ae98fca47 330 pc.printf("X: %f \t",mg_offsets[0]);
asyrofi 14:033ae98fca47 331 pc.printf("Y: %f \t",mg_offsets[1]);
asyrofi 14:033ae98fca47 332 pc.printf("Z: %f \n",mg_offsets[2]);
asyrofi 14:033ae98fca47 333 }
asyrofi 14:033ae98fca47 334
asyrofi 14:033ae98fca47 335 wait(5);
asyrofi 14:033ae98fca47 336
asyrofi 14:033ae98fca47 337 while(1)
asyrofi 14:033ae98fca47 338 {
asyrofi 14:033ae98fca47 339 wait(0.25);
asyrofi 14:033ae98fca47 340 pc.printf("\nCurrent IMU reading:\n");
asyrofi 14:033ae98fca47 341 acc_read();
asyrofi 14:033ae98fca47 342 display_fine_acc();
asyrofi 14:033ae98fca47 343 }
yihui 0:e910d9bb040f 344 }
asyrofi 14:033ae98fca47 345
asyrofi 14:033ae98fca47 346
asyrofi 14:033ae98fca47 347 void acc_read()
asyrofi 14:033ae98fca47 348 {
asyrofi 14:033ae98fca47 349 accelerometer.getOutput(readings);
asyrofi 14:033ae98fca47 350
asyrofi 14:033ae98fca47 351 x=(int16_t)readings[0] * acc_res_mg;
asyrofi 14:033ae98fca47 352 y=(int16_t)readings[1] * acc_res_mg;
asyrofi 14:033ae98fca47 353 z=(int16_t)readings[2] * acc_res_mg;
asyrofi 14:033ae98fca47 354
asyrofi 14:033ae98fca47 355 mag_g = sqrt(pow(x,2)+pow(y,2)+pow(z,2));
asyrofi 14:033ae98fca47 356 mag_lsb = mag_g/acc_res_mg;
asyrofi 14:033ae98fca47 357 }
asyrofi 14:033ae98fca47 358
asyrofi 14:033ae98fca47 359 void display_acc()
asyrofi 14:033ae98fca47 360 {
asyrofi 14:033ae98fca47 361 pc.printf("Raw: %i, %i, %i : %.1f\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2], mag_lsb); //Raw data
asyrofi 14:033ae98fca47 362 pc.printf("mG's: %.3f, %.3f, %.3f : %.4f\n", x, y, z, mag_g); //In G's data
asyrofi 14:033ae98fca47 363 }
asyrofi 14:033ae98fca47 364
asyrofi 14:033ae98fca47 365 void display_fine_acc()
asyrofi 14:033ae98fca47 366 {
asyrofi 14:033ae98fca47 367 x -= mg_offsets[0];
asyrofi 14:033ae98fca47 368 y -= mg_offsets[1];
asyrofi 14:033ae98fca47 369 z -= mg_offsets[2];
asyrofi 14:033ae98fca47 370
asyrofi 14:033ae98fca47 371 readings[0] -= (int16_t) (mg_offsets[0]/acc_res_mg);
asyrofi 14:033ae98fca47 372 readings[1] -= (int16_t) (mg_offsets[1]/acc_res_mg);
asyrofi 14:033ae98fca47 373 readings[2] -= (int16_t) (mg_offsets[2]/acc_res_mg);
asyrofi 14:033ae98fca47 374
asyrofi 14:033ae98fca47 375 mag_g = sqrt(pow(x,2)+pow(y,2)+pow(z,2));
asyrofi 14:033ae98fca47 376 mag_lsb = mag_g/acc_res_mg;
asyrofi 14:033ae98fca47 377
asyrofi 14:033ae98fca47 378 pc.printf("Raw: %i, %i, %i : %.1f\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2], mag_lsb); //Raw data
asyrofi 14:033ae98fca47 379 pc.printf("mG's: %.3f, %.3f, %.3f : %.4f\n", x, y, z, mag_g); //In G's data
asyrofi 14:033ae98fca47 380 pc.printf("Fine offset in mG's: %.3f, %.3f, %.3f\n", mg_offsets[0], mg_offsets[1], mg_offsets[2]); //In G's data
asyrofi 14:033ae98fca47 381 }
asyrofi 14:033ae98fca47 382
asyrofi 14:033ae98fca47 383 void acc_store_offsets()
asyrofi 14:033ae98fca47 384 {
asyrofi 14:033ae98fca47 385 for(int v=0; v<=2; v++)
asyrofi 14:033ae98fca47 386 {
asyrofi 14:033ae98fca47 387 offset2_8bit_int[v] = (int8_t) calc_offsets[v];
asyrofi 14:033ae98fca47 388 if(abs(calc_offsets[v] - offset2_8bit_int[v]) >= 0.5)
asyrofi 14:033ae98fca47 389 offset2_8bit_int[v] += (int8_t) sign(calc_offsets[v]);
asyrofi 14:033ae98fca47 390
asyrofi 14:033ae98fca47 391 offset2_8bit_int[v] = ~offset2_8bit_int[v];
asyrofi 14:033ae98fca47 392 offset2_8bit_int[v] += 1;
asyrofi 14:033ae98fca47 393
asyrofi 14:033ae98fca47 394 offset2_8bit_char[v] = offset2_8bit_int[v];
asyrofi 14:033ae98fca47 395 }
asyrofi 14:033ae98fca47 396
asyrofi 14:033ae98fca47 397 //Go into standby mode to configure the device.
asyrofi 14:033ae98fca47 398 accelerometer.setPowerControl(0x00);
asyrofi 14:033ae98fca47 399
asyrofi 14:033ae98fca47 400 accelerometer.setOffset(0x00, offset2_8bit_char[0]);
asyrofi 14:033ae98fca47 401 wait(0.1); //wait a bit
asyrofi 14:033ae98fca47 402
asyrofi 14:033ae98fca47 403 accelerometer.setOffset(0x01, offset2_8bit_char[1]);
asyrofi 14:033ae98fca47 404 wait(0.1); //wait a bit
asyrofi 14:033ae98fca47 405
asyrofi 14:033ae98fca47 406 accelerometer.setOffset(0x02, offset2_8bit_char[2]);
asyrofi 14:033ae98fca47 407 wait(0.1); //wait a bit
asyrofi 14:033ae98fca47 408
asyrofi 14:033ae98fca47 409 //Measurement mode.
asyrofi 14:033ae98fca47 410 accelerometer.setPowerControl(0x08);
asyrofi 14:033ae98fca47 411 }
asyrofi 14:033ae98fca47 412
asyrofi 14:033ae98fca47 413 int sign(float input)
asyrofi 14:033ae98fca47 414 {
asyrofi 14:033ae98fca47 415 if (input<0)
asyrofi 14:033ae98fca47 416 return -1;
asyrofi 14:033ae98fca47 417 else if(input>0)
asyrofi 14:033ae98fca47 418 return 1;
asyrofi 14:033ae98fca47 419 else
asyrofi 14:033ae98fca47 420 return 0;
asyrofi 14:033ae98fca47 421 }