modularization/src/driver/driver_radio_collision/car_status_data.cpp

#include "all_head.h"
#include "p900.h"

extern iv::Ivlog * givlog;
extern iv::Ivfault *gfault;
#define PI  3.14159265358979

VehicleStatus front_car;

//CarStatusData::CarStatusData(const char * strmap)
//{
//    givlog->debug("GPS","[%s] map: %s",__func__,strmap);
//    qDebug()<<"map"<<strmap;
//
//}


/**
 * @brief GaussProjCal
 * @param lon
 * @param lat
 * @param X
 * @param Y
 *  iv::GPSData data(new iv::GPS_INS);
 *  GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
 */
void CarStatusData::GaussProjCal(double lon, double lat, double *X, double *Y)
{
    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
    double a = 6378140.0;
    double e2 = 0.006694384999588;
    double ep2 = e2/(1-e2);

    // 原点所在经度
    double lon_origin = 6.0*int(lon/6) + 3.0;
    lon_origin *= PI / 180.0;

    double k0 = 0.9996;

    // 角度转弧度
    double lat1 = lat * PI / 180.0;
    double lon1 = lon * PI / 180.0;


    // 经线在该点处的曲率半径,
    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));


    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
    // 首先计算前四项的系数 a1~a4.
    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
    double a4 = (35*e2*e2*e2)/3072;
    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));

    // 辅助量
    double T = tan(lat1)*tan(lat1);
    double C = ep2*cos(lat1)*cos(lat1);
    double A = (lon1 - lon_origin)*cos(lat1);

    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
    *Y = M + N * tan(lat1) * ((A*A)/2 +
                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);


    *Y *= k0;
    return;
}

bool CarStatusData::loadMapFromFile(std::string fileName)
{
    std::ifstream fis(fileName);//获取文件
    std::string line;
    std::vector<std::string> des;

    givlog->debug("map","[%s] load map start: %s",__func__, fileName.data());

    if (fis.is_open() == false)
    {
        givlog->error("map","[%s]open map faile",__func__);
        return false;
    }
    try {
        navigation_data.clear();
        while (std::getline(fis, line)) {//开始一行一行的读数据
            boost::split(des, line, boost::is_any_of("\t"));
            if (des.size() < 10){
                throw "error";
            }
            iv::GPSData data(new iv::GPS_INS);
            data->index = atoi(des[0].c_str());
            data->gps_lng = atof(des[1].c_str());
            data->gps_lat = atof(des[2].c_str());
            data->speed_mode = atoi(des[3].c_str());
            data->mode2 = atoi(des[4].c_str());
            data->ins_heading_angle = atof(des[5].c_str());
            data->runMode = atoi(des[6].c_str());
            data->roadMode = atoi(des[7].c_str());
            data->roadSum = atoi(des[8].c_str());
            data->roadOri = atoi(des[9].c_str());
            GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);

            if(data->roadMode==4){
                data->ins_heading_angle=data->ins_heading_angle+180;
                if(data->ins_heading_angle>=360)
                    data->ins_heading_angle=data->ins_heading_angle-360;
            }
            navigation_data.push_back(data);
        }

    }
    catch (...) {
        fis.close();
        givlog->error("map","[%s] load fail,size error",__func__);
        return false;
    }
    givlog->info("map","[%s] load map success: %s",__func__, fileName.data());
    fis.close();
    mcurrent_index = 0;
    return true;

}

double CarStatusData::GetDistance(iv::GPS_INS p1, iv::GPS_INS p2)
{
    return sqrt((p1.gps_x - p2.gps_x) * (p1.gps_x - p2.gps_x) + (p1.gps_y - p2.gps_y) * (p1.gps_y - p2.gps_y));
}


int CarStatusData::getNearestPointIndex(iv::GPS_INS rp, const std::vector<iv::GPSData> gpsMap)
{
    int index = -1;
    double mindis = 40;
    int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
    int endIndex = gpsMap.size() - 1;

    for (int j = startIndex; j < endIndex; j++)
    {
        int i = (j + gpsMap.size()) % gpsMap.size();
        double tmpdis = GetDistance(rp, (*gpsMap[i]));

        if (tmpdis < mindis )
//            if (tmpdis < mindis && (abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle) < 80
//                                                  || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle - 360) < 80
//                                                  || abs(rp.ins_heading_angle - (*gpsMap[i]).ins_heading_angle + 360) < 80)
//                    )
        {
            index = i;
            mindis = tmpdis;
        }
    }
    /*if (index == -1) {
        index = 0;
    }*/

    mcurrent_index = index;
    return index;
}

