From b1b078103aace743fdd2a678fbcaaec0185e92a3 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 13 Feb 2015 14:29:59 +0100 Subject: [PATCH] GPS: Remove GPS disable debug code --- src/drivers/gps/gps.cpp | 117 ++++++---------------------------------- 1 file changed, 17 insertions(+), 100 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1a190bf405..c880482a17 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -81,12 +81,6 @@ #endif static const int ERROR = -1; -//DEBUG BEGIN - #include -static int sp_man_sub = -1; -static struct manual_control_setpoint_s sp_man; -//DEBUG END - /* class for dynamic allocation of satellite info data */ class GPS_Sat_Info { @@ -277,27 +271,6 @@ GPS::task_main_trampoline(void *arg) g_dev->task_main(); } -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - // check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - - void GPS::task_main() { @@ -315,61 +288,27 @@ GPS::task_main() uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - //DEBUG BEGIN - sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - memset(&sp_man, 0, sizeof(sp_man)); - //DEBUG END - /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { - - //DEBUG BEGIN: Disable GPS using aux1 - orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { - _report_gps_pos.timestamp_position = hrt_absolute_time(); - _report_gps_pos.timestamp_variance = hrt_absolute_time(); - _report_gps_pos.timestamp_velocity = hrt_absolute_time(); - _report_gps_pos.fix_type = 0; - _report_gps_pos.satellites_used = 0; - - //Don't modify Lat/Lon/AMSL - - _report_gps_pos.eph = (float)0xFFFF; - _report_gps_pos.epv = (float)0xFFFF; - _report_gps_pos.s_variance_m_s = (float)0xFFFF; - - _report_gps_pos.vel_m_s = 0.0f; - _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_ned_valid = false; - - _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.c_variance_rad = (float)0xFFFF; - } - //DEBUG END - - else { - _report_gps_pos.timestamp_position = 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.timestamp_variance = hrt_absolute_time(); - _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.timestamp_velocity = hrt_absolute_time(); - _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.timestamp_position = 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.timestamp_variance = hrt_absolute_time(); + _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.timestamp_velocity = hrt_absolute_time(); + _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 @@ -426,28 +365,6 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { - //DEBUG BEGIN: Disable GPS using aux1 - orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { - _report_gps_pos.fix_type = 0; - _report_gps_pos.satellites_used = 0; - - //Don't modify Lat/Lon/AMSL - - _report_gps_pos.eph = (float)0xFFFF; - _report_gps_pos.epv = (float)0xFFFF; - _report_gps_pos.s_variance_m_s = (float)0xFFFF; - - _report_gps_pos.vel_m_s = 0.0f; - _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_ned_valid = false; - - _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.c_variance_rad = (float)0xFFFF; - } - if (_report_gps_pos_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);