Ejemplo n.º 1
0
uint16_t  mode_get(void)
{
    uint16_t mode;
    uint8_t get_str;
    //rt_kprintf("----------------------mode---------------------\n");
    drv_fls_read(PARAM_MODE_ADDR,(uint8_t*)&mode,sizeof(uint16_t));

    if(mode == CUSTOM_MODE){
        get_str = 0;
    }
    else if(mode == HIGHWAY_MODE){
        get_str = 1;
    }
    else if(mode == MOUNTAIN_MODE){
        get_str = 2;
    }
    else if(mode == CITY_MODE){
        get_str = 3;
    }
    else{ 
        OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"mode reading error!!\n");
    }
    
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"mode value = %04X,mode is %s\n", mode,mode_string[get_str]);

    return mode;

}
Ejemplo n.º 2
0
int dsmp_recv(wnet_envar_t *p_wnet, wnet_rxinfo_t *rxinfo, 
                       uint8_t *pdata, uint32_t length)
{
    int ret = 0;
    int len = 0;
    uint8_t id = 0;
    uint32_t aid = 0;
    dsmp_hdr_t *p_dsmp;
    security_info_t *p_sec_info = NULL;    
    security_hdr_t *p_sec_hdr  = (security_hdr_t *)pdata;
    

    if(p_sec_hdr->security_type)
    {
        /* security_header */
        len = SECURITY_HEADER_LEN;
        pdata += len;
        p_sec_info = (security_info_t *)pdata;
        len += SECURITY_INFO_LEN;
        pdata += len;        
    }
    else
    {
        len = SECURITY_HEADER_LEN;
        pdata += len;
    }


    p_dsmp = (dsmp_hdr_t *)pdata;        
    aid = cv_ntohl(p_dsmp->aid);
    if (aid == WAVE_AID_vehicle_safety)
    {
        id = p_dsmp->element_id;
        switch (id) 
        {
            case WAVE_ELEMENT_ID_WSM:
            {
                pdata += DSMP_HEADER_LEN;
                length = cv_ntohs(p_dsmp->dsm_length);
                ret = vam_rcp_recv(rxinfo, pdata, length);             
                break;
            }
            default:
            {
                /* realese rxbuf */
                ret = -1;
                OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "Invalid wave elementID[0x%04x], dropped.\n", id);
                break; 
            }
        }
    }
    else
    {
        /* not support. but need to realese rxbuf */
        ret = -1;
        OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "Invalid dsmp aid[0x%04x], dropped.\n", aid);
    }       
    
    return ret;
}
Ejemplo n.º 3
0
int rcp_parse_msg(vam_envar_t *p_vam,
                  wnet_rxinfo_t *rxinfo, 
                  uint8_t *databuf, 
                  uint32_t datalen)
{
    rcp_msgid_t *p_msgid;


    if (datalen < sizeof(rcp_msg_head_t))
    {
        return -1;
    }

    p_msgid = (rcp_msgid_t *)databuf;

    switch(p_msgid->id)
    {
        case RCP_MSG_ID_BSM:
        {
            
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_TRACE, "recieve bsm!\n");
            rcp_parse_bsm(p_vam, rxinfo, databuf, datalen);
            break;
        }

#ifndef RSU_TEST
        case RCP_MSG_ID_EVAM:
        {
            /* receive evam, then pause sending bsm msg */
            if(2 == p_vam->working_param.bsm_pause_mode)
            {
                vsm_pause_bsm_broadcast(p_vam);
            }
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_TRACE, "recieve evam!\n");
            rcp_parse_evam(p_vam, rxinfo, databuf, datalen);
            break;
        }
        case RCP_MSG_ID_RSA:
        {
            rcp_parse_rsa(p_vam, rxinfo, databuf, datalen);
            break;
        }
#endif    
        default:
        {
            break;
        }          
    }

    return p_msgid->id;
}
Ejemplo n.º 4
0
/**
  * @brief  Break up ethernet frame.
  * @param  See below.
  * @retval Not sure.
  */
