diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-13 14:29:59 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-02-13 14:29:59 +0100 |
commit | b1b078103aace743fdd2a678fbcaaec0185e92a3 (patch) | |
tree | f3c0af62390955b4934b60ebdf1338b2dafed4cd | |
parent | f640f438690848bea6281e0b254571d58d75f092 (diff) | |
download | px4-firmware-b1b078103aace743fdd2a678fbcaaec0185e92a3.tar.gz px4-firmware-b1b078103aace743fdd2a678fbcaaec0185e92a3.tar.bz2 px4-firmware-b1b078103aace743fdd2a678fbcaaec0185e92a3.zip |
GPS: Remove GPS disable debug code
-rw-r--r-- | src/drivers/gps/gps.cpp | 117 |
1 files changed, 17 insertions, 100 deletions
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1a190bf40..c880482a1 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 <uORB/topics/manual_control_setpoint.h> -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); |