diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b09f53e35f..543c1796b6 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -421,6 +421,17 @@ # define MAGNETOMETER ENABLED #endif +// expected magnetic field strength +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + #ifndef COMPASS_MAGFIELD_EXPECTED + # define COMPASS_MAGFIELD_EXPECTED 330 // maximum mag field length for pre-arm checks + #endif +#else // APM1, PX4, SITL + #ifndef COMPASS_MAGFIELD_EXPECTED + #define COMPASS_MAGFIELD_EXPECTED 530 // maximum mag field length for pre-arm checks + #endif +#endif + ////////////////////////////////////////////////////////////////////////////// // OPTICAL_FLOW #if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 0060ad1583..71ef94d200 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -246,6 +246,17 @@ static void pre_arm_checks(bool display_failure) return; } + // check for unreasonable mag field length + float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z); + if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.5 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.5) { + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field")); + } + return; + }else{ + cliSerial->printf_P(PSTR("\nMagField: %4.2f vs %4.2f ~ %4.2f"),mag_field, (float)COMPASS_MAGFIELD_EXPECTED*1.5, COMPASS_MAGFIELD_EXPECTED*0.5); + } + // barometer health check if(!barometer.healthy) { if (display_failure) { diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 95fb284e7c..4075f7611a 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -660,7 +660,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) // calculate and display interference compensation at full throttle as % of total mag field if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { // interference is impact@fullthrottle / mag field * 100 - interference_pct = motor_compensation.length() / 330.0f * 100.0f; + interference_pct = motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; }else{ // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 interference_pct = motor_compensation.length() * (current_amps_max/throttle_pct_max) / 330.0f * 100.0f;