Browse Source

Plane: GPS now logs its own data

master
Peter Barker 7 years ago committed by Randy Mackay
parent
commit
5ef417a669
  1. 10
      ArduPlane/ArduPlane.cpp
  2. 8
      ArduPlane/Log.cpp
  3. 1
      ArduPlane/Plane.h

10
ArduPlane/ArduPlane.cpp

@ -412,17 +412,7 @@ void Plane::update_GPS_50Hz(void) @@ -412,17 +412,7 @@ void Plane::update_GPS_50Hz(void)
ahrs.get_relative_position_D_home(relative_altitude);
relative_altitude *= -1.0f;
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
gps.update();
for (uint8_t i=0; i<gps.num_sensors(); i++) {
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
last_gps_reading[i] = gps.last_message_time_ms(i);
if (should_log(MASK_LOG_GPS)) {
Log_Write_GPS(i);
}
}
}
}
/*

8
ArduPlane/Log.cpp

@ -272,13 +272,6 @@ void Plane::Log_Write_AETR() @@ -272,13 +272,6 @@ void Plane::Log_Write_AETR()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
void Plane::Log_Write_GPS(uint8_t instance)
{
if (!ahrs.have_ekf_logging()) {
DataFlash.Log_Write_GPS(gps, instance);
}
}
void Plane::Log_Write_IMU()
{
DataFlash.Log_Write_IMU();
@ -396,7 +389,6 @@ void Plane::Log_Write_Optflow() {} @@ -396,7 +389,6 @@ void Plane::Log_Write_Optflow() {}
#endif
void Plane::Log_Arm_Disarm() {}
void Plane::Log_Write_GPS(uint8_t instance) {}
void Plane::Log_Write_IMU() {}
void Plane::Log_Write_RC(void) {}
void Plane::Log_Write_Airspeed(void) {}

1
ArduPlane/Plane.h

@ -801,7 +801,6 @@ private: @@ -801,7 +801,6 @@ private:
void Log_Write_Sonar();
void Log_Write_Optflow();
void Log_Arm_Disarm();
void Log_Write_GPS(uint8_t instance);
void Log_Write_IMU();
void Log_Write_RC(void);
void Log_Write_Airspeed(void);

Loading…
Cancel
Save