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; }
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; }
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; }
/** * @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; }
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; }
/** * @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); }
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); } } }
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; }
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); } } }
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"); }
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; } }
/***************************************************************************** @funcname: vam_deinit @brief : vam module unstall @param : None @return : *****************************************************************************/ void vam_deinit() { OSAL_MODULE_DBGPRT(MODULE_NAME, OSAL_DEBUG_INFO, "module deinit\n"); }
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"); }
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"); }
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); }
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; } } }