service/soft-sensor-manager/SoftSensorPlugin/IndoorTrajectorySensor/src/tizen_proximity.cpp

   1 /******************************************************************
   2 *
   3 * Copyright 2014 Samsung Electronics All Rights Reserved.
   4 *
   5 *
   6 *
   7 * Licensed under the Apache License, Version 2.0 (the "License");
   8 * you may not use this file except in compliance with the License.
   9 * You may obtain a copy of the License at
  10 *
  11 *      http://www.apache.org/licenses/LICENSE-2.0
  12 *
  13 * Unless required by applicable law or agreed to in writing, software
  14 * distributed under the License is distributed on an "AS IS" BASIS,
  15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  16 * See the License for the specific language governing permissions and
  17 * limitations under the License.
  18 *
  19 ******************************************************************/
  20
  21 #include <stdio.h>
  22 #include <iostream>
  23 #include "tizen_log.h"
  24 #include <pthread.h>
  25 #include <unistd.h>
  26 #include "tizen_proximity.h"
  27 #include "tizen_CbleDevice.h"
  28 #include "lib_proximity.h"
  29
  30 #include "ITS.h"
  31
  32 CProximity *prox = NULL;
  33 ITSName::Things *output = NULL;
  34 int CProximity::get_rssi_cnt = 0;
  35 int CProximity::num_set_rssi = 0;
  36 int CProximity::loop_flag = JOB_GET_RSSI;
  37 CProximity::rssiMAP CProximity::map_rssi;
  38
  39 // Proximity variables
  40 int **rssi = 0;
  41 int *rssicnt = 0;
  42 int *startindex = 0;
  43 int *flag = 0;
  44
  45
  46 void proximity_init( void *(*func)(void *) )
  47 {
  48 #if __INTERNAL_DEBUG_
  49     DBG("Enter.");
  50 #endif
  51     prox = new CProximity( func );
  52     output = NULL;
  53 }
  54
  55 void proximity_loop( void *param )
  56 {
  57 //  DBG("called. main loop function.");
  58     CbleDevice *pdevice = (CbleDevice *)param;
  59
  60     if ( prox == NULL || pdevice == NULL )
  61     {
  62         DBG("Error : proximity_loop is occurred error.");
  63         return ;
  64     }
  65
  66     // Get BLE RSSI and Calculate Proximity value. ( choi job )
  67     if ( prox->loop_flag == JOB_GET_RSSI )
  68     {
  69         prox->GetRSSI( pdevice );
  70         prox->loop_flag = JOB_BUSY;
  71     }
  72
  73     if ( prox->loop_flag == JOB_GET_PROX )
  74     {
  75         prox->GetProximity( pdevice );
  76         prox->loop_flag = JOB_PUT_OUTPUT;
  77     }
  78
  79     if ( prox->loop_flag == JOB_PUT_OUTPUT )
  80     {
  81         // Result Setting and transmit data from proximity to ITS
  82         prox->set_output( pdevice );
  83         pthread_create(&prox->Thandle, NULL, prox->threadFunc, output);
  84         prox->loop_flag = JOB_GET_RSSI;
  85     }
  86
  87     usleep(100);
  88 //  DBG("Exit.");
  89 }
  90
  91
  92
  93
  94 /****************************************
  95  *
  96  * CProximity class definition.
  97  *
  98  */
  99
 100 CProximity::CProximity( workfunc func )
 101 {
 102 #if __INTERNAL_DEBUG_
 103     DBG("Enter.");
 104 #endif
 105     if ( set_threadFunc(func) == false)
 106         DBG("Error: set_threadFunc is failed.");
 107
 108     loop_flag = JOB_GET_RSSI;
 109     get_rssi_cnt = 0;
 110     num_set_rssi = 0;
 111
 112     reference_ea = 0;
 113     m_distance = 0;
 114     m_proximity = 0;
 115     map_rssi.clear();
 116 }
 117
 118 CProximity::~CProximity( void )
 119 {
 120 #if __INTERNAL_DEBUG_
 121     DBG("Enter.");
 122 #endif
 123     if ( rssi )
 124     {
 125         delete [] rssi;
 126         rssi = NULL;
 127     }
 128
 129     if ( rssicnt )
 130     {
 131         delete rssicnt;
 132         rssicnt = NULL;
 133     }
 134
 135     if ( startindex )
 136     {
 137         delete startindex;
 138         startindex = NULL;
 139     }
 140
 141     if ( flag )
 142     {
 143         delete flag;
 144         flag = NULL;
 145     }
 146
 147     if ( m_distance )
 148     {
 149         delete m_distance;
 150         m_distance = NULL;
 151     }
 152
 153     if ( m_proximity )
 154     {
 155         delete m_proximity;
 156         m_proximity = NULL;
 157     }
 158
 159 }
 160
 161 bool CProximity::find_referenceThing( bt_adapter_device_discovery_info_s *discovery_info ,
 162                                       CbleDevice *pdevice )
 163 {
 164 #if __INTERNAL_DEBUG_
 165     DBG("Enter");
 166 #endif
 167     if ( pdevice->find(discovery_info->remote_address) == false  )
 168     {
 169         return false;
 170     }
 171 #if __INTERNAL_DEBUG_
 172     DBG("Find %s device.", discovery_info->remote_address);
 173 #endif
 174     if ( pdevice->setRSSI( discovery_info->remote_address,
 175                            discovery_info->rssi ) == discovery_info->rssi )
 176     {
 177         map_rssi[ discovery_info->remote_address ].init( RSSI_EA );
 178         map_rssi[ discovery_info->remote_address ].rssi[get_rssi_cnt - 1] = discovery_info->rssi - 65535;
 179 #if __INTERNAL_DEBUG_
 180         DBG("Get RSSI. %d", map_rssi[ discovery_info->remote_address ].rssi[get_rssi_cnt - 1] );
 181 #endif
 182         num_set_rssi++;
 183     }
 184
 185     return true;
 186 }
 187
 188 // Called by bt_adapter_start_device_discovery / bt_adapter_stop_device_discovery function.
 189 void CProximity::CB_discover_state_change(int result,
 190         bt_adapter_device_discovery_state_e discovery_state, \
 191         bt_adapter_device_discovery_info_s *discovery_info, void *user_data)
 192 {
 193     DBG("callback called.");
 194 #if __INTERNAL_DEBUG_
 195     DBG("result = %d", result);
 196 #endif
 197
 198     CbleDevice *pdevice = (CbleDevice *)user_data;
 199
 200     if ( discovery_state == BT_ADAPTER_DEVICE_DISCOVERY_FOUND )
 201     {
 202         if ( discovery_info == NULL)
 203             goto BT_ERROR;
 204 #if __INTERNAL_DEBUG_
 205         printf("=========== Get RSSI =============\n");
 206         printf("= address  \t : %s \n", discovery_info->remote_address);
 207         printf("= name     \t : %s \n", discovery_info->remote_name);
 208         printf("= rssi     \t : %d \n", (discovery_info->rssi - 65535) );
 209         printf("===========================================\n");
 210 #endif
 211         find_referenceThing( discovery_info , pdevice );
 212
 213         if ( num_set_rssi == pdevice->size()  )
 214         {
 215 #if __INTERNAL_DEBUG_
 216             DBG("Stop searching device.");
 217 #endif
 218             BT_ERROR_CHECK( bt_adapter_stop_device_discovery() );
 219         }
 220     }
 221     else if ( discovery_state == BT_ADAPTER_DEVICE_DISCOVERY_FINISHED )
 222     {
 223         DBG("state == BT_ADAPTER_DEVICE_DISCOVERY_FINISHED");
 224
 225         num_set_rssi = 0;
 226         if ( get_rssi_cnt == RSSI_EA )
 227         {
 228             get_rssi_cnt = 0;
 229             loop_flag = JOB_GET_PROX;
 230         }
 231         else
 232             loop_flag = JOB_GET_RSSI;
 233     }
 234
 235     return ;
 236
 237 BT_ERROR:
 238     return ;
 239 }
 240
 241
 242 bool CProximity::GetRSSI(CbleDevice *pdevice)
 243 {
 244 #if __INTERNAL_DEBUG_
 245     DBG("Enter");
 246 #endif
 247     if ( num_set_rssi == 0 && get_rssi_cnt == 0)
 248         map_rssi.clear();
 249
 250     num_set_rssi = 0;
 251     get_rssi_cnt++;
 252
 253     for ( int i = 0; i < pdevice->size(); i++ )
 254         pdevice->setRSSI( (pdevice->getNext(i))->remote_address, 0);
 255
 256     BT_ERROR_CHECK( bt_adapter_set_device_discovery_state_changed_cb(
 257                         CProximity::CB_discover_state_change, pdevice) );
 258
 259     BT_ERROR_CHECK( bt_adapter_start_device_discovery() );
 260
 261     return true;
 262
 263 BT_ERROR:
 264     return false;
 265 }
 266
 267 bool is_first = true;
 268 void CProximity::GetProximity(CbleDevice *pdevice)
 269 {
 270 #if __INTERNAL_DEBUG_
 271     DBG("Enter.");
 272 #endif
 273 //#if __INTERNAL_DEBUG_
 274     DBG("thing ea = %d", map_rssi.size() );
 275     DBG("RSSI ea = %d", RSSI_EA );
 276
 277     for (int num = 0; num < map_rssi.size(); num++ )
 278     {
 279         std::string addr = pdevice->getNext(num)->remote_address;
 280         DBG("Addr -> %s", addr.c_str() );
 281         for (int i = 0; i < RSSI_EA ; i++)
 282             DBG(" %d th. rssi = %d", i, map_rssi[addr].rssi[i] );
 283     }
 284 //#endif
 285
 286     reference_ea = map_rssi.size();
 287     float *avg = new float[reference_ea];
 288
 289     if ( is_first )
 290         prox_first_setting( reference_ea );
 291
 292     for (int i = 0; i < reference_ea; i++)
 293     {
 294         std::string addr = pdevice->getNext(i)->remote_address;
 295
 296         if ( rssicnt[i] > arraysize - 1)
 297             rssicnt[i] = rssicnt[i] % arraysize;
 298
 299         for (int j = 0; j < RSSI_EA; j++)
 300         {
 301             rssi[i][rssicnt[i]] = map_rssi[addr].rssi[j];
 302             rssicnt[i]++;
 303         }
 304
 305         avg[i] = CalculateExponentialAverage(RSSI_EA, rssi[i], startindex[i], flag[i]);
 306         m_distance[i] = calculateDistance(avg[i], -58);
 307
 308         if ( m_distance[i] <= 1.0 )
 309         {
 310             m_proximity[i] = 1;
 311         }
 312         else if (m_distance[i] <= 2)
 313         {
 314             m_proximity[i] = 2;
 315         }
 316         else
 317         {
 318             m_proximity[i] = 3;
 319         }
 320
 321         DBG("distance = %f", m_distance[i]);
 322         DBG("proximity = %d", m_proximity[i]);
 323         //Serial.println(calculateDistance(avg[i], -58));
 324         startindex[i] += RSSI_EA;
 325
 326         if (startindex[i] >= arraysize)
 327         {
 328             startindex[i] = 0;
 329         }
 330
 331         if (flag[i] < (arraysize / RSSI_EA))
 332         {
 333             flag[i]++;
 334         }
 335     } // for
 336
 337     delete avg;
 338 }
 339
 340 bool CProximity::set_threadFunc(workfunc func)
 341 {
 342 #if __INTERNAL_DEBUG_
 343     DBG("Enter.");
 344 #endif
 345     if ( func == NULL )
 346     {
 347         DBG("Error: func is null.");
 348         return false;
 349     }
 350
 351     threadFunc = func;
 352     return true;
 353 }
 354
 355 CProximity::workfunc CProximity::get_threadFunc(void)
 356 {
 357     return threadFunc;
 358 }
 359
 360 void CProximity::set_output( CbleDevice *pdevice )
 361 {
 362 #if __INTERNAL_DEBUG_
 363     DBG("Enter.");
 364 #endif
 365
 366     if (output)
 367     {
 368         delete output;
 369         output = NULL;
 370     }
 371
 372     if ( output == NULL )
 373     {
 374         output = new ITSName::Things(reference_ea);
 375     }
 376
 377     if ( output == NULL )
 378     {
 379         DBG("Error : output is NULL.");
 380         return ;
 381     }
 382
 383     // Set Output value.
 384     for (int i = 0; i < reference_ea; i++)
 385     {
 386         output->things[i].ID = pdevice->getNext(i)->remote_address;
 387         output->things[i].SD = 0.0;
 388         output->things[i].distance = m_distance[i];
 389         output->things[i].proximity = m_proximity[i];
 390     }
 391 }
 392
 393
 394
 395
 396 void CProximity::prox_first_setting( int reference_ea )
 397 {
 398     if ( rssi == NULL )
 399     {
 400         rssi = new int *[reference_ea];
 401
 402         for (int i = 0; i < reference_ea; i++)
 403         {
 404             rssi[i] = new int[arraysize];
 405             for (int j = 0; j < arraysize; j++)
 406                 rssi[i][j] = 0;
 407         }
 408     }
 409
 410     if ( rssicnt == NULL )
 411     {
 412         rssicnt = new int[reference_ea];
 413         for (int j = 0; j < reference_ea; j++)
 414             rssicnt[j] = 0;
 415     }
 416
 417     if ( startindex == NULL )
 418     {
 419         startindex = new int[reference_ea];
 420         for (int j = 0; j < reference_ea; j++)
 421             startindex[j] = 0;
 422     }
 423
 424     if ( flag == NULL)
 425     {
 426         flag = new int[reference_ea];
 427         for (int j = 0; j < reference_ea; j++)
 428             flag[j] = 0;
 429     }
 430
 431     if ( m_distance == NULL )
 432     {
 433         m_distance = new float[reference_ea];
 434         for (int j = 0; j < reference_ea; j++)
 435             m_distance[j] = 0;
 436     }
 437
 438     if ( m_proximity == NULL )
 439     {
 440         m_proximity = new int[reference_ea];
 441         for (int j = 0; j < reference_ea; j++)
 442             m_proximity[j] = 0;
 443     }
 444
 445     is_first = false;
 446 }