Update of /cvsroot/playerstage/code/player/server/drivers/mixed/p2os
In directory
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv30607/server/drivers/mixed/p2os
Modified Files:
sip.cc
Log Message:
fix for sonar allocation, thanks to Luke Gumbley
Index: sip.cc
===================================================================
RCS file: /cvsroot/playerstage/code/player/server/drivers/mixed/p2os/sip.cc,v
retrieving revision 1.27
retrieving revision 1.28
diff -C2 -d -r1.27 -r1.28
*** sip.cc 2 Dec 2007 09:11:13 -0000 1.27
--- sip.cc 5 Dec 2007 22:15:38 -0000 1.28
***************
*** 141,145 ****
// bumper
int bump_count = PlayerRobotParams[param_idx].NumFrontBumpers +
PlayerRobotParams[param_idx].NumRearBumpers;
! if (data->bumper.bumpers_count != (unsigned int)bump_count)
{
data->bumper.bumpers_count = bump_count;
--- 141,145 ----
// bumper
int bump_count = PlayerRobotParams[param_idx].NumFrontBumpers +
PlayerRobotParams[param_idx].NumRearBumpers;
! if (data->bumper.bumpers_count != bump_count)
{
data->bumper.bumpers_count = bump_count;
***************
*** 418,454 ****
cnt += sizeof(unsigned char);
! if (buffer[cnt] == 0 && sonars != NULL)
! {
! // No sonar readings
! delete[] sonars;
! sonars = NULL;
! }
! else if (sonars == NULL)
! {
! // No space for sonar readings yet but need some
! sonars = new unsigned short[buffer[cnt]];
! }
! else if (buffer[cnt] != sonarreadings)
{
! // Sonar readings count has changed, reallocate the sonar readings array
! if (sonars != NULL)
! delete[] sonars;
! sonars = new unsigned short[buffer[cnt]];
! }
! sonarreadings = buffer[cnt];
! cnt += sizeof(unsigned char);
!
! //printf("%hu sonar readings:\n", sonarreadings);
! for(unsigned char i = 0;i < sonarreadings;i++) {
sonars[buffer[cnt]]= (unsigned short)
rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
PlayerRobotParams[param_idx].RangeConvFactor);
! //printf("%d %hu:",buffer[cnt],*((unsigned short *)&buffer[cnt+1]));
! //
! //printf("%hu %hu %hu\n", buffer[cnt], buffer[cnt+1], buffer[cnt+2]);
! //printf("index %d value %hu\n", buffer[cnt], sonars[buffer[cnt]]);
! cnt += 3*sizeof(unsigned char);
}
- //printf("\n");
timer = (buffer[cnt] | (buffer[cnt+1] << 8));
--- 418,454 ----
cnt += sizeof(unsigned char);
! unsigned char numSonars=buffer[cnt];
! cnt+=sizeof(unsigned char);
!
! if(numSonars>0)
{
! //find the largest sonar index supplied
! unsigned char maxSonars=sonarreadings;
! for(unsigned char i=0;i<numSonars;i++)
! {
! unsigned char sonarIndex=buffer[cnt+i*(sizeof(unsigned
char)+sizeof(unsigned short))];
! if((sonarIndex+1)>maxSonars) maxSonars=sonarIndex+1;
! }
!
! //if necessary make more space in the array and preserve existing readings
! if(maxSonars>sonarreadings)
! {
! unsigned short *newSonars=new unsigned short[maxSonars];
! for(unsigned char i=0;i<sonarreadings;i++)
! newSonars[i]=sonars[i];
! if(sonars!=NULL) delete[] sonars;
! sonars=newSonars;
! sonarreadings=maxSonars;
! }
!
! //update the sonar readings array with the new readings
! for(unsigned char i=0;i<numSonars;i++)
! {
sonars[buffer[cnt]]= (unsigned short)
rint((buffer[cnt+1] | (buffer[cnt+2] << 8)) *
PlayerRobotParams[param_idx].RangeConvFactor);
! cnt+=sizeof(unsigned char)+sizeof(unsigned short);
! }
}
timer = (buffer[cnt] | (buffer[cnt+1] << 8));
-------------------------------------------------------------------------
SF.Net email is sponsored by: The Future of Linux Business White Paper
from Novell. From the desktop to the data center, Linux is going
mainstream. Let it simplify your IT future.
http://altfarm.mediaplex.com/ad/ck/8857-50307-18918-4
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
