// --------------------------------------------------
int WSAClientSocket::readUpto(void *p, int l)
{
int bytesRead=0;
while (l)
{
int r = recv(sockNum, (char *)p, l, 0);
if (r == SOCKET_ERROR)
{
// non-blocking sockets always fall through to here
checkTimeout(true,false);
}else if (r == 0)
{
break;
}else
{
stats.add(Stats::BYTESIN,r);
if (host.localIP())
stats.add(Stats::LOCALBYTESIN,r);
updateTotals(r,0);
bytesRead += r;
l -= r;
p = (char *)p+r;
}
}
return bytesRead;
}
void DistanceEstimation::updateFeatureIntern(const base::samples::LaserScan &feature, const Eigen::Affine3d &featureInOdometry)
{
checkTimeout();
if(feature.isValidBeam(0) && feature.ranges.front() > base::samples::MAX_RANGE_ERROR)
{
if(actualFeature.ranges.front() <= base::samples::MAX_RANGE_ERROR)
{
actualFeature.ranges.front() = feature.ranges.front();
}
else
{
double new_range = (double)feature.ranges.front() * 0.001;
double actual_range = (double)actualFeature.ranges.front() * 0.001;
double dist = abs(new_range - actual_range);
if(dist > max_distance)
{
// limit distance to the next point
new_range = actual_range + ((new_range - actual_range) / dist) * max_distance;
}
actual_range = actual_range * weightOldValue + new_range * weightNewValue;
actualFeature.ranges.front() = (uint32_t)(actual_range * 1000.0);
}
actualFeature.start_angle = feature.start_angle;
actualFeature.time = feature.time;
}
}
bool Socket::think()
{
if(connecting)
{
fd_set monitor;
timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 0;
FD_ZERO(&monitor);
FD_SET(s, &monitor);
select(s+1, 0, &monitor, 0, &tv);
if(FD_ISSET(s, &monitor))
{
int status;
socklen_t len = sizeof(status);
getsockopt(s, SOL_SOCKET, SO_ERROR, (char *)&status, &len);
if(status != 0) // Error
{
error = ErrorConnect;
return true;
}
connected = true;
connecting = false;
resetTimer();
}
else
checkTimeout();
}
return error != ErrorNone;
}
bool Socket::readChunk()
{
dataBegin = dataEnd = 0;
int r = recv(s, staticBuffer, 1024, 0);
if(r != -1)
{
if(r == 0)
{
connected = false;
return true; // No more data
}
dataBegin = staticBuffer;
dataEnd = staticBuffer + r;
resetTimer();
}
else if(sockError() != EWOULDBLOCK)
{
error = ErrorRecv;
return true; // Error
}
else
{
checkTimeout();
}
return error != ErrorNone;
}
void SolverDefaultImplementation::writeToFile(const int& stp, const double& t, const double& h)
{
#ifdef RUNTIME_PROFILING
MEASURETIME_REGION_DEFINE(writeFunctionStartValues, "solverWriteOutput");
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_START(writeFunctionStartValues, solverWriteOutputHandler, "solverWriteOutput");
}
#endif
if(_settings->getGlobalSettings()->getOutputFormat()!= EMPTY)
{
IWriteOutput* writeoutput_system = dynamic_cast<IWriteOutput*>(_system);
if(_outputCommand & IWriteOutput::WRITEOUT)
{
writeoutput_system->writeOutput(_outputCommand);
}
}
checkTimeout();
#ifdef RUNTIME_PROFILING
if(MeasureTime::getInstance() != NULL)
{
MEASURETIME_END(writeFunctionStartValues, writeFunctionEndValues, measureTimeFunctionsArray[0], solverWriteOutputHandler);
}
#endif
}
bool Socket::trySend(char const*& b, char const* e)
{
assert(b <= e);
if(b == e)
return true;
int sent = ::send(s, b, e - b, 0);
if(sent == -1)
{
if(sockError() != EWOULDBLOCK) // Error
{
error = ErrorSend;
return true;
}
else
checkTimeout();
}
else
{
b += sent;
if(b == e)
return true;
}
return error != ErrorNone;
}
// --------------------------------------------------
void WSAClientSocket::write(const void *p, int l)
{
while (l)
{
int r = send(sockNum, (char *)p, l, 0);
if (r == SOCKET_ERROR)
{
checkTimeout(false,true);
}
else if (r == 0)
{
throw SockException("Closed on write");
}
else
if (r > 0)
{
stats.add(Stats::BYTESOUT,r);
if (host.localIP())
stats.add(Stats::LOCALBYTESOUT,r);
updateTotals(0,r);
l -= r;
p = (char *)p+r;
}
}
}
double DistanceEstimation::getActualDistance()
{
checkTimeout();
if(actualFeature.ranges.front() <= base::samples::MAX_RANGE_ERROR)
return -1;
return (double)actualFeature.ranges.front() * 0.001;
}
void throughPutTest(void)
{
static enum {INIT,LOADING} state = INIT;
static whoPacket_t clear = {CLEAR_STATS}, get = {GET_STATS};
switch(state)
{
case INIT:
clear.who.to = testdest;
clear.who.from = whoami();
if (sendNormalPacketLink((Byte *)&clear, sizeof(clear), loadLink))
{
whoPacket_t *wp = (whoPacket_t *)testPacket;
print ("\nstarting throughput test...");
flush();
setPacketHandler(STATS, testResults);
state = LOADING;
framesOut = packetsInGroup;
setTimeout(0, &testLength_to);
setTimeout(1 TO_SEC, &sendwait_to);
get.who.to = testdest;
get.who.from = whoami();
wp->who.to = testdest;
wp->who.from = whoami();
}
break;
case LOADING:
if (framesOut)
{
bool flag;
if (loadLink->enableSps)
flag = sendSecurePacketLink(testPacket, sizeof(testPacket), loadLink);
else
flag = sendNormalPacketLink(testPacket, sizeof(testPacket), loadLink);
if (flag)
{
framesOut--;
setTimeout(1 TO_SEC, &sendwait_to);
}
if (checkTimeout(&sendwait_to))
{
print ("\n timedout - ");
printDec((Long)sinceTimeout(&testLength_to));
print(" ms; giving up throughput test\n");
state = INIT;
return;
}
}
else if (sendNormalPacketLink((Byte *)&get, sizeof(get), loadLink))
{
state = INIT;
print (" done. getting results...");
flush();
return;
}
break;
}
activate(throughPutTest);
}
void GLTouchScreen::onTouchMove(MotionEvent* event) {
checkTimeout();
if (touchDown) {
MotionPointer* pointer = event->getPointer(0);
record(pointer->x, pointer->y);
}
}
/* Mode Getter */
bool MAVLink::getMode(uint32_t &mode) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_MODE);
if (mm == NULL) {
return false;
}
mode = mavlink_msg_set_mode_get_mode(&mm->msg);
return checkTimeout(mm,MAVLINK_MODE_TIMEOUT);
}
void *loop(void *threadid)
{
while (1)
{
sleep(SECOND);
checkTimeout();
}
return NULL ;
}
static bool checkLauncherWithTimeout(uint32_t testDelay,
double startTime,
double timeout,
uint64_t launchId,
MpiOperatorContext* ctx)
{
bool rc = checkLauncher(testDelay, launchId, ctx);
assert(rc);
return (checkTimeout(startTime, timeout, launchId, ctx) && rc);
}
void GLTouchScreen::onTouchEnd(MotionEvent* event) {
checkTimeout();
if (touchDown) {
touchDown = false;
touchTime = currentTimeMillis();
MotionPointer* pointer = event->getPointer(0);
analyze(pointer->x, pointer->y);
}
}
/* Attitude Command Getter */
bool MAVLink::getAttitudeCommand(float &roll, float &pitch, float &yaw, float &thrust) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST);
if (mm == NULL) {
return false;
}
roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(&mm->msg);
pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(&mm->msg);
yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(&mm->msg);
thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(&mm->msg);
return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT);
}
void daemonHandlerThread::work()
{
MVERBOSE_LOG(logWrapper(), verboseFlag_, "started.");
while (!die_)
{
// Check if timeouts occured every few seconds.
checkTimeout();
btg::core::os::Sleep::sleepSecond(1);
}
}
/* VFR HUD Getter */
bool MAVLink::getVFRHUD(float &airspeed, float &groundspeed, int16_t &heading, uint16_t &throttle, float &alt, float &climb) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_VFR_HUD);
if (mm == NULL) {
return false;
}
airspeed = mavlink_msg_vfr_hud_get_airspeed(&mm->msg);
groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&mm->msg);
heading = mavlink_msg_vfr_hud_get_heading(&mm->msg);
throttle = mavlink_msg_vfr_hud_get_throttle(&mm->msg);
alt = mavlink_msg_vfr_hud_get_alt(&mm->msg);
climb = mavlink_msg_vfr_hud_get_climb(&mm->msg);
return checkTimeout(mm,MAVLINK_VFR_HUD_TIMEOUT);
}
/* Attitude Getter */
bool MAVLink::getAttitude(float &roll, float &pitch, float &yaw, float &p, float &q, float &r) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_ATTITUDE);
if (mm == NULL) {
return false;
}
roll = mavlink_msg_attitude_get_roll(&mm->msg);
pitch = mavlink_msg_attitude_get_pitch(&mm->msg);
yaw = mavlink_msg_attitude_get_yaw(&mm->msg);
p = mavlink_msg_attitude_get_rollspeed(&mm->msg);
q = mavlink_msg_attitude_get_pitchspeed(&mm->msg);
r = mavlink_msg_attitude_get_yawspeed(&mm->msg);
return checkTimeout(mm,MAVLINK_ATTITUDE_TIMEOUT);
}
bool MAVLink::getGlobalPositionInt(int32_t &lat, int32_t &lng, int32_t &alt, int16_t &vx, int16_t &vy, int16_t &vz) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
if (mm == NULL) {
return false;
}
lat = mavlink_msg_global_position_int_get_lat(&mm->msg);
lng = mavlink_msg_global_position_int_get_lon(&mm->msg);
alt = mavlink_msg_global_position_int_get_alt(&mm->msg);
vx = mavlink_msg_global_position_int_get_vx(&mm->msg);
vy = mavlink_msg_global_position_int_get_vy(&mm->msg);
vz = mavlink_msg_global_position_int_get_vz(&mm->msg);
return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT);
}
bool MAVLink::getSystemStatus(uint8_t &mode, uint8_t &nav_mode, uint8_t &status, uint16_t &load, uint16_t &vbat, uint16_t &battery_remaining, uint16_t &packet_drop) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SYS_STATUS);
if (mm == NULL) {
return false;
}
mode = mavlink_msg_sys_status_get_mode(&mm->msg);
nav_mode = mavlink_msg_sys_status_get_nav_mode(&mm->msg);
status = mavlink_msg_sys_status_get_status(&mm->msg);
load = mavlink_msg_sys_status_get_load(&mm->msg);
vbat = mavlink_msg_sys_status_get_vbat(&mm->msg);
battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&mm->msg);
packet_drop = mavlink_msg_sys_status_get_packet_drop(&mm->msg);
return checkTimeout(mm,MAVLINK_STATUS_TIMEOUT);
}
/* RC Override Getter */
bool MAVLink::getRCOverride(uint16_t &c1, uint16_t &c2, uint16_t &c3, uint16_t &c4, uint16_t &c5, uint16_t &c6, uint16_t &c7, uint16_t &c8) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE);
if (mm == NULL) {
return false;
}
c1 = mavlink_msg_rc_channels_override_get_chan1_raw(&mm->msg);
c2 = mavlink_msg_rc_channels_override_get_chan2_raw(&mm->msg);
c3 = mavlink_msg_rc_channels_override_get_chan3_raw(&mm->msg);
c4 = mavlink_msg_rc_channels_override_get_chan4_raw(&mm->msg);
c5 = mavlink_msg_rc_channels_override_get_chan5_raw(&mm->msg);
c6 = mavlink_msg_rc_channels_override_get_chan6_raw(&mm->msg);
c7 = mavlink_msg_rc_channels_override_get_chan7_raw(&mm->msg);
c8 = mavlink_msg_rc_channels_override_get_chan8_raw(&mm->msg);
return checkTimeout(mm,MAVLINK_RC_OVERRIDE_TIMEOUT);
}
/* Raw GPS Getter */
bool MAVLink::getRawGPS(int &fix, float &lat, float &lng, float &alt, float &eph, float &epv, float &v, float &crs) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_GPS_RAW);
if (mm == NULL) {
return false;
}
fix = mavlink_msg_gps_raw_get_fix_type(&mm->msg);
lat = mavlink_msg_gps_raw_get_lat(&mm->msg);
lng = mavlink_msg_gps_raw_get_lon(&mm->msg);
alt = mavlink_msg_gps_raw_get_alt(&mm->msg);
eph = mavlink_msg_gps_raw_get_eph(&mm->msg);
epv = mavlink_msg_gps_raw_get_epv(&mm->msg);
v = mavlink_msg_gps_raw_get_v(&mm->msg);
crs = mavlink_msg_gps_raw_get_hdg(&mm->msg);
return checkTimeout(mm,MAVLINK_RAW_GPS_TIMEOUT);
}
/* Raw Servo Getter */
bool MAVLink::getRawServos(uint16_t &s1, uint16_t &s2, uint16_t &s3, uint16_t &s4, uint16_t &s5, uint16_t &s6, uint16_t &s7, uint16_t &s8) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW);
if (mm == NULL) {
return false;
}
s1 = mavlink_msg_servo_output_raw_get_servo1_raw(&mm->msg);
s2 = mavlink_msg_servo_output_raw_get_servo2_raw(&mm->msg);
s3 = mavlink_msg_servo_output_raw_get_servo3_raw(&mm->msg);
s4 = mavlink_msg_servo_output_raw_get_servo4_raw(&mm->msg);
s5 = mavlink_msg_servo_output_raw_get_servo5_raw(&mm->msg);
s6 = mavlink_msg_servo_output_raw_get_servo6_raw(&mm->msg);
s7 = mavlink_msg_servo_output_raw_get_servo7_raw(&mm->msg);
s8 = mavlink_msg_servo_output_raw_get_servo8_raw(&mm->msg);
return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT);
}
/* Raw Servo Getter */
bool MAVLink::getScaledServos(int16_t &s1, int16_t &s2, int16_t &s3, int16_t &s4, int16_t &s5, int16_t &s6, int16_t &s7, int16_t &s8, uint8_t &rssi) {
MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
if (mm == NULL) {
return false;
}
s1 = mavlink_msg_rc_channels_scaled_get_chan1_scaled(&mm->msg);
s2 = mavlink_msg_rc_channels_scaled_get_chan2_scaled(&mm->msg);
s3 = mavlink_msg_rc_channels_scaled_get_chan3_scaled(&mm->msg);
s4 = mavlink_msg_rc_channels_scaled_get_chan4_scaled(&mm->msg);
s5 = mavlink_msg_rc_channels_scaled_get_chan5_scaled(&mm->msg);
s6 = mavlink_msg_rc_channels_scaled_get_chan6_scaled(&mm->msg);
s7 = mavlink_msg_rc_channels_scaled_get_chan7_scaled(&mm->msg);
s8 = mavlink_msg_rc_channels_scaled_get_chan8_scaled(&mm->msg);
return checkTimeout(mm,MAVLINK_SERVO_TIMEOUT);
}
// --------------------------------------------------
int UClientSocket::read(void *p, int l)
{
int bytesRead=0;
while (l)
{
if (rbDataSize >= l) {
memcpy(p, &apReadBuf[rbPos], l);
rbPos += l;
rbDataSize -= l;
return l;
} else if (rbDataSize > 0) {
memcpy(p, &apReadBuf[rbPos], rbDataSize);
p = (char *) p + rbDataSize;
l -= rbDataSize;
bytesRead += rbDataSize;
}
rbPos = 0;
rbDataSize = 0;
//int r = recv(sockNum, (char *)p, l, 0);
int r = recv(sockNum, apReadBuf, RBSIZE, 0);
if (r == SOCKET_ERROR)
{
// non-blocking sockets always fall through to here
checkTimeout(true,false);
}else if (r == 0)
{
throw SockException("Closed on read");
}else
{
stats.add(Stats::BYTESIN,r);
if (host.localIP())
stats.add(Stats::LOCALBYTESIN,r);
updateTotals(r,0);
//bytesRead += r;
//l -= r;
//p = (char *)p+r;
rbDataSize += r;
}
}
return bytesRead;
}
void GLTouchScreen::onTouchStart(MotionEvent* event) {
checkTimeout();
MotionPointer* pointer = event->getPointer(0);
float x = pointer->x;
float y = pointer->y;
if (checkBounds(x, y)) {
if (canStartTouch()) {
touchDown = true;
touchList.clear();
touchLast = NULL;
touchStart = currentTimeMillis();
record(x, y);
if (!touchCancel) {
touchTimeout = touchStart + 1000;
}
}
}
}
void loop() {
checkSerialAPI();
checkTimeout();
switch(toupper(CMD[0])){
case 'R': runningLights(DEL, BRI);
break;
case 'F': flashingLights(DEL, BRI);
break;
case 'P': police(DEL, BRI);
break;
case 'S': still(BRI);
break;
case 'D': disco(DEL, BRI);
break;
case 'G': glow(DEL, BRI);
break;
case 'O':
default: off();
}
}
TTYBase::TTY_RESPONSE TTYBase::readSection(uint8_t *buffer, uint32_t nsize, uint8_t stop_byte, uint8_t timeout, uint32_t *nbytes_read)
{
#ifdef _WIN32
return TTY_ERRNO;
#else
if (m_PortFD == -1)
return TTY_ERRNO;
int bytesRead = 0;
TTY_RESPONSE timeoutResponse = TTY_OK;
*nbytes_read = 0;
memset(buffer, 0, nsize);
uint8_t *read_char = nullptr;
DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: Request to read until stop char '%#02X' with %d timeout for m_PortFD %d", __FUNCTION__, stop_byte, timeout, m_PortFD);
for (;;)
{
if ((timeoutResponse = checkTimeout(timeout)))
return timeoutResponse;
read_char = reinterpret_cast<uint8_t*>(buffer + *nbytes_read);
bytesRead = ::read(m_PortFD, read_char, 1);
if (bytesRead < 0)
return TTY_READ_ERROR;
DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: buffer[%d]=%#X (%c)", __FUNCTION__, (*nbytes_read), *read_char, *read_char);
(*nbytes_read)++;
if (*read_char == stop_byte)
return TTY_OK;
else if (*nbytes_read >= nsize)
return TTY_OVERFLOW;
}
#endif
}
TTYBase::TTY_RESPONSE TTYBase::read(uint8_t *buffer, uint32_t nbytes, uint8_t timeout, uint32_t *nbytes_read)
{
#ifdef _WIN32
return TTY_ERRNO;
#else
if (m_PortFD == -1)
return TTY_ERRNO;
uint32_t numBytesToRead = nbytes;
int bytesRead = 0;
TTY_RESPONSE timeoutResponse = TTY_OK;
*nbytes_read = 0;
if (nbytes <= 0)
return TTY_PARAM_ERROR;
DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: Request to read %d bytes with %d timeout for m_PortFD %d", __FUNCTION__, nbytes, timeout, m_PortFD);
while (numBytesToRead > 0)
{
if ((timeoutResponse = checkTimeout(timeout)))
return timeoutResponse;
bytesRead = ::read(m_PortFD, buffer + (*nbytes_read), numBytesToRead);
if (bytesRead < 0)
return TTY_READ_ERROR;
DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%d bytes read and %d bytes remaining...", bytesRead, numBytesToRead - bytesRead);
for (uint32_t i = *nbytes_read; i < (*nbytes_read + bytesRead); i++)
DEBUGFDEVICE(m_DriverName, m_DebugChannel, "%s: buffer[%d]=%#X (%c)", __FUNCTION__, i, buffer[i], buffer[i]);
*nbytes_read += bytesRead;
numBytesToRead -= bytesRead;
}
return TTY_OK;
#endif
}
// --------------------------------------------------
void UClientSocket::write(const void *p, int l)
{
while (l)
{
int r = send(sockNum, (char *)p, l, MSG_DONTWAIT|MSG_NOSIGNAL);
if (r == SOCKET_ERROR)
{
// non-blocking sockets always fall through to here
checkTimeout(false,true);
}else if (r == 0)
{
throw SockException("Closed on write");
}else
{
stats.add(Stats::BYTESOUT,r);
if (host.localIP())
stats.add(Stats::LOCALBYTESOUT,r);
updateTotals(0,r);
l -= r;
p = (char *)p+r;
}
}
}
