Add GPS Drivers

.clang-format

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

gps_ubxm8_i2c/gps.cpp

@@ -0,0 +1,117 @@
+#include "gps.h"
+
+#include "ubxHelpers.h"
+#include "ubxMessages.h"
+#include "ubxPacket.h"
+
+#define I2C_BUFFER_SIZE 1024
+
+GPS::GPS() {
+    packetReader = UBXPacketReader();
+    state = GPS::State::REQUEST_NOT_SENT;
+}
+
+const GPS::State GPS::getState() { return state; }
+
+/**
+ * Sends a request for position data if none are currently pending.
+ * Then, checks if there is data available from the GPS. If there is none,
+ * it just returns a NO_DATA response. If there is data, it reads up to 1024 bytes (configurable at the top of gps.cpp)
+ * After reading, it will return one of these:
+ * - RECEIVE_IN_PROGRESS: it found a valid header for the data we're looking for (UBX packet) but hasn't read all of it yet.
+ * - NO_UBX_DATA: it successfully read data but there was no header for the data we're looking for (UBX packet)
+ * - POLL_JUST_FINISHED: it got data. Now you can read it with GPS::getSolution.
+ *
+ * There are also special return types for different types of error, that are hopefully verbose enough to explain themselves.
+ */
+const GPS::PollResult GPS::pollUpdate(I2C_HandleTypeDef* i2c) {
+    if (state == GPS::State::REQUEST_NOT_SENT) {
+        uint8_t message[4] = {0x01, 0x07, 0x00, 0x00};
+        sendUBX(message, 4, &hi2c3);
+        state = GPS::State::POLLING_RESPONSE;
+    }
+
+    if (state == GPS::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, 100);
+        if (dataLenReadStatus != HAL_OK) {
+            return GPS::PollResult::DATA_LEN_POLL_FAILED;
+        }
+
+        dataLen = lenBytes[0] << 8 | lenBytes[1];
+        if (dataLen == 0) {
+            return GPS::PollResult::NO_DATA;
+        }
+
+        if (dataLen > I2C_BUFFER_SIZE) {
+            dataLen = I2C_BUFFER_SIZE;
+        }
+        HAL_StatusTypeDef rcvStatus = HAL_I2C_Master_Receive(i2c, 0x42 << 1, buffer, dataLen, 100);
+        if (rcvStatus != HAL_OK) {
+            return GPS::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 GPS::PollResult::DATA_RECEIVE_CHECKSUM_FAILED;
+                }
+                if (packetReader.isComplete()) {
+                    state = GPS::State::RESPONSE_READY;
+                    return GPS::PollResult::POLL_JUST_FINISHED;
+                }
+            }
+            return GPS::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 GPS::PollResult::DATA_RECEIVE_CHECKSUM_FAILED;
+                        }
+                        i++;
+                    }
+                }
+            }
+
+            if (!packetReader.isInProgress()) {
+                return GPS::PollResult::NO_UBX_DATA;
+            }
+
+            if (packetReader.isComplete()) {
+                state = GPS::State::RESPONSE_READY;
+                return GPS::PollResult::POLL_JUST_FINISHED;
+            }
+            return GPS::PollResult::RECEIVE_IN_PROGRESS;
+        }
+    }
+    return GPS::PollResult::POLL_ALREADY_FINISHED;
+}
+
+/**
+ * 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.
+ */
+const UBX_NAV_PVT_PAYLOAD GPS::getSolution() { return *(UBX_NAV_PVT_PAYLOAD*)packetReader.getPayload(); }
+
+/**
+ * Puts the GPS state back to its initial value so that `pollUpdate` knows
+ * it needs to send a new data request.
+ */
+void GPS::reset() {
+    state = GPS::State::REQUEST_NOT_SENT;
+    packetReader.reset();
+}

gps_ubxm8_i2c/gps.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <i2c.h>
+
+#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:
+ * ```
+ *     GPS myGPS;
+ *     while(1) {
+ *        if(myGPS.pollUpdate(&i2c1) == GPS::PollResult::POLL_FINISHED) {
+ *            UBX_NAV_PVT_PAYLOAD pvtData = myGPS.getSolution();
+ *            myGPS.reset();
+ *        }
+ *     }
+ * ```
+ *
+ * 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 GPS {
+  public:
+    enum class State {
+        REQUEST_NOT_SENT,
+        POLLING_RESPONSE,
+        RESPONSE_READY
+    };
+    enum class PollResult {
+        POLL_JUST_FINISHED,
+        POLL_ALREADY_FINISHED,
+        RECEIVE_IN_PROGRESS,
+        NO_DATA,
+        NO_UBX_DATA,
+        DATA_LEN_POLL_FAILED,
+        DATA_RECEIVE_I2C_FAILED,
+        DATA_RECEIVE_CHECKSUM_FAILED
+    };
+    GPS();
+    const State getState();
+    const PollResult pollUpdate(I2C_HandleTypeDef* i2c);
+    const UBX_NAV_PVT_PAYLOAD getSolution();
+    void reset();
+
+  private:
+    UBXPacketReader packetReader;
+    State state;
+};

gps_ubxm8_i2c/ubxHelpers.cpp

