aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Arming: accept accel/gyro if OK in last 10 seconds

Browse Source 
this should prevent short periods of movement from triggering arming
status change alarms
This commit is contained in:
Andrew Tridgell 2015-03-31 15:29:50 -07:00
parent 6a33588f67
commit 5acc4c333b
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Arming/AP_Arming.cpp
View File

@ -57,6 +57,8 @@ AP_Arming::AP_Arming(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &comp
, _compass(compass) , _compass(compass)
, home_is_set(home_set) , home_is_set(home_set)
, gcs_send_text_P(gcs_print_func) , gcs_send_text_P(gcs_print_func)
, last_accel_pass_ms(0)
, last_gyro_pass_ms(0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -167,8 +169,12 @@ bool AP_Arming::ins_checks(bool report)
// get next accel vector // get next accel vector
const Vector3f &accel_vec = ins.get_accel(i); const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec; Vector3f vec_diff = accel_vec - prime_accel_vec;
// allow for up to 0.3 m/s/s difference // allow for up to 0.3 m/s/s difference. Has to pass
if (vec_diff.length() > 0.3f) { // in last 10 seconds
if (vec_diff.length() <= 0.3f) {
last_accel_pass_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - last_accel_pass_ms > 10000) {
if (report) { if (report) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers"));
} }
@ -184,8 +190,12 @@ bool AP_Arming::ins_checks(bool report)
// get next gyro vector // get next gyro vector
const Vector3f &gyro_vec = ins.get_gyro(i); const Vector3f &gyro_vec = ins.get_gyro(i);
Vector3f vec_diff = gyro_vec - prime_gyro_vec; Vector3f vec_diff = gyro_vec - prime_gyro_vec;
// allow for up to 5 degrees/s difference // allow for up to 5 degrees/s difference. Pass if its
if (vec_diff.length() > radians(5)) { // been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - last_gyro_pass_ms > 10000) {
if (report) { if (report) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros")); gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros"));
} }

2
libraries/AP_Arming/AP_Arming.h
View File

@ -77,6 +77,8 @@ private:
Compass &_compass; Compass &_compass;
const enum HomeState &home_is_set; const enum HomeState &home_is_set;
gcs_send_t_p gcs_send_text_P; gcs_send_t_p gcs_send_text_P;
uint32_t last_accel_pass_ms;
uint32_t last_gyro_pass_ms;
void set_enabled_checks(uint16_t); void set_enabled_checks(uint16_t);