yb_arm/driver/tfu/ublox/inc/ublox_drv.h
2025-05-20 01:20:32 +08:00

569 lines
18 KiB
C
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*------------------------------------------------------------
// Copyright (c) 2022 SmartLogic.
// ALL RIGHTS RESERVED
// -----------------------------------------------------------------
// Filename : stc_drv.c
// Author : weihua.li
// Created On : 2022-11-23
// Last Modified :
// -----------------------------------------------------------------
// Description:
//
//
// ----------------------------------------------------------------*/
#ifndef __UBLOX_DRV_H__
#define __UBLOX_DRV_H__
#define GTM_OK 0
#define GTM_ERROR -1
#define GTM_YES 1
#define GTM_NO 0
#define GTM_NULL_PTR 0
#define GTM_MOD_ADD(augend, addend, modulus) \
(((augend) + (addend)) % (modulus))
#define GTM_MOD_SUB(minuend, subtrahend, modulus) \
(((minuend) >= (subtrahend)) ? ((minuend) - (subtrahend)) : ((minuend) + (modulus) - (subtrahend)))
#define REVS16(x) x
#define REVS32(x) x
#define IRQF_DISABLED 0x00000020
#if 0
#define REVS16(x) (uint16_t) ((((x) & 0xFF00)>>8)|(((x) & 0x00FF)<<8))
#define REVS32(x) (uint32_t) ((((x) & 0xFF000000)>>24)|(((x) & 0x00FF0000)>>8)|\
(((x) & 0x0000FF00)<<8)|(((x) & 0x000000FF)<<24))
#endif
#define MIN(x,y) ((x)<(y)?(x):(y))
#define MAX(x,y) ((x)>(y)?(x):(y))
#define GPS_SECONDS(tow,wnum) (604800*(wnum) + (tow))
#define MOD_2N(x,y) ((x) & (y-1))
#define GPS_FIX_OK 0x03
#define NAV_ALMANACH 0x04
/*ublox sync header*/
#define SYNC1_UBLOX 0xB5
#define SYNC2_UBLOX 0x62
#define UBLOX_SYNC_LEN 0x2
#define INDEX_OF_PAYLOAD 0x6 //SYNC(2) + CLSID + MSGID + LENGTH(2)
#define UBLOX_CHECK_SUM_LEN 0x2 //2 BYTES
#define UBLOX_HEADER_SUM_LEN 0x8 //SYNC(2) + CLSID + MSGID + LENGTH(2) + CHECKSUM(2)
#define UBLOX_MSG_TOTAL_LEN(payload_len) ((payload_len) + UBLOX_HEADER_SUM_LEN)
#define POLL_LENGTH 0
#define CFG_PRT_LEN 20
#define PORT_NUMBER 1
/*----GPS Time Solution----*/
#define NAV_TIMEGPS 0x20
#define TIME_VALID 0x03 /*Time of Week and Week Number valid*/
#define LEAP_VALID 0x04 /*Valid UTC (Leap Seconds already known)*/
/*--sat chn num--*/
#define NUMCHN 20
#define TOD_LINK_ALARM_VALUE 0x02
#define ANT_OPEN_ALARM_VALUE 0x04
#define ANT_SHORT_ALARM_VALUE 0x03
#define GPS_ANT_SIGNAL_WEEK_ALARM_VALUE 0x05
#define GPS_ANT_SIGNAL_STRONG_ALARM_VALUE 0x06
/*8bit ,1 stop bit , no portiy*/
#define PORT_MODE_8N1 (0x3<<6)|(0x4<<9)|(0x0<<12)
#define PORT_BAUDRATE 9600
#define PORT_PROTOCAL_IN 0x07 /*UBX+NMEA+RTCM2*/
#define PORT_PROTOCAL_OUT 0x01 /*UBX*/
#define CFG_ANT_LEN 4
#define ANT_FLAGS 0x1F
#define CABDLY_DELAY_CRC_BYTE1 0xa5
#define CABDLY_DELAY_CRC_BYTE2 0x5a
#define CABDLY_DELAY 12 //205 1m 4ns
#define CABLE_DELAY_ADDR 0x643800
#define CABLE_DELAY_SIZE 2048
#define CFG_TP_LEN 20
#define CFG_TP5_LEN 32
#define TP_IDX0 0
#define TP_IDX11 1
#define TP_INTERVAL 1000000 //1s
#define TP_POSITIVE 1
#define TP_NEGATIVE -1
#define TP_TIME_GPS 1
#define TP_TIME_UTC 0
#define TP_PLUSE_WIDTH 1000 //1ms
#define CFG_NAV5_LEN 36
#define NAV5_FIXMODE 2
#define NAV5_PACC 100
#define NAV5_TACC 100
#define CFG_MSG_LEN 3
/*--ublox max message length--*/
#define UBLOX_MAX_MEG_LENGTH 280
#define UBLOX_MIN_MEG_LENGTH 10
#define UBLOX_CABDLY 205
/*class id and msg id*/
#define ACK 0x05
#define ACK_ACK 0x01
#define ACK_NACK 0x00
#define NAV 0x01
#define NAV_AOPSTATUS 0x60
#define NAV_CLOCK 0x22
#define NAV_DGPS 0x31
#define NAV_DOP 0x04
#define NAV_EKFSTATUS 0x40
#define NAV_POSECEF 0x01
#define NAV_POSLLH 0x02
#define NAV_SBAS 0x32
#define NAV_SOL 0x06
#define NAV_STATUS 0x03
#define NAV_SVINFO 0x30
#define NAV_TIMEGPS 0x20
#define NAV_TIMEUTC 0x21
#define NAV_VELECEF 0x11
#define NAV_VELNED 0x12
#define CFG 0x06
#define CFG_ANT 0x13
#define CFG_CFG 0x09
#define CFG_DAT 0x06
#define CFG_EKF 0x12
#define CFG_ESFGWT 0x29
#define CFG_FXN 0xE
#define CFG_INF 0x2
#define CFG_ITFM 0x39
#define CFG_MSG 0x1
#define CFG_NAV5 0x24
#define CFG_NAVX5 0x23
#define CFG_NMEA 0x17
#define CFG_NVS 0x22
#define CFG_PM2 0x3B
#define CFG_PM 0x32
#define CFG_PRT 0x00
#define CFG_RATE 0x8
#define CFG_RINV 0x34
#define CFG_RST 0x4
#define CFG_RXM 0x11
#define CFG_SBAS 0x16
#define CFG_TMODE2 0x3D
#define CFG_TMODE 0x1D
#define CFG_TP5 0x31
#define CFG_TP 0x7
#define CFG_USB 0x1B
#define CFG_RST_LEN 4
#define RST_NAVBBR_COLD 0xFFFF
#define RST_MODE_HW 0x00
#define MON 0x0a
#define MON_HW2 0xb
#define MON_HW 0x9
#define MON_IO 0x2
#define MON_MSGPP 0x6
#define MON_RXBUF 0x7
#define MON_RXR 0x21
#define MON_TXBUF 0x8
#define MON_VER 0x4
/*close name mes*/
#define NMEA 0xf0
#define RMC 0x04
#define VTG 0x05
#define GGA 0x00
#define GSA 0x02
#define GSV 0x03
#define GLL 0x01
#define ZDA 0x08
#define GPS_TIME_JUMP_20MIN 1200 /* 60*20 */
#define HOLD_OVER_TIME_UNIT 900 /* 15*60 */
/*#define FPGA_BASE_ADDR 0x74000000*/
#define PHASE_DIFF_COUNT_L_REG 0xe
#define PHASE_DIFF_COUNT_H_REG 0xd
#define PHASE_DIFF_FLAG_REG 0xf
#define PP2S_SYNC_EN_REG 0xb
#define PP2S_CTRL 0xc
#define PHASE_FLAG 0xf /*1:<3A><>ǰ 0:<3A>ͺ<EFBFBD>*/
#define SYNC_EN_FLAG 0x01
#define PP2S_COUNTER_ADV 0x10 /*<2A><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
#define PP2S_COUNTER_DELAY 0x11 /*<2A>ͺ<EFBFBD><CDBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
#define PP2S_ADV_PPS_MASK 0x8000
#define PP2S_PPS_PHD_ABS 0x7FFF
#define CPU_TIMER_MASK 15
#define AD9548_M 6
/* should be update when hardware changed */
#define PH_ERR_NS 8 /*1000/122.88M ~= 8.138*/
#define PH_SYNC 15 /*122.88/5 ~= 25*/
#define PH_FSYNC 18 /*122.88/20.48*3 = 18*/
#define PH_LIMIT_1 120
#define PH_LIMIT_2 130
#define HOLD_OVER_IN 90 /* 3*30 = 90 gpsʧ<73><CAA7><EFBFBD><EFBFBD>ÿ3s<33><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>pp1s<31>ź<EFBFBD>*/
#define HOLD_OVER_1_HOUR 3600 /* 60*60 */
#define HOLD_OVER_1_DAY 86400/* 3600*24 */
#define UBX_SEM_NUM 13
#define MY_IPCKEY 0x10005500
#define TX_SEMA_NAME "tx_sema"
typedef void (*FUNCPTR)(void *);
#define UBLOX_TX_BUF_SIZE 256
#define UBLOX_RX_BUF_SIZE 1024
#define HALF_BUFSIZE 511
#define RT_MASK 1023 //(RTBUFSIZE - 1)
/*-----some get/set types protocols' sem-------*/
typedef enum
{
CFG_PRT_SEM,
CFG_MSG_SEM,
CFG_TP_SEM,
CFG_CFG_SEM,
CFG_ANT_SEM,
CFG_NAV5_SEM,
CFG_RST_SEM,
NAV_POSLLH_SEM,
NAV_STATUS_SEM,
NAV_GPSTIME_SEM,
NAV_SVINFO_SEM,
MON_HW_SEM,
MON_VER_SEM
}ubx_sem_e;
/* arg for semctl system calls. */
union semun {
int val; /* value for SETVAL */
struct semid_ds *buf; /* buffer for IPC_STAT & IPC_SET */
unsigned short *array; /* array for GETALL & SETALL */
struct seminfo *__buf; /* buffer for IPC_INFO */
};
typedef struct
{
uint16_t reg_addr;
uint8_t reg_val;
}reg_value_s;
//typedef unsigned int time_t;/*32bit*/
typedef void irqreturn_t;
typedef irqreturn_t (*irq_handler_t)(int, void *);
/*Start of repeated block (numCh times) in payload of NAV-SVINFO*/
typedef struct
{
uint8_t chn; /*Channel number*/
uint8_t svid; /*Satellite ID*/
uint8_t flags; /*Bitmask,bit2,Orbit information is available for this SV (Ephemeris or Almanach)*/
uint8_t quality; /**/
uint8_t cno; /*Carrier to Noise Ratio (Signal Strength)*/
int8_t elev; /*Elevation in integer degrees*/
int16_t azim; /*Azimuth in integer degrees*/
int32_t prres; /*Pseudo range residual in centimetres*/
}__attribute__((packed)) chn_info_s;
/*gps消息结构体*/
typedef struct
{
uint8_t cls_id;
uint8_t msg_id;
uint16_t length;
union
{
/*Class NAV 0X01--------------------------------- */
/*Message NAV-POSLLH 0X02*/
struct
{
uint32_t i_tow; /*gps millsecond time of week,ms*/
int32_t lon; /* longitude,deg scaling: 1e-7*/
int32_t lat; /*latitude,deg scaling: 1e-7*/
int32_t height; /*Height above Ellipsoid,mm*/
int32_t hmsl; /*Height above mean sea level,mm*/
uint32_t hacc; /*Horizontal Accuracy Estimate,mm*/
uint32_t vacc; /*Vertical Accuracy Estimate,mm*/
}__attribute__((packed)) nav_pos;
/*message NAV-STATUS 0x03*/
struct
{
uint32_t i_tow;
uint8_t gps_fix; /*GPSfix Type,0x00 no fix,others */
uint8_t flags; /*bit0:gpsFixOk,bit1:1 if DGPS used,bit2:1 if Week Number valid,bit3:1 if Time of Week valid*/
uint8_t diff_stat;
uint8_t res;
uint32_t ttff; /*Time to first fix (millisecond time tag)*/
uint32_t msss; /*Milliseconds since Startup / Reset*/
}__attribute__((packed)) nav_status;
/*message NAV-TIMEGPS*/
struct
{
uint32_t i_tow;
int32_t f_low; /* Fractional Nanoseconds remainder of rounded ms above,*/
int16_t week; /*GPS week (GPS time)*/
int8_t leap; /*Leap Seconds (GPS-UTC)*/
uint8_t valid; /*Validity Flags,bit0=1Valid Time of Week,bit1=1 Valid Week Number,bit2=1 Valid Leap Seconds,*/
uint32_t vacc; /*Time Accuracy Estimate*/
}__attribute__((packed)) nav_timegps;
/*message NAV-SVINFO*/
struct
{
uint32_t i_tow;
uint8_t num_ch; /*Number of channels*/
uint8_t global_flags;
uint16_t res;
chn_info_s chn_info;
}__attribute__((packed)) nav_svinfo;
/*--------------------------------------------------------*/
/*----class ACK---------*/
/*--message ACK-(NAK 0X00 or ACK 0X01)--*/
struct
{
uint8_t clsid; /*Class ID of the Not-Acknowledged/Acknowledged Message*/
uint8_t msgid; /*Message ID of the Not-Acknowledged/Acknowledged Message*/
}__attribute__((packed)) ack_msg;
/*--------------------------------------------------------*/
/*----class CFG 0X06---------*/
/*--message CFG-PRT 0X00 (Get/Set Port Configuration for UART)--*/
struct
{
uint8_t portid; /*Port Identifier Number (= 1 or 2 for UART ports)*/
uint8_t res0;
uint16_t res1;
uint32_t mode; /*A bit mask describing the UART mode, bit7-bit6=11(8bit),*/
uint32_t baudrate; /*Baudrate in bits/second*/
uint16_t inportmask; /*A mask describing which input protocols are active.bit0=UBX*/
uint16_t outportmask; /*A mask describing which output protocols are active.*/
uint16_t flags;
uint16_t pad;
}__attribute__((packed)) config_port;
/*--message CFG-MSG 0X01 --*/
struct
{
uint8_t cls;
uint8_t msgid;
uint8_t rate;
}__attribute__((packed))config_msg;
/*--Message CFG-RST 0X04--*/
struct
{
uint16_t nav_bbr;
uint8_t rst_mode;
uint8_t res;
}__attribute__((packed)) config_rst;
#if 0
/*--message CFG-TP 0X07(Get/Set TimePulse Parameters)--*/
struct
{
uint32_t interval; /*Time interval for time pulse*/
uint32_t length; /*Length of time pulse*/
int8_t status; /*Time pulse config setting,>0 positive,=0 off,<0 negative*/
uint8_t time_ref; /*Tlignment to reference time,0=UTC time,1=GPS time*/
uint8_t flags; /*bit0=1 TP allowed to be asynchronized and available even when time is not valid*/
uint8_t res;
int16_t cab_dly; /*Antenna Cable Delay*/
int16_t rf_grp_dly; /*Receiver RF Group Delay*/
int32_t user_dly; /*User Time Function Delay (positive delay results in earlier pulse)*/
}__attribute__((packed)) config_tp;
#endif
struct
{
uint8_t tp_idx;
uint8_t version;
uint8_t rsv[2]; /*reserved*/
int16_t cab_dly; /*Antenna Cable Delay: ns*/
int16_t rf_dly; /*RF group delay: ns*/
uint32_t freq_period; /*pulse frequence or period: Hz_us*/
uint32_t freq_period_locked; /*pulse frequence or period when locked to GNSS: Hz_us*/
uint32_t pulse_len; /*pulse width or duty: us_%*/
uint32_t pulse_len_locked; /*pulse width or duty when locked to GNSS: us_%*/
int32_t user_delay; /*user-configurable time pulse delay: ns*/
uint32_t active:1;
uint32_t lock_gnss_freq:1;
uint32_t lock_other_set:1;
uint32_t is_freq:1;
uint32_t is_length:1;
uint32_t align2tow:1;
uint32_t polarity:1;
uint32_t grid_utc_gnss:4;
uint32_t sync_mode:3;
uint32_t flag_rsv:18;
}__attribute__((packed)) config_tp5;
/*--message CFG-NAV5 Get/Set Navigation Engine Settings--*/
struct
{
uint16_t mask; /*Parameters Bitmask.*/
uint8_t dyn_model; /*Dynamic Platform model:*/
uint8_t fix_mode; /*Position Fixing Mode.*/
int32_t fixed_alt; /*Fixed altitude*/
uint32_t fixed_alt_var; /*Fixed altitude variance for 2D mode.*/
int8_t min_elev; /*Minimum Elevation for a GNSS satellite to be used in NAV*/
uint8_t drlimit; /*Maximum time to perform dead reckoning*/
uint16_t pdop; /*Position DOP Mask to use*/
uint16_t tdop; /*Time DOP Mask to use*/
uint16_t pacc; /* Position Accuracy Mask */
uint16_t tacc; /* Time Accuracy Mask */
uint8_t static_hold_thresh; /* Static hold threshold */
uint8_t dgps_time_out; /* DGPS timeout */
uint32_t res2; /* reserved, set to 0 */
uint32_t res3; /* reserved, set to 0 */
uint32_t res4; /* reserved, set to 0 */
}__attribute__((packed)) config_nav5;
/*--message CFG-CFG 0X09 (Clear, Save and Load configurations)--*/
struct
{
uint32_t clearmask;
uint32_t savemask;
uint32_t loadmask;
}__attribute__((packed)) config_cfg;
/*--message CFG-ANT 0X13 (Get/Set Antenna Control Settings)--*/
struct
{
uint16_t flags;
uint16_t pins;/*READ-ONLY,when get*/
}__attribute__((packed)) config_ant;
/*---------------------------------------------------------------*/
/*--class MON 0x0A--*/
/*--Message MON-HW0x09--*/
struct
{
uint32_t pinsel;
uint32_t pinbank;
uint32_t pindir;
uint32_t pinvar;
uint16_t noiseperms;
uint16_t agccnt;
uint8_t astatus;
uint8_t apower;
uint8_t flags;
uint8_t res1;
uint32_t usedmask;
uint8_t vp[25];
uint8_t res2[3];
uint32_t pinirq;
uint32_t pullh;
uint32_t pulll;
}__attribute__((packed)) monitor_hw;
struct
{
uint8_t sw_version[30];
uint8_t hw_version[10];
/*
uint8_t rom_version[30];
uint8_t extension[30];
*/
}__attribute__((packed)) monitor_ver;
/*--------------------------------------------------------*/
}__attribute__((packed))pay_load;
}__attribute__((packed)) ubx_msg_s;
typedef struct
{
int32_t latitude; /*degree 1e-7*/
int32_t longitude;/*degree 1e-7*/
int32_t altitude; /*millimeter*/
uint8_t gps_fix;
uint8_t fix_flag;
uint32_t time_of_week;
uint8_t valid_flag;
int8_t leap_sec;
uint16_t week_number;
uint8_t num_ch;
int16_t cab_dly; /*cable delay time,uint:ns*/
uint32_t pulse_width; /*Length of time pulse*/
int8_t pps_status; /*PPS: >0,postive; =0,no;<0, negative*/
uint8_t ant_state;
uint8_t sw_version[30];
uint8_t hw_version[10];
uint8_t sta_id[NUMCHN]; /*satellite ID*/
uint8_t cno[NUMCHN]; /*Carrier to Noise Ratio (Signal Strength)*/
uint8_t tracking_num;
uint8_t visable_num;
uint8_t sig_over_high;
uint8_t lock_state; /*1: gps lock 0: gps unlock*/
}drv_gps_msg_s;
/********************************************************************/
typedef struct
{
uint8_t recv_buf[UBLOX_RX_BUF_SIZE];
/*union
{
uint32_t whole;
struct
{
uint16_t recv_ptr; //接收指针
uint16_t deal_ptr; //处理指针
}
}*/
uint16_t recv_ptr; /*接收指针*/
uint16_t deal_ptr; /*处理指针*/
uint16_t recv_cnt; /*最新接收到的消息长度*/
uint16_t deal_cnt; /*未处理消息长度*/
int32_t send_sem;
ubx_msg_s *send_buf_ptr;
uint16_t send_cnt; /*待发送消息长度*/
uint16_t frame_flag; //帧头
}ublox_uart_s;
typedef struct
{
int32_t latitude; //纬度 单位:度/1e-7
int32_t longitude; //经度 单位:度/1e-7
int32_t altitude; //高度 单位:毫米
}location_info_s;
typedef struct
{
uint8_t lock_flag; /* GPS Fixed ok */
uint8_t leap_valid; /* leap second valid */
uint8_t view_sats; /* Satellites in view */
uint8_t track_sats; /* Satellites under tracking */
uint8_t survey_in; /*0 not in Survey-in mode*/
uint8_t alma_complete; /* 0 Calendar not complete */
uint8_t resv[2]; /* resv*/
float sig_level[12]; /* signal level of satellites */
}tracking_info_s;
void drv_gps_recv_task(void);
int32_t drv_uart_read(uint8_t *pdata, uint32_t len);
int32_t init_clock_module(void);
int32_t drv_tfu_receiver_init(void);
int32_t get_receiver_fix_status();
int32_t get_src_time(time_t* time_ptr);
int32_t drv_tfu_get_ublox_seconds(time_t* sec_ptr);
int32_t get_tfu_location_info(location_info_s *locat_ptr);
int32_t get_clock_tracking_state(tracking_info_s *trk_ptr);
int32_t reset_clock_module(void);
#ifdef __cplusplus
#if __cplusplus
}
#endif
#endif
#endif
/**********************************************************************/