rtu_linux_modules/air530z/air530z.c

#include "air530z.h"
#include "../swapi/subjects/serial/serial.h"
#include "./nmealib/include/nmea/nmea.h"

// 模块名称
static const char MODULE_NAME[] = "Air530Z";

// 设置输出频率
static const char *PCAS02 = "$PCAS02,1000*2E\r\n";
// 设置输出格式
static const char *PCAS03 = "$PCAS03,1,0,0,1,1,0,0,0,0,0,,,0,0*03\r\n";
// 设置卫星组合
static const char *PCAS04 = "$PCAS04,13,13,11*29\r\n";
// 设置接收模式: "静态模式"
static const char *PCAS11 = "$PCAS11,1*1C\r\n";

// 定义关键控制符号
#define LF '\n' // 换行

// 输出控制精度阈值
#define HDOP_MAX               2.0 // 水平精度判定阈值
#define MIN_SATELLITES         5   // 最小可见卫星(个)
#define STATIONARY_RADIUS      3.0 // 静止判定半径(米)
#define CONT_STATIONARY_FRAMES 4   // 累计连续静止帧数
#define POS_SMOOTHING_NORMAL   0.6 // 正常定位滑动系数
#define POS_SMOOTHING_WEAK     0.2 // 弱定位时滑动系数
#define CONT_WEAK_FRAMES       8   // 连续弱定位的帧数

// 定义无效的坐标值
#define COORD_NAN NAN
#define IS_COORD_NAN(x) isnan(x)

// 与模块的串口通讯
typedef struct
{
  void *h; // 打开的串口句柄

  nmeaINFO info;     // decoded NMEA data
  nmeaPARSER parser; // NMEA decoder解码器
  struct
  { // 存储经过平滑处理的经纬度坐标(滤波轨迹)
    double lat;
    double lon;
    int stationary_cnt; // 连续静止帧数统计
    int weak_cnt; // 连续弱定位帧数统计
  } filter;

  struct
  {
    bool bOK; // 定位是否成功
    char lat[MAX_LINE_CHARS]; // 纬度字符串
    char lon[MAX_LINE_CHARS]; // 经度字符串
  } outPos2d;
  void *out_rwlock;
} SAir530ZCom;

static SAir530ZCom s_myCom;

// 接收、处理来自Air530Z模块的数据报文帧, 串口-线程回调
static int comio_data_recv_proc(unsigned long wParam/*传递打开的串口句柄*/, unsigned long lParam/*保留暂未使用*/)
{
  SAir530ZCom *pComIO = &s_myCom; void *pSerial = pComIO->h;
  const unsigned char *pRecvBuf = serial_get_recv_buffer(pSerial); int nRecvBytes = serial_get_recv_buffer_bytes(pSerial);
  const char *log_prefix = serial_get_log_prefix(pSerial); int ret; bool bWeakPos;

  if (nRecvBytes <= 0 || pRecvBuf[nRecvBytes - 1] != LF) goto ret_p;

  sw_log_trace("[%s] %s received a frame(%d bytes): %s", MODULE_NAME, log_prefix, nRecvBytes, pRecvBuf);

  ret = nmea_parse(&pComIO->parser, (const char *)pRecvBuf, nRecvBytes, &pComIO->info);
  serial_clear_recv_buffer(pSerial); // 当前数据帧已解析, 清空接收缓存区, 等待下一数据帧到来
  if(ret <= 0) { /*sw_log_error("[%s] failed to parse a NMEA frame!!", MODULE_NAME);*/ goto ret_p; }

  if(pComIO->info.sig > 0)
  {
    if(pComIO->info.HDOP <= HDOP_MAX && pComIO->info.satinfo.inview >= MIN_SATELLITES) { bWeakPos = false; pComIO->filter.weak_cnt = 0; }
    else { bWeakPos = true; pComIO->filter.weak_cnt++; }

    double lat = pComIO->info.lat, lon = pComIO->info.lon;

    if(IS_COORD_NAN(pComIO->filter.lat) || IS_COORD_NAN(pComIO->filter.lon))
    {
      pComIO->filter.lat = lat;
      pComIO->filter.lon = lon;
    }

    nmeaPOS pos1, pos2;
    nmea_info2pos(&pComIO->info, &pos1);
    pos2.lat = nmea_ndeg2radian(pComIO->filter.lat);
    pos2.lon = nmea_ndeg2radian(pComIO->filter.lon);

    double dist = nmea_distance(&pos2, &pos1);
    if(dist < STATIONARY_RADIUS) { pComIO->filter.stationary_cnt++; }
    else
    {
      pComIO->filter.stationary_cnt = 0;
      if(bWeakPos) pComIO->filter.lat = pComIO->filter.lon = COORD_NAN;
      else { pComIO->filter.lat = lat; pComIO->filter.lon = lon; }
    }

    if(pComIO->filter.stationary_cnt >= CONT_STATIONARY_FRAMES)
    { /////静止状态处理
      // 1-应用滑动平均
      double smoothing = bWeakPos ? POS_SMOOTHING_WEAK : POS_SMOOTHING_NORMAL;
      pComIO->filter.lat = smoothing * lat + (1 - smoothing) * pComIO->filter.lat;
      pComIO->filter.lon = smoothing * lon + (1 - smoothing) * pComIO->filter.lon;

      // 2-输出成字符串
      double out_lat = nmea_ndeg2degree(pComIO->filter.lat); double out_lon = nmea_ndeg2degree(pComIO->filter.lon);

      sw_rwlock_wrlock(pComIO->out_rwlock, WAIT_FOREVER);
      sprintf(pComIO->outPos2d.lat, "%.6f", out_lat);
      sprintf(pComIO->outPos2d.lon, "%.6f", out_lon);
      pComIO->outPos2d.bOK = true;
      sw_rwlock_unlock(pComIO->out_rwlock);

      // 3-打印定位结果
      sw_log_trace("[%s] 已定位: lat=%.6f, lon=%.6f, HDOP=%.1f, Sats=%d/%d, DeltaMeters=%.1fm, IsWeakPos=%d", MODULE_NAME, \
                   out_lat, out_lon, pComIO->info.HDOP, \
                   pComIO->info.satinfo.inuse, pComIO->info.satinfo.inview, dist, bWeakPos);
    }

    if(pComIO->filter.weak_cnt >= CONT_WEAK_FRAMES)
    {
      pComIO->filter.weak_cnt = pComIO->filter.stationary_cnt = 0;
      pComIO->filter.lat = pComIO->filter.lon = COORD_NAN;
    }
  }

ret_p: return 1; // 持续累加的接收新数据
}

