vehicleservice/positioning/server/src/Sensor/DeadReckoning_DataMasterMain.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 name    :DeadReckoning_DataMasterMain.cpp
  19  *  System name    :GPF
  20  *  Subsystem name  :Vehicle sensor process
  21  *  Program name  :Vehicle SW Data Master
  22  *  Module configuration  :DeadReckoningInitDataMaster()    Guess Navigation Data Master Initialization Function
  23  *          :DeadReckoningSetDataMaster()      Estimated Navigational Data Master SW Data Set Processing
  24  *          :DeadReckoningGetDataMaster()      Estimated Navigational Data Master Get Processing
  25  ******************************************************************************/
  26
  27 #include <vehicle_service/positioning_base_library.h>
  28 #include "DeadReckoning_DataMaster.h"
  29
  30 /*************************************************/
  31 /*       Global variable                           */
  32 /*************************************************/
  33
  34 #define DR_DEBUG 0
  35
  36 /*******************************************************************************
  37 * MODULE   : DeadReckoningInitDataMaster
  38 * ABSTRACT  : Initialization of Guess Navigation Data Master
  39 * FUNCTION  : Initialize the estimated navigation data master
  40 * ARGUMENT  : void
  41 * NOTE    :
  42 * RETURN   : void
  43 ******************************************************************************/
  44 void DeadReckoningInitDataMaster(void) {  // LCOV_EXCL_START 8: dead code.
  45     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
  46     /* Vehicle sensor data master initialization */
  47     DeadReckoningInitLongitudeDr();
  48     DeadReckoningInitLatitudeDr();
  49     DeadReckoningInitAltitudeDr();
  50     DeadReckoningInitSpeedDr();
  51     DeadReckoningInitHeadingDr();
  52     DeadReckoningInitSnsCounterDr();
  53     DeadReckoningInitGyroOffsetDr();
  54     DeadReckoningInitGyroScaleFactorDr();
  55     DeadReckoningInitGyroScaleFactorLevelDr();
  56     DeadReckoningInitSpeedPulseScaleFactorDr();
  57     DeadReckoningInitSpeedPulseScaleFactorLevelDr();
  58 }
  59
  60 /*******************************************************************************
  61 * MODULE   : DeadReckoning_SetDataMaster_Sub
  62 * ABSTRACT  : Estimated Navigational Data Master SW Data Set Processing
  63 * FUNCTION  : Set estimated navigation data
  64 * ARGUMENT  : *p_data      : SW vehicle signal notification data
  65 *       : p_datamaster_set_n  : Data Master Set Notification(Callback function)
  66 * NOTE    :
  67 * RETURN   : void
  68 ******************************************************************************/
  69 void DeadReckoningSetDataMaster(const DEADRECKONING_DATA_MASTER *p_data,
  70                                 PFUNC_DR_DMASTER_SET_N p_datamaster_set_n) {
  71     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
  72     u_int8  chg_type;
  73
  74     static u_int8  gyro_parameter_chg_type      = 0;
  75     static u_int8  speedpulse_parameter_chg_type  = 0;
  76
  77     /*------------------------------------------------------*/
  78     /*  Call the data set processing associated with the DID      */
  79     /*  Call the data master set notification process      */
  80     /*------------------------------------------------------*/
  81     switch (p_data->ul_did) {
  82         case VEHICLE_DID_DR_LONGITUDE:
  83         {
  84 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
  85             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
  86                           p_data->ul_did, p_data->us_size);
  87 #endif
  88             chg_type = DeadReckoningSetLongitudeDr(p_data);
  89             /* Implementation of delivery process at LATITUDE updating timings */
  90             /* Since the order of transmission (updating) at the main receiver is fixed, there is no problem. */
  91             break;
  92         }
  93         case VEHICLE_DID_DR_LATITUDE:
  94         {
  95 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
  96             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
  97                           p_data->ul_did, p_data->us_size);
  98 #endif
  99             chg_type = DeadReckoningSetLatitudeDr(p_data);
 100             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 101             break;
 102         }
 103         case VEHICLE_DID_DR_ALTITUDE:
 104         {
 105 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 106             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 107                           p_data->ul_did, p_data->us_size);
 108 #endif
 109             chg_type = DeadReckoningSetAltitudeDr(p_data);
 110             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 111             break;
 112         }
 113         case VEHICLE_DID_DR_SPEED:
 114         {
 115 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 116             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 117                            p_data->ul_did, p_data->us_size);
 118 #endif
 119             chg_type = DeadReckoningSetSpeedDr(p_data);
 120             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 121             break;
 122         }
 123         case VEHICLE_DID_DR_HEADING:
 124         {
 125 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 126             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 127                           p_data->ul_did, p_data->us_size);
 128 #endif
 129             chg_type = DeadReckoningSetHeadingDr(p_data);
 130             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 131             break;
 132         }
 133         case VEHICLE_DID_DR_SNS_COUNTER:
 134         {
 135 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 136             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 137                           p_data->ul_did, p_data->us_size);
 138 #endif
 139             chg_type = DeadReckoningSetSnsCounterDr(p_data);
 140             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 141             break;
 142         }
 143         case VEHICLE_DID_DR_GYRO_OFFSET:
 144         {
 145 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 146             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 147                           p_data->ul_did, p_data->us_size);
 148 #endif
 149             chg_type = DeadReckoningSetGyroOffsetDr(p_data);
 150             /* Distribution processing not performed in this DID                                */
 151             /* Delivery processing is executed when VEHICLE_DID_DR_GYRO_SCALE_FACTOR_LEVEL is updated.        */
 152             /* The GyroParameter order is defined by DeadReckoning_RcvMsg().              */
 153             if (chg_type == DEADRECKONING_NEQ) {
 154                 gyro_parameter_chg_type = DEADRECKONING_NEQ;
 155             }
 156             break;
 157         }
 158         case VEHICLE_DID_DR_GYRO_SCALE_FACTOR:
 159         {
 160 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 161             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 162                           p_data->ul_did, p_data->us_size);
 163 #endif
 164             chg_type = DeadReckoningSetGyroScaleFactorDr(p_data);
 165             /* Distribution processing not performed in this DID                                */
 166             /* Delivery processing is executed when VEHICLE_DID_DR_GYRO_SCALE_FACTOR_LEVEL is updated.        */
 167             /* The GyroParameter order is defined by DeadReckoning_RcvMsg().              */
 168             if (chg_type == DEADRECKONING_NEQ) {
 169                 gyro_parameter_chg_type = DEADRECKONING_NEQ;
 170             }
 171             break;
 172         }
 173         case VEHICLE_DID_DR_GYRO_SCALE_FACTOR_LEVEL:
 174         {
 175 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 176             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 177                           p_data->ul_did, p_data->us_size);
 178 #endif
 179             chg_type = DeadReckoningSetGyroScaleFactorLevelDr(p_data);
 180             if (gyro_parameter_chg_type == DEADRECKONING_NEQ) {
 181                 chg_type        = DEADRECKONING_NEQ;
 182                 gyro_parameter_chg_type  = DEADRECKONING_EQ;
 183             }
 184             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 185             break;
 186         }
 187         case VEHICLE_DID_DR_SPEED_PULSE_SCALE_FACTOR:
 188         {
 189 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 190             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 191                           p_data->ul_did, p_data->us_size);
 192 #endif
 193             chg_type = DeadReckoningSetSpeedPulseScaleFactorDr(p_data);
 194             /* Distribution processing not performed in this DID                                */
 195             /* Delivery processing is executed when VEHICLE_DID_DR_SPEED_PULSE_SCALE_FACTOR_LEVEL is updated.    */
 196             /* The SpeedPulseParameter order is defined by DeadReckoning_RcvMsg().          */
 197             if (chg_type == DEADRECKONING_NEQ) {
 198                 speedpulse_parameter_chg_type = DEADRECKONING_NEQ;
 199             }
 200             break;
 201         }
 202         case VEHICLE_DID_DR_SPEED_PULSE_SCALE_FACTOR_LEVEL:
 203         {
 204 #if (DR_DEBUG == 1)  /* PastModel002_DR_VEHICLE  */
 205             FRAMEWORKUNIFIEDLOG(ZONE_INFO, __FUNCTION__, "DeadReckoningSetDataMaster DID=[0x%08X] DSIZE=[%d] \r\n",
 206                           p_data->ul_did, p_data->us_size);
 207 #endif
 208             chg_type = DeadReckoningSetSpeedPulseScaleFactorLevelDr(p_data);
 209             if (speedpulse_parameter_chg_type == DEADRECKONING_NEQ) {
 210                 chg_type            = DEADRECKONING_NEQ;
 211                 speedpulse_parameter_chg_type  = DEADRECKONING_EQ;
 212             }
 213             (*p_datamaster_set_n)(p_data->ul_did, chg_type, DEADRECKONING_GETMETHOD_DR);
 214             break;
 215         }
 216         default:
 217             break;
 218     }
 219 }
 220
 221 /*******************************************************************************
 222 * MODULE   : DeadReckoningGetDataMaster
 223 * ABSTRACT  : Estimated Navigational Data Master Get Processing
 224 * FUNCTION  : Provide an estimated navigation data master
 225 * ARGUMENT  : ul_did    : Data ID corresponding to the vehicle information
 226 *       : *p_data    : Pointer to the data master provider
 227 * NOTE    :
 228 * RETURN   : void
 229 ******************************************************************************/
 230 void DeadReckoningGetDataMaster(DID ul_did, DEADRECKONING_DATA_MASTER *p_data) {
 231     AGL_ASSERT_NOT_TESTED();  // LCOV_EXCL_LINE 200: test assert
 232     /*------------------------------------------------------*/
 233     /*  Call the data Get processing associated with the DID        */
 234     /*------------------------------------------------------*/
 235     switch (ul_did) {
 236         /*------------------------------------------------------*/
 237         /*  Vehicle sensor data group                                   */
 238         /*------------------------------------------------------*/
 239         case VEHICLE_DID_DR_LONGITUDE:
 240         {
 241             DeadReckoningGetLongitudeDr(p_data);
 242             break;
 243         }
 244         case VEHICLE_DID_DR_LATITUDE:
 245         {
 246             DeadReckoningGetLatitudeDr(p_data);
 247             break;
 248         }
 249         case VEHICLE_DID_DR_ALTITUDE:
 250         {
 251             DeadReckoningGetAltitudeDr(p_data);
 252             break;
 253         }
 254         case VEHICLE_DID_DR_SPEED:
 255         {
 256             DeadReckoningGetSpeedDr(p_data);
 257             break;
 258         }
 259         case VEHICLE_DID_DR_HEADING:
 260         {
 261             DeadReckoningGetHeadingDr(p_data);
 262             break;
 263         }
 264         case VEHICLE_DID_DR_SNS_COUNTER:
 265         {
 266             DeadReckoningGetSnsCounterDr(p_data);
 267             break;
 268         }
 269         case VEHICLE_DID_DR_GYRO_OFFSET:
 270         {
 271             DeadReckoningGetGyroOffsetDr(p_data);
 272             break;
 273         }
 274         case VEHICLE_DID_DR_GYRO_SCALE_FACTOR:
 275         {
 276             DeadReckoningGetGyroScaleFactorDr(p_data);
 277             break;
 278         }
 279         case VEHICLE_DID_DR_GYRO_SCALE_FACTOR_LEVEL:
 280         {
 281             DeadReckoningGetGyroScaleFactorLevelDr(p_data);
 282             break;
 283         }
 284         case VEHICLE_DID_DR_SPEED_PULSE_SCALE_FACTOR:
 285         {
 286             DeadReckoningGetSpeedPulseScaleFactorDr(p_data);
 287             break;
 288         }
 289         case VEHICLE_DID_DR_SPEED_PULSE_SCALE_FACTOR_LEVEL:
 290         {
 291             DeadReckoningGetSpeedPulseScaleFactorLevelDr(p_data);
 292             break;
 293         }
 294         default:
 295             break;
 296     }
 297 }
 298 // LCOV_EXCL_STOP