AP_GPS: fixed uninitialied variable bugs found with valgrind
This commit is contained in:
parent
56809a9df2
commit
b3ec5187f2
@ -24,8 +24,6 @@ void AP_GPS_406::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting
|
||||
_configure_gps(); // Function to configure GPS, to output only the desired msg's
|
||||
|
||||
AP_GPS_SIRF::init(s, nav_setting); // let the superclass do anything it might need here
|
||||
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
// Private Methods //////////////////////////////////////////////////////////////
|
||||
|
@ -28,7 +28,6 @@ void
|
||||
AP_GPS_Auto::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
{
|
||||
_port = s;
|
||||
idleTimeout = 1200;
|
||||
_nav_setting = nav_setting;
|
||||
}
|
||||
|
||||
|
@ -18,7 +18,6 @@
|
||||
void AP_GPS_HIL::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
{
|
||||
_port = s;
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
bool AP_GPS_HIL::read(void)
|
||||
|
@ -43,8 +43,6 @@ AP_GPS_MTK::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
|
||||
// set initial epoch code
|
||||
_epoch = TIME_OF_DAY;
|
||||
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
// Process bytes available from the stream
|
||||
|
@ -44,7 +44,6 @@ AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
_epoch = TIME_OF_DAY;
|
||||
_time_offset = 0;
|
||||
_offset_calculated = false;
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
// Process bytes available from the stream
|
||||
|
@ -103,8 +103,6 @@ void AP_GPS_NMEA::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_settin
|
||||
|
||||
// send the ublox init strings
|
||||
_port->print_P((const prog_char_t *)_ublox_init_string);
|
||||
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
bool AP_GPS_NMEA::read(void)
|
||||
|
@ -37,7 +37,6 @@ AP_GPS_SIRF::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
|
||||
// send SiRF binary setup messages
|
||||
_write_progstr_block(_port, (const prog_char *)init_messages, sizeof(init_messages));
|
||||
idleTimeout = 1200;
|
||||
}
|
||||
|
||||
// Process bytes available from the stream
|
||||
|
@ -44,7 +44,6 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting)
|
||||
_port->flush();
|
||||
|
||||
_epoch = TIME_OF_WEEK;
|
||||
idleTimeout = 1200;
|
||||
|
||||
// configure the GPS for the messages we want
|
||||
_configure_gps();
|
||||
|
@ -19,17 +19,19 @@ extern const AP_HAL::HAL& hal;
|
||||
|
||||
GPS::GPS(void) :
|
||||
// ensure all the inherited fields are zeroed
|
||||
time(0),
|
||||
num_sats(0),
|
||||
new_data(false),
|
||||
fix(FIX_NONE),
|
||||
valid_read(false),
|
||||
last_fix_time(0),
|
||||
_have_raw_velocity(false),
|
||||
_idleTimer(0),
|
||||
_status(GPS::NO_FIX),
|
||||
_last_ground_speed_cm(0),
|
||||
_velocity_north(0),
|
||||
_velocity_east(0),
|
||||
_velocity_down(0)
|
||||
_velocity_down(0)
|
||||
{
|
||||
}
|
||||
|
||||
@ -44,9 +46,9 @@ GPS::update(void)
|
||||
|
||||
tnow = hal.scheduler->millis();
|
||||
|
||||
// if we did not get a message, and the idle timer has expired, re-init
|
||||
// if we did not get a message, and the idle timer of 1.2 seconds has expired, re-init
|
||||
if (!result) {
|
||||
if ((tnow - _idleTimer) > idleTimeout) {
|
||||
if ((tnow - _idleTimer) > 1200) {
|
||||
Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
|
||||
_status = NO_GPS;
|
||||
|
||||
|
@ -125,14 +125,6 @@ public:
|
||||
virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude,
|
||||
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
|
||||
|
||||
/// Time in milliseconds after which we will assume the GPS is no longer
|
||||
/// sending us updates and attempt a re-init.
|
||||
///
|
||||
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
|
||||
/// rate.
|
||||
///
|
||||
uint32_t idleTimeout;
|
||||
|
||||
// components of velocity in 2D, in m/s
|
||||
float velocity_north(void) {
|
||||
return _status >= GPS_OK_FIX_2D ? _velocity_north : 0;
|
||||
|
Loading…
Reference in New Issue
Block a user