AP_GPS: fixed handling of driver destruction

this allows for more complex destructors, and ensures we don't have
multiple backends allocated at once per instance
This commit is contained in:
Andrew Tridgell 2014-04-01 06:11:55 +11:00
parent 7ceba8dce4
commit 9781175bf5
2 changed files with 8 additions and 3 deletions

View File

@ -182,9 +182,6 @@ AP_GPS::detect_instance(uint8_t instance)
if (new_gps != NULL) {
state[instance].status = NO_FIX;
if (drivers[instance] != NULL) {
delete drivers[instance];
}
drivers[instance] = new_gps;
timing[instance].last_message_time_ms = now;
}
@ -222,6 +219,10 @@ AP_GPS::update_instance(uint8_t instance)
if (tnow - timing[instance].last_message_time_ms > 1200) {
state[instance].status = NO_GPS;
timing[instance].last_message_time_ms = tnow;
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers[instance];
drivers[instance] = NULL;
}
} else {
timing[instance].last_message_time_ms = tnow;

View File

@ -25,6 +25,10 @@ class AP_GPS_Backend
public:
AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// we declare a virtual destructor so that GPS drivers can
// override with a custom destructor if need be.
virtual ~AP_GPS_Backend(void) {}
// The read() method is the only one needed in each driver. It
// should return true when the backend has successfully received a
// valid packet from the GPS.