#include <stdio.h>
|
#include <stdlib.h>
|
#include <unistd.h>
|
#include <semaphore.h>
|
#include <string.h>
|
#include <pthread.h>
|
#include <signal.h>
|
#include <errno.h>
|
#include <time.h>
|
#include <assert.h>
|
#include <math.h>
|
|
#include "probe/probe.h"
|
#include "ptz/ptz.h"
|
#include "proto_comm.h"
|
#include "proto_dbg.h"
|
|
static PtzDevs *gPtzDevs = NULL;
|
|
/*get the image wideth*/
|
static int img_w_get(PtzDevs *DevData) {
|
|
int ret;
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(DevData->profiles != NULL);
|
|
ret = DevData->profiles->venc.Width;
|
|
return ret;
|
}
|
|
/*get the image height*/
|
static int img_h_get(PtzDevs *DevData) {
|
|
int ret;
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(DevData->profiles != NULL);
|
|
ret = DevData->profiles->venc.Height;
|
|
return ret;
|
}
|
|
static void cb_probe(char *DeviceXAddr) {
|
|
int len;
|
char *pos_s, *pos_e;
|
|
if (gPtzDevs == NULL) {
|
|
gPtzDevs = malloc(sizeof(PtzDevs));
|
|
SOAP_ASSERT(gPtzDevs != NULL);
|
|
memset(gPtzDevs, 0x00, sizeof(PtzDevs));
|
|
pos_s = strstr(DeviceXAddr, ADDR_PREFIX);
|
pos_e = strstr(DeviceXAddr + strlen(ADDR_PREFIX), ADDR_LAST);
|
len = pos_e - DeviceXAddr - strlen(ADDR_PREFIX);
|
memcpy(gPtzDevs->ip, DeviceXAddr + strlen(ADDR_PREFIX), len);
|
}
|
|
proto_GetCapabilities(DeviceXAddr, &(gPtzDevs->capa), USERNAME, PASSWORD);
|
proto_GetProfiles(gPtzDevs->capa.MediaXAddr, &(gPtzDevs->profiles), USERNAME, PASSWORD);
|
|
}
|
|
/*preset tour task*/
|
static void *task_preset_touring(void *arg) {
|
int ret;
|
int i;
|
float data;
|
PosCur pos_get;
|
float pval, tval;
|
struct timespec time_exp;
|
|
time_exp.tv_sec = WT_TIME;
|
time_exp.tv_nsec = 0;
|
//int data = *(int *)arg;
|
|
pthread_detach(pthread_self());
|
if (gPtzDevs != NULL) {
|
ret = sem_init(&(gPtzDevs->sem_tour), 0, 0);
|
SOAP_ASSERT(ret == 0);
|
|
timer_init(&(gPtzDevs->timer_id), ptz_timer_handler);
|
|
gPtzDevs->dev_stat = DISABLE;
|
pthread_mutex_init(&(gPtzDevs->node_mutex), NULL);
|
pthread_cond_init(&(gPtzDevs->cond), NULL);
|
|
while(1) {
|
ret = sem_timedwait(&(gPtzDevs->sem_tour), &time_exp);
|
if (gPtzDevs->proc_stat == DISABLE) {
|
printf("now task_preset_touring is exitting!\n");
|
|
pthread_cond_destroy(&(gPtzDevs->cond));
|
pthread_mutex_destroy(&(gPtzDevs->node_mutex));
|
sem_destroy(&(gPtzDevs->sem_tour));
|
|
timer_destroy(gPtzDevs->timer_id);
|
|
free(gPtzDevs->profiles);
|
|
free(gPtzDevs);
|
|
gPtzDevs = NULL;
|
|
break;
|
|
} else if (ret == 0) {
|
|
if (gPtzDevs->dev_stat == ENABLE) {
|
|
if (gPtzDevs->tour_type == TOUR_PRIV) {
|
printf("now the priv touring is calling:\n");
|
|
if (gPtzDevs->posNode != NULL) {
|
|
while(gPtzDevs->dev_stat == ENABLE) {
|
|
for (i = 0; i < gPtzDevs->posNode_count; i++) {
|
|
proto_PTZ_GetStatus(gPtzDevs, &pos_get, USERNAME, PASSWORD);
|
|
proto_PTZGetPT(gPtzDevs, (gPtzDevs->posNode + i)->x, (gPtzDevs->posNode + i)->y, &pval, &tval, USERNAME, PASSWORD);
|
|
proto_PTZSet(gPtzDevs, pval, tval, pos_get.z, USERNAME, PASSWORD);
|
|
proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);
|
|
data = proto_PTZZoom_get(gPtzDevs, (gPtzDevs->posNode + i)->pos0_x, (gPtzDevs->posNode + i)->pos0_y, \
|
(gPtzDevs->posNode + i)->pos1_x, (gPtzDevs->posNode + i)->pos1_y);
|
|
proto_PTZSet(gPtzDevs, 0, 0, data, USERNAME, PASSWORD);
|
|
proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);
|
|
timer_start(gPtzDevs->timer_id, (gPtzDevs->posNode + i)->time_sec);
|
|
pthread_mutex_lock(&(gPtzDevs->node_mutex));
|
while((gPtzDevs->dev_stat == ENABLE) && (gPtzDevs->wt_stat == ENABLE)) {
|
pthread_cond_wait(&(gPtzDevs->cond), &(gPtzDevs->node_mutex));
|
}
|
pthread_mutex_unlock(&(gPtzDevs->node_mutex));
|
|
proto_PTZSet(gPtzDevs, 0, 0, 1, USERNAME, PASSWORD);
|
proto_PTZWaitStopped(gPtzDevs, USERNAME, PASSWORD);
|
|
gPtzDevs->wt_stat = ENABLE;
|
|
if (gPtzDevs->dev_stat == DISABLE) {
|
|
break;
|
}
|
}
|
}
|
|
timer_stop(gPtzDevs->timer_id);
|
|
}
|
|
} else {
|
|
if (gPtzDevs->postradNode != NULL) {
|
|
while(gPtzDevs->dev_stat == ENABLE) {
|
|
for (i = 0; i < gPtzDevs->postradNode_count; i++) {
|
|
proto_PTZPreset(gPtzDevs, NULL, PRESET_GOTO, &((gPtzDevs->postradNode + i)->posToken), USERNAME, PASSWORD);
|
|
timer_start(gPtzDevs->timer_id, (gPtzDevs->postradNode + i)->time_sec);
|
|
pthread_mutex_lock(&(gPtzDevs->node_mutex));
|
while((gPtzDevs->dev_stat == ENABLE) && (gPtzDevs->wt_stat == ENABLE)) {
|
pthread_cond_wait(&(gPtzDevs->cond), &(gPtzDevs->node_mutex));
|
}
|
pthread_mutex_unlock(&(gPtzDevs->node_mutex));
|
|
gPtzDevs->wt_stat = ENABLE;
|
|
if (gPtzDevs->dev_stat == DISABLE) {
|
|
printf("the comm type tour stop is triggered, now stop it!\n");
|
|
break;
|
}
|
}
|
}
|
|
timer_stop(gPtzDevs->timer_id);
|
}
|
}
|
}
|
}
|
}
|
}
|
|
return NULL;
|
}
|
|
/*the timer process heandler*/
|
void ptz_timer_handler(union sigval para) {
|
|
if (gPtzDevs != NULL) {
|
|
pthread_mutex_lock(&(gPtzDevs->node_mutex));
|
|
gPtzDevs->wt_stat = DISABLE;
|
|
pthread_cond_signal(&(gPtzDevs->cond));
|
|
pthread_mutex_unlock(&(gPtzDevs->node_mutex));
|
}
|
|
return;
|
}
|
|
/*initialize the PTZ*/
|
int proto_PTZInit(void) {
|
|
int ret;
|
int flag;
|
pthread_t tid;
|
|
ret = DiscoverDevice(cb_probe);
|
if (ret <= 0) {
|
return -1;
|
}
|
|
ret = pthread_create(&tid, NULL, task_preset_touring, &flag);
|
if (ret) {
|
return -1;
|
}
|
|
return 0;
|
}
|
|
/*release all the resources when exitted*/
|
void proto_PTZClose(void) {
|
|
if (gPtzDevs != NULL) {
|
|
if (gPtzDevs->dev_stat == ENABLE) {
|
proto_PTZPreset_tour_stop(gPtzDevs);
|
}
|
|
gPtzDevs->proc_stat = DISABLE;
|
}
|
|
}
|
|
PtzDevs *proto_PTZGethandle(void) {
|
return gPtzDevs;
|
}
|
|
/*wait for PTZ stopped*/
|
void proto_PTZWaitStopped(PtzDevs *DevData, const char *username, const char *passwd) {
|
PosCur pos_get;
|
|
proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
|
while(pos_get.stat == RUNNING) {
|
proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
|
sleep(1);
|
}
|
|
sleep(STOP_WT_TIME);
|
}
|
|
/*
|
*(x0, y0): the left-up pixel;
|
*(x1, y1): the right-down pixel;
|
*
|
*(x0, y0) and (x1, y1) must be aligned tightly with the center of the image.
|
*/
|
float proto_PTZZoom_get(PtzDevs *DevData, int x0, int y0, int x1, int y1) {
|
int w;
|
int h;
|
float data0;
|
float data1;
|
float temp;
|
float temp_w;
|
float temp_h;
|
float result;
|
|
w = img_w_get(DevData);
|
h = img_h_get(DevData);
|
|
data0 = w * h * MAX_IMG_SCALE;
|
data1 = abs(x1 - x0) * abs(y1 - y0);
|
|
temp = sqrtf(data0 / data1);
|
temp_w = (w / 2) / (abs(x1 - w / 2));
|
temp_h = (h / 2) / (abs(y1 - h / 2));
|
|
result = min(temp_w, temp_h);
|
result = min(result, temp);
|
|
if (result <= 1) {
|
result = 1;
|
}
|
|
return result;
|
}
|
|
int proto_PTZ_GetStatus(PtzDevs *DevData, PosCur *pos_get, const char *username, const char *passwd) {
|
int result = 0;
|
struct soap *soap = NULL;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
struct _tptz__GetStatus *getStatus = NULL;
|
struct _tptz__GetStatusResponse *getStatusResponse = NULL;
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
getStatus = soap_new__tptz__GetStatus(soap, 1);
|
getStatusResponse = soap_new__tptz__GetStatusResponse(soap, 1);
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
|
getStatus->ProfileToken = DevData->profiles->token;
|
result = soap_call___tptz__GetStatus(soap, PTZXAddr, NULL, getStatus, getStatusResponse);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZ_GetStatus");
|
|
EXIT:
|
|
if ((SOAP_OK != result) || (SOAP_OK != soap->error)) {
|
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
|
return -1;
|
}
|
|
if (pos_get != NULL) {
|
|
pos_get->p = pos_calc_p(getStatusResponse->PTZStatus->Position->PanTilt->x);
|
pos_get->t = pos_calc_t(getStatusResponse->PTZStatus->Position->PanTilt->y);
|
pos_get->z = pos_calc_z(getStatusResponse->PTZStatus->Position->Zoom->x);
|
|
if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__IDLE) {
|
|
pos_get->stat = IDLE;
|
|
} else if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__MOVING) {
|
|
pos_get->stat = RUNNING;
|
|
} else if(*(getStatusResponse->PTZStatus->MoveStatus->PanTilt) == tt__MoveStatus__UNKNOWN){
|
|
pos_get->stat = UNKNOWN;
|
|
}
|
}
|
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
|
return 0;
|
}
|
|
static float pos_calc_p(float pos) {
|
float data = COEF_H_A * pos + COEF_H_B;
|
|
return data;
|
}
|
|
static float pos_calc_t(float pos) {
|
float data = COEF_V_A * pos + COEF_V_B;
|
|
return data;
|
}
|
|
static float pos_calc_z(float pos) {
|
float data = COEF_Z_A * pos + COEF_Z_B;
|
|
return data;
|
}
|
|
static float pos_res_p(float pos) {
|
int pos_temp;
|
float data;
|
|
pos_temp = (int)pos % MAX_VAL;
|
data = (float) (pos_temp - COEF_H_B) * 1.0 / COEF_H_A;
|
|
data = para_check(data);
|
return data;
|
}
|
|
static float pos_res_t(float pos) {
|
int pos_temp;
|
float data;
|
|
pos_temp = (int)pos % MAX_VAL;
|
data = (float)(pos_temp - COEF_V_B) * 1.0 / COEF_V_A;
|
|
data = para_check(data);
|
return data;
|
}
|
|
static float pos_res_z(float pos) {
|
float data = (float)(abs(pos) - COEF_Z_B) * 1.0 / COEF_Z_A;
|
|
data = para_check(data);
|
if (data <= 0) {
|
data *= -1;
|
}
|
return data;
|
}
|
|
/*set the self-defined tour parameters table*/
|
int proto_PTZPreset_tour_set(PtzDevs *DevData, PresetToure_node *PosArr, int n) {
|
|
int i;
|
|
if ((PosArr == NULL) || (n < MIN_TOUR_POS) || (DevData == NULL)) {
|
|
SOAP_DBGERR("invalid parameters!\n");
|
|
return -1;
|
}
|
|
if (DevData->dev_stat == ENABLE) {
|
SOAP_DBGERR("the tour is in progress, the paras can not been set in this stat!\n");
|
|
return -1;
|
}
|
|
pthread_mutex_lock(&(DevData->node_mutex));
|
|
if (DevData->posNode != NULL) {
|
free(DevData->posNode);
|
}
|
|
DevData->posNode = (PresetToure_node *)malloc(sizeof(PresetToure_node) * n);
|
if (DevData->posNode == NULL) {
|
SOAP_DBGERR("in %s: out of memory!\n", __FUNCTION__);
|
|
exit(1);
|
}
|
DevData->posNode_count = n;
|
|
memcpy(DevData->posNode, PosArr, sizeof(PresetToure_node) * n);
|
|
for (i = 0; i < n; i++) {
|
if ((DevData->posNode + i)->time_sec < MIN_TOUR_TIME) {
|
|
(DevData->posNode + i)->time_sec = MIN_TOUR_TIME;
|
|
}
|
}
|
|
pthread_mutex_unlock(&(DevData->node_mutex));
|
|
return 0;
|
}
|
|
/*start the tour*/
|
int proto_PTZPreset_tour_start(PtzDevs *DevData, enum tourType type) {
|
|
if (DevData->dev_stat == ENABLE) {
|
SOAP_DBGERR("the tour is in progress now, it can not been re-started yet!\n");
|
|
return -1;
|
}
|
|
if (type == TOUR_PRIV) {
|
if ((DevData == NULL) || (DevData->posNode == NULL)) {
|
SOAP_DBGERR("invalid parameters! configure the paras firstly please!\n");
|
|
return -1;
|
}
|
} else {
|
if ((DevData == NULL) || (DevData->postradNode == NULL)) {
|
SOAP_DBGERR("invalid parameters! configure the paras firstly please!\n");
|
|
return -1;
|
}
|
}
|
|
pthread_mutex_lock(&(DevData->node_mutex));
|
|
DevData->dev_stat = ENABLE;
|
DevData->tour_type = type;
|
sem_post(&(DevData->sem_tour));
|
|
pthread_mutex_unlock(&(DevData->node_mutex));
|
|
return 0;
|
}
|
|
/*stop the tour*/
|
void proto_PTZPreset_tour_stop(PtzDevs *DevData) {
|
|
if ((DevData != NULL) && (DevData->dev_stat == ENABLE)) {
|
|
pthread_mutex_lock(&(DevData->node_mutex));
|
|
DevData->dev_stat = DISABLE;
|
|
pthread_cond_signal(&(DevData->cond));
|
|
pthread_mutex_unlock(&(DevData->node_mutex));
|
}
|
}
|
|
/*set the tradditional tour parameters table such as the one from hikon*/
|
int proto_PTZPreset_tradtour_set(PtzDevs *DevData, PresetToure_tradnode *PosArr, int n) {
|
|
int i;
|
|
if ((PosArr == NULL) || (n < 2) || (DevData == NULL)) {
|
|
SOAP_DBGERR("invalid parameters!\n");
|
|
return -1;
|
}
|
|
if (DevData->dev_stat == ENABLE) {
|
SOAP_DBGERR("the tour is in progress, the paras can not been set in this stat!\n");
|
|
return -1;
|
}
|
|
pthread_mutex_lock(&(DevData->node_mutex));
|
|
if (DevData->postradNode != NULL) {
|
free(DevData->postradNode);
|
}
|
|
DevData->postradNode = (PresetToure_tradnode *)malloc(sizeof(PresetToure_tradnode) * n);
|
if (DevData->postradNode == NULL) {
|
SOAP_DBGERR("in %s: out of memory!\n", __FUNCTION__);
|
|
exit(1);
|
}
|
DevData->postradNode_count = n;
|
|
memcpy(DevData->postradNode, PosArr, sizeof(PresetToure_tradnode) * n);
|
|
for (i = 0; i < n; i++) {
|
if ((DevData->postradNode + i)->time_sec < MIN_TOUR_TIME) {
|
(DevData->postradNode + i)->time_sec = MIN_TOUR_TIME;
|
}
|
}
|
|
pthread_mutex_unlock(&(DevData->node_mutex));
|
|
return 0;
|
}
|
|
/*pos_p:
|
* the angle that will be rotated in pan
|
*pos_t:
|
* the angle that will be rotated in tilt
|
*pos_z:
|
* the zoom value that will be zoomed
|
*/
|
int proto_PTZSet(PtzDevs *DevData, float pos_p, float pos_t, float pos_z, \
|
const char *username, const char *passwd) {
|
int result = 0;
|
float data_p, data_t, data_z;
|
struct soap *soap = NULL;
|
PosCur pos_get;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
|
|
data_p = pos_res_p(pos_get.p - pos_p);
|
data_t = pos_res_t(pos_get.t - pos_t);
|
data_z = pos_res_z(pos_z);
|
|
struct _tptz__AbsoluteMove *absMove = soap_new__tptz__AbsoluteMove(soap, 1);
|
struct _tptz__AbsoluteMoveResponse *absMoveResponse = soap_new__tptz__AbsoluteMoveResponse(soap, 1);
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
absMove->ProfileToken = DevData->profiles->token;
|
|
absMove->Position = soap_new_tt__PTZVector(soap, 1);
|
absMove->Position->PanTilt = soap_new_tt__Vector2D(soap, 1);
|
absMove->Position->Zoom = soap_new_tt__Vector1D(soap, 1);
|
absMove->Speed = soap_new_tt__PTZSpeed(soap, 1);
|
absMove->Speed->PanTilt = soap_new_tt__Vector2D(soap, 1);
|
absMove->Speed->Zoom = soap_new_tt__Vector1D(soap, 1);
|
|
absMove->Position->PanTilt->x = data_p;
|
absMove->Position->PanTilt->y = data_t;
|
absMove->Position->Zoom->x = data_z;
|
|
result = soap_call___tptz__AbsoluteMove(soap, PTZXAddr, NULL, absMove, absMoveResponse);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZAbsoluteMove");
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
return 0;
|
}
|
|
/*stop the PTZ*/
|
int proto_PTZStop(PtzDevs *DevData, const char *username, const char *passwd) {
|
int result = 0;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
struct soap *soap = NULL;
|
enum xsd__boolean xsd_true = xsd__boolean__true_;
|
enum xsd__boolean xsd_false = xsd__boolean__false_;
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
struct _tptz__Stop *tptzStop = soap_new__tptz__Stop(soap, 1);
|
struct _tptz__StopResponse *tptzStopResponse = soap_new__tptz__StopResponse(soap, 1);
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
tptzStop->ProfileToken = DevData->profiles->token;
|
|
tptzStop->PanTilt = &xsd_true;
|
tptzStop->Zoom = &xsd_false;
|
result = soap_call___tptz__Stop(soap, PTZXAddr, NULL, tptzStop, tptzStopResponse);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZStopMove");
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
return result;
|
}
|
|
/*
|
* speed_p && speed_t && speed_z: 0 - MAX_ZOOM
|
*/
|
int proto_PTZControl(PtzDevs *DevData, enum PTZCMD cmd, float speed_p, float speed_t, float speed_z,
|
const char *username, const char *passwd) {
|
int result = 0;
|
float data_p, data_t, data_z;
|
struct soap *soap = NULL;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
struct _tptz__ContinuousMove* contMove = soap_new__tptz__ContinuousMove(soap, 1);
|
struct _tptz__ContinuousMoveResponse* contMoveResponse = soap_new__tptz__ContinuousMoveResponse(soap, 1);
|
|
data_p = pos_res_z(speed_p);
|
data_t = pos_res_z(speed_t);
|
data_z = pos_res_z(speed_z);
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
contMove->ProfileToken = DevData->profiles->token;
|
contMove->Velocity = soap_new_tt__PTZSpeed(soap, 1);
|
memset(contMove->Velocity, 0x00, sizeof(struct tt__PTZSpeed));
|
contMove->Velocity->PanTilt = soap_new_tt__Vector2D(soap, 1);
|
memset(contMove->Velocity->PanTilt, 0x00, sizeof(struct tt__Vector2D));
|
contMove->Velocity->Zoom = soap_new_tt__Vector1D(soap, 1);
|
memset(contMove->Velocity->Zoom, 0x00, sizeof(struct tt__Vector1D));
|
|
switch (cmd)
|
{
|
case PTZ_CMD_LEFT:
|
contMove->Velocity->PanTilt->x = data_p * (-1);
|
|
break;
|
|
case PTZ_CMD_RIGHT:
|
contMove->Velocity->PanTilt->x = data_p;
|
|
break;
|
|
case PTZ_CMD_UP:
|
contMove->Velocity->PanTilt->y = data_t;
|
|
break;
|
|
case PTZ_CMD_DOWN:
|
contMove->Velocity->PanTilt->y = data_t * (-1);
|
|
break;
|
|
case PTZ_CMD_LEFTUP:
|
contMove->Velocity->PanTilt->x = data_p * (-1);
|
contMove->Velocity->PanTilt->y = data_t;
|
|
break;
|
|
case PTZ_CMD_LEFTDOWN:
|
contMove->Velocity->PanTilt->x = data_p * (-1);
|
contMove->Velocity->PanTilt->y = data_t * (-1);
|
|
break;
|
|
case PTZ_CMD_RIGHTUP:
|
contMove->Velocity->PanTilt->x = data_p;
|
contMove->Velocity->PanTilt->y = data_t;
|
|
break;
|
|
case PTZ_CMD_RIGHTDOWN:
|
contMove->Velocity->PanTilt->x = data_p;
|
contMove->Velocity->PanTilt->y = data_t * (-1);
|
|
break;
|
|
case PTZ_CMD_ZOOM_IN:
|
contMove->Velocity->Zoom->x = data_z;
|
|
break;
|
|
case PTZ_CMD_ZOOM_OUT:
|
contMove->Velocity->Zoom->x = data_z * (-1);
|
|
break;
|
|
default:
|
break;
|
}
|
|
result = soap_call___tptz__ContinuousMove(soap, PTZXAddr, NULL, contMove, contMoveResponse);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZContinousMove");
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
return result;
|
}
|
|
__attribute__((unused)) static float ptz_fovh_get(float pos) {
|
|
float data = COEF_FH_B / (COEF_FH_C + pos);
|
|
float ret = expf(data) * COEF_FH_A;
|
|
return ret;
|
}
|
|
__attribute__((unused)) static float ptz_fovv_get(float pos) {
|
|
float data = COEF_FV_B / (COEF_FV_C + pos);
|
|
float ret = expf(data) * COEF_FV_A;
|
|
return ret;
|
}
|
|
int proto_PTZGetPT(PtzDevs *DevData, int x, int y, float *p, float *t, const char *username, const char *passwd) {
|
float fovx, fovy;
|
float lx, ly, lz;
|
float tx, ty, tz;
|
PosCur pos_get;
|
float pan;
|
float tilt;
|
int ret;
|
int w, h;
|
|
ret = proto_PTZ_GetStatus(DevData, &pos_get, username, passwd);
|
if (ret != 0) {
|
return ret;
|
}
|
|
#if 0
|
float temp;
|
|
temp = pos_res_z(pos_get.z);
|
fovx = ptz_fovh_get(temp);
|
fovy = ptz_fovv_get(temp);
|
#else
|
fovx = MAX_FOVH;
|
fovy = MAX_FOVV;
|
#endif
|
|
pan = 0.0;
|
tilt = 0.0;
|
|
w = img_w_get(DevData);
|
h = img_h_get(DevData);
|
|
lx = (2 * x / w - 1) * tanf(ANG2RAD(fovx / 2));
|
ly = (2 * y / h - 1) * tanf(ANG2RAD(fovy / 2));
|
lz = 1;
|
|
tx = cosf(pan) * cosf(tilt) * lx - cosf(tilt) * sinf(pan) * ly - sinf(tilt) * lz;
|
ty = sinf(pan) * lx + cosf(pan) * ly;
|
tz = cosf(pan) * sinf(tilt) * lx - sinf(pan) * sinf(tilt) * ly + cosf(tilt) * lz;
|
|
pan = atan2f(tx, tz);
|
tilt = atan2f(ty, tz);
|
|
if (p != NULL) {
|
*p = RAD2ANG(pan);
|
}
|
|
if (t != NULL) {
|
*t = RAD2ANG(tilt);
|
}
|
|
return 0;
|
}
|
|
/*Set the position based on the current pos*/
|
int proto_PTZSetStep(PtzDevs *DevData, enum PTZCMD cmd, float pos_p, float pos_t, float pos_z, \
|
const char *username, const char *passwd) {
|
int result = 0;
|
struct soap *soap = NULL;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
pos_p = para_check(pos_p);
|
pos_t = para_check(pos_t);
|
pos_z = para_check(pos_z);
|
|
struct _tptz__RelativeMove *relMove = soap_new__tptz__RelativeMove(soap, 1);
|
struct _tptz__RelativeMoveResponse *relMoveResponse = soap_new__tptz__RelativeMoveResponse(soap, 1);
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
relMove->ProfileToken = DevData->profiles->token;
|
relMove->Translation = (struct tt__PTZVector *)soap_malloc(soap, sizeof(struct tt__PTZVector));
|
memset(relMove->Translation, 0x00, sizeof(struct tt__PTZVector));
|
relMove->Translation->PanTilt = soap_new_tt__Vector2D(soap, 1);
|
relMove->Translation->Zoom = soap_new_tt__Vector1D(soap, 1);
|
|
memset(relMove->Translation->PanTilt, 0x00, sizeof(struct tt__Vector2D));
|
memset(relMove->Translation->Zoom, 0x00, sizeof(struct tt__Vector1D));
|
|
switch (cmd) {
|
case PTZ_CMD_LEFT:
|
relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);
|
|
break;
|
|
case PTZ_CMD_RIGHT:
|
relMove->Translation->PanTilt->x = fabsf(pos_p);
|
|
break;
|
case PTZ_CMD_UP:
|
relMove->Translation->PanTilt->y = fabsf(pos_t);
|
|
break;
|
|
case PTZ_CMD_DOWN:
|
relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);
|
|
break;
|
|
case PTZ_CMD_LEFTUP:
|
relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);
|
relMove->Translation->PanTilt->y = fabsf(pos_t);
|
|
break;
|
|
case PTZ_CMD_LEFTDOWN:
|
relMove->Translation->PanTilt->x = fabsf(pos_p) * (-1);
|
relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);
|
|
break;
|
case PTZ_CMD_RIGHTUP:
|
relMove->Translation->PanTilt->x = fabsf(pos_p);
|
relMove->Translation->PanTilt->y = fabsf(pos_t);
|
|
break;
|
|
case PTZ_CMD_RIGHTDOWN:
|
relMove->Translation->PanTilt->x = fabsf(pos_p);
|
relMove->Translation->PanTilt->y = fabsf(pos_t) * (-1);
|
|
break;
|
|
case PTZ_CMD_ZOOM_IN:
|
relMove->Translation->Zoom->x = fabsf(pos_z);
|
|
break;
|
|
case PTZ_CMD_ZOOM_OUT:
|
relMove->Translation->Zoom->x = fabsf(pos_z) * (-1);
|
|
break;
|
|
default:
|
break;
|
}
|
|
result = soap_call___tptz__RelativeMove(soap, PTZXAddr, NULL, relMove, relMoveResponse);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZRelativeMove");
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
|
return result;
|
}
|
|
/*set the focus-on functionality*/
|
int proto_PTZ_ImagingSet(PtzDevs *DevData, enum PTZCMD cmd, float speed, const char *username, const char *passwd) {
|
int result = 0;
|
float speed_val;
|
struct soap *soap = NULL;
|
struct tt__FocusMove *stFocusMove = NULL;
|
struct tt__ContinuousFocus *stContFocus = NULL;
|
struct _timg__Move *stMoveReq = NULL;
|
struct _timg__MoveResponse *stMoveRes = NULL;
|
struct _timg__Stop *stStopReq = NULL;
|
struct _timg__StopResponse *stStopRes = NULL;
|
|
SOAP_ASSERT(NULL != DevData);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
stFocusMove = soap_new_tt__FocusMove(soap, 1);
|
stContFocus = soap_new_tt__ContinuousFocus(soap, 1);
|
|
stMoveReq = soap_new__timg__Move(soap, 1);
|
stMoveRes = soap_new__timg__MoveResponse(soap, 1);
|
|
stStopReq = soap_new__timg__Stop(soap, 1);
|
stStopRes = soap_new__timg__StopResponse(soap, 1);
|
|
memset(stMoveReq, 0x00, sizeof(struct _timg__Move));
|
memset(stMoveRes, 0x00, sizeof(struct _timg__MoveResponse));
|
|
memset(stStopReq, 0x00, sizeof(struct _timg__Stop));
|
memset(stStopRes, 0x00, sizeof(struct _timg__StopResponse));
|
|
stMoveReq->Focus = stFocusMove;
|
stMoveReq->Focus->Continuous = stContFocus;
|
|
stMoveReq->VideoSourceToken = DevData->profiles->videoCfg.sourceToken;
|
stStopReq->VideoSourceToken = DevData->profiles->videoCfg.sourceToken;
|
|
proto_SetAuthInfo(soap, username, passwd);
|
|
speed_val = pos_res_z(speed);
|
switch(cmd) {
|
case PTZ_CMD_FOCUS_IN:
|
stContFocus->Speed = speed_val;
|
|
break;
|
|
case PTZ_CMD_FOCUS_OUT:
|
stContFocus->Speed = speed_val * (-1);
|
|
break;
|
|
default:
|
result = soap_call___timg__Stop(soap, DevData->capa.MediaXAddr, NULL, stStopReq, stStopRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZ_ImagingSet");
|
|
break;
|
}
|
|
stFocusMove->Continuous = stContFocus;
|
stFocusMove->Absolute = NULL;
|
stFocusMove->Relative = NULL;
|
|
result = soap_call___timg__Move(soap, DevData->capa.MediaXAddr, NULL, stMoveReq, stMoveRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZ_ImagingSet");
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
|
return 0;
|
}
|
|
/*Set the thradditional preset pos such as the one from hikon*/
|
int proto_PTZPreset(PtzDevs *DevData, const char *posName, enum PreSetCMD cmd, \
|
int *posToken, const char *username, const char *passwd) {
|
int result;
|
char buf[100] = { 0x00 };
|
struct soap *soap = NULL;
|
char PTZXAddr[PROTO_ADDRESS_SIZE] = { 0x00 };
|
|
SOAP_ASSERT(DevData != NULL);
|
SOAP_ASSERT(NULL != DevData->profiles);
|
SOAP_ASSERT(NULL != (soap = proto_soap_new(SOAP_SOCK_TIMEOUT)));
|
|
strncpy(PTZXAddr, DevData->capa.PTZXAddr, PROTO_ADDRESS_SIZE);
|
proto_SetAuthInfo(soap, username, passwd);
|
|
if (cmd == PRESET_SET) {
|
struct _tptz__SetPreset *setPreset = soap_new__tptz__SetPreset(soap, 1);
|
struct _tptz__SetPresetResponse *setPresetRes = soap_new__tptz__SetPresetResponse(soap, 1);
|
|
memset(setPreset, 0x00, sizeof(struct _tptz__SetPreset));
|
memset(setPresetRes, 0x00, sizeof(struct _tptz__SetPresetResponse));
|
|
setPreset->ProfileToken = DevData->profiles->token;
|
if (posName != NULL) {
|
setPreset->PresetName = (char *)posName;
|
}
|
|
result = soap_call___tptz__SetPreset(soap, PTZXAddr, NULL, setPreset, setPresetRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");
|
|
if (posToken != NULL) {
|
*posToken = atoi(setPresetRes->PresetToken);
|
}
|
|
} else if (cmd == PRESET_GET) {
|
struct _tptz__GetPresets *getPresets = soap_new__tptz__GetPresets(soap, 1);
|
struct _tptz__GetPresetsResponse *getPresetsRes = soap_new__tptz__GetPresetsResponse(soap, 1);
|
|
memset(getPresets, 0x00, sizeof(struct _tptz__GetPresets));
|
memset(getPresetsRes, 0x00, sizeof(struct _tptz__GetPresetsResponse));
|
|
getPresets->ProfileToken = DevData->profiles->token;
|
|
result = soap_call___tptz__GetPresets(soap, PTZXAddr, NULL, getPresets, getPresetsRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");
|
|
#if defined(PROTO_DEBUG)
|
dump__GetPresetsResponse(getPresetsRes);
|
#endif
|
|
} else if (cmd == PRESET_GOTO) {
|
struct _tptz__GotoPreset *gotoPreset = soap_new__tptz__GotoPreset(soap, 1);
|
struct _tptz__GotoPresetResponse *gotoPresetRes = soap_new__tptz__GotoPresetResponse(soap, 1);
|
|
memset(gotoPreset, 0x00, sizeof(struct _tptz__GotoPreset));
|
memset(gotoPresetRes, 0x00, sizeof(struct _tptz__GotoPresetResponse));
|
|
gotoPreset->ProfileToken = DevData->profiles->token;
|
|
if (posToken != NULL) {
|
sprintf(buf, "%d", *posToken);
|
gotoPreset->PresetToken = buf;
|
}
|
|
result = soap_call___tptz__GotoPreset(soap, PTZXAddr, NULL, gotoPreset, gotoPresetRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");
|
|
} else if (cmd == PRESET_REMOVE) {
|
struct _tptz__RemovePreset *rmPreset = soap_new__tptz__RemovePreset(soap, 1);
|
struct _tptz__RemovePresetResponse *rmPresetRes = soap_new__tptz__RemovePresetResponse(soap, 1);
|
|
memset(rmPreset, 0x00, sizeof(struct _tptz__RemovePreset));
|
memset(rmPresetRes, 0x00, sizeof(struct _tptz__RemovePresetResponse));
|
|
rmPreset->ProfileToken = DevData->profiles->token;
|
if (posToken != NULL) {
|
sprintf(buf, "%d", *posToken);
|
rmPreset->PresetToken = buf;
|
}
|
|
result = soap_call___tptz__RemovePreset(soap, PTZXAddr, NULL, rmPreset, rmPresetRes);
|
SOAP_CHECK_ERROR(result, soap, "proto_PTZPreset");
|
}
|
|
EXIT:
|
if (NULL != soap) {
|
proto_soap_delete(soap);
|
}
|
|
return result;
|
}
|
|
