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

Copter: remove dcm check

This commit is contained in:
Randy Mackay 2015-03-26 23:23:17 -07:00
parent 0f540fc30c
commit 74b98b89d0
6 changed files with 30 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -740,7 +740,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
#endif
{ update_notify, 8, 10 },
{ one_hz_loop, 400, 42 },
{ ekf_dcm_check, 40, 2 },
{ ekf_check, 40, 2 },
{ crash_check, 40, 2 },
{ landinggear_update, 40, 1 },
{ gcs_check_input, 8, 550 },
@ -815,7 +815,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
#endif
{ update_notify, 2, 100 },
{ one_hz_loop, 100, 420 },
{ ekf_dcm_check, 10, 20 },
{ ekf_check, 10, 20 },
{ crash_check, 10, 20 },
{ landinggear_update, 10, 10 },
{ gcs_check_input, 2, 550 },

5
ArduCopter/Parameters.h
View File

@ -125,9 +125,9 @@ public:
k_param_acro_expo,
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // 59
k_param_dcmcheck_thresh, // deprecated - remove
k_param_log_bitmask,
k_param_cli_enabled,
k_param_cli_enabled, // 61
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
@ -404,7 +404,6 @@ public:
AP_Int8 land_repositioning;
AP_Float ekfcheck_thresh;
AP_Float dcmcheck_thresh;
#if FRAME_CONFIG == HELI_FRAME
// Heli

7
ArduCopter/Parameters.pde
View File

@ -441,13 +441,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(ekfcheck_thresh, "EKF_CHECK_THRESH", EKFCHECK_THRESHOLD_DEFAULT),
// @Param: DCM_CHECK_THRESH
// @DisplayName: DCM yaw error threshold
// @Description: Allows setting the maximum acceptable yaw error as a sin of the yaw error (0 to disable check)
// @Values: 0:Disabled, 0.6:Strict, 0.8:Default, 0.98:Relaxed
// @User: Advanced
GSCALAR(dcmcheck_thresh, "DCM_CHECK_THRESH", DCMCHECK_THRESHOLD_DEFAULT),
#if FRAME_CONFIG == HELI_FRAME
// @Group: HS1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp

5
ArduCopter/config.h
View File

@ -259,13 +259,10 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// EKF & DCM Checker
// EKF Checker
#ifndef EKFCHECK_THRESHOLD_DEFAULT
# define EKFCHECK_THRESHOLD_DEFAULT 0.8f // EKF checker's default compass and velocity variance above which the EKF's horizontal position will be considered bad
#endif
#ifndef DCMCHECK_THRESHOLD_DEFAULT
# define DCMCHECK_THRESHOLD_DEFAULT 0.8f // DCM checker's default yaw error threshold above which we will abandon horizontal position hold. The units are sin(angle) so 0.8 = about 60degrees of error
#endif
#ifndef EKF_ORIGIN_MAX_DIST_M
# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km

6
ArduCopter/defines.h
View File

@ -331,7 +331,7 @@ enum FlipState {
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
#define ERROR_SUBSYSTEM_PARACHUTE 15
#define ERROR_SUBSYSTEM_EKFINAV_CHECK 16
#define ERROR_SUBSYSTEM_EKFCHECK 16
#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17
#define ERROR_SUBSYSTEM_BARO 18
#define ERROR_SUBSYSTEM_CPU 19
@ -357,8 +357,8 @@ enum FlipState {
// parachute failed to deploy because of low altitude
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
// EKF check definitions
#define ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED 0
#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2
#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0
// Baro specific error codes
#define ERROR_CODE_BARO_GLITCH 2

73
ArduCopter/ekf_check.pde
View File

@ -15,66 +15,44 @@
# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds
#endif
// Enumerator for types of check
enum EKFCheckType {
CHECK_NONE = 0,
CHECK_DCM = 1,
CHECK_EKF = 2
};
////////////////////////////////////////////////////////////////////////////////
// EKF_check strucutre
////////////////////////////////////////////////////////////////////////////////
static struct {
uint8_t fail_count_compass; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_compass : 1; // true if dcm or ekf should be considered untrusted (fail_count_compass has exceeded EKF_CHECK_ITERATIONS_MAX)
uint8_t fail_count; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_variance : 1; // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS
} ekf_check_state;
// ekf_dcm_check - detects if ekf variances or dcm yaw errors that are out of tolerance and triggers failsafe
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void ekf_dcm_check()
void ekf_check()
{
EKFCheckType check_type = CHECK_NONE;
// decide if we should check ekf or dcm
if (ahrs.have_inertial_nav() && g.ekfcheck_thresh > 0.0f) {
check_type = CHECK_EKF;
} else if (g.dcmcheck_thresh > 0.0f) {
check_type = CHECK_DCM;
}
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors.armed() || ap.usb_connected || check_type == CHECK_NONE) {
ekf_check_state.fail_count_compass = 0;
ekf_check_state.bad_compass = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
if (!motors.armed() || ap.usb_connected || (g.ekfcheck_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
failsafe_ekf_off_event(); // clear failsafe
return;
}
// compare compass and velocity variance vs threshold
if ((check_type == CHECK_EKF && ekf_over_threshold()) || (check_type == CHECK_DCM && dcm_over_threshold())) {
if (ekf_over_threshold()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_compass) {
if (!ekf_check_state.bad_variance) {
// increase counter
ekf_check_state.fail_count_compass++;
ekf_check_state.fail_count++;
// if counter above max then trigger failsafe
if (ekf_check_state.fail_count_compass >= EKF_CHECK_ITERATIONS_MAX) {
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count_compass = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_compass = true;
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_BAD_VARIANCE);
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((hal.scheduler->millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
if (check_type == CHECK_EKF) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
} else {
gcs_send_text_P(SEVERITY_HIGH,PSTR("DCM bad heading"));
}
gcs_send_text_P(SEVERITY_HIGH,PSTR("EKF variance"));
ekf_check_state.last_warn_time = hal.scheduler->millis();
}
failsafe_ekf_event();
@ -82,14 +60,14 @@ void ekf_dcm_check()
}
} else {
// reduce counter
if (ekf_check_state.fail_count_compass > 0) {
ekf_check_state.fail_count_compass--;
if (ekf_check_state.fail_count > 0) {
ekf_check_state.fail_count--;
// if compass is flagged as bad and the counter reaches zero then clear flag
if (ekf_check_state.bad_compass && ekf_check_state.fail_count_compass == 0) {
ekf_check_state.bad_compass = false;
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFINAV_CHECK, ERROR_CODE_EKFINAV_CHECK_VARIANCE_CLEARED);
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
@ -97,18 +75,11 @@ void ekf_dcm_check()
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_compass;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
// To-Do: add ekf variances to extended status
}
// dcm_over_threshold - returns true if the dcm yaw error is over the tolerance
static bool dcm_over_threshold()
{
// return true if yaw error is over the threshold
return (g.dcmcheck_thresh > 0.0f && ahrs.get_error_yaw() > g.dcmcheck_thresh);
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
static bool ekf_over_threshold()
{