//void iv::CarStatus::getMapFromFile()
//{

//}
uint16_t CarStatusData::getRate()
{
    uint16_t rate;
    int max_index = 0;
    mmutex.lock();
    max_index = navigation_data.size();
    mmutex.unlock();
    rate = qRound(((double)(mcurrent_index)/(double)(max_index))*10000);
    givlog->verbose("sendData","[%s]max_index %d current_index %d,rate:%d",__func__, max_index, mcurrent_index,rate);
    return rate;
}
//return: cm
uint16_t CarStatusData::getDis()
{
    return mcurrent_index;
}

void CarStatusData::genrateCurrentGPSINS(iv::gps::gpsimu _proto_gpsimu)
{
    mcurrent_GPS_INS.index = 0;
    mcurrent_GPS_INS.gps_lng = _proto_gpsimu.lon();
    mcurrent_GPS_INS.gps_lat = _proto_gpsimu.lat();
    mcurrent_GPS_INS.ins_heading_angle = _proto_gpsimu.heading();

    GaussProjCal(mcurrent_GPS_INS.gps_lng, mcurrent_GPS_INS.gps_lat, &mcurrent_GPS_INS.gps_x, &mcurrent_GPS_INS.gps_y);

//    if(mcurrent_GPS_INS->roadMode==4){
//        data->ins_heading_angle=data->ins_heading_angle+180;
//        if(data->ins_heading_angle>=360)
//            data->ins_heading_angle=data->ins_heading_angle-360;

}


void CarStatusData::frameDataInit(uint8_t _car_id)
{
    memset(&mcar_state_frame, 0, 25);
    mcar_state_frame.frame.car_id = _car_id;
    mcar_state_frame.frame.head1 = 0xAA;
    mcar_state_frame.frame.head2 = 0x16;
    mcar_state_frame.frame.length = 0x18;
    mcar_state_frame.frame.map_index = 1;
    mcar_state_frame.frame.reserved2 = 0;
    mcar_state_frame.frame.reserved3 = 0;
    mcar_state_frame.frame.status_flag = 0;

    return;
}

QByteArray CarStatusData::fill_frame(iv::gps::gpsimu _proto_gpsimu)
{
    QByteArray _send_data;
//    uint8_t  car_id;
    int32_t lat,lon;
    int16_t head, speed, rate, dis,desired_speed;

    genrateCurrentGPSINS(_proto_gpsimu);
    mmutex.lock();
    getNearestPointIndex(mcurrent_GPS_INS,navigation_data);
    mmutex.unlock();
    lat     = _proto_gpsimu.lat()*10000000;
    lon     = _proto_gpsimu.lon()*10000000;
    speed   = qRound(qSqrt(qPow(_proto_gpsimu.ve(),2)+qPow(_proto_gpsimu.vn(),2))*36);
    head    = qRound(_proto_gpsimu.heading());
    rate    = getRate();
    dis     = getDis();
    givlog->verbose("sendData","[%s] lat:%d lon:%d speed:%d head:%d rate:%d dis:%d ",__func__, lat, lon, speed, head, rate, dis);

    if(CAR_TYPE==1)
    {
        front_car.vehicleSpeed=speed;
        front_car.vehicleProcess=rate;
        front_car.vehicleDistance=dis;
        desired_speed=back_car.vehicleDesiredSpeed;
    }else if(CAR_TYPE==2)
    {
        back_car.vehicleSpeed=speed;
        back_car.vehicleProcess=rate;
        back_car.vehicleDistance=dis;
        desired_speed=40;   //用不到
    }

    mcar_state_frame.frame.lat1 = (lat & 0xFF000000) >> 24;
    mcar_state_frame.frame.lat2 = (lat & 0xFF0000) >> 16;
    mcar_state_frame.frame.lat3 = (lat & 0xFF00) >> 8;
    mcar_state_frame.frame.lat4 = (lat & 0xFF);
    mcar_state_frame.frame.lon1 = (lon & 0xFF000000) >> 24;
    mcar_state_frame.frame.lon2 = (lon & 0xFF0000) >> 16;
    mcar_state_frame.frame.lon3 = (lon & 0xFF00) >> 8;
    mcar_state_frame.frame.lon4 = (lon & 0xFF);

    mcar_state_frame.frame.speadH = (speed & 0xFF00) >> 8;
    mcar_state_frame.frame.speadL = (speed & 0xFF);
    mcar_state_frame.frame.posH = (head & 0xFF00) >> 8;
    mcar_state_frame.frame.posL = (head & 0xFF);
    mcar_state_frame.frame.disH = (dis & 0xFF00) >> 8;
    mcar_state_frame.frame.disL = (dis & 0xFF);
    mcar_state_frame.frame.rateH = (rate & 0xFF00) >> 8;
    mcar_state_frame.frame.rateL = (rate & 0xFF);
    mcar_state_frame.frame.reserved2 = (desired_speed & 0xFF00) >> 8;
    mcar_state_frame.frame.reserved3 = (desired_speed & 0xFF);
    mcar_state_frame.frame.status_flag =1 ;


    mcar_state_frame.frame.cksum = calSum(mcar_state_frame);



    //设置car status flag
//    mcar_state_frame.frame.status_flag = ;

    _send_data = initToQByteArray(mcar_state_frame.byte,25);

    return _send_data;

}