int enet_receive
(
    /* Pointer to wireless network layer's environment structure. */
    wnet_envar_t *p_wnet, 

    /* Pointer to wireless network layer receive information. */
    wnet_rxinfo_t *rxinfo, 

    /* Data address. */
    uint8_t *pdata, 

    /* Data length. */
    uint32_t length
)
{
    int                              result = -1;
    wnet_enet_header_st_ptr enet_header_ptr = (wnet_enet_header_st_ptr)pdata;


    /* Check the frame length. */
    if (length < WNET_ENET_HEADER_ST_LEN)
    {
        OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "Recv frame length [%d] is too short, dropped.\n", length);
        return -1;
    }

    /* Prepare for next step */
    pdata += WNET_ENET_HEADER_ST_LEN;
    length -= WNET_ENET_HEADER_ST_LEN;

    switch (cv_ntohs(enet_header_ptr->ether_type)) 
    {
        case LLC_ETHERTYPE_DSMPv1:
        {
            rxinfo->protocol = WNET_TRANS_PROT_DSMP;
            result = dsmp_recv(p_wnet, rxinfo, pdata, length);    
            break;       
        }
        
        default:
        {
            result = -1;
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "Invalid ether type[0x%04x], dropped.\n", enet_header_ptr->ether_type);
            break;
        }
    }   

    return result;
}
Ejemplo n.º 5
0
int vam_add_event_queue(vam_envar_t *p_vam, 
                             uint16_t msg_id, 
                             uint16_t msg_len, 
                             uint32_t msg_argc,
                             void    *msg_argv)
{
    int err = OSAL_STATUS_NOMEM;
    sys_msg_t *p_msg;

    p_msg = osal_malloc(sizeof(sys_msg_t));
    if (p_msg) {
        p_msg->id = msg_id;
        p_msg->len = msg_len;
        p_msg->argc = msg_argc;
        p_msg->argv = msg_argv;
        err = osal_queue_send(p_vam->queue_vam, p_msg);
    }

    if (err != OSAL_STATUS_SUCCESS) {
        OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_WARN, "%s: failed=[%d], msg=%04x\n",\
                           __FUNCTION__, err, msg_id);
        osal_free(p_msg);                   
    }

    return err;
}
Ejemplo n.º 6
0
/**
  * @brief  Build ethernet frame and send out.
  * @param  See below.
  * @retval Not sure.
  */
