modularization/src/ui/ui_ads_hmi/gps_collect.cpp

#include "gps_collect.h"

// General constants.
#define NOUTPUT_PACKET_LENGTH  (72)       //!< NCom packet length.
#define NCOM_SYNC           (0xE7)        //!< NCom sync byte.
#define PKT_PERIOD          (0.01)        //!< 10ms updates.
#define TIME2SEC            (1e-3)        //!< Units of 1 ms.
#define FINETIME2SEC        (4e-6)        //!< Units of 4 us.
#define TIMECYCLE           (60000)       //!< Units of TIME2SEC (i.e. 60 seconds).
#define WEEK2CYCLES         (10080)       //!< Time cycles in a week.
#define ACC2MPS2            (1e-4)        //!< Units of 0.1 mm/s^2.
#define RATE2RPS            (1e-5)        //!< Units of 0.01 mrad/s.
#define VEL2MPS             (1e-4)        //!< Units of 0.1 mm/s.
#define ANG2RAD             (1e-6)        //!< Units of 0.001 mrad.
#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
#define POSA2M              (1e-3)        //!< Units of 1 mm.
#define VELA2MPS            (1e-3)        //!< Units of 1 mm/s.
#define ANGA2RAD            (1e-5)        //!< Units of 0.01 mrad.
#define GB2RPS              (5e-6)        //!< Units of 0.005 mrad/s.
#define AB2MPS2             (1e-4)        //!< Units of 0.1 mm/s^2.
#define GSFACTOR            (1e-6)        //!< Units of 1 ppm.
#define ASFACTOR            (1e-6)        //!< Units of 1 ppm.
#define GBA2RPS             (1e-6)        //!< Units of 0.001 mrad/s.
#define ABA2MPS2            (1e-5)        //!< Units of 0.01 mm/s^2.
#define GSAFACTOR           (1e-6)        //!< Units of 1 ppm.
#define ASAFACTOR           (1e-6)        //!< Units of 1 ppm.
#define GPSPOS2M            (1e-3)        //!< Units of 1 mm.
#define GPSATT2RAD          (1e-4)        //!< Units of 0.1 mrad.
#define GPSPOSA2M           (1e-4)        //!< Units of 0.1 mm.
#define GPSATTA2RAD         (1e-5)        //!< Units of 0.01 mrad.
#define INNFACTOR           (0.1)         //!< Resolution of 0.1.
#define DIFFAGE2SEC         (1e-2)        //!< Units of 0.01 s.
#define REFPOS2M            (0.0012)      //!< Units of 1.2 mm.
#define REFANG2RAD          (1e-4)        //!< Units of 0.1 mrad.
#define OUTPOS2M            (1e-3)        //!< Units of 1 mm.
#define ZVPOS2M             (1e-3)        //!< Units of 1 mm.
#define ZVPOSA2M            (1e-4)        //!< Units of 0.1 mm.
#define NSPOS2M             (1e-3)        //!< Units of 1 mm.
#define NSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
#define ALIGN2RAD           (1e-4)        //!< Units of 0.1 mrad.
#define ALIGNA2RAD          (1e-5)        //!< Units of 0.01 mrad.
#define SZVDELAY2S          (1.0)         //!< Units of 1.0 s.
#define SZVPERIOD2S         (0.1)         //!< Units of 0.1 s.
#define TOPSPEED2MPS        (0.5)         //!< Units of 0.5 m/s.
#define NSDELAY2S           (0.1)         //!< Units of 0.1 s.
#define NSPERIOD2S          (0.02)        //!< Units of 0.02 s.
#define NSACCEL2MPS2        (0.04)        //!< Units of 0.04 m/s^2.
#define NSSPEED2MPS         (0.1)         //!< Units of 0.1 m/s.
#define NSRADIUS2M          (0.5)         //!< Units of 0.5 m.
#define INITSPEED2MPS       (0.1)         //!< Units of 0.1 m/s.
#define HLDELAY2S           (1.0)         //!< Units of 1.0 s.
#define HLPERIOD2S          (0.1)         //!< Units of 0.1 s.
#define STATDELAY2S         (1.0)         //!< Units of 1.0 s.
#define STATSPEED2MPS       (0.01)        //!< Units of 1.0 cm/s.
#define WSPOS2M             (1e-3)        //!< Units of 1 mm.
#define WSPOSA2M            (1e-4)        //!< Units of 0.1 mm.
#define WSSF2PPM            (0.1)         //!< Units of 0.1 pulse per metre (ppm).
#define WSSFA2PC            (0.002)       //!< Units of 0.002% of scale factor.
#define WSDELAY2S           (0.1)         //!< Units of 0.1 s.
#define WSNOISE2CNT         (0.1)         //!< Units of 0.1 count for wheel speed noise.
#define UNDUL2M             (0.005)       //!< Units of 5 mm.
#define DOPFACTOR           (0.1)         //!< Resolution of 0.1.
#define OMNISTAR_MIN_FREQ   (1.52e9)      //!< (Hz) i.e. 1520.0 MHz.
#define OMNIFREQ2HZ         (1000.0)      //!< Resolution of 1 kHz.
#define SNR2DB              (0.2)         //!< Resolution of 0.2 dB.
#define LTIME2SEC           (1.0)         //!< Resolution of 1.0 s.
#define TEMPK_OFFSET        (203.15)      //!< Temperature offset in degrees K.
#define ABSZERO_TEMPC       (-273.15)     //!< Absolute zero (i.e. 0 deg K) in deg C.

