David Spillman
Crude navigation
Dependencies: GPS2 L3GD20 LSM303DLHC2 PID mbed SDFileSystem
Fork of
GPSNavigation
by 
David Spillman
Revision 7:ebc76b0f21da, committed 2015-04-07
- Comitter:
- Spilly
- Date:
- Tue Apr 07 03:49:50 2015 +0000
- Parent:
- 6:1341c11ad70c
- Child:
- 8:c77ab7615b21
- Commit message:
- Added documentation/comments
Changed in this revision
--- a/GPS.lib Sun Apr 05 01:50:25 2015 +0000 +++ b/GPS.lib Tue Apr 07 03:49:50 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/GPS2/#af055728e564 +http://developer.mbed.org/users/Spilly/code/GPS2/#7aac669b0075
--- a/PID.lib Sun Apr 05 01:50:25 2015 +0000 +++ b/PID.lib Tue Apr 07 03:49:50 2015 +0000 @@ -1,1 +1,1 @@ -http://developer.mbed.org/users/Spilly/code/PID/#8a8bb3164e1c +http://developer.mbed.org/users/Spilly/code/PID/#e3ef24ca1fd1
--- a/main.cpp Sun Apr 05 01:50:25 2015 +0000
+++ b/main.cpp Tue Apr 07 03:49:50 2015 +0000
@@ -1,7 +1,7 @@
/**************************************************************************************************************************************************************
// Created by: Ryan Spillman
//
-// Last updated 4/4/2015
+// Last updated 4/6/2015
//
// This is the software for my teams autonomous boat that is our graduation/final project at Isothermal Community College
//
@@ -23,12 +23,7 @@
#include "navigation.h"
#include "GPS.h"
-#define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters
-#define ARRIVED 2.0f //Tolerance for whether or not vehicle has arrived at goal position
-#define MAGN_PERIOD (1 / 15.0f) //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
-#define ACCEL_PERIOD (1 / 50.0f) //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
-#define GPS_PERIOD (1 / 5.0f) //GPS polling period (5 Hz) must manually update hardware ODR registers if chaging this value
-#define GPS_ALPHA 0.3f
+#define GPS_ALPHA 0.3f //GPS low pass alpha
#define DELAYTIME 0.1f //delay time between recieved chars for increasing motor speed
#define userKc 1.8f //directly proportional
#define userTi 7.7f //a larger number makes the integral have less affect on the output
@@ -39,21 +34,20 @@
PID headingPID(userKc, userTi, userTd, MAGN_PERIOD); //Kc, Ti, Td, interval
-Timer t2;
+Timer GPSTimer;
Timer headingTime;
Timer acc;
Timer magn;
Timer inputTimer;
//Prototype functions
-void updateMotorSpeed(char tempChar);
+void updateManualSpeed(char tempChar);
void updateDir(char tempChar);
-void setGoalPos(float lat, float lon);
-void makeVector(float posZero, float posOne, float curPos[2]);
+void makeVector(double posZero, double posOne, double curPos[2]);
//End of prototype functions
//Enter new positions here
-float goalPos[10][2] = { {35.478407, -81.981978},
+double goalPos[10][2] = { {35.478407, -81.981978},
{35.478344, -81.981986},
{35.478407, -81.981978},
{35.478344, -81.981986},
@@ -63,11 +57,16 @@
{35.478407, -81.981978},
{35.478344, -81.981986},
{35.478407, -81.981978}
- };
+ };
-float polarVector[2] = {0,0};
+double polarVector[2] = {0,0};
float motorSpeed = 0.5f;
-float curPos[2] = {0,0};
+float manualSpeed = 0.5f;
+double curPos[2] = {0,0};
+
+/**************************************************************************************************************************************************************************************************
+// MAIN
+**************************************************************************************************************************************************************************************************/
int main()
{
@@ -84,33 +83,27 @@
//initialize magnetometer, accelerometer
compass.init();
-
+ wait(1);
//Setup the GPS
gps.Init();
xBee.printf("gps initialized\n");
xBee.printf("attempting to get a fix\n");
- t2.start();
-
- //wait until we have a gps fix
- while(gps.fixtype == 0)
+ GPSTimer.start();
+
+ while(gps.fixtype != 2) //wait until we have a DGPS fix (more accurate)
+ //while(gps.fixtype != 2) //wait for a GPS fix only (faster startup)
{
- if(t2.read() >= GPS_PERIOD)
+ if(GPSTimer.read() >= GPS_PERIOD)
{
- //xBee.printf("fix %d\n", gps.fixtype);
gps.parseData();
- xBee.printf("fix %d\n", gps.fixtype);
- getAccel();
-
- //xBee.printf("fix %d\n", gps.fixtype);
- xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
- //wait(GPSPERIOD);
- t2.reset();
+ xBee.printf("Waiting for DGPS fix\tcurrent fix = %d\n", gps.fixtype);
+ GPSTimer.reset();
}
}
//put GPS in sleep mode to prevent overflowing the buffer
- gps.Sleep(1);
+ //gps.Sleep(1);
//get IMU data and calculate the tilt compensated compass
getAccel();
@@ -118,15 +111,15 @@
updateAngles();
//set current position
- curPos[0] = gps.latitude;
- curPos[1] = gps.longitude;
+ curPos[0] = gps.degLat;
+ curPos[1] = gps.degLon;
makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
- xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
+ xBee.printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.degLat, gps.degLon, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
xBee.printf("dist %f\theading %f\n", polarVector[0], polarVector[1]);
- xBee.printf("starting main loop\n");
+ xBee.printf("\n\nstarting main loop\n");
- //PID control of left and right motors based on input from gyro
+ //PID control of left and right motors based on input from tilt compensated compass
//Goal is to have the vehicle go straight
headingPID.setInputLimits(-180, 180);
headingPID.setOutputLimits(-0.5f, 0.5f);
@@ -141,25 +134,23 @@
headingTime.start();
acc.start();
magn.start();
- xBee.printf("Starting Motors!\n");
- goForward(0.5f, 0.5f);
int wakeUp = 1;
+ float leastHDOP = 100;
while (1)
{
- //manual mode
+ /**********************************************************************************************************************************************************************************************
+ // manual mode
+ **********************************************************************************************************************************************************************************************/
+
if(mode == 0)
{
- if(t2.read() >= GPS_PERIOD)
+ if(GPSTimer.read() >= GPS_PERIOD)
{
- //check GPS data
gps.parseData();
- curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
- curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
- xBee.printf("Current Postion:\t%f\t%f\n", curPos[0], curPos[1]);
-
- t2.reset();
+ xBee.printf("lat %f\tlon %f\tHDOP %f\tfix %d\tsat %d\tspd %f\n", gps.degLat, gps.degLon, gps.HDOP, gps.fixtype, gps.satellites, gps.speed);
+ GPSTimer.reset();
}
- //checks to see if data is available on xBee
+ //check to see if data is available on xBee
if(xBee.readable())
{
char recChar = xBee.getc();
@@ -182,7 +173,7 @@
updateDir(recChar);
}
- else updateMotorSpeed(recChar);
+ else updateManualSpeed(recChar);
//reset timer
inputTimer.reset();
@@ -202,21 +193,25 @@
//has the delay time elapsed since the first same char
else if(inputTimer.read_ms() >= DELAYTIME)
{
- updateMotorSpeed(recChar);
+ updateManualSpeed(recChar);
}
}
}
- //autonomous mode
+
+ /**********************************************************************************************************************************************************************************************
+ // autoznomous mode
+ **********************************************************************************************************************************************************************************************/
+
if(mode == 1)
{
//only send wake up once
if(wakeUp == 1)
{
//wake up GPS
- gps.Sleep(0);
+ //gps.Sleep(0);
wakeUp = 0;
}
- //Emergency stop
+ //check xBee
if(xBee.readable())
{
char recChar = xBee.getc();
@@ -224,41 +219,39 @@
{
xBee.printf("emergency stop\n");
goStop(1,1);
- while(1);
+ mode = 0;
}
//change to manual mode
if(recChar == 'y')
{
mode = 0;
+ wayPtNum = 0;
}
}
else
{
- if(t2.read() >= GPS_PERIOD)
+ if(GPSTimer.read() >= GPS_PERIOD)
{
- //check GPS data
gps.parseData();
- //printf("lat %f\tlon %f\thead %f\talt %f\tspd %f\tfix %d\tsat %d\n", gps.latitude, gps.longitude, gps.heading, gps.altitude, gps.speed, gps.fixtype, gps.satellites);
- curPos[0] = curPos[0] + GPS_ALPHA * (gps.latitude - curPos[0]);
- curPos[1] = curPos[1] + GPS_ALPHA * (gps.longitude - curPos[1]);
- t2.reset();
+ //leastHDOP will reset every 5 readings
+ //keeps the most accurate reading
+ if(gps.HDOP < leastHDOP)
+ {
+ curPos[0] = gps.degLat;
+ curPos[1] = gps.degLon;
+ }
gpsCount = gpsCount + 1;
- //low pass filter 5 gps data sets before recalculating heading and reseting PID data (should make for less oscillation)
if(gpsCount == 5)
{
makeVector(goalPos[wayPtNum][0],goalPos[wayPtNum][1], curPos);
- //latDif = sqrt((goalPos[wayPtNum][0] - curPos[0]) * (goalPos[wayPtNum][0] - curPos[0]));
- //longDif = sqrt((goalPos[wayPtNum][1] - curPos[1]) * (goalPos[wayPtNum][1] - curPos[1]));
- //xBee.printf("\n\nupdated vector\n\n");
-
- headingPID.reset(); //Reset PID data since we have a new heading
+ headingPID.reset(); //Reset PID data since we have a new heading
gpsCount = 0;
+ leastHDOP = 100;
}
xBee.printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n", polarVector[0], magDiff, curPos[0], curPos[1], goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
- //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
- t2.reset();
+ GPSTimer.reset();
}
if(acc.read() >= ACCEL_PERIOD)
@@ -271,17 +264,9 @@
getMagn();
updateAngles();
magn.reset();
- //compassAvg();
-
filtered = lowPass(yaw, filtered, 0);
- //xBee.printf("yaw = %f\tfiltered = %f\n", yaw, filtered);
- //set heading to a fixed number for testing
magDiff = whichWay(filtered, polarVector[1]);
- //magDiff = whichWay(yaw, polarVector[1]);
- //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], yaw);
- //xBee.printf("magDiff = %f\theading = %f\t mag = %f\n",magDiff, polarVector[1], filtered);
headingPID.setProcessValue(magDiff);
-
motorSpeed = headingPID.compute();
goForward(0.5f - motorSpeed,0.5f + motorSpeed);
@@ -300,24 +285,22 @@
xBee.printf("Moving to Goal Position %d\n", wayPtNum);
}
}
-
- //xBee.printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
- //printf("dist %f\tMagDiff %f\tClat %f\tClon %f\tGlat %f\tGlon %f\n\n", polarVector[0], magDiff, gps.latitude, gps.longitude, goalPos[wayPtNum][0],goalPos[wayPtNum][1]);
-
- //wait for elapsed time to be greater than the period of the heading loop
- //while(headingTime.read() < RATE);
- //headingTime.reset();
}
}
- //record position
+
+ /**********************************************************************************************************************************************************************************************
+ // record position mode
+ **********************************************************************************************************************************************************************************************/
+
if(mode == 3)
{
- xBee.printf("Please enter position number (0-9), or press y to return to manual mode\nenter selection again if confirmation does not appear\n");
+ xBee.printf("\nPlease enter position number (0-9), or press y to return to manual mode\n");
while(!xBee.readable());
char recChar = xBee.getc();
recChar = xBee.getc();
+
//return to manual mode
if(recChar == 'y')
{
@@ -325,6 +308,25 @@
}
else
{
+ xBee.printf("\nFinding most accurate GPS position\nThis will take a few seconds\n\n");
+
+ float lowestHDOP = 100;
+
+ //take 25 GPS readings and keep the position with the lowest horizontal dilution of precision (HDOP)
+ for(int i = 0; i< 50; i++)
+ {
+ gps.parseData();
+
+ if(gps.HDOP <= lowestHDOP)
+ {
+ lowestHDOP = gps.HDOP;
+ curPos[0] = gps.degLat;
+ curPos[1] = gps.degLon;
+ }
+
+ while(GPSTimer.read() < GPS_PERIOD);
+ GPSTimer.reset();
+ }
if(recChar == '0')
{
goalPos[0][0] = curPos[0];
@@ -375,120 +377,110 @@
goalPos[9][0] = curPos[0];
goalPos[9][1] = curPos[1];
}
- xBee.printf("position %c updated\n", recChar);
+ xBee.printf("position %c updated\t", recChar);
}
- xBee.printf("exiting record mode\n");
+ xBee.printf("returning to manual mode\n\n");
mode = 0;
}
}
}
-void updateMotorSpeed(char tempChar)
+/**************************************************************************************************************************************************************************************************
+// Update PWM Value for Manual Mode
+**************************************************************************************************************************************************************************************************/
+
+void updateManualSpeed(char tempChar)
{
if(tempChar == '1')
{
goStop(1, 1);
- xBee.printf("Stop\n%.2f\t", motorSpeed);
+ xBee.printf("Stop\n");
}
else if(tempChar == 'p')
{
- if(motorSpeed < 1)
+ if(manualSpeed < 1)
{
- motorSpeed = motorSpeed + 0.01f;
- xBee.printf("Increasing MotorSpeed\n%.2f\t", motorSpeed);
+ manualSpeed = manualSpeed + 0.01f;
+ xBee.printf("Increasing manualSpeed\t%.2f\n", manualSpeed);
}
- else(xBee.printf("Maximum motorSpeed reached\n%.2f\t", motorSpeed));
+ else(xBee.printf("Maximum manualSpeed reached\t%.2f\n", manualSpeed));
- //errors with floating point calculations are causing motorSpeed to exceed 1
+ //errors with floating point calculations are causing manualSpeed to exceed 1
//this is a workaround
- if(motorSpeed > 1)
+ if(manualSpeed > 1)
{
- motorSpeed = 1;
+ manualSpeed = 1;
}
}
else if(tempChar == 'm')
{
- if(motorSpeed > 0.01f)
+ if(manualSpeed > 0.01f)
{
- motorSpeed = motorSpeed - 0.01f;
- xBee.printf("MotorSpeed decreased\n%.2f\t", motorSpeed);
+ manualSpeed = manualSpeed - 0.01f;
+ xBee.printf("manualSpeed decreased\t%.2f\n", manualSpeed);
}
else
{
- //calculation errors may prevent motorSpeed from reaching actual zero
- motorSpeed = 0;
- xBee.printf("Minimum motorSpeed reached\n%.2f\t", motorSpeed);
+ //calculation errors may prevent manualSpeed from reaching actual zero
+ manualSpeed = 0;
+ xBee.printf("Minimum manualSpeed reached\t%.2f\n", manualSpeed);
}
}
}
+/**************************************************************************************************************************************************************************************************
+// Update Direction for Manual Mode
+**************************************************************************************************************************************************************************************************/
+
void updateDir(char tempChar)
{
if(tempChar == 'w')
{
- goForward(motorSpeed, motorSpeed);
- //wait(time);
- //goStop(1,1);
- xBee.printf("Going forward\n%.2f\t", motorSpeed);
+ goForward(manualSpeed, manualSpeed);
+ xBee.printf("Going forward\t%.2f\n", manualSpeed);
}
else if(tempChar == 's')
{
- goBackward(motorSpeed, motorSpeed);
- //wait(time);
- //goStop(1,1);
- xBee.printf("Going backward\n%.2f\t", motorSpeed);
+ goBackward(manualSpeed, manualSpeed);
+ xBee.printf("Going backward\t%.2f\n", manualSpeed);
}
else if(tempChar == 'd')
{
- goRight(motorSpeed, motorSpeed);
- //wait(time);
- //goStop(1,1);
- xBee.printf("Going right\n%.2f\t", motorSpeed);
+ goRight(manualSpeed, manualSpeed);
+ xBee.printf("Going right\t%.2f\n", manualSpeed);
}
else if(tempChar == 'a')
{
- goLeft(motorSpeed, motorSpeed);
- //wait(time);
- //goStop(1,1);
- xBee.printf("Going left\n%.2f\t", motorSpeed);
+ goLeft(manualSpeed, manualSpeed);
+ xBee.printf("Going left\t%.2f\n", manualSpeed);
}
}
+/**************************************************************************************************************************************************************************************************
+// create polar vector based on two sets of latitude and longitude
+**************************************************************************************************************************************************************************************************/
-//create polar vector based on two sets of latitude and longitude
-void makeVector(float posZero, float posOne, float curPos[2])
+void makeVector(double posZero, double posOne, double curPos[2])
{
- float arcLength[2];
- float goalPos[2];
+ double arcLength[2];
+ double goalPos[2];
goalPos[0] = posZero;
goalPos[1] = posOne;
- //arc length = radius * angle
+ /*Note: arc length = radius * angle*/
//Y
- //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] * goalPos[0])) - sqrt((curPos[0] * curPos[0])));
- //arcLength[1] = EARTHRADIUS * (sqrt((goalPos[0] - curPos[0]) * (goalPos[0] - curPos[0])));
arcLength[1] = EARTHRADIUS * ((goalPos[0] - curPos[0]) * DEGREETORAD);
- //xBee.printf("%f\n", arcLength[1]);
//X
- //arcLength[0] = EARTHRADIUS * (sqrt((goalPos[1] - curPos[1]) * (goalPos[1] - curPos[1])));
arcLength[0] = EARTHRADIUS * ((goalPos[1] - curPos[1]) * DEGREETORAD);
- //printf("G0 = %f\t G1 = %f\tC0 = %f\tC1 = %f\tA0 = %f\tA1 = %f\n", goalPos[0], goalPos[1], curPos[0], curPos[1], arcLength[0], arcLength[1]);
- //xBee.printf("%f\n", goalPos[1]);
- //xBee.printf("%f\n", curPos[1]);
- //xBee.printf("%f\n", arcLength[0]);
//calculate magnitude of vector
polarVector[0] = sqrt((arcLength[0] * arcLength[0]) + (arcLength[1] * arcLength[1]));
- //xBee.printf("%f\n", polarVector[0]);
- //Use arcTan(-x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
+ //Use arcTan(x/y) b/c we want our heading to be in respect to North (North = 0 degrees, East = 90 deg, etc.)
polarVector[1] = (RADTODEGREE * (atan2(arcLength[0], arcLength[1])));
- //printf("0 = %f\t1 = %f\theading = %f\n", arcLength[0], arcLength[1], polarVector[1]);
-
//make negative angles positive
if(polarVector[1] < 0) polarVector[1] = polarVector[1] + 360;
- //xBee.printf("%f\n", polarVector[1]);
}
--- a/modSensData.h Sun Apr 05 01:50:25 2015 +0000
+++ b/modSensData.h Tue Apr 07 03:49:50 2015 +0000
@@ -19,20 +19,19 @@
L3GD20 gyro(PTE25, PTE24);
LSM303DLHC compass(PTE25, PTE24);
-//Variable for low pass filter
-#define ALPHA_H 0.2f
-#define ALPHA_A 0.1f
-#define MAX_DELTA_HEADING 20
-
+#define ARRIVED 4.0f //Tolerance, in meters, for when goal location is reached
+#define ALPHA_H 0.2f //Heading alpha (variable for low pass filter)
+#define ALPHA_A 0.1f //Heading accelerometer (variable for low pass filter)
+#define EARTHRADIUS 6378100.000000000000000000f //Radius of the earth in meters (DO NOT MODIFY)
+#define MAGN_PERIOD (1 / 15.0f) //magnetometer polling period (15 Hz) must manually update hardware ODR registers if chaging this value
+#define ACCEL_PERIOD (1 / 50.0f) //accelerometer polling period (50 Hz) must manually update hardware ODR registers if chaging this value
+#define GYRO_SAMPLE_PERIOD 0.01f //gyro sample period in seconds
+#define GPS_PERIOD (1 / 5.0f) //GPS polling period (5 Hz)
#define M_PI 3.1415926535897932384626433832795f
#define TWO_PI 6.283185307179586476925286766559f
#define RADTODEGREE 57.295779513082320876798154814105f
#define DEGREETORAD 0.01745329251994329576923690768489f
#define GRAVITY 1.0f
-#define THRESHOLD 0.02f
-
-//gyro sample period in seconds
-#define GYRO_SAMPLE_PERIOD 0.01f
#define CALIBRATION_COUNT 64
#define AVG_COUNT 10
@@ -52,12 +51,6 @@
#define MAGN_Z_MIN (-0.618367f)
#define MAGN_Z_MAX (0.408163f)
-
-//Period in seconds of heading PID loop
-#define COMPASS_PERIOD 0.060f
-
-#define YAW_VAR_TOLER 5.0f
-
// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
@@ -84,24 +77,14 @@
float totalAccel = 0;
float normalizedGravity;
-float filteredYaw, filteredPitch, filteredRoll;
-
float lowPassAccel[3] = {0.0000001f,0.0000001f,0.0000001f};
-float lowPassMagnetom[3] = {0.0000001f,0.0000001f,0.0000001f};
float prevYaw = 0;
-int cycleCount = 0;
-int firstPass = 1;
-/*
-float lowPass(float input, float output)
-{
- if (output == 0.0000001f) return input;
-
- output = (ALPHA * input) + ((1 - ALPHA) * output);
-
- return output;
-}
-*/
+
+/********************************************************************************************************************************/
+// Low Pass
+/********************************************************************************************************************************/
+
float lowPass(float input, float output, int select)
{
//Accelerometer alpha
@@ -156,12 +139,17 @@
lowPassAccel[i] = lowPass(accel[i], lowPassAccel[i], 1);
}
}
+
+/********************************************************************************************************************************/
+// Integrate Yaw With Gyro Data
+/********************************************************************************************************************************/
+
void gyroIntegral()
{
float yawDiff = yaw - prevYaw;
float absYawDiff = sqrt(yawDiff * yawDiff);
//vC[x] = vP[x] + ((aP[x] + ((inertialAccel[x] - aP[x]) / 2)) * deltaT);
- float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * COMPASS_PERIOD);
+ float gyroInt = prevYaw + ((prevDegGyro[2] + ((degGyro[2] - prevDegGyro[2]) / 2)) * MAGN_PERIOD);
float absGyroInt = sqrt(gyroInt * gyroInt);
prevDegGyro[2] = degGyro[2];
if(absYawDiff > absGyroInt)
@@ -169,6 +157,7 @@
yaw = prevYaw + gyroInt;
}
}
+
/********************************************************************************************************************************/
// Compass Heading
/********************************************************************************************************************************/
@@ -178,37 +167,27 @@
float mag_x;
float mag_y;
- //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
- //{
- float cos_roll;
- float sin_roll;
- float cos_pitch;
- float sin_pitch;
-
- //saves a few processor cycles by calculating and storing values
- cos_roll = cos(roll);
- sin_roll = sin(roll);
- cos_pitch = cos(pitch);
- sin_pitch = sin(pitch);
-
- // Tilt compensated magnetic field X
- //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
- mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
- // Tilt compensated magnetic field Y
- //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
- mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
- //}
- /*
- else
- {
- mag_x = magnetom[0] ;
- mag_y = magnetom[1];
- }
- */
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ //saves a few processor cycles by calculating and storing values
+ cos_roll = cos(roll);
+ sin_roll = sin(roll);
+ cos_pitch = cos(pitch);
+ sin_pitch = sin(pitch);
+
+ // Tilt compensated magnetic field X
+ //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+ mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+ // Tilt compensated magnetic field Y
+ //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+ mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+
// Magnetic Heading
yaw = atan2(-mag_x, mag_y);
-
-
+
//make negative angles positive
if(yaw < 0) yaw = yaw + TWO_PI;
//yaw = yaw + M_PI;
@@ -221,74 +200,28 @@
void getAttitude()
{
- float temp1[3];
- float temp2[3];
- float xAxis[3] = {1.0f, 0.0f, 0.0f};
-
- // GET PITCH
- // Using y-z-plane-component/x-component of gravity vector
- pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
-
- //printf("P = %f", pitch);
-
- // GET ROLL
- // Compensate pitch of gravity vector
- temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
- temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
- temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
-
- temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
- temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
- temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-
- // Since we compensated for pitch, x-z-plane-component equals z-component:
- roll = atan2(temp2[1], temp2[2]);
-}
-
-/********************************************************************************************************************************/
-// Compass Heading
-/********************************************************************************************************************************/
-
-void Filtered_Compass_Heading()
-{
-
- float mag_x;
- float mag_y;
- //if((roll >= 0.0873f) || (roll <= -0.0873f) || (pitch >= 0.0873f) || (pitch <= -0.0873f))
- //{
- float cos_roll;
- float sin_roll;
- float cos_pitch;
- float sin_pitch;
-
- //saves a few processor cycles by calculating and storing values
- cos_roll = cos(filteredRoll);
- sin_roll = sin(filteredRoll);
- cos_pitch = cos(filteredPitch);
- sin_pitch = sin(filteredPitch);
-
- // Tilt compensated magnetic field X
- //mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
- mag_x = lowPassMagnetom[0] * cos_pitch + lowPassMagnetom[1] * sin_roll * sin_pitch + lowPassMagnetom[2] * cos_roll * sin_pitch;
- // Tilt compensated magnetic field Y
- //mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
- mag_y = lowPassMagnetom[1] * cos_roll - lowPassMagnetom[2] * sin_roll;
- //}
- /*
- else
- {
- mag_x = magnetom[0] ;
- mag_y = magnetom[1];
- }
- */
- // Magnetic Heading
- filteredYaw = atan2(-mag_x, mag_y);
+ float temp1[3];
+ float temp2[3];
+ float xAxis[3] = {1.0f, 0.0f, 0.0f};
+
+ // GET PITCH
+ // Using y-z-plane-component/x-component of gravity vector
+ pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
+
+ //printf("P = %f", pitch);
+ // GET ROLL
+ // Compensate pitch of gravity vector
+ temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
+ temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
+ temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
- //make negative angles positive
- if(filteredYaw < 0) filteredYaw = filteredYaw + TWO_PI;
- //yaw = yaw + M_PI;
- filteredYaw = filteredYaw * RADTODEGREE;
+ temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+ temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+ temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+
+ // Since we compensated for pitch, x-z-plane-component equals z-component:
+ roll = atan2(temp2[1], temp2[2]);
}
/********************************************************************************************************************************/
@@ -297,28 +230,28 @@
void Filtered_Attitude()
{
- float temp1[3];
- float temp2[3];
- float xAxis[3] = {1.0f, 0.0f, 0.0f};
-
- // GET PITCH
- // Using y-z-plane-component/x-component of gravity vector
- pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
-
- //printf("P = %f", pitch);
-
- // GET ROLL
- // Compensate pitch of gravity vector
- temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
- temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
- temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
-
- temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
- temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
- temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
-
- // Since we compensated for pitch, x-z-plane-component equals z-component:
- roll = atan2(temp2[1], temp2[2]);
+ float temp1[3];
+ float temp2[3];
+ float xAxis[3] = {1.0f, 0.0f, 0.0f};
+
+ // GET PITCH
+ // Using y-z-plane-component/x-component of gravity vector
+ pitch = -atan2(lowPassAccel[0], sqrt((lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2])));
+
+ //printf("P = %f", pitch);
+
+ // GET ROLL
+ // Compensate pitch of gravity vector
+ temp1[0] = (lowPassAccel[1] * xAxis[2]) - (lowPassAccel[2] * xAxis[1]);
+ temp1[1] = (lowPassAccel[2] * xAxis[0]) - (lowPassAccel[0] * xAxis[2]);
+ temp1[2] = (lowPassAccel[0] * xAxis[1]) - (lowPassAccel[1] * xAxis[0]);
+
+ temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
+ temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
+ temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
+
+ // Since we compensated for pitch, x-z-plane-component equals z-component:
+ roll = atan2(temp2[1], temp2[2]);
}
@@ -329,132 +262,11 @@
void updateAngles()
//int updateAngles()
{
- //readIMU();
- //getAttitude();
Filtered_Attitude();
Compass_Heading();
- //Filtered_Attitude();
- //Filtered_Compass_Heading();
- /*
- totalAccel = sqrt((lowPassAccel[0] * lowPassAccel[0]) + (lowPassAccel[1] * lowPassAccel[1]) + (lowPassAccel[2] * lowPassAccel[2]));
-
- prevYaw = yaw;
- getAttitude();
- Compass_Heading();
-
- if((sqrt((prevYaw - yaw) * (prevYaw - yaw)) >= YAW_VAR_TOLER) && (firstPass != 1))
- {
- yaw = prevYaw;
- }
- else if(firstPass == 1)
- {
- firstPass = 0;
- }
-
- //if((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * accel[2]) <= THRESHOLD))
- if((totalAccel <= (normalizedGravity + THRESHOLD)) && (totalAccel >= (normalizedGravity - THRESHOLD)))
- {
- //float prevYaw = yaw;
- getAttitude();
- Compass_Heading();
- //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));
- //check for noise
- //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
- //{
- return 1;
- //}
- //return 0;
-
- }
- */
- //return 0;
- //else Compass_Heading();
- //else return 0;
- //getAttitude();
- //Compass_Heading();
- //gyroIntegral();
}
/********************************************************************************************************************************/
-// magAvg
-/********************************************************************************************************************************/
-
-void compassAvg()
-{
- float accumulator = 0;
- //float prevYaw[AVG_COUNT];
- int sampleCount = 0;
-
-
- //We'll take a certain number of samples and then average them to calculate the offset
- while (sampleCount < AVG_COUNT)
- {
- updateAngles();
-
- //add current sample to previous samples
- accumulator += yaw;
-
- sampleCount++;
- //Make sure the gyro has had enough time to take a new sample.
- wait(COMPASS_PERIOD);
- }
-
- //divide by number of samples to get average offset
- yaw = accumulator / AVG_COUNT;
-
-
-}
-
-/********************************************************************************************************************************/
-// magLowPass
-/********************************************************************************************************************************/
-void compLowPass()
-{
- /*
- prevMagnetom[5][3];
- //read sensors five times
- for(int x = 0; x<= 5; i++)
- {
- for(int i = 0; i<=3; i++)
- {
- prevMagnetom[x, i] = magnetom[i];
- }
- readIMU();
- }
- float diff[5][3];
- for(int x = 1; x<= 5; i++)
- {
- sqrt(prevMagnetom[x][0]
- for(int i = 0; i<=3; i++)
- {
- if(diff[x][i] = sqrt((prevMagnetom[x][i] - prevMagnemtom[x - 1]) * (prevMagnetom[x][i] - prevMagnemtom[x - 1])) >= MAG_VAR_TOLER)
- {
-
- }
- }
- }
-
- float deltaHeading = 0;
- updateAngles();
- deltaHeading = sqrt((yaw - prevYaw[cycleCount]) * (yaw - prevYaw[cycleCount]));
-
- if(deltaHeading >= MAX_DELTA_HEADING)
- {
- yaw = (yaw + prevYaw[cycleCount]) / 2.0f;
- }
- cycleCount = cycleCount + 1;
- prevYaw[cycleCount] = yaw;
-
- if(cycleCount >= 4)
- {
- cycleCount = 0;
- prevYaw = {0,0,0,0,0}
- }
-*/
-}
-
-
-/********************************************************************************************************************************/
// normalizeGravity
/********************************************************************************************************************************/
@@ -541,6 +353,10 @@
}
}
+/********************************************************************************************************************************/
+// Only Get Gyro Data
+/********************************************************************************************************************************/
+
void gyroOnly()
{
gyro.read(°Gyro[0], °Gyro[1], °Gyro[2]);
@@ -549,5 +365,4 @@
{
degGyro[i] -= gyroOffset[i];
}
-}
-
+}
\ No newline at end of file