Add GPS Drivers

.clang-format

@@ -2,3 +2,4 @@ BasedOnStyle:  Google
 AccessModifierOffset: -2
 ColumnLimit: 200
 IndentWidth: 4
+AllowShortEnumsOnASingleLine: false

gps_ubxm8_i2c/coordHelpers.cc

@@ -0,0 +1,39 @@
+#include "coordHelpers.h"
+
+#include <cmath>
+#define M_PI 3.14159265358979323
+
+// Constants for WGS84 ellipsoid
+const double WGS84_A = 6378137.0;                   // semi-major axis in meters
+const double WGS84_INV_FLATTENING = 298.257223563;  // inverse flattening
+
+// Convert degrees to radians
+double Deg2Rad(double deg) { return deg * M_PI / 180.0; }
+
+// Convert radians to degrees
+double Rad2Deg(double rad) { return rad * 180.0 / M_PI; }
+
+// Convert GPS coordinates (latitude, longitude, altitude) to ECEF coordinates
+void Gps2Ecef(double lat, double lon, double alt, double& x, double& y, double& z) {
+    double cos_lat = cos(Deg2Rad(lat));
+    double sin_lat = sin(Deg2Rad(lat));
+    double cos_lon = cos(Deg2Rad(lon));
+    double sin_lon = sin(Deg2Rad(lon));
+
+    double N = WGS84_A / sqrt(1 - pow((1 - 1 / WGS84_INV_FLATTENING) * sin_lat, 2));
+
+    x = (N + alt) * cos_lat * cos_lon;
+    y = (N + alt) * cos_lat * sin_lon;
+    z = (N * (1 - 1 / WGS84_INV_FLATTENING) + alt) * sin_lat;
+}
+
+// Convert NED (North-East-Down) velocity to ECEF velocity
+void Ned2EcefVel(double lat, double lon, double alt, double vn, double ve, double vd, double& vx, double& vy, double& vz) {
+    double x, y, z;
+    Gps2Ecef(lat, lon, alt, x, y, z);
+
+    // Convert NED velocity to ECEF velocity
+    vx = -ve * sin(Deg2Rad(lon)) - vn * sin(Deg2Rad(lat)) * cos(Deg2Rad(lon)) + vd * cos(Deg2Rad(lat)) * cos(Deg2Rad(lon));
+    vy = ve * cos(Deg2Rad(lon)) - vn * sin(Deg2Rad(lat)) * sin(Deg2Rad(lon)) + vd * cos(Deg2Rad(lat)) * sin(Deg2Rad(lon));
+    vz = vn * cos(Deg2Rad(lat)) + vd * sin(Deg2Rad(lat));
+}

gps_ubxm8_i2c/coordHelpers.h

@@ -0,0 +1,11 @@
+#pragma once
+// Convert degrees to radians
+double Deg2Rad(double deg);
+
+// Convert radians to degrees
+double Rad2Deg(double rad);
+// Convert GPS coordinates (latitude, longitude, altitude) to ECEF coordinates
+void Gps2Ecef(double lat, double lon, double alt, double& x, double& y, double& z);
+
+// Convert NED (North-East-Down) velocity to ECEF velocity
+void Ned2EcefVel(double lat, double lon, double alt, double vn, double ve, double vd, double& vx, double& vy, double& vz);

gps_ubxm8_i2c/gps.cc

