/**************************************************************************** * * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * * 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 PX4 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 OWNER 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 U-Blox protocol definitions */ #ifndef UBX_H_ #define UBX_H_ #include "gps_helper.h" #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 /* ClassIDs (the ones that are used) */ #define UBX_CLASS_NAV 0x01 //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 //#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 #define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_ACK_NAK 0x00 #define UBX_MESSAGE_CFG_PRT 0x00 #define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ #define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 #define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ #define UBX_CFG_NAV5_LENGTH 36 #define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ #define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ // ************ /** the structures of the binary packets */ #pragma pack(push, 1) typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t lon; /**< Longitude * 1e-7, deg */ int32_t lat; /**< Latitude * 1e-7, deg */ int32_t height; /**< Height above Ellipsoid, mm */ int32_t height_msl; /**< Height above mean sea level, mm */ uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ int16_t week; /**< GPS week (GPS time) */ uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ uint8_t flags; int32_t ecefX; int32_t ecefY; int32_t ecefZ; uint32_t pAcc; int32_t ecefVX; int32_t ecefVY; int32_t ecefVZ; uint32_t sAcc; uint16_t pDOP; uint8_t reserved1; uint8_t numSV; uint32_t reserved2; uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_sol_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ uint16_t year; /**< Year, range 1999..2099 (UTC) */ 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 min; /**< Minute of Hour, range 0..59 (UTC) */ uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; //typedef struct { // uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ // uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ // uint16_t pDOP; /**< Position DOP (scaling 0.01) */ // uint16_t tDOP; /**< Time DOP (scaling 0.01) */ // uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ // uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ // uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ // uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ // uint8_t ck_a; // uint8_t ck_b; //} gps_bin_nav_dop_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ uint8_t svid; /**< Satellite ID */ uint8_t flags; uint8_t quality; uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ int8_t elev; /**< Elevation in integer degrees */ int16_t azim; /**< Azimuth in integer degrees */ int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; typedef struct { uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_svinfo_part3_packet_t; typedef struct { uint32_t time_milliseconds; // GPS Millisecond Time of Week int32_t velN; //NED north velocity, cm/s int32_t velE; //NED east velocity, cm/s int32_t velD; //NED down velocity, cm/s uint32_t speed; //Speed (3-D), cm/s uint32_t gSpeed; //Ground Speed (2-D), cm/s int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 uint32_t sAcc; //Speed Accuracy Estimate, cm/s uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_velned_packet_t; //typedef struct { // int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ // int16_t week; /**< Measurement GPS week number */ // uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // //} gps_bin_rxm_svsi_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint8_t ck_a; uint8_t ck_b; } gps_bin_ack_ack_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint8_t ck_a; uint8_t ck_b; } gps_bin_ack_nak_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint16_t length; uint8_t portID; uint8_t res0; uint16_t res1; uint32_t mode; uint32_t baudRate; uint16_t inProtoMask; uint16_t outProtoMask; uint16_t flags; uint16_t pad; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_prt_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint16_t length; uint16_t measRate; uint16_t navRate; uint16_t timeRef; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_rate_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint16_t length; uint16_t mask; uint8_t dynModel; uint8_t fixMode; int32_t fixedAlt; uint32_t fixedAltVar; int8_t minElev; uint8_t drLimit; uint16_t pDop; uint16_t tDop; uint16_t pAcc; uint16_t tAcc; uint8_t staticHoldThresh; uint8_t dgpsTimeOut; uint32_t reserved2; uint32_t reserved3; uint32_t reserved4; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_nav5_packet_t; typedef struct { uint8_t clsID; uint8_t msgID; uint16_t length; uint8_t msgClass_payload; uint8_t msgID_payload; uint8_t rate[6]; uint8_t ck_a; uint8_t ck_b; } type_gps_bin_cfg_msg_packet_t; // END the structures of the binary packets // ************ typedef enum { UBX_CONFIG_STATE_PRT = 0, UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, UBX_CONFIG_STATE_RATE, UBX_CONFIG_STATE_NAV5, UBX_CONFIG_STATE_MSG_NAV_POSLLH, UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, UBX_CONFIG_STATE_MSG_NAV_DOP, UBX_CONFIG_STATE_MSG_NAV_SVINFO, UBX_CONFIG_STATE_MSG_NAV_SOL, UBX_CONFIG_STATE_MSG_NAV_VELNED, // UBX_CONFIG_STATE_MSG_RXM_SVSI, UBX_CONFIG_STATE_CONFIGURED } ubx_config_state_t; typedef enum { CLASS_UNKNOWN = 0, NAV = 1, RXM = 2, ACK = 3, CFG = 4 } ubx_message_class_t; typedef enum { //these numbers do NOT correspond to the message id numbers of the ubx protocol ID_UNKNOWN = 0, NAV_POSLLH, NAV_SOL, NAV_TIMEUTC, // NAV_DOP, NAV_SVINFO, NAV_VELNED, // RXM_SVSI, CFG_NAV5, ACK_ACK, ACK_NAK, } ubx_message_id_t; typedef enum { UBX_DECODE_UNINIT = 0, UBX_DECODE_GOT_SYNC1, UBX_DECODE_GOT_SYNC2, UBX_DECODE_GOT_CLASS, UBX_DECODE_GOT_MESSAGEID, UBX_DECODE_GOT_LENGTH1, UBX_DECODE_GOT_LENGTH2 } ubx_decode_state_t; //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; #pragma pack(pop) #define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer class UBX : public GPS_Helper { public: UBX(); ~UBX(); void reset(void); void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate); void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); private: /** * Reset the parse state machine for a fresh start */ void decodeInit(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ void addByteToChecksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ void addChecksumToMessage(uint8_t*, const unsigned); ubx_config_state_t _config_state; bool _waiting_for_ack; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; ubx_message_class_t _message_class; ubx_message_id_t _message_id; unsigned _payload_size; }; #endif /* UBX_H_ */