static void *entry_adapter(void *ptr)
{
pthdata_t *data = (pthdata_t *) ptr;
int rv;
// set the threads name
#ifdef __PX4_DARWIN
rv = pthread_setname_np(data->name);
#else
rv = pthread_setname_np(pthread_self(), data->name);
#endif
if (rv) {
PX4_ERR("px4_task_spawn_cmd: failed to set name of thread %d %d\n", rv, errno);
}
data->entry(data->argc, data->argv);
free(ptr);
PX4_DEBUG("Before px4_task_exit");
px4_task_exit(0);
PX4_DEBUG("After px4_task_exit");
return nullptr;
}
static void *entry_adapter ( void *ptr )
{
pthdata_t *data;
data = (pthdata_t *) ptr;
data->entry(data->argc, data->argv);
free(ptr);
PX4_DEBUG("Before px4_task_exit");
px4_task_exit(0);
PX4_DEBUG("After px4_task_exit");
return NULL;
}
static void *entry_adapter(void *ptr)
{
pthdata_t *data;
data = (pthdata_t *) ptr;
data->entry(data->argc, data->argv);
//PX4_WARN("Before waiting infinte busy loop");
//for( ;; )
//{
// volatile int x = 0;
// ++x;
// }
free(ptr);
px4_task_exit(0);
return NULL;
}
void
GPS::task_main()
{
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
/* tell the dtor that we are exiting, set error code */
_task = -1;
px4_task_exit(1);
}
_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
initializeCommunicationDump();
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;
/* 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.alt_ellipsoid = 10000;
_report_gps_pos.s_variance_m_s = 0.5f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.8f;
_report_gps_pos.epv = 1.2f;
_report_gps_pos.hdop = 0.9f;
_report_gps_pos.vdop = 0.9f;
_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;
_report_gps_pos.satellites_used = 10;
/* no time and satellite information simulated */
publish();
usleep(2e5);
} else {
if (_helper != nullptr) {
delete(_helper);
/* set to zero to ensure parser is not used while not instantiated */
_helper = nullptr;
}
switch (_mode) {
case GPS_DRIVER_MODE_NONE:
_mode = GPS_DRIVER_MODE_UBX;
//no break
case GPS_DRIVER_MODE_UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
break;
case GPS_DRIVER_MODE_ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
break;
default:
break;
}
/* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication
*/
if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
if (_mode == GPS_DRIVER_MODE_UBX) {
/* Publish initial report that we have access to a GPS,
* but set all critical state fields to indicate we have
* no valid position lock
*/
/* reset the timestamp for data, because we have no data yet */
_report_gps_pos.timestamp = 0;
_report_gps_pos.timestamp_time_relative = 0;
/* set a massive variance */
_report_gps_pos.eph = 10000.0f;
_report_gps_pos.epv = 10000.0f;
_report_gps_pos.fix_type = 0;
publish();
/* GPS is obviously detected successfully, reset statistics */
_helper->resetUpdateRates();
}
int helper_ret;
while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
if (helper_ret & 1) {
publish();
last_rate_count++;
}
if (_p_report_sat_info && (helper_ret & 2)) {
publishSatelliteInfo();
}
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
_rate = last_rate_count / dt;
_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_last_rate_rtcm_injection_count = 0;
_helper->storeUpdateRates();
_helper->resetUpdateRates();
}
if (!_healthy) {
// Helpful for debugging, but too verbose for normal ops
// const char *mode_str = "unknown";
//
// switch (_mode) {
// case GPS_DRIVER_MODE_UBX:
// mode_str = "UBX";
// break;
//
// case GPS_DRIVER_MODE_MTK:
// mode_str = "MTK";
// break;
//
// case GPS_DRIVER_MODE_ASHTECH:
// mode_str = "ASHTECH";
// break;
//
// default:
// break;
// }
//
// PX4_WARN("module found: %s", mode_str);
_healthy = true;
}
}
if (_healthy) {
PX4_WARN("GPS module lost");
_healthy = false;
_rate = 0.0f;
_rate_rtcm_injection = 0.0f;
}
}
if (_mode_auto) {
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
default:
break;
}
}
}
}
PX4_INFO("exiting");
orb_unsubscribe(_orb_inject_data_fd);
if (_dump_communication_pub) {
orb_unadvertise(_dump_communication_pub);
}
::close(_serial_fd);
orb_unadvertise(_report_gps_pos_pub);
/* tell the dtor that we are exiting */
_task = -1;
px4_task_exit(0);
}
Mavlink::Mavlink() :
VDev("mavlink-log", MAVLINK_LOG_DEVICE),
_device_name(DEFAULT_DEVICE_NAME),
_task_should_exit(false),
next(nullptr),
_instance_id(0),
_mavlink_fd(-1),
_task_running(false),
_hil_enabled(false),
_use_hil_gps(false),
_forward_externalsp(false),
_is_usb_uart(false),
_wait_to_transmit(false),
_received_messages(false),
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_manager(nullptr),
_parameters_manager(nullptr),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer {},
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
#ifndef __PX4_QURT
_uart_fd(-1),
#endif
_baudrate(57600),
_datarate(1000),
_datarate_events(500),
_rate_mult(1.0f),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_last_write_success_time(0),
_last_write_try_time(0),
_bytes_tx(0),
_bytes_txerr(0),
_bytes_rx(0),
_bytes_timestamp(0),
_rate_tx(0.0f),
_rate_txerr(0.0f),
_rate_rx(0.0f),
_rstatus {},
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
_param_initialized(false),
_param_system_id(0),
_param_component_id(0),
_param_system_type(MAV_TYPE_FIXED_WING),
_param_use_hil_gps(0),
_param_forward_externalsp(0),
_system_type(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
_instance_id = Mavlink::instance_count();
/* set channel according to instance id */
switch (_instance_id) {
case 0:
_channel = MAVLINK_COMM_0;
break;
case 1:
_channel = MAVLINK_COMM_1;
break;
case 2:
_channel = MAVLINK_COMM_2;
break;
case 3:
_channel = MAVLINK_COMM_3;
break;
#ifdef MAVLINK_COMM_4
case 4:
_channel = MAVLINK_COMM_4;
break;
#endif
#ifdef MAVLINK_COMM_5
case 5:
_channel = MAVLINK_COMM_5;
break;
#endif
#ifdef MAVLINK_COMM_6
case 6:
_channel = MAVLINK_COMM_6;
break;
#endif
default:
warnx("instance ID is out of range");
px4_task_exit(1);
break;
}
_rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
void
GPS::task_main()
{
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_serial_fd < 0) {
PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno);
/* tell the dtor that we are exiting, set error code */
_task = -1;
px4_task_exit(1);
}
#ifndef __PX4_QURT
// TODO: this call is not supported on Snapdragon just yet.
// However it seems to be nonblocking anyway and working.
int flags = fcntl(_serial_fd, F_GETFL, 0);
fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK);
#endif
for (int i = 0; i < _orb_inject_data_fd_count; ++i) {
_orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i);
}
initializeCommunicationDump();
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;
/* 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 */
publish();
usleep(2e5);
} else {
if (_helper != nullptr) {
delete(_helper);
/* set to zero to ensure parser is not used while not instantiated */
_helper = nullptr;
}
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_helper = new GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
_helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos);
break;
case GPS_DRIVER_MODE_ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
break;
default:
break;
}
/* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication
*/
if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
if (_mode == GPS_DRIVER_MODE_UBX) {
/* Publish initial report that we have access to a GPS,
* but set all critical state fields to indicate we have
* no valid position lock
*/
/* reset the timestamp for data, because we have no data yet */
_report_gps_pos.timestamp = 0;
_report_gps_pos.timestamp_time_relative = 0;
/* set a massive variance */
_report_gps_pos.eph = 10000.0f;
_report_gps_pos.epv = 10000.0f;
_report_gps_pos.fix_type = 0;
publish();
/* GPS is obviously detected successfully, reset statistics */
_helper->resetUpdateRates();
}
int helper_ret;
while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
if (helper_ret & 1) {
publish();
last_rate_count++;
}
if (_p_report_sat_info && (helper_ret & 2)) {
publishSatelliteInfo();
}
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f;
_rate = last_rate_count / dt;
_rate_rtcm_injection = _last_rate_rtcm_injection_count / dt;
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
_last_rate_rtcm_injection_count = 0;
_helper->storeUpdateRates();
_helper->resetUpdateRates();
}
if (!_healthy) {
// Helpful for debugging, but too verbose for normal ops
// const char *mode_str = "unknown";
//
// switch (_mode) {
// case GPS_DRIVER_MODE_UBX:
// mode_str = "UBX";
// break;
//
// case GPS_DRIVER_MODE_MTK:
// mode_str = "MTK";
// break;
//
// case GPS_DRIVER_MODE_ASHTECH:
// mode_str = "ASHTECH";
// break;
//
// default:
// break;
// }
//
// PX4_WARN("module found: %s", mode_str);
_healthy = true;
}
//check for disarming
if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) {
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
vehicle_status_s vehicle_status;
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status);
bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ||
(vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
if (armed) {
_is_armed = true;
} else if (_is_armed) {
//disable communication dump when disarming
close(_dump_from_gps_device_fd);
_dump_from_gps_device_fd = -1;
close(_dump_to_gps_device_fd);
_dump_to_gps_device_fd = -1;
_is_armed = false;
}
}
}
}
if (_healthy) {
PX4_WARN("GPS module lost");
_healthy = false;
_rate = 0.0f;
_rate_rtcm_injection = 0.0f;
}
}
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
default:
break;
}
}
}
PX4_WARN("exiting");
for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) {
orb_unsubscribe(_orb_inject_data_fd[i]);
_orb_inject_data_fd[i] = -1;
}
if (_dump_to_gps_device_fd != -1) {
close(_dump_to_gps_device_fd);
_dump_to_gps_device_fd = -1;
}
if (_dump_from_gps_device_fd != -1) {
close(_dump_from_gps_device_fd);
_dump_from_gps_device_fd = -1;
}
if (_vehicle_status_sub != -1) {
orb_unsubscribe(_vehicle_status_sub);
_vehicle_status_sub = -1;
}
::close(_serial_fd);
orb_unadvertise(_report_gps_pos_pub);
/* tell the dtor that we are exiting */
_task = -1;
px4_task_exit(0);
}