diff options
Diffstat (limited to 'positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp')
| -rw-r--r-- | positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp | 78 |
1 files changed, 38 insertions, 40 deletions
diff --git a/positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp b/positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp index ddbb09ab..4475b240 100644 --- a/positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp +++ b/positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp @@ -1,5 +1,5 @@ /* - * @copyright Copyright (c) 2016-2019 TOYOTA MOTOR CORPORATION. + * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -44,7 +44,7 @@ static VEHICLESENS_DATA_MASTER gstMotionHeading_n; // NOLINT(readability/no * @retval none */ void VehicleSensInitMotionHeadingN(void) { -// SENSORMOTION_HEADINGINFO_DAT st_heading; + SENSORMOTION_HEADINGINFO_DAT st_heading; memset(&gstMotionHeading_n, 0x00, sizeof(VEHICLESENS_DATA_MASTER)); @@ -52,18 +52,16 @@ void VehicleSensInitMotionHeadingN(void) { gstMotionHeading_n.ul_did = VEHICLE_DID_MOTION_HEADING_NAVI; /** Data size setting */ -// gstMotionHeading_n.us_size = sizeof(SENSORMOTION_HEADINGINFO_DAT); + gstMotionHeading_n.us_size = sizeof(SENSORMOTION_HEADINGINFO_DAT); /** Data content setting */ -// memset(&st_heading, 0x00, sizeof(st_heading)); -// st_heading.getMethod = SENSOR_GET_METHOD_NAVI; -// st_heading.SyncCnt = 0x00; -// st_heading.isEnable = SENSORMOTION_STATUS_DISABLE; -// st_heading.isExistDR = 0x00; -// st_heading.DRStatus = SENSORMOTION_DRSTATUS_INVALID; -// st_heading.posSts = 0x00; -// st_heading.Heading = 0x00; -// memcpy(&gstMotionHeading_n.uc_data[0], &st_heading, sizeof(st_heading)); + memset(&st_heading, 0x00, sizeof(st_heading)); + st_heading.getMethod = SENSOR_GET_METHOD_NAVI; + st_heading.SyncCnt = 0x00; + st_heading.isEnable = SENSORMOTION_STATUS_DISABLE; + st_heading.posSts = 0x00; + st_heading.Heading = 0x00; + memcpy(&gstMotionHeading_n.uc_data[0], &st_heading, sizeof(st_heading)); return; } @@ -80,23 +78,23 @@ void VehicleSensInitMotionHeadingN(void) { * @retval VEHICLESENS_EQ : No data change * @retval VEHICLESENS_NEQ : Data change */ -//u_int8 VehicleSensSetMotionHeadingN(const SENSORMOTION_HEADINGINFO_DAT *pst_heading) { -// u_int8 uc_ret; -// VEHICLESENS_DATA_MASTER *pst_master; -// -// pst_master = &gstMotionHeading_n; -// -// /** With the contents of the current data master,Compare received data */ -// uc_ret = VehicleSensmemcmp(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT)); -// -// /** Received data is set in the data master. */ -// pst_master->ul_did = VEHICLE_DID_MOTION_HEADING_NAVI; -// pst_master->us_size = sizeof(SENSORMOTION_HEADINGINFO_DAT); -// pst_master->uc_rcvflag = VEHICLE_RCVFLAG_ON; -// memcpy(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT)); -// -// return(uc_ret); -//} +u_int8 VehicleSensSetMotionHeadingN(const SENSORMOTION_HEADINGINFO_DAT *pst_heading) { + u_int8 uc_ret; + VEHICLESENS_DATA_MASTER *pst_master; + + pst_master = &gstMotionHeading_n; + + /** With the contents of the current data master,Compare received data */ + uc_ret = VehicleSensmemcmp(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT)); + + /** Received data is set in the data master. */ + pst_master->ul_did = VEHICLE_DID_MOTION_HEADING_NAVI; + pst_master->us_size = sizeof(SENSORMOTION_HEADINGINFO_DAT); + pst_master->uc_rcvflag = VEHICLE_RCVFLAG_ON; + memcpy(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT)); + + return(uc_ret); +} /** * @brief @@ -138,27 +136,27 @@ void VehicleSensGetMotionHeadingN(VEHICLESENS_DATA_MASTER *pst_data) { // LCOV_ */ void VehicleSensGetMotionHeadingnCnvData(VEHICLESENS_DATA_MASTER *pst_data) { const VEHICLESENS_DATA_MASTER *pst_master; -// SENSORMOTION_HEADINGINFO_DAT st_heading; + SENSORMOTION_HEADINGINFO_DAT st_heading; int16 i_heading; pst_master = &gstMotionHeading_n; /* Perform the orientation conversion[-179 to +180] -> [0 to 359] */ -// memcpy(&st_heading, pst_master->uc_data, sizeof(st_heading)); -// i_heading = static_cast<int16>(st_heading.Heading); -// if (i_heading > 0) { -// i_heading = static_cast<int16>(360 - i_heading); -// } else { -// i_heading = static_cast<int16>(i_heading * -1); -// } -// /* Perform unit conversion[Once] -> [0.01 degree] */ -// st_heading.Heading = (u_int16)(i_heading * 100); + memcpy(&st_heading, pst_master->uc_data, sizeof(st_heading)); + i_heading = static_cast<int16>(st_heading.Heading); + if (i_heading > 0) { + i_heading = static_cast<int16>(360 - i_heading); + } else { + i_heading = static_cast<int16>(i_heading * -1); + } + /* Perform unit conversion[Once] -> [0.01 degree] */ + st_heading.Heading = (u_int16)(i_heading * 100); /** Store the data master in the specified destination. */ pst_data->ul_did = pst_master->ul_did; pst_data->us_size = pst_master->us_size; pst_data->uc_rcvflag = pst_master->uc_rcvflag; -// memcpy(pst_data->uc_data, &st_heading, sizeof(st_heading)); + memcpy(pst_data->uc_data, &st_heading, sizeof(st_heading)); return; } |