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

AP_Arming: fixed accel cal test in arming

Browse Source 
use a per-imu time of last cal pass, and triple accel threshold for
IMU3. Raise threshold to 0.5 for IMU1/IMU2
This commit is contained in:
Andrew Tridgell 2015-05-08 20:23:21 +10:00
parent baed82d1c2
commit 2552acbf12
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Arming/AP_Arming.cpp
View File

@ -57,10 +57,10 @@ AP_Arming::AP_Arming(const AP_AHRS &ahrs_ref, const AP_Baro &baro, Compass &comp
, _compass(compass)
, home_is_set(home_set)
, 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);
memset(last_accel_pass_ms, 0, sizeof(last_accel_pass_ms));
memset(last_gyro_pass_ms, 0, sizeof(last_gyro_pass_ms));
}
bool AP_Arming::is_armed()
@ -169,12 +169,21 @@ bool AP_Arming::ins_checks(bool report)
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
// allow for up to 0.3 m/s/s difference. Has to pass
// allow for up to 0.5 m/s/s difference. Has to pass
// in last 10 seconds
if (vec_diff.length() <= 0.3f) {
last_accel_pass_ms = hal.scheduler->millis();
float threshold = 0.5f;
if (i >= 2) {
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1/IMU2,
and is not used for accel data in the EKF
*/
threshold *= 3;
}
if (hal.scheduler->millis() - last_accel_pass_ms > 10000) {
if (vec_diff.length() <= threshold) {
last_accel_pass_ms[i] = hal.scheduler->millis();
}
if (hal.scheduler->millis() - last_accel_pass_ms[i] > 10000) {
if (report) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent Accelerometers"));
}
@ -193,9 +202,9 @@ bool AP_Arming::ins_checks(bool report)
// allow for up to 5 degrees/s difference. Pass if its
// been OK in last 10 seconds
if (vec_diff.length() <= radians(5)) {
last_gyro_pass_ms = hal.scheduler->millis();
last_gyro_pass_ms[i] = hal.scheduler->millis();
}
if (hal.scheduler->millis() - last_gyro_pass_ms > 10000) {
if (hal.scheduler->millis() - last_gyro_pass_ms[i] > 10000) {
if (report) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: inconsistent gyros"));
}

4
libraries/AP_Arming/AP_Arming.h
View File

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