// For more accurate and complete local coordinates
#define FINEANG2RAD (1.74532925199433e-9) //!< Units of 0.1 udeg.
#define ALT2M               (1e-3)        //!< Units of 1 mm.

// For GPS supply voltage
#define SUPPLYV2V           (0.1)         //!< Units of 0.1 V.

// Mathematical constant definitions
#ifndef M_PI
#define M_PI (3.1415926535897932384626433832795)  //!< Pi.
#endif
#define DEG2RAD             (M_PI/180.0)  //!< Convert degrees to radians.
#define RAD2DEG             (180.0/M_PI)  //!< Convert radians to degrees.
#define POS_INT_24          (8388607)     //!< Maximum value of a two's complement 24 bit integer.
#define NEG_INT_24          (-8388607)    //!< Minimum value of a two's complement 24 bit integer.
#define INV_INT_24          (-8388608)    //!< Represents an invalid two's complement 24 bit integer.

#define NCOM_COUNT_TOO_OLD  (150)         //!< Cycle counter for data too old.
#define NCOM_STDCNT_MAX     (0xFF)        //!< Definition for the RTBNS accuracy counter.
#define MIN_HORZ_SPEED      (0.07)        //!< 0.07 m/s hold distance.
#define MIN_VERT_SPEED      (0.07)        //!< 0.07 m/s hold distance.
#define SPEED_HOLD_FACTOR   (2.0)         //!< Hold distance when speed within 2 sigma of 0.
#define MINUTES_IN_WEEK     (10080)       //!< Number of minutes in a week.

// OmniStar status definitions
#define NCOM_OMNI_STATUS_UNKNOWN      (0xFF)
#define NCOM_OMNI_STATUS_VBSEXPIRED   (0x01)
#define NCOM_OMNI_STATUS_VBSREGION    (0x02)
#define NCOM_OMNI_STATUS_VBSNOBASE    (0x04)
#define NCOM_OMNI_STATUS_HPEXPIRED    (0x08)
#define NCOM_OMNI_STATUS_HPREGION     (0x10)
#define NCOM_OMNI_STATUS_HPNOBASE     (0x20)
#define NCOM_OMNI_STATUS_HPNOCONVERGE (0x40)
#define NCOM_OMNI_STATUS_HPKEYINVALID (0x80)

// GPS hardware status definitions
#define NCOM_GPS_ANT_STATUS_BITMASK   (0x03)
#define NCOM_GPS_ANT_STATUS_DONTKNOW  (0x03)
#define NCOM_GPS_ANT_STATUS_BITSHIFT  (0)
#define NCOM_GPS_ANT_POWER_BITMASK    (0x0C)
#define NCOM_GPS_ANT_POWER_DONTKNOW   (0x0C)
#define NCOM_GPS_ANT_POWER_BITSHIFT   (2)

// GPS feature set 1 definitions
#define NCOM_GPS_FEATURE_PSRDIFF      (0x01)
#define NCOM_GPS_FEATURE_SBAS         (0x02)
#define NCOM_GPS_FEATURE_OMNIVBS      (0x08)
#define NCOM_GPS_FEATURE_OMNIHP       (0x10)
#define NCOM_GPS_FEATURE_L1DIFF       (0x20)
#define NCOM_GPS_FEATURE_L1L2DIFF     (0x40)

// GPS feature set 2 definitions
#define NCOM_GPS_FEATURE_GLONASS      (0x01)
#define NCOM_GPS_FEATURE_GALILEO      (0x02)
#define NCOM_GPS_FEATURE_RAWRNG       (0x04)
#define NCOM_GPS_FEATURE_RAWDOP       (0x08)
#define NCOM_GPS_FEATURE_RAWL1        (0x10)
#define NCOM_GPS_FEATURE_RAWL2        (0x20)
#define NCOM_GPS_FEATURE_RAWL5        (0x40)

// GPS feature valid definition
#define NCOM_GPS_FEATURE_VALID        (0x80)

// The start of GPS time in a time_t style. In this version it is a constant, but this constant assumes that
// the local machine uses 00:00:00 01/01/1970 as its Epoch time. If your machine is different then you need to
// convert 00:00:00 06/01/1980 in to the local machine's time_t time.
#define GPS_TIME_START_TIME_T         (315964800)

// Second order filter class
#define INPUT_JITTER_TOLERANCE     (0.01)  // i.e. 1%


/*index*/
#define		PI_SYNC             0
#define		PI_TIME             1
#define		PI_ACCEL_X          3
#define		PI_ACCEL_Y          6
#define		PI_ACCEL_Z          9
#define		PI_ANG_RATE_X      12
#define		PI_ANG_RATE_Y      15
#define		PI_ANG_RATE_Z      18
#define		PI_INS_NAV_MODE    21
#define		PI_CHECKSUM_1      22
#define		PI_POS_LAT         23
#define		PI_POS_LON         31
#define		PI_POS_ALT         39
#define		PI_VEL_N           43
#define		PI_VEL_E           46
#define		PI_VEL_D           49
#define		PI_ORIEN_H         52
#define		PI_ORIEN_P         55
#define		PI_ORIEN_R         58
#define		PI_CHECKSUM_2      61
#define		PI_CHANNEL_INDEX   62
#define		PI_CHANNEL_STATUS  63
#define		PI_SAT_NUM		   67
#define		PI_RTK_STATUS	   68
#define		PI_CHECKSUM_3      71

