gps_ublox.h

/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, 
 * this list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form must reproduce the above copyright notice, 
 * this list of conditions and the following disclaimer in the documentation 
 * and/or other materials provided with the distribution.
 * 
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file gps_ublox.h
 * 
 * \author MAV'RIC Team
 * \author Nicolas Dousse
 *   
 * \brief This file decodes the messages from the UBLOX GPS
 * 
 ******************************************************************************/


#ifndef GPS_UBLOX_H__
#define GPS_UBLOX_H__

#ifdef __cplusplus
extern "C" {
#endif

#include "stdint.h"
#include "stdbool.h"
#include "maths.h"
#include "streams.h"
#include "buffer.h"
#include "uart_int.h"

/*
 *  try to put a UBlox into binary mode. This is in two parts. First we
 *  send a PUBX asking the UBlox to receive NMEA and UBX, and send UBX,
 *  with a baudrate of 38400. Then we send a UBX message setting rate 1
 *  for the NAV_SOL message. The setup of NAV_SOL is to cope with
 *  configurations where all UBX binary message types are disabled.
 * 0001: UBX
 * 0002: NMEA
 * 0003: NMEA + UBX
 */
//#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*26\n\265\142\006\001\003\000\001\006\001\022\117"

// changed by ndousse
#define UBLOX_SET_BINARY "$PUBX,41,1,0003,0001,38400,0*25\r\n"//\265\142\006\001\003\000\001\006\001\022\117"

/*
Structure of an UBlox binary message

PREAMBLE1       PREAMBLE2       CLASS       MSG_ID      LENGTH1         LENGTH2         MESSAGE         CHECKSUMA       CHECKSUMB
1 byte          1 byte          1 byte      1 byte      Lowest byte     Highest byte    LENGTH size     1 byte          1 byte

The information is received in the Little endian format (least significant byte first).
*/

#define UBX_PREAMBLE1 0xb5
#define UBX_PREAMBLE2 0x62

/*
Name Class Description
NAV 0x01 Navigation Results: Position, Speed, Time, Acc, Heading, DOP, SVs used
RXM 0x02 Receiver Manager Messages: Satellite Status, RTC Status
INF 0x04 Information Messages: Printf-Style Messages, with IDs such as Error, Warning, Notice
ACK 0x05 Ack/Nack Messages: as replies to CFG Input Messages
CFG 0x06 Configuration Input Messages: Set Dynamic Model, Set DOP Mask, Set Baud Rate, etc.
MON 0x0A Monitoring Messages: Comunication Status, CPU Load, Stack Usage, Task Status
AID 0x0B AssistNow Aiding Messages: Ephemeris, Almanac, other A-GPS data input
TIM 0x0D Timing Messages: Timepulse Output, Timemark Results
*/
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_INF 0x04
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
#define UBX_CLASS_AID 0x0B
#define UBX_CLASS_TIM 0x0D
#define UBX_CLASS_ESF 0x10

// Type of Messages that can be received in each class
#define MSG_ACK_NACK 0x00
#define MSG_ACK_ACK 0x01

#define MSG_INF_ERROR 0x00
#define MSG_INF_WARNING 0x01
#define MSG_INF_NOTICE 0x02
#define MSG_INF_TEST 0x03
#define MSG_INF_DEBUG 0x04

#define MSG_NAV_POSLLH 0x02
#define MSG_NAV_STATUS 0x03
#define MSG_NAV_AOPSTATUS 0x60
#define MSG_NAV_CLOCK 0x22
#define MSG_NAV_SOL 0x06
#define MSG_NAV_VELNED 0x12
#define MSG_NAV_VELCEF 0x11
#define MSG_NAV_TIMEGPS 0x20
#define MSG_NAV_TIMEUTC 0x21
#define MSG_NAV_SVINFO 0x30

#define MSG_CFG_PRT 0x00
#define MSG_CFG_RATE 0x08
#define MSG_CFG_SET_RATE 0x01
#define MSG_CFG_NAV_SETTINGS 0x24
#define MSG_CFG_NMEA 0x17

#define MSG_MON_RXR 0x21

#define MSG_TIM_TP 0x01
#define MSG_TIM_VRFY 0x06

#define UBX_PLATFORM_PORTABLE 0x00
#define UBX_PLATFORM_STATIONARY 0x02
#define UBX_PLATFORM_PEDESTRIAN 0x03
#define UBX_PLATFORM_AUTO   0x04
#define UBX_PLATFORM_SEA    0x05
#define UBX_PLATFORM_1GAIR  0x06
#define UBX_PLATFORM_2GAIR  0x07
#define UBX_PLATFORM_4GAIR  0x08

#define GPS_FIX_TYPE_NOFIX 0x00
#define GPS_FIX_TYPE_DEADRECK 0x01
#define GPS_FIX_TYPE_2DFIX 0x02
#define GPS_FIX_TYPE_3DFIX 0x03
#define GPS_FIX_TYPE_GPSDEADRECK 0x04
#define GPS_FIX_TYPE_TIMEONLY 0x05

// For binary message
#define UBX_CFG_MSG 0xF1
#define UBX_CFG_MSG_ID 0x41

// Sizes
#define UBX_SIZE_NAV_POSLLH 28
#define UBX_SIZE_NAV_STATUS 16
#define UBX_SIZE_NAV_SOL 52
#define UBX_SIZE_NAV_VELNED 36
#define UBX_SIZE_NAV_SVINFO 30 //8 + 12*num_channel
#define UBX_SIZE_NAV_SETTINGS 36

#define UBX_SIZE_CFG_RATE 6
#define UBX_SIZE_CFG_GETSET_RATE 3

#define UBX_SIZE_MON_RXR 1

#define UBX_SIZE_TIM_TP 16
#define UBX_SIZE_TIM_VRFY 20

#define NAV_STATUS_FIX_NVALID 0
#define NAV_STATUS_FIX_VALID 1

//epoch
#define TIME_OF_DAY 0 //<
#define TIME_OF_WEEK 1 //< Ublox
#define TIME_OF_YEAR 2 //< MTK, NMEA
#define UNIX_EPOCH 3

#define DEG2RAD PI/180

// The UART bytes are sent in a little endian format from the GPS, if the processor is big endian, define BIG_ENDIAN
// Otherwise comment the following line
#define BIG_ENDIAN

#ifdef BIG_ENDIAN

    typedef struct
    {
        uint16_t length;                    
        uint8_t msg_id_header;              
        uint8_t msg_class;                  
        uint8_t preamble2;                  
        uint8_t preamble1;                  
    }ubx_header_t;

    typedef struct
    {
        uint16_t timeref;                   
        uint16_t nav_rate;                  
        uint16_t measure_rate_ms;           
    }ubx_cfg_nav_rate_t;
    
    // We still have to send to message in the correct order to the GPS
    typedef struct
    {
        uint16_t measure_rate_ms;           
        uint16_t nav_rate;                  
        uint16_t timeref;                   
    }ubx_cfg_nav_rate_send_t;

    typedef struct
    {
        uint8_t rate;                       
        uint8_t msg_id_rate;                
        uint8_t msg_class;                  
    }ubx_cfg_msg_rate_t;

    // We still have to send to message in the correct order to the GPS
    typedef struct
    {
        uint8_t msg_class;                  
        uint8_t msg_id_rate;                
        uint8_t rate;                       
    }ubx_cfg_msg_rate_send_t;

    typedef struct
    {
        uint32_t res4;                      
        uint32_t res3;                      
        uint32_t res2;                      
        uint8_t dgps_timeout;               
        uint8_t static_hold_thresh;         
        uint16_t t_acc;                     
        uint16_t p_acc;                     
        uint16_t t_dop;                     
        uint16_t p_dop;                     
        uint8_t dr_limit;                   
        int8_t min_elev;                        
        uint32_t fixed_alt_var;             
        int32_t fixed_alt;                  
        uint8_t fix_mode;                   
        uint8_t dyn_model;                  
        uint16_t mask;                      
    }ubx_cfg_nav_settings_t;

    typedef struct
    {
        uint32_t vertical_accuracy;         
        uint32_t horizontal_accuracy;       
        int32_t altitude_msl;               
        int32_t altitude_ellipsoid;         
        int32_t latitude;                   
        int32_t longitude;                  
        uint32_t itow;                      
    }ubx_nav_pos_llh_t;

    typedef struct
    {
        uint32_t uptime;                    
        uint32_t time_to_first_fix;         
        uint8_t flags2;                     
        uint8_t fix_status;                 
        uint8_t flags;                      
        uint8_t fix_type;                   
        uint32_t itow;                      
    }ubx_nav_status_t;

    typedef struct
    {
        uint32_t res2;                      
        uint8_t satellites;                 
        uint8_t res;                        
        uint16_t position_DOP;              
        uint32_t speed_accuracy;            
        int32_t ecef_z_velocity;            
        int32_t ecef_y_velocity;            
        int32_t ecef_x_velocity;            
        uint32_t position_accuracy_3d;      
        int32_t ecef_z;                     
        int32_t ecef_y;                     
        int32_t ecef_x;                     
        uint8_t fix_status;                 
        uint8_t fix_type;                   
        int16_t week;                       
        int32_t time_nsec;                  
        uint32_t itow;                      
    }ubx_nav_solution_t;

    typedef struct
    {
        uint32_t heading_accuracy;          
        uint32_t speed_accuracy;            
        int32_t heading_2d;                 
        uint32_t ground_speed_2d;           
        uint32_t speed_3d;                  
        int32_t ned_down;                   
        int32_t ned_east;                   
        int32_t ned_north;                  
        uint32_t itow;                      
    }ubx_nav_vel_ned_t;

    typedef struct
    {
        struct
        {
            int32_t pr_res;                 
            int16_t azim;                   
            int8_t elev;                    
            uint8_t cno;                    
            uint8_t quality;                
            uint8_t flags;                  
            uint8_t svid;                   
            uint8_t chn;                    
        } channel_data[16];
        
        uint16_t reserved;                  
        uint8_t global_flags;               
        uint8_t num_ch;                     
        uint32_t itow;                      
    }ubx_nav_sv_info_t;

    typedef struct
     {
        uint8_t awake_flag;                 
    }ubx_mon_rxr_struct_t;

    typedef struct
    {
        uint8_t res;                        
        uint8_t flags;                      
        uint16_t week;                      
        int32_t q_err;                      
        uint32_t tow_sub_ms;                    
        uint32_t tow_ms;                        
    }ubx_tim_tp_t;

    typedef struct
    {
        uint8_t res;                        
        uint8_t flags;                      
        uint16_t wno;                       
        int32_t delta_ns;                   
        int32_t delta_ms;                   
        int32_t frac;                       
        int32_t itow;                       
    }ubx_tim_vrfy_t;

#else   

    typedef struct
    {
        uint8_t preamble1;                  
        uint8_t preamble2;                  
        uint8_t msg_class;                  
        uint8_t msg_id_header;              
        uint16_t length;                    
    }ubx_header_t;

    typedef struct
    {
        uint16_t measure_rate_ms;           
        uint16_t nav_rate;                  
        uint16_t timeref;                   
    }ubx_cfg_nav_rate_t;

    typedef struct
    {
        uint8_t msg_class;                  
        uint8_t msg_id_rate;                
        uint8_t rate;                       
    }ubx_cfg_msg_rate_t;

    typedef struct
    {
        uint16_t mask;                      
        uint8_t dyn_model;                  
        uint8_t fix_mode;                   
        int32_t fixed_alt;                  
        uint32_t fixed_alt_var;             
        int8_t min_elev;                        
        uint8_t dr_limit;                   
        uint16_t p_dop;                     
        uint16_t t_dop;                     
        uint16_t p_acc;                     
        uint16_t t_acc;                     
        uint8_t static_hold_thresh;         
        uint8_t dgps_timeout;               
        uint32_t res2;                      
        uint32_t res3;                      
        uint32_t res4;                      
    }ubx_cfg_nav_settings_t;

    typedef struct
    {
        uint32_t itow;                      
        int32_t longitude;                  
        int32_t latitude;                   
        int32_t altitude_ellipsoid;         
        int32_t altitude_msl;               
        uint32_t horizontal_accuracy;       
        uint32_t vertical_accuracy;         
    }ubx_nav_pos_llh_t;

    typedef struct
    {
        uint32_t itow;                      
        uint8_t fix_type;                   
        uint8_t flags;                      
        uint8_t fix_status;                 
        uint8_t flags2;                     
        uint32_t time_to_first_fix;         
        uint32_t uptime;                    
    }ubx_nav_status_t;

    typedef struct
    {
        uint32_t itow;                      
        int32_t time_nsec;                  
        int16_t week;                       
        uint8_t fix_type;                   
        uint8_t fix_status;                 
        int32_t ecef_x;                     
        int32_t ecef_y;                     
        int32_t ecef_z;                     
        uint32_t position_accuracy_3d;      
        int32_t ecef_x_velocity;            
        int32_t ecef_y_velocity;            
        int32_t ecef_z_velocity;            
        uint32_t speed_accuracy;            
        uint16_t position_DOP;              
        uint8_t res;                        
        uint8_t satellites;                 
        uint32_t res2;                      
    }ubx_nav_solution_t;

    typedef struct
    {
        uint32_t itow;                      
        int32_t ned_north;                  
        int32_t ned_east;                   
        int32_t ned_down;                   
        uint32_t speed_3d;                  
        uint32_t ground_speed_2d;           
        int32_t heading_2d;                 
        uint32_t speed_accuracy;            
        uint32_t heading_accuracy;          
    }ubx_nav_vel_ned_t;

    typedef struct
    {
        uint32_t itow;                      
        uint8_t num_ch;                     
        uint8_t global_flags;               
        uint16_t reserved;                  
    
        struct
        {
            uint8_t chn;                    
            uint8_t svid;                   
            uint8_t flags;                  
            uint8_t quality;                
            uint8_t cno;                    
            int8_t elev;                    
            int16_t azim;                   
            int32_t pr_res;                 
        } channel_data[16];
    }ubx_nav_sv_info_t;

    typedef struct
    {
        uint8_t awake_flag;                 
    }ubx_mon_rxr_struct_t;

    typedef struct  
    {
        uint32_t tow_ms;                        
        uint32_t tow_sub_ms;                    
        int32_t q_err;                      
        uint16_t week;                      
        uint8_t flags;                      
        uint8_t res;                        
    }ubx_tim_tp_t;

    typedef struct
    {
        int32_t itow;                       
        int32_t frac;                       
        int32_t delta_ms;                   
        int32_t delta_ns;                   
        uint16_t wno;                       
        uint8_t flags;                      
        uint8_t res;                        
    }ubx_tim_vrfy_t;

#endif

ubx_cfg_nav_settings_t nav_settings;        

uint8_t cksum_a;                            
uint8_t cksum_b;                            

// State machine state
uint8_t         step;                       
uint8_t         msg_id;                     
uint16_t        payload_length;             
uint16_t        payload_counter;            

uint8_t         ubx_class;

// do we have new position and speed information?
bool new_position;                          
bool new_speed;                             

uint8_t disable_counter;                    

bool next_fix;                              
bool have_raw_velocity;                     

#define NO_GPS 0                            
#define NO_FIX 1                            
#define GPS_OK 2                            

typedef enum {
    GPS_ENGINE_NONE        = -1,            
    GPS_ENGINE_PORTABLE    = 0,             
    GPS_ENGINE_STATIONARY  = 2,             
    GPS_ENGINE_PEDESTRIAN  = 3,             
    GPS_ENGINE_AUTOMOTIVE  = 4,             
    GPS_ENGINE_SEA         = 5,             
    GPS_ENGINE_AIRBORNE_1G = 6,             
    GPS_ENGINE_AIRBORNE_2G = 7,             
    GPS_ENGINE_AIRBORNE_4G = 8              
}gps_engine_setting_t;

gps_engine_setting_t engine_nav_setting;        


#define UBX_TIMEOUT_CYCLES 2                
#define UBX_POSITION_PRECISION 20           
#define UBX_ALTITUDE_PRECISION 20           
#define UBX_SPEED_PRECISION 5               

#define UBX_HEADING_PRECISION 5000000       


typedef struct
{
    double latitude;                        
    double longitude;                       
    float altitude;                         
    float alt_elips;                        
    float speed;                            
    float ground_speed;                     
    float north_speed;                      
    float east_speed;                       
    float vertical_speed;                   
    float course;                           
    
    float horizontal_accuracy;              
    float vertical_accuracy;                    
    
    float speed_accuracy;                   
    float heading_accuracy;                 
    
    uint8_t num_sats;                       
    uint16_t hdop;                          
    
    uint32_t time_last_msg;                 
    uint32_t time_gps;                      
    
    uint8_t  status;                        
    
    uint8_t  horizontal_status;             
    
    uint8_t  altitude_status;               
    uint8_t  speed_status;                  
    uint8_t  course_status;                 
    uint8_t  accuracy_status;               
    
    buffer_t gps_buffer;                    
    byte_stream_t gps_stream_in;            
    byte_stream_t gps_stream_out;           
} gps_t;


uint32_t idle_timer;                            
uint32_t idle_timeout;                      
uint32_t last_fix_time;                     


void gps_ublox_init(gps_t *gps, int32_t UID, usart_config_t usart_conf_gps);


void gps_ublox_configure_gps(gps_t *gps);


void gps_ublox_update(gps_t *gps);

#ifdef __cplusplus
}
#endif

#endif //GPS_UBLOX_H__