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;
}
}
}