int enet_send
(
    /* Pointer to wireless network layer's environment structure. */
    wnet_envar_t *p_wnet, 

    /* Pointer to wireless network layer transfer information. */
    wnet_txinfo_t *txinfo, 

    /* Data address. */
    uint8_t *pdata, 

    /* Data length. */
    uint32_t length
)
{
    wnet_enet_header_st_ptr enet_header_ptr = NULL;

    
    /* Reserve room for ENET header. */
    pdata -= WNET_ENET_HEADER_ST_LEN;
    length += WNET_ENET_HEADER_ST_LEN;
    enet_header_ptr = (wnet_enet_header_st_ptr)pdata;

    /* Initialize enet frame header.  */
    memset(enet_header_ptr, 0, WNET_ENET_HEADER_ST_LEN);

    /* Set enet frame destination address and source address. */
    COPY_MAC_ADDR(enet_header_ptr->dest_address, BroadcastAddr);
    
    /* Mac address is bind to the CPU's unique ID */
    enet_header_ptr->src_address[0] = 0x00;
    enet_header_ptr->src_address[1] = 0x11;
    enet_header_ptr->src_address[2] = 0x22;
    enet_header_ptr->src_address[3] = des(2);
    enet_header_ptr->src_address[4] = des(1);
    enet_header_ptr->src_address[5] = des(0);


    switch (txinfo->protocol) 
    {
        case WNET_TRANS_PROT_DSMP:
        { 
            enet_header_ptr->ether_type = cv_ntohs(LLC_ETHERTYPE_DSMPv1);    break;
        }
    
        default:
        {
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "Invalid protocol[0x%04x], dropped.\n", txinfo->protocol);
            return -1;
        }
    }
    
    return fp_send(p_wnet, txinfo, pdata, length);
}
Ejemplo n.º 7
0
void vam_thread_entry(void *parameter)
{
    rt_err_t err;
    sys_msg_t *p_msg;
    vam_envar_t *p_vam = (vam_envar_t *)parameter;

    OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "%s: ---->\n", __FUNCTION__);

	while(1){
        err = osal_queue_recv(p_vam->queue_vam, &p_msg, RT_WAITING_FOREVER);
        if (err == RT_EOK){
            vam_main_proc(p_vam, p_msg);
            osal_free(p_msg);
        }
        else{
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_ERROR, "%s: osal_queue_recv error [%d]\n",\
                            __FUNCTION__, err);
        }
	}
}
Ejemplo n.º 8
0
int vam_add_event_queue_2(vam_envar_t *p_vam, void *p_msg)
{
    int err;
    
    err = osal_queue_send(p_vam->queue_vam, p_msg);
    if (err != OSAL_STATUS_SUCCESS){
        OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_WARN, "%s: failed=[%d], msg=%04x\n",\
                         __FUNCTION__, err, ((sys_msg_t *)p_msg)->id);
    }

    return err;
}
Ejemplo n.º 9
0
void wnet_tx_thread_entry(void *parameter)
{
    int err;
    wnet_envar_t *p_wnet = (wnet_envar_t *)parameter;


    OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_TRACE, "%s: ---->\n", __FUNCTION__);

	while(1)
    {
	    err = osal_sem_take(p_wnet->sem_wnet_tx, OSAL_WAITING_FOREVER);

        if (err == OSAL_STATUS_SUCCESS)
        {
            fp_tx_handler(p_wnet);
        }
        else
        {
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_ERROR, "%s:failed to take semaphore(%d)\n", __FUNCTION__, err);
        }
	}
}
Ejemplo n.º 10
0
void param_get(void)
{
    //cfg_param_t  *param_temp;

    //drv_fls_read(PARAM_ADDR,(uint8_t *)param_temp,sizeof(cfg_param_t));
        
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"-------------------parameters in ram------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"work mode is %d\n",param_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------vam---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"ID(0)=%d%d%d%d\n",cms_param.pid[0],cms_param.pid[1],cms_param.pid[2],cms_param.pid[3]);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_hops(1)=%d\n", cms_param.vam.bsm_hops);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_mode(2)=%d\n", cms_param.vam.bsm_boardcast_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_saftyfactor(3)=%d\n", cms_param.vam.bsm_boardcast_saftyfactor);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_pause_mode(4)=%d\n", cms_param.vam.bsm_pause_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_pause_hold_time(5)=%d (s)\n", cms_param.vam.bsm_pause_hold_time);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_period(6)=%d (ms)\n", cms_param.vam.bsm_boardcast_period);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_hops(7)=%d\n", cms_param.vam.evam_hops);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_broadcast_type(8)=%d\n", cms_param.vam.evam_broadcast_type);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_broadcast_peroid(9)=%d (ms)\n\n", cms_param.vam.evam_broadcast_peroid);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------vsa---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.danger_detect_speed_threshold(10)=%d (km/h)\n", cms_param.vsa.danger_detect_speed_threshold);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.lane_dis(11)=%d (m)\n", cms_param.vsa.lane_dis);
    
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_saftyfactor(12)=%d\n", cms_param.vsa.crd_saftyfactor);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_oppsite_speed(13)=%d (km/h)\n", cms_param.vsa.crd_oppsite_speed);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_oppsite_rear_speed(14)=%d (km/h)\n", cms_param.vsa.crd_oppsite_rear_speed);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_rear_distance(15)=%d (m)\n", cms_param.vsa.crd_rear_distance);

        
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_mode(16)=%d\n", cms_param.vsa.ebd_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_acceleration_threshold(17)=%d (m/s2)\n", cms_param.vsa.ebd_acceleration_threshold);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_alert_hold_time(18)=%d (s)\n\n", cms_param.vsa.ebd_alert_hold_time);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------gsnr---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.gsnr_cal_step(19)=%d\n",cms_param.gsnr.gsnr_cal_step);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.gsnr_cal_thr(20)=%d\n",cms_param.gsnr.gsnr_cal_thr);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.gsnr_ebd_thr(21)=%d\n",cms_param.gsnr.gsnr_ebd_thr);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.gsnr_ebd_cnt(22)=%d\n",cms_param.gsnr.gsnr_ebd_cnt);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceV_x(23)=%d\n",cms_param.gsnr.AcceV_x);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceV_y(24)=%d\n",cms_param.gsnr.AcceV_y);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceV_z(25)=%d\n",cms_param.gsnr.AcceV_z);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceAhead_x(26)=%d\n",cms_param.gsnr.AcceAhead_x);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceAhead_y(27)=%d\n",cms_param.gsnr.AcceAhead_y);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"gsnr.AcceAhead_z(28)=%d\n",cms_param.gsnr.AcceAhead_z);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------wnet---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"wnet.channel(29)=%d\n",cms_param.wnet.channel);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"wnet.txrate(30)=%d\n",cms_param.wnet.txrate);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------voc----------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"voc.fg_volume(32)=%d\n",cms_param.voc.fg_volume);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"voc.bg_volume(33)=%d\n",cms_param.voc.bg_volume);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"voc.speed(34)=%d\n",cms_param.voc.speed);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"...\n");

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------end---------------------\n");
}
Ejemplo n.º 11
0
void vam_main_proc(vam_envar_t *p_vam, sys_msg_t *p_msg)
{
    switch(p_msg->id){
        case VAM_MSG_START:
            OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_TRACE, "%s: VAM_MSG_START\n", 
                                __FUNCTION__);

            p_vam->flag |= VAM_FLAG_RX;
            osal_timer_start(p_vam->timer_neighbour_life);
            
            if (p_vam->working_param.bsm_boardcast_mode != BSM_BC_MODE_DISABLE){
                p_vam->flag |= VAM_FLAG_TX_BSM;
                vsm_start_bsm_broadcast(p_vam);
            }
            
            break;

        case VAM_MSG_STOP:

            if (p_vam->flag && VAM_FLAG_TX_BSM){
                vsm_stop_bsm_broadcast(p_vam);
            }

            p_vam->flag &= ~(VAM_FLAG_RX|VAM_FLAG_TX_BSM);
            osal_timer_stop(p_vam->timer_neighbour_life);
            
            break;

        case VAM_MSG_RCPTX:
            if (p_msg->argc == RCP_MSG_ID_BSM){
                rcp_send_bsm(p_vam);
            }
            if (p_msg->argc == RCP_MSG_ID_EVAM){
                rcp_send_evam(p_vam);
            }
            if (p_msg->argc == RCP_MSG_ID_RSA){
                rcp_send_rsa(p_vam);
            }

            break;

        case VAM_MSG_RCPRX:
            rcp_parse_msg(p_vam, (wnet_rxinfo_t *)p_msg->argv, \
                          (uint8_t *)p_msg->argc, p_msg->len);

            wnet_release_rxbuf(WNET_RXBUF_PTR(p_msg->argv));
            
            break;

        case VAM_MSG_NEIGH_TIMEOUT:
            vam_update_sta(p_vam);
            break;

        case VAM_MSG_GPSDATA:
            lip_gps_proc(p_vam, (uint8_t *)p_msg->argv, p_msg->len);

            break;
            
        default:
            break;
    }
}
Ejemplo n.º 12
0
/*****************************************************************************
 @funcname: vam_deinit
 @brief   : vam module unstall
 @param   : None
 @return  : 
*****************************************************************************/
void vam_deinit()
{
	OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "module deinit\n");
}
Ejemplo n.º 13
0
void vam_init(void)
{
    int i;
    vam_envar_t *p_vam = &p_cms_envar->vam;
    uint8_t zero_pid[RCP_TEMP_ID_LEN] = {0};

    p_vam_envar = p_vam;
    
    memset(p_vam, 0, sizeof(vam_envar_t));
    memcpy(&p_vam->working_param, &p_cms_param->vam, sizeof(vam_config_t));
    if (0 == memcmp(p_cms_param->pid, zero_pid, RCP_TEMP_ID_LEN)){
        for (i=0; i<RCP_TEMP_ID_LEN; i++){
            p_vam->local.pid[i] = des(RCP_TEMP_ID_LEN-1-i);
        }
    }
    else {
        memcpy(p_vam->local.pid, p_cms_param->pid, RCP_TEMP_ID_LEN);
    }
    OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "PID: %02x %02x %02x %02x\r\n", \
        p_vam->local.pid[0], p_vam->local.pid[1], p_vam->local.pid[2], p_vam->local.pid[3]);

    INIT_LIST_HEAD(&p_vam->neighbour_list);
    INIT_LIST_HEAD(&p_vam->sta_free_list);
    for(i = 0;i< VAM_NEIGHBOUR_MAXNUM;i++){
        list_add_tail(&p_vam->remote[i].list, &p_vam->sta_free_list);
    }

     /* os object for vam */
    p_vam->queue_vam = osal_queue_create("q-vam", VAM_QUEUE_SIZE);
    osal_assert(p_vam->queue_vam != RT_NULL);
   
	p_vam->task_vam = osal_task_create("t-vam",
                           vam_thread_entry, p_vam,
                           RT_VAM_THREAD_STACK_SIZE, RT_VAM_THREAD_PRIORITY);
    osal_assert(p_vam->task_vam != RT_NULL)

    p_vam->timer_send_bsm = osal_timer_create("tm-sb",timer_send_bsm_callback,p_vam,\
        BSM_SEND_PERIOD_DEFAULT, RT_TIMER_FLAG_PERIODIC); 					
    osal_assert(p_vam->timer_send_bsm != NULL);

    p_vam->timer_bsm_pause = osal_timer_create("tm-bp",timer_bsm_pause_callback,p_vam,\
        BSM_PAUSE_HOLDTIME_DEFAULT,RT_TIMER_FLAG_ONE_SHOT); 					
    osal_assert(p_vam->timer_bsm_pause != RT_NULL);

    p_vam->timer_send_evam = osal_timer_create("tm-se",timer_send_evam_callback, p_vam,\
        EVAM_SEND_PERIOD_DEFAULT,RT_TIMER_FLAG_PERIODIC); 					
    osal_assert(p_vam->timer_send_evam != RT_NULL);

    p_vam->timer_gps_life = osal_timer_create("tm-gl",timer_gps_life_callback,p_vam,\
        BSM_GPS_LIFE_DEFAULT, RT_TIMER_FLAG_ONE_SHOT); 					
    osal_assert(p_vam->timer_gps_life != RT_NULL);

    p_vam->timer_neighbour_life = osal_timer_create("tm-nl",timer_neigh_time_callback,p_vam,\
        NEIGHBOUR_LIFE_ACCUR, RT_TIMER_FLAG_PERIODIC); 					
    osal_assert(p_vam->timer_neighbour_life != RT_NULL);

    p_vam->sem_sta = osal_sem_create("s-sta", 1);
    osal_assert(p_vam->sem_sta != RT_NULL);

	OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "module initial\n");
}
Ejemplo n.º 14
0
void param_get(void)
{
    //cfg_param_t  *param_temp;

    //drv_fls_read(PARAM_ADDR,(uint8_t *)param_temp,sizeof(cfg_param_t));
        
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"-------------------parameters in ram------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"work mode is %d\n",param_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------vam---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"ID(0)=%d%d%d%d\n",p_cms_param->pid[0],p_cms_param->pid[1],p_cms_param->pid[2],p_cms_param->pid[3]);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_hops(1)=%d\n", p_cms_param->vam.bsm_hops);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_mode(2)=%d\n", p_cms_param->vam.bsm_boardcast_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_saftyfactor(3)=%d\n", p_cms_param->vam.bsm_boardcast_saftyfactor);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_pause_mode(4)=%d\n", p_cms_param->vam.bsm_pause_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_pause_hold_time(5)=%d (s)\n", p_cms_param->vam.bsm_pause_hold_time);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.bsm_boardcast_period(6)=%d (ms)\n", p_cms_param->vam.bsm_boardcast_period);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_hops(7)=%d\n", p_cms_param->vam.evam_hops);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_broadcast_type(8)=%d\n", p_cms_param->vam.evam_broadcast_type);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vam.evam_broadcast_peroid(9)=%d (ms)\n\n", p_cms_param->vam.evam_broadcast_peroid);

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------vsa---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.danger_detect_speed_threshold(10)=%d (km/h)\n", p_cms_param->vsa.danger_detect_speed_threshold);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.lane_dis(11)=%d (m)\n", p_cms_param->vsa.lane_dis);
    
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_saftyfactor(12)=%d\n", p_cms_param->vsa.crd_saftyfactor);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_oppsite_speed(13)=%d (km/h)\n", p_cms_param->vsa.crd_oppsite_speed);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_oppsite_rear_speed(14)=%d (km/h)\n", p_cms_param->vsa.crd_oppsite_rear_speed);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.crd_rear_distance(15)=%d (m)\n", p_cms_param->vsa.crd_rear_distance);

        
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_mode(16)=%d\n", p_cms_param->vsa.ebd_mode);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_acceleration_threshold(17)=%d (m/s2)\n", p_cms_param->vsa.ebd_acceleration_threshold);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"vsa.ebd_alert_hold_time(18)=%d (s)\n\n", p_cms_param->vsa.ebd_alert_hold_time);


    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------wnet---------------------\n");
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"wnet.channel(29)=%d\n",p_cms_param->wnet.channel);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"wnet.txrate(30)=%d\n",p_cms_param->wnet.txrate);
    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"...\n");

    OSAL_MODULE_DBGPRT(MODULE_NAME,OSAL_DEBUG_INFO,"----------------------end---------------------\n");
}
Ejemplo n.º 15
0
double vsm_get_relative_pos(vam_stastatus_t *p_src, vam_stastatus_t *p_dest,uint8_t vsa_print_en)
{
    double lat1, lng1, lat2, lng2, lat3, lng3;
    double distance_1_2, distance_2_3;
    double angle, delta;

    /* reference point */
    lat1 = p_src->pos.lat;
    lng1 = p_src->pos.lon;

    /* destination point */
    lat2 = p_dest->pos.lat;
    lng2 = p_dest->pos.lon;

    /* temp point */
    lat3 = lat1;
    lng3 = lng2;

    distance_1_2 = getDistanceVer2(lat1, lng1, lat2, lng2);
    distance_2_3 = getDistanceVer2(lat2, lng2, lat3, lng3);
    angle = acos(distance_2_3/distance_1_2)*180/PI;

    /* calculate the relative angle against north, clockwise  */
    if (lat2 >= lat1){
    /* north */
        if (lng2 >= lng1){
        /* easts */
            //equal
        }
        else{
            angle = 360-angle;
        }
    }
    else{
    /* south */
        if (lng2 >= lng1){
        /* easts */
            angle = 180-angle;
        }
        else{
            angle = 180+angle;
        }
    }

    /* calculate the angle detra between local front and remote position  */
    if (angle > p_src->dir){
        delta = angle - p_src->dir;
    }
    else{
        delta = p_src->dir - angle;
    }

    if (delta > 180){
        delta = 360 - delta;
    }

    distance_1_2 *= 1000; /* convert from Km to m */
/*	
	rt_kprintf("My head:%s(%d), Your pos:%s(%d); Our delta:%d, distance:%d\n", \
				   _directfromangle((int)p_src->dir), (int)p_src->dir,\
				   _directfromangle((int)angle), (int)angle,\
				  (int)delta, (int)distance_1_2);

if((print_flag)&&(vsa_print_en))	
    rt_kprintf("tick =  %lu, My head:%s(%d),ID:%d%d%d%d, Your pos:%s(%d),ID:%d%d%d%d, Our delta:%d, distance:%d\n",rt_tick_get(), \
               _directfromangle((int)p_src->dir), (int)p_src->dir,p_src->pid[0],p_src->pid[1],p_src->pid[2],p_src->pid[3],\
               _directfromangle((int)p_dest->dir), (int)p_dest->dir,p_dest->pid[0],p_dest->pid[1],p_dest->pid[2],p_dest->pid[3],\
              (int)delta, (int)distance_1_2);
*/
    OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_LOUD,"My head:%s(%d),ID:%d%d%d%d, Your pos:%s(%d),ID:%d%d%d%d, Our delta:%d, distance:%d\n",\
               _directfromangle((int)p_src->dir), (int)p_src->dir,p_src->pid[0],p_src->pid[1],p_src->pid[2],p_src->pid[3],\
               _directfromangle((int)p_dest->dir), (int)p_dest->dir,p_dest->pid[0],p_dest->pid[1],p_dest->pid[2],p_dest->pid[3],\
              (int)delta, (int)distance_1_2);
              
