int
GPSSIM::devIOCTL(unsigned long cmd, unsigned long arg)
{
_sync.lock();
int ret = OK;
switch (cmd) {
case SENSORIOCRESET:
cmd_reset();
break;
default:
/* give it to parent if no one wants it */
ret = VirtDevObj::devIOCTL(cmd, arg);
break;
}
_sync.unlock();
return ret;
}
void SyncObjTest::_doTests()
{
SyncObj so;
bool passed = true;
so.lock();
so.unlock();
reportResult("Simple lock/unlock", passed);
uint64_t now = offsetTime();
unsigned long wait_in_us = 5000;
so.lock();
int rv = so.waitOnSignal(wait_in_us);
so.unlock();
uint64_t after = offsetTime();
uint64_t delta_usec = after - now;
if (rv != ETIMEDOUT) {
DF_LOG_ERR("waitSignal() did not return ETIMEDOUT");
passed = false;
}
#ifdef __APPLE__
const float error_factor = 2.0f;
#else
const float error_factor = 1.2f;
#endif
bool dtime_ok = (delta_usec > wait_in_us) && (delta_usec < wait_in_us * error_factor);
if (!dtime_ok) {
DF_LOG_ERR("waitSignal() timeout of %luus was %" PRIu64 "us", wait_in_us, delta_usec);
passed = false;
}
reportResult("waitSignal() timeout", passed && dtime_ok);
}
void
GPSSIM::task_main()
{
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
if (_fake_gps) {
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s *
_report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(m_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
usleep(2e5);
} else {
//Publish initial report that we have access to a GPS
//Make sure to clear any stale data in case driver is reset
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.timestamp_time_relative = 0;
if (!(m_pub_blocked)) {
if (_report_gps_pos_pub != nullptr) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(m_pub_blocked)) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {
if (_report_sat_info_pub != nullptr) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
}
// FIXME - if ioctl is called then it will deadlock
_sync.lock();
}
}
PX4_INFO("exiting");
/* tell the dtor that we are exiting */
_task = -1;
return;
}
