vehicleservice/positioning/server/src/Sensor/VehicleSens_Did_MotionHeading_n.cpp

   1 /*
   2  * @copyright Copyright (c) 2016-2020 TOYOTA MOTOR CORPORATION.
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16
  17 /**
  18  * @file
  19  *  VehicleSens_Did_MotionHeading_n.cpp
  20  * @brief
  21  */
  22 /*---------------------------------------------------------------------------------*
  23  * Include Files                                                                   *
  24  *---------------------------------------------------------------------------------*/
  25 #include <vehicle_service/positioning_base_library.h>
  26 #include "VehicleSens_DataMaster.h"
  27 #include "SensorMotion_API.h"
  28 #include "CommonDefine.h"
  29
  30 /*---------------------------------------------------------------------------------*
  31  * Global Value                                                                    *
  32  *---------------------------------------------------------------------------------*/
  33 static  VEHICLESENS_DATA_MASTER  gstMotionHeading_n;    // NOLINT(readability/nolint)
  34
  35 /**
  36  * @brief
  37  *  Orientation information data master initialization process(NAVI information)
  38  *
  39  *  Initialize the orientation information data master
  40  *
  41  * @param[in]   none
  42  * @param[out]  none
  43  * @return      none
  44  * @retval      none
  45  */
  46 void VehicleSensInitMotionHeadingN(void) {
  47     SENSORMOTION_HEADINGINFO_DAT  st_heading;
  48
  49     memset(&gstMotionHeading_n, 0x00, sizeof(VEHICLESENS_DATA_MASTER));
  50
  51     /** Data ID setting */
  52     gstMotionHeading_n.ul_did   = VEHICLE_DID_MOTION_HEADING_NAVI;
  53
  54     /** Data size setting */
  55     gstMotionHeading_n.us_size  = sizeof(SENSORMOTION_HEADINGINFO_DAT);
  56
  57     /** Data content setting */
  58     memset(&st_heading, 0x00, sizeof(st_heading));
  59     st_heading.getMethod = SENSOR_GET_METHOD_NAVI;
  60     st_heading.SyncCnt   = 0x00;
  61     st_heading.isEnable  = SENSORMOTION_STATUS_DISABLE;
  62     st_heading.posSts    = 0x00;
  63     st_heading.Heading   = 0x00;
  64     memcpy(&gstMotionHeading_n.uc_data[0], &st_heading, sizeof(st_heading));
  65
  66     return;
  67 }
  68
  69 /**
  70  * @brief
  71  *  Compass Data Master SET Processing(NAVI information)
  72  *
  73  *  Update the orientation information data master
  74  *
  75  * @param[in]  VEHICLESENS_DATA_MASTER *pst_heading     : Pointer to the data master acquisition destination
  76  * @param[out] none
  77  * @return     u_int8
  78  * @retval     VEHICLESENS_EQ  : No data change
  79  * @retval     VEHICLESENS_NEQ : Data change
  80  */
  81 u_int8 VehicleSensSetMotionHeadingN(const SENSORMOTION_HEADINGINFO_DAT *pst_heading) {
  82     u_int8            uc_ret;
  83     VEHICLESENS_DATA_MASTER    *pst_master;
  84
  85     pst_master = &gstMotionHeading_n;
  86
  87     /** With the contents of the current data master,Compare received data */
  88     uc_ret = VehicleSensmemcmp(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT));
  89
  90     /** Received data is set in the data master. */
  91     pst_master->ul_did    = VEHICLE_DID_MOTION_HEADING_NAVI;
  92     pst_master->us_size    = sizeof(SENSORMOTION_HEADINGINFO_DAT);
  93     pst_master->uc_rcvflag  = VEHICLE_RCVFLAG_ON;
  94     memcpy(pst_master->uc_data, pst_heading, sizeof(SENSORMOTION_HEADINGINFO_DAT));
  95
  96     return(uc_ret);
  97 }
  98
  99 /**
 100  * @brief
 101  *  Compass Data Master GET Processing(NAVI information)
 102  *
 103  *  Provide an orientation information data master
 104  *
 105  * @param[in]  none
 106  * @param[out] VEHICLESENS_DATA_MASTER *pst_data : Pointer to the data master acquisition destination
 107  * @return     none
 108  * @retval     none
 109  */
 110 void VehicleSensGetMotionHeadingN(VEHICLESENS_DATA_MASTER *pst_data) {  // LCOV_EXCL_START 8: dead code.
 111     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 112     const VEHICLESENS_DATA_MASTER *pst_master;
 113
 114     pst_master = &gstMotionHeading_n;
 115
 116     /** Store the data master in the specified destination. */
 117     pst_data->ul_did      = pst_master->ul_did;
 118     pst_data->us_size     = pst_master->us_size;
 119     pst_data->uc_rcvflag  = pst_master->uc_rcvflag;
 120     memcpy(pst_data->uc_data, pst_master->uc_data, pst_master->us_size);
 121
 122     return;
 123 }
 124 // LCOV_EXCL_STOP
 125
 126 /**
 127  * @brief
 128  *  Compass Data Master GET Processing(NAVI information)
 129  *
 130  *  Providing orientation information data master with orientation and unit conversion
 131  *
 132  * @param[in]  none
 133  * @param[out] VEHICLESENS_DATA_MASTER *pst_data : Pointer to the data master acquisition destination
 134  * @return     none
 135  * @retval     none
 136  */
 137 void VehicleSensGetMotionHeadingnCnvData(VEHICLESENS_DATA_MASTER *pst_data) {
 138     const VEHICLESENS_DATA_MASTER *pst_master;
 139     SENSORMOTION_HEADINGINFO_DAT   st_heading;
 140     int16 i_heading;
 141
 142     pst_master = &gstMotionHeading_n;
 143
 144     /* Perform the orientation conversion[-179 to +180] -> [0 to 359] */
 145     memcpy(&st_heading, pst_master->uc_data, sizeof(st_heading));
 146     i_heading = static_cast<int16>(st_heading.Heading);
 147     if (i_heading > 0) {
 148         i_heading = static_cast<int16>(360 - i_heading);
 149     } else {
 150         i_heading = static_cast<int16>(i_heading * -1);
 151     }
 152     /* Perform unit conversion[Once] -> [0.01 degree] */
 153     st_heading.Heading = (u_int16)(i_heading * 100);
 154
 155     /** Store the data master in the specified destination. */
 156     pst_data->ul_did    = pst_master->ul_did;
 157     pst_data->us_size    = pst_master->us_size;
 158     pst_data->uc_rcvflag  = pst_master->uc_rcvflag;
 159     memcpy(pst_data->uc_data, &st_heading, sizeof(st_heading));
 160
 161     return;
 162 }