//#endif
    return (delta <= 45)? distance_1_2:(-distance_1_2);
}
Ejemplo n.º 16
0
static void key_thread_entry(void *parameter)
{
    sys_envar_t *p_sys = (sys_envar_t *)parameter;
    uint32_t  key_value = 0;
	
	
	key_GPIO_Configuration();
	
	key = (struct rtgui_key*)rt_malloc (sizeof(struct rtgui_key));
    if (key == RT_NULL)
		return ; /* no memory yet */
	
	
	key->key_last = 0;
	key->key_current = 0;
	key->key_get = 0;
	key->key_debounce_count = 0;
	key->key_long_count = 0;
	key->key_special_count = 0;
	key->key_relase_count = 0;
	key->key_flag = 0;	
	
	while(1)
	{	
		rt_thread_delay(2);	
		
        key->key_current = key_up_GETVALUE();
        key->key_current |= key_down_GETVALUE()<<1;	
        key->key_current |= key_third_GETVALUE()<<2;
		
	  #if LCD_VERSION==1


	  #else 
		key->key_current=~(key->key_current);
		key->key_current&=0x00000007;
	
	  #endif

		key->key_flag &= ~C_FLAG_SHORT;
		key->key_flag &= ~C_FLAG_COUNT;
		key->key_flag &= ~C_FLAG_LONG;
		key->key_get = 0;	


	/*对于有长按和短按的特殊键做处理*/	
	if ((key->key_flag)&C_FLAG_RELASE)
	{//检查放键
		if (key->key_current == 0)
		{
			if ((++(key->key_relase_count)) >= C_RELASE_COUT)
			{ //按键已经放开
				key->key_relase_count = 0;
				key->key_flag &= ~C_FLAG_RELASE;
			}
		}
		else
		{
			key->key_relase_count = 0;
		}
	}
	else
	{//检查按键
		if (key->key_current == C_SPECIAL_KEY)		
		{
			if ((++(key->key_special_count)) >= C_SPECIAL_LONG_COUT)
			{
				key->key_special_count = 0;
				
				key->key_get = C_HOME_KEY;      
				key->key_flag |= C_FLAG_LONG;	//特殊键 长按键按下
				key->key_flag |= C_FLAG_RELASE;;//按下后要求检测放键
			}
		}
		else
		{//放开键后才检测短按
			if ((key->key_special_count >= C_SHORT_COUT) && (key->key_special_count <C_SHORT_COUT+30)) 
			{
				key->key_get = C_SPECIAL_KEY;
				key->key_flag |= C_FLAG_SHORT;	//特殊键 短按键按下
			}
			key->key_special_count = 0;
		}
	}
	
// 普通按键处理
	if((key->key_current == 0)||(key->key_current != key->key_last)|| (key->key_current == C_SPECIAL_KEY))
	{
		key->key_debounce_count = 0;	//第一次	
		key->key_long_count=0;	        //清除长按键计数器
	}
	else
	{
		if(++(key->key_debounce_count) == DEBOUNCE_SHORT_TIME)
		{
			key->key_get = key->key_current;
			key->key_flag |= C_FLAG_SHORT;	//短按键按下
		}
		if(key->key_debounce_count == DEBOUNCE_COUT_FIRST + DEBOUNCE_COUT_INTERVAL)
		{
			key->key_get = key->key_current;
			key->key_flag |= C_FLAG_COUNT;	//连按键 按键按下
			key->key_debounce_count = DEBOUNCE_COUT_FIRST;
			++(key->key_long_count);			
		}
	
		if(key->key_long_count == DEBOUNCE_LONG_TIME)
		{
			key->key_get = key->key_current;
			key->key_flag |= C_FLAG_LONG;	//短按键按下
			key->key_long_count=DEBOUNCE_LONG_TIME+1;
		}		
	}
	
	key->key_last = key->key_current;				// 保存本次键值
			
	if (key->key_get)
	{	
		if (((key->key_get)==C_UP_KEY) && ((key->key_flag) & C_FLAG_SHORT))
            {
                key_value = C_UP_KEY;
                OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO,"key0 press!\n\n");

            }      
        
	
		if (((key->key_get)==C_DOWN_KEY) && ((key->key_flag) & C_FLAG_SHORT))
            {      
                key_value = C_DOWN_KEY;
                OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO,"key1 press!\n\n");

            }

		if (((key->key_get)==C_LEFT_KEY) && ((key->key_flag) & C_FLAG_SHORT))
            {      
                key_value = C_LEFT_KEY;
                OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO,"key2 press!\n\n");

            }
		if(key_value)	
			hi_add_event_queue(p_sys,SYS_MSG_HI_IN_UPDATE,key_value,HI_IN_KEY_PRESSED,NULL);

		key_value = 0;
	}	
	
	}
}