/*RTK IMU status check*/
#define		RTK_IMU_OK		0
#define		IMU_STATUS_ERR	1
#define		RTK_STATUS_ERR	2
#define		UNKNOWN			0xFF

struct GPS_INS
{
    int valid = 0xff;
    int index = 0;	//gps点序号

    double gps_lat = 0;//纬度
    double gps_lng = 0;//经度

    double gps_x = 0;
    double gps_y = 0;
    double gps_z = 0;

    double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
    double ins_pitch_angle = 0;	//俯仰角
    double ins_heading_angle = 0;	//航向角

    int ins_status = 0;	//惯导状态 4
    int rtk_status = 0;	//rtk状态 6 -5 -3
    int gps_satelites_num = 0;

    //-----加速度--------------
    double accel_x = 0;
    double accel_y = 0;
    double accel_z = 0;

    //-------角速度------------
    double ang_rate_x = 0;
    double ang_rate_y = 0;
    double ang_rate_z = 0;

    //-----------方向速度--------------
    double vel_N = 0;
    double vel_E = 0;
    double vel_D = 0;

    int speed_mode = 0;
    int mode2 = 0;
    double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速

};

typedef boost::shared_ptr<GPS_INS> GPSData;

class CarStatus : public boost::noncopyable {
public:
    float speed;			//车速
    std::int16_t wheel_angle;		//方向盘转角
    std::uint8_t braking_pressure;	//刹车压力
    GPSData location;		//当前车辆位置

    CarStatus() {
        speed = 0;
        braking_pressure = 0;
        wheel_angle = 0;
        location = boost::shared_ptr<GPS_INS>(new GPS_INS);
    }
    ~CarStatus() {

    }
};
typedef boost::serialization::singleton<CarStatus> CarStatusSingleton;

#define ServiceCarStatus CarStatusSingleton::get_mutable_instance()


static double   cast_8_byte_to_double(const uint8_t *b);
static int32_t cast_3_byte_to_int32(const uint8_t *b);

GPSSensor::GPSSensor() {
}

GPSSensor::~GPSSensor() {

}

void GPSSensor::start()
{
    thread_sensor_run_ = new boost::thread(boost::bind(&GPSSensor::processSensor, this));
}

void GPSSensor::stop()
{
    thread_sensor_run_->interrupt();
    thread_sensor_run_->join();
}

void GPSSensor::obs_modechange(int rec)
{
    _mtx.lock();
    obs_modes = rec;
    _mtx.unlock();
}

void GPSSensor::speed_modechange(int rec)
{
    _mtx.lock();
    speed_modes = rec;
    _mtx.unlock();
}

void GPSSensor::lane_num_modechange(int rec)
{
    _mtx.lock();
    lane_num = rec;
    _mtx.unlock();
}

void GPSSensor::lane_status_modechange(int rec)
{
    _mtx.lock();
    lane_status = rec;
    _mtx.unlock();
}

void GPSSensor::start_or_end(bool rec)
{
    _mtx2.lock();
    writegps = rec;
    _mtx2.unlock();
}

void GPSSensor::collect_modechange(bool rec)
{
    _mtx3.lock();
    is_forbidden = rec;
    _mtx3.unlock();
}

void GPSSensor::jianju_change(double rec)
{
    _mtx4.lock();
    jianju = rec;
    _mtx4.unlock();
}

bool GPSSensor::isRunning() const
{
    return (thread_sensor_run_ != NULL && !thread_sensor_run_->timed_join(boost::posix_time::milliseconds(10)));
}