uint8_t CarStatusData::calSum(car_to_server_data _data)
{
    uint8_t sum = 0;
    uint64_t sums = 0;
    for(int i = 0; i < 24; i++)
    {
        sums = sums + _data.byte[i];
    }
    sum = sums & 0xFF;
    return sum;
}
QByteArray CarStatusData::initToQByteArray(uint8_t _data[], int _len)
{
    QByteArray _ba;
    _ba.resize(_len);
    for(int i = 0; i < _len; i++)
    {
        _ba[i] = _data[i];
    }
    return _ba;
}
//此函数作模拟测试使用：
// 从map.txt中获取一个pgs点位作为车的当前位置
QByteArray CarStatusData::test_use_map_gps(int index)
{
    QByteArray _send_data;
//    uint8_t  car_id;
    int32_t lat,lon;
    uint16_t head, speed, rate, dis;
    iv::GPS_INS virtual_gpsST;

    _send_data.clear();
    if(navigation_data.empty())
        return _send_data;
    mcurrent_index = index;
    virtual_gpsST = *navigation_data[index];


    lat     = virtual_gpsST.gps_lat*10000000;
    lon     = virtual_gpsST.gps_lng*10000000;
    speed   = 100;
    head    = qRound(virtual_gpsST.ins_heading_angle);
    rate    = getRate();
    dis     = getDis();

    mcar_state_frame.frame.lat1 = (lat & 0xFF000000) >> 24;
    mcar_state_frame.frame.lat2 = (lat & 0xFF0000) >> 16;
    mcar_state_frame.frame.lat3 = (lat & 0xFF00) >> 8;
    mcar_state_frame.frame.lat4 = (lat & 0xFF);
    mcar_state_frame.frame.lon1 = (lon & 0xFF000000) >> 24;
    mcar_state_frame.frame.lon1 = (lon & 0xFF0000) >> 16;
    mcar_state_frame.frame.lon1 = (lon & 0xFF00) >> 8;
    mcar_state_frame.frame.lon1 = (lon & 0xFF);

    mcar_state_frame.frame.speadH = (speed & 0xFF00) >> 8;
    mcar_state_frame.frame.speadL = (speed & 0xFF);
    mcar_state_frame.frame.posH = (head & 0xFF00) >> 8;
    mcar_state_frame.frame.posL = (head & 0xFF);
    mcar_state_frame.frame.disH = (dis & 0xFF00) >> 8;
    mcar_state_frame.frame.disL = (dis & 0xFF);
    mcar_state_frame.frame.rateH = (rate & 0xFF00) >> 8;
    mcar_state_frame.frame.rateL = (rate & 0xFF);
    mcar_state_frame.frame.cksum = calSum(mcar_state_frame);

    _send_data = initToQByteArray(mcar_state_frame.byte,25);
    givlog->debug("sendData","rate:%d index:%d,mcurrent index:%d",rate,index, mcurrent_index);
//    qDebug("virtual send test");
//    qDebug()<<_send_data.toHex();
    return _send_data;

}