video_in_hal/vehicle_hal/src/vehicle_hal.cpp

   1 /*
   2  * @copyright Copyright (c) 2017-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 #include <string.h>
  18 #include <stdlib.h>
  19 #include <stdio.h>
  20 #include <unistd.h>
  21 #include <agl_thread.h>
  22 #include <native_service/frameworkunified_framework_if.h>
  23 #include <native_service/frameworkunified_dispatcher.h>
  24 #include <native_service/frameworkunified_multithreading.h>
  25 #include "vehicle_hal_frameworkunifiedlog.h"
  26
  27 extern "C" {
  28 //  #include "carsignal_mng_api.h"
  29 }
  30
  31 #include "vehicle_hal.h"
  32
  33 /**
  34 * \~english environment variable define
  35 */
  36 #define VEHICLEHAL_LINESENSKIND_ADIM_DATA                 "VEHICLEHAL_LINESENSKIND_ADIM"
  37 /**
  38 * \~english environment variable define
  39 */
  40 #define VEHICLEHAL_LINESENSKIND_MIC_DATA                  "VEHICLEHAL_LINESENSKIND_MIC"
  41 /**
  42 * \~english environment variable define
  43 */
  44 #define VEHICLEHAL_LINESENSKIND_VB_DATA                   "VEHICLEHAL_LINESENSKIND_VB"
  45 /**
  46 * \~english environment variable define
  47 */
  48 #define VEHICLEHAL_LINESENSKIND_RHEOSTAT_DATA             "VEHICLEHAL_LINESENSKIND_RHEOSTAT"
  49
  50 #define VEHICLEHAL_LOWVB_VALUE  75
  51
  52 // send message flag
  53 static HANDLE g_sendmsg_handle = NULL;
  54
  55 // work thread exit flag
  56 static BOOL g_loopendflag = FALSE;
  57
  58 // polling thread exit flag
  59 static BOOL g_polloopendflag = FALSE;
  60
  61 // isOpen flag
  62 static BOOL g_isopenflag = FALSE;
  63
  64 // thread id
  65 static pthread_t g_threadid = 0;
  66
  67 // hard object struct
  68 //static VclCtlApiObj g_sclientobj;
  69
  70 // app name
  71 static CHAR g_appname[MAX_NAME_SIZE_APP];
  72
  73 // line sense list
  74 const UINT8 kLineSensList[VEHICLEHAL_LINESENSKIND_NUM] = {
  75 //  CARSIGNAL_IG,
  76 //  CARSIGNAL_PKB,
  77 //  CARSIGNAL_REV,
  78 //  CARSIGNAL_ILL,
  79 };
  80
  81 static void *VehicleHalProcess(void *arg);
  82
  83 HANDLE g_vehiclehalext_thread;
  84 HANDLE g_vehiclehalpol_thread;
  85 EFrameworkunifiedStatus VehicleHalProcessExtStart(HANDLE happ);
  86 EFrameworkunifiedStatus VehicleHalProcessExtStop(HANDLE happ);
  87 EFrameworkunifiedStatus VehicleHalProcessRecvSpeed(HANDLE happ);
  88
  89 EFrameworkunifiedStatus VehicleHalProcessPolStart(HANDLE happ);
  90 EFrameworkunifiedStatus VehicleHalProcessPolStop(HANDLE happ);
  91
  92 static const FrameworkunifiedProtocolCallbackHandler kVehicleHalSpd[] = { {
  93     CID_VEHICLEHAL_SPD, VehicleHalProcessRecvSpeed } };
  94
  95 #define VEHICLEHAL_SPEED "VehicleHalSpd"
  96 #define VEHICLEHAL_EXT "VehicleHalExt"
  97 #define VEHICLEHAL_POL "VehicleHalPol"
  98
  99 VEHICLEHAL_RET_TYPE VehicleHalStart(HANDLE happ) {
 100   if (NULL == happ) {
 101     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Handle is NULL");
 102     return VEHICLEHAL_RET_ERR_PARAM;
 103   }
 104
 105   // check open flag
 106   if (TRUE == g_isopenflag) {
 107     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Already Opened");
 108     return VEHICLEHAL_RET_SUCCESS;
 109   }
 110
 111 //  memset(&g_sclientobj, 0, sizeof(g_sclientobj));
 112
 113   // enter loop
 114   g_loopendflag = FALSE;
 115
 116   g_polloopendflag = FALSE;
 117
 118   // get app name
 119   PCSTR app_name = FrameworkunifiedGetAppName(happ);
 120   if (NULL == app_name) {
 121     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Get App Name Failed");
 122     return VEHICLEHAL_RET_ERR_PARAM;
 123   }
 124
 125   memset(g_appname, 0, sizeof(g_appname));
 126   memcpy(g_appname, app_name, sizeof(g_appname));
 127   g_appname[MAX_NAME_SIZE_APP] = '\0';
 128
 129 //  // Start VLC data input control API
 130 //  UINT32 ret = VclCtlApiOpen(&g_sclientobj);
 131 //  if ( CARSIGNAL_CTL_RET_SUCCESS != ret ) {
 132 //    FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Ten Api Open Failed");
 133 //    return VEHICLEHAL_RET_ERR_HARDINIT;
 134 //  }
 135
 136   // Open message queue for sending
 137   g_sendmsg_handle = McOpenSender(g_appname);
 138   if (NULL == g_sendmsg_handle) {
 139     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Open Mc Sender Failed");
 140 //    VclCtlApiClose(&g_sclientobj);
 141     return VEHICLEHAL_RET_ERR_MCOPEN;
 142   }
 143
 144   // create thread
 145   int iret = pthread_create(&g_threadid,
 146                           NULL,
 147                           VehicleHalProcess,
 148                           NULL);
 149   if ( 0 != iret ) {
 150     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Create Thread Failed");
 151     McClose(g_sendmsg_handle);
 152 //    VclCtlApiClose(&g_sclientobj);
 153     return VEHICLEHAL_RET_ERR_THREADSTART;
 154   }
 155
 156   g_vehiclehalext_thread = FrameworkunifiedCreateChildThread(happ, VEHICLEHAL_EXT,
 157                                                 VehicleHalProcessExtStart,
 158                                                 VehicleHalProcessExtStop);
 159
 160   EFrameworkunifiedStatus estatus = FrameworkunifiedStartChildThread(happ, g_vehiclehalext_thread, 0, NULL);
 161   if (eFrameworkunifiedStatusOK != estatus) {
 162     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "FrameworkunifiedStartChildThread Failed");
 163     McClose(g_sendmsg_handle);
 164     return VEHICLEHAL_RET_ERR_THREADSTART;
 165   }
 166
 167   g_vehiclehalpol_thread = FrameworkunifiedCreateChildThread(happ, VEHICLEHAL_POL,
 168                                                 VehicleHalProcessPolStart,
 169                                                 VehicleHalProcessPolStop);
 170
 171   EFrameworkunifiedStatus estatus_pol = FrameworkunifiedStartChildThread(happ, g_vehiclehalpol_thread, 0, NULL);
 172   if (eFrameworkunifiedStatusOK != estatus_pol) {
 173     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "FrameworkunifiedStartChildThread polling Failed");
 174     McClose(g_sendmsg_handle);
 175     return VEHICLEHAL_RET_ERR_THREADSTART;
 176   }
 177
 178
 179   // set open flag true
 180   g_isopenflag = TRUE;
 181
 182   return VEHICLEHAL_RET_SUCCESS;
 183 }
 184
 185 VEHICLEHAL_RET_TYPE VehicleHalStop(HANDLE happ) {
 186   if (NULL == happ) {
 187     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Handle is NULL");
 188     return VEHICLEHAL_RET_ERR_PARAM;
 189   }
 190
 191   if (NULL != g_vehiclehalext_thread) {
 192     EFrameworkunifiedStatus estatus = FrameworkunifiedDestroyChildThread(happ, g_vehiclehalext_thread);
 193     if (eFrameworkunifiedStatusOK != estatus) {
 194       FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "FrameworkunifiedDestroyChildThread Failed");
 195       return VEHICLEHAL_RET_ERR_PARAM;
 196     }
 197     g_vehiclehalext_thread = NULL;
 198   }
 199
 200   // set open flag of polling to fasle
 201   g_polloopendflag = TRUE;
 202
 203   if (NULL != g_vehiclehalpol_thread) {
 204     EFrameworkunifiedStatus estatus = FrameworkunifiedStopChildThread(happ, g_vehiclehalpol_thread, 0, NULL);
 205     if (eFrameworkunifiedStatusOK != estatus) {
 206       FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "FrameworkunifiedDestroyChildThread polling Failed");
 207       return VEHICLEHAL_RET_ERR_PARAM;
 208     }
 209     estatus = FrameworkunifiedDestroyChildThread(happ, g_vehiclehalpol_thread);
 210     if (eFrameworkunifiedStatusOK != estatus) {
 211       FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "FrameworkunifiedDestroyChildThread polling Failed");
 212       return VEHICLEHAL_RET_ERR_PARAM;
 213     }
 214     g_vehiclehalpol_thread = NULL;
 215   }
 216
 217   // set open flag fasle
 218   g_isopenflag = FALSE;
 219
 220   // exit loop
 221   g_loopendflag = TRUE;
 222
 223   // destroy thead
 224   pthread_join(g_threadid, NULL);
 225
 226   // Close message queue
 227   McClose(g_sendmsg_handle);
 228
 229 //  // End VLC data input control API
 230 //  VclCtlApiClose(&g_sclientobj);
 231
 232   return VEHICLEHAL_RET_SUCCESS;
 233 }
 234
 235 void *VehicleHalProcess(void *arg) {
 236   // Line Sense Message
 237   VehicleHalLineSensMsg s_linesensmsg;
 238   memset( &s_linesensmsg, 0xFF, sizeof(s_linesensmsg) );
 239
 240   // main loop
 241   while (TRUE != g_loopendflag) {
 242     char* p_env_vb = NULL;
 243     p_env_vb = getenv(VEHICLEHAL_LINESENSKIND_VB_DATA);
 244     if (NULL != p_env_vb) {
 245       // low voltage
 246       if (VEHICLEHAL_LOWVB_VALUE >= atoi(p_env_vb)) {
 247         s_linesensmsg.stdata[VEHICLEHAL_LINESENSKIND_LOWVOLTAGE] = 1;
 248       } else {
 249         s_linesensmsg.stdata[VEHICLEHAL_LINESENSKIND_LOWVOLTAGE] = 0;
 250       }
 251     }
 252
 253     // loop to get line sense status
 254     for (UI_32 iloopnum = 0; iloopnum <= VEHICLEHAL_LINESENSKIND_ILL; ++iloopnum) {
 255       // line sense status
 256       UINT8 signalret = 0;
 257
 258 //      // get line sense status
 259 //      UINT32 ret = VclCtlApiRcvVclData(&g_sclientobj, kLineSensList[iloopnum], &signalret);
 260 //      if ( CARSIGNAL_CTL_RET_SUCCESS != ret ) {
 261 //        FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "Get LineSens [%d]  status faild [%d]", kLineSensList[iloopnum], ret);
 262 //        continue;
 263 //      }
 264
 265       // convert 0 to 1 or 1 to 0 for IG,REV according car signal api reference
 266       switch (kLineSensList[iloopnum]) {
 267 //        case CARSIGNAL_IG:
 268 //        case CARSIGNAL_REV:
 269 //          signalret = 1 - signalret;
 270 //          break;
 271
 272         default:
 273           break;
 274       }
 275
 276       // check status
 277       if (signalret != s_linesensmsg.stdata[iloopnum]) {
 278         s_linesensmsg.stdata[iloopnum] = signalret;
 279       }
 280     }
 281
 282     char* p_env_adim = NULL;
 283     p_env_adim = getenv(VEHICLEHAL_LINESENSKIND_ADIM_DATA);
 284     if (NULL != p_env_adim) {
 285       s_linesensmsg.stdata[VEHICLEHAL_LINESENSKIND_ADIM] = atoi(p_env_adim);
 286     }
 287
 288     char* p_env_mic = NULL;
 289     p_env_mic = getenv(VEHICLEHAL_LINESENSKIND_MIC_DATA);
 290     if (NULL != p_env_mic) {
 291       s_linesensmsg.stdata[VEHICLEHAL_LINESENSKIND_MIC] = atoi(p_env_mic);
 292     }
 293
 294     // send line sens
 295     EFrameworkunifiedStatus eretval = McSend(g_sendmsg_handle, TN_LINE_SENS_READ, CID_VEHICLEHAL_LINESENS,
 296                                  sizeof(s_linesensmsg), (PVOID)&s_linesensmsg);
 297     if (eFrameworkunifiedStatusOK != eretval) {
 298       FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "McSend LineSens failed");
 299     }
 300
 301     // wait 10ms
 302     usleep(10000);
 303   }
 304   return NULL;
 305 }
 306
 307 EFrameworkunifiedStatus VehicleHalProcessExtStart(HANDLE happ) {
 308   EFrameworkunifiedStatus estatus = FrameworkunifiedAttachCallbacksToDispatcher(happ, VEHICLEHAL_SPEED,
 309                                                       kVehicleHalSpd,
 310                                                       static_cast<UI_32>(_countof(kVehicleHalSpd)));
 311   if (eFrameworkunifiedStatusOK != estatus) {
 312     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __func__,
 313                "Error FrameworkunifiedAttachCallbacksToDispatcher Vehicle_Hal_Speed:%d", estatus);
 314     return estatus;
 315   }
 316
 317   return eFrameworkunifiedStatusOK;
 318 }
 319
 320 EFrameworkunifiedStatus VehicleHalProcessExtStop(HANDLE happ) {
 321   return eFrameworkunifiedStatusOK;
 322 }
 323
 324 EFrameworkunifiedStatus VehicleHalProcessRecvSpeed(HANDLE happ) {
 325   // Speed Pusle Message
 326   VehicleHalSpeedPulse speedpulsemsg;
 327   memset( &speedpulsemsg, 0xFF, sizeof(speedpulsemsg) );
 328
 329   // get vehicle speed
 330   UINT16 speed = 0;
 331   EFrameworkunifiedStatus estatus = eFrameworkunifiedStatusOK;
 332
 333   estatus = FrameworkunifiedGetMsgDataOfSize(happ, &speed, sizeof(speed), eSMRRelease);
 334   if (eFrameworkunifiedStatusOK != estatus) {
 335     FRAMEWORKUNIFIEDLOG(ZONE_ERR, __FUNCTION__, "failed FrameworkunifiedGetMsgDataOfSize() Status=[%d]", estatus);
 336     return estatus;
 337   }
 338
 339   // calculate speed to speed pulse
 340   float speedpulse = static_cast<float>(speed * (4.0*637.0) / (60.0 * 60.0));
 341   if ( speedpulse != speedpulsemsg.pulse ) {
 342     speedpulsemsg.pulse = speedpulse;
 343     if (0.0 != speedpulse) {
 344       speedpulsemsg.period = static_cast<float>((1 / speedpulse) * 1000);
 345     } else {
 346       speedpulsemsg.period = 0;
 347     }
 348   }
 349
 350   // send line sens
 351   EFrameworkunifiedStatus eretval = McSend(g_sendmsg_handle, TN_LINE_SENS_READ, CID_VEHICLEHAL_SPDPULSE,
 352                                sizeof(speedpulsemsg), (PVOID)&speedpulsemsg);
 353   if (eFrameworkunifiedStatusOK != eretval) {
 354     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "McSend Speed Pulse failed");
 355   }
 356
 357   eretval = McSend(g_sendmsg_handle, TN_LINE_SENS_READ, CID_VEHICLEHAL_SPD,
 358                                    sizeof(UINT16), (PVOID)&speed);
 359   if (eFrameworkunifiedStatusOK != eretval) {
 360     FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "McSend Speed failed");
 361   }
 362
 363   return eFrameworkunifiedStatusOK;
 364 }
 365
 366 EFrameworkunifiedStatus VehicleHalProcessPolStart(HANDLE happ) {
 367   VehicleHalPol s_pol;
 368   memset( &s_pol, 0xFF, sizeof(s_pol) );
 369
 370   while (!g_polloopendflag) {
 371     char* p_env_vb = NULL;
 372     char* p_env_rheostat = NULL;
 373     p_env_vb = getenv(VEHICLEHAL_LINESENSKIND_VB_DATA);
 374     p_env_rheostat = getenv(VEHICLEHAL_LINESENSKIND_RHEOSTAT_DATA);
 375     if (NULL != p_env_vb) {
 376       // covert voltage value
 377       s_pol.vb = atoi(p_env_vb);
 378     }
 379
 380     if (NULL != p_env_rheostat) {
 381       // covert RHEOSTAT value
 382       s_pol.rheostat = atoi(p_env_rheostat);
 383     }
 384     // send vb and rheostat
 385     EFrameworkunifiedStatus eretval = McSend(g_sendmsg_handle, TN_LINE_SENS_READ, CID_VEHICLEHAL_LINESENS_POL,
 386                                    sizeof(s_pol), (PVOID)&s_pol);
 387     if (eFrameworkunifiedStatusOK != eretval) {
 388       FRAMEWORKUNIFIEDLOG(ZONE_WARN, __FUNCTION__, "McSend Vb and RHEOSTAT failed");
 389     }
 390     // sleep 700ms
 391     usleep(700000);
 392   }
 393   return eFrameworkunifiedStatusOK;
 394 }
 395
 396 EFrameworkunifiedStatus VehicleHalProcessPolStop(HANDLE happ) {
 397   return eFrameworkunifiedStatusOK;
 398 }