void GPSSensor::processSensor()
{
    //todo  GPS/惯导 设备接口 对接
    /*Initialize udp server, listen on port 3000.*/
    /*int sockfd;
    struct sockaddr_in addr;
    socklen_t addrlen;
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0)
    {
       printf("socket failed\n");
       exit(EXIT_FAILURE);
    }
    addrlen = sizeof(struct sockaddr_in);
    bzero(&addr, addrlen);
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(3000);
    if (bind(sockfd, (struct sockaddr*)(&addr), addrlen) < 0)
    {
       printf("bind fail\n");
       exit(EXIT_FAILURE);
    }*/
    QUdpSocket *udpsocket;//summer
    udpsocket = new QUdpSocket();
    udpsocket->bind(QHostAddress::Any, 3000);
    /*    udpsocket->bind(3000);
    if(!udpsocket->waitForConnected())
    {
        printf("bind fail\n");
        exit(EXIT_FAILURE);
    }
*/
    unsigned char recvBuf[100] = { 0 };

    GPSData data(new GPS_INS);
    int x;

    ServiceCarStatus.location->gps_x = 0;
    ServiceCarStatus.location->gps_y = 0;

    std::string sk;
    std::stringstream ss;
    while (true)
    {
        ss.clear();
        ss.str("");
        sk.clear();
        gps_index = 0;
        testcount = 0;
        _mtx2.lock();
        while (writegps == false)
        {
            _mtx2.unlock();
            if(should_exit == true)
            {
                udpsocket->close();
                ready_exit++;
                return;
            }
            //boost::this_thread::sleep(boost::posix_time::milliseconds(10));

#ifdef linux
            usleep(10000);
#endif
#ifdef WIN32
            boost::this_thread::sleep(boost::posix_time::milliseconds(10));
            //         Sleep(10);
#endif
            _mtx2.lock();
        }
        std::ofstream fout;
        fout.setf(std::ios::fixed, std::ios::floatfield);  // 设定为 fixed 模式，以小数点表示浮点数
        fout.precision(12);  // 设置精度 2
        time_t now;
        struct tm *timenow;
        time(&now);
        timenow = localtime(&now);
        ss << "/home/adc/" << timenow->tm_year+1900 << "-" << timenow->tm_mon+1 << "-" << timenow->tm_mday << "-" << timenow->tm_hour << "h-" << timenow->tm_min << "m-" << timenow->tm_sec << "s.txt";
        sk = ss.str();
        fout.open(sk);
        while (writegps) {
            _mtx2.unlock();
            if(should_exit == true)
            {
                udpsocket->close();
                fout.close();
                ready_exit++;
                return;
            }
            char *buf = new char[100];
            memset(buf,0,100);
            int rec = 0;
            if(udpsocket->waitForReadyRead())
                rec = udpsocket->read(buf,100);
            convertStrToUnChar(buf,recvBuf);
            //int rec = recvfrom(sockfd, recvBuf, 100, 0, (struct sockaddr *)(&addr), &addrlen);

            if (rec != NOUTPUT_PACKET_LENGTH) {
                std::cout << "ERR: rec must be 72 bytes\n" << std::endl;
                continue;
            }

            if (recvBuf[PI_SYNC] != NCOM_SYNC) {
                std::cout << "ERR: head always be 0xE7\n" << std::endl;
                continue;
            }
            data->gps_lat = cast_8_byte_to_double(recvBuf + PI_POS_LAT) * RAD2DEG;
            data->gps_lng = cast_8_byte_to_double(recvBuf + PI_POS_LON) * RAD2DEG;

            x = cast_3_byte_to_int32(recvBuf + PI_ORIEN_H);
            if (x != INV_INT_24) data->ins_heading_angle = x * (ANG2RAD * RAD2DEG);
            if (data->ins_heading_angle < 0.0)
            {
                data->ins_heading_angle += 360.0;
            }

            if (abs(data->gps_lng - 117.0) < 5)
            {
                ServiceCarStatus.location->gps_lat = data->gps_lat;
                ServiceCarStatus.location->gps_lng = data->gps_lng;
                ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
                if (testcount < 1000)
                {
                    testcount++;
                }
                else
                {
                    double x = (ServiceCarStatus.location->gps_x - data->gps_x)*(ServiceCarStatus.location->gps_x - data->gps_x) + (ServiceCarStatus.location->gps_y - data->gps_y)*(ServiceCarStatus.location->gps_y - data->gps_y);

                    if (x > (jianju*jianju))
                    {
                        if (is_forbidden == false)
                        {
                            _mtx.try_lock();
                            fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status << std::endl;
                            _mtx.unlock();
                        }
                        else
                        {
                            _mtx.try_lock();
                            fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << std::endl;
                            _mtx.unlock();
                        }
                        gps_index++;
                        ServiceCarStatus.location->gps_x = data->gps_x;
                        ServiceCarStatus.location->gps_y = data->gps_y;
                    }
                }
            }
            _mtx2.lock();
        }
        _mtx2.unlock();
        fout.close();                  //关闭文件
    }
    udpsocket->close();
}

void GPSSensor::wait_exit()
{
    while(true)
    {
        should_exit = true;
#ifdef linux
        usleep(5000);
#endif
#ifdef WIN32
        boost::this_thread::sleep(boost::posix_time::milliseconds(5));
        //           Sleep(5);
#endif
        if(ready_exit == 1)
        {
            return;
        }
    }
}

static double cast_8_byte_to_double(const uint8_t *b)
{
    union { double x; uint8_t c[8]; } u;
    u.c[0] = b[0];
    u.c[1] = b[1];
    u.c[2] = b[2];
    u.c[3] = b[3];
    u.c[4] = b[4];
    u.c[5] = b[5];
    u.c[6] = b[6];
    u.c[7] = b[7];
    return u.x;
}

static int32_t cast_3_byte_to_int32(const uint8_t *b)
{
    union { int32_t x; uint8_t c[4]; } u;
    u.c[1] = b[0];
    u.c[2] = b[1];
    u.c[3] = b[2];
    return u.x >> 8;
}

