Andrew Olguin / sensor

wrapper library for the Seagoat sensor suite

sensor.h@1:6126bf6cdfae, 2016-07-25 (annotated)

Committer:
aolgu003
Date:
Mon Jul 25 00:19:24 2016 +0000
Revision:
1:6126bf6cdfae
Parent:
0:13ac527b4c4b 
Added a sensor class.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aolgu003 0:13ac527b4c4b 1 #include "MPU6050.h"
aolgu003 0:13ac527b4c4b 2 #include "MS5803.h"
aolgu003 0:13ac527b4c4b 3 #include "MS5837.h"
aolgu003 0:13ac527b4c4b 4 #include "IMU.h"
aolgu003 0:13ac527b4c4b 5 #include "HMC5883L.h"
aolgu003 0:13ac527b4c4b 6
aolgu003 0:13ac527b4c4b 7 MPU6050 mpu6050;
aolgu003 1:6126bf6cdfae 8 HMC5883L compass(I2C_SDA, I2C_SCL);
aolgu003 1:6126bf6cdfae 9 float depth, heading;
aolgu003 0:13ac527b4c4b 10 int16_t mag[3] = {0};
aolgu003 0:13ac527b4c4b 11
aolgu003 0:13ac527b4c4b 12
aolgu003 0:13ac527b4c4b 13 void sensor_init() {
aolgu003 0:13ac527b4c4b 14 IMUinit(mpu6050);
aolgu003 0:13ac527b4c4b 15 wait_ms(100);
aolgu003 0:13ac527b4c4b 16
aolgu003 0:13ac527b4c4b 17 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 18 pc.printf("Initialized begining readings\n");
aolgu003 0:13ac527b4c4b 19 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 20 }
aolgu003 0:13ac527b4c4b 21
aolgu003 0:13ac527b4c4b 22 void sensor_update() {
aolgu003 0:13ac527b4c4b 23 //IMU Update gets pitch and roll
aolgu003 0:13ac527b4c4b 24 IMUUpdate(mpu6050);
aolgu003 0:13ac527b4c4b 25
aolgu003 0:13ac527b4c4b 26 //gets heading
aolgu003 0:13ac527b4c4b 27 compass.getXYZ(mag);
aolgu003 0:13ac527b4c4b 28
aolgu003 0:13ac527b4c4b 29 float xh, yh;
aolgu003 0:13ac527b4c4b 30 xh = mag[0]*cos(pitch*PI/180) + mag[1]*sin(roll*PI/180)*sin(pitch*PI/180) - mag[2]*cos(roll*PI/180)*sin(pitch*PI/180);
aolgu003 0:13ac527b4c4b 31 yh = mag[1]*cos(roll*PI/180) + mag[2]*sin(roll*PI/180);
aolgu003 0:13ac527b4c4b 32 heading = atan2(yh, xh) * 180/PI;
aolgu003 0:13ac527b4c4b 33 }
aolgu003 0:13ac527b4c4b 34
aolgu003 0:13ac527b4c4b 35 void sensor_msg() {
aolgu003 0:13ac527b4c4b 36 //yaw,pitch,roll,raw heading, corrected heading, depth
aolgu003 0:13ac527b4c4b 37 pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth);
aolgu003 0:13ac527b4c4b 38 }
aolgu003 0:13ac527b4c4b 39
aolgu003 0:13ac527b4c4b 40 void print_data() {
aolgu003 0:13ac527b4c4b 41 pc.printf("================================\n");
aolgu003 0:13ac527b4c4b 42 pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll);
aolgu003 0:13ac527b4c4b 43 pc.printf("Depth: %f\n", depth);
aolgu003 0:13ac527b4c4b 44 pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg());
aolgu003 0:13ac527b4c4b 45 pc.printf("Heading with tilt correction: %f\n", heading);
aolgu003 0:13ac527b4c4b 46
aolgu003 0:13ac527b4c4b 47 }