Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate EKF check 

Check runs at 10hz
Log ekf check failures and ekd failsafes
This commit is contained in:
Randy Mackay 2014-07-21 19:12:17 +09:00
parent a4a4334b13
commit 7bda6cbadf
4 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -413,6 +413,7 @@ static struct {
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
@ -786,6 +787,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
#endif
{ update_notify, 8, 10 },
{ one_hz_loop, 400, 42 },
{ ekf_check, 40, 2 },
{ crash_check, 40, 2 },
{ gcs_check_input, 8, 550 },
{ gcs_send_heartbeat, 400, 150 },
@ -850,6 +852,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
#endif
{ update_notify, 2, 100 },
{ one_hz_loop, 100, 420 },
{ ekf_check, 10, 20 },
{ crash_check, 10, 20 },
{ gcs_check_input, 2, 550 },
{ gcs_send_heartbeat, 100, 150 },

2
ArduCopter/GCS_Mavlink.pde
View File

@ -46,7 +46,7 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs) {
if (failsafe.radio || failsafe.battery || failsafe.gps || failsafe.gcs || failsafe.ekf) {
system_status = MAV_STATE_CRITICAL;
}

5
ArduCopter/defines.h
View File

@ -350,6 +350,8 @@ enum FlipState {
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
#define ERROR_SUBSYSTEM_PARACHUTE 15
#define ERROR_SUBSYSTEM_EKF_CHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKF 17
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
@ -373,6 +375,9 @@ enum FlipState {
#define ERROR_CODE_AUTOTUNE_BAD_GAINS 2
// parachute failed to deploy because of low altitude
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
// EKF check definitions
#define ERROR_CODE_EKF_CHECK_BAD_COMPASS 2
#define ERROR_CODE_EKF_CHECK_BAD_COMPASS_CLEARED 0
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00

3
ArduCopter/system.pde
View File

@ -321,7 +321,8 @@ static void startup_ground(bool force_gyro_cal)
static bool GPS_ok()
{
if (ap.home_is_set && gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
!gps_glitch.glitching() && !failsafe.gps) {
!gps_glitch.glitching() && !failsafe.gps &&
!ekf_check_state.bad_compass && !failsafe.ekf) {
return true;
}else{
return false;