- 前言
- 一、主程序
- 二、receive函数
- 三、parseChar函数
- 四、handleMessage函数
PX4 1.12.3
一、主程序程序位置：
void
GPS::run()
{
打开端口，具体的端口由参数GPS_XXX_CONFIG确定
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
return;
}
#ifdef __PX4_LINUX
如果在LINUX平台上，并且接口为SPI协议，则打开SPI接口
if (_interface == GPSHelper::Interface::SPI) {
int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi)
int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
return;
}
status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed);
if (status_value < 0) {
PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno);
return;
}
}
#endif
设计GPS偏航角的偏移
param_t handle = param_find("GPS_YAW_OFFSET");
float heading_offset = 0.f;
if (handle != PARAM_INVALID) {
param_get(handle, &heading_offset);
heading_offset = matrix::wrap_pi(math::radians(heading_offset));
}
设置UBX的三种模式
int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration
handle = param_find("GPS_UBX_DYNMODEL");
if (handle != PARAM_INVALID) {
param_get(handle, &gps_ubx_dynmodel);
}
handle = param_find("GPS_UBX_MODE");
GPSDriverUBX::UBXMode ubx_mode{GPSDriverUBX::UBXMode::Normal};
if (handle != PARAM_INVALID) {
int32_t gps_ubx_mode = 0;
param_get(handle, &gps_ubx_mode);
if (gps_ubx_mode == 1) { // heading
if (_instance == Instance::Main) {
ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase;
} else {
ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
}
}
}
设置GNSS系统的参数
int32_t gnssSystemsParam = static_cast(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS); if (_instance == Instance::Main) { handle = param_find("GPS_1_GNSS"); param_get(handle, &gnssSystemsParam); } else if (_instance == Instance::Secondary) { handle = param_find("GPS_2_GNSS"); param_get(handle, &gnssSystemsParam); }
初始化日志记录，根据参数GPS_DUMP_COMM判断是否将GPS通信相关的信息记录到log日志中
initializeCommunicationDump();
uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0;
大循环处理接收到的GPS数据
while (!should_exit()) {
if (_helper != nullptr) {
delete (_helper);
_helper = nullptr;
}
根据GPS模式调用不同的GPS驱动类，并设置设备类型，GPS模式由参数GPS_XXX_PROTOCOL指定
switch (_mode) {
case GPS_DRIVER_MODE_NONE:
_mode = GPS_DRIVER_MODE_UBX;
case GPS_DRIVER_MODE_UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
gps_ubx_dynmodel, heading_offset, ubx_mode);
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
#ifndef CONSTRAINED_FLASH
case GPS_DRIVER_MODE_MTK:
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
set_device_type(DRV_GPS_DEVTYPE_MTK);
break;
case GPS_DRIVER_MODE_ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
set_device_type(DRV_GPS_DEVTYPE_ASHTECH);
break;
case GPS_DRIVER_MODE_EMLIDREACH:
_helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH);
break;
case GPS_DRIVER_MODE_FEMTOMES:
_helper = new GPSDriverFemto(&GPS::callback, this, &_report_gps_pos);
set_device_type(DRV_GPS_DEVTYPE_FEMTOMES);
break;
#endif // CONSTRAINED_FLASH
default:
break;
}
前面根据不同的GPS模式，都实例化了一个对象_helper，通过_helper调用相应的类里面的configure成员函数来配置GPS的参数
_baudrate = _configured_baudrate;
GPSHelper::GPSConfig gpsConfig{};
gpsConfig.output_mode = GPSHelper::OutputMode::GPS;
gpsConfig.gnss_systems = static_cast(gnssSystemsParam);
if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) {
重置GPS的报文
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = heading_offset;
GPS检测成功，重置统计
if (_mode == GPS_DRIVER_MODE_UBX) {
_helper->resetUpdateRates();
设置不同的GPS板子类型
if (get_device_type() == DRV_GPS_DEVTYPE_UBX) {
GPSDriverUBX *driver_ubx = (GPSDriverUBX *)_helper;
switch (driver_ubx->board()) {
case GPSDriverUBX::Board::u_blox6:
set_device_type(DRV_GPS_DEVTYPE_UBX_6);
break;
case GPSDriverUBX::Board::u_blox7:
set_device_type(DRV_GPS_DEVTYPE_UBX_7);
break;
case GPSDriverUBX::Board::u_blox8:
set_device_type(DRV_GPS_DEVTYPE_UBX_8);
break;
case GPSDriverUBX::Board::u_blox9:
set_device_type(DRV_GPS_DEVTYPE_UBX_9);
break;
case GPSDriverUBX::Board::u_blox9_F9P:
set_device_type(DRV_GPS_DEVTYPE_UBX_F9P);
break;
default:
set_device_type(DRV_GPS_DEVTYPE_UBX);
break;
}
}
}
函数返回值，在后面用到
int helper_ret;
设置超时频率
unsigned receive_timeout = TIMEOUT_5HZ;
if (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) {
receive_timeout = TIMEOUT_1HZ;
}
大循环，在 _helper->receive函数中调用解析GPS数据的函数，如果解析正常且没有收到退出指令，则一直进行解析，receive函数见第二节。
while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) {
解析正常，则发布解析后的GPS的报文，包括经纬度等数据
if (helper_ret & 1) {
publish();
last_rate_count++;
}
发布卫星信息
if (_p_report_sat_info && (helper_ret & 2)) {
publishSatelliteInfo();
}
需要时重启
reset_if_scheduled();
每5秒种计算一次GPS的更新频率
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
_rate = last_rate_count / dt;
_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
_rate_reading = _num_bytes_read / dt;
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_last_rate_rtcm_injection_count = 0;
_num_bytes_read = 0;
_helper->storeUpdateRates();
_helper->resetUpdateRates();
}
如果在WHILE循环中，说明解析正常，将_healthy赋值为true
if (!_healthy) {
_healthy = true;
}
}
如果在WHILE循环外，说明解析失败，将_healthy赋值为false
if (_healthy) {
_healthy = false;
_rate = 0.0f;
_rate_rtcm_injection = 0.0f;
}
}
如果参数GPS_XXX_PROTOCOL设置为auto（0），则遍历所有的GPS模式，直到找到匹配GPS板卡的模式
if (_mode_auto) {
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
#ifndef CONSTRAINED_FLASH
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_EMLIDREACH;
break;
case GPS_DRIVER_MODE_EMLIDREACH:
_mode = GPS_DRIVER_MODE_FEMTOMES;
break;
case GPS_DRIVER_MODE_FEMTOMES:
#endif // CONSTRAINED_FLASH
_mode = GPS_DRIVER_MODE_UBX;
px4_usleep(500000); // tried all possible drivers. Wait a bit before next round
break;
default:
break;
}
} else {
px4_usleep(500000);
}
}
PX4_INFO("exiting");
if (_serial_fd >= 0) {
::close(_serial_fd);
_serial_fd = -1;
}
}
二、receive函数
以Ashtech驱动为例：
int GPSDriverAshtech::receive(unsigned timeout)
{
{
存放从GPS口读取的数据的数组，大小为150个字节
uint8_t buf[GPS_READ_BUFFER_SIZE];
uint64_t time_started = gps_absolute_time(); int j = 0; int bytes_count = 0;
while大循环解析数据
while (true) {
bytes_count就是从GPS口读取的数据的字节数，将接收到的字节通过parseChar函数传递给_rx_buffer，parseChar函数见第三节。
while (j < bytes_count) {
int l = 0;
if ((l = parseChar(buf[j])) > 0) {
利用_rx_buffer解析数据得到经纬度等信息，handleMessage函数见第四节
int ret = handleMessage(l);
if (ret > 0) {
return ret;
}
}
j++;
}
经过上面的while循环后，所有读取到的数据已经解析完毕，将bytes_count和j置0.
j = bytes_count = 0;
根据设置的超时时间的两倍，读取串口缓冲区的数据
int ret = read(buf, sizeof(buf), timeout * 2);
读取失败
if (ret < 0) {
return -1;
}
未读到数据
else if (ret == 0) {
}
正常读取，赋值bytes_count给上面的循环使用
else if (ret > 0) {
bytes_count = ret;
}
超时返回-1
if (time_started + timeout * 1000 * 2 < gps_absolute_time()) {
return -1;
}
}
}
}
三、parseChar函数
parseChar函数主要是把GPS端口读取到的字节b赋值到 解析数据用的数组
int GPSDriverAshtech::parseChar(uint8_t b)
{
返回值
int iRet = 0;
状态机
switch (_decode_state) {
一帧NEMA数据（以GPGGA语句为例）的格式如下，以符号$开头，后面跟的是具体的那个数据，如GPGGA，GPGST等，相当于MAVLINK协议的MSGID，不同的语句的内容含义不同，最后一个*号后面跟的是校验位，采用异或校验
$GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
NEMA格式的第一个数据为符号$，在uninit状态下，如果收到该符号，说明收到一帧新的语句，将_rx_buffer_bytes置零，从头开始赋值数组_rx_buffer。将状态置为got_sync1。
case NMEADecodeState::uninit:
if (b == '$') {
_decode_state = NMEADecodeState::got_sync1;
_rx_buffer_bytes = 0;
_rx_buffer[_rx_buffer_bytes++] = b;
如果是RTCM格式的数据，通过addByte函数将数据赋值到解析RTCM的数组中，一般NEMA协议用于实时定位结果数据，RTCM协议用于实时RTK改正数据，因此发给飞控的一般是NEMA数据，不会进行下面的这个判断
} else if (b == RTCM3_PREAMBLE && _rtcm_parsing) {
_decode_state = NMEADecodeState::decode_rtcm3;
_rtcm_parsing->addByte(b);
}
break;
如果状态是got_sync1，说明说明已经收到一帧新的语句的开头，将_rx_buffer_bytes值为零。并开始对数组_rx_buffer进行赋值
case NMEADecodeState::got_sync1:
if (b == '$') {
_decode_state = NMEADecodeState::got_sync1;
_rx_buffer_bytes = 0;
如果收到*号，说明后面就是语句的校验位，将状态置为got_asteriks，并对语句的内容进行异或校验
} else if (b == '*') {
_decode_state = NMEADecodeState::got_asteriks;
}
数组_rx_buffer溢出处理。
if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
ASH_DEBUG("buffer overflow");
_decode_state = NMEADecodeState::uninit;
_rx_buffer_bytes = 0;
其他情况就对数组_rx_buffer进行正常填充
} else {
_rx_buffer[_rx_buffer_bytes++] = b;
}
break;
*号后面跟着的就是第一个校验位字节，将状态置为got_first_cs_byte
case NMEADecodeState::got_asteriks: _rx_buffer[_rx_buffer_bytes++] = b; _decode_state = NMEADecodeState::got_first_cs_byte; break;
取数组中间的有效数据用于后面的校验，就是_rx_buffer + 1到_rx_buffer + _rx_buffer_bytes - 3之间的数据
case NMEADecodeState::got_first_cs_byte: {
_rx_buffer[_rx_buffer_bytes++] = b;
uint8_t checksum = 0;
uint8_t *buffer = _rx_buffer + 1;
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
进行异或校验
for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
判断上一步求出的校验值是否等于语句中的校验值，由于上一步算出来的校验值是16进制数，NEMA语句中的值是ASCII码，因此需要进行转换，将16进制数转换成ASCII码，先进行了移位操作，取前半个字节和后半个字节，然后转换成ASCII码，例如将0xAB，转换成字母A和字母B，转换函数HEXDIGIT_CHAR定义如下：
#define HEXDIGIT_CHAr(d) ((char)((d) + (((d) < 0xA) ? ‘0’ : ‘A’-0xA)))
如果校验通过，则返回=数组长度_rx_buffer_bytes，用于后面的解析
if ((HEXDIGIT_CHAr(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
(HEXDIGIT_CHAr(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
iRet = _rx_buffer_bytes;
}
decodeInit();
}
break;
如果是RTCM的协议，将数据通过addByte加入到解析RTCM数据的数组中。原理类似，这里不做介绍
case NMEADecodeState::decode_rtcm3:
if (_rtcm_parsing->addByte(b)) {
ASH_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength());
gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength());
decodeInit();
}
break;
}
return iRet;
}
四、handleMessage函数
handleMessage函数主要是上局上一节parseChar函数赋值的数组_rx_buffer解析出具体的GPS经纬度等信息
int GPSDriverAshtech::handleMessage(int len)
{
char *endp;
数据长度小于7则返回
if (len < 7) {
return 0;
}
统计逗号数量
int uiCalcComma = 0;
for (int i = 0 ; i < len; i++) {
if (_rx_buffer[i] == ',') { uiCalcComma++; }
}
从_rx_buffer的第七个字节开始取数据
char *bufptr = (char *)(_rx_buffer + 6); int ret = 0;
下面就是根据语句的头判断是哪个NEMA语句，然后根据不同的语句类型，进行解析，得到语句中的有效载荷，原理和MAVLINK协议类似。
解析ZDA语句，ZDA语句中包含的数据可以在注释中查看，包括下面这些数据
0 Message ID $GPZDA
1 UTC
2 Day, ranging between 01 and 31
3 Month, ranging between 01 and 12
4 Year
5 Local time zone offset from GMT, ranging from 00 through 13 hours
6 Local time zone offset from GMT, ranging from 00 through 59 minutes
7 The checksum data, always begins with *
具体的解析方法就是用strtol函数将数据的ASCII转换成long型(或者其他的如strtod将ASCII转换成double型,原理类似)
以下面语句为例
long int strtol(const char *nptr, char **endptr, int base)
strtol()会将nptr指向的字符串，根据参数base，按权转化为long int, 然后返回这个值。参数base的范围为2~36,和0；它决定了字符串以被转换为整数的权值。
可以被转换的合法字符依据base而定，举例来说，当base为2时，合法字符为‘0’，‘1’；base为8时，合法字符为‘0’，‘1’，……‘7’；base为10时，合法字符为‘0’，‘1’，……‘9’；当字符合法时，‘0’，……‘9’依次被转换为十进制的0～9。
strtol()函数检测到第一个非法字符时，立即停止检测，其后的所有字符都会被当作非法字符处理。
合法字符串会被转换为long int, 作为函数的返回值。
非法字符串，即从第一个非法字符的地址，被赋给endptr。
**endptr是个双重指针，即指针的指针。strtol()函数就是通过它改变endptr的值，即把第一个非法字符的地址传给endptr。
由于NEMA语句的不同的数据之间通过逗号’,‘隔开，因此逗号’,‘就会被当成非法字符，从而经过一次strtol()函数转换后，就会指向下一个数据前面的逗号’,‘，bufptr = endp相当于把当前指针指向了下一个需要处理的数据前面的逗号’,‘，因此经过bufptr && *(++bufptr) != ','之后，相当于把指针bufptr向后移了移位指向了下一个需要转换的数据的地址，并且确保其数据不是逗号’,'，此时再次利用strtol()函数进行转换，这样一直进行装换即可将这一条数据中的所有有效数据转换完毕，最终完成了GPS数据的解析，通过uorb发布，后面其他语句的解析原理同相同
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0;
ASH_UNUSED(local_time_off_min);
ASH_UNUSED(local_time_off_hour);
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
int ashtech_hour = static_cast(ashtech_time / 10000);
int ashtech_minute = static_cast((ashtech_time - ashtech_hour * 10000) / 100);
double ashtech_sec = static_cast(ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100);
struct tm timeinfo = {};
timeinfo.tm_year = year - 1900;
timeinfo.tm_mon = month - 1;
timeinfo.tm_mday = day;
timeinfo.tm_hour = ashtech_hour;
timeinfo.tm_min = ashtech_minute;
timeinfo.tm_sec = int(ashtech_sec);
timeinfo.tm_isdst = 0;
#ifndef NO_MKTIME
time_t epoch = mktime(&timeinfo);
if (epoch > GPS_EPOCH_SECS) {
uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1000000;
// FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
// and control its drift. Since we rely on the HRT for our monotonic
// clock, updating it from time to time is safe.
timespec ts{};
ts.tv_sec = epoch;
ts.tv_nsec = usecs * 1000;
setClock(ts);
_gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL;
_gps_position->time_utc_usec += usecs;
} else {
_gps_position->time_utc_usec = 0;
}
#else
_gps_position->time_utc_usec = 0;
#endif
_last_timestamp_time = gps_absolute_time();
}
GGA分为GPGGA和GNGGA，前者是单模GPS，后者是多模联合定位，这条语句比较重要，里面包含了经纬度等主要信息
else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14) && !_got_pashr_pos_message) {
double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv = 0, fix_quality = 0;
double hdop = 99.9;
char ns = '?', ew = '?';
ASH_UNUSED(ashtech_time);
ASH_UNUSED(num_of_sv);
ASH_UNUSED(hdop);
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = static_cast((int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lon = static_cast((int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast(alt * 1000);
_rate_count_lat_lon++;
if (fix_quality <= 0) {
_gps_position->fix_type = 0;
} else {
if (fix_quality == 5) { fix_quality = 3; }
_gps_position->fix_type = 3 + fix_quality - 1;
}
_gps_position->timestamp = gps_absolute_time();
_gps_position->vel_m_s = 0;
_gps_position->vel_n_m_s = 0;
_gps_position->vel_e_m_s = 0;
_gps_position->vel_d_m_s = 0;
_gps_position->cog_rad =
0;
_gps_position->vel_ned_valid = true;
_gps_position->c_variance_rad = 0.1f;
ret = 1;
} else if (memcmp(_rx_buffer, "$GPHDT,", 7) == 0 && uiCalcComma == 2) {
float heading = 0.f;
if (bufptr && *(++bufptr) != ',') {
heading = strtof(bufptr, &endp); bufptr = endp;
ASH_DEBUG("heading update: %.3f", (double)heading);
heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi]
heading -= _heading_offset; // range: [-pi, 3pi]
if (heading > M_PI_F) {
heading -= 2.f * M_PI_F; // final range is [-pi, pi]
}
_gps_position->heading = heading;
}
}
后面的解析方法都类似，不做赘述
else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
_got_pashr_pos_message = true;
bufptr = (char *)(_rx_buffer + 10);
int coordinatesFound = 0;
double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv = 0, fix_quality = 0;
double track_true = 0.0, ground_speed = 0.0, age_of_corr = 0.0;
double hdop = 99.9, vdop = 99.9, pdop = 99.9, tdop = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
ASH_UNUSED(ashtech_time);
ASH_UNUSED(num_of_sv);
ASH_UNUSED(age_of_corr);
ASH_UNUSED(pdop);
ASH_UNUSED(tdop);
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') {
lat = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
lon = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
alt = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = static_cast((static_cast(lat * 0.01) + static_cast(lat * 0.01 - int(
lat * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->lon = static_cast((static_cast(lon * 0.01) + static_cast(lon * 0.01 - int(
lon * 0.01)) * 100.0 / 60.0) * 10000000);
_gps_position->alt = static_cast(alt * 1000);
_gps_position->hdop = static_cast(hdop);
_gps_position->vdop = static_cast(vdop);
_rate_count_lat_lon++;
if (coordinatesFound < 3) {
_gps_position->fix_type = 0;
} else {
if (fix_quality == 9 || fix_quality == 10) { // SBAS differential or BeiDou differential
_gps_position->fix_type = 4; // use RTCM differential
} else if (fix_quality == 12 || fix_quality == 22) { // RTK float or RTK float dithered
_gps_position->fix_type = 5;
} else if (fix_quality == 13 || fix_quality == 23) { // RTK fixed or RTK fixed dithered
_gps_position->fix_type = 6;
} else {
_gps_position->fix_type = 3 + fix_quality;
}
// we got a valid position, activate correction output if needed
if (_configure_done && _output_mode == OutputMode::RTCM &&
_board == AshtechBoard::trimble_mb_two && !_correction_output_activated) {
activateCorrectionOutput();
}
}
_gps_position->timestamp = gps_absolute_time();
float track_rad = static_cast(track_true) * M_PI_F / 180.0f;
float velocity_ms = static_cast(ground_speed) / 1.9438445f;
float velocity_north = static_cast(velocity_ms) * cosf(track_rad);
float velocity_east = static_cast(velocity_ms) * sinf(track_rad);
_gps_position->vel_m_s = velocity_ms;
_gps_position->vel_n_m_s = velocity_north;
_gps_position->vel_e_m_s = velocity_east;
_gps_position->vel_d_m_s = static_cast(-vertic_vel);
_gps_position->cog_rad =
track_rad;
_gps_position->vel_ned_valid = true;
_gps_position->c_variance_rad = 0.1f;
_rate_count_vel++;
ret = 1;
} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
double ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
double min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
ASH_UNUSED(ashtech_time);
ASH_UNUSED(min_err);
ASH_UNUSED(maj_err);
ASH_UNUSED(deg_from_north);
ASH_UNUSED(rms_err);
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
_gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err)
+ static_cast(lon_err) * static_cast(lon_err));
_gps_position->epv = static_cast(alt_err);
_gps_position->s_variance_m_s = 0;
} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
bool bGPS = false;
if (memcmp(_rx_buffer, "$GP", 3) != 0) {
return 0;
} else {
bGPS = true;
}
int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
struct gsv_sat {
int svid;
int elevation;
int azimuth;
int snr;
int prn;
} sat[4];
memset(sat, 0, sizeof(sat));
if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
return 0;
}
if (this_msg_num == 0 && bGPS && _satellite_info) {
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
memset(_satellite_info->prn, 0, sizeof(_satellite_info->prn));
}
int end = 4;
if (this_msg_num == all_msg_num) {
end = tot_sv_visible - (this_msg_num - 1) * 4;
_gps_position->satellites_used = tot_sv_visible;
if (_satellite_info) {
_satellite_info->count = MIN(tot_sv_visible, satellite_info_s::SAT_INFO_MAX_SATELLITES);
_satellite_info->timestamp = gps_absolute_time();
ret = 2;
}
}
if (_satellite_info) {
for (int y = 0 ; y < end ; y++) {
if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
_satellite_info->used[y + (this_msg_num - 1) * 4] = (sat[y].snr > 0);
_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
_satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth;
_satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr;
_satellite_info->prn[y + (this_msg_num - 1) * 4] = sat[y].prn;
}
}
} else if (memcmp(_rx_buffer, "$PASHR,NAK", 10) == 0) {
ASH_DEBUG("Nack received");
if (_command_state == NMEACommandState::waiting) {
_command_state = NMEACommandState::nack;
}
} else if (memcmp(_rx_buffer, "$PASHR,ACK", 10) == 0) {
ASH_DEBUG("Ack received");
if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::Acked) {
_command_state = NMEACommandState::received;
}
} else if (memcmp(_rx_buffer, "$PASHR,PRT,", 11) == 0 && uiCalcComma == 3) {
if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::PRT) {
_command_state = NMEACommandState::received;
_port = _rx_buffer[11];
ASH_DEBUG("Connected port: %c", _port);
}
} else if (memcmp(_rx_buffer, "$PASHR,RID,", 11) == 0) {
if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RID) {
_command_state = NMEACommandState::received;
if (memcmp(_rx_buffer + 11, "MB2", 3) == 0) {
_board = AshtechBoard::trimble_mb_two;
} else {
_board = AshtechBoard::other;
}
ASH_DEBUG("Connected board: %i", (int)_board);
}
} else if (memcmp(_rx_buffer, "$PASHR,RECEIPT,", 15) == 0) {
// this is the response to $PASHS,POS,AVG,100
// example: $PASHR,RECEIPT,POS,AVG,STARTED,INTERVAL,100,114502.56,28.12.2011
if (_command_state == NMEACommandState::waiting && _waiting_for_command == NMEACommand::RECEIPT) {
_command_state = NMEACommandState::received;
}
// when finished we get one of the follwing messages:
// - successful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,114642.81,28.12.2011,5542.5178481,N,03739.2954994,E,176.334,OK,CONTINUOUS,100.20*09
// - unsuccessful: $PASHR,RECEIPT,POS,AVG,100,FINISHED,124628.01,28.12.2011,ERR
char *finished_find = strstr((char *)_rx_buffer, "FINISHED,");
if (finished_find) {
const bool error = strstr((const char *)_rx_buffer, "ERR");
_survey_in_start = 0;
if (error) {
sendSurveyInStatusUpdate(false, false);
} else {
// extract the position
double lat = 0., lon = 0.;
float alt = 0.f;
char ns = '?', ew = '?';
bufptr = finished_find + 9; // skip over FINISHED,
// skip the next 2 arguments
bufptr = strstr(bufptr, ",");
if (bufptr) { bufptr = strstr(bufptr + 1, ","); }
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') { lat = -lat; }
if (ew == 'W') { lon = -lon; }
lat = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0;
lon = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0;
sendSurveyInStatusUpdate(false, true, lat, lon, alt);
activateRTCMOutput(true);
}
}
}
if (ret == 1) {
_gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp);
}
// handle survey-in status update
if (_survey_in_start != 0) {
const gps_abstime now = gps_absolute_time();
uint32_t survey_in_duration = (now - _survey_in_start) / 1000000;
if (survey_in_duration != _base_settings.settings.survey_in.min_dur) {
_base_settings.settings.survey_in.min_dur = survey_in_duration;
sendSurveyInStatusUpdate(true, false);
}
}
return ret;
}