@@ -0,0 +1,34 @@
+#include "ubxHelpers.h"
+
+#include "ubxMessages.h"
+#include "ubxPacket.h"
+
+#define I2C_BUFFER_SIZE 1024
+
+static uint8_t magicBytes[2] = {0xB5, 0x62};
+
+bool sendUBX(uint8_t* message, uint16_t len, I2C_HandleTypeDef* i2c) {
+    uint8_t CK_A{0}, CK_B{0};
+
+    HAL_StatusTypeDef status;
+    status = HAL_I2C_Master_Transmit(i2c, 0x42 << 1, magicBytes, 2, 100);
+    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, 100);
+    if (status != HAL_OK) {
+        return false;
+    }
+    status = HAL_I2C_Master_Transmit(i2c, 0x42 << 1, CK, 2, 100);
+    if (status != HAL_OK) {
+        return false;
+    }
+    return true;
+}

gps_ubxm8_i2c/ubxHelpers.h

@@ -0,0 +1,5 @@
+#pragma once
+#include "i2c.h"
+#include "ubxMessages.h"
+
+bool sendUBX(uint8_t* message, uint16_t len, I2C_HandleTypeDef* i2c);

gps_ubxm8_i2c/ubxMessages.h

@@ -0,0 +1,48 @@
+#pragma once
+#include <stdint.h>
+
+#pragma pack(push, 1)
+struct UBX_NAV_PVT_PAYLOAD {
+    uint32_t iTOW;
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t hour;
+    uint8_t min;
+    uint8_t sec;
+    uint8_t valid;
+    uint32_t tAcc;
+    int32_t nano;
+    GPSFixType fixType;
+    uint8_t flags;
+    uint8_t flags2;
+    uint8_t numSV;
+    int32_t lon;
+    int32_t lat;
+    int32_t height;
+    int32_t hMSL;
+    uint32_t hAcc;
+    uint32_t vAcc;
+    int32_t velN;
+    int32_t velE;
+    int32_t velD;
+    int32_t gSpeed;
+    int32_t headMot;
+    uint32_t sAcc;
+    uint32_t headAcc;
+    uint16_t pDOP;
+    uint8_t reserved1[6];
+    int32_t headVeh;
+    int16_t magDec;
+    uint16_t magAcc;
+};
+#pragma pack(pop)
+
+enum class GPSFixType : uint8_t {
+    NO_FIX,
+    DEAD_RECKONING,
+    FIX_2D,
+    FIX_3D,
+    GNSS_AND_DEAD_RECKONING,
+    TIME_ONLY_FIX
+};

gps_ubxm8_i2c/ubxPacket.cpp

@@ -0,0 +1,69 @@
+#include "ubxPacket.h"
+
+// make a ubxPacket class that has a method to update one byte at a time
+UBXPacketReader::UBXPacketReader() {
+    payloadLength = 0;
+    packetIndex = 0;
+    ckA = 0;
+    ckB = 0;
+    complete = false;
+    inProgress = false;
+}
+
+UBXPacketUpdateResult UBXPacketReader::update(uint8_t newByte) {
+    if (complete) {
+        return UBXPacketUpdateResult::PACKET_ALREADY_COMPLETE;
+    }
+    inProgress = true;
+    int currentPacketIndex = packetIndex;
+    packetIndex++;
+    if (currentPacketIndex == 0) {
+        messageClass = newByte;
+    } else if (currentPacketIndex == 1) {
+        messageId = newByte;
+    } else if (currentPacketIndex == 2) {
+        payloadLength = newByte;
+    } else if (currentPacketIndex == 3) {
+        payloadLength |= (uint16_t)newByte << 8;
+    } else if (currentPacketIndex >= 4 + payloadLength && currentPacketIndex < 6 + payloadLength) {
+        if (currentPacketIndex == 4 + payloadLength) {
+            if (ckA != newByte) {
+                return UBXPacketUpdateResult::CHECKSUM_FAILED;
+            }
+        } else {
+            if (ckB != newByte) {
+                return UBXPacketUpdateResult::CHECKSUM_FAILED;
+            } else {
+                complete = true;
+                inProgress = false;
+            }
+        }
+        return UBXPacketUpdateResult::UPDATE_OK;  // return early to avoid updating checksums
+    } else {
+        payload[currentPacketIndex - 4] = newByte;
+    }
+    ckA = ckA + newByte;
+    ckB = ckB + ckA;
+    return UBXPacketUpdateResult::UPDATE_OK;
+}
+
+void* UBXPacketReader::getPayload() { return (void*)payload; }
+
+uint8_t UBXPacketReader::getPayloadLength() { return payloadLength; }
+
+uint8_t UBXPacketReader::getMessageClass() { return messageClass; }
+
+uint8_t UBXPacketReader::getMessageId() { return messageId; }
+
+bool UBXPacketReader::isComplete() { return complete; }
+
+bool UBXPacketReader::isInProgress() { return inProgress; }
+
+void UBXPacketReader::reset() {
+    payloadLength = 0;
+    packetIndex = 0;
+    ckA = 0;
+    ckB = 0;
+    complete = false;
+    inProgress = false;
+}

gps_ubxm8_i2c/ubxPacket.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <stdint.h>
+
+enum class UBXPacketUpdateResult {
+    UPDATE_OK,
+    CHECKSUM_FAILED,
+    PACKET_ALREADY_COMPLETE
+};
+
+class UBXPacketReader {
+  public:
+    UBXPacketReader();
+    UBXPacketUpdateResult update(uint8_t byte);
+    bool isComplete();
+    bool isInProgress();
+    void* getPayload();
+    uint8_t getPayloadLength();
+    uint8_t getMessageClass();
+    uint8_t getMessageId();
+    void reset();
+
+  private:
+    uint8_t payload[512];
+    uint8_t payloadLength;
+    uint8_t packetIndex;
+    uint8_t messageClass;
+    uint8_t messageId;
+    uint8_t ckA;
+    uint8_t ckB;
+    bool complete;
+    bool inProgress;
+};