static int32_t
af_inet_bind_to_port_lt_ceiling(int fd, struct sockaddr *sockaddr,
socklen_t sockaddr_len, uint32_t ceiling)
{
#if GF_DISABLE_PRIVPORT_TRACKING
_assign_port(sockaddr, 0);
return bind(fd, sockaddr, sockaddr_len);
#else
int32_t ret = -1;
uint16_t port = ceiling - 1;
unsigned char ports[GF_PORT_ARRAY_SIZE] = {
0,
};
int i = 0;
loop:
ret = gf_process_reserved_ports(ports, ceiling);
while (port) {
if (port == GF_CLIENT_PORT_CEILING) {
ret = -1;
break;
}
/* ignore the reserved ports */
if (BIT_VALUE(ports, port)) {
port--;
continue;
}
_assign_port(sockaddr, port);
ret = bind(fd, sockaddr, sockaddr_len);
if (ret == 0)
break;
if (ret == -1 && errno == EACCES)
break;
port--;
}
/* In case if all the secure ports are exhausted, we are no more
* binding to secure ports, hence instead of getting a random
* port, lets define the range to restrict it from getting from
* ports reserved for bricks i.e from range of 49152 - 65535
* which further may lead to port clash */
if (!port) {
ceiling = port = GF_CLNT_INSECURE_PORT_CEILING;
for (i = 0; i <= ceiling; i++)
BIT_CLEAR(ports, i);
goto loop;
}
return ret;
#endif /* GF_DISABLE_PRIVPORT_TRACKING */
}
void vmcs_write_nmi_window_bit(struct _VMCS_OBJECT *vmcs, BOOLEAN value)
{
vmcs_update(vmcs, VMCS_CONTROL_VECTOR_PROCESSOR_EVENTS,
FALSE == value ? 0 : (UINT64) -1, BIT_VALUE(NMI_WINDOW_BIT));
if (value)
nmi_window_set();
else
nmi_window_clear();
}
static int bitmap_is_set(struct vbfs_bitmap *bitmap, size_t bit)
{
uint32_t *bm = NULL;
uint32_t value = 0;
bm = bitmap->bitmap;
bm += UNIT_OFFSET(bit);
value = le32_to_cpu(*bm);
return !!(value & BIT_VALUE(bit));
}
int bitmap_set_bit(struct vbfs_bitmap *bitmap, size_t bit)
{
uint32_t *bm = NULL;
uint32_t value = 0;
if (bitmap->max_bit <= bit)
return -1;
bm = bitmap->bitmap;
bm += UNIT_OFFSET(bit);
value = le32_to_cpu(*bm);
value |= BIT_VALUE(bit);
*bm = le32_to_cpu(value);
return 0;
}
/**@brief Function for decoding a ITU Sensor Service Configuration.
*
* @param[in] p_iss ITU Sensor Service structure
* @param[out] p_encoded_buffer Buffer where the encoded data will be written.
*
* @return Size of encoded data.
*/
static void its_conf_decode(iss_t * p_iss,uint16_t data_length, uint8_t * data)
{
if(data_length > 0){
//p_iss->gatt_db_needs_cleaning = true;
uint8_t data_bits = data[0];
//if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_OVERRIDE_BIT_PLACEMENT)){
//OVERRIDE.. Client wants to set new values
if(data_length > 1){
uint8_t i = 1;
//Check if Seq. nr. is set
if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_SPACE_TIME_BIT_PLACEMENT)){
p_iss->coord = data[i];
i++;
p_iss->curr_seq_nr = uint16_decode(&data[i]);
i += 2;
}
//Check if Unit is set
if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_UNIT_BIT_PLACEMENT)){
p_iss->unit = uint16_decode(&data[i]);
i += 2;
}
//Check if Type is set
if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_TYPE_MAKE_BIT_PLACEMENT)){
p_iss->type = data[i];
i++;
p_iss->make = data[i];
i++;
}
//Check if ID is set
if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_ID_BIT_PLACEMENT)){
p_iss->ID = uint16_decode(&data[i]);
i += 2;
}
//Check if Sampling freq. is set
if(BIT_VALUE(data_bits,ITS_MEAS_FLAG_SAMP_FREQ_BIT_PLACEMENT)){
p_iss->samp_freq_in_m_sec = uint32_decode(&data[i]);
p_iss->p_update_samp_freq();
}
}
/*}else{
//set presentation
// SET Space + Time
p_iss->space_time_present = BIT_VALUE(data_bits,ITS_MEAS_FLAG_SPACE_TIME_BIT_PLACEMENT);
//SET Unit
p_iss->unit_present = BIT_VALUE(data_bits,ITS_MEAS_FLAG_UNIT_BIT_PLACEMENT);
//SET Type
p_iss->type_make_present = BIT_VALUE(data_bits,ITS_MEAS_FLAG_TYPE_MAKE_BIT_PLACEMENT);
//SET ID
p_iss->ID_present = BIT_VALUE(data_bits,ITS_MEAS_FLAG_ID_BIT_PLACEMENT);
//SET Sampling freq.
p_iss->samp_freq_present = BIT_VALUE(data_bits,ITS_MEAS_FLAG_SAMP_FREQ_BIT_PLACEMENT);
}
uint16_t data_len = MAX_HTM_LEN;
uint8_t data_buf[MAX_HTM_LEN];
memset(data_buf, 0, sizeof(data_buf));
data_len = its_conf_encode(p_iss, data_buf);
sd_ble_gatts_value_set(p_iss->meas_conf_handles.value_handle,
0,
&data_len,
data_buf);*/
}
}
//------------------------------------------------------------------------------
// Process all messages for this driver.
int MotorDriver::ProcessMessage( QueuePointer& respQueue,
player_msghdr* pHeader, void* pData )
{
// New motor command - velocity
if( Message::MatchMessage( pHeader, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION3D_CMD_SET_VEL,
this->device_addr ) )
{
player_position3d_cmd_vel_t* pCmd = (player_position3d_cmd_vel_t*)pData;
F32 depthSpeed = MOTOR_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.pz, 1.0f ) );
F32 normalisedFrontPWM = (depthSpeed + 1.0f)/2.0f;
S32 frontDuty = MIN_DUTY_CYCLE_US
+ (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedFrontPWM);
F32 PITCH_MOTER_PER = 0.5;
F32 pitchSpeed = PITCH_MOTER_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.ppitch, 1.0f ) );
F32 normalisedBackPWM = (pitchSpeed + 1.0f)/2.0f;
S32 backDuty = MIN_DUTY_CYCLE_US
+ (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedBackPWM);
F32 linearSpeed = MAX( -1.0f, MIN( (F32)pCmd->vel.px, 1.0f ) );
F32 yawSpeed = MAX( -1.0f, MIN( (F32)pCmd->vel.pyaw, 1.0f ) );
F32 leftSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed + yawSpeed, 1.0f ) );
F32 rightSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed - yawSpeed, 1.0f ) );
// if ( yawSpeed > 0 )
// {
// F32 leftSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed + yawSpeed, 1.0f ) );
// F32 rightSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed, 1.0f ) );
// }
// else
// {
// F32 leftSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed, 1.0f ) );
// F32 rightSpeed = MOTOR_PER*MAX( -1.0f, MIN( linearSpeed - yawSpeed , 1.0f ) );
// }
F32 normalisedLeftPWM = (leftSpeed + 1.0f)/2.0f;
F32 normalisedRightPWM = (rightSpeed + 1.0f)/2.0f;
S32 leftDuty = MIN_DUTY_CYCLE_US
+ (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedLeftPWM);
S32 rightDuty = MIN_DUTY_CYCLE_US
+ (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedRightPWM);
// Offset duty cycles to account for dodgy motor controllers
leftDuty = MAX( MIN_DUTY_CYCLE_US, MIN( leftDuty + LEFT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
rightDuty = MAX( MIN_DUTY_CYCLE_US, MIN( rightDuty + RIGHT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
frontDuty = MAX( MIN_DUTY_CYCLE_US, MIN( frontDuty + FRONT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
backDuty = MAX( MIN_DUTY_CYCLE_US, MIN( backDuty + BACK_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
//printf( "Depth speed: %2.3f, Pitch speed: %2.3f, yawSpeed %2.3f\n",
// depthSpeed, pitchSpeed, yawSpeed );
//printf( "Seting PWMS %i, %i, %i, %i\n",
// leftDuty, rightDuty, frontDuty, backDuty );
if ( mbInitialisedPWM )
{
U8 motorControlFlags = (S32)pCmd->state;
if ( BIT_VALUE( motorControlFlags, 3 ) == MOTOR_CONTROLLED )
{
//printf( "Send L: %i ", leftDuty );
pwm_SetPulse( LEFT_MOTOR_CHANNEL, PWM_FREQUENCY_US, leftDuty );
}
if ( BIT_VALUE( motorControlFlags, 2 ) == MOTOR_CONTROLLED )
{
//printf( "Send R: %i ", rightDuty );
pwm_SetPulse( RIGHT_MOTOR_CHANNEL, PWM_FREQUENCY_US, rightDuty );
}
if ( BIT_VALUE( motorControlFlags, 1 ) == MOTOR_CONTROLLED )
{
//printf( "Send F: %i ", frontDuty );
pwm_SetPulse( FRONT_MOTOR_CHANNEL, PWM_FREQUENCY_US, frontDuty );
}
if ( BIT_VALUE( motorControlFlags, 0 ) == MOTOR_CONTROLLED )
{
//printf( "Send B: %i ", backDuty );
pwm_SetPulse( BACK_MOTOR_CHANNEL, PWM_FREQUENCY_US, backDuty );
}
if ( 15 != motorControlFlags )
{
//printf( "\n" );
}
}
return 0;
}
// Request to enable motor power
else if( Message::MatchMessage( pHeader, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION3D_REQ_MOTOR_POWER,
this->device_addr ) )
{
printf( "Ignoring motor request" );
Publish( this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
PLAYER_POSITION3D_REQ_MOTOR_POWER );
return 0;
}
else if ( Message::MatchMessage(
pHeader, PLAYER_MSGTYPE_DATA, PLAYER_IMU_DATA_STATE, mCompassID ) )
{
player_imu_data_state_t* pCompassData = (player_imu_data_state_t*)pData;
mYawCompassAngle = pCompassData->pose.pyaw;
mPitchCompassAngle = pCompassData->pose.ppitch;
mRollCompassAngle = pCompassData->pose.proll;
mbCompassAngleValid = true;
mCompassAngleTimestamp = pHeader->timestamp;
return 0;
}
else if ( Message::MatchMessage( pHeader, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION3D_CMD_SET_POS, this->device_addr ) )
{
// AB: This is a complete abuse of the interface but basically
// we use this message to set the motor powers directly.
// So, motor powers are place in the 'vel' component and are
// 'normalised' so -1.0 is full reverse and 1.0 is full forward.
// Motors are
// x = left
// y = right
// roll = forward
// pitch = back
// yaw = Motor control bits (0 == controlled, 1 == uncontrolled)
player_position3d_cmd_pos_t* pCmd = (player_position3d_cmd_pos_t*)pData;
F32 normalisedLeftPWM = -MOTOR_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.px, 1.0f ) );
F32 normalisedRightPWM = -MOTOR_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.py, 1.0f ) );
F32 normalisedFrontPWM = -MOTOR_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.proll, 1.0f ) );
F32 normalisedBackPWM = -MOTOR_PER*MAX( -1.0f, MIN( (F32)pCmd->vel.ppitch, 1.0f ) );
normalisedLeftPWM = (normalisedLeftPWM + 1.0f)/2.0f;
normalisedRightPWM = (normalisedRightPWM + 1.0f)/2.0f;
normalisedFrontPWM = (normalisedFrontPWM + 1.0f)/2.0f;
normalisedBackPWM = (normalisedBackPWM + 1.0f)/2.0f;
S32 leftDuty = MIN_DUTY_CYCLE_US + (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedLeftPWM);
S32 rightDuty = MIN_DUTY_CYCLE_US + (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedRightPWM);
S32 frontDuty = MIN_DUTY_CYCLE_US + (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedFrontPWM);
S32 backDuty = MIN_DUTY_CYCLE_US + (U32)((MAX_DUTY_CYCLE_US-MIN_DUTY_CYCLE_US)*normalisedBackPWM);
// Offset duty cycles to account for dodgy motor controllers
leftDuty = MAX( MIN_DUTY_CYCLE_US, MIN( leftDuty + LEFT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
rightDuty = MAX( MIN_DUTY_CYCLE_US, MIN( rightDuty + RIGHT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
frontDuty = MAX( MIN_DUTY_CYCLE_US, MIN( frontDuty + FRONT_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
backDuty = MAX( MIN_DUTY_CYCLE_US, MIN( backDuty + BACK_PWM_OFFSET, MAX_DUTY_CYCLE_US ) );
printf( "Seting PWMS %i, %i, %i, %i\n",
leftDuty, rightDuty, frontDuty, backDuty );
if ( mbInitialisedPWM )
{
S32 motorControlFlags = (S32)pCmd->vel.pyaw;
if ( BIT_VALUE( motorControlFlags, 3 ) == MOTOR_CONTROLLED )
{
pwm_SetPulse( LEFT_MOTOR_CHANNEL, PWM_FREQUENCY_US, leftDuty );
}
if ( BIT_VALUE( motorControlFlags, 2 ) == MOTOR_CONTROLLED )
{
pwm_SetPulse( RIGHT_MOTOR_CHANNEL, PWM_FREQUENCY_US, rightDuty );
}
if ( BIT_VALUE( motorControlFlags, 1 ) == MOTOR_CONTROLLED )
{
pwm_SetPulse( FRONT_MOTOR_CHANNEL, PWM_FREQUENCY_US, frontDuty );
}
if ( BIT_VALUE( motorControlFlags, 0 ) == MOTOR_CONTROLLED )
{
pwm_SetPulse( BACK_MOTOR_CHANNEL, PWM_FREQUENCY_US, backDuty );
}
}
return 0;
}
else if ( Message::MatchMessage(
pHeader, PLAYER_MSGTYPE_DATA, PLAYER_POSITION1D_DATA_STATE, mDepthSensorID ) )
{
player_position1d_data* pDepthSensorData = (player_position1d_data*)pData;
mDepthSensorDepth = pDepthSensorData->pos;
mbDepthSensorDepthValid = true;
mDepthSensorDepthTimestamp = pHeader->timestamp;
return 0;
}
printf( "Unhandled message\n" );
return -1;
}
void GTrieNode::readFromFile(FILE *f) {
int nchilds, i, j, pos, bits, ncond;
iPair p;
char aux, buf[MAX_BUF];
GTrieNode *c;
if (fgets(buf, MAX_BUF, f)) {
aux = buf[0]-BASE_FIRST;
if (BIT_VALUE(aux, 0)) is_graph=true;
else is_graph=false;
int nbytes = aux>>1;
pos = 1;
for (i=0, j=1, nchilds=0; i<nbytes; i++, j*=BASE_FORMAT) {
aux = buf[pos++]-BASE_FIRST;
nchilds += int(aux)*j;
}
// Connections: outgoing
aux = buf[pos++]-BASE_FIRST;
for (i=0, bits=0; i<depth; i++, bits++) {
if (bits==BASE_BITS) {
aux = buf[pos++]-BASE_FIRST;
bits=0;
}
if (BIT_VALUE(aux, bits)) out[i] = true;
else out[i] = false;
}
// Connections: ingoing
aux = buf[pos++]-BASE_FIRST;
for (i=0, bits=0; i<depth; i++, bits++) {
if (bits==BASE_BITS) {
aux = buf[pos++]-BASE_FIRST;
bits=0;
}
if (BIT_VALUE(aux, bits)) in[i] = true;
else in[i] = false;
}
// Previous Conditions
aux = buf[pos++]-BASE_FIRST;
if (aux==0) cond_ok = true;
else cond_ok = false;
if (aux>0) {
ncond = aux;
for (i=0; i<ncond; i++) {
list<iPair> newcond;
while(1) {
aux = buf[pos++]-BASE_FIRST-1;
if (aux<0) break;
p.first = aux;
aux = buf[pos++]-BASE_FIRST-1;
p.second = aux;
newcond.push_back(p);
}
cond.push_back(newcond);
}
}
if (buf[pos]!='\n') {
fprintf(stderr, "ERROR: [%s] !%d!%c!\n", buf, pos, buf[pos]);
fprintf(stderr,"[%d](%s) |", depth, is_graph?"X":" ");
for (i=0, bits=0; i<depth; i++, bits++)
fprintf(stderr,"%s", out[i]?"1":"0");
fprintf(stderr,"|\n");
}
// Conn and nconn variables (was missing)
for (i=0; i<depth; i++)
if (out[i] || in[i])
conn[(nconn)++] = i;
for (i=0; i<nchilds; i++) {
c = new GTrieNode(depth+1);
c->readFromFile(f);
child.push_back(c);
}
}
}
