Sonya's revisions to sensor.h
Fork of sensor by
sensor.h
- Committer:
- srago001
- Date:
- 2016-07-25
- Revision:
- 2:be5fd562cbd4
- Parent:
- 1:6126bf6cdfae
File content as of revision 2:be5fd562cbd4:
#include "MPU6050.h" #include "MS5803.h" //#include "MS5837.h" #include "IMU.h" #include "HMC5883L.h" #include <string> #include <vector> Serial device(PA_9,PA_10); // pressure sensor MPU6050 mpu6050; HMC5883L compass(I2C_SDA, I2C_SCL); float depth = 0.0, heading; int16_t mag[3] = {0}; void callback(){ // pressure sensor interrupt char a = 0; char i = 0; char buffer[10] = {' '}; if (device.readable()) { while(a != 'd') { a = device.getc(); if ((a >= '0' && a <='9') || a == '.'){ buffer[i] = a; i++; } } depth = atof(buffer); //pc.printf("Depth: '%f'\n", depth); } } void sensor_init() { IMUinit(mpu6050); wait_ms(100); pc.printf("================================\n"); pc.printf("Initialized begining readings\n"); pc.printf("================================\n"); } void sensor_update() { //IMU Update gets pitch and roll IMUUpdate(mpu6050); //gets heading compass.getXYZ(mag); float mx,my,mz; mx = mag[1]; my = -1*mag[0]; mz = mag[2]; float xh, yh; xh = mx*cos(pitch*PI/180) + my*sin(roll*PI/180)*sin(pitch*PI/180) - mz*cos(roll*PI/180)*sin(pitch*PI/180); yh = my*cos(roll*PI/180) + mz*sin(roll*PI/180); heading = atan2(yh, xh) * 180/PI; } void sensor_msg() { //yaw,pitch,roll,raw heading, corrected heading, depth pc.printf("%f,%f,%f,%f,%f,%f\r\n", yaw, pitch, roll, compass.getHeadingXYDeg(), heading, depth); } void print_data() { pc.printf("================================\n"); pc.printf("Yaw, pitch, roll: %f, %f, %f\n", yaw, pitch, roll); pc.printf("Depth: %f\n", depth); pc.printf("Heading w.o. tilt correction: %f\n", compass.getHeadingXYDeg()); pc.printf("Heading with tilt correction: %f\n", heading); }