GPS_Collect::GPS_Collect(QWidget *parent):
    QWidget(parent)
{
    //gps_collector_close = new boost::signals2::signal<void()>();

    this->setObjectName(QStringLiteral("GPS_Collector"));
    this->resize(800, 700);
    frame = new QFrame(this);
    frame->setObjectName(QStringLiteral("frame"));
    frame->setGeometry(QRect(440, 390, 120, 80));
    frame->setFrameShape(QFrame::StyledPanel);
    frame->setFrameShadow(QFrame::Raised);
    label = new QLabel(this);
    label->setObjectName(QStringLiteral("label"));
    label->setGeometry(QRect(20, 20, 90, 35));
    comboBox = new QComboBox(this);
    comboBox->setObjectName(QStringLiteral("comboBox"));
    comboBox->setGeometry(QRect(120, 60, 100, 35));
    QFont font;
    font.setPointSize(16);
    comboBox->setFont(font);
    label_2 = new QLabel(this);
    label_2->setObjectName(QStringLiteral("label_2"));
    label_2->setGeometry(QRect(20, 60, 90, 35));
    lineEdit = new QLineEdit(this);
    lineEdit->setObjectName(QStringLiteral("lineEdit"));
    lineEdit->setGeometry(QRect(120, 20, 100, 35));
    lineEdit->setFont(font);
    lineEdit->setAlignment(Qt::AlignCenter);
    label_3 = new QLabel(this);
    label_3->setObjectName(QStringLiteral("label_3"));
    label_3->setGeometry(QRect(270, 20, 30, 16));
    label_4 = new QLabel(this);
    label_4->setObjectName(QStringLiteral("label_4"));
    label_4->setGeometry(QRect(270, 40, 30, 16));
    label_5 = new QLabel(this);
    label_5->setObjectName(QStringLiteral("label_5"));
    label_5->setGeometry(QRect(270, 60, 60, 16));
    label_6 = new QLabel(this);
    label_6->setObjectName(QStringLiteral("label_6"));
    label_6->setGeometry(QRect(440, 20, 80, 16));
    label_7 = new QLabel(this);
    label_7->setObjectName(QStringLiteral("label_7"));
    label_7->setGeometry(QRect(440, 80, 80, 16));
    label_8 = new QLabel(this);
    label_8->setObjectName(QStringLiteral("label_8"));
    label_8->setGeometry(QRect(440, 40, 80, 16));
    label_9 = new QLabel(this);
    label_9->setObjectName(QStringLiteral("label_9"));
    label_9->setGeometry(QRect(440, 60, 80, 16));
    lineEdit_2 = new QLineEdit(this);
    lineEdit_2->setObjectName(QStringLiteral("lineEdit_2"));
    lineEdit_2->setGeometry(QRect(310, 20, 120, 20));
    lineEdit_3 = new QLineEdit(this);
    lineEdit_3->setObjectName(QStringLiteral("lineEdit_3"));
    lineEdit_3->setGeometry(QRect(310, 40, 120, 20));
    lineEdit_4 = new QLineEdit(this);
    lineEdit_4->setObjectName(QStringLiteral("lineEdit_4"));
    lineEdit_4->setGeometry(QRect(340, 60, 90, 20));
    lineEdit_5 = new QLineEdit(this);
    lineEdit_5->setObjectName(QStringLiteral("lineEdit_5"));
    lineEdit_5->setGeometry(QRect(520, 20, 90, 20));
    lineEdit_6 = new QLineEdit(this);
    lineEdit_6->setObjectName(QStringLiteral("lineEdit_6"));
    lineEdit_6->setGeometry(QRect(520, 40, 90, 20));
    lineEdit_7 = new QLineEdit(this);
    lineEdit_7->setObjectName(QStringLiteral("lineEdit_7"));
    lineEdit_7->setGeometry(QRect(520, 60, 90, 20));
    lineEdit_8 = new QLineEdit(this);
    lineEdit_8->setObjectName(QStringLiteral("lineEdit_8"));
    lineEdit_8->setGeometry(QRect(520, 80, 90, 20));

    this->setWindowTitle(QApplication::translate("GPS_Collector", "GPS_Collector", nullptr));
    label->setText(QApplication::translate("GPS_Collector", "\351\207\207\351\233\206\347\202\271\351\227\264\350\267\235(m)", nullptr));
    label_2->setText(QApplication::translate("GPS_Collector", "\347\246\201\347\224\250\351\231\204\345\212\240\345\261\236\346\200\247", nullptr));
    label_3->setText(QApplication::translate("GPS_Collector", "\347\273\217\345\272\246", nullptr));
    label_4->setText(QApplication::translate("GPS_Collector", "\347\272\254\345\272\246", nullptr));
    label_5->setText(QApplication::translate("GPS_Collector", "\345\267\262\351\207\207\351\233\206\347\202\271\346\225\260", nullptr));
    label_6->setText(QApplication::translate("GPS_Collector", "\351\201\277\351\232\234\346\250\241\345\274\217", nullptr));
    label_7->setText(QApplication::translate("GPS_Collector", "\346\211\200\345\234\250\350\275\246\351\201\223", nullptr));
    label_8->setText(QApplication::translate("GPS_Collector", "\351\200\237\345\272\246\346\216\247\345\210\266\346\250\241\345\274\217", nullptr));
    label_9->setText(QApplication::translate("GPS_Collector", "\350\275\246\351\201\223\346\200\273\346\225\260", nullptr));

    pushButtonstart = new QPushButton(this);
    pushButtonstart->setObjectName(QStringLiteral("pushButtonstart"));
    pushButtonstart->setGeometry(QRect(630, 11, 158, 50));
    pushButtonstart->setText("start");
    connect(pushButtonstart, SIGNAL(clicked()), this, SLOT(ClickButton_start()));
    pushButtonend = new QPushButton(this);
    pushButtonend->setObjectName(QStringLiteral("pushButtonend"));
    pushButtonend->setGeometry(QRect(630, 70, 158, 50));
    pushButtonend->setText("end");
    connect(pushButtonend, SIGNAL(clicked()), this, SLOT(ClickButton_end()));

    pushButton0 = new QPushButton(this);
    pushButton0->setObjectName(QStringLiteral("pushButton0"));
    pushButton0->setGeometry(QRect(20, 130, 192, 93));
    pushButton0->setText(QStringLiteral("停障"));
    connect(pushButton0, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_0()));
    pushButton1 = new QPushButton(this);
    pushButton1->setObjectName(QStringLiteral("pushButton1"));
    pushButton1->setGeometry(QRect(20, 223, 192, 93));
    pushButton1->setText(QStringLiteral("避障"));
    connect(pushButton1, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_1()));
    pushButton2 = new QPushButton(this);
    pushButton2->setObjectName(QStringLiteral("pushButton2"));
    pushButton2->setGeometry(QRect(20, 316, 192, 93));
    pushButton2->setText(QStringLiteral("不停不避"));
    connect(pushButton2, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_2()));
    pushButton33 = new QPushButton(this);
    pushButton33->setObjectName(QStringLiteral("pushButton33"));
    pushButton33->setGeometry(QRect(20, 409, 192, 93));
    pushButton33->setText(QStringLiteral("保留"));
    connect(pushButton33, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_33()));
    pushButton34 = new QPushButton(this);
    pushButton34->setObjectName(QStringLiteral("pushButton34"));
    pushButton34->setGeometry(QRect(20, 502, 192, 93));
    pushButton34->setText(QStringLiteral("保留"));
    connect(pushButton34, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_34()));

    pushButton3 = new QPushButton(this);
    pushButton3->setObjectName(QStringLiteral("pushButton3"));
    pushButton3->setGeometry(QRect(212, 130, 96, 46));
    pushButton3->setText(QStringLiteral("常速行驶"));
    connect(pushButton3, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_3()));
    pushButton4 = new QPushButton(this);
    pushButton4->setObjectName(QStringLiteral("pushButton4"));
    pushButton4->setGeometry(QRect(308, 130, 96, 46));
    pushButton4->setText(QStringLiteral("入口"));
    connect(pushButton4, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_4()));
    pushButton5 = new QPushButton(this);
    pushButton5->setObjectName(QStringLiteral("pushButton5"));
    pushButton5->setGeometry(QRect(212, 176, 96, 47));
    pushButton5->setText(QStringLiteral("事故区"));
    connect(pushButton5, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_5()));
    pushButton6 = new QPushButton(this);
    pushButton6->setObjectName(QStringLiteral("pushButton6"));
    pushButton6->setGeometry(QRect(308, 176, 96, 47));
    pushButton6->setText(QStringLiteral("驻车点"));
    connect(pushButton6, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_6()));
    pushButton7 = new QPushButton(this);
    pushButton7->setObjectName(QStringLiteral("pushButton7"));
    pushButton7->setGeometry(QRect(212, 223, 96, 46));
    pushButton7->setText(QStringLiteral("隧道"));
    connect(pushButton7, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_7()));
    pushButton18 = new QPushButton(this);
    pushButton18->setObjectName(QStringLiteral("pushButton18"));
    pushButton18->setGeometry(QRect(308, 223, 96, 46));
    pushButton18->setText(QStringLiteral("低速"));
    connect(pushButton18, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_18()));
    pushButton19 = new QPushButton(this);
    pushButton19->setObjectName(QStringLiteral("pushButton19"));
    pushButton19->setGeometry(QRect(212, 269, 96, 47));
    pushButton19->setText(QStringLiteral("红绿灯"));
    connect(pushButton19, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_19()));
    pushButton20 = new QPushButton(this);
    pushButton20->setObjectName(QStringLiteral("pushButton20"));
    pushButton20->setGeometry(QRect(308, 269, 96, 47));
    pushButton20->setText(QStringLiteral("行人"));
    connect(pushButton20, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_20()));
    pushButton21 = new QPushButton(this);
    pushButton21->setObjectName(QStringLiteral("pushButton21"));
    pushButton21->setGeometry(QRect(212, 316, 96, 46));
    pushButton21->setText(QStringLiteral("雾区"));
    connect(pushButton21, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_21()));
    pushButton22 = new QPushButton(this);
    pushButton22->setObjectName(QStringLiteral("pushButton22"));
    pushButton22->setGeometry(QRect(308, 316, 96, 46));
    pushButton22->setText(QStringLiteral("变道停车"));
    connect(pushButton22, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_22()));
    pushButton23 = new QPushButton(this);
    pushButton23->setObjectName(QStringLiteral("pushButton23"));
    pushButton23->setGeometry(QRect(212, 362, 96, 47));
    pushButton23->setText(QStringLiteral("等人停车"));
    connect(pushButton23, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_23()));
    pushButton24 = new QPushButton(this);
    pushButton24->setObjectName(QStringLiteral("pushButton24"));
    pushButton24->setGeometry(QRect(308, 362, 96, 47));
    pushButton24->setText(QStringLiteral("疯狂加速"));
    connect(pushButton24, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_24()));
    pushButton25 = new QPushButton(this);
    pushButton25->setObjectName(QStringLiteral("pushButton25"));
    pushButton25->setGeometry(QRect(212, 409, 96, 46));
    pushButton25->setText(QStringLiteral("跟随"));
    connect(pushButton25, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_25()));
    pushButton26 = new QPushButton(this);
    pushButton26->setObjectName(QStringLiteral("pushButton26"));
    pushButton26->setGeometry(QRect(308, 409, 96, 46));
    pushButton26->setText(QStringLiteral("保留"));
    connect(pushButton26, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_26()));
    pushButton27 = new QPushButton(this);
    pushButton27->setObjectName(QStringLiteral("pushButton27"));
    pushButton27->setGeometry(QRect(212, 455, 96, 47));
    pushButton27->setText(QStringLiteral("保留"));
    connect(pushButton27, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_27()));
    pushButton28 = new QPushButton(this);
    pushButton28->setObjectName(QStringLiteral("pushButton28"));
    pushButton28->setGeometry(QRect(308, 455, 96, 47));
    pushButton28->setText(QStringLiteral("保留"));
    connect(pushButton28, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_28()));
    pushButton29 = new QPushButton(this);
    pushButton29->setObjectName(QStringLiteral("pushButton29"));
    pushButton29->setGeometry(QRect(212, 502, 96, 46));
    pushButton29->setText(QStringLiteral("保留"));
    connect(pushButton29, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_29()));
    pushButton30 = new QPushButton(this);
    pushButton30->setObjectName(QStringLiteral("pushButton30"));
    pushButton30->setGeometry(QRect(308, 502, 96, 46));
    pushButton30->setText(QStringLiteral("保留"));
    connect(pushButton30, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_30()));
    pushButton31 = new QPushButton(this);
    pushButton31->setObjectName(QStringLiteral("pushButton31"));
    pushButton31->setGeometry(QRect(212, 548, 96, 47));
    pushButton31->setText(QStringLiteral("保留"));
    connect(pushButton31, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_31()));
    pushButton32 = new QPushButton(this);
    pushButton32->setObjectName(QStringLiteral("pushButton32"));
    pushButton32->setGeometry(QRect(308, 548, 96, 47));
    pushButton32->setText(QStringLiteral("保留"));
    connect(pushButton32, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_32()));

    pushButton8 = new QPushButton(this);
    pushButton8->setObjectName(QStringLiteral("pushButton8"));
    pushButton8->setGeometry(QRect(404, 130, 192, 93));
    pushButton8->setText(QStringLiteral("单车道"));
    connect(pushButton8, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_8()));
    pushButton9 = new QPushButton(this);
    pushButton9->setObjectName(QStringLiteral("pushButton9"));
    pushButton9->setGeometry(QRect(404, 223, 192, 93));
    pushButton9->setText(QStringLiteral("双车道"));
    connect(pushButton9, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_9()));
    pushButton10 = new QPushButton(this);
    pushButton10->setObjectName(QStringLiteral("pushButton10"));
    pushButton10->setGeometry(QRect(404, 316, 192, 93));
    pushButton10->setText(QStringLiteral("三车道"));
    connect(pushButton10, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_10()));
    pushButton11 = new QPushButton(this);
    pushButton11->setObjectName(QStringLiteral("pushButton11"));
    pushButton11->setGeometry(QRect(404, 409, 192, 93));
    pushButton11->setText(QStringLiteral("四车道"));
    connect(pushButton11, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_11()));
    pushButton12 = new QPushButton(this);
    pushButton12->setObjectName(QStringLiteral("pushButton12"));
    pushButton12->setGeometry(QRect(404, 502, 192, 93));
    pushButton12->setText(QStringLiteral("五车道"));
    connect(pushButton12, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_12()));

    pushButton13 = new QPushButton(this);
    pushButton13->setObjectName(QStringLiteral("pushButton13"));
    pushButton13->setGeometry(QRect(596, 130, 192, 93));
    pushButton13->setText(QStringLiteral("在车道0"));
    connect(pushButton13, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_13()));
    pushButton14 = new QPushButton(this);
    pushButton14->setObjectName(QStringLiteral("pushButton0"));
    pushButton14->setGeometry(QRect(596, 223, 192, 93));
    pushButton14->setText(QStringLiteral("在车道1"));
    connect(pushButton14, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_14()));
    pushButton15 = new QPushButton(this);
    pushButton15->setObjectName(QStringLiteral("pushButton0"));
    pushButton15->setGeometry(QRect(596, 316, 192, 93));
    pushButton15->setText(QStringLiteral("在车道2"));
    connect(pushButton15, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_15()));
    pushButton16 = new QPushButton(this);
    pushButton16->setObjectName(QStringLiteral("pushButton0"));
    pushButton16->setGeometry(QRect(596, 409, 192, 93));
    pushButton16->setText(QStringLiteral("在车道3"));
    connect(pushButton16, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_16()));
    pushButton17 = new QPushButton(this);
    pushButton17->setObjectName(QStringLiteral("pushButton0"));
    pushButton17->setGeometry(QRect(596, 502, 192, 93));
    pushButton17->setText(QStringLiteral("在车道4"));
    connect(pushButton17, SIGNAL(clicked()), this, SLOT(ClickButton_nomall_17()));

    lineEdit->setText(QStringLiteral("0.1"));
    connect(lineEdit, SIGNAL(textEdited(const QString &)), this, SLOT(savestabuyEditinfo(const QString &)));
    comboBox->addItem(QStringLiteral("  禁用"));
    comboBox->addItem(QStringLiteral(" 不禁用"));
    connect(comboBox, SIGNAL(currentIndexChanged(const QString &)), this, SLOT(mycombox(const QString &)));

    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot1()));
    connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot2()));
    timer->start(20);

    sensor_gps = new GPSSensor();
    sensor_gps->start();

}

