modularization/src/driver/driver_radio_collision/p900.cpp

#include "p900.h"
#define CONNECTED true
#define DISCONNECTED false
//#define DEBUG
extern iv::Ivlog * givlog;
extern iv::Ivfault *gfault;

radio * gradio;

VehicleStatus back_car;


radio::radio(const char * struartdev,const char * strmsgradio_name_send,  const char * strmsgradio_car_id, \
             const char * strmemgps, const char * strmemmap_index, const char * strcollision_index)
{
    mTime.start();
    mset_cmd_mode = 0;
    virtual_gps_index_counter = 0;

    mbSerialOpen = false;
    mnNotRecvCount = 0;
    mnLastOpenTime = 0;
    mnLastServerDataTime = 0;
    mserial_recv_data.clear();

    gradio = this;

    mpmem_radio_gps_addr = iv::modulecomm::RegisterRecv(strmemgps,Listen_gpsimu);
    mpmem_radio_send_addr = iv::modulecomm::RegisterSend(strmsgradio_name_send,100,1);
    mpmem_map_addr = iv::modulecomm::RegisterSend(strmemmap_index,5,1);

    ModuleFun funswitchmap = std::bind(&radio::recvCurMapIndex,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    iv::modulecomm::RegisterRecvPlus("current_map_index",funswitchmap);
    ModuleFun recBrainCmd = std::bind(&radio::recvBrainCMD,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    iv::modulecomm::RegisterRecvPlus("current_map_index",recBrainCmd);

    mcar_id = atoi(strmsgradio_car_id);
    mcollision_point = atoi(strcollision_index);
    mcar_map_index = 1;
    mserver_map_index = 1;
    mserver_status = DISCONNECTED;
    mcar_control_speed = 0;
    givlog->debug("init","[%s]car id %d",__func__, mcar_id);

    mapInit();

    serialPortInit(struartdev);
    mradio_protobuf_send.set_server_status(0);
    mradio_protobuf_send.set_car_control_speed(0);
    mradio_protobuf_send.set_cmd_mode(0);
    mradio_protobuf_send.set_cmd_control(0);
    mcar_status_data = new CarStatusData;
    mcar_status_data->mmutex.lock();
    mcar_status_data->loadMapFromFile(mvmap_name.at(0));
    micur_maptrace_index = 0;
    mcar_status_data->mmutex.unlock();
    mcar_status_data->frameDataInit(mcar_id);

    QTimer * timer = new QTimer(); //car数据发送定时器，每隔30ms发送一次
    connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
    timer->setTimerType(Qt::PreciseTimer);
    timer->start(30);

    QTimer * timer_server = new QTimer(); //服务器数据接收定时器，每隔300ms检查一次
    connect(timer_server,SIGNAL(timeout()),this,SLOT(onTimerServer()));
    timer_server->setTimerType(Qt::PreciseTimer);
    timer_server->start(30);
}


radio::~radio()
{
    m_serialPort_Radio->close();
    delete gradio;
    givlog->debug("init","[%s] radio bye",__func__);
}

void Listen_gpsimu(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
{
    iv::gps::gpsimu proto_gpsimu;
    if(false == proto_gpsimu.ParseFromArray(_strdata,_nSize))
    {
        givlog->error("GPS","[%s] Listencansend Parse fail.",__func__);
        return;
    }

    gradio->mserial_send_data = gradio->mcar_status_data->fill_frame(proto_gpsimu);
//    givlog->verbose("listen gps data");
}
void radio::recvCurMapIndex(const char * _strdata,const unsigned int _nSize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
{

    iv::formation_map_index::map map_index;

    if(false == map_index.ParseFromArray(_strdata,_nSize))
    {
        givlog->error("p900","[%s] Listencansend Parse fail.",__func__);
        return;
    }
    if(map_index.has_currentindex())
    {
        micur_maptrace_index = map_index.currentindex();
        givlog->info("map","[%s] Rec Current map index from maptrace,Index:%d",micur_maptrace_index);
        mcar_status_data->mimaptrace_index = micur_maptrace_index;
    }
}
void radio::recvBrainCMD(const char *pdata, const int ndatasize,const unsigned int _index,const QDateTime * _dt,const char * _strmemname)
{
    if(ndatasize < sizeof(iv::hmi::HMIBasic))
        return;

    iv::hmi::hmimsg xhmimsg;
    if(!xhmimsg.ParseFromArray(pdata,ndatasize))
    {
        givlog->error("iv::decition::BrainDecition::UpdateHMI parse error");
        return;
    }

    mbRunPause = xhmimsg.mbpause();
    givlog->info("collision","[%s] rec brain state from HMI");
}
bool radio::serialPortInit(const char * struartdev)
{
    m_serialPort_Radio = new QSerialPort();
    m_serialPort_Radio->setPortName(struartdev);
    m_serialPort_Radio->setBaudRate(QSerialPort::Baud115200);
    m_serialPort_Radio->setParity(QSerialPort::NoParity);
    m_serialPort_Radio->setDataBits(QSerialPort::Data8);
    m_serialPort_Radio->setStopBits(QSerialPort::OneStop);
    m_serialPort_Radio->setFlowControl(QSerialPort::NoFlowControl);
    m_serialPort_Radio->setReadBufferSize(0);
    connect(m_serialPort_Radio,SIGNAL(readyRead()),this,SLOT(receiveData()));
    if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
    {
        givlog->debug("init","[%s] open serial port success",__func__);
        mbSerialOpen = true;
        return true;
    }
    else
    {
        givlog->error("init","[%s]open serial faile",__func__);
        mbSerialOpen = false;
        return false;
    }
}

void radio::run()
{

    while(!QThread::isInterruptionRequested())
    {
        sleep(1);
    }
}
//定时发送车辆状态到服务器
void radio::onTimer()
{
#ifdef DEBUG
    mbSerialOpen = true;
#endif
    if(mbSerialOpen)
    {
#ifdef DEBUG
        //模拟发送数据
        mserial_send_data = mcar_status_data->test_use_map_gps(virtual_gps_index_counter);
        if(mserial_send_data.isEmpty())
        {
            givlog->debug("sendData","[%s] test data error",__func__);
            return;
        }
        if(virtual_gps_index_counter<mcar_status_data->navigation_data.size())
            virtual_gps_index_counter++;
        else
            virtual_gps_index_counter = 0;
        qDebug()<<virtual_gps_index_counter;
#endif

        m_serialPort_Radio->write(mserial_send_data);
        givlog->verbose("sendData","[%s] send car statue frame: %s",__func__,mserial_send_data.toHex().data());

    }
    else{
        if((QDateTime::currentMSecsSinceEpoch() - mnLastOpenTime)>=100)
        {
            if(m_serialPort_Radio->open(QSerialPort::ReadWrite))
            {
                givlog->debug("state","[%s] open serial port success",__func__);
                mbSerialOpen = true;
            }
            else
            {
                givlog->debug("state","[%s] open faile",__func__);
            }
            mnLastOpenTime = QDateTime::currentMSecsSinceEpoch();
        }
    }
}
// 服务器状态监控
void radio::onTimerServer()
{
    if((QDateTime::currentMSecsSinceEpoch() - mnLastServerDataTime)>=900)
    {
        mserver_status = DISCONNECTED;
        mradio_protobuf_send.set_server_status(mserver_status);
        protoMsgSend();
        givlog->error("state","[%s] server lose connect",__func__);
    }

#ifdef DEBUG_REC
    //模拟接受服务器数据
    QByteArray _ba;
    static int count = 1;
    int sum =0;
    _ba.clear();
    _ba[0] = 0xAA;
    _ba[1] = 0x26;
    _ba[2] = 0x09;
    _ba[3] = 0x11;
    _ba[4] = 0;
    _ba[5] = 0;
    _ba[6] = count;
    if(count < 10)
        count++;
    else
        count = 1;
    _ba[7] = 0;
    _ba[8] = 0;
    for(int i =0; i<9; i++)
       sum = sum+_ba[i];
    _ba[9] = sum &0x00ff;
    givlog->debug("virtual","[%s] %s",__func__, _ba.toHex().data());
    virtual_gps_index_counter = 0;
    serialData_Decode(_ba);
#endif

}
// 预处理数据，截取完整数据包
bool radio::preprocData()
{
    int32_t index = 0;
    while((mserial_recv_data.mid(index,2).toHex() != "aa26") && \
           (mserial_recv_data.mid(index,2).toHex() != "aa28") && \
           (index<mserial_recv_data.length()))
    {
        index = index + 1;
    }
    givlog->verbose("recData","[%s] mserial_recv_data:[rec:] %s index: %d",__func__,mserial_recv_data.toHex().data(),index);

    if(index)
        mserial_recv_data.remove(0,index);
    givlog->verbose("recData","[%s] mserial_recv_data:[after:] %s",__func__, mserial_recv_data.toHex().data());
    if(checkSum(0))
    {
        return true;
    }
    return false;
}

void radio::receiveData()
{
    if(mbSerialOpen)
    {
        mnLastServerDataTime = QDateTime::currentMSecsSinceEpoch();
        QByteArray _ba;
        _ba.clear();
        _ba = m_serialPort_Radio->readAll();
        serialData_Decode(_ba);
        givlog->verbose("recData","[%s] %s",__func__,_ba.toHex().data());
#if 0
        mserial_recv_data.append(_ba);
        qDebug()<<"ba"<<_ba.toHex();
        radioServerDataDecode();
#endif

    }
}

void radio::radioServerDataDecode()
{
    uint8_t _length;
    uint8_t _data[15];

    if(!preprocData())
    {
        return;
    }
    _length = (mserial_recv_data.data()[2]&0xff) + 1;

    memcpy(_data,mserial_recv_data.data(),_length);

//    qDebug()<<"_data"<<QByteArray((char *)_data,_length).toHex();

    if(_data[3] == mcar_id)
    {
        mserver_status = CONNECTED;
        radioServerDataDecodeSen(_data);
    }
    //清除此次已处理的数据段
    mserial_recv_data.remove(0,_length);
    if(!mserial_recv_data.isEmpty())
    {
        radioServerDataDecode();
    }
    return;
}

void radio::radioServerDataDecodeSen(const uint8_t *_data)
{
    //处理服务器决策信息
    mcar_control_speed =(double)(_data[4]<< 8 | _data[5])/10.0;
    mradio_protobuf_send.set_car_control_speed(mcar_control_speed);
    mradio_protobuf_send.set_server_status(mserver_status);
    mradio_protobuf_send.set_cmd_mode(mset_cmd_mode);
    mradio_protobuf_send.set_cmd_control(0);

    givlog->verbose("recData","[%s:] 0x26 id: %d speed: %f cmd_mode: %d",__func__,_data[3],mcar_control_speed,mset_cmd_mode);

    protoMsgSend();
}

void radio::protoMsgSend()
{
    char * strser;
    bool bser;
    int nbytesize;
    nbytesize = mradio_protobuf_send.ByteSize();
    strser = new char[nbytesize];
    bser = mradio_protobuf_send.SerializeToArray(strser,nbytesize);
    if(bser)
        iv::modulecomm::ModuleSendMsg(mpmem_radio_send_addr,strser,nbytesize);
    else
    {
        givlog->error("sendData","[%s:] radio serialize error.",__func__);
//        gfault->SetFaultState(1, 0, "radio serialize err");
    }
    delete strser;

}

void radio::mapProtoMsgSend()
{
    char * strser;
    bool bser;
    int nbytesize;
    nbytesize = mmap_protobuf_index.ByteSize();
    strser = new char[nbytesize];
    bser = mmap_protobuf_index.SerializeToArray(strser,nbytesize);
    if(bser)
        iv::modulecomm::ModuleSendMsg(mpmem_map_addr,strser,nbytesize);
    else
    {
        givlog->error("sendData","[%s:] map index error.",__func__);
//        gfault->SetFaultState(1, 0, "radio serialize err");
    }
    delete strser;

}

bool radio::checkSum(const uint32_t _index)
{
    if( mserial_recv_data.length() > 9 && mserial_recv_data.length() > (mserial_recv_data.data()[2]&0xff) )
    {
        uint8_t _len = (mserial_recv_data.data()[_index+2]&0xff) + _index;
        uint8_t _sum = 0;
        for(int i = _index; i < _len; i++)
        {
            _sum = _sum + (mserial_recv_data.data()[i]&0xff);
        }
        _sum = _sum & 0xFF;
        if(_sum == (uint8_t)mserial_recv_data.data()[_len])
        {
            givlog->debug("recData","check sum success");
            return true;
        }
    }
    givlog->debug("recData","[%s],check sum fail",__func__);
    return false;
}

//串口数据解析
void radio::serialData_Decode(QByteArray rx_data)
{
           uint8_t active_char=0;
           VehicleStatus vehicle_status;
           static uint8_t protocal_flag=0,protocal_index=0;//协议解析标记
           static uint8_t receive_data[30];
           int nbytes=rx_data.length();
           uint16_t check_sum=0;
           double car_control_speed;
           for(int nbytes_index=0;nbytes_index<nbytes;nbytes_index++)
           {
                active_char=rx_data[nbytes_index];
                switch(protocal_flag)
                {
                    case 0:
                          if(active_char == 0xAA)
                          {
                              protocal_index=0;
                              protocal_flag=1;
                              receive_data[protocal_index]=active_char;
                          }
                          break;
                    case 1:
                          protocal_index++;
                          receive_data[protocal_index]=active_char;
                          if(active_char == 0x16)
                          {
                              protocal_flag=2;
                          }
                          break;
                    case 2:
                          protocal_index++;
                          receive_data[protocal_index]=active_char;

                          if(protocal_index>=24) //计算校验和
                          {
                              check_sum=0;
                              for(uint8_t check_i=0;check_i<protocal_index;check_i++)
                              {
                                  check_sum+=receive_data[check_i];
                              }
                              if(receive_data[protocal_index]==(check_sum&0x00ff))
                              {
                                  protocal_flag=3; //校验通过
                              }else{
                                  protocal_flag=0;
                              }
                          }
                          if(protocal_flag==3)
                          {
                            // 解析数据

                              vehicle_status.vehicleID=receive_data[3];
                              vehicle_status.vehicleLat=((receive_data[4]<<24)|(receive_data[5]<<16)|(receive_data[6]<<8)|(receive_data[7]));
                              vehicle_status.vehicleLon=((receive_data[8]<<24)|(receive_data[9]<<16)|(receive_data[10]<<8)|(receive_data[11]));
                              vehicle_status.vehicleSpeed=((receive_data[12]<<8)|(receive_data[13]));
                              vehicle_status.vehicleAng=((receive_data[14]<<8)|(receive_data[15]));
                              vehicle_status.vehicleDistance=((receive_data[16]<<8)|(receive_data[17]));
                              vehicle_status.vehicleProcess=((receive_data[18]<<8)|(receive_data[19]));
                              vehicle_status.map_type=receive_data[20];
                              vehicle_status.status_flag=receive_data[23];
                              car_control_speed=((receive_data[21]<<8)|(receive_data[22]))/10;
                              if(CAR_TYPE==1)
                              {
                                  back_car.vehicleSpeed=vehicle_status.vehicleSpeed;
                                  back_car.vehicleProcess=vehicle_status.vehicleProcess;
                                  back_car.vehicleDistance=vehicle_status.vehicleDistance;
                                  if(back_car.vehicleProcess<4000)
                                  {
                                        formationcontrol(front_car,back_car,&back_car,2);
                                        mradio_protobuf_send.set_car_control_speed(400);
                                  }else{
                                        formationcontrol(front_car,back_car,&back_car,1);
                                        mradio_protobuf_send.set_car_control_speed(600);
                                  }

                              }else if(CAR_TYPE==2)
                              {
                                  mradio_protobuf_send.set_car_control_speed(car_control_speed);
                              }


                              mradio_protobuf_send.set_server_status(true);
                              mradio_protobuf_send.set_cmd_mode(0);
                              mradio_protobuf_send.set_cmd_control(0);
                              givlog->verbose("recData","[%s] receive 0x26 : car_id %d,speed %f,map:%d",__func__,mcar_id, \
                                              car_control_speed, mserver_map_index);

                              protocal_flag=0;
                              protocal_index=0;
                          }
                          break;
                     default:
                          break;
                }
           }
           protoMsgSend();

}

void radio::mapInit()
{
    char * strx = getenv("HOME");
    std::string strmap_dir = strx;
    strmap_dir = strmap_dir + "/map/";
    mvmap_name.append(strmap_dir + "map.txt");
    for(int i = 0; i < mvmap_name.length(); i++)
        givlog->debug("map","[%s] map_name: %s", __func__, mvmap_name.at(i).data());
}







void formationcontrol(VehicleStatus Master,VehicleStatus Slave,VehicleStatus *formationD,unsigned char controlMode)
{
    float kp=3.0,kd=0.6;  //10cm,error=1,kp=2,+0.2km/h
    float error=0,speed_out=0;
    if(controlMode==1){   //进度控制模式
                       kp=3.0;
                       kd=0.6;  //10cm,error=1,kp=2,+0.2km/h
                       error=0;
                       speed_out=0;
                       if((Slave.vehicleProcess>2)&&(abs(Master.vehicleProcess-Slave.vehicleProcess)<1000))
                       {
                           error=Master.vehicleProcess-Slave.vehicleProcess;
                           speed_out=kp*error+kd*(error-Slave.last_error);
                           formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;

                           qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
                           qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;

                           Slave.last_error=error;
                       }else{
                           formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
                       }
                       if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
                       {
                           formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
                       }
                       else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
                       {
                           formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
                       }
    }else if(controlMode==2){  //相对位移控制模式
                      kp=3.0;
                      kd=0.6;  //10cm,error=1,kp=2,+0.2km/h
                      error=0;
                      speed_out=0;
                      if((Slave.vehicleDistanceRel>2)&&(abs(Master.vehicleDistanceRel-Slave.vehicleDistanceRel)<200))
                      {
                          error=Master.vehicleDistanceRel-Slave.vehicleDistanceRel;
                          speed_out=kp*error+kd*(error-Slave.last_error);
                          formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+(int)speed_out;

                          qDebug() <<"error:"<<error<<"last_error:"<<Slave.last_error<<"speed_out:"<<speed_out<<endl;
                          qDebug() <<"Slave_speed:"<<Slave.vehicleSpeed<<"Desired_speed:"<<formationD->vehicleDesiredSpeed<<endl;

                          Slave.last_error=error;
                      }else{
                          formationD->vehicleDesiredSpeed=Slave.INIT_SPEED;
                      }
                      if((formationD->vehicleDesiredSpeed)<Slave.INIT_SPEED-8)
                      {
                          formationD->vehicleDesiredSpeed=Slave.INIT_SPEED-8;
                      }
                      else if((formationD->vehicleDesiredSpeed)>Slave.INIT_SPEED+8)
                      {
                          formationD->vehicleDesiredSpeed=Slave.INIT_SPEED+8;
                      }

    }
}