@@ -0,0 +1,186 @@
+#include "gps.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "coordHelpers.h"
+#include "ubxMessages.h"
+#include "ubxPacket.h"
+
+#define I2C_BUFFER_SIZE 1024
+#define GPS_I2C_TIMEOUT 100
+
+/**
+ * @param ubxMessage one of the messages from `ubxMessages.h` to send.
+ */
+GpsUbxM8I2c::GpsUbxM8I2c(GPIO_TypeDef* gpioResetPort, uint16_t gpioResetPin, I2C_HandleTypeDef* i2c, uint8_t* ubxMessage) {
+    _packetReader = UBXPacketReader();
+    _state = GpsUbxM8I2c::State::REQUEST_NOT_SENT;
+    _gpioResetPin = gpioResetPin;
+    _gpioResetPort = gpioResetPort;
+    _i2c = i2c;
+    _ubxMessage = ubxMessage;
+}
+
+void GpsUbxM8I2c::Init() { HAL_GPIO_WritePin(_gpioResetPort, _gpioResetPin, GPIO_PIN_SET); }
+
+const GpsUbxM8I2c::State GpsUbxM8I2c::GetState() { return _state; }
+
+/**
+ * @brief Sends a request for position data if none are currently pending, checks data available,
+ * and returns a status indicator.
+ *
+ * @return a GpsUbxM8I2c::PollResult object. See the definition of that enum for details.
+ */
+const GpsUbxM8I2c::PollResult GpsUbxM8I2c::PollUpdate() {
+    if (_state == GpsUbxM8I2c::State::REQUEST_NOT_SENT) {
+        sendUBX(_ubxMessage);
+        _state = GpsUbxM8I2c::State::POLLING_RESPONSE;
+    }
+
+    if (_state == GpsUbxM8I2c::State::POLLING_RESPONSE) {
+        uint8_t lenBytes[2];
+        uint16_t dataLen = 0;
+        uint8_t buffer[I2C_BUFFER_SIZE];
+
+        HAL_StatusTypeDef dataLenReadStatus = HAL_I2C_Mem_Read(_i2c, 0x42 << 1, 0xFD, 1, lenBytes, 2, GPS_I2C_TIMEOUT);
+        if (dataLenReadStatus != HAL_OK) {
+            return GpsUbxM8I2c::PollResult::DATA_LEN_POLL_FAILED;
+        }
+
+        dataLen = lenBytes[0] << 8 | lenBytes[1];
+        if (dataLen == 0) {
+            return GpsUbxM8I2c::PollResult::NO_DATA;
+        }
+
+        if (dataLen > I2C_BUFFER_SIZE) {
+            dataLen = I2C_BUFFER_SIZE;
+        }
+        HAL_StatusTypeDef rcvStatus = HAL_I2C_Master_Receive(_i2c, 0x42 << 1, buffer, dataLen, GPS_I2C_TIMEOUT);
+        if (rcvStatus != HAL_OK) {
+            return GpsUbxM8I2c::PollResult::DATA_RECEIVE_I2C_FAILED;
+        }
+
+        if (_packetReader.isInProgress()) {
+            for (uint16_t i = 0; i < dataLen; i++) {
+                UBXPacketUpdateResult res = _packetReader.update(buffer[i]);
+                if (res == UBXPacketUpdateResult::CHECKSUM_FAILED) {
+                    _packetReader.reset();
+                    return GpsUbxM8I2c::PollResult::DATA_RECEIVE_CHECKSUM_FAILED;
+                }
+                if (_packetReader.isComplete()) {
+                    _state = GpsUbxM8I2c::State::RESPONSE_READY;
+                    return GpsUbxM8I2c::PollResult::POLL_JUST_FINISHED;
+                }
+            }
+            return GpsUbxM8I2c::PollResult::RECEIVE_IN_PROGRESS;
+        } else {
+            for (uint16_t i = 0; i < dataLen; i++) {
+                if (buffer[i] == 0xB5 && buffer[i + 1] == 0x62) {
+                    i += 2;  // skip the header
+                    while (i < dataLen && !_packetReader.isComplete()) {
+                        UBXPacketUpdateResult res = _packetReader.update(buffer[i]);
+                        if (res != UBXPacketUpdateResult::UPDATE_OK) {
+                            _packetReader.reset();
+                            return GpsUbxM8I2c::PollResult::DATA_RECEIVE_CHECKSUM_FAILED;
+                        }
+                        i++;
+                    }
+                }
+            }
+
+            if (!_packetReader.isInProgress()) {
+                return GpsUbxM8I2c::PollResult::NO_UBX_DATA;
+            }
+
+            if (_packetReader.isComplete()) {
+                _state = GpsUbxM8I2c::State::RESPONSE_READY;
+                return GpsUbxM8I2c::PollResult::POLL_JUST_FINISHED;
+            }
+            return GpsUbxM8I2c::PollResult::RECEIVE_IN_PROGRESS;
+        }
+    }
+    return GpsUbxM8I2c::PollResult::POLL_ALREADY_FINISHED;
+}
+
+/**
+ * @brief Returns GPS position solution info. This will only give valid data
+ * if it's called after `pollUpdate` returns a POLL_JUST_FINISHED response,
+ * which will put the GPS's state in RESPONSE_READY mode. If you call this before that,
+ * the data in the struct is undefined.
+ *
+ * After calling this and using the info returned, getting the next
+ * packet requires you to call `reset` and `pollUpdate` again.
+ *
+ * @retval pointer to payload. Can be casted to a struct in ubxMessages.
+ */
+const void* GpsUbxM8I2c::GetSolution() { return _packetReader.getPayload(); }
+
+/**
+ * @brief Puts the GPS state back to its initial value so that `pollUpdate` knows
+ * it needs to send a new data request.
+ */
+void GpsUbxM8I2c::Reset() {
+    _state = GpsUbxM8I2c::State::REQUEST_NOT_SENT;
+    _packetReader.reset();
+}
+
+UCIRP_GPS_PAYLOAD ConvertPayloadToECEF(UBX_NAV_PVT_PAYLOAD pvtPayload) {
+    UCIRP_GPS_PAYLOAD payload;
+    payload.gpsFix = pvtPayload.fixType;
+    payload.positionAcc = std::max((double)pvtPayload.hAcc / 1000, (double)pvtPayload.vAcc / 1000);
+    payload.speedAcc = (double)pvtPayload.sAcc / 1000;
+
+    double lat = (double)pvtPayload.lat / 1e7;
+    double lon = (double)pvtPayload.lon / 1e7;
+    double alt = (double)pvtPayload.height / 1000;
+
+    Gps2Ecef(lat, lon, alt, payload.ecefX, payload.ecefY, payload.ecefZ);
+
+    // convert velocity to ECEF about the lat and lon
+    double velN = (double)pvtPayload.velN / 1000;
+    double velE = (double)pvtPayload.velE / 1000;
+    double velD = (double)pvtPayload.velD / 1000;
+
+    Ned2EcefVel(lat, lon, alt, velN, velE, velD, payload.ecefVX, payload.ecefVY, payload.ecefVZ);
+}
+
+/**
+ * @brief Sends a UBX protocol message over i2c
+ *
+ * @param message Array containing
+ *                {messageClass, messageID, payloadLenHighBits, payloadLenLowBits},
+ *                and if the payload length >0, all the payload bytes. Note: you don't need to
+ *                include the UBX start sequence in this or the checksums.
+ *
+ * @param len Length of `message`
+ * @param i2c The HAL i2c handle to use for transmission.
+ */
+bool GpsUbxM8I2c::sendUBX(uint8_t* message) {
+    static uint8_t magicBytes[2] = {0xB5, 0x62};
+    uint16_t len = 4 + ((uint16_t)message[2] << 8) + message[3];
+
+    uint8_t CK_A{0}, CK_B{0};
+
+    HAL_StatusTypeDef status;
+    status = HAL_I2C_Master_Transmit(_i2c, 0x42 << 1, magicBytes, 2, GPS_I2C_TIMEOUT);
+    if (status != HAL_OK) {
+        return false;
+    }
+
+    for (uint16_t i = 0; i < len; i++) {
+        CK_A = CK_A + message[i];
+        CK_B = CK_B + CK_A;
+    }
+
+    uint8_t CK[2] = {CK_A, CK_B};
+    status = HAL_I2C_Master_Transmit(_i2c, 0x42 << 1, message, len, GPS_I2C_TIMEOUT);
+    if (status != HAL_OK) {
+        return false;
+    }
+    status = HAL_I2C_Master_Transmit(_i2c, 0x42 << 1, CK, 2, GPS_I2C_TIMEOUT);
+    if (status != HAL_OK) {
+        return false;
+    }
+    return true;
+}

gps_ubxm8_i2c/gps.h

@@ -0,0 +1,95 @@
+#pragma once
+
+// if this is shitting itself, just locally replace it with whatever
+// header on your machine that can get you `I2C_HandleTypeDef`
+#if defined(STM32F1)
+#include "stm32f1xx_hal.h"
+#elif defined(STM32F4xx)
+#include "stm32f4xx_hal.h"
+#endif
+
+#include "ubxMessages.h"
+#include "ubxPacket.h"
+
+/**
+ * Class for interfacing over i2c with any M8 GPS.
+ * Before this class is used, the GPS reset pin needs to be pulled high.
+ *
+ * This class keeps its own finite state machine for abstracting away polling data availability and retrying requests.
+ * An example usage is as follows:
+ * ```
+  int main(void)
+  {
+      HAL_Init();
+
+      SystemClock_Config();
+
+      MX_GPIO_Init();
+
+      MX_I2C1_Init();
+
+      GPS gps(GPS_RST_GPIO_Port, GPS_RST_Pin, &hi2c1, PVT_MESSAGE);
+      gps.Init();
+      HAL_Delay(1000);
+
+      int lastITOW = 0;
+      while(1) {
+          volatile GPS::PollResult res = gps.PollUpdate();    
+          auto state = gps.GetState();
+          if(state == GPS::State::RESPONSE_READY) {
+              UBX_NAV_PVT_PAYLOAD sol = *(UBX_NAV_PVT_PAYLOAD*)gps.GetSolution();            
+              volatile int diff = sol.iTOW - lastITOW;
+              lastITOW = sol.iTOW;
+              if((GPSFixType)sol.fixType == GPSFixType::FIX_3D) {
+                  HAL_Delay(1000);
+              }
+              gps.Reset();
+          } else {
+              HAL_Delay(100);
+          }
+      }
+  }
+  ```
+ *
+ * After a packet is received, you _need_ to call the reset method.
+ * If you don't, `pollUpdate` will just do nothing and return a `POLL_ALREADY_FINISHED` response.
+ *
+ * The datasheet is here:
+ * https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf
+ * The most relevant parts are "UBX Protocol" (section 32) and "DDC Port" (section 15).
+ *
+ */
+class GpsUbxM8I2c {
+  public:
+    enum class State {
+        REQUEST_NOT_SENT,
+        POLLING_RESPONSE,
+        RESPONSE_READY
+    };
+    enum class PollResult {
+        POLL_JUST_FINISHED,           // the ubx data has been received on this invocation, and is now ready to be retrieved
+        POLL_ALREADY_FINISHED,        // the ubx data was already ready after a previous PollUpdate call
+        RECEIVE_IN_PROGRESS,          // we found the UBX start bytes in the incoming stream but haven't read the entire message
+        NO_DATA,                      // no data from GPS available
+        NO_UBX_DATA,                  // data read successfully but no UBX start bytes found
+        DATA_LEN_POLL_FAILED,         // there was a failure in the I2C mem read for getting the quantity of data available
+        DATA_RECEIVE_I2C_FAILED,      // there was a failure in the I2C receive to read the available data from the GPS
+        DATA_RECEIVE_CHECKSUM_FAILED  // we received UBX data but the checksum didn't match the message.
+    };
+    GpsUbxM8I2c(GPIO_TypeDef* gpioResetPort, uint16_t gpioResetPin, I2C_HandleTypeDef* i2c, uint8_t* ubxMessage);
+    void Init();
+    const State GetState();
+    const PollResult PollUpdate();
+    const void* GetSolution();
+    void Reset();
+    UCIRP_GPS_PAYLOAD ConvertPayloadToECEF(UBX_NAV_PVT_PAYLOAD pvtPayload);
+
+  private:
+    UBXPacketReader _packetReader;
+    State _state;
+    GPIO_TypeDef* _gpioResetPort;
+    uint16_t _gpioResetPin;
+    I2C_HandleTypeDef* _i2c;
+    uint8_t* _ubxMessage;
+    bool sendUBX(uint8_t* message);
+};