// 打开与模块的通讯, 返回: 0成功, <0时失败
int Air530Z_ComInit()
{
  nmea_zero_INFO(&s_myCom.info);
  s_myCom.filter.lat = s_myCom.filter.lon = COORD_NAN;
  nmea_parser_init(&s_myCom.parser);

  s_myCom.out_rwlock = sw_rwlock_create();
  if(!s_myCom.out_rwlock)
  {
    sw_log_error("[%s] failed to create the \"outPos2d\" rwlock!!", MODULE_NAME);
    Air530Z_ComExit(); return -1;
  }

#ifdef _DEBUG // 上位机单元测试时使用
  const char *serialName = "/dev/ttyS0";   int baudrate = 9600;
#else
  const char *serialName = "/dev/ttymxc6"; int baudrate = 9600;
#endif
  const char *parityCheck = "none"; // 无校检
  s_myCom.h = serial_open(serialName, baudrate, parityCheck, \
                          comio_data_recv_proc, comio_data_recv_proc, NULL);
  if(!s_myCom.h)
  {
    sw_log_error("[%s] failed to open the \"%s:%d(%s parity)\" device!!", \
                 MODULE_NAME, serialName, baudrate, parityCheck);
    Air530Z_ComExit(); return -2;
  }

  serial_send_data(s_myCom.h, (unsigned char *)PCAS02, strlen(PCAS02));
  serial_send_data(s_myCom.h, (unsigned char *)PCAS03, strlen(PCAS03));
  serial_send_data(s_myCom.h, (unsigned char *)PCAS04, strlen(PCAS04));
  serial_send_data(s_myCom.h, (unsigned char *)PCAS11, strlen(PCAS11));

  return 0;
}

// 关闭与模块的通讯, 返回: 0成功, <0时失败
int Air530Z_ComExit()
{
  if(s_myCom.h) serial_close(s_myCom.h, WAITTHRD_SAFEEXIT_TIMEOUT);
  if(s_myCom.out_rwlock) sw_rwlock_destroy(s_myCom.out_rwlock);
  if(s_myCom.parser.buffer) nmea_parser_destroy(&s_myCom.parser);
  memset(&s_myCom, 0, sizeof(s_myCom));
  return 0;
}

// 获取当前的2D位置, 返回: 0成功, -1未定位
int Air530Z_GetPos2D(char lat[MAX_LINE_CHARS], char lon[MAX_LINE_CHARS])
{
  bool bOK = false;
  sw_rwlock_rdlock(s_myCom.out_rwlock, WAIT_FOREVER);
  bOK = s_myCom.outPos2d.bOK;
  if(bOK) {
    strcpy(lat, s_myCom.outPos2d.lat);
    strcpy(lon, s_myCom.outPos2d.lon);
  }
  sw_rwlock_unlock(s_myCom.out_rwlock);
  return bOK ? 0 : -1;
}

#ifdef _DEBUG
void *Air530Z_GetSerialHandle()
{
  return s_myCom.h;
}
#endif