GPS_Collect::~GPS_Collect()
{

}
//刷新
void GPS_Collect::paintEvent(QPaintEvent *)
{

}

void GPS_Collect::savestabuyEditinfo(const QString &txt)
{
    jianju = txt.toDouble();
    if (jianju > 0.01)
    {
        sensor_gps->jianju_change(jianju);
    }
}

void GPS_Collect::mycombox(const QString &txt)
{
    if (txt == QStringLiteral("  禁用"))
    {
        is_forbidden = true;
    }
    else
    {
        is_forbidden = false;
    }
    sensor_gps->collect_modechange(is_forbidden);
}

void GPS_Collect::timeoutslot1()
{
    //update();
}

void GPS_Collect::timeoutslot2()
{
    lineEdit_2->setText(QString::number(ServiceCarStatus.location->gps_lng));
    lineEdit_3->setText(QString::number(ServiceCarStatus.location->gps_lat));
    lineEdit_4->setText(QString::number(sensor_gps->gps_index));
    lineEdit_5->setText(QString::number(obs_modes));
    lineEdit_6->setText(QString::number(speed_modes));
    lineEdit_7->setText(QString::number(lane_num));
    lineEdit_8->setText(QString::number(lane_status));
}

void GPS_Collect::ClickButton_start()
{
    starts = true;
    sensor_gps->start_or_end(starts);
}
void GPS_Collect::ClickButton_end()
{
    starts = false;
    sensor_gps->start_or_end(starts);
}




