Browse Source

Copter: log event when primary GPS changes

master
Randy Mackay 8 years ago
parent
commit
8140353c64
  1. 1
      ArduCopter/Copter.h
  2. 6
      ArduCopter/Log.cpp
  3. 1
      ArduCopter/defines.h

1
ArduCopter/Copter.h

@ -331,6 +331,7 @@ private: @@ -331,6 +331,7 @@ private:
struct {
uint8_t baro : 1; // true if baro is healthy
uint8_t compass : 1; // true if compass is healthy
uint8_t primary_gps; // primary gps index
} sensor_health;
// Motor Output

6
ArduCopter/Log.cpp

@ -621,6 +621,12 @@ void Copter::Log_Sensor_Health() @@ -621,6 +621,12 @@ void Copter::Log_Sensor_Health()
sensor_health.compass = compass.healthy();
Log_Write_Error(ERROR_SUBSYSTEM_COMPASS, (sensor_health.compass ? ERROR_CODE_ERROR_RESOLVED : ERROR_CODE_UNHEALTHY));
}
// check primary GPS
if (sensor_health.primary_gps != gps.primary_sensor()) {
sensor_health.primary_gps = gps.primary_sensor();
Log_Write_Event(DATA_GPS_PRIMARY_CHANGED);
}
}
struct PACKED log_Heli {

1
ArduCopter/defines.h

@ -393,6 +393,7 @@ enum DevOptions { @@ -393,6 +393,7 @@ enum DevOptions {
#define DATA_AVOIDANCE_ADSB_DISABLE 64
#define DATA_AVOIDANCE_PROXIMITY_ENABLE 65
#define DATA_AVOIDANCE_PROXIMITY_DISABLE 66
#define DATA_GPS_PRIMARY_CHANGED 67
// Centi-degrees to radians
#define DEGX100 5729.57795f

Loading…
Cancel
Save