Update of /cvsroot/playerstage/code/stage/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv24023/src
Modified Files:
model_ptz.c p_ptz.cc stage.h
Log Message:
applied patch to support velocity control of ptz
Index: p_ptz.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/p_ptz.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** p_ptz.cc 23 Aug 2007 19:58:49 -0000 1.3
--- p_ptz.cc 5 Sep 2007 19:01:43 -0000 1.4
***************
*** 83,88 ****
assert( this->mod->cfg_len == sizeof(stg_ptz_config_t) );
- //stg_ptz_cmd_t* cmd = (stg_ptz_cmd_t*)this->mod->cmd;
stg_ptz_config_t scfg;
memcpy( &scfg, this->mod->cfg, sizeof(scfg));
--- 83,88 ----
assert( this->mod->cfg_len == sizeof(stg_ptz_config_t) );
+ //stg_ptz_cmd_t* cmd = (stg_ptz_cmd_t*)this->mod->cmd;
stg_ptz_config_t scfg;
memcpy( &scfg, this->mod->cfg, sizeof(scfg));
***************
*** 93,96 ****
--- 93,97 ----
this->addr))
{
+
if( hdr->size == sizeof(player_ptz_cmd_t) )
{
***************
*** 105,112 ****
//scfg.zoomgoal = pcmd->zoomspeed; // not in player
! //printf( "setting goals: p%.2f t%.2f z %.2f\n",
! // scfg.pangoal,
! // scfg.tiltgoal,
! // scfg.zoomgoal );
stg_model_set_cfg( this->mod, &scfg, sizeof(scfg));
--- 106,113 ----
//scfg.zoomgoal = pcmd->zoomspeed; // not in player
! // printf( "setting goals: p%.2f t%.2f z %.2f\n",
! // scfg.goal.pan,
! // scfg.goal.tilt,
! // scfg.goal.zoom );
stg_model_set_cfg( this->mod, &scfg, sizeof(scfg));
***************
*** 122,125 ****
--- 123,128 ----
this->addr))
{
+
+
if(hdr->size == 0)
{
***************
*** 145,150 ****
return(0);
}
! }
!
// Don't know how to handle this message.
PRINT_WARN2( "Stage PTZ interface doesn't support msg with type/subtype
%d/%d",
--- 148,177 ----
return(0);
}
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
! PLAYER_PTZ_REQ_CONTROL_MODE,
! this->addr))
! {
!
! if( hdr->size == sizeof( player_ptz_req_control_mode_t) )
! {
! // convert from Player to Stage format
! player_ptz_req_control_mode_t* pcntrl = (
player_ptz_req_control_mode_t*)data;
!
! if(pcntrl->mode == PLAYER_PTZ_POSITION_CONTROL)
! scfg.position_mode=TRUE;
! else
! scfg.position_mode=FALSE;
!
! stg_model_set_cfg( this->mod, &scfg, sizeof(scfg));
! this->driver->Publish( this->addr, resp_queue,
! PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_PTZ_REQ_CONTROL_MODE);
!
! return 0;
! }
!
! }
!
// Don't know how to handle this message.
PRINT_WARN2( "Stage PTZ interface doesn't support msg with type/subtype
%d/%d",
Index: model_ptz.c
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/model_ptz.c,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** model_ptz.c 15 May 2007 00:08:32 -0000 1.3
--- model_ptz.c 5 Sep 2007 19:01:43 -0000 1.4
***************
*** 118,122 ****
--- 118,125 ----
cfg.speed.pan = STG_PTZ_SPEED_PAN;
cfg.speed.tilt = STG_PTZ_SPEED_TILT;
+
+ cfg.position_mode = TRUE;
+
stg_model_set_cfg( mod, &cfg, sizeof(cfg) );
stg_model_set_data( mod, &data, sizeof(data) );
***************
*** 195,215 ****
stg_ptz_data_t *data = (stg_ptz_data_t*)mod->data;
stg_ptz_config_t *cfg = (stg_ptz_config_t*)mod->cfg;
!
! double pandist;
! if(cfg->speed.pan > 0.0)
! pandist = cfg->speed.pan * mod->world->sim_interval/1e3;
! else
! pandist = STG_PTZ_SPEED_PAN * mod->world->sim_interval/1e3;
double panerror = cfg->goal.pan - data->pan;
! if( panerror < pandist ) pandist = panerror;
data->pan += pandist;
-
double zoomdist = cfg->speed.zoom * mod->world->sim_interval/1e3;
double zoomerror = cfg->goal.zoom - data->zoom;
! if( zoomerror < zoomdist ) zoomdist = zoomerror;
! data->zoom += zoomdist;
model_change( mod, &mod->data );
!
//printf( "PAN: %.2f\n", cfg->pan );
//printf( "ZOOM: %.2f\n", cfg->zoom );
--- 198,216 ----
stg_ptz_data_t *data = (stg_ptz_data_t*)mod->data;
stg_ptz_config_t *cfg = (stg_ptz_config_t*)mod->cfg;
!
! double pandist = cfg->speed.pan * mod->world->sim_interval/1e3;
double panerror = cfg->goal.pan - data->pan;
! if( panerror < pandist && cfg->position_mode==TRUE) //ignore goal commands
in velocity mode
! pandist = panerror;
data->pan += pandist;
double zoomdist = cfg->speed.zoom * mod->world->sim_interval/1e3;
double zoomerror = cfg->goal.zoom - data->zoom;
! if( zoomerror < zoomdist ) zoomdist = zoomerror;
! if(cfg->position_mode == TRUE) //again, ignore goal
command in velocity mode
! data->zoom += zoomdist;
model_change( mod, &mod->data );
!
!
//printf( "PAN: %.2f\n", cfg->pan );
//printf( "ZOOM: %.2f\n", cfg->zoom );
Index: stage.h
===================================================================
RCS file: /cvsroot/playerstage/code/stage/src/stage.h,v
retrieving revision 1.191
retrieving revision 1.192
diff -C2 -d -r1.191 -r1.192
*** stage.h 5 Oct 2006 22:27:07 -0000 1.191
--- stage.h 5 Sep 2007 19:01:43 -0000 1.192
***************
*** 834,837 ****
--- 834,838 ----
stg_ptz_t goal; ///< The current desired angles. The device servos
towards these values.
stg_ptz_t speed; ///< The PTZ servo speeds.
+ stg_bool_t position_mode; ///< PTZ movement mode (position or velocity
commands)
} stg_ptz_config_t;
-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems? Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
