diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index d5207a4..0ee5df1 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -35,6 +35,9 @@ set(FALCON_PROTOS_DIR ${CMAKE_CURRENT_LIST_DIR}/../../modules/falcon-protos) zephyr_nanopb_sources(app ${FALCON_PROTOS_DIR}/HelloWorldPacket.proto) zephyr_nanopb_sources(app ${FALCON_PROTOS_DIR}/TelemetryPacket.proto) +# LwGPS NMEA parser library +set(LWGPS_DIR ${CMAKE_CURRENT_LIST_DIR}/../../modules/lwgps) + target_sources(app PRIVATE src/main.c src/data.c @@ -51,11 +54,15 @@ target_sources(app PRIVATE src/state_machine/states/landed.c src/pyro/pyro_thread.c src/radio/radio_thread.c + src/gps/gps_thread.c + ${LWGPS_DIR}/lwgps/src/lwgps/lwgps.c ) target_include_directories(app PRIVATE src src/state_machine + src/gps + ${LWGPS_DIR}/lwgps/src/include ) if(DEFINED DATA_FILE) diff --git a/firmware/src/data.c b/firmware/src/data.c index 1baeb32..15b5162 100644 --- a/firmware/src/data.c +++ b/firmware/src/data.c @@ -9,12 +9,14 @@ struct imu_data g_imu_data; struct baro_data g_baro_data; struct state_data g_state_data; struct pyro_data g_pyro_data; +struct gps_data g_gps_data; // Mutexes for thread safety K_MUTEX_DEFINE(imu_mutex); K_MUTEX_DEFINE(baro_mutex); K_MUTEX_DEFINE(state_mutex); K_MUTEX_DEFINE(pyro_data_mutex); +K_MUTEX_DEFINE(gps_mutex); // Setter functions void set_imu_data(const struct imu_data *src) @@ -106,3 +108,17 @@ void get_pyro_data(struct pyro_data *dst) *dst = g_pyro_data; k_mutex_unlock(&pyro_data_mutex); } + +void set_gps_data(const struct gps_data *src) +{ + k_mutex_lock(&gps_mutex, K_FOREVER); + g_gps_data = *src; + k_mutex_unlock(&gps_mutex); +} + +void get_gps_data(struct gps_data *dst) +{ + k_mutex_lock(&gps_mutex, K_FOREVER); + *dst = g_gps_data; + k_mutex_unlock(&gps_mutex); +} diff --git a/firmware/src/data.h b/firmware/src/data.h index 0531c3a..b46e81d 100644 --- a/firmware/src/data.h +++ b/firmware/src/data.h @@ -66,11 +66,22 @@ struct pyro_data { bool main_fire_requested; }; +struct gps_data { + float latitude; // Degrees + float longitude; // Degrees + float altitude; // Altitude in meters + float speed; // Speed in knots + uint8_t sats; // Satellites in use + uint8_t fix; // Fix quality + int64_t timestamp; +}; + // Global instances extern struct imu_data g_imu_data; extern struct baro_data g_baro_data; extern struct state_data g_state_data; extern struct pyro_data g_pyro_data; +extern struct gps_data g_gps_data; // Getters and setters void set_imu_data(const struct imu_data *src); @@ -85,4 +96,7 @@ void get_state_data(struct state_data *dst); void set_pyro_data(const struct pyro_data *src); void get_pyro_data(struct pyro_data *dst); +void set_gps_data(const struct gps_data *src); +void get_gps_data(struct gps_data *dst); + #endif diff --git a/firmware/src/gps/gps_thread.c b/firmware/src/gps/gps_thread.c new file mode 100644 index 0000000..6c7a588 --- /dev/null +++ b/firmware/src/gps/gps_thread.c @@ -0,0 +1,144 @@ +#include +#include +#include +#include +#include +#include +#include +#include "data.h" + +LOG_MODULE_REGISTER(gps_thread, LOG_LEVEL_INF); + +#define GPS_THREAD_STACK_SIZE 2048 +#define GPS_THREAD_PRIORITY 6 +#define GPS_THREAD_PERIOD_MS 1000 + +/* SPI protocol constants (ulysses-gnss-radio spec) */ +#define SPI_CMD_GPS_RX 0x05 +#define SPI_DUMMY_SIZE 4 +#define SPI_HEADER_SIZE (1 + SPI_DUMMY_SIZE) /* 5: cmd + dummy bytes */ +#define GPS_PAYLOAD_SIZE 87 /* Max NMEA sentence length */ + +/* Total SPI transaction: [CMD:1][DUMMY:4][PAYLOAD:87] = 92 bytes */ +#define SPI_GPS_RX_SIZE (SPI_HEADER_SIZE + GPS_PAYLOAD_SIZE) /* 92 */ + +K_THREAD_STACK_DEFINE(gps_stack, GPS_THREAD_STACK_SIZE); +static struct k_thread gps_thread; + +static const struct spi_dt_spec gps_spi = + SPI_DT_SPEC_GET(DT_ALIAS(radio0), + SPI_OP_MODE_MASTER | SPI_TRANSFER_MSB | SPI_WORD_SET(8)); + +static lwgps_t gps; + +static int gps_spi_read(uint8_t *payload_out) +{ + uint8_t tx_buf[SPI_GPS_RX_SIZE] = {0}; + uint8_t rx_buf[SPI_GPS_RX_SIZE] = {0}; + + tx_buf[0] = SPI_CMD_GPS_RX; + /* bytes 1-4 are dummy, rest is zero (master clocks out zeros) */ + + const struct spi_buf spi_tx = { + .buf = tx_buf, + .len = sizeof(tx_buf), + }; + const struct spi_buf_set tx_set = { + .buffers = &spi_tx, + .count = 1, + }; + + const struct spi_buf spi_rx = { + .buf = rx_buf, + .len = sizeof(rx_buf), + }; + const struct spi_buf_set rx_set = { + .buffers = &spi_rx, + .count = 1, + }; + + int ret = spi_transceive_dt(&gps_spi, &tx_set, &rx_set); + if (ret < 0) { + return ret; + } + + /* Payload starts after header (cmd + dummy bytes) */ + memcpy(payload_out, &rx_buf[SPI_HEADER_SIZE], GPS_PAYLOAD_SIZE); + + return 0; +} + +static void gps_thread_fn(void *p1, void *p2, void *p3) +{ + if (!spi_is_ready_dt(&gps_spi)) { + LOG_ERR("GPS SPI device not ready"); + return; + } + LOG_INF("GPS SPI device ready"); + + lwgps_init(&gps); + + while (1) { + uint8_t payload[GPS_PAYLOAD_SIZE]; + + int ret = gps_spi_read(payload); + if (ret < 0) { + LOG_ERR("GPS SPI read failed: %d", ret); + k_sleep(K_MSEC(GPS_THREAD_PERIOD_MS)); + continue; + } + + /* Check for empty payload (all zero bytes from SPI) */ + bool payload_empty = true; + for (size_t i = 0; i < GPS_PAYLOAD_SIZE; i++) { + if (payload[i] != 0) { + payload_empty = false; + break; + } + } + if (payload_empty) { + LOG_WRN("GPS SPI payload is empty (all zeros)"); + k_sleep(K_MSEC(GPS_THREAD_PERIOD_MS)); + continue; + } + + /* Null-terminate and find actual sentence length */ + char nmea[GPS_PAYLOAD_SIZE + 1]; + memcpy(nmea, payload, GPS_PAYLOAD_SIZE); + nmea[GPS_PAYLOAD_SIZE] = '\0'; + size_t len = strlen(nmea); + + if (len == 0 || nmea[0] != '$') { + k_sleep(K_MSEC(GPS_THREAD_PERIOD_MS)); + continue; + } + + lwgps_process(&gps, nmea, len); + + struct gps_data gps_out = { + .latitude = gps.latitude, + .longitude = gps.longitude, + .altitude = gps.altitude, + .speed = gps.speed, + .sats = gps.sats_in_use, + .fix = gps.fix, + .timestamp = k_uptime_get(), + }; + set_gps_data(&gps_out); + + LOG_INF("NMEA: %s", nmea); + LOG_INF("GPS: lat=%.6f, lon=%.6f, alt=%.1f m, " + "sats=%u, fix=%u, speed=%.1f kn", + (double)gps.latitude, (double)gps.longitude, + (double)gps.altitude, gps.sats_in_use, gps.fix, + (double)gps.speed); + + k_sleep(K_MSEC(GPS_THREAD_PERIOD_MS)); + } +} + +void start_gps_thread(void) +{ + k_thread_create(&gps_thread, gps_stack, K_THREAD_STACK_SIZEOF(gps_stack), + gps_thread_fn, NULL, NULL, NULL, GPS_THREAD_PRIORITY, 0, K_NO_WAIT); +} diff --git a/firmware/src/gps/gps_thread.h b/firmware/src/gps/gps_thread.h new file mode 100644 index 0000000..4cf5cbe --- /dev/null +++ b/firmware/src/gps/gps_thread.h @@ -0,0 +1,6 @@ +#ifndef GPS_THREAD_H +#define GPS_THREAD_H + +void start_gps_thread(void); + +#endif diff --git a/firmware/src/gps/lwgps_opts.h b/firmware/src/gps/lwgps_opts.h new file mode 100644 index 0000000..75253c4 --- /dev/null +++ b/firmware/src/gps/lwgps_opts.h @@ -0,0 +1,10 @@ +#ifndef LWGPS_OPTS_H +#define LWGPS_OPTS_H + +/* Use float instead of double to save memory on Cortex-M4 */ +#define LWGPS_CFG_DOUBLE 0 + +/* We don't need satellite detail descriptors */ +#define LWGPS_CFG_STATEMENT_GPGSV_SAT_DET 0 + +#endif diff --git a/firmware/src/log_format.h b/firmware/src/log_format.h index b531ae2..9c8e959 100644 --- a/firmware/src/log_format.h +++ b/firmware/src/log_format.h @@ -11,6 +11,7 @@ struct log_frame { struct baro_data baro; struct state_data state; struct pyro_data pyro; + struct gps_data gps; }; #endif diff --git a/firmware/src/logger_thread.c b/firmware/src/logger_thread.c index 8d30500..64fc4a0 100644 --- a/firmware/src/logger_thread.c +++ b/firmware/src/logger_thread.c @@ -94,9 +94,10 @@ static int write_csv_header(void) "Pyro_Status,Pyro_Timestamp(ms)," "Drogue_Fired,Main_Fired,Drogue_Fail,Main_Fail," "Drogue_Cont_OK,Main_Cont_OK,Drogue_Fire_ACK,Main_Fire_ACK," - "Drogue_Fire_Requested,Main_Fire_Requested\n"; - -#ifdef CONFIG_BOARD_NATIVE_SIM + "Drogue_Fire_Requested,Main_Fire_Requested," + "GPS_Timestamp(ms),GPS_Lat(deg),GPS_Lon(deg)," + "GPS_Alt(m),GPS_Speed(kn),GPS_Sats,GPS_Fix\n"; + #ifdef CONFIG_BOARD_NATIVE_SIM size_t written = fwrite(header, 1, strlen(header), log_file_ptr); if (written != strlen(header)) { LOG_ERR("Failed to write header"); @@ -193,11 +194,12 @@ static int format_log_entry(const struct log_frame *frame, char *buffer, size_t { return snprintf( buffer, buffer_size, - "%lld,%lld,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%lld," - "%.3f,%.3f,%.3f,%.3f,%u,%d," - "%.3f,%.3f,%.3f,%.3f,%u,%d," - "%.3f,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%lld," - "%u,%lld,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", + "%lld,%lld,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%lld," // Log_Timestamp, IMU_Timestamp, Accel_X/Y/Z, Gyro_X/Y/Z, Baro_Timestamp + "%.3f,%.3f,%.3f,%.3f,%u,%d," // Baro0_Pressure, Baro0_Temperature, Baro0_Altitude, Baro0_NIS, Baro0_Faults, Baro0_Healthy + "%.3f,%.3f,%.3f,%.3f,%u,%d," // Baro1_Pressure, Baro1_Temperature, Baro1_Altitude, Baro1_NIS, Baro1_Faults, Baro1_Healthy + "%.3f,%.3f,%.3f,%.3f,%.3f,%d,%.3f,%lld," // KF_Altitude, KF_Altitude_AGL, KF_AltVar, KF_Velocity, KF_VelVar, State, State_Ground_Altitude, State_Timestamp + "%u,%lld,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d," // Pyro_Status, Pyro_Timestamp, Drogue_Fired, Main_Fired, Drogue_Fail, Main_Fail, Drogue_Cont_OK, Main_Cont_OK, Drogue_Fire_ACK, Main_Fire_ACK, Drogue_Fire_Requested, Main_Fire_Requested + "%lld,%.6f,%.6f,%.1f,%.1f,%u,%u\n", // GPS_Timestamp, GPS_Lat, GPS_Lon, GPS_Alt, GPS_Speed, GPS_Sats, GPS_Fix frame->log_timestamp, frame->imu.timestamp, (double)frame->imu.accel[0], (double)frame->imu.accel[1], (double)frame->imu.accel[2], @@ -218,7 +220,12 @@ static int format_log_entry(const struct log_frame *frame, char *buffer, size_t frame->pyro.drogue_fail ? 1 : 0, frame->pyro.main_fail ? 1 : 0, frame->pyro.drogue_cont_ok ? 1 : 0, frame->pyro.main_cont_ok ? 1 : 0, frame->pyro.drogue_fire_ack ? 1 : 0, frame->pyro.main_fire_ack ? 1 : 0, - frame->pyro.drogue_fire_requested ? 1 : 0, frame->pyro.main_fire_requested ? 1 : 0); + frame->pyro.drogue_fire_requested ? 1 : 0, frame->pyro.main_fire_requested ? 1 : 0, + frame->gps.timestamp, + (double)frame->gps.latitude, (double)frame->gps.longitude, + (double)frame->gps.altitude, (double)frame->gps.speed, + (unsigned int)frame->gps.sats, (unsigned int)frame->gps.fix + ); } static void write_log_frame_to_file(const struct log_frame *frame) @@ -267,6 +274,7 @@ static void logger_thread_fn(void *p1, void *p2, void *p3) get_baro_data(&frame.baro); get_state_data(&frame.state); get_pyro_data(&frame.pyro); + get_gps_data(&frame.gps); write_log_frame_to_file(&frame); diff --git a/firmware/src/main.c b/firmware/src/main.c index 31b12c1..fb861e7 100644 --- a/firmware/src/main.c +++ b/firmware/src/main.c @@ -9,6 +9,7 @@ #include "state_machine/state_machine.h" #include "pyro/pyro_thread.h" #include "radio/radio_thread.h" +#include "gps/gps_thread.h" #include "data.h" LOG_MODULE_REGISTER(falcon_main, LOG_LEVEL_INF); @@ -24,6 +25,7 @@ int main(void) start_pyro_thread(); start_state_machine_thread(); start_radio_thread(); + start_gps_thread(); return 0; } diff --git a/firmware/src/radio/radio_thread.c b/firmware/src/radio/radio_thread.c index e732658..11518e8 100644 --- a/firmware/src/radio/radio_thread.c +++ b/firmware/src/radio/radio_thread.c @@ -81,10 +81,12 @@ static void radio_thread_fn(void *p1, void *p2, void *p3) struct imu_data imu; struct baro_data baro; struct state_data state; + struct gps_data gps; get_imu_data(&imu); get_baro_data(&baro); get_state_data(&state); + get_gps_data(&gps); message.counter = counter; message.timestamp_ms = (uint32_t)k_uptime_get(); @@ -105,6 +107,13 @@ static void radio_thread_fn(void *p1, void *p2, void *p3) message.ground_altitude = state.ground_altitude; + message.gps_latitude = gps.latitude; + message.gps_longitude = gps.longitude; + message.gps_altitude = gps.altitude; + message.gps_speed = gps.speed; + message.gps_sats = gps.sats; + message.gps_fix = gps.fix; + pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); bool status = pb_encode(&stream, TelemetryPacket_fields, &message); size_t message_length = stream.bytes_written; diff --git a/west.yml b/west.yml index 2b6cdaf..98def29 100644 --- a/west.yml +++ b/west.yml @@ -4,6 +4,8 @@ manifest: url-base: https://github.com/zephyrproject-rtos - name: ubc-rocket url-base: https://github.com/UBC-Rocket + - name: lwgps-remote + url-base: https://github.com/MaJerle projects: - name: zephyr @@ -14,6 +16,10 @@ manifest: remote: ubc-rocket revision: main path: modules/falcon-protos + - name: lwgps + remote: lwgps-remote + revision: main + path: modules/lwgps self: path: app