video_in_hal/positioning_hal/src/GpsCommon/MDev_Gps_API.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 * @file MDev_Gps_API.cpp
  18 */
  19
  20 /*---------------------------------------------------------------------------*/
  21 // Include files
  22
  23 #include "MDev_Gps_API.h"
  24 #include "positioning_hal.h"
  25 #include "positioning_def.h"
  26 #include "MDev_Gps_Nmea.h"
  27
  28 /*---------------------------------------------------------------------------*/
  29 // Functions
  30
  31 /******************************************************************************
  32 @brief      SendNmeaGps<BR>
  33             NMEA transmission process
  34 @outline    Send NMEA to VehicleSensor Thread
  35 @param[in]  TG_GPS_NMEA* : pstNmeaData ... NMEA data
  36 @param[out] none
  37 @return     RET_API
  38 @retval     RET_NORMAL   : Normal completion
  39 @retval     RET_ERROR    : ABENDs
  40 *******************************************************************************/
  41 RET_API SendNmeaGps(const MDEV_GPS_NMEA* p_nmea_data) {
  42   MDEV_GPS_RAWDATA_NMEA_MSG s_send_msg;
  43   SENSOR_MSG_GPSDATA        s_msg_buf;
  44   RET_API                   ret = RET_NORMAL;
  45   PNO                       u_pno = PNO_VEHICLE_SENSOR;
  46
  47   /* Create GPS Data Notification Message */
  48   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
  49
  50   /* Message header */
  51   s_send_msg.h_dr.hdr.sndpno = PNO_NAVI_GPS_MAIN;
  52   s_send_msg.h_dr.hdr.respno = 0x0000;
  53   s_send_msg.h_dr.hdr.cid = CID_GPS_DATA;
  54   s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_RAWDATA_NMEA);
  55   s_send_msg.h_dr.hdr.rid = 0x00;
  56
  57   /* Message data */
  58   s_send_msg.st_data.e_kind = MDEV_GPS_DATA_RAWDATA_NMEA;
  59   (void)memcpy(&(s_send_msg.st_data.st_nmea_data), p_nmea_data, sizeof(s_send_msg.st_data.st_nmea_data));
  60
  61   /* Create message buffer */
  62   (void)memset(&s_msg_buf, 0, sizeof(s_msg_buf));
  63   (void)memcpy(&(s_msg_buf.st_head.hdr), &(s_send_msg.h_dr.hdr), sizeof(T_APIMSG_HEADER));
  64
  65   s_msg_buf.st_para.ul_did = POS_DID_GPS_NMEA;
  66   s_msg_buf.st_para.us_size = GPS_NMEA_SZ;
  67
  68   (void)memcpy(&(s_msg_buf.st_para.uc_data), &(s_send_msg.st_data.st_nmea_data), s_msg_buf.st_para.us_size);
  69
  70   ret = _pb_SndMsg(u_pno, sizeof(s_msg_buf), &s_msg_buf, 0);
  71
  72   if (ret != RET_NORMAL) {
  73     POSITIONING_LOG("_pb_SndMsg ERROR!! [ret=%d]\n", ret);
  74     ret = RET_ERROR;
  75   }
  76
  77   return ret;
  78 }
  79
  80 /**
  81  * @brief
  82  *   GPS processing data transmission process
  83  *
  84  * @param[in]  pstGpsTime   SENSOR_MSG_GPSTIME - GPS time information
  85  * @param[in]  p_lonlat     SENSORLOCATION_LONLATINFO - Latitude and longitude information
  86  * @param[in]  p_altitude   SENSOR_LOCATION_ALTITUDEINFO - Altitude information
  87  * @param[in]  p_heading    SENSORMOTION_HEADINGINFO_DAT - Bearing information
  88  *
  89  * @return  RET_NORMAL    Normal completion
  90  *          RET_ERROR     ABENDs
  91  */
  92 RET_API SendCustomGps(const SENSOR_MSG_GPSTIME* p_gps_time,
  93                       const SENSORLOCATION_LONLATINFO_DAT* p_lonlat,
  94                       const SENSORLOCATION_ALTITUDEINFO_DAT* p_altitude,
  95                       const SENSORMOTION_HEADINGINFO_DAT* p_heading,
  96                       const NAVIINFO_DIAG_GPS* p_diag_data) {
  97   SENSOR_MSG_GPSDATA    s_send_msg = {0};
  98   MDEV_GPS_CUSTOMDATA*  p_custom_data = NULL;
  99   RET_API               ret   = RET_NORMAL;
 100   PNO                   u_pno = PNO_VEHICLE_SENSOR;
 101
 102   /** Create GPS Data Notification Message */
 103   /* Fast _CWORD71_ processing(memset fix) */
 104   /* Initializes an area whose value is undefined in the message buffer. */
 105   (void)memset(&s_send_msg.st_head, 0x00, sizeof(s_send_msg.st_head));
 106
 107   /** Message header */
 108   s_send_msg.st_head.hdr.sndpno       = PNO_NAVI_GPS_MAIN;
 109   s_send_msg.st_head.hdr.respno       = 0x0000;
 110   s_send_msg.st_head.hdr.cid          = CID_GPS_DATA;
 111   s_send_msg.st_head.hdr.msgbodysize  = sizeof(MDEV_GPS_CUSTOMDATA);
 112   s_send_msg.st_head.hdr.rid          = 0x00;
 113
 114   /** Message data */
 115   s_send_msg.st_para.ul_did           = VEHICLE_DID_GPS_CUSTOMDATA;
 116   s_send_msg.st_para.us_size          = sizeof(MDEV_GPS_CUSTOMDATA);
 117   p_custom_data                       = reinterpret_cast<MDEV_GPS_CUSTOMDATA *>(&(s_send_msg.st_para.uc_data));
 118
 119   (void)memcpy(&(p_custom_data->st_lonlat),   p_lonlat,   sizeof(SENSORLOCATION_LONLATINFO_DAT));
 120   (void)memcpy(&(p_custom_data->st_altitude), p_altitude, sizeof(SENSORLOCATION_ALTITUDEINFO_DAT));
 121   (void)memcpy(&(p_custom_data->st_heading),  p_heading,  sizeof(SENSORMOTION_HEADINGINFO_DAT));
 122   (void)memcpy(&(p_custom_data->st_diag_gps),  p_diag_data,  sizeof(NAVIINFO_DIAG_GPS));
 123   (void)memcpy(&(p_custom_data->st_gps_time),  p_gps_time,  sizeof(SENSOR_MSG_GPSTIME));
 124   /* Messaging */
 125   ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), reinterpret_cast<void*>(&s_send_msg), 0);
 126
 127   if (ret != RET_NORMAL) {
 128     ret = RET_ERROR;
 129   }
 130
 131   return ret;
 132 }
 133
 134 /******************************************************************************
 135 @brief      SendSpeedGps<BR>
 136             Rate information transmission processing
 137 @outline    Sending speed information to vehicle sensor thread
 138 @param[in]  SENSORMOTION_SPEEDINFO_DAT* : p_seed    ... Velocity information
 139 @param[in]  u_int16                     : us_peed ... Vehicle speed(km/h)
 140 @param[out] none
 141 @return     RET_API
 142 @retval     RET_NORMAL                  : Normal completion
 143 @retval     RET_ERROR                   : ABENDs
 144 *******************************************************************************/
 145 RET_API SendSpeedGps(const SENSORMOTION_SPEEDINFO_DAT* p_seed, u_int16 us_peed) {
 146   MDEV_GPS_NAVISPEED_MSG  s_send_msg;
 147   RET_API                 ret = RET_NORMAL;
 148   PNO                     u_pno = PNO_VEHICLE_SENSOR;
 149
 150   /** Create GPS Data Notification Message */
 151   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 152
 153   /** Message header */
 154   s_send_msg.h_dr.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 155   s_send_msg.h_dr.hdr.respno      = 0x0000;
 156   s_send_msg.h_dr.hdr.cid         = CID_GPS_DATA;
 157   s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_NAVISPEED);
 158   s_send_msg.h_dr.hdr.rid         = 0x00;
 159
 160   /** Message data */
 161   s_send_msg.st_data.e_kind         = MDEV_GPS_DATA_NAVISPEED;
 162   s_send_msg.st_data.us_speed_kmph  = us_peed;
 163
 164   (void)memcpy( &s_send_msg.st_data.st_speed, p_seed, sizeof(s_send_msg.st_data.st_speed) );
 165
 166   /* Messaging */
 167   ret = _pb_SndMsg(u_pno, (sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(MDEV_GPS_NAVISPEED)), &s_send_msg, 0);
 168
 169   if (ret != RET_NORMAL) {
 170     ret = RET_ERROR;
 171   }
 172
 173   return ret;
 174 }
 175
 176 /* ++ #GPF_60_103 */
 177 /******************************************************************************
 178 @brief      SendTimeGps<BR>
 179             Time information transmission processing
 180 @outline    Send GPS time information to vehicle sensor thread
 181 @param[in]  MDEV_GPS_RTC* : p_rtc    ... GPS time information
 182 @param[out] none
 183 @return     RET_API
 184 @retval     RET_NORMAL    : Normal completion
 185 @retval     RET_ERROR     : ABENDs
 186 *******************************************************************************/
 187 RET_API SendTimeGps(const MDEV_GPS_RTC* p_rtc) {
 188   MDEV_GPS_GPSTIME_MGS  s_send_msg;
 189   RET_API               ret = RET_NORMAL;
 190   PNO                   u_pno = PNO_VEHICLE_SENSOR;
 191
 192   /** Create GPS data notification message */
 193   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 194
 195   /** Message header */
 196   s_send_msg.h_dr.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 197   s_send_msg.h_dr.hdr.respno      = 0x0000;
 198   s_send_msg.h_dr.hdr.cid         = CID_GPS_DATA;
 199   s_send_msg.h_dr.hdr.msgbodysize = sizeof(MDEV_GPS_GPSTIME);
 200   s_send_msg.h_dr.hdr.rid         = 0x00;
 201
 202   /** Message data */
 203   s_send_msg.st_data.e_kind = MDEV_GPS_DATA_GPSTIME;
 204
 205   (void)memcpy(&s_send_msg.st_data.st_rtc_data, p_rtc, sizeof(s_send_msg.st_data.st_rtc_data));
 206
 207   /* Messaging */
 208   ret = _pb_SndMsg(u_pno, (sizeof(T_APIMSG_MSGBUF_HEADER) + sizeof(MDEV_GPS_GPSTIME)), &s_send_msg, 0);
 209
 210   if (ret != RET_NORMAL) {
 211     ret = RET_ERROR;
 212   }
 213
 214   return ret;
 215 }
 216
 217 /**
 218  * @brief
 219  *   GPS clock drift transmit process
 220  *
 221  * @param[in]  drift  Clock drift
 222  *
 223  * @return  RET_NORMAL    Normal completion
 224  * @return  RET_ERROR    ABENDs
 225  */
 226 RET_API SendClockDriftGps(int32_t drift) {
 227   SENSOR_MSG_GPSDATA  s_send_msg;
 228   RET_API             ret = RET_NORMAL;
 229   PNO                 u_pno = PNO_VEHICLE_SENSOR;
 230
 231   /** Create GPS Data Notification Message */
 232   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 233
 234   /** Message header */
 235   s_send_msg.st_head.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 236   s_send_msg.st_head.hdr.respno      = 0x0000;
 237   s_send_msg.st_head.hdr.cid         = CID_GPS_DATA;
 238   s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT);
 239   s_send_msg.st_head.hdr.rid         = 0x00;
 240
 241   /** Message data */
 242   s_send_msg.st_para.ul_did   = VEHICLE_DID_GPS_CLOCK_DRIFT;
 243   s_send_msg.st_para.us_size  = sizeof(int32_t);
 244
 245   (void)memcpy(&(s_send_msg.st_para.uc_data), &drift, s_send_msg.st_para.us_size);
 246
 247   /* Messaging */
 248   ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0);
 249
 250   if (ret != RET_NORMAL) {
 251     ret = RET_ERROR;
 252   }
 253
 254   return ret;
 255 }
 256
 257 /**
 258  * @brief
 259  *   GPS clock-frequency transmit process
 260  *
 261  * @param[in]  Freq Clocking frequencies
 262  *
 263  * @return  RET_NORMAL    Normal completion
 264  * @return  RET_ERROR    ABENDs
 265  */
 266 RET_API SendClockFrequencyGps(uint32_t freq) {
 267   SENSOR_MSG_GPSDATA  s_send_msg;
 268   RET_API             ret = RET_NORMAL;
 269   PNO                 u_pno = PNO_VEHICLE_SENSOR;
 270
 271   /** Create GPS Data Notification Message */
 272   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 273
 274   /** Message header */
 275   s_send_msg.st_head.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 276   s_send_msg.st_head.hdr.respno      = 0x0000;
 277   s_send_msg.st_head.hdr.cid         = CID_GPS_DATA;
 278   s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT);
 279   s_send_msg.st_head.hdr.rid         = 0x00;
 280
 281   /** Message data */
 282   s_send_msg.st_para.ul_did   = VEHICLE_DID_GPS_CLOCK_FREQ;
 283   s_send_msg.st_para.us_size  = sizeof(uint32_t);
 284
 285   (void)memcpy(&(s_send_msg.st_para.uc_data), &freq, s_send_msg.st_para.us_size);
 286
 287   /* Messaging */
 288   ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0);
 289
 290   if (ret != RET_NORMAL) {
 291     ret = RET_ERROR;
 292   }
 293
 294   return(ret);
 295 }
 296
 297 /**
 298  * @brief
 299  *   GPS rollover standard week number transmission processing
 300  *
 301  * @param[in] *p_week_rollover GPS rollover base week number
 302  *
 303  * @return  RET_NORMAL    Normal completion
 304  * @return  RET_ERROR    ABENDs
 305  */
 306 RET_API DevGpsSndWknRollover(const SensorWknRollOverHal* p_week_rollover) {
 307   SENSOR_MSG_GPSDATA  s_send_msg;
 308   RET_API             ret = RET_NORMAL;
 309   PNO                 u_pno = PNO_VEHICLE_SENSOR;
 310
 311   /** Create GPS Data Notification Message */
 312   (void)memset( &s_send_msg, 0x00, sizeof(s_send_msg) );
 313
 314   /** Message header */
 315   s_send_msg.st_head.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 316   s_send_msg.st_head.hdr.respno      = 0x0000;
 317   s_send_msg.st_head.hdr.cid         = CID_GPS_DATA;
 318   s_send_msg.st_head.hdr.msgbodysize = sizeof(SensorWknRollOverHal);
 319   s_send_msg.st_head.hdr.rid         = 0x00;
 320
 321   /** Message data */
 322   s_send_msg.st_para.ul_did   = VEHICLE_DID_GPS_WKNROLLOVER;
 323   s_send_msg.st_para.us_size  = sizeof(SensorWknRollOverHal);
 324
 325   (void)memcpy(&(s_send_msg.st_para.uc_data), p_week_rollover, s_send_msg.st_para.us_size);
 326
 327   /* Messaging */
 328   ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0);
 329
 330   if (ret != RET_NORMAL) {
 331     ret = RET_ERROR;
 332   }
 333
 334   return ret;
 335 }
 336
 337 /******************************************************************************
 338 @brief      DevGpsRstAnsSend<BR>
 339             Reset response issuance processing
 340 @outline    Issue a reset response
 341 @param[in]  PNO        : u_pno    ... Notify-To Process No.
 342 @param[in]  RID        : uc_rid    ... Response resource ID
 343 @param[in]  u_int32    : ul_rst_sts ... Response result
 344 @param[out] none
 345 @return     int32
 346 @retval     RET_NORMAL : Normal
 347 @retval     RET_ERROR  : Abnormality
 348 *******************************************************************************/
 349 int32 DevGpsRstAnsSend(PNO u_pno, RID uc_rid, u_int32 ul_rst_sts) {
 350   TG_GPS_RET_RESET_MSG  s_send_msg;
 351   RET_API               ret = RET_NORMAL;
 352   PCSTR                 l_thread_name;
 353
 354   if (u_pno != PNO_NONE) {
 355     /** Create GPS Reset Notification Message */
 356     (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 357
 358     /** Message data */
 359     s_send_msg.data.ret_rst_status = ul_rst_sts;
 360
 361     /* Messaging */
 362     l_thread_name = _pb_CnvPno2Name(u_pno);
 363     /* External Process Transmission and Reception Messages */
 364     ret = _pb_SndMsg_Ext(l_thread_name,
 365                            CID_POSIF_REQ_GPS_RESET,
 366                            sizeof(s_send_msg.data),
 367                            reinterpret_cast<void *>(&(s_send_msg.data)),
 368                            0);
 369     if (ret != RET_NORMAL) {
 370       POSITIONING_LOG("GpsCommCtl # DevGpsRstAnsSend SndMsg Error ret[%d] pno[%03X]\n", ret, u_pno);
 371       ret = RET_ERROR;
 372     }
 373   }
 374
 375   return ret;
 376 }
 377
 378
 379 /**
 380  * @brief      Time setting response issuance processing
 381  *
 382  * @param[in]  us_pno    Notify-To Process No.
 383  * @param[in]  uc_rid    Response resource ID
 384  * @param[in]  ul_rst_sts Response result
 385  *
 386  * @return     Processing result
 387  * @retval     RET_NORMAL : Normal
 388  * @retval     RET_ERROR  : Abnormality
 389  */
 390 int32 DevGpsTimesetAnsSend(PNO us_pno, RID uc_rid, u_int32 ul_rst_sts) {
 391     TG_GPS_RET_TIMESET_MSG  st_snd_msg;
 392     RET_API         ret = RET_NORMAL;
 393
 394     /** Create GPS Reset Notification Message */
 395     memset( &st_snd_msg, 0x00, sizeof(st_snd_msg) ); /* QAC 3200 */
 396     /** Message header */
 397     st_snd_msg.header.hdr.sndpno     = PNO_NAVI_GPS_MAIN;
 398     st_snd_msg.header.hdr.respno     = 0x0000;
 399     st_snd_msg.header.hdr.cid        = CID_GPS_RETTIMESETTING;
 400     st_snd_msg.header.hdr.msgbodysize = sizeof(st_snd_msg.status);
 401     st_snd_msg.header.hdr.rid        = uc_rid;
 402     /** Message data */
 403     st_snd_msg.status                = ul_rst_sts;
 404
 405     /* Messaging */
 406     if (us_pno != PNO_NONE) {
 407         ret = _pb_SndMsg(us_pno, sizeof(st_snd_msg), &st_snd_msg, 0);
 408
 409         if (ret != RET_NORMAL) {
 410             POSITIONING_LOG("DevGpsTimesetAnsSend SndMsg Error ret[%d] pno[%03X] \r\n", ret, us_pno);
 411
 412             ret = RET_ERROR;
 413         }
 414     }
 415
 416     return(ret);
 417 }
 418
 419
 420 /**
 421  * @brief
 422  *   GPS clock drift transmit process
 423  *
 424  * @param[in]  drift  Clock drift
 425  *
 426  * @return  RET_NORMAL    Normal completion
 427  * @return  RET_ERROR    ABENDs
 428  *
 429  */
 430 RET_API DevSendGpsConnectError(BOOL is_err) {
 431   SENSOR_MSG_GPSDATA  s_send_msg;
 432   RET_API             ret = RET_NORMAL;
 433   PNO                 u_pno = PNO_VEHICLE_SENSOR;
 434
 435   /** Create GPS Data Notification Message */
 436   (void)memset(&s_send_msg, 0x00, sizeof(s_send_msg));
 437
 438   /** Message header */
 439   s_send_msg.st_head.hdr.sndpno      = PNO_NAVI_GPS_MAIN;
 440   s_send_msg.st_head.hdr.respno      = 0x0000;
 441   s_send_msg.st_head.hdr.cid         = CID_GPS_DATA;
 442   s_send_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT);
 443   s_send_msg.st_head.hdr.rid         = 0x00;
 444
 445   /** Message data */
 446   s_send_msg.st_para.ul_did   = POSHAL_DID_GPS_CONNECT_ERROR;
 447   s_send_msg.st_para.us_size  = sizeof(uint32_t);
 448
 449   (void)memcpy(&(s_send_msg.st_para.uc_data), &is_err, s_send_msg.st_para.us_size);
 450
 451   /* Messaging */
 452   ret = _pb_SndMsg(u_pno, sizeof(s_send_msg), &s_send_msg, 0);
 453
 454   if (ret != RET_NORMAL) {
 455     ret = RET_ERROR;
 456   }
 457
 458   return ret;
 459 }
 460
 461
 462 RET_API SndGpsTimeRaw(const SENSOR_GPSTIME_RAW* ps_gpstime_raw) {
 463     SENSOR_MSG_GPSDATA  st_snd_msg;
 464     RET_API       ret;
 465     PNO         _us_pno = PNO_VEHICLE_SENSOR;
 466
 467     /** Create GPS Data Notification Message */
 468     (void)memset( &st_snd_msg, 0x00, sizeof(st_snd_msg) ); /* QAC 3200 */
 469     /** Message header */
 470     st_snd_msg.st_head.hdr.sndpno  = PNO_NAVI_GPS_MAIN;
 471     st_snd_msg.st_head.hdr.respno  = 0x0000;
 472     st_snd_msg.st_head.hdr.cid   = CID_GPS_DATA;
 473     st_snd_msg.st_head.hdr.msgbodysize = sizeof(SENSOR_MSG_GPSDATA_DAT);
 474     st_snd_msg.st_head.hdr.rid   = 0x00;
 475     /** Message data */
 476     st_snd_msg.st_para.ul_did   = VEHICLE_DID_GPS_TIME_RAW;
 477     st_snd_msg.st_para.us_size    = sizeof(SENSOR_GPSTIME_RAW);
 478     (void)memcpy(&(st_snd_msg.st_para.uc_data), ps_gpstime_raw, st_snd_msg.st_para.us_size); /* QAC 3200 */
 479
 480     /* Messaging */
 481     ret = _pb_SndMsg( _us_pno, sizeof(st_snd_msg), &st_snd_msg, 0 );
 482     if (ret != RET_NORMAL) {
 483         POSITIONING_LOG("_pb_SndMsg ERROR!! [ret=%d]\n", ret);
 484         ret = RET_ERROR;
 485     }
 486
 487     return(ret);
 488 }
 489
 490 /*---------------------------------------------------------------------------*/
 491 /*EOF*/