/////////////////////////////////////////////////////
void GPS_Collect::ClickButton_nomall_0()
{
    obs_modes = 0;
    sensor_gps->obs_modechange(obs_modes);
}

void GPS_Collect::ClickButton_nomall_1()
{
    obs_modes = 1;
    sensor_gps->obs_modechange(obs_modes);
}

void GPS_Collect::ClickButton_nomall_2()
{
    obs_modes = 2;
    sensor_gps->obs_modechange(obs_modes);
}




////////////////////////////////////////////////////////
void GPS_Collect::ClickButton_nomall_3()
{
    speed_modes = 0;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_4()
{
    speed_modes = 1;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_5()
{
    speed_modes = 2;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_6()
{
    speed_modes = 3;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_7()
{
    speed_modes = 4;
    sensor_gps->speed_modechange(speed_modes);
}



/////////////////////////////////////////////////////////
void GPS_Collect::ClickButton_nomall_8()
{
    lane_num = 1;
    sensor_gps->lane_num_modechange(lane_num);
}

void GPS_Collect::ClickButton_nomall_9()
{
    lane_num = 2;
    sensor_gps->lane_num_modechange(lane_num);
}

void GPS_Collect::ClickButton_nomall_10()
{
    lane_num = 3;
    sensor_gps->lane_num_modechange(lane_num);
}

void GPS_Collect::ClickButton_nomall_11()
{
    lane_num = 4;
    sensor_gps->lane_num_modechange(lane_num);
}

void GPS_Collect::ClickButton_nomall_12()
{
    lane_num = 5;
    sensor_gps->lane_num_modechange(lane_num);
}



//////////////////////////////////////////////////////////
void GPS_Collect::ClickButton_nomall_13()
{
    lane_status = 0;
    sensor_gps->lane_status_modechange(lane_status);
}

void GPS_Collect::ClickButton_nomall_14()
{
    lane_status = 1;
    sensor_gps->lane_status_modechange(lane_status);
}

void GPS_Collect::ClickButton_nomall_15()
{
    lane_status = 2;
    sensor_gps->lane_status_modechange(lane_status);
}

void GPS_Collect::ClickButton_nomall_16()
{
    lane_status = 3;
    sensor_gps->lane_status_modechange(lane_status);
}

void GPS_Collect::ClickButton_nomall_17()
{
    lane_status = 4;
    sensor_gps->lane_status_modechange(lane_status);
}



/////////////////////////////////////////////////////////
void GPS_Collect::ClickButton_nomall_18()
{
    speed_modes = 5;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_19()
{
    speed_modes = 6;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_20()
{
    speed_modes = 7;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_21()
{
    speed_modes = 8;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_22()
{
    speed_modes = 9;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_23()
{
    speed_modes = 10;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_24()
{
    speed_modes = 11;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_25()
{
    speed_modes = 12;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_26()
{
    speed_modes = 13;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_27()
{
    speed_modes = 14;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_28()
{
    speed_modes = 15;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_29()
{
    speed_modes = 16;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_30()
{
    speed_modes = 17;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_31()
{
    speed_modes = 18;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_32()
{
    speed_modes = 19;
    sensor_gps->speed_modechange(speed_modes);
}

void GPS_Collect::ClickButton_nomall_33()
{
    obs_modes = 3;
    sensor_gps->obs_modechange(obs_modes);
}

void GPS_Collect::ClickButton_nomall_34()
{
    obs_modes = 4;
    sensor_gps->obs_modechange(obs_modes);
}


void GPS_Collect::closeEvent(QCloseEvent *event)
{
    if(should_close == false)
    {
        QMessageBox::StandardButton button;
        button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
        if(button==QMessageBox::No)
        {
            event->ignore(); // 忽略退出信号，程序继续进行
        }
        else if(button==QMessageBox::Yes)
        {
            sensor_gps->wait_exit();
            event->accept(); // 接受退出信号，程序退出
            gps_collector_close();
        }
    }
    else
    {
        event->accept(); // 接受退出信号，程序退出
    }
}

void GPSSensor::convertStrToUnChar(char* str, unsigned char* UnChar)
{
    int i = strlen(str), j = 0, counter = 0;
    char c[2];
    unsigned int bytes[2];

    for (j = 0; j < i; j += 2)
    {
        if(0 == j % 2)
        {
            c[0] = str[j];
            c[1] = str[j + 1];
            sscanf(c, "%02x" , &bytes[0]);
            UnChar[counter] = bytes[0];
            counter++;
        }
    }
}