mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-04 13:15:08 +08:00
gpssim: cleanup (remove unused fields)
This commit is contained in:
@@ -107,20 +107,12 @@ protected:
|
|||||||
private:
|
private:
|
||||||
|
|
||||||
bool _task_should_exit; ///< flag to make the main worker task exit
|
bool _task_should_exit; ///< flag to make the main worker task exit
|
||||||
int _serial_fd; ///< serial interface to GPS
|
|
||||||
unsigned _baudrate; ///< current baudrate
|
|
||||||
char _port[20]; ///< device / serial port path
|
|
||||||
volatile int _task; ///< worker task
|
volatile int _task; ///< worker task
|
||||||
bool _healthy; ///< flag to signal if the GPS is ok
|
|
||||||
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
|
|
||||||
bool _mode_changed; ///< flag that the GPS mode has changed
|
|
||||||
//gps_driver_mode_t _mode; ///< current mode
|
|
||||||
GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
|
GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
|
||||||
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
|
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
|
||||||
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
|
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
|
||||||
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
|
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
|
||||||
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
|
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
|
||||||
float _rate; ///< position update rate
|
|
||||||
SyncObj _sync;
|
SyncObj _sync;
|
||||||
int _fix_type;
|
int _fix_type;
|
||||||
int _num_sat;
|
int _num_sat;
|
||||||
@@ -175,27 +167,17 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info,
|
|||||||
int fix_type, int num_sat, int noise_multiplier) :
|
int fix_type, int num_sat, int noise_multiplier) :
|
||||||
VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10),
|
VirtDevObj("gps", GPSSIM_DEVICE_PATH, nullptr, 1e6 / 10),
|
||||||
_task_should_exit(false),
|
_task_should_exit(false),
|
||||||
//_healthy(false),
|
|
||||||
//_mode_changed(false),
|
|
||||||
//_mode(GPS_DRIVER_MODE_UBX),
|
|
||||||
//_Helper(nullptr),
|
|
||||||
_Sat_Info(nullptr),
|
_Sat_Info(nullptr),
|
||||||
|
_report_gps_pos{},
|
||||||
_report_gps_pos_pub(nullptr),
|
_report_gps_pos_pub(nullptr),
|
||||||
_p_report_sat_info(nullptr),
|
_p_report_sat_info(nullptr),
|
||||||
_report_sat_info_pub(nullptr),
|
_report_sat_info_pub(nullptr),
|
||||||
_rate(0.0f),
|
|
||||||
_fix_type(fix_type),
|
_fix_type(fix_type),
|
||||||
_num_sat(num_sat),
|
_num_sat(num_sat),
|
||||||
_noise_multiplier(noise_multiplier)
|
_noise_multiplier(noise_multiplier)
|
||||||
{
|
{
|
||||||
// /* store port name */
|
|
||||||
// strncpy(_port, uart_path, sizeof(_port));
|
|
||||||
// /* enforce null termination */
|
|
||||||
// _port[sizeof(_port) - 1] = '\0';
|
|
||||||
|
|
||||||
/* we need this potentially before it could be set in task_main */
|
/* we need this potentially before it could be set in task_main */
|
||||||
g_dev = this;
|
g_dev = this;
|
||||||
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
|
|
||||||
|
|
||||||
/* create satellite info data object if requested */
|
/* create satellite info data object if requested */
|
||||||
if (enable_sat_info) {
|
if (enable_sat_info) {
|
||||||
@@ -329,9 +311,6 @@ GPSSIM::task_main()
|
|||||||
/* 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) {
|
||||||
|
|
||||||
// GPS is obviously detected successfully, reset statistics
|
|
||||||
//_Helper->reset_update_rates();
|
|
||||||
|
|
||||||
int recv_ret = receive(TIMEOUT_100MS);
|
int recv_ret = receive(TIMEOUT_100MS);
|
||||||
|
|
||||||
if (recv_ret > 0) {
|
if (recv_ret > 0) {
|
||||||
@@ -382,7 +361,6 @@ GPSSIM::print_info()
|
|||||||
//GPS Mode
|
//GPS Mode
|
||||||
PX4_INFO("protocol: SIM");
|
PX4_INFO("protocol: SIM");
|
||||||
|
|
||||||
PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
|
|
||||||
PX4_INFO("sat info: %s, noise: %d, jamming detected: %s",
|
PX4_INFO("sat info: %s, noise: %d, jamming detected: %s",
|
||||||
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
|
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
|
||||||
_report_gps_pos.noise_per_ms,
|
_report_gps_pos.noise_per_ms,
|
||||||
|
|||||||
Reference in New Issue
Block a user