Compass.cpp

00001 #include "Compass.h"
00002 
00003 //constructor
00004 Compass::Compass(ST7565 * lcd, Item * back, DigitalOut *gyro, DigitalOut *thermo)
00005 {
00006     this->title = " Compass";
00007     this->st7565= lcd;
00008     this->back = back;
00009     this->gyro = gyro;
00010     this->thermo = thermo;
00011     
00012     ct = NULL;
00013 }
00014 
00015 //display function, starts the thread and displays the compass
00016 void Compass::display(void)
00017 {
00018     //create a new thread to get and update compass - do later
00019     if(ct != NULL) return; //thread is already running
00020     thermo->write(0); gyro->write(1);
00021     ct = new Thread(&Compass::ct_start, this);
00022     ct->signal_set(START_THREAD);
00023 }
00024 
00025 //trigger for starting the thread
00026 void Compass::ct_start(void const *args)
00027 {
00028     Compass *c = (Compass*)args;
00029     c->compass_update();
00030 }
00031 
00032 //function that the thread runs, waits for the signal before starting
00033 void Compass::compass_update()
00034 {
00035     ct->signal_wait(START_THREAD);
00036     initSensors();
00037     
00038     SensorState_t state;
00039     //get degrees from other functions and display the compass
00040     while(true) {
00041         #ifdef LSM303_on
00042             LSM303(&state);
00043         #endif
00044         draw_compass(state.heading - 33); //add offset
00045         Thread::wait(30);
00046         st7565->clear();
00047     }
00048 }
00049 
00050 //update function handles updates from the user - cancels the thread
00051 //and returns to main menu
00052 void Compass::update(char c)
00053 {
00054     //kill thread and go back
00055     if(c == 'y') {
00056         ct->terminate();
00057         free(ct); ct = NULL;
00058         gyro->write(0);
00059         st7565->clear();//clear everything
00060         this->setSelectedScreen(back);
00061     } else
00062         return;
00063 }
00064 
00065 //get direction
00066 char * get_direction(double degrees)
00067 {
00068     if(degrees >= 330 || degrees < 30)
00069         return "East";
00070     else if(degrees >= 30 && degrees <= 60)
00071         return "North-East";
00072     else if(degrees >= 60 && degrees < 120)
00073         return "North";
00074     else if(degrees >= 120 && degrees < 150)
00075         return "North-West";
00076     else if(degrees >= 150 && degrees < 210)
00077         return "West";
00078     else if(degrees >= 210 && degrees < 240)
00079         return "South-West";
00080     else if(degrees >= 240 && degrees < 300)
00081         return "South";
00082     else
00083         return "South-East";
00084 }
00085 
00086 //function that draws the compass on the screen
00087 void Compass::draw_compass(double degrees)
00088 {
00089     //variables
00090     int x_temp, y_temp;
00091     double rad = (degrees + 90) * M_PI / 180; //radians
00092 
00093     //calculate coordinates to point
00094     x_temp = X_CENTER + (int) (POINTER_LENGTH * cos(rad));
00095     y_temp = Y_CENTER + (int) (POINTER_LENGTH * (-sin(rad)));
00096 
00097     //draw the main circle and small one
00098     st7565->drawcircle(X_CENTER, Y_CENTER, RADIUS, 20);
00099     st7565->drawcircle(X_CENTER, Y_CENTER, 2, 20);
00100 
00101     //draw the lines
00102     st7565->drawline(X_CENTER, Y_CENTER, x_temp, y_temp, 20); //draw line from center to coordinates
00103     st7565->drawline(X_CENTER, Y_CENTER - RADIUS, X_CENTER, Y_CENTER - 15, 20); //north line
00104     st7565->drawline(X_CENTER, Y_CENTER + RADIUS, X_CENTER, Y_CENTER + 15, 20); //south line
00105     st7565->drawline(X_CENTER + RADIUS, Y_CENTER, X_CENTER + 15, Y_CENTER, 20); //east line
00106     st7565->drawline(X_CENTER - RADIUS, Y_CENTER, X_CENTER - 15, Y_CENTER, 20); //west line
00107 
00108     //draw the initials
00109     st7565->drawstring(X_CENTER - 2, 1, "N");
00110     st7565->drawstring(X_CENTER - 2, 7, "S");
00111     st7565->drawstring(X_CENTER + 21, 4, "E");
00112     st7565->drawstring(X_CENTER - 25, 4, "W");
00113 
00114     //display pointing direction
00115     st7565->drawstring(0, 2, "Pointing:");
00116     char * pointer = get_direction(degrees + 90);
00117     st7565->drawstring(0, 4, pointer);
00118 
00119     //display degrees and radians in bottom left corner
00120     char s_deg[10], s_rad[10];
00121     sprintf(s_deg, "DEG:%d", (int)degrees % 360);
00122     sprintf(s_rad, "RAD:%0.2f", (int)degrees % 360 * M_PI / 180.0);
00123     st7565->drawstring(1, 6, s_deg);
00124     st7565->drawstring(1, 7, s_rad);
00125 
00126     st7565->display();
00127 }