Skip to content
New issue

Have a question about this project? # for a free GitHub account to open an issue and contact its maintainers and the community.

#

By clicking “#”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? # to your account

GPS Time #162

Draft
wants to merge 2 commits into
base: master
Choose a base branch
from
Draft

GPS Time #162

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1c7567a
simplify gps parse + post date/time
irvingywang Mar 4, 2025
9af8d08
post milliseconds
irvingywang Mar 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions common/daq/can_config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -556,6 +556,21 @@
"msg_hlp":3,
"msg_pgn":141
},
{ "msg_name":"gps_time",
"msg_desc":"gps time in UTC",
"signals":[
{"sig_name":"year","type":"uint8_t","unit": "yr"},
{"sig_name":"month","type":"uint8_t","unit": "mo"},
{"sig_name":"day", "type":"uint8_t","unit": "d"},
{"sig_name":"hour", "type":"uint8_t","unit": "h"},
{"sig_name":"minute", "type":"uint8_t","unit": "m"},
{"sig_name":"second", "type":"uint8_t","unit": "s"},
{"sig_name":"millisecond", "type":"uint16_t","unit": "ms"}
],
"msg_period":40,
"msg_hlp":3,
"msg_pgn":149
},
{ "msg_name":"imu_gyro",
"msg_desc":"gyroscope from imu",
"signals":[
Expand Down
23 changes: 23 additions & 0 deletions source/torque_vector/can/can_parse.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#define ID_GPS_SPEED 0xc001137
#define ID_GPS_POSITION 0xc002337
#define ID_GPS_COORDINATES 0xc002377
#define ID_GPS_TIME 0xc002577
#define ID_IMU_GYRO 0xc0002f7
#define ID_IMU_ACCEL 0xc0023b7
#define ID_BMM_MAG 0xc0023f7
Expand Down Expand Up @@ -60,6 +61,7 @@
#define DLC_GPS_SPEED 4
#define DLC_GPS_POSITION 2
#define DLC_GPS_COORDINATES 8
#define DLC_GPS_TIME 8
#define DLC_IMU_GYRO 6
#define DLC_IMU_ACCEL 6
#define DLC_BMM_MAG 6
Expand Down Expand Up @@ -119,6 +121,18 @@
data_a->gps_coordinates.longitude = longitude_;\
canTxSendToBack(&msg);\
} while(0)
#define SEND_GPS_TIME(year_, month_, day_, hour_, minute_, second_, millisecond_) do {\
CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_GPS_TIME, .DLC=DLC_GPS_TIME, .IDE=1};\
CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\
data_a->gps_time.year = year_;\
data_a->gps_time.month = month_;\
data_a->gps_time.day = day_;\
data_a->gps_time.hour = hour_;\
data_a->gps_time.minute = minute_;\
data_a->gps_time.second = second_;\
data_a->gps_time.millisecond = millisecond_;\
canTxSendToBack(&msg);\
} while(0)
#define SEND_IMU_GYRO(imu_gyro_x_, imu_gyro_y_, imu_gyro_z_) do {\
CanMsgTypeDef_t msg = {.Bus=CAN1, .ExtId=ID_IMU_GYRO, .DLC=DLC_IMU_GYRO, .IDE=1};\
CanParsedData_t* data_a = (CanParsedData_t *) &msg.Data;\
Expand Down Expand Up @@ -258,6 +272,15 @@ typedef union {
uint64_t latitude: 32;
uint64_t longitude: 32;
} gps_coordinates;
struct {
uint64_t year: 8;
uint64_t month: 8;
uint64_t day: 8;
uint64_t hour: 8;
uint64_t minute: 8;
uint64_t second: 8;
uint64_t millisecond: 16;
} gps_time;
struct {
uint64_t imu_gyro_x: 16;
uint64_t imu_gyro_y: 16;
Expand Down
126 changes: 55 additions & 71 deletions source/torque_vector/gps/gps.c
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include "gps.h"
#include "can_parse.h"
//#include "sfs_pp.h"
//#include "SFS.h"

Expand Down Expand Up @@ -31,43 +33,35 @@ signed long prev_iTOW;
uint16_t counter;
uint16_t diff;
// Nav Message
GPS_Handle_t gps_handle = {.raw_message = {0},
.g_speed = 0,
.g_speed_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.g_speed_rounded = 0,
.longitude_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.longitude = 0,
.lon_rounded = 0,
.latitude_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.latitude = 0,
.lat_rounded = 0,
.height_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.height = 0,
.height_rounded = 0,
.n_vel_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.n_vel = 0,
.n_vel_rounded = 0,
.e_vel_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.e_vel = 0,
.e_vel_rounded = 0,
.d_vel_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.d_vel = 0,
.d_vel_rounded = 0,
.headVeh_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.headVeh = 0,
.headVeh_rounded = 0,
.mag_dec_bytes = {0xFF, 0xFF},
.mag_dec = 0,
.fix_type = -1,
.iTOW_bytes = {0xFF, 0xFF, 0xFF, 0xFF},
.iTOW = 0,
.unique_iTOW = true,
.messages_received = -1};
GPS_Handle_t gps_handle = {
.raw_message = {0},
.g_speed = 0,
.g_speed_rounded = 0,
.longitude = 0,
.lon_rounded = 0,
.latitude = 0,
.lat_rounded = 0,
.height = 0,
.height_rounded = 0,
.n_vel = 0,
.n_vel_rounded = 0,
.e_vel = 0,
.e_vel_rounded = 0,
.d_vel = 0,
.d_vel_rounded = 0,
.headVeh = 0,
.headVeh_rounded = 0,
.mag_dec = 0,
.fix_type = -1,
.iTOW = 0,
.unique_iTOW = true,
.messages_received = -1
};

// Parse velocity from raw message
bool parseVelocity(GPS_Handle_t *GPS)
{
// For future reference, we use the UBX protocol to communicate with GPS - Specifically UBX-NAV-PVT
// For future reference, we use the UBX protocol to communicate with GPS - Specifically UBX-NAV-PVT
// Validate the message header, class, and id
if (((GPS->raw_message)[0] == UBX_NAV_PVT_HEADER_B0) && (GPS->raw_message[1] == UBX_NAV_PVT_HEADER_B1) &&
((GPS->raw_message)[2] == UBX_NAV_PVT_CLASS) && ((GPS->raw_message)[3] == UBX_NAV_PVT_MSG_ID))
Expand All @@ -79,10 +73,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
{
correctFix = true;
// Collect Ground Speed
GPS->g_speed_bytes[0] = GPS->raw_message[66];
GPS->g_speed_bytes[1] = GPS->raw_message[67];
GPS->g_speed_bytes[2] = GPS->raw_message[68];
GPS->g_speed_bytes[3] = GPS->raw_message[69];
iLong.bytes[0] = GPS->raw_message[66];
iLong.bytes[1] = GPS->raw_message[67];
iLong.bytes[2] = GPS->raw_message[68];
Expand All @@ -91,10 +81,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->g_speed_rounded = (int16_t)(GPS->g_speed / 10);

// Collect Longitude
GPS->longitude_bytes[0] = GPS->raw_message[30];
GPS->longitude_bytes[1] = GPS->raw_message[31];
GPS->longitude_bytes[2] = GPS->raw_message[32];
GPS->longitude_bytes[3] = GPS->raw_message[33];
iLong.bytes[0] = GPS->raw_message[30];
iLong.bytes[1] = GPS->raw_message[31];
iLong.bytes[2] = GPS->raw_message[32];
Expand All @@ -103,10 +89,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->lon_rounded = (int32_t)(GPS->longitude);

// Colect Latitude
GPS->latitude_bytes[0] = GPS->raw_message[34];
GPS->latitude_bytes[1] = GPS->raw_message[35];
GPS->latitude_bytes[2] = GPS->raw_message[36];
GPS->latitude_bytes[3] = GPS->raw_message[37];
iLong.bytes[0] = GPS->raw_message[34];
iLong.bytes[1] = GPS->raw_message[35];
iLong.bytes[2] = GPS->raw_message[36];
Expand All @@ -115,10 +97,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->lat_rounded = (int32_t)(GPS->latitude);

// Collect Height above mean sea level
GPS->height_bytes[0] = GPS->raw_message[42];
GPS->height_bytes[1] = GPS->raw_message[43];
GPS->height_bytes[2] = GPS->raw_message[44];
GPS->height_bytes[3] = GPS->raw_message[45];
iLong.bytes[0] = GPS->raw_message[42];
iLong.bytes[1] = GPS->raw_message[43];
iLong.bytes[2] = GPS->raw_message[44];
Expand All @@ -127,10 +105,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->height_rounded = (int16_t)(GPS->height / 10);

// Collect North Velocity
GPS->n_vel_bytes[0] = GPS->raw_message[54];
GPS->n_vel_bytes[1] = GPS->raw_message[55];
GPS->n_vel_bytes[2] = GPS->raw_message[56];
GPS->n_vel_bytes[3] = GPS->raw_message[57];
iLong.bytes[0] = GPS->raw_message[54];
iLong.bytes[1] = GPS->raw_message[55];
iLong.bytes[2] = GPS->raw_message[56];
Expand All @@ -139,10 +113,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->n_vel_rounded = (int16_t)(GPS->n_vel / 10);

// Collect East Velocity
GPS->e_vel_bytes[0] = GPS->raw_message[58];
GPS->e_vel_bytes[1] = GPS->raw_message[59];
GPS->e_vel_bytes[2] = GPS->raw_message[60];
GPS->e_vel_bytes[3] = GPS->raw_message[61];
iLong.bytes[0] = GPS->raw_message[58];
iLong.bytes[1] = GPS->raw_message[59];
iLong.bytes[2] = GPS->raw_message[60];
Expand All @@ -151,10 +121,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->e_vel_rounded = (int16_t)(GPS->e_vel / 10);

// Collect Down Velocity
GPS->d_vel_bytes[0] = GPS->raw_message[62];
GPS->d_vel_bytes[1] = GPS->raw_message[63];
GPS->d_vel_bytes[2] = GPS->raw_message[64];
GPS->d_vel_bytes[3] = GPS->raw_message[65];
iLong.bytes[0] = GPS->raw_message[62];
iLong.bytes[1] = GPS->raw_message[63];
iLong.bytes[2] = GPS->raw_message[64];
Expand All @@ -163,10 +129,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->d_vel_rounded = (int16_t)(GPS->d_vel / 10);

// Collect Vehicle Heading of Motion
GPS->headVeh_bytes[0] = GPS->raw_message[70];
GPS->headVeh_bytes[1] = GPS->raw_message[71];
GPS->headVeh_bytes[2] = GPS->raw_message[72];
GPS->headVeh_bytes[3] = GPS->raw_message[73];
iLong.bytes[0] = GPS->raw_message[70];
iLong.bytes[1] = GPS->raw_message[71];
iLong.bytes[2] = GPS->raw_message[72];
Expand All @@ -175,16 +137,35 @@ bool parseVelocity(GPS_Handle_t *GPS)
GPS->headVeh_rounded = (int16_t)(GPS->headVeh / 10000); /* deg * 10^1 */

// Collect iTOW (Time of Week)
GPS->iTOW_bytes[0] = GPS->raw_message[6];
GPS->iTOW_bytes[1] = GPS->raw_message[7];
GPS->iTOW_bytes[2] = GPS->raw_message[8];
GPS->iTOW_bytes[3] = GPS->raw_message[9];
uLong.bytes[0] = GPS->raw_message[6];
uLong.bytes[1] = GPS->raw_message[7];
uLong.bytes[2] = GPS->raw_message[8];
uLong.bytes[3] = GPS->raw_message[9];
GPS->iTOW = uLong.uLong; /* ms is the raw data */


// Collect date and time (UTC)
iShort.bytes[0] = GPS->raw_message[10];
iShort.bytes[1] = GPS->raw_message[11];
GPS->year = (uint8_t)(iShort.iShort - 2000); // chop off 2000 from the year and store as uint8_t

GPS->month = GPS->raw_message[12];
GPS->day = GPS->raw_message[13];
GPS->hour = GPS->raw_message[14];
GPS->minute = GPS->raw_message[15];
GPS->second = GPS->raw_message[16];

iLong.bytes[0] = GPS->raw_message[18];
iLong.bytes[1] = GPS->raw_message[19];
iLong.bytes[2] = GPS->raw_message[20];
iLong.bytes[3] = GPS->raw_message[21];
GPS->millisecond = (int16_t)(iLong.iLong / 1000000); // convert nanoseconds to milliseconds

GPS->is_valid_date = (GPS->raw_message[17] & GPS_VALID_DATE) ? true : false;
GPS->is_valid_time = (GPS->raw_message[17] & GPS_VALID_TIME) ? true : false;
GPS->is_fully_resolved = (GPS->raw_message[17] & GPS_VALID_FULLY_RESOLVED) ? true : false;
GPS->is_valid_mag = (GPS->raw_message[17] & GPS_VALID_MAG) ? true : false;

/* Determine if data is new */
if (GPS->iTOW != prev_iTOW) {
GPS->unique_iTOW = true;
Expand All @@ -196,6 +177,11 @@ bool parseVelocity(GPS_Handle_t *GPS)
++counter;
}

// Only post onto CAN if GPS is fully resolved
if (GPS->is_fully_resolved) {
SEND_GPS_TIME(GPS->year, GPS->month, GPS->day, GPS->hour, GPS->minute, GPS->second, GPS->millisecond);
}

SEND_GPS_VELOCITY(GPS->n_vel_rounded, GPS->e_vel_rounded, GPS->d_vel_rounded);
SEND_GPS_SPEED(GPS->g_speed_rounded, GPS->headVeh_rounded);

Expand All @@ -204,8 +190,6 @@ bool parseVelocity(GPS_Handle_t *GPS)
}

// Collect Magnetic Declination
GPS->mag_dec_bytes[0] = GPS->raw_message[94];
GPS->mag_dec_bytes[1] = GPS->raw_message[95];
iShort.bytes[0] = GPS->raw_message[94];
iShort.bytes[1] = GPS->raw_message[95];
GPS->mag_dec = iShort.iShort; /* deg * 10^2 is the raw data */
Expand Down
28 changes: 18 additions & 10 deletions source/torque_vector/gps/gps.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,47 +42,49 @@ typedef struct
signed long messages_received;
uint8_t raw_message[100];

uint8_t g_speed_bytes[4];
signed long g_speed;
int16_t g_speed_rounded;

uint8_t longitude_bytes[4];
signed long longitude;
int32_t lon_rounded;

uint8_t latitude_bytes[4];
signed long latitude;
int32_t lat_rounded;

uint8_t height_bytes[4];
signed long height;
int16_t height_rounded;

uint8_t n_vel_bytes[4];
signed long n_vel;
int16_t n_vel_rounded;

uint8_t e_vel_bytes[4];
signed long e_vel;
int16_t e_vel_rounded;

uint8_t d_vel_bytes[4];
signed long d_vel;
int16_t d_vel_rounded;

uint8_t headVeh_bytes[4];
signed long headVeh;
int16_t headVeh_rounded;

uint8_t mag_dec_bytes[2];
signed short mag_dec;

uint8_t fix_type;

uint16_t iTOW_bytes[4];
unsigned long iTOW;
bool unique_iTOW;

uint8_t year; // Year (UTC) - (milenium truncated)
uint8_t month; // Month, range 1..12 (UTC)
uint8_t day; // Day of month, range 1..31 (UTC)
uint8_t hour; // Hour of day, range 0..23 (UTC)
uint8_t minute; // Minute of hour, range 0..59 (UTC)
uint8_t second; // Second of minute, range 0..60 (UTC)
int16_t millisecond; // Millisecond of second (calculated from nanoseconds)
bool is_valid_date;
bool is_valid_time;
bool is_fully_resolved;
bool is_valid_mag;

uint8_t gyro_OK;

} GPS_Handle_t; // GPS handle
Expand All @@ -100,6 +102,12 @@ typedef struct
#define GPS_FIX_GNSS_DEAD_RECKONING 4
#define GPS_FIX_TIME_ONLY 5


#define GPS_VALID_DATE 0x01 // Valid UTC Date
#define GPS_VALID_TIME 0x02 // Valid UTC Time
#define GPS_VALID_FULLY_RESOLVED 0x04 // UTC fully resolved
#define GPS_VALID_MAG 0x08 // Valid Magnetic Declination

/**
* @brief Function to Parse periodic GPS UBX message
*
Expand Down