From d962e6c403678e14a64a6b01be8773e98660bb24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 13:50:32 -0800 Subject: [PATCH] Removed some unnecessairy flags, home position back working --- apps/commander/commander.c | 9 +++-- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/gps/ubx.cpp | 49 ++++--------------------- apps/drivers/gps/ubx.h | 5 --- apps/uORB/topics/vehicle_gps_position.h | 2 +- 5 files changed, 16 insertions(+), 51 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f19f1d0e6f..6b1bc0f9b5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1683,7 +1683,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if gps fix is ok */ // XXX magic number - float dop_threshold_m = 2.0f; + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; /* * If horizontal dilution of precision (hdop / eph) @@ -1694,8 +1695,10 @@ int commander_thread_main(int argc, char *argv[]) * the system is currently not armed, set home * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) - && (vdop_m < dop_threshold_m) + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 8c2775adbb..45f18158fb 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -591,7 +591,7 @@ start(const char *uart_path) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - printf("Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } exit(0); diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index eec1df9809..a823271758 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -50,12 +50,7 @@ UBX::UBX() : _config_state(UBX_CONFIG_STATE_PRT), -_waiting_for_ack(false), -_new_nav_posllh(false), -_new_nav_timeutc(false), -//_new_nav_dop(false), -_new_nav_sol(false), -_new_nav_velned(false) +_waiting_for_ack(false) { reset(); } @@ -413,13 +408,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m + gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m /* Add timestamp to finish the report */ gps_position->timestamp_position = hrt_absolute_time(); - _new_nav_posllh = true; /* set flag to trigger publishing of new position */ pos_updated = true; @@ -445,9 +439,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_variance = hrt_absolute_time(); - - _new_nav_sol = true; - } else { warnx("NAV_SOL: checksum invalid"); } @@ -502,8 +493,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_time = hrt_absolute_time(); - _new_nav_timeutc = true; - } else { warnx("NAV_TIMEUTC: checksum invalid"); } @@ -581,20 +570,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->satellite_elevation[i] = 0; gps_position->satellite_azimuth[i] = 0; } + gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set flag if any sat info is available */ - if (!packet_part1->numCh > 0) { - gps_position->satellite_info_available = 1; - + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + gps_position->satellite_info_available = true; } else { - gps_position->satellite_info_available = 0; + gps_position->satellite_info_available = false; } - - gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones gps_position->timestamp_satellites = hrt_absolute_time(); - // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report - } else { warnx("NAV_SVINFO: checksum invalid"); } @@ -619,9 +604,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->vel_ned_valid = true; gps_position->timestamp_velocity = hrt_absolute_time(); - _new_nav_velned = true; - - } else { warnx("NAV_VELNED: checksum invalid"); } @@ -760,21 +742,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ default: break; } - - /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { - - /* we have received everything, this most probably means that the configuration is fine */ - config_needed = false; - - /* Reset the flags */ - _new_nav_posllh = false; - _new_nav_timeutc = false; -// _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; - - } return; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 43cded02f7..a23bb55025 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -369,11 +369,6 @@ private: ubx_message_class_t _message_class; ubx_message_id_t _message_id; unsigned _payload_size; - bool _new_nav_posllh; - bool _new_nav_timeutc; -// bool _new_nav_dop; - bool _new_nav_sol; - bool _new_nav_velned; }; #endif /* UBX_H_ */ diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index aa75c22acb..5463a460da 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -86,7 +86,7 @@ struct vehicle_gps_position_s uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /**