dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GPS: Remove GPS disable debug code

This commit is contained in:
Johan Jansen 2015-02-13 14:29:59 +01:00
parent f640f43869
commit b1b078103a
1 changed files with 17 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
src/drivers/gps/gps.cpp
View File

@ -81,12 +81,6 @@
#endif #endif
static const int ERROR = -1; 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 for dynamic allocation of satellite info data */
class GPS_Sat_Info class GPS_Sat_Info
{ {
@ -277,27 +271,6 @@ GPS::task_main_trampoline(void *arg)
g_dev->task_main(); 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 void
GPS::task_main() GPS::task_main()
{ {
@ -315,61 +288,27 @@ GPS::task_main()
uint64_t last_rate_measurement = hrt_absolute_time(); uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0; 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 */ /* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) { while (!_task_should_exit) {
if (_fake_gps) { if (_fake_gps) {
_report_gps_pos.timestamp_position = hrt_absolute_time();
//DEBUG BEGIN: Disable GPS using aux1 _report_gps_pos.lat = (int32_t)47.378301e7f;
orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); _report_gps_pos.lon = (int32_t)8.538777e7f;
if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) { _report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.timestamp_position = hrt_absolute_time(); _report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 0; _report_gps_pos.fix_type = 3;
_report_gps_pos.satellites_used = 0; _report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
//Don't modify Lat/Lon/AMSL _report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.eph = (float)0xFFFF; _report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.epv = (float)0xFFFF; _report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.s_variance_m_s = (float)0xFFFF; _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_m_s = 0.0f; _report_gps_pos.vel_ned_valid = true;
_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;
}
//no time and satellite information simulated //no time and satellite information simulated
@ -426,28 +365,6 @@ GPS::task_main()
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
if (helper_ret & 1) { 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) { if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);