|
| 1 | +using System; |
| 2 | +using System.Text; |
| 3 | +using Windows.Devices.I2c; |
| 4 | + |
| 5 | +namespace nanoframework.Drivers.GPS |
| 6 | +{ |
| 7 | + public class IesShieldGps |
| 8 | + { |
| 9 | + private readonly int _address; |
| 10 | + private I2cDevice _gpsController; |
| 11 | + public const byte GPAM_DEFAULT_ADDDRESS = 0x68; // GPM I2C Register |
| 12 | + |
| 13 | + /// <summary> |
| 14 | + /// I2C address of the IES-SHIELD-GPS device. |
| 15 | + /// </summary> |
| 16 | + public int Address => _address; |
| 17 | + |
| 18 | + /// <summary> |
| 19 | + /// Creates a driver for the IES-SHIELD-GPS. |
| 20 | + /// </summary> |
| 21 | + /// <param name="i2cBus">The I2C bus where the device is connected to.</param> |
| 22 | + /// <param name="address">The I2C address of the device.</param> |
| 23 | + public IesShieldGps(string i2cBus, int address = GPAM_DEFAULT_ADDDRESS) |
| 24 | + { |
| 25 | + // store I2C address |
| 26 | + _address = address; |
| 27 | + |
| 28 | + // instantiate I2C controller |
| 29 | + _gpsController = I2cDevice.FromId(i2cBus, new I2cConnectionSettings(address) |
| 30 | + { |
| 31 | + BusSpeed = I2cBusSpeed.FastMode, |
| 32 | + SharingMode = I2cSharingMode.Shared |
| 33 | + }); |
| 34 | + } |
| 35 | + |
| 36 | + /// <summary> |
| 37 | + /// Gets the current pitch of the device |
| 38 | + /// </summary> |
| 39 | + /// <returns>the pitch</returns> |
| 40 | + public int GetPitch() |
| 41 | + { |
| 42 | + int pitch = 0; |
| 43 | + byte data = (byte)GetSingleRegister(63); // Read Pitch registers from GPM |
| 44 | + if ((data & 0x80) != 0) |
| 45 | + { // If negative : |
| 46 | + |
| 47 | + data &= 0x7F; |
| 48 | + pitch = (int)data; |
| 49 | + pitch *= -1; // Convert to minus value |
| 50 | + } |
| 51 | + else |
| 52 | + { |
| 53 | + pitch = (int)data; |
| 54 | + } |
| 55 | + |
| 56 | + return pitch; |
| 57 | + } |
| 58 | + |
| 59 | + /// <summary> |
| 60 | + /// Gets the current roll of the device |
| 61 | + /// </summary> |
| 62 | + /// <returns>the roll</returns> |
| 63 | + public int GetRoll() |
| 64 | + { |
| 65 | + int roll = 0; |
| 66 | + byte data = (byte)GetSingleRegister(64); // Read Roll registers from GPM |
| 67 | + if ((data & 0x80) != 0) |
| 68 | + { // If negative : |
| 69 | + data &= 0x7F; |
| 70 | + roll = (int)data; |
| 71 | + roll *= -1; // convert to minus value |
| 72 | + } |
| 73 | + else |
| 74 | + { |
| 75 | + roll = (int)data; |
| 76 | + } |
| 77 | + |
| 78 | + return roll; |
| 79 | + } |
| 80 | + |
| 81 | + /// <summary> |
| 82 | + /// Gets the current heading of the device |
| 83 | + /// </summary> |
| 84 | + /// <returns>the heading in degrees</returns> |
| 85 | + public float GetHeading() |
| 86 | + { |
| 87 | + var heading = (float)GetSingleRegister(44) * 100; |
| 88 | + heading += (float)GetSingleRegister(45) * 10; |
| 89 | + heading += (float)GetSingleRegister(46); |
| 90 | + heading += (float)GetSingleRegister(47) / 10; |
| 91 | + return heading; |
| 92 | + } |
| 93 | + |
| 94 | + /// <summary> |
| 95 | + /// Gets the current speed from the device |
| 96 | + /// </summary> |
| 97 | + /// <returns>the speed in metres per second</returns> |
| 98 | + public float GetSpeed() |
| 99 | + { |
| 100 | + var speed = (float)GetSingleRegister(52) * 100; |
| 101 | + speed += (float)GetSingleRegister(53) * 10; |
| 102 | + speed += (float)GetSingleRegister(54); |
| 103 | + speed += (float)GetSingleRegister(55) / 10; |
| 104 | + return speed; |
| 105 | + } |
| 106 | + |
| 107 | + /// <summary> |
| 108 | + /// Gets the current longitude from the device |
| 109 | + /// </summary> |
| 110 | + /// <returns>the longitude in degrees</returns> |
| 111 | + public float GetLongitude() |
| 112 | + { |
| 113 | + var longitude_degrees = GetSingleRegister(23) * 100; |
| 114 | + longitude_degrees += GetDoubleRegister(24); |
| 115 | + float longitude_minutes = GetDoubleRegister(26); |
| 116 | + float longitude_seconds = (float)GetSingleRegister(28) / 10; |
| 117 | + longitude_seconds += (float)GetSingleRegister(29) / 100; |
| 118 | + longitude_seconds += (float)GetSingleRegister(30) / 1000; |
| 119 | + longitude_seconds += (float)GetSingleRegister(31) / 10000; |
| 120 | + var longitude_direction = (char)GetSingleRegister(32); |
| 121 | + |
| 122 | + return ConvertDegreeAngleToFloat(longitude_degrees, longitude_minutes, longitude_seconds, longitude_direction); |
| 123 | + } |
| 124 | + |
| 125 | + /// <summary> |
| 126 | + /// Gets the current latitude from the device |
| 127 | + /// </summary> |
| 128 | + /// <returns>the latitude in degrees</returns> |
| 129 | + public float GetLatitude() |
| 130 | + { |
| 131 | + |
| 132 | + var latitude_degrees = GetDoubleRegister(14); |
| 133 | + float latitude_minutes = GetDoubleRegister(16); |
| 134 | + float latitude_seconds = (float)GetSingleRegister(18) / 10; |
| 135 | + latitude_seconds += (float)GetSingleRegister(19) / 100; |
| 136 | + latitude_seconds += (float)GetSingleRegister(20) / 1000; |
| 137 | + latitude_seconds += (float)GetSingleRegister(21) / 10000; |
| 138 | + var latitude_direction = (char)GetSingleRegister(22); |
| 139 | + |
| 140 | + return ConvertDegreeAngleToFloat(latitude_degrees, latitude_minutes, latitude_seconds, latitude_direction); |
| 141 | + } |
| 142 | + |
| 143 | + /// <summary> |
| 144 | + /// Gets the current Date and Time from the device |
| 145 | + /// </summary> |
| 146 | + /// <returns>Current DateTime</returns> |
| 147 | + public DateTime GetDateTime() |
| 148 | + { |
| 149 | + var y1 = GetDoubleRegister(10); //Read Year 1 registers from GPM and print to PC |
| 150 | + var y2 = GetDoubleRegister(12); //Read Year 2 registers from GPM and print to PC |
| 151 | + var mo = GetDoubleRegister(8); //Read Month registers from GPM and print to PC |
| 152 | + var d = GetDoubleRegister(6); //Read Day registers from GPM and print to PC |
| 153 | + |
| 154 | + var hh = GetDoubleRegister(0); //Read Hours registers from GPM and print to PC |
| 155 | + var mm = GetDoubleRegister(2); //Read Minutes registers from GPM and print to PC |
| 156 | + var ss = GetDoubleRegister(4); //Read Seconds registers from GPM and print to PC |
| 157 | + |
| 158 | + var yy = (y1 * 100) + y2; |
| 159 | + |
| 160 | + if (yy != 0 && mo != 0 && d != 0 && hh <= 24 && mm <= 59 && ss <= 59) //sanity check, otherwise the parse could fail |
| 161 | + { |
| 162 | + return new DateTime(yy, mo, d, hh, mm, ss); |
| 163 | + } |
| 164 | + else |
| 165 | + { |
| 166 | + return new DateTime(); |
| 167 | + } |
| 168 | + } |
| 169 | + |
| 170 | + /// <summary> |
| 171 | + /// Converts DMS to DD |
| 172 | + /// </summary> |
| 173 | + /// <param name="degrees"></param> |
| 174 | + /// <param name="minutes"></param> |
| 175 | + /// <param name="seconds"></param> |
| 176 | + /// <param name="direction"></param> |
| 177 | + /// <returns></returns> |
| 178 | + private float ConvertDegreeAngleToFloat(float degrees, float minutes, float seconds, char direction) |
| 179 | + { |
| 180 | + //Example: 17.21.18S |
| 181 | + |
| 182 | + var multiplier = ((direction == 'S') || (direction == 'W')) ? -1 : 1; //handle south and west |
| 183 | + |
| 184 | + return (degrees + (minutes / 60) + (seconds / 3600)) * multiplier; |
| 185 | + } |
| 186 | + |
| 187 | + /// <summary> |
| 188 | + /// Get double register value from GPAM |
| 189 | + /// </summary> |
| 190 | + /// <param name="register">The register to get</param> |
| 191 | + /// <returns></returns> |
| 192 | + private int GetDoubleRegister(byte register) |
| 193 | + { |
| 194 | + |
| 195 | + byte[] highByte = new byte[1]; |
| 196 | + byte[] lowByte = new byte[1]; |
| 197 | + |
| 198 | + _gpsController.Write(new byte[] { register }); |
| 199 | + _gpsController.Read(highByte); |
| 200 | + |
| 201 | + _gpsController.Write(new byte[] { register += 1 }); |
| 202 | + _gpsController.Read(lowByte); |
| 203 | + |
| 204 | + int value = (highByte[0] * 10) + lowByte[0]; |
| 205 | + return (value); |
| 206 | + } |
| 207 | + |
| 208 | + /// <summary> |
| 209 | + /// Get single register value from GPAM |
| 210 | + /// </summary> |
| 211 | + /// <param name="register">The register to get</param> |
| 212 | + /// <returns></returns> |
| 213 | + private int GetSingleRegister(byte register) |
| 214 | + { |
| 215 | + |
| 216 | + byte[] buffer = new byte[1]; |
| 217 | + _gpsController.Write(new byte[] { register }); |
| 218 | + _gpsController.Read(buffer); |
| 219 | + return (buffer[0]); |
| 220 | + } |
| 221 | + } |
| 222 | +} |
0 commit comments