zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

GPS: Remove GPS disable debug code

sbg
Johan Jansen 10 years ago
parent
f640f43869
commit
b1b078103a
1 changed files with 17 additions and 100 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 117
      src/drivers/gps/gps.cpp

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

@ -81,12 +81,6 @@ @@ -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) @@ -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() @@ -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() @@ -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);

Write Preview
Loading…
Cancel
Save