Guido Ottaviani
A porting of a GPS decoding and presenting program within the mbos RTOS. It is not a definitive application but a study program to test NMEA full decoding library and a first approach to an RTOS. Many thanks to Andrew Levido for his support and his patience on teaching me the RTOS principles from the other side of the Earth. It uses NMEA library by Tim (xtimor@gmail.com) ported by Ken Todotani (http://mbed.org/users/todotani/) on public mbed library (http://mbed.org/users/todotani/programs/GPS_nmeaLib/5yo4h) also available, as original universal C library, on http://nmea.sourceforge.net
Dependencies: mbos Watchdog TextLCD mbed ConfigFile
LeonardoMbos.cpp@0:d177c0087d1f, 2012-01-29 (annotated)
- Committer:
- guiott
- Date:
- Sun Jan 29 16:06:12 2012 +0000
- Revision:
- 0:d177c0087d1f
- Child:
- 1:360c4a23cb1d
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| guiott | 0:d177c0087d1f | 1 | /* //////////////////////////////////////////////////////////////////////////// |
| guiott | 0:d177c0087d1f | 2 | ** File: LeonardoMbos.cpp |
| guiott | 0:d177c0087d1f | 3 | */ |
| guiott | 0:d177c0087d1f | 4 | /* 12345678901234567890 */ |
| guiott | 0:d177c0087d1f | 5 | char Ver1[] = "Leonardo's GPS 0.1.0"; |
| guiott | 0:d177c0087d1f | 6 | char Ver2[] = " by Guiott 01-12"; |
| guiott | 0:d177c0087d1f | 7 | /** |
| guiott | 0:d177c0087d1f | 8 | * \mainpage Leonardo.cpp |
| guiott | 0:d177c0087d1f | 9 | * \author Guido Ottaviani-->guido@guiott.com<-- |
| guiott | 0:d177c0087d1f | 10 | * \version 0.1.0 |
| guiott | 0:d177c0087d1f | 11 | * \date 01/2012 |
| guiott | 0:d177c0087d1f | 12 | * \details This is a test program to study the capability of a GPS module to |
| guiott | 0:d177c0087d1f | 13 | control navigation of a robot in an outdoor environment. |
| guiott | 0:d177c0087d1f | 14 | |
| guiott | 0:d177c0087d1f | 15 | This version is developed within the mbos RTOS: |
| guiott | 0:d177c0087d1f | 16 | http://mbed.org/users/AndrewL/libraries/mbos/lqn3ca |
| guiott | 0:d177c0087d1f | 17 | |
| guiott | 0:d177c0087d1f | 18 | It uses NMEA library by Tim (xtimor@gmail.com) |
| guiott | 0:d177c0087d1f | 19 | available on public mbed library or http://nmea.sourceforge.net |
| guiott | 0:d177c0087d1f | 20 | |
| guiott | 0:d177c0087d1f | 21 | The original gmath.c has been modified to fix a bug in nmea_distance_ellipsoid() function |
| guiott | 0:d177c0087d1f | 22 | according to bug report ID: 2945855 |
| guiott | 0:d177c0087d1f | 23 | |
| guiott | 0:d177c0087d1f | 24 | http://sourceforge.net/tracker/?func=detail&aid=2945855&group_id=192054&atid=939854 |
| guiott | 0:d177c0087d1f | 25 | |
| guiott | 0:d177c0087d1f | 26 | // while ((delta_lambda > 1e-12) && (remaining_steps > 0)) original by xtimor |
| guiott | 0:d177c0087d1f | 27 | while (( remaining_steps == 20 ) || ((fabs(delta_lambda) > 1e-12) && (remaining_steps > 0))) |
| guiott | 0:d177c0087d1f | 28 | |
| guiott | 0:d177c0087d1f | 29 | the original code always returns a zero distance if the arrival point longitude |
| guiott | 0:d177c0087d1f | 30 | is equal or smaller than the starting point one. |
| guiott | 0:d177c0087d1f | 31 | |
| guiott | 0:d177c0087d1f | 32 | The mbed module is interfaced with a Garmin GPS sensor (used in standard mode) |
| guiott | 0:d177c0087d1f | 33 | with an external antenna, a 20x4 LCD text display and a 5 keys keypad. |
| guiott | 0:d177c0087d1f | 34 | A curiosity: |
| guiott | 0:d177c0087d1f | 35 | All the hardware components of the test set are mounted on the top of a wooden |
| guiott | 0:d177c0087d1f | 36 | box, looking like a kind of a steampunk navigator. When my daughter looked at |
| guiott | 0:d177c0087d1f | 37 | the box she said: "it looks alike the navigator of the da Vinci car". |
| guiott | 0:d177c0087d1f | 38 | This is the reason why the name of this project is "Leonardo". |
| guiott | 0:d177c0087d1f | 39 | ** |
| guiott | 0:d177c0087d1f | 40 | ------------------------------------------------------------------------------- |
| guiott | 0:d177c0087d1f | 41 | * \copyright 2012 Guido Ottaviani |
| guiott | 0:d177c0087d1f | 42 | guido@guiott.com |
| guiott | 0:d177c0087d1f | 43 | |
| guiott | 0:d177c0087d1f | 44 | LeonardoMbos is free software: you can redistribute it and/or modify |
| guiott | 0:d177c0087d1f | 45 | it under the terms of the GNU General Public License as published by |
| guiott | 0:d177c0087d1f | 46 | the Free Software Foundation, either version 3 of the License, or |
| guiott | 0:d177c0087d1f | 47 | (at your option) any later version. |
| guiott | 0:d177c0087d1f | 48 | |
| guiott | 0:d177c0087d1f | 49 | dsPID33 is distributed in the hope that it will be useful, |
| guiott | 0:d177c0087d1f | 50 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
| guiott | 0:d177c0087d1f | 51 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| guiott | 0:d177c0087d1f | 52 | GNU General Public License for more details. |
| guiott | 0:d177c0087d1f | 53 | |
| guiott | 0:d177c0087d1f | 54 | You should have received a copy of the GNU General Public License |
| guiott | 0:d177c0087d1f | 55 | along with LeonardoMbos.cpp. If not, see <http://www.gnu.org/licenses/>. |
| guiott | 0:d177c0087d1f | 56 | |
| guiott | 0:d177c0087d1f | 57 | ------------------------------------------------------------------------------- |
| guiott | 0:d177c0087d1f | 58 | */ |
| guiott | 0:d177c0087d1f | 59 | #include "Common.h" |
| guiott | 0:d177c0087d1f | 60 | #include "mbed.h" |
| guiott | 0:d177c0087d1f | 61 | #include <string> |
| guiott | 0:d177c0087d1f | 62 | #include "TextLCD.h" |
| guiott | 0:d177c0087d1f | 63 | #include "nmea/nmea.h" |
| guiott | 0:d177c0087d1f | 64 | #include "Init.h" |
| guiott | 0:d177c0087d1f | 65 | #include "Prototype.h" |
| guiott | 0:d177c0087d1f | 66 | #include "mbos.h" |
| guiott | 0:d177c0087d1f | 67 | #include "Tasks.h" |
| guiott | 0:d177c0087d1f | 68 | #include <Watchdog.h> |
| guiott | 0:d177c0087d1f | 69 | |
| guiott | 0:d177c0087d1f | 70 | TextLCD lcd(p12, p11, p24, p23, p22, p21, TextLCD::LCD20x4); // rs, e, d4-d7 |
| guiott | 0:d177c0087d1f | 71 | |
| guiott | 0:d177c0087d1f | 72 | Watchdog wd; |
| guiott | 0:d177c0087d1f | 73 | |
| guiott | 0:d177c0087d1f | 74 | // Set up RTOS; |
| guiott | 0:d177c0087d1f | 75 | mbos os(NUM_TASKS, NUM_TIMERS, NUM_RESOURCES); |
| guiott | 0:d177c0087d1f | 76 | |
| guiott | 0:d177c0087d1f | 77 | int main() |
| guiott | 0:d177c0087d1f | 78 | { |
| guiott | 0:d177c0087d1f | 79 | Initialize(); |
| guiott | 0:d177c0087d1f | 80 | if (wd.WatchdogCausedReset()) |
| guiott | 0:d177c0087d1f | 81 | { |
| guiott | 0:d177c0087d1f | 82 | pc.printf("Watchdog caused reset**********************************\r\n"); |
| guiott | 0:d177c0087d1f | 83 | } |
| guiott | 0:d177c0087d1f | 84 | wd.Configure(WDT_TIMER); // sets the timeout interval |
| guiott | 0:d177c0087d1f | 85 | |
| guiott | 0:d177c0087d1f | 86 | os.Start(); // the RTOS starts now |
| guiott | 0:d177c0087d1f | 87 | } |
| guiott | 0:d177c0087d1f | 88 | |
| guiott | 0:d177c0087d1f | 89 | //functions ===================================================================== |
| guiott | 0:d177c0087d1f | 90 | |
| guiott | 0:d177c0087d1f | 91 | int CmpRead(void) |
| guiott | 0:d177c0087d1f | 92 | {/** |
| guiott | 0:d177c0087d1f | 93 | *\brief Magnetic Compass reading |
| guiott | 0:d177c0087d1f | 94 | */ |
| guiott | 0:d177c0087d1f | 95 | int Cmp; |
| guiott | 0:d177c0087d1f | 96 | |
| guiott | 0:d177c0087d1f | 97 | // To Be Done |
| guiott | 0:d177c0087d1f | 98 | Cmp = 0; // ************debug |
| guiott | 0:d177c0087d1f | 99 | return(Cmp); |
| guiott | 0:d177c0087d1f | 100 | } |
| guiott | 0:d177c0087d1f | 101 | |
| guiott | 0:d177c0087d1f | 102 | void mbosIdleTask(void) |
| guiott | 0:d177c0087d1f | 103 | {/** |
| guiott | 0:d177c0087d1f | 104 | *\brief TASK 0 watchdog kick |
| guiott | 0:d177c0087d1f | 105 | */ |
| guiott | 0:d177c0087d1f | 106 | while(1) |
| guiott | 0:d177c0087d1f | 107 | { |
| guiott | 0:d177c0087d1f | 108 | wd.Service(); // kick the dog before the timeout |
| guiott | 0:d177c0087d1f | 109 | } |
| guiott | 0:d177c0087d1f | 110 | } |
| guiott | 0:d177c0087d1f | 111 | |
| guiott | 0:d177c0087d1f | 112 | void TempTask(void) |
| guiott | 0:d177c0087d1f | 113 | {/** |
| guiott | 0:d177c0087d1f | 114 | *\brief TASK 8 used to temporary test some functions |
| guiott | 0:d177c0087d1f | 115 | */ |
| guiott | 0:d177c0087d1f | 116 | static int TempAng; |
| guiott | 0:d177c0087d1f | 117 | while (1) |
| guiott | 0:d177c0087d1f | 118 | { |
| guiott | 0:d177c0087d1f | 119 | os.WaitEvent(TEMP_EVT); |
| guiott | 0:d177c0087d1f | 120 | |
| guiott | 0:d177c0087d1f | 121 | if((TempAng+=5) >=360) |
| guiott | 0:d177c0087d1f | 122 | { |
| guiott | 0:d177c0087d1f | 123 | TempAng=0; |
| guiott | 0:d177c0087d1f | 124 | } |
| guiott | 0:d177c0087d1f | 125 | Ang[Mag]=TempAng; |
| guiott | 0:d177c0087d1f | 126 | Ang[Dir]=360-TempAng; |
| guiott | 0:d177c0087d1f | 127 | Ang[Gps]=180-TempAng; |
| guiott | 0:d177c0087d1f | 128 | printf("%i %i %i \n", Ang[0], Ang[1], Ang[2]); |
| guiott | 0:d177c0087d1f | 129 | } |
| guiott | 0:d177c0087d1f | 130 | } |
| guiott | 0:d177c0087d1f | 131 | |
| guiott | 0:d177c0087d1f | 132 | void GpsStringParse(void) |
| guiott | 0:d177c0087d1f | 133 | {/** |
| guiott | 0:d177c0087d1f | 134 | *\brief parse the GPS string to extract info |
| guiott | 0:d177c0087d1f | 135 | */ |
| guiott | 0:d177c0087d1f | 136 | gps.attach(NULL); // Suspend serial interrupt while buffer change |
| guiott | 0:d177c0087d1f | 137 | size = writePointer; |
| guiott | 0:d177c0087d1f | 138 | writePointer = 0; |
| guiott | 0:d177c0087d1f | 139 | bufferSelect++; // Change buffer |
| guiott | 0:d177c0087d1f | 140 | gps.attach( &GpsSerialIsr ); // Resume serial interrupt |
| guiott | 0:d177c0087d1f | 141 | nmea_parse(&parser, msgBuff[(bufferSelect-1)&1], size, &info); |
| guiott | 0:d177c0087d1f | 142 | Coordinates(); // transform nmea coordinates in decimal degrees |
| guiott | 0:d177c0087d1f | 143 | } |
| guiott | 0:d177c0087d1f | 144 | |
| guiott | 0:d177c0087d1f | 145 | void GpsDist(void) |
| guiott | 0:d177c0087d1f | 146 | {/** |
| guiott | 0:d177c0087d1f | 147 | *\brief compute the distance and direction (forward and reverse) |
| guiott | 0:d177c0087d1f | 148 | from point A to point B on the ellipsoid |
| guiott | 0:d177c0087d1f | 149 | */ |
| guiott | 0:d177c0087d1f | 150 | |
| guiott | 0:d177c0087d1f | 151 | nmea_info2pos(&info, &Pos[0]); // current position |
| guiott | 0:d177c0087d1f | 152 | nmea_info2pos(&Dest, &Pos[1]); // destination |
| guiott | 0:d177c0087d1f | 153 | |
| guiott | 0:d177c0087d1f | 154 | Path.Dist = nmea_distance_ellipsoid(&Pos[0], &Pos[1], &Path.Azimuth[1], &Path.Azimuth[0]); |
| guiott | 0:d177c0087d1f | 155 | |
| guiott | 0:d177c0087d1f | 156 | if(Path.Azimuth[0] > NMEA_PI) |
| guiott | 0:d177c0087d1f | 157 | {// reverse direction |
| guiott | 0:d177c0087d1f | 158 | Path.Azimuth[0] = Path.Azimuth[0] - NMEA_PI; |
| guiott | 0:d177c0087d1f | 159 | } |
| guiott | 0:d177c0087d1f | 160 | else |
| guiott | 0:d177c0087d1f | 161 | { |
| guiott | 0:d177c0087d1f | 162 | Path.Azimuth[1] = Path.Azimuth[1] + NMEA_PI; |
| guiott | 0:d177c0087d1f | 163 | } |
| guiott | 0:d177c0087d1f | 164 | |
| guiott | 0:d177c0087d1f | 165 | for(int i=0; i < 2; i++) |
| guiott | 0:d177c0087d1f | 166 | { |
| guiott | 0:d177c0087d1f | 167 | Path.Azimuth[i]=(nmea_radian2degree(Path.Azimuth[i])); |
| guiott | 0:d177c0087d1f | 168 | if(Path.Azimuth[i] > 360) |
| guiott | 0:d177c0087d1f | 169 | { |
| guiott | 0:d177c0087d1f | 170 | Path.Azimuth[i] = Path.Azimuth[i] - 360; |
| guiott | 0:d177c0087d1f | 171 | } |
| guiott | 0:d177c0087d1f | 172 | if(Path.Azimuth[i] < 0) |
| guiott | 0:d177c0087d1f | 173 | { |
| guiott | 0:d177c0087d1f | 174 | Path.Azimuth[i] = Path.Azimuth[i] + 360; |
| guiott | 0:d177c0087d1f | 175 | } |
| guiott | 0:d177c0087d1f | 176 | } |
| guiott | 0:d177c0087d1f | 177 | } |
| guiott | 0:d177c0087d1f | 178 | |
| guiott | 0:d177c0087d1f | 179 | void LcdLightDimTask(void) |
| guiott | 0:d177c0087d1f | 180 | {/** |
| guiott | 0:d177c0087d1f | 181 | *\brief TASK 7, control the LCD backlight intensity to smoothly |
| guiott | 0:d177c0087d1f | 182 | switch it on or off |
| guiott | 0:d177c0087d1f | 183 | */ |
| guiott | 0:d177c0087d1f | 184 | static float LightVal=1; |
| guiott | 0:d177c0087d1f | 185 | |
| guiott | 0:d177c0087d1f | 186 | while(1) |
| guiott | 0:d177c0087d1f | 187 | { |
| guiott | 0:d177c0087d1f | 188 | os.WaitEvent(LCD_LIGHT_DIM_ON_EVT | LCD_LIGHT_DIM_OFF_EVT); |
| guiott | 0:d177c0087d1f | 189 | if(os.GetEvent()==LCD_LIGHT_DIM_ON_EVT) |
| guiott | 0:d177c0087d1f | 190 | { |
| guiott | 0:d177c0087d1f | 191 | if(LightVal<1) |
| guiott | 0:d177c0087d1f | 192 | { |
| guiott | 0:d177c0087d1f | 193 | for (LightVal=0; LightVal<=1; LightVal+=0.01f) |
| guiott | 0:d177c0087d1f | 194 | { |
| guiott | 0:d177c0087d1f | 195 | LcdBklight=LightVal; |
| guiott | 0:d177c0087d1f | 196 | wait_ms(10); |
| guiott | 0:d177c0087d1f | 197 | } |
| guiott | 0:d177c0087d1f | 198 | } |
| guiott | 0:d177c0087d1f | 199 | // Set the timer for the power saving backlight switch off |
| guiott | 0:d177c0087d1f | 200 | os.ClearTimer(LCD_LIGHT_DIM_OFF_TMR); |
| guiott | 0:d177c0087d1f | 201 | os.SetTimer(LCD_LIGHT_DIM_OFF_TMR, LCD_LIGHT_DIM_TIMER, 0); |
| guiott | 0:d177c0087d1f | 202 | } |
| guiott | 0:d177c0087d1f | 203 | else if(os.GetEvent()==LCD_LIGHT_DIM_OFF_EVT) |
| guiott | 0:d177c0087d1f | 204 | {// The dimming off is slower than lighting up |
| guiott | 0:d177c0087d1f | 205 | for (LightVal=1; LightVal>=0; LightVal-=0.01f) |
| guiott | 0:d177c0087d1f | 206 | { |
| guiott | 0:d177c0087d1f | 207 | LcdBklight=LightVal; |
| guiott | 0:d177c0087d1f | 208 | wait_ms(30); |
| guiott | 0:d177c0087d1f | 209 | } |
| guiott | 0:d177c0087d1f | 210 | } |
| guiott | 0:d177c0087d1f | 211 | } |
| guiott | 0:d177c0087d1f | 212 | } |
| guiott | 0:d177c0087d1f | 213 | |
| guiott | 0:d177c0087d1f | 214 | void Coordinates(void) |
| guiott | 0:d177c0087d1f | 215 | {/** |
| guiott | 0:d177c0087d1f | 216 | *\brief transform nmea coordinates in decimal degrees |
| guiott | 0:d177c0087d1f | 217 | */ |
| guiott | 0:d177c0087d1f | 218 | degrees = trunc(info.lat / 100.0); |
| guiott | 0:d177c0087d1f | 219 | minutes = info.lat - (degrees * 100.0); |
| guiott | 0:d177c0087d1f | 220 | latitude = degrees + minutes / 60.0; |
| guiott | 0:d177c0087d1f | 221 | degrees = trunc(info.lon / 100.0); |
| guiott | 0:d177c0087d1f | 222 | minutes = info.lon - (degrees * 100.0); |
| guiott | 0:d177c0087d1f | 223 | longitude = degrees + minutes / 60.0; |
| guiott | 0:d177c0087d1f | 224 | } |
| guiott | 0:d177c0087d1f | 225 | |
| guiott | 0:d177c0087d1f | 226 | void Deg2DegMinSec(double DecDeg, DegMinSec *DecSec) |
| guiott | 0:d177c0087d1f | 227 | {/** |
| guiott | 0:d177c0087d1f | 228 | *\brief convert decimalDeg to Deg Min decimalSec |
| guiott | 0:d177c0087d1f | 229 | */ |
| guiott | 0:d177c0087d1f | 230 | DecSec->Deg = trunc(DecDeg); |
| guiott | 0:d177c0087d1f | 231 | double MinDec = (DecDeg - DecSec->Deg); |
| guiott | 0:d177c0087d1f | 232 | DecSec->Min = trunc(MinDec * 60); |
| guiott | 0:d177c0087d1f | 233 | DecSec->Sec = (MinDec * 3600) - (DecSec->Min * 60); |
| guiott | 0:d177c0087d1f | 234 | } |
| guiott | 0:d177c0087d1f | 235 | |
| guiott | 0:d177c0087d1f | 236 | void ShowPcTask(void) |
| guiott | 0:d177c0087d1f | 237 | {/** |
| guiott | 0:d177c0087d1f | 238 | *\brief TASK 6, if a PC is connected, debug information can be sent to the console |
| guiott | 0:d177c0087d1f | 239 | */ |
| guiott | 0:d177c0087d1f | 240 | static int it = 0; |
| guiott | 0:d177c0087d1f | 241 | int i; |
| guiott | 0:d177c0087d1f | 242 | DegMinSec DecCoord; |
| guiott | 0:d177c0087d1f | 243 | |
| guiott | 0:d177c0087d1f | 244 | os.SetTimer(SHOW_PC_TMR, 1000, 1000); |
| guiott | 0:d177c0087d1f | 245 | while(1) |
| guiott | 0:d177c0087d1f | 246 | { |
| guiott | 0:d177c0087d1f | 247 | os.WaitEvent(SHOW_PC_EVT); |
| guiott | 0:d177c0087d1f | 248 | |
| guiott | 0:d177c0087d1f | 249 | if(pc.readable()) |
| guiott | 0:d177c0087d1f | 250 | {// wait for an input |
| guiott | 0:d177c0087d1f | 251 | PcMonitor = (pc.getc()-48); // digit a number to en/dis-able debug |
| guiott | 0:d177c0087d1f | 252 | } |
| guiott | 0:d177c0087d1f | 253 | |
| guiott | 0:d177c0087d1f | 254 | if( PcMonitor==1 || PcMonitor>5) |
| guiott | 0:d177c0087d1f | 255 | {// Display Info parameters |
| guiott | 0:d177c0087d1f | 256 | pc.printf( |
| guiott | 0:d177c0087d1f | 257 | "%03d, Lat: %f, Lon: %f, Sig:%d, Fix:%d, Inuse:%d\r\n", |
| guiott | 0:d177c0087d1f | 258 | it++, latitude, longitude, info.sig, info.fix, info.satinfo.inuse ); |
| guiott | 0:d177c0087d1f | 259 | for (i = 0; i < NMEA_MAXSAT; i++) |
| guiott | 0:d177c0087d1f | 260 | { |
| guiott | 0:d177c0087d1f | 261 | if (info.satinfo.sat[i].sig > 0) |
| guiott | 0:d177c0087d1f | 262 | pc.printf(" sat_id:%02d, sig:%02d, Inuse:%d\r\n", |
| guiott | 0:d177c0087d1f | 263 | info.satinfo.sat[i].id , info.satinfo.sat[i].sig, |
| guiott | 0:d177c0087d1f | 264 | info.satinfo.sat[i].in_use); |
| guiott | 0:d177c0087d1f | 265 | } |
| guiott | 0:d177c0087d1f | 266 | pc.printf("\r\n"); |
| guiott | 0:d177c0087d1f | 267 | } |
| guiott | 0:d177c0087d1f | 268 | |
| guiott | 0:d177c0087d1f | 269 | if( PcMonitor==2 || PcMonitor>5) |
| guiott | 0:d177c0087d1f | 270 | {// Display Distance parameters |
| guiott | 0:d177c0087d1f | 271 | Deg2DegMinSec(nmea_radian2degree(Pos[0].lat), &DecCoord); |
| guiott | 0:d177c0087d1f | 272 | pc.printf("Lat1:%d %d\'%.3f\" ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec); |
| guiott | 0:d177c0087d1f | 273 | Deg2DegMinSec(nmea_radian2degree(Pos[0].lon), &DecCoord); |
| guiott | 0:d177c0087d1f | 274 | pc.printf("Lon1:%d %d\'%.3f\" ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec); |
| guiott | 0:d177c0087d1f | 275 | Deg2DegMinSec(nmea_radian2degree(Pos[1].lat), &DecCoord); |
| guiott | 0:d177c0087d1f | 276 | pc.printf("Lat2:%d %d\'%.3f\" ", DecCoord.Deg, DecCoord.Min, DecCoord.Sec); |
| guiott | 0:d177c0087d1f | 277 | Deg2DegMinSec(nmea_radian2degree(Pos[1].lon), &DecCoord); |
| guiott | 0:d177c0087d1f | 278 | pc.printf("Lon2:%d %d\'%.3f\" \n", DecCoord.Deg, DecCoord.Min, DecCoord.Sec); |
| guiott | 0:d177c0087d1f | 279 | pc.printf("Dist:%f Azimuth Start:%f Azimuth Final:%f \n\n", |
| guiott | 0:d177c0087d1f | 280 | Path.Dist, Path.Azimuth[0], Path.Azimuth[1]); |
| guiott | 0:d177c0087d1f | 281 | } |
| guiott | 0:d177c0087d1f | 282 | } |
| guiott | 0:d177c0087d1f | 283 | } |
| guiott | 0:d177c0087d1f | 284 | |
| guiott | 0:d177c0087d1f | 285 | void ShowLcdTask(void) |
| guiott | 0:d177c0087d1f | 286 | {/** |
| guiott | 0:d177c0087d1f | 287 | *\brief TASK 3, display desired data on LCD |
| guiott | 0:d177c0087d1f | 288 | */ |
| guiott | 0:d177c0087d1f | 289 | |
| guiott | 0:d177c0087d1f | 290 | static int Previous=0; |
| guiott | 0:d177c0087d1f | 291 | |
| guiott | 0:d177c0087d1f | 292 | os.SetTimer(SHOW_LCD_TMR, SHOW_LCD_TIMER, 0); |
| guiott | 0:d177c0087d1f | 293 | while(1) |
| guiott | 0:d177c0087d1f | 294 | { |
| guiott | 0:d177c0087d1f | 295 | os.WaitEvent(SHOW_LCD_EVT); |
| guiott | 0:d177c0087d1f | 296 | |
| guiott | 0:d177c0087d1f | 297 | if(Previous != Menu) |
| guiott | 0:d177c0087d1f | 298 | {// clear the display when function changes |
| guiott | 0:d177c0087d1f | 299 | lcd.cls(); |
| guiott | 0:d177c0087d1f | 300 | Previous=Menu; |
| guiott | 0:d177c0087d1f | 301 | } |
| guiott | 0:d177c0087d1f | 302 | |
| guiott | 0:d177c0087d1f | 303 | switch(Menu) |
| guiott | 0:d177c0087d1f | 304 | { |
| guiott | 0:d177c0087d1f | 305 | case 0: |
| guiott | 0:d177c0087d1f | 306 | showSatLcd(); |
| guiott | 0:d177c0087d1f | 307 | Previous=0; |
| guiott | 0:d177c0087d1f | 308 | break; |
| guiott | 0:d177c0087d1f | 309 | |
| guiott | 0:d177c0087d1f | 310 | case 1: |
| guiott | 0:d177c0087d1f | 311 | if(info.sig != 0) |
| guiott | 0:d177c0087d1f | 312 | { |
| guiott | 0:d177c0087d1f | 313 | showInfoLcd(); |
| guiott | 0:d177c0087d1f | 314 | } |
| guiott | 0:d177c0087d1f | 315 | else |
| guiott | 0:d177c0087d1f | 316 | { |
| guiott | 0:d177c0087d1f | 317 | showSatLcd(); |
| guiott | 0:d177c0087d1f | 318 | } |
| guiott | 0:d177c0087d1f | 319 | Previous=1; |
| guiott | 0:d177c0087d1f | 320 | break; |
| guiott | 0:d177c0087d1f | 321 | |
| guiott | 0:d177c0087d1f | 322 | case 2: |
| guiott | 0:d177c0087d1f | 323 | showMenuLcd(); |
| guiott | 0:d177c0087d1f | 324 | Previous=2; |
| guiott | 0:d177c0087d1f | 325 | break; |
| guiott | 0:d177c0087d1f | 326 | |
| guiott | 0:d177c0087d1f | 327 | case 3: |
| guiott | 0:d177c0087d1f | 328 | showMenuLcd1(); |
| guiott | 0:d177c0087d1f | 329 | Previous=3; |
| guiott | 0:d177c0087d1f | 330 | break; |
| guiott | 0:d177c0087d1f | 331 | |
| guiott | 0:d177c0087d1f | 332 | case 4: |
| guiott | 0:d177c0087d1f | 333 | static int ChooseDir=0; |
| guiott | 0:d177c0087d1f | 334 | Ang[Gps]=info.direction; |
| guiott | 0:d177c0087d1f | 335 | Ang[Mag]=CmpRead()-info.declination; //Compass reading corrected by declination |
| guiott | 0:d177c0087d1f | 336 | Ang[Dir]=Path.Azimuth[0]; |
| guiott | 0:d177c0087d1f | 337 | |
| guiott | 0:d177c0087d1f | 338 | ChooseDir++; |
| guiott | 0:d177c0087d1f | 339 | if(ChooseDir<7) |
| guiott | 0:d177c0087d1f | 340 | { |
| guiott | 0:d177c0087d1f | 341 | showDirLcd(Mag); |
| guiott | 0:d177c0087d1f | 342 | } |
| guiott | 0:d177c0087d1f | 343 | else if(ChooseDir>=7 && ChooseDir<12) |
| guiott | 0:d177c0087d1f | 344 | { |
| guiott | 0:d177c0087d1f | 345 | showDirLcd(Gps); |
| guiott | 0:d177c0087d1f | 346 | } |
| guiott | 0:d177c0087d1f | 347 | else if(ChooseDir>=12) |
| guiott | 0:d177c0087d1f | 348 | { |
| guiott | 0:d177c0087d1f | 349 | ChooseDir=0; |
| guiott | 0:d177c0087d1f | 350 | } |
| guiott | 0:d177c0087d1f | 351 | showDirLcd(Dir); |
| guiott | 0:d177c0087d1f | 352 | |
| guiott | 0:d177c0087d1f | 353 | Previous=4; |
| guiott | 0:d177c0087d1f | 354 | break; |
| guiott | 0:d177c0087d1f | 355 | |
| guiott | 0:d177c0087d1f | 356 | default: |
| guiott | 0:d177c0087d1f | 357 | showInfoLcd(); |
| guiott | 0:d177c0087d1f | 358 | break; |
| guiott | 0:d177c0087d1f | 359 | } |
| guiott | 0:d177c0087d1f | 360 | //restart timer for other options |
| guiott | 0:d177c0087d1f | 361 | os.SetTimer(SHOW_LCD_TMR, SHOW_LCD_TIMER, 0); |
| guiott | 0:d177c0087d1f | 362 | } |
| guiott | 0:d177c0087d1f | 363 | } |
| guiott | 0:d177c0087d1f | 364 | |
| guiott | 0:d177c0087d1f | 365 | void showDirLcd(int Indx) |
| guiott | 0:d177c0087d1f | 366 | {/** |
| guiott | 0:d177c0087d1f | 367 | *\brief display a sort of compass on LCD |
| guiott | 0:d177c0087d1f | 368 | */ |
| guiott | 0:d177c0087d1f | 369 | int Angle; |
| guiott | 0:d177c0087d1f | 370 | |
| guiott | 0:d177c0087d1f | 371 | Angle=Ang[Indx]/18; |
| guiott | 0:d177c0087d1f | 372 | |
| guiott | 0:d177c0087d1f | 373 | lcd.locate(CmpPos[Indx],0); |
| guiott | 0:d177c0087d1f | 374 | lcd.printf(" %c ",0xA5); |
| guiott | 0:d177c0087d1f | 375 | lcd.locate(CmpPos[Indx],1); |
| guiott | 0:d177c0087d1f | 376 | lcd.printf("%c %c",0xA5, 0xA5); |
| guiott | 0:d177c0087d1f | 377 | lcd.locate(CmpPos[Indx],2); |
| guiott | 0:d177c0087d1f | 378 | lcd.printf("%c %c",0xA5, 0xA5); |
| guiott | 0:d177c0087d1f | 379 | lcd.locate(CmpPos[Indx],3); |
| guiott | 0:d177c0087d1f | 380 | lcd.printf(" %c ",0xA5); |
| guiott | 0:d177c0087d1f | 381 | |
| guiott | 0:d177c0087d1f | 382 | lcd.locate(CmpPos[Indx]+1,1); |
| guiott | 0:d177c0087d1f | 383 | lcd.printf(Lab[Indx]); |
| guiott | 0:d177c0087d1f | 384 | lcd.locate(CmpPos[Indx]+1,2); |
| guiott | 0:d177c0087d1f | 385 | lcd.printf("%03i",Ang[Indx]); |
| guiott | 0:d177c0087d1f | 386 | |
| guiott | 0:d177c0087d1f | 387 | switch (Angle) |
| guiott | 0:d177c0087d1f | 388 | { |
| guiott | 0:d177c0087d1f | 389 | case 0: |
| guiott | 0:d177c0087d1f | 390 | lcd.locate(CmpPos[Indx]+2,0); |
| guiott | 0:d177c0087d1f | 391 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 392 | break; |
| guiott | 0:d177c0087d1f | 393 | case 1: |
| guiott | 0:d177c0087d1f | 394 | lcd.locate(CmpPos[Indx]+2,0); |
| guiott | 0:d177c0087d1f | 395 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 396 | lcd.locate(CmpPos[Indx]+3,0); |
| guiott | 0:d177c0087d1f | 397 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 398 | break; |
| guiott | 0:d177c0087d1f | 399 | case 2: |
| guiott | 0:d177c0087d1f | 400 | lcd.locate(CmpPos[Indx]+3,0); |
| guiott | 0:d177c0087d1f | 401 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 402 | break; |
| guiott | 0:d177c0087d1f | 403 | case 3: |
| guiott | 0:d177c0087d1f | 404 | lcd.locate(CmpPos[Indx]+3,0); |
| guiott | 0:d177c0087d1f | 405 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 406 | lcd.locate(CmpPos[Indx]+4,1); |
| guiott | 0:d177c0087d1f | 407 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 408 | break; |
| guiott | 0:d177c0087d1f | 409 | case 4: |
| guiott | 0:d177c0087d1f | 410 | lcd.locate(CmpPos[Indx]+4,1); |
| guiott | 0:d177c0087d1f | 411 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 412 | break; |
| guiott | 0:d177c0087d1f | 413 | case 5: |
| guiott | 0:d177c0087d1f | 414 | lcd.locate(CmpPos[Indx]+4,1); |
| guiott | 0:d177c0087d1f | 415 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 416 | lcd.locate(CmpPos[Indx]+4,2); |
| guiott | 0:d177c0087d1f | 417 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 418 | break; |
| guiott | 0:d177c0087d1f | 419 | case 6: |
| guiott | 0:d177c0087d1f | 420 | lcd.locate(CmpPos[Indx]+4,2); |
| guiott | 0:d177c0087d1f | 421 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 422 | break; |
| guiott | 0:d177c0087d1f | 423 | case 7: |
| guiott | 0:d177c0087d1f | 424 | lcd.locate(CmpPos[Indx]+4,2); |
| guiott | 0:d177c0087d1f | 425 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 426 | lcd.locate(CmpPos[Indx]+3,3); |
| guiott | 0:d177c0087d1f | 427 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 428 | break; |
| guiott | 0:d177c0087d1f | 429 | case 8: |
| guiott | 0:d177c0087d1f | 430 | lcd.locate(CmpPos[Indx]+3,3); |
| guiott | 0:d177c0087d1f | 431 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 432 | break; |
| guiott | 0:d177c0087d1f | 433 | case 9: |
| guiott | 0:d177c0087d1f | 434 | lcd.locate(CmpPos[Indx]+3,3); |
| guiott | 0:d177c0087d1f | 435 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 436 | lcd.locate(CmpPos[Indx]+2,3); |
| guiott | 0:d177c0087d1f | 437 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 438 | break; |
| guiott | 0:d177c0087d1f | 439 | case 10: |
| guiott | 0:d177c0087d1f | 440 | lcd.locate(CmpPos[Indx]+2,3); |
| guiott | 0:d177c0087d1f | 441 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 442 | break; |
| guiott | 0:d177c0087d1f | 443 | case 11: |
| guiott | 0:d177c0087d1f | 444 | lcd.locate(CmpPos[Indx]+2,3); |
| guiott | 0:d177c0087d1f | 445 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 446 | lcd.locate(CmpPos[Indx]+1,3); |
| guiott | 0:d177c0087d1f | 447 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 448 | break; |
| guiott | 0:d177c0087d1f | 449 | case 12: |
| guiott | 0:d177c0087d1f | 450 | lcd.locate(CmpPos[Indx]+1,3); |
| guiott | 0:d177c0087d1f | 451 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 452 | break; |
| guiott | 0:d177c0087d1f | 453 | case 13: |
| guiott | 0:d177c0087d1f | 454 | lcd.locate(CmpPos[Indx]+1,3); |
| guiott | 0:d177c0087d1f | 455 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 456 | lcd.locate(CmpPos[Indx]+0,2); |
| guiott | 0:d177c0087d1f | 457 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 458 | break; |
| guiott | 0:d177c0087d1f | 459 | case 14: |
| guiott | 0:d177c0087d1f | 460 | lcd.locate(CmpPos[Indx]+0,2); |
| guiott | 0:d177c0087d1f | 461 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 462 | break; |
| guiott | 0:d177c0087d1f | 463 | case 15: |
| guiott | 0:d177c0087d1f | 464 | lcd.locate(CmpPos[Indx]+0,2); |
| guiott | 0:d177c0087d1f | 465 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 466 | lcd.locate(CmpPos[Indx]+0,1); |
| guiott | 0:d177c0087d1f | 467 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 468 | break; |
| guiott | 0:d177c0087d1f | 469 | case 16: |
| guiott | 0:d177c0087d1f | 470 | lcd.locate(CmpPos[Indx]+0,1); |
| guiott | 0:d177c0087d1f | 471 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 472 | break; |
| guiott | 0:d177c0087d1f | 473 | case 17: |
| guiott | 0:d177c0087d1f | 474 | lcd.locate(CmpPos[Indx]+0,1); |
| guiott | 0:d177c0087d1f | 475 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 476 | lcd.locate(CmpPos[Indx]+1,0); |
| guiott | 0:d177c0087d1f | 477 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 478 | break; |
| guiott | 0:d177c0087d1f | 479 | case 18: |
| guiott | 0:d177c0087d1f | 480 | lcd.locate(CmpPos[Indx]+1,0); |
| guiott | 0:d177c0087d1f | 481 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 482 | break; |
| guiott | 0:d177c0087d1f | 483 | case 19: |
| guiott | 0:d177c0087d1f | 484 | lcd.locate(CmpPos[Indx]+1,0); |
| guiott | 0:d177c0087d1f | 485 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 486 | lcd.locate(CmpPos[Indx]+2,0); |
| guiott | 0:d177c0087d1f | 487 | lcd.printf("%c",0xFF); |
| guiott | 0:d177c0087d1f | 488 | break; |
| guiott | 0:d177c0087d1f | 489 | } |
| guiott | 0:d177c0087d1f | 490 | } |
| guiott | 0:d177c0087d1f | 491 | |
| guiott | 0:d177c0087d1f | 492 | void showMenuLcd(void) |
| guiott | 0:d177c0087d1f | 493 | {/** |
| guiott | 0:d177c0087d1f | 494 | *\brief display a selection menu on LCD |
| guiott | 0:d177c0087d1f | 495 | */ |
| guiott | 0:d177c0087d1f | 496 | lcd.locate(0,0); |
| guiott | 0:d177c0087d1f | 497 | lcd.printf("1 - item 1"); |
| guiott | 0:d177c0087d1f | 498 | lcd.locate(0,1); |
| guiott | 0:d177c0087d1f | 499 | lcd.printf("2 - item 2"); |
| guiott | 0:d177c0087d1f | 500 | lcd.locate(0,2); |
| guiott | 0:d177c0087d1f | 501 | lcd.printf("3 - item 3"); |
| guiott | 0:d177c0087d1f | 502 | lcd.locate(0,3); |
| guiott | 0:d177c0087d1f | 503 | lcd.printf("4 - item 4"); |
| guiott | 0:d177c0087d1f | 504 | } |
| guiott | 0:d177c0087d1f | 505 | |
| guiott | 0:d177c0087d1f | 506 | void showMenuLcd1(void) |
| guiott | 0:d177c0087d1f | 507 | {/** |
| guiott | 0:d177c0087d1f | 508 | *\brief display a selection menu on LCD |
| guiott | 0:d177c0087d1f | 509 | */ |
| guiott | 0:d177c0087d1f | 510 | lcd.locate(0,0); |
| guiott | 0:d177c0087d1f | 511 | lcd.printf("5 - item 5"); |
| guiott | 0:d177c0087d1f | 512 | lcd.locate(0,1); |
| guiott | 0:d177c0087d1f | 513 | lcd.printf("6 - item 6"); |
| guiott | 0:d177c0087d1f | 514 | lcd.locate(0,2); |
| guiott | 0:d177c0087d1f | 515 | lcd.printf("7 - item 7"); |
| guiott | 0:d177c0087d1f | 516 | lcd.locate(0,3); |
| guiott | 0:d177c0087d1f | 517 | lcd.printf("8 - item 8"); |
| guiott | 0:d177c0087d1f | 518 | } |
| guiott | 0:d177c0087d1f | 519 | |
| guiott | 0:d177c0087d1f | 520 | void showSatLcd(void) |
| guiott | 0:d177c0087d1f | 521 | {/** |
| guiott | 0:d177c0087d1f | 522 | *\brief displays satellite informations |
| guiott | 0:d177c0087d1f | 523 | */ |
| guiott | 0:d177c0087d1f | 524 | for (int i = 0; i < NMEA_MAXSAT; i++) |
| guiott | 0:d177c0087d1f | 525 | { |
| guiott | 0:d177c0087d1f | 526 | if(info.satinfo.sat[i].id < 20) |
| guiott | 0:d177c0087d1f | 527 | { |
| guiott | 0:d177c0087d1f | 528 | lcd.locate((info.satinfo.sat[i].id % 20) - 1,0); |
| guiott | 0:d177c0087d1f | 529 | } |
| guiott | 0:d177c0087d1f | 530 | else |
| guiott | 0:d177c0087d1f | 531 | { |
| guiott | 0:d177c0087d1f | 532 | lcd.locate((info.satinfo.sat[i].id % 20) - 1,1); |
| guiott | 0:d177c0087d1f | 533 | } |
| guiott | 0:d177c0087d1f | 534 | |
| guiott | 0:d177c0087d1f | 535 | if (info.satinfo.sat[i].sig > 0) |
| guiott | 0:d177c0087d1f | 536 | { |
| guiott | 0:d177c0087d1f | 537 | lcd.printf("%d",info.satinfo.sat[i].sig/10); |
| guiott | 0:d177c0087d1f | 538 | } |
| guiott | 0:d177c0087d1f | 539 | else |
| guiott | 0:d177c0087d1f | 540 | { |
| guiott | 0:d177c0087d1f | 541 | lcd.printf(" "); |
| guiott | 0:d177c0087d1f | 542 | } |
| guiott | 0:d177c0087d1f | 543 | } |
| guiott | 0:d177c0087d1f | 544 | lcd.locate(0,2); |
| guiott | 0:d177c0087d1f | 545 | lcd.printf("12345678901234567890"); |
| guiott | 0:d177c0087d1f | 546 | lcd.locate(0, 3); |
| guiott | 0:d177c0087d1f | 547 | lcd.printf("P%2.1f H%2.1f V%2.1f %iD", |
| guiott | 0:d177c0087d1f | 548 | info.PDOP, info.HDOP, info.VDOP,info.fix); |
| guiott | 0:d177c0087d1f | 549 | } |
| guiott | 0:d177c0087d1f | 550 | |
| guiott | 0:d177c0087d1f | 551 | void showInfoLcd(void) |
| guiott | 0:d177c0087d1f | 552 | {/** |
| guiott | 0:d177c0087d1f | 553 | *\brief Show nmea info on LCD |
| guiott | 0:d177c0087d1f | 554 | */ |
| guiott | 0:d177c0087d1f | 555 | |
| guiott | 0:d177c0087d1f | 556 | // static int lastSat = 0; |
| guiott | 0:d177c0087d1f | 557 | // int satInview = 0; |
| guiott | 0:d177c0087d1f | 558 | |
| guiott | 0:d177c0087d1f | 559 | if(info.sig != 0) |
| guiott | 0:d177c0087d1f | 560 | { |
| guiott | 0:d177c0087d1f | 561 | lcd.locate(0, 0); |
| guiott | 0:d177c0087d1f | 562 | lcd.printf("%2.5f%c %3.5f%c", |
| guiott | 0:d177c0087d1f | 563 | latitude, info.lat >= 0 ? 'N': 'S', |
| guiott | 0:d177c0087d1f | 564 | longitude, info.lon >= 0 ? 'E': 'W'); |
| guiott | 0:d177c0087d1f | 565 | |
| guiott | 0:d177c0087d1f | 566 | lcd.locate(0, 1); |
| guiott | 0:d177c0087d1f | 567 | lcd.printf("H%.0f D%.0f_%.1f %iD", info.elv, info.direction, info.declination, info.fix); |
| guiott | 0:d177c0087d1f | 568 | |
| guiott | 0:d177c0087d1f | 569 | lcd.locate(0, 2); |
| guiott | 0:d177c0087d1f | 570 | lcd.printf("P%2.1f H%2.1f V%2.1f S%i/%i", |
| guiott | 0:d177c0087d1f | 571 | info.PDOP, info.HDOP, info.VDOP, info.satinfo.inuse, info.satinfo.inview); |
| guiott | 0:d177c0087d1f | 572 | |
| guiott | 0:d177c0087d1f | 573 | /* lcd.locate(0, 3); |
| guiott | 0:d177c0087d1f | 574 | lcd.printf("%02d-%02d-%04d %02d:%02d:%02d ", |
| guiott | 0:d177c0087d1f | 575 | info.utc.day, info.utc.mon + 1, info.utc.year + 1900, |
| guiott | 0:d177c0087d1f | 576 | info.utc.hour, info.utc.min, info.utc.sec); // Display JST (UTC + 9) |
| guiott | 0:d177c0087d1f | 577 | */ |
| guiott | 0:d177c0087d1f | 578 | |
| guiott | 0:d177c0087d1f | 579 | /* |
| guiott | 0:d177c0087d1f | 580 | lcd.locate(0, 5); |
| guiott | 0:d177c0087d1f | 581 | for (int i = 0; i < NMEA_MAXSAT; i++) { |
| guiott | 0:d177c0087d1f | 582 | if (info.satinfo.sat[i].sig > 0) { |
| guiott | 0:d177c0087d1f | 583 | satInview++; |
| guiott | 0:d177c0087d1f | 584 | lcd.printf(" sat_id:%02d, sig:%02d, Inuse:%d \n", |
| guiott | 0:d177c0087d1f | 585 | info.satinfo.sat[i].id , info.satinfo.sat[i].sig, |
| guiott | 0:d177c0087d1f | 586 | info.satinfo.sat[i].in_use); |
| guiott | 0:d177c0087d1f | 587 | } |
| guiott | 0:d177c0087d1f | 588 | } |
| guiott | 0:d177c0087d1f | 589 | for (int j = satInview; j <= lastSat; j++) |
| guiott | 0:d177c0087d1f | 590 | lcd.printf(" \n"); // delete line |
| guiott | 0:d177c0087d1f | 591 | lastSat = satInview; |
| guiott | 0:d177c0087d1f | 592 | */ |
| guiott | 0:d177c0087d1f | 593 | |
| guiott | 0:d177c0087d1f | 594 | } |
| guiott | 0:d177c0087d1f | 595 | else |
| guiott | 0:d177c0087d1f | 596 | { |
| guiott | 0:d177c0087d1f | 597 | lcd.cls(); |
| guiott | 0:d177c0087d1f | 598 | lcd.locate(0,0); |
| guiott | 0:d177c0087d1f | 599 | lcd.printf("NO FIX"); |
| guiott | 0:d177c0087d1f | 600 | } |
| guiott | 0:d177c0087d1f | 601 | |
| guiott | 0:d177c0087d1f | 602 | // Grab a snapshot of the current RTC time. |
| guiott | 0:d177c0087d1f | 603 | time_t seconds = time(NULL); |
| guiott | 0:d177c0087d1f | 604 | char buffer[32]; |
| guiott | 0:d177c0087d1f | 605 | if((info.sig != 0)&&(SetTimeOk!=0)) |
| guiott | 0:d177c0087d1f | 606 | { |
| guiott | 0:d177c0087d1f | 607 | strftime(buffer, 32, "%x %X", localtime(&seconds)); |
| guiott | 0:d177c0087d1f | 608 | } |
| guiott | 0:d177c0087d1f | 609 | else |
| guiott | 0:d177c0087d1f | 610 | {// if GPS time not valid doesn't display seconds |
| guiott | 0:d177c0087d1f | 611 | strftime(buffer, 32, "%x %H:%M", localtime(&seconds)); |
| guiott | 0:d177c0087d1f | 612 | SetTimeOk = 0; // RTC was not set to a valid time |
| guiott | 0:d177c0087d1f | 613 | } |
| guiott | 0:d177c0087d1f | 614 | lcd.locate(0,3); |
| guiott | 0:d177c0087d1f | 615 | lcd.printf("%s", buffer); |
| guiott | 0:d177c0087d1f | 616 | } |
| guiott | 0:d177c0087d1f | 617 | |
| guiott | 0:d177c0087d1f | 618 | void SetTimeTask(void) |
| guiott | 0:d177c0087d1f | 619 | {/** |
| guiott | 0:d177c0087d1f | 620 | *\brief TASK 5, Set RTC system time if the GPS time is valid |
| guiott | 0:d177c0087d1f | 621 | */ |
| guiott | 0:d177c0087d1f | 622 | struct tm t; |
| guiott | 0:d177c0087d1f | 623 | |
| guiott | 0:d177c0087d1f | 624 | os.SetTimer(SET_TIME_TMR, 60000, 60000); |
| guiott | 0:d177c0087d1f | 625 | while(1) |
| guiott | 0:d177c0087d1f | 626 | { |
| guiott | 0:d177c0087d1f | 627 | os.WaitEvent(SET_TIME_EVT); |
| guiott | 0:d177c0087d1f | 628 | |
| guiott | 0:d177c0087d1f | 629 | if(info.sig != 0) |
| guiott | 0:d177c0087d1f | 630 | { |
| guiott | 0:d177c0087d1f | 631 | t.tm_mday=info.utc.day; |
| guiott | 0:d177c0087d1f | 632 | t.tm_mon=info.utc.mon; |
| guiott | 0:d177c0087d1f | 633 | t.tm_year=info.utc.year; |
| guiott | 0:d177c0087d1f | 634 | t.tm_hour=info.utc.hour; |
| guiott | 0:d177c0087d1f | 635 | t.tm_min=info.utc.min; |
| guiott | 0:d177c0087d1f | 636 | t.tm_sec=info.utc.sec; |
| guiott | 0:d177c0087d1f | 637 | |
| guiott | 0:d177c0087d1f | 638 | time_t seconds = mktime(&t); |
| guiott | 0:d177c0087d1f | 639 | set_time(seconds); |
| guiott | 0:d177c0087d1f | 640 | |
| guiott | 0:d177c0087d1f | 641 | SetTimeOk = 1; // RTC was set |
| guiott | 0:d177c0087d1f | 642 | } |
| guiott | 0:d177c0087d1f | 643 | } |
| guiott | 0:d177c0087d1f | 644 | } |
| guiott | 0:d177c0087d1f | 645 | |
| guiott | 0:d177c0087d1f | 646 | void LedBlinkTask(void) |
| guiott | 0:d177c0087d1f | 647 | {/** |
| guiott | 0:d177c0087d1f | 648 | *\brief TASK 4 |
| guiott | 0:d177c0087d1f | 649 | LED 1: Quick blink=NOT fix, Slow blink=fix OK |
| guiott | 0:d177c0087d1f | 650 | LED 2: blinks proportionally to HDOP |
| guiott | 0:d177c0087d1f | 651 | LED 3: blinks proportionally to VDOP |
| guiott | 0:d177c0087d1f | 652 | */ |
| guiott | 0:d177c0087d1f | 653 | |
| guiott | 0:d177c0087d1f | 654 | static int LedCnt1=0; |
| guiott | 0:d177c0087d1f | 655 | // static int LedCnt2=0; |
| guiott | 0:d177c0087d1f | 656 | int OnH=0; |
| guiott | 0:d177c0087d1f | 657 | int OnV=0; |
| guiott | 0:d177c0087d1f | 658 | |
| guiott | 0:d177c0087d1f | 659 | #define MAX 20 |
| guiott | 0:d177c0087d1f | 660 | |
| guiott | 0:d177c0087d1f | 661 | os.SetTimer(LED_BLINK_TMR, 100, 100); |
| guiott | 0:d177c0087d1f | 662 | |
| guiott | 0:d177c0087d1f | 663 | while(1) |
| guiott | 0:d177c0087d1f | 664 | { |
| guiott | 0:d177c0087d1f | 665 | os.WaitEvent(LED_BLINK_EVT); |
| guiott | 0:d177c0087d1f | 666 | |
| guiott | 0:d177c0087d1f | 667 | if((info.HDOP>0)&&(info.HDOP<=2)) |
| guiott | 0:d177c0087d1f | 668 | { |
| guiott | 0:d177c0087d1f | 669 | OnH=2; |
| guiott | 0:d177c0087d1f | 670 | } |
| guiott | 0:d177c0087d1f | 671 | else if((info.HDOP>2)&&(info.HDOP<=4)) |
| guiott | 0:d177c0087d1f | 672 | { |
| guiott | 0:d177c0087d1f | 673 | OnH=4; |
| guiott | 0:d177c0087d1f | 674 | } |
| guiott | 0:d177c0087d1f | 675 | else if((info.HDOP>4)&&(info.HDOP<=6)) |
| guiott | 0:d177c0087d1f | 676 | { |
| guiott | 0:d177c0087d1f | 677 | OnH=6; |
| guiott | 0:d177c0087d1f | 678 | } |
| guiott | 0:d177c0087d1f | 679 | else if((info.HDOP>6)) |
| guiott | 0:d177c0087d1f | 680 | { |
| guiott | 0:d177c0087d1f | 681 | OnH=MAX/2; |
| guiott | 0:d177c0087d1f | 682 | } |
| guiott | 0:d177c0087d1f | 683 | |
| guiott | 0:d177c0087d1f | 684 | if((info.VDOP>0)&&(info.VDOP<=2)) |
| guiott | 0:d177c0087d1f | 685 | { |
| guiott | 0:d177c0087d1f | 686 | OnV=2; |
| guiott | 0:d177c0087d1f | 687 | } |
| guiott | 0:d177c0087d1f | 688 | else if((info.VDOP>2)&&(info.VDOP<=4)) |
| guiott | 0:d177c0087d1f | 689 | { |
| guiott | 0:d177c0087d1f | 690 | OnV=4; |
| guiott | 0:d177c0087d1f | 691 | } |
| guiott | 0:d177c0087d1f | 692 | else if((info.VDOP>4)&&(info.VDOP<=6)) |
| guiott | 0:d177c0087d1f | 693 | { |
| guiott | 0:d177c0087d1f | 694 | OnV=6; |
| guiott | 0:d177c0087d1f | 695 | } |
| guiott | 0:d177c0087d1f | 696 | else if((info.VDOP>6)) |
| guiott | 0:d177c0087d1f | 697 | { |
| guiott | 0:d177c0087d1f | 698 | OnV=MAX/2; |
| guiott | 0:d177c0087d1f | 699 | } |
| guiott | 0:d177c0087d1f | 700 | |
| guiott | 0:d177c0087d1f | 701 | if(info.sig == 0) |
| guiott | 0:d177c0087d1f | 702 | { |
| guiott | 0:d177c0087d1f | 703 | led1=!led1; |
| guiott | 0:d177c0087d1f | 704 | led2=0; |
| guiott | 0:d177c0087d1f | 705 | led3=0; |
| guiott | 0:d177c0087d1f | 706 | } |
| guiott | 0:d177c0087d1f | 707 | else |
| guiott | 0:d177c0087d1f | 708 | { |
| guiott | 0:d177c0087d1f | 709 | if(LedCnt1<=MAX/2) |
| guiott | 0:d177c0087d1f | 710 | { |
| guiott | 0:d177c0087d1f | 711 | LedCnt1++; |
| guiott | 0:d177c0087d1f | 712 | led1=0; |
| guiott | 0:d177c0087d1f | 713 | if(LedCnt1<OnH) |
| guiott | 0:d177c0087d1f | 714 | { |
| guiott | 0:d177c0087d1f | 715 | led2=!led2; |
| guiott | 0:d177c0087d1f | 716 | } |
| guiott | 0:d177c0087d1f | 717 | else |
| guiott | 0:d177c0087d1f | 718 | { |
| guiott | 0:d177c0087d1f | 719 | led2=0; |
| guiott | 0:d177c0087d1f | 720 | } |
| guiott | 0:d177c0087d1f | 721 | |
| guiott | 0:d177c0087d1f | 722 | if(LedCnt1<OnV) |
| guiott | 0:d177c0087d1f | 723 | { |
| guiott | 0:d177c0087d1f | 724 | led3=!led3; |
| guiott | 0:d177c0087d1f | 725 | } |
| guiott | 0:d177c0087d1f | 726 | else |
| guiott | 0:d177c0087d1f | 727 | { |
| guiott | 0:d177c0087d1f | 728 | led3=0; |
| guiott | 0:d177c0087d1f | 729 | } |
| guiott | 0:d177c0087d1f | 730 | } |
| guiott | 0:d177c0087d1f | 731 | else if((LedCnt1>MAX/2)&&(LedCnt1<=MAX)) |
| guiott | 0:d177c0087d1f | 732 | { |
| guiott | 0:d177c0087d1f | 733 | LedCnt1++; |
| guiott | 0:d177c0087d1f | 734 | led1=1; |
| guiott | 0:d177c0087d1f | 735 | led2=0; |
| guiott | 0:d177c0087d1f | 736 | led3=0; |
| guiott | 0:d177c0087d1f | 737 | } |
| guiott | 0:d177c0087d1f | 738 | else if(LedCnt1>MAX) |
| guiott | 0:d177c0087d1f | 739 | { |
| guiott | 0:d177c0087d1f | 740 | LedCnt1=0; |
| guiott | 0:d177c0087d1f | 741 | } |
| guiott | 0:d177c0087d1f | 742 | } |
| guiott | 0:d177c0087d1f | 743 | } |
| guiott | 0:d177c0087d1f | 744 | } |
| guiott | 0:d177c0087d1f | 745 | |
| guiott | 0:d177c0087d1f | 746 | |
| guiott | 0:d177c0087d1f | 747 | void KeypadTask(void) |
| guiott | 0:d177c0087d1f | 748 | {/** |
| guiott | 0:d177c0087d1f | 749 | *\brief TASK 2, Keypad management. |
| guiott | 0:d177c0087d1f | 750 | It uses just one ADC port. Five keys switch a 5 resistor ladder |
| guiott | 0:d177c0087d1f | 751 | obtaining a unique voltage for each key, with a priority that |
| guiott | 0:d177c0087d1f | 752 | depends from the position of the resistor in the ladder. |
| guiott | 0:d177c0087d1f | 753 | This function makes an average of 50 analog values (50ms) before |
| guiott | 0:d177c0087d1f | 754 | confirming the output, to filter out some noise and debounce keys |
| guiott | 0:d177c0087d1f | 755 | */ |
| guiott | 0:d177c0087d1f | 756 | |
| guiott | 0:d177c0087d1f | 757 | static float KeypadAcc; // accumulator to compute average |
| guiott | 0:d177c0087d1f | 758 | static float KeypadPrev; // previous value to compute variation |
| guiott | 0:d177c0087d1f | 759 | static int KeypadCount; // all samples number |
| guiott | 0:d177c0087d1f | 760 | static int KeypadRealCount; // valid samples only |
| guiott | 0:d177c0087d1f | 761 | float KeypadVal; |
| guiott | 0:d177c0087d1f | 762 | static char KeyPrev='-'; |
| guiott | 0:d177c0087d1f | 763 | |
| guiott | 0:d177c0087d1f | 764 | os.SetTimer(KEYPAD_TMR, 1, 1); |
| guiott | 0:d177c0087d1f | 765 | while(1) |
| guiott | 0:d177c0087d1f | 766 | { |
| guiott | 0:d177c0087d1f | 767 | os.WaitEvent(KEYPAD_EVT); |
| guiott | 0:d177c0087d1f | 768 | float InValue = KeypadIn.read(); |
| guiott | 0:d177c0087d1f | 769 | if ((InValue > 0.3) && (abs(InValue - KeypadPrev) < 0.1)) |
| guiott | 0:d177c0087d1f | 770 | {// makes the average only of the values above a threshold |
| guiott | 0:d177c0087d1f | 771 | // and within an almost stable range |
| guiott | 0:d177c0087d1f | 772 | KeypadAcc+=InValue; |
| guiott | 0:d177c0087d1f | 773 | KeypadRealCount++; |
| guiott | 0:d177c0087d1f | 774 | } |
| guiott | 0:d177c0087d1f | 775 | |
| guiott | 0:d177c0087d1f | 776 | KeypadCount++; |
| guiott | 0:d177c0087d1f | 777 | KeypadPrev=InValue; |
| guiott | 0:d177c0087d1f | 778 | |
| guiott | 0:d177c0087d1f | 779 | if (KeypadCount >=50) |
| guiott | 0:d177c0087d1f | 780 | { |
| guiott | 0:d177c0087d1f | 781 | if(KeypadRealCount > 25) |
| guiott | 0:d177c0087d1f | 782 | { |
| guiott | 0:d177c0087d1f | 783 | KeypadVal=KeypadAcc/KeypadRealCount; |
| guiott | 0:d177c0087d1f | 784 | } |
| guiott | 0:d177c0087d1f | 785 | else |
| guiott | 0:d177c0087d1f | 786 | {// not enough values to average |
| guiott | 0:d177c0087d1f | 787 | |
| guiott | 0:d177c0087d1f | 788 | KeypadVal=0; |
| guiott | 0:d177c0087d1f | 789 | } |
| guiott | 0:d177c0087d1f | 790 | |
| guiott | 0:d177c0087d1f | 791 | KeypadAcc=0; |
| guiott | 0:d177c0087d1f | 792 | KeypadCount=0; |
| guiott | 0:d177c0087d1f | 793 | KeypadRealCount=0; |
| guiott | 0:d177c0087d1f | 794 | |
| guiott | 0:d177c0087d1f | 795 | if(KeypadVal <0.15) |
| guiott | 0:d177c0087d1f | 796 | { |
| guiott | 0:d177c0087d1f | 797 | Key='-'; |
| guiott | 0:d177c0087d1f | 798 | } |
| guiott | 0:d177c0087d1f | 799 | else if(KeypadVal>=0.3 && KeypadVal<0.35) |
| guiott | 0:d177c0087d1f | 800 | { |
| guiott | 0:d177c0087d1f | 801 | Key='E'; |
| guiott | 0:d177c0087d1f | 802 | } |
| guiott | 0:d177c0087d1f | 803 | else if(KeypadVal>=0.42 && KeypadVal<0.50) |
| guiott | 0:d177c0087d1f | 804 | { |
| guiott | 0:d177c0087d1f | 805 | Key='v'; |
| guiott | 0:d177c0087d1f | 806 | } |
| guiott | 0:d177c0087d1f | 807 | else if(KeypadVal>=0.60 && KeypadVal<0.65) |
| guiott | 0:d177c0087d1f | 808 | { |
| guiott | 0:d177c0087d1f | 809 | Key='^'; |
| guiott | 0:d177c0087d1f | 810 | } |
| guiott | 0:d177c0087d1f | 811 | else if(KeypadVal>=0.74 && KeypadVal<0.78) |
| guiott | 0:d177c0087d1f | 812 | { |
| guiott | 0:d177c0087d1f | 813 | Key='>'; |
| guiott | 0:d177c0087d1f | 814 | } |
| guiott | 0:d177c0087d1f | 815 | else if(KeypadVal>=0.85) |
| guiott | 0:d177c0087d1f | 816 | { |
| guiott | 0:d177c0087d1f | 817 | Key='<'; |
| guiott | 0:d177c0087d1f | 818 | } |
| guiott | 0:d177c0087d1f | 819 | |
| guiott | 0:d177c0087d1f | 820 | if (Key!='-' && Key!=KeyPrev) |
| guiott | 0:d177c0087d1f | 821 | {// switch on the LCD backlight if key pressed |
| guiott | 0:d177c0087d1f | 822 | os.SetEvent(LCD_LIGHT_DIM_ON_EVT, LCD_LIGHT_DIM_TASK); |
| guiott | 0:d177c0087d1f | 823 | } |
| guiott | 0:d177c0087d1f | 824 | KeyPrev=Key; |
| guiott | 0:d177c0087d1f | 825 | switch (Key) |
| guiott | 0:d177c0087d1f | 826 | { |
| guiott | 0:d177c0087d1f | 827 | case '^': |
| guiott | 0:d177c0087d1f | 828 | Menu=0; |
| guiott | 0:d177c0087d1f | 829 | break; |
| guiott | 0:d177c0087d1f | 830 | |
| guiott | 0:d177c0087d1f | 831 | case '>': |
| guiott | 0:d177c0087d1f | 832 | Menu=1; |
| guiott | 0:d177c0087d1f | 833 | break; |
| guiott | 0:d177c0087d1f | 834 | |
| guiott | 0:d177c0087d1f | 835 | case 'v': |
| guiott | 0:d177c0087d1f | 836 | Menu=2; |
| guiott | 0:d177c0087d1f | 837 | break; |
| guiott | 0:d177c0087d1f | 838 | |
| guiott | 0:d177c0087d1f | 839 | case '<': |
| guiott | 0:d177c0087d1f | 840 | Menu=3; |
| guiott | 0:d177c0087d1f | 841 | break; |
| guiott | 0:d177c0087d1f | 842 | |
| guiott | 0:d177c0087d1f | 843 | case 'E': |
| guiott | 0:d177c0087d1f | 844 | Menu=4; |
| guiott | 0:d177c0087d1f | 845 | break; |
| guiott | 0:d177c0087d1f | 846 | } |
| guiott | 0:d177c0087d1f | 847 | } |
| guiott | 0:d177c0087d1f | 848 | } |
| guiott | 0:d177c0087d1f | 849 | } |
| guiott | 0:d177c0087d1f | 850 | |
| guiott | 0:d177c0087d1f | 851 | void trace_h(const char *str, int str_size) |
| guiott | 0:d177c0087d1f | 852 | {/** |
| guiott | 0:d177c0087d1f | 853 | *\brief output on console what's received on GPS serial |
| guiott | 0:d177c0087d1f | 854 | Callback function for NMEA parser buffer trace |
| guiott | 0:d177c0087d1f | 855 | */ |
| guiott | 0:d177c0087d1f | 856 | if( PcMonitor==5 || PcMonitor>5) |
| guiott | 0:d177c0087d1f | 857 | { |
| guiott | 0:d177c0087d1f | 858 | for (int i = 0; i < str_size; i++) |
| guiott | 0:d177c0087d1f | 859 | { |
| guiott | 0:d177c0087d1f | 860 | pc.putc(*str++); |
| guiott | 0:d177c0087d1f | 861 | } |
| guiott | 0:d177c0087d1f | 862 | } |
| guiott | 0:d177c0087d1f | 863 | } |
| guiott | 0:d177c0087d1f | 864 | |
| guiott | 0:d177c0087d1f | 865 | void error_h(const char *str, int str_size) |
| guiott | 0:d177c0087d1f | 866 | {/** |
| guiott | 0:d177c0087d1f | 867 | *\brief Callback function for NMEA parser error |
| guiott | 0:d177c0087d1f | 868 | */ |
| guiott | 0:d177c0087d1f | 869 | for (int i = 0; i < str_size; i++) |
| guiott | 0:d177c0087d1f | 870 | { |
| guiott | 0:d177c0087d1f | 871 | pc.putc(*str++); |
| guiott | 0:d177c0087d1f | 872 | } |
| guiott | 0:d177c0087d1f | 873 | } |
| guiott | 0:d177c0087d1f | 874 | |
| guiott | 0:d177c0087d1f | 875 | void GpsSerialTask(void) |
| guiott | 0:d177c0087d1f | 876 | {/** |
| guiott | 0:d177c0087d1f | 877 | *\brief TASK 1, wait for the event then get the input char |
| guiott | 0:d177c0087d1f | 878 | */ |
| guiott | 0:d177c0087d1f | 879 | while(1) |
| guiott | 0:d177c0087d1f | 880 | { |
| guiott | 0:d177c0087d1f | 881 | os.WaitEvent(GPS_SERIAL_IN_EVT); |
| guiott | 0:d177c0087d1f | 882 | GpsStringParse(); |
| guiott | 0:d177c0087d1f | 883 | |
| guiott | 0:d177c0087d1f | 884 | Dest.lat= 4151.32496; // ***************debug |
| guiott | 0:d177c0087d1f | 885 | Dest.lon= 1229.34; // *************debug |
| guiott | 0:d177c0087d1f | 886 | |
| guiott | 0:d177c0087d1f | 887 | GpsDist(); |
| guiott | 0:d177c0087d1f | 888 | } |
| guiott | 0:d177c0087d1f | 889 | } |
| guiott | 0:d177c0087d1f | 890 | |
| guiott | 0:d177c0087d1f | 891 | void GpsSerialIsr(void) |
| guiott | 0:d177c0087d1f | 892 | {/** |
| guiott | 0:d177c0087d1f | 893 | *\brief Interrupt handler for serial Rx |
| guiott | 0:d177c0087d1f | 894 | set the event for the serial task |
| guiott | 0:d177c0087d1f | 895 | */ |
| guiott | 0:d177c0087d1f | 896 | char c = gps.getc(); |
| guiott | 0:d177c0087d1f | 897 | msgBuff[bufferSelect & 1][writePointer] = c; |
| guiott | 0:d177c0087d1f | 898 | if (writePointer++ == BUFF_SIZE) |
| guiott | 0:d177c0087d1f | 899 | { |
| guiott | 0:d177c0087d1f | 900 | writePointer = 0; |
| guiott | 0:d177c0087d1f | 901 | } |
| guiott | 0:d177c0087d1f | 902 | if (writePointer > 200) |
| guiott | 0:d177c0087d1f | 903 | {// GPS input buffer full, start computing coordinates |
| guiott | 0:d177c0087d1f | 904 | os.SetEvent(GPS_SERIAL_IN_EVT, GPS_SERIAL_TASK); |
| guiott | 0:d177c0087d1f | 905 | } |
| guiott | 0:d177c0087d1f | 906 | } |
| guiott | 0:d177c0087d1f | 907 | |
| guiott | 0:d177c0087d1f | 908 | double trunc(double v) |
| guiott | 0:d177c0087d1f | 909 | {/** |
| guiott | 0:d177c0087d1f | 910 | *\brief Return nearest integer vaule less than input |
| guiott | 0:d177c0087d1f | 911 | * |
| guiott | 0:d177c0087d1f | 912 | *\parameters double variable to get nearest ingeger |
| guiott | 0:d177c0087d1f | 913 | * |
| guiott | 0:d177c0087d1f | 914 | *\return double |
| guiott | 0:d177c0087d1f | 915 | */ |
| guiott | 0:d177c0087d1f | 916 | if(v < 0.0) |
| guiott | 0:d177c0087d1f | 917 | { |
| guiott | 0:d177c0087d1f | 918 | v*= -1.0; |
| guiott | 0:d177c0087d1f | 919 | v = floor(v); |
| guiott | 0:d177c0087d1f | 920 | v*=-1.0; |
| guiott | 0:d177c0087d1f | 921 | } else { |
| guiott | 0:d177c0087d1f | 922 | v = floor(v); |
| guiott | 0:d177c0087d1f | 923 | } |
| guiott | 0:d177c0087d1f | 924 | return v; |
| guiott | 0:d177c0087d1f | 925 | } |