PX4 Firmware
PX4 Autopilot Software http://px4.io
ashtech.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2013. All rights reserved.
4  * Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
5  * Kistanov Alexander <akistanov@gramant.ru>
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * 1. Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * 2. Redistributions in binary form must reproduce the above copyright
14  * notice, this list of conditions and the following disclaimer in
15  * the documentation and/or other materials provided with the
16  * distribution.
17  * 3. Neither the name PX4 nor the names of its contributors may be
18  * used to endorse or promote products derived from this software
19  * without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
28  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
29  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  ****************************************************************************/
35 
36 /** @file ASHTECH protocol definitions */
37 
38 #pragma once
39 
40 #include "gps_helper.h"
41 #include "base_station.h"
42 #include "../../definitions.h"
43 #include <math.h>
44 
45 class RTCMParsing;
46 
47 #define ASHTECH_RECV_BUFFER_SIZE 512
48 
49 #define ASH_RESPONSE_TIMEOUT 200 // ms, timeout for waiting for a response
50 
52 {
53 public:
54  /**
55  * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement.
56  */
57  GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position,
58  struct satellite_info_s *satellite_info, float heading_offset = 0.f);
59  virtual ~GPSDriverAshtech();
60 
61  int receive(unsigned timeout) override;
62  int configure(unsigned &baudrate, OutputMode output_mode) override;
63 
64 private:
65  enum class NMEACommand {
66  Acked, // Command that returns a (N)Ack
67  PRT, // port config
68  RID, // board identification
69  RECEIPT// board identification
70  };
71 
72  enum class NMEACommandState {
73  idle,
74  waiting,
75  nack,
76  received
77  };
78 
79  enum class NMEADecodeState {
80  uninit,
81  got_sync1,
82  got_asteriks,
83  got_first_cs_byte,
84  decode_rtcm3
85  };
86 
87  enum class AshtechBoard {
88  trimble_mb_two,
89  other
90  };
91 
92  void decodeInit(void);
93  int handleMessage(int len);
94  int parseChar(uint8_t b);
95 
96  /**
97  * Write a command and wait for a (N)Ack
98  * @return 0 on success, <0 otherwise
99  */
100  int writeAckedCommand(const void *buf, int buf_length, unsigned timeout);
101 
102  int waitForReply(NMEACommand command, const unsigned timeout);
103 
104  /**
105  * receive data for at least the specified amount of time
106  */
107  void receiveWait(unsigned timeout_min);
108 
109  /**
110  * enable output of correction output
111  */
113 
114  void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = (double)NAN, double longitude = (double)NAN,
115  float altitude = NAN);
116 
117  void activateRTCMOutput();
118 
121  uint64_t _last_timestamp_time{0};
122 
125  uint16_t _rx_buffer_bytes{};
126  bool _got_pashr_pos_message{false}; /**< If we got a PASHR,POS message, we will ignore GGA messages */
127 
130  char _port{'A'}; /**< port we are connected to (e.g. 'A') */
131  AshtechBoard _board{AshtechBoard::other}; /**< board we are connected to */
132 
134 
136 
139  bool _configure_done{false};
140 
142 };
143 
RTCMParsing * _rtcm_parsing
Definition: ashtech.h:133
int handleMessage(int len)
Definition: ashtech.cpp:76
int writeAckedCommand(const void *buf, int buf_length, unsigned timeout)
Write a command and wait for a (N)Ack.
Definition: ashtech.cpp:918
int receive(unsigned timeout) override
receive & handle new data from the device
Definition: ashtech.cpp:769
NMEACommandState _command_state
Definition: ashtech.h:129
bool _configure_done
Definition: ashtech.h:139
struct satellite_info_s * _satellite_info
Definition: ashtech.h:119
hrt_abstime gps_abstime
Definition: definitions.h:58
bool _correction_output_activated
Definition: ashtech.h:138
struct vehicle_gps_position_s * _gps_position
Definition: ashtech.h:120
void activateCorrectionOutput()
enable output of correction output
Definition: ashtech.cpp:1135
uint8_t _rx_buffer[ASHTECH_RECV_BUFFER_SIZE]
Definition: ashtech.h:124
bool _got_pashr_pos_message
If we got a PASHR,POS message, we will ignore GGA messages.
Definition: ashtech.h:126
NMEACommand _waiting_for_command
Definition: ashtech.h:128
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int configure(unsigned &baudrate, OutputMode output_mode) override
configure the device
Definition: ashtech.cpp:943
int(* GPSCallbackPtr)(GPSCallbackType type, void *data1, int data2, void *user)
Callback function for platform-specific stuff.
Definition: gps_helper.h:132
void receiveWait(unsigned timeout_min)
receive data for at least the specified amount of time
Definition: ashtech.cpp:760
GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, float heading_offset=0.f)
Definition: ashtech.cpp:53
float _heading_offset
Definition: ashtech.h:141
uint64_t _last_timestamp_time
Definition: ashtech.h:121
void decodeInit(void)
Definition: ashtech.cpp:904
int parseChar(uint8_t b)
Definition: ashtech.cpp:829
void sendSurveyInStatusUpdate(bool active, bool valid, double latitude=(double) NAN, double longitude=(double) NAN, float altitude=NAN)
Definition: ashtech.cpp:1224
GPS driver base class with Base Station Support.
Definition: base_station.h:49
virtual ~GPSDriverAshtech()
Definition: ashtech.cpp:64
gps_abstime _survey_in_start
Definition: ashtech.h:135
uint16_t _rx_buffer_bytes
Definition: ashtech.h:125
int waitForReply(NMEACommand command, const unsigned timeout)
Definition: ashtech.cpp:927
OutputMode _output_mode
Definition: ashtech.h:137
normal GPS output
AshtechBoard _board
board we are connected to
Definition: ashtech.h:131
#define ASHTECH_RECV_BUFFER_SIZE
Definition: ashtech.h:47
NMEADecodeState _decode_state
Definition: ashtech.h:123
char _port
port we are connected to (e.g.
Definition: ashtech.h:130
void activateRTCMOutput()
Definition